Scenario-Based Trajectory Optimization in Uncertain Dynamic Environments

Delft University of Technology

Scenario-Based Trajectory Optimization in Uncertain Dynamic Environments

de Groot, O.M.; Brito, Bruno; Ferranti, Laura; Gavrila, Dariu; Alonso-Mora, Javier

DOI

10.1109/LRA.2021.3074866

Publication date 2021

Document Version Final published version Published in

IEEE Robotics and Automation Letters

Citation (APA)

de Groot, O. M., Brito, B., Ferranti, L., Gavrila, D., & Alonso-Mora, J. (2021). Scenario-Based Trajectory Optimization in Uncertain Dynamic Environments. IEEE Robotics and Automation Letters, 6(3), 5389-5396. https://doi.org/10.1109/LRA.2021.3074866

Important note

To cite this publication, please use the final published version (if applicable). Please check the document version above.

Copyright

Other than for strictly personal use, it is not permitted to download, forward or distribute the text or part of it, without the consent of the author(s) and/or copyright holder(s), unless the work is under an open content license such as Creative Commons. Takedown policy

Please contact us and provide details if you believe this document breaches copyrights. We will remove access to the work immediately and investigate your claim.

This work is downloaded from Delft University of Technology.

Scenario-Based Trajectory Optimization in Uncertain

Dynamic Environments

Oscar de Groot

, Bruno Brito

, Laura Ferranti

, Dariu Gavrila

, and Javier Alonso-Mora

Abstract—We present an optimization-based method to plan the

motion of an autonomous robot under the uncertainties associated with dynamic obstacles, such as humans. Our method bounds the marginal risk of collisions at each point in time by incorporating chance constraints into the planning problem. This problem is not suitable for online optimization outright for arbitrary probability distributions. Hence, we sample from these chance constraints us-ing an uncertainty model, to generate “scenarios,” which translate the probabilistic constraints into deterministic ones. In practice, each scenario represents the collision constraint for a dynamic obstacle at the location of the sample. The number of theoretically required scenarios can be very large. Nevertheless, by exploiting the geometry of the workspace, we show how to prune most scenarios before optimization and we demonstrate how the reduced scenarios can still provide probabilistic guarantees on the safety of the motion plan. Since our approach is scenario based, we are able to handle arbitrary uncertainty distributions. We apply our method in a Model Predictive Contouring Control framework and demonstrate its benefits in simulations and experiments with a moving robot platform navigating among pedestrians, running in real-time.

Index Terms—Motion and path planning, optimization and

optimal control, collision avoidance.

I. INTRODUCTION

M

OBILE robots are increasingly becoming part of our society, with applications in warehouses [1], automo-tive [2], maritime transportation [3], etc. In all these domains, it is essential that the robots can safely operate in dynamic environ-ments (e.g., near humans). However, uncertainty is omnipresent, for example, in the future motion paths of the dynamic obstacles or in sensing (i.e., localization) errors. Our goal is to design a local robot motion planning algorithm able to plan collision-free trajectories in the presence of possibly unbounded and arbitrary uncertainties.

Optimization-based motion planning methods avoid colli-sions by imposing constraints in the optimization problem. Clas-sical methods consider deterministic obstacle predictions, that

Manuscript received October 15, 2020; accepted February 20, 2021. Date of publication April 21, 2021; date of current version May 11, 2021. This letter was recommended for publication by Associate Editor A. Faust and Editor N. Amato upon evaluation of the reviewers’ comments. This work was supported by the Dutch Science Foundation NWO-TTW, within the SafeVRU Project (nr. 14667) and Veni Award (nr. 15916). (Corresponding author: Oscar de Groot.) The authors are with the Department of Cognitive Robotics, TU Delft, CD Delft 2628, The Netherlands (e-mail: o.m.degroot@tudelft.nl; bruno.debrito @tudelft.nl; l.ferranti@tudelft.nl; D.M.Gavrila@tudelft.nl; j.alonsomora@ tudelft.nl).

This letter has supplementary downloadable material available at https://doi. org/10.1109/LRA.2021.3074866, provided by the authors.

Digital Object Identifier 10.1109/LRA.2021.3074866

is, they do not account for the presence of uncertainties. When uncertainties come into the picture, deterministic frameworks fail to achieve safety, since they do not consider the possible spread of outcomes. In the case of bounded uncertainties, that is, if the probability density function is non-zero in a bounded domain of the robot’s workspace and is zero elsewhere, then it is possible to set the acceptable level of risk to zero. This approach is referred to as robust optimization. On the one hand, this approach allows for the addition of uncertainties in the deterministic framework. On the other hand, the assumption that the distribution is bounded can be limiting (e.g., when obstacle predictions are Gaussian). Additionally, it becomes conservative when the domain of support is large. In the presence of un-bounded uncertainties, chance constraint optimization allows one to constrain the probability of collisions to be below an acceptable level of risk. In this work, and likewise to [4], [5], we consider the marginal probabilities of collision at each point in time. This is, we constrain the chance of collision for each step of the trajectory, separately.

Directly evaluating these chance constraints is intractable, especially for arbitrary shapes of the distribution. Instead they are often either approximated (e.g., using particle filters [6]) or bounded. Approximation techniques have received most atten-tion, due to their sample efficiency. However, the safety of these approaches cannot be guaranteed, especially when operating in unknown environments.

Contribution. In this work, we assume that a perception

module provides predictions of the motion of dynamic obsta-cles together with a description of their (unbounded, possibly non Gaussian) uncertainty. To provide probabilistic safety for each step of the planned trajectory with respect to the modeled uncertainties, our work presents a novel probabilistic trajectory

optimization framework for motion planning in uncertain

dy-namic environments, that is, a Scenario-based Model Predictive Contouring Control (S-MPCC) design. Our S-MPCC builds on nonconvex scenario-optimization framework [7] and the model predictive contouring control (MPCC) design of [8]. We show that in contrast with the general a posteriori results in [7], we obtain the perceived risk of our motion plan before optimization. The support subsample, which is the key indicator for the risk in [7], is obtained through the geometry of the problem, leading to efficient evaluation of the samples. While sampling-based chance constrained approaches are generally considered in-tractable for real-time motion planning, our method is compet-itive in terms of computation times with state-of-the-art plan-ning methods, while applicable to generic uncertainties. The

5390 IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 6, NO. 3, JULY 2021

Fig. 1. Grid wise evaluation of the collision probability, withr = 0.5 m of chance constraints (2c) and (7b) in an example whereδ follows a Mixture-of-Gaussians (MoG) distribution. The red square denotes the linearization point.

approach handles multiple obstacles and accounts for the size of the vehicle and obstacles.

We show how our approach allows the robot to move con-tinuously through its environment while reasoning about its probability of colliding with dynamic obstacles. In our frame-work, illustrated in Fig. 2, instead of directly solving the chance constrained motion planning problem, we solve an associated deterministic problem obtained as follows. First, we apply a tailored linearization of the chance constraints, then we sample from the linearized chance constraints a large set of deterministic constraints, known as scenarios. The number of scenarios drawn is linked with the associated risk of collisions. This allows us to reformulate the original planning problem in a deterministic one, known as a scenario program. Using this approach we effectively resolve the chance constraints in a preprocessing step.

Uncertainty of predictions is generally non Gaussian and appears, for example, when Gaussian uncertainty is propagated through nonlinear dynamics. Our method is applicable to generic uncertainties. We demonstrate our framework for Gaussian and non Gaussian uncertainties using an autonomous ground robot, both in simulation and in experiments.

Related Work: Trajectory optimization with chance

con-straints for collision avoidance has previously been considered in the case of Gaussian uncertainties. For example, [8] defined collision avoidance constraints in an MPC framework, by model-ing the dynamic obstacles as ellipses. This representation allows the planner to accommodate Gaussian uncertainties (as the level set of a Gaussian distribution are ellipses) and solve a deter-ministic nonlinear optimization problem online. The approach was applied for autonomous driving in [9]. In [4] the chance constraint problem is solved explicitly. Their approach linearizes the collision chance constraints, and uses the prior of Gaussian uncertainty to formulate deterministic constraints on the mean and covariance of the distribution.

Literature on motion planning for non Gaussian uncertainty distributions is still limited. Inspired by particle filter ap-proaches, [6] introduces a method to approximately evaluate the chance constraints using particles. Their method uses a relatively small sample size, but cannot provide any guarantees on the safety of the solution. In [10], a Rapidly-expanding Random Trees (RRT) algorithm is presented where the un-certainty is evaluated for each node in the tree. The dynamic

obstacle trajectories are predicted using Gaussian Processes. In [11], assuming full knowledge of the probability distribution, polynomial chance constraints are transformed to deterministic inequalities using the statistical moments of the non Gaussian distribution.

Compared to the previous methods, our approach bounds the probability of collision using the scenario optimization framework. This framework is well established for convex op-timization ([5], [12]–[16]). A framework for non-convex sce-nario optimization was recently introduced in [7]. We rely on this framework and extend it in the context of robot motion planning.

II. PROBLEMFORMULATION

We consider the motion planning problem of a mobile robot, whose dynamics can be represented by the following nonlinear discrete-time system:

xk+1= f(xk,uk), (1)

where xk∈ Rn and uk ∈ Rm denote the states and inputs, respectively. The robot can move within a workspace (e.g., the 2D plane when we consider ground robots). In the workspace, the robot must avoid collisions with dynamic obstacles. We model the collision region of the robotV_kat time k as the union of n_ccircles, and the collision region of the dynamic obstacles

Dv

kat time k as a single circle.

The position of dynamic obstacles along the planning horizon of the robot is uncertain. We denote the uncertainty of the obstacles at stage k with a tuple(Δk,Dk,Pk,real), where Δkis a probability space equipped with a σ-algebraDkand a probability measurePk,real. We allow the probability spaces to be unbounded and non Gaussian. We assume that at each step a perception module provides the motion planner with an independent model of the uncertainty, formalized as follows.

Assumption 1: The planner is provided with a modelPk of the real probability measureP_k,realfor each k.

Assumption 2: Random variables δ_j∼ P_j and δ_l∼ P_l are independent for all stages j, l∈ {1, . . . , N}, where j = l.

Assumption 2 implies that the dependency induced, for exam-ple, by the dynamics of an obstacle, is handled by the perception module such that the uncertainties are independent as viewed from the perspective of the motion planner. The assumption is common in state-of-the-art perception modules, for exam-ple [17]–[19].

Under the, possibly unbounded, uncertainty of the dynamic obstacles, we constrain the marginal probability of collision at each time step of the trajectory using chance constraints, simi-larly to [4], [5]. Each chance constraint is subject to an acceptable risk level k, which can be tuned accordingly. This implies that we cannot give a non-conservative bound on the collision risk of the full motion plan. However, by frequently recomputing the motion plan, for example in an MPC framework, the actions in the near future are probabilistically safe and risk in later stages is reconsidered when the robot moves closer. We formulate the

Fig. 2. Our approach exemplified for one robot’s disc and one dynamic obstacle for a single stage. The robot and the obstacle are drawn in blue and red, respectively. Fig. 2(a) shows the 1σ to 3 σ interval of the uncertainty in red shades. Fig. 2(b) shows the probabilistic collision region when linearized from the robot disc at the front. Fig. 2(c) shows the sampled locations in red and boundaries of the constraints in black. Fig. 2(d) shows the resulting minimal polytope in blue.

motion planning problem as follows: min u∈U N  k=1 J(xk,uk) (2a) s.t. xk+1= f(xk,uk), x ∈ X (2b) Pk  ||xd k− δvk||2> r,∀d, v  ≥ 1 − k, ∀k, (2c) whereu = {u₁, . . . ,uN} ∈ U are the optimized system inputs subject to input constraints,δv_k ∈ Δv

k is the uncertain position of obstacle v at stage k and J(xk,uk) ≥ 0 is the cost function specifying performance metrics. The radius r is the sum of vehi-cle and obstavehi-cle radii. To simplify the notation, we assume this radius to be a constant. The chance constraint, (2c), constrains the probability of collisions between each collision circle d of the vehicle and the collision circle of each dynamic obstacle v at prediction step k to be below the risk level k, as visualized in Fig. 2(a). The probability measurePk refers to the modeled uncertainty.

Problem (2) is a chance constrained optimization problem. As discussed in Section IV, to solve this problem, we rely on the nonconvex scenario optimization (NSO) framework of [7], for which we provide an overview in the following section. This framework can in general provide a bound on the risk with respect to the unknown probability distributionPreal, by sampling from the real system. In the real-time setting of this letter, however, collecting samples online is intractable. Instead we propose to sample from the model distributionP, as defined in Assumption 1. For consistency of notation, the results of [7] are presented here using the modelP.

III. NONCONVEXSCENARIOOPTIMIZATION

The NSO framework allows us to replace chance constraints with deterministic constraints by sampling. Consider the Chance Constrained Problem (CCP)

min

u∈U J(u) (3a)

s.t. P [g(u, δ) ≤ 0] ≥ 1 − , δ ∈ Δ, (3b) where u are decision variables, δ ∈ Δ is the realization of the uncertainty and the function g: X × Δ → R is a nonlinear function associated with the nonconvex constraint g(x, δ) ≤ 0.

The authors of [7] established a link between CCP (3) and the deterministic Scenario Program (SP):

min

u∈U J(u) (4a)

s.t. g(u, δi) ≤ 0, δi ∈ Δ, ∀i ∈ S. (4b) We denote its solution by u∗_SP. Each of the S constraints in (4b) is constructed by drawing a sample δi from Δ, and formulating the constraint g(u, δi_{) ≤ 0 in the scenario where} the sampleδi is a realization of the uncertainty. Since each of the samples specifies a scenario, the samples themselves are called scenarios and the constraints (4b) are known as scenario

constraints. The violation probability, V : U → [0, 1], given by V(u) := P [δ ∈ Δ : g(u, δ) > 0] , (5) defines the probability that inputu violates a newly observed scenario. The solution of the SP in (4) depends on randomly sampled scenarios and hence its violation probability is a random variable over the product probability measure, given byPS₌ P × · · · × P (S times). To link the SP of (4) with the CCP of (3), we are therefore interested in bounding the probability that

V(u∗_SP) satisfies our risk bound , a probability which we refer

to as the confidence. A key definition in this direction is the

support subsample.

Definition [7]: A support subsample of an SP is a subset of

scenariosSsupport ⊆ S that results in the same optimizer as the original SP. The cardinality of the support subsample, that is, the support subsample size, is denoted by s. The smallest support subsample size is denoted by s∗.

Theorem 1 in [7] provides the following confidence bound PS_{[V (u}∗ SP) > (s∗)] ≤ S−1_ s=0  S s  [1 − (s)]S−s_{= β. (6)} Here (s) : {0, . . . , S} → [0, 1] can be designed subject to (6) and (S) = 1, an example can be found in [7, Sec. II]. Equation (6) theoretically links the sampling size S, confidence parameter

β (complement of the confidence) and risk , based on the

observed support sample size. Notice that in this work, as a consequence of using model distributionP, the bound (6) applies to the modeled uncertainty rather than the real robot, in contrast with [7, Th. 1].

5392 IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 6, NO. 3, JULY 2021

IV. PROPOSEDAPPROACH

Our method relies on the Model Predictive Contouring Con-trol (MPCC) framework [8] to define the objective to optimize to plan a suitable path for the robots. Our method differs from [8] in the way we deal with dynamic obstacles, as detailed in the rest of the section. As such we will refer to our approach as Scenario-MPCC (S-MPCC). To present the method, we consider a single dynamic obstacle and one of the discs used to represent the vehicle.1

A. Chance Constraints Linearized in the Robot Position

Chance constraints (2c) are nonconvex in the robot position when sampled (see discs in Fig. 2(c)) and the associated SP may have many local optima and a sizable support subsample. We therefore consider a linearization of the collision regions (depicted by the lines in Fig. 2(c)) before sampling to decrease the support subsample size of the SP. This step reduces the risk of its solution significantly. We modify the constraints as

Ak = δk− ˆxk ||δk− ˆxk|| , b_k = A_kT(δ_k− A_kr), (7a) Pk  AT kxk≤ bk  ≥ 1 − k,∀k, δk ∈ Δk, (7b) where we linearize the collision region with respect toˆx_k, the

k-step ahead prediction of the robot position. We employ the

trajectory of the previous planning cycle, forward propagated, as predictor. That is,2_ˆx

t|k= xt−1|k+1andˆxt|N = xt−1|N. Hence, we search for collision-free solutions around the planned trajec-tory of the previous planning cycle. We show in Section IV-C that after linearization, the free-space of the resulting SP is convex in the robot position. A comparison between chance constraints (2c) and (7b) for an example is provided in Fig. 1. The linearized chance constraints capture less of the shape of the distribution, but are accurate nearˆxkand thus sufficient for motion planning. Note that the linearizations are performed for each stage of the trajectory, as illustrated in Fig. 2(b).

B. Scenario Program

For each of the chance constraints in (7b) we construct a set of deterministic constraints by sampling from the uncertainty. The red circles in Fig. 2(c) represent these samples and the black lines are the scenarios (Section III). The resulting SP is given by min u∈U N  k=1 J(xk,uk) (8a) s.t. xk+1= f(xk,uk), x ∈ X (8b) AT k(δik,ˆxk)xk≤bk(δik,ˆxk), ∀i ∈ Sk, ∀k. (8c) The theoretic properties of SPs, discussed in Section III, are limited to CCPs with one chance constraint. However, (7b) 1_{Section IV-D shows how this case extends linearly to multiple dynamic}

obstacles and multiple discs.

2_{We denote byx}

t|kthek-step ahead prediction of the robot trajectory for the MPC planning cycle at timet

describes multiple chance constraints, one for every stage of the planned trajectory. We now show that multiple chance con-straints can be handled separately, resulting in a probabilistic feasibility property per stage.

Theorem 1: Under Assumption 2, the probability that the

solution of SP (8) violates its associated chance constraint at stage k, satisfies PSk k [Vk(u∗SP) > k(s∗k)] ≤ βk(Sk), (9) where β_k(S_k) := Sk−1 s=0  S_k s  [1 − k(s)]Sk−s. (10)

Proof of Th. 1: The proof follows along the lines of the convex

proof [5, Th. 4.1]. In the following, we derive the result for

k= 1. The proof is analogous for all other k. We use the notation

ωk= {δ1k, . . . ,δSkk} to denote the collection of all samples per stage. Consider the complement of the confidence of the first stage, when the samples of all other stages have been drawn,

PS1

1 [V1(u∗SP(ω1)) > 1(s1∗) | ω2, . . .ωN], ω1∈ ΔS1. (11) Under Assumption 2, the samplesω₁are drawn independently from the samples ω₂, . . . ,ω_N. Moreover, sinceω₂, . . . ,ω_N

have been observed, we can merge their respective constraints into the feasible set

˜X2:N ₌N k=2

{xk| g(xk,ωk) ≤ 0}. (12) This results in the following modified optimisation problem

min u∈U N  k=1 J(xk,uk) (13a) s.t. x_k+1= f(x_k,u_k), x ∈ X (13b) P1[g(x1,δ1) ≤ 0] ≥ 1 − 1 (13c) x2:N∈ ˜X2:N. (13d)

This problem is a nonconvex CCP of the form (3) with one chance constraint, hence we can apply (6), which shows that the confidence of the first stage satisfies the proposed theorem for

k= 1 and analogous derivations apply for k = 2, . . . , N. Even

though constraints (13d) are deterministic, the solution to the optimization problem has not changed compared to SP (8). We therefore conclude that the result holds. 

C. Probabilistic Safety Guarantees

The key insight that makes our approach tractable is that due to the geometric structure of the problem, the free space may be described by only a small subset of the scenarios. To see this, first note that each scenario constraint in (8c) defines a half-space. The collision-free space, if it exists, is formed by the intersection of half-spaces and is convex, as i) each half-space is convex and ii) the intersection of convex constraints is convex. This results in a free space polytopeP_k (see Fig. 2(d)), spanned by those half-spaces that form the boundary of the polytope. We may

Algorithm 1: S-MPCC.

1: Compute Skfrom k,¯s, for all k 2: for allt= 1, 2, . . . do

3: Δt

k← Retrieve uncertainty from perception module 4: for allk= 1, . . . , N do

5: Sampleδi_k ∈ Δt

k, i= {1, . . . , Sk} 6: ComputeAi_k, bi

kfrom (7a) for all samples 7: FindHkand verify|Hk| ≤ ¯s

8: end for

9: ut← Solve (14) 10: Output:u_t|1 11: end for

define this subset of half-spaces by their indices as

Hk:= {i | ∃xk∈ Pk,ATk(δik,ˆxk)xk = bk(δik,ˆxk)}. The usefulness of the setH_k is twofold. First, we may replace (8c) with only those half-spaces that span polytopePk, greatly reducing the size of the online optimization problem. Second, the setHkcontains indices of the constraints that may be active during optimization and hence the support subsample is bounded by its cardinality, that is, s∗_k ≤ |Hk|. We use the latter fact to establish the link between the CCP subject to (7b) and SP (8). There always exists an upper bound,¯s, for the cardinality of Hk and for our problem we find experimentally that this upper bound ¯s is much smaller than the sample size. That is, for uncertainty distributions where the samples are not cluttered at the boundary, only few scenarios are active.

We can now compute sampling size S_k offline, using (i) Theorem 1, (ii) upper bound¯s, (iii) confidence parameter β_k, and (iv) risk _k. The SP we solve online is given by:

min u∈U N  k=1 J(x_k,u_k) (14a) s.t. xk+1= f(xk,uk), x ∈ X (14b) AT k(δik,ˆxk)xk≤ bk(δik,ˆxk), i ∈ Hk. (14c) Algorithm 1 summarizes our method. Online, we sample from the distribution and identify the minimal polytope and the support subsample size (line 4-9). We then solve optimization problem (14) (line 10) and use the first input as control input (line 11). In the following we provide a result for improving performance by discarding outlier scenarios.

Theorem 2: Consider solving the CCP (3) using the SP (4),

where after sampling, part of the scenarios are discarded. Sup-pose that we have a discarding algorithmR that removes R of the S scenarios, leaving P = S − R scenarios to be considered for the optimization. Let (s) be a function such that (P ) = 1 and β(S, P ) =  S P P −1 s=0  P s  [1 − (s)]P −s_.

Then the probability that the solution of the SP (4) is infeasible for the original CCP (3) satisfies the upper bound

PS_{[V (u}∗

SP) > (s∗)] ≤ β(S, P ). (15)

Proof: Consider the partitioning of the probability space:

ΔS Ip= {δ

S _{∈ Δ}S_{| R(δ}S_{) = I}

p}, (16)

The setsΔS

Ip are events where the picking algorithm selected the indicesI_p. Define the set where the risk bound is violated

BIp= {δS|R(δS) = Ip, V(u∗Ip)>(s∗Ip)}. (17) Notice that the last condition is upper bounded by (6) with

S = P . But the distribution of the samples is biased due to the

samples that were removed from the iid sample set. We obtain the following bound on the biased sample set

PS_[B Ip] ≤ P −1 s=0  P s  [1 − (s)]P −s_{. (18)} This result holds for all index sets which also contains all the possible biases introduced byR. Hence, the upper bound

PS_{[B] = P}S Ip

BIp

≤ β(S, P ), (19) is attained by independence of the samples. 

Remark 1: Bound (19) is conservative. For example if we

pick a random discarding algorithm forR, then the samples are still iid and we can use (6) directly with S = P , giving

P −1_ s=0  P s  [1 − (s)]P −s_{= β,}

which is generally much tighter than (19). However, even if the bound is conservative we can use it to remove extreme scenarios, leading to generally better performance.

D. Multiple Dynamic Obstacles and Discs

To apply the strategy above to more than one obstacle, we use the fact that scenario optimization is distribution agnostic. We combine the predictions of the obstacles into a probability space Δk= [Δ0k . . . ΔVk]T, where samples are denoted

δk= [δ0k . . . δVk]T. Although the stacked distribution δk could be used to model the correlation between the movement of obstacles, we will sample each component separately from individual probability distributions. The chance constraints (7b) need to include all obstacles and are modified as follows Pk  AT k(δvk,ˆxk)xk≤ bk(δvk,ˆxk), ∀v  ≥ 1 − k,δk ∈ Δk,∀k. The rest of the method follows analogously to the single obstacle approach but where the scenarios are drawn for each obstacle, resulting in more scenarios to process before obtaining the free space polytope. In the case of multiple vehicle discs, we formulate multiple chance constraints of the form (2c), one for each collision disc. We apply the method described in this Section per disc as samples for each of the discs are independent.

5394 IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 6, NO. 3, JULY 2021

Algorithm 2: Detailed S-MPCC for (Truncated) Gaussian. 1: Determine Skfrom k, βk,¯s, R 2: ui_{← U × U, ∀i = {1, . . . , S} k} (uniform random) 3: zi 0=  −2 ln ui

1cos (2πui2), ∀i = {1, . . . , Sk} 4: zi

1= 

−2 ln ui

1sin (2πui2), ∀i = {1, . . . , Sk} 5: Verify relevance of samplesz, prune irrelevant 6: for allt= 1, 2, . . . do

7: for allk= 1, . . . , N do

8: δi_k← (20)

9: ˆδl_k← apply R to closest R + l scenarios in δi_k 10: P_k← intersection algorithm on H(ˆδl_k)Hrange_k 11: end for

12: end for

V. S-MPCCWITHGAUSSIANUNCERTAINTIES

A common class of uncertainties are the (truncated) Gaussian uncertainties. This section presents a detailed formulation of Algorithm 1, namely Algorithm 2, one can use in the case of (truncated) Gaussian uncertainty.

The first step of Algorithm 2 is to determine the sample size. We set k= 1 − 0.9889, equivalent to the probability mass un-der the3 σ interval of a bivariate Gaussian (generally considered as safe). Since the risk has logarithmic dependency on βk[7], βk is generally small. We pick βk = 1 · 10−6, i.e., one in a million SPs may not be feasible for the original CCP3_{. The removal} size R= 50 is empirically determined, verifying that outliers are removed. Upper bound¯s is guessed and increased until it is never exceeded in practice. We find¯s = 20. Evaluating (19), we are able to pick Sk ≈ 53050 (line 1). We note that the main dependency of the sample size is the acceptable risk _k. Sampling more scenarios results in a higher probability of safety, but at the cost of more conservative trajectories and increased computation times.

Instead of online sampling, we may sample a set of parameter-ized samples offline followed by an online transformation. This reduces the online operations, resulting in lower computation times. We describe this approach for the (truncated) Gaussian case. We generate offline a number of batches with Skbivariate Gaussian samples, centered at the origin and withΣ = I, where

I is the identity matrix (line 2-4). These samples are obtained

using the Box-Muller Transformation (BMT) [20], which also allows us to draw radially truncated Gaussian samples by simply changing the support domain of u₁to[e−r22 , 1] [21]. Most of

the samples will be in the center of the distribution and will not be relevant online. Hence, we run our online algorithm for scenario selection (explained later), offline and aggregate the set of selected scenarios. Scenarios that are not in this set are pruned offline (line 5). In the3 σ example, approximately 95% of the scenarios are removed offline.

Online, we are only required to transform the offline samples from the standard bivariate normal distribution to the estimated 3_{Note that the designer can choose to keep safety margin in the obstacle radius}

such that a failure does not have to result in a collision.

mean and variance of the uncertainty (line 8), which is computed using

δi

k= ATkzik+ μk, ATkAk = Σk. (20) We select for each obstacle only one batch of samples. The obstacle predictions are sampled with that batch for all stages and all time steps. This provides the motion planner with consistent constraints. To further reduce the computational load, we search online only for the l+ R scenarios closest to considered vehicle position, where we use l= 150 in the following experiments, we assume that this set contains the support subsample. We then apply the discarding algorithm R, which removes the

R scenarios furthest from the mean of the distribution (line

9). We construct half-spaces from the remaining l scenario and add four half-spaces to constrain the vehicle in a square workspace. To find the minimal polygon in 2D from this set of half-spaces, we use an intersection based algorithm (line 10). The algorithm explores the intersections in the inner polygon in a counter-clockwise fashion. The lines traveled form the minimal polygon. In the following simulations and experiments, we incorporate our dynamic obstacle avoidance method in the MPCC framework [8]. We introduce a cost term that activates when the robot gets close to the boundaries of the free space polygon, to penalize movement close to pedestrians.

VI. RESULTS

In this section, we present simulation and real-world results for a mobile robot navigating among pedestrians. Moreover, we present a qualitative analysis and performance results of our method against two baselines: MPCC [8] and Collision Avoidance with Deep RL (CADRL) [22].

A. Experimental Settings

Our experimental platform is the Clearpath Jackal robot equipped with an Intel i5 CPU@2.6 GHz. For the robot and pedestrian’s localization we have used the OptiTrack sys-tem [23]. Our simulations use the open-source ROS implemen-tation of the Jackal Gazebo for the robot simulation and Social Forces model [24] for pedestrian simulation.

To solve SP (14), we use the ForcesPro [25] solver. The robot dynamics are described by a continuous-time second-order unicycle model [26]. The model is discretized with steps of 200 ms. The time horizon is set to 3 seconds divided into 15 stages. The sampling period for control is 50 ms.

B. Simulation Results

We compare the proposed method against two methods for Gaussian uncertainties. The first is a baseline MPCC ap-proach [8] in which the ellipses used to represent the obstacles are obtained from the level sets of a known Gaussian distribution of the uncertainties. For comparison, we use the same tuning for both approaches (the interested reader can refer to [8] for details on the definition of the cost function and general constraints). The main difference between the two approaches is the handling of dynamic obstacles (i.e., ellipsoidal level sets vs. scenario

Fig. 3. Simulations using our S-MPCC with 6 crossing pedestrians for 3 types of uncertainties. The top row visualizes the robot (blue) and pedestrian (red) trajectories, where newer positions are depicted with lighter shades. The bottom row visualizes the free space and active samples at stages 1, 8 and 15 in red, orange and yellow. All samples considered online are shown in black. The robot’s current and predicted occupied area are denoted in black and blue, respectively.

TABLE I

STATISTICRESULTS OF THEPROBABILITY OFCOLLISIONWITHRESPECT TO THEESTIMATEDUNCERTAINTY FOR THEFIRSTSTAGE(EVALUATEDUSINGMONTE

CARLOSAMPLING)ANDVIOLATIONS OF THESPECIFIEDRISK,THETASKCOMPLETIONTIME AND THECOMPUTATIONTIMES. THERESULTSARECOLLECTED

FROM100 SIMULATIONS OF ACROSSINGSCENARIO FORn ∈ {2, 4, 6} PEDESTRIANS

constraints). The second method for comparison is CADRL [22]. We use the open source ROS implementation in the following simulations. Similar to MPCC we employ ellipsoidal level sets as the collision region of the obstacles.

The simulation environment consists of a straight road where pedestrians are crossing freely, as depicted in Fig. 3. The robot objective is to follow the centerline of the road. We evaluate our method for 2, 4 and 6 pedestrians. The uncertainty of the pedestrian predictions is Gaussian with a variance ofΣ = 0.12I. We set a pedestrian radius of zero. Fig. 3(a) depicts one simu-lation of S-MPCC with 6 pedestrians. Aggregated results over 100 simulations are presented in Table I. In all tested cases, collisions are prevented by S-MPCC, while additionally the risk, evaluated over the perceived uncertainty, remains below the specified3 σ threshold. The MPCC method frequently switches between locally optimal trajectories resulting in collisions when it becomes infeasible. CADRL is reactive, which in the simu-lated environment leads it to positions where collisions may not be avoided. This behavior becomes worse with more obstacles. Interestingly, we find that S-MPCC results in smoother trajec-tories than both methods which results in earlier arrival at the goal. The downside is that the computation time of our method is higher. The computation time may be decreased by considering only the pedestrians close to the estimate ˆxk. We repeated the simulation with 6 pedestrians in this case. The computation time was reduced to 6.86 ms mean and 40.94 ms maximum.

Evaluation of S-MPCC for non Gaussian uncertainties is depicted in Fig. 3. Here, the previous Gaussian predictions are radially truncated at 3.5 σ (Fig. 3(b)) and truncated in their width at 2.5 σ (Fig. 3(c)). In this scenario, width truncated uncertainties incorporate the domain knowledge that pedestrians

are expected to cross at a crosswalk. Level set based approaches are not applicable in this case, as the geometry of the level sets depends on the specified risk threshold. We adapt the pedestrian locations to simulate a crosswalk. In contrast to the previous simulations, we specify an obstacle radius of 0.3 m and a variance ofΣ = 0.082I. We evaluate the probability of collision in the first stage, with respect to the estimated uncertainty over 100 tests using Monte Carlo sampling. We find a maximum risk of 0.00 305 for radial truncation and 0.02 038 for width truncation. The violation of our method in the case of width truncation corresponds to a single case where the horizon is not long enough to correctly assess the risk of the full task a priori. This leads the robot to a state where our method cannot find a trajectory that satisfies the risk bound along the horizon and the optimization becomes infeasible. By increasing the horizon, the risk can be anticipated earlier, improving feasibility at the cost of larger computation times. The maximum risk over the other simulations was at most 0.0070.

C. Real-World Results

We evaluated our method on real navigation situations with pedestrians. In the experiment, the robot navigates on a road following the lane central line when two pedestrians cross the robot’s path. We modeled the noise on the pedestrian predictions as Gaussian distributions truncated at 3.5 σ. Fig. 4 provides snapshots of one experiment.4

5396 IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 6, NO. 3, JULY 2021

Fig. 4. Experimental results with the robot avoiding two crossing pedestrians. The orange circles depict the robot’s plan, while the blue and green circles the pedestrians’ (constant velocity) predictions. The solid black lines depict the road boundaries.

VII. CONCLUSIONS ANDFUTUREWORK

In this letter we presented a Scenario-based Model Predic-tive Contouring Control (S-MPCC) method for mobile robot motion planning in the presence of dynamic obstacles with arbitrary position distributions. The main idea was to pursue a scenario-based method (translating probabilistic constraints into deterministic ones), generating scenarios from a model of the uncertainty. By using geometry considerations we were able to prune the possible outcomes (scenarios), while providing a bound on the marginal risk with respect to the modeled probability distribution. We demonstrated in simulations that the proposed method outperformed two recent baselines, in the sense that it generated trajectories that were significantly safer and more efficient. This came at a higher processing cost, but the method is still real-time capable. We furthermore illustrated the proposed method in a real-world experiment with a moving robot platform navigating among pedestrians. To further reduce the uncertainties and improve the navigation of the robot, incor-porating the interactions between robot and pedestrians would be useful. The risk bounds that our method provides on the modeled uncertainty can still be improved by alleviating the standing assumption that requires our uncertainty models per stage to be independent. Additionally, the risk bound on the planned trajectory is relatively conservative. A tighter bound can be useful for planning safer long term motion, especially when the robot dynamics are slow. Alleviating these limitations are part of our future work.

REFERENCES

[1] M. Simon, Inside Amazon Warehouse Where Hum. Machines Become One, 2019. [Online]. Available: https://www.wired.com/story/amazon-warehouse-robots/

[2] J. Walker, Self-Driving Car Timeline - Predictions from Top 11 Global Automakers, 2019. [Online]. Available: https://emerj.com/ai-adoption-timelines/self-driving-car-timeline-themselves-top-11-automakers/ [3] MI News Network, 7 Major Developments Auton. Shipping in 2018,

2018. [Online]. Available: https://www.marineinsight.com/know-more/7-major-developments-in-autonomous-shipping-in-2018/

[4] H. Zhu and J. Alonso-Mora, “Chance-constrained collision avoidance for MAVs in dynamic environments,” IEEE Robot. Automat. Lett., vol. 4, no. 2, pp. 776–783, Apr. 2019.

[5] G. Schildbach, L. Fagiano, and M. Morari, “Randomized solutions to convex programs with multiple chance constraints,” SIAM J. Optim., vol. 23, no. 4, pp. 2479–2501, Jan. 2013.

[6] L. Blackmore, M. Ono, A. Bektassov, and B. C. Williams, “A probabilistic particle-control approximation of chance-constrained stochastic predictive control,” IEEE Trans. Robot., vol. 26, no. 3, pp. 502–517, Jun. 2010.

[7] M. C. Campi, S. Garatti, and F. A. Ramponi, “A general scenario theory for nonconvex optimization and decision making,” IEEE Trans. Autom. Control, vol. 63, no. 12, pp. 4067–4078, Dec. 2018.

[8] B. Brito, B. Floor, L. Ferranti, and J. Alonso-Mora, “Model predictive contouring control for collision avoidance in unstructured dynamic en-vironments,” IEEE Robot. Automat. Lett., vol. 4, no. 4, pp. 4459–4466, Oct. 2019.

[9] L. Ferranti et al., “SafeVRU: A research platform for the interaction of self-driving vehicles with vulnerable road users,” in Proc. IEEE Intell. Veh., 2019, pp. 1660–1666.

[10] K. Berntorp, P. Inani, R. Quirynen, and S. Di Cairano, “Motion planning of autonomous road vehicles by particle filtering: Implementation and validation,” in Proc. Amer. Control Conf., Jul. 2019, pp. 1382–1387. [11] A. Wang, A. Jasour, and B. C. Williams, “Non-Gaussian

chance-constrained trajectory planning for autonomous vehicles under agent uncertainty,” IEEE Robot. Automat. Lett., vol. 5, no. 4, pp. 6041–6048, Oct. 2020.

[12] G. Calafiore and M. Campi, “The scenario approach to robust con-trol design,” IEEE Trans. Autom. Concon-trol, vol. 51, no. 5, pp. 742–753, May 2006.

[13] M. C. Campi and S. Garatti, “The exact feasibility of randomized so-lutions of uncertain convex programs,” SIAM J. Optim., vol. 19, no. 3, pp. 1211–1230, Jan. 2008.

[14] G. C. Calafiore, “Random convex programs,” SIAM J. Optim., vol. 20, no. 6, pp. 3427–3464, Jan. 2010.

[15] M. C. Campi and S. Garatti, “A sampling-and-discarding approach to chance-constrained optimization: Feasibility and optimality,” J. Optim. Theory Appl., vol. 148, no. 2, pp. 257–280, Feb. 2011.

[16] G. Schildbach, L. Fagiano, C. Frei, and M. Morari, “The scenario ap-proach for stochastic model predictive control with bounds on closed-loop constraint violations,” Automatica, vol. 50, no. 12, pp. 3009–3018, Dec. 2014.

[17] Y. Chai, B. Sapp, M. Bansal, and D. Anguelov, “MultiPath: Multiple prob-abilistic anchor trajectory hypotheses for behavior prediction,” Oct. 2019. [Online]. Available: http://arxiv.org/abs/1910.05449

[18] N. Deo and M. M. Trivedi, “Multi-modal trajectory prediction of sur-rounding vehicles with maneuver based LSTMs,” in Proc. IEEE Intell. Veh., Jun. 2018, pp. 1179–1184, iSSN: 1931–0587.

[19] J. F. P. Kooij, F. Flohr, E. A. I. Pool, and D. M. Gavrila, “Context-based path prediction for targets with switching dynamics,” Int. J. Comput. Vis., vol. 127, no. 3, pp. 239–262, Mar. 2019.

[20] M. E. Muller and G. E. P. Box, “A note on the generation of random numbers,” Ann. Math. Stat., vol. 29, pp. 610–11, 1958.

[21] L. Martinet, D. Luengo, and J. Miguez, “Efficient sampling from truncated bivariate Gaussians via the box-muller transformation,” Electron. Lett., vol. 48, pp. 1533–1534, 2012.

[22] M. Everett, Y. F. Chen, and J. P. How, “Motion planning among dynamic, decision-making agents with deep reinforcement learn-ing,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Oct. 2018, pp. 3052–3059.

[23] NaturalPoint, “Optitrack,” Natural Point, Inc, 2011. [Online]. Available: https://optitrack.com/

[24] D. Helbing and P. Molnar, “Social force model for pedestrian dynamics,” Phys. Rev. E, vol. 51, no. 5, 1995, Art. no. 4282, publisher: APS. [25] A. Domahidi and J. Jerez, “Forces professional,” Jul. 2014. [Online].

Available: https://embotech.com/FORCES-Pro

[26] R. Siegwart and I. R. Nourbakhsh, Introduction to Autonomous Mobile Robots, 2nd ed. The MIT Press, 2011.