Scenario-based motion planning with bounded probability of collision

Abstract

Robots will increasingly operate near humans that introduce uncertainties in the motion planning problem due to their complex nature. Optimization-based planners typically avoid humans through collision avoidance chance constraints. This allows the planner to optimize performance while guaranteeing probabilistic safety. However, existing real-time methods do not consider the actual probability of collision for the planned trajectory but rather its marginalization, that is, the independent collision probabilities for each planning step and/or dynamic obstacle, resulting in conservative trajectories. To address this issue, we introduce a novel real-time capable method termed Safe Horizon MPC that explicitly constrains the joint probability of collision with all obstacles over the duration of the motion plan. This is achieved by reformulating the chance-constrained planning problem using scenario optimization and predictive control. Out of sampled realizations of human motion, we identify which cases affect the optimization. This allows us to certify the planned trajectory in real-time. Our method is less conservative than state-of-the-art approaches, applicable to arbitrary probability distributions of the obstacles’ trajectories, computationally tractable and scalable. We demonstrate our proposed approach using a mobile robot and an autonomous vehicle in an environment shared with humans.

1. Introduction

Mobile robots can improve our quality of life, from transporting goods in warehouses (Simon, 2019) to helping us commute more efficiently and safely using self-driving vehicles (Walker, 2019). In most applications where robots are currently deployed, the operating domain does not allow the robots to operate near humans or they do so in a conservative manner to simplify the robot navigation task. To deploy mobile robots in real-world environments, such as our cities, they need to be capable of navigating among humans, which remains challenging.

In crowded environments, the robot needs to understand and infer the motion of humans in order to move safely and efficiently. Unfortunately, human behavior is hard to predict and varies per person. In addition, human intentions cannot be explicitly communicated to the robot. This inherently makes the motion prediction of humans nondeterministic. Recent perception methods, such as Kooij et al. (2019), Salzmann et al. (2021), and Yue et al. (2023), infer human intentions, returning probabilistic information concerning humans. These probabilistic predictions need to be considered and exploited by the motion planner.

If we look at the motion planning problem from a probabilistic perspective, the occurrence of a collision is a probabilistic event. Our goal is to assess and bound the Collision Probability (CP) of the planned trajectory. This is challenging because of the non-Gaussian and multi-modal, that is, multiple paths are possible, distributions involved when predicting human behavior. In addition, when the collision probability spans a duration, for example, over the planned trajectory, then it must consider the correlation over time. The time correlation exists because the first collision in a trajectory renders it unsafe, such that all subsequent collisions can be ignored. Almost all of previous works, such as Blackmore et al. (2006), Luders et al. (2010), van Den Berg et al. (2011), Zhu and Alonso-Mora (2019), Wang et al. (2020a, b), and De Groot et al. (2021), do not account for this correlation, which leads to conservative trajectories. Similarly, the collision probability in these works is computed per obstacle, which degrades performance when more obstacles influence the plan, that is, in crowded environments. By accounting for the correlation in time and by considering all obstacles, one would be able to plan more efficient trajectories without compromising safety.

We achieve this goal by devising a novel probabilistic safe motion planner based on scenario optimization and nonlinear Model Predictive Control (MPC). Nonconvex scenario optimization (Campi et al., 2018) is a sampling-based approach for assessing the probability that a constraint holds under uncertainty. In the context of this paper, the imposed constraint probabilistically enforces collision avoidance between the controlled robot and the dynamic obstacles over the duration of the motion plan (see Figure 1). Scenario optimization has found limited applications in robotics. Its a posteriori safety verification is computationally demanding and it cannot directly be applied to distributions with unbounded support (e.g., Gaussians). We address these limitations in this work for our target application of motion planning in dynamic 2D environments, enabling its use in similar robotics applications.

Figure 1.

Overview of the proposed scenario-based motion planner. Predicted obstacle trajectories are sampled to obtain scenarios, where each scenario represents a trajectory for all obstacles over the planning horizon. By ensuring safety for all scenarios, probabilistic safety of the motion plan is guaranteed.

In practice, the uncertainty associated with the motion of detected obstacles is predicted forward in time (Step 1). We sample scenarios from these predictions that describe the trajectories of all dynamic obstacles during the planning horizon (Step 2) and construct collision avoidance constraints around each of the scenarios (Step 3). The robot trajectory is optimized with respect to the constraints (Step 4), which provides probabilistic collision avoidance. This is, to our knowledge, the first real-time capable method that bounds the CP of the planned trajectory jointly, in contrast with the existing state-of-the-art where the same quantity is conservatively approximated through its marginals. Our method therefore provides more consistent real-time guarantees on the CP, irrespective of the number of time steps and obstacles, and the underlying probability distribution that describes obstacle motion. This ultimately enables our method to trade-off safety and planning performance more accurately. We refer to our novel probabilistic safe motion planner as Safe Horizon MPC (SH-MPC).

2. Related work and contribution

A sizable body of literature exists on motion planning for safe, autonomous navigation. In this section, we review some of the existing work on trajectory optimization with collision avoidance under uncertainty. For a general overview of autonomous navigation, we refer the reader to Paden et al. (2016) and Schwarting et al. (2018b).

2.1. Collision avoidance under uncertainty

An important problem in autonomous navigation is to prevent collisions with dynamic obstacles. In trajectory optimization (Brito et al., 2019; Schwarting et al., 2018a), the navigation problem is formulated as an optimization problem where performance criteria are optimized (e.g., lane following and progress towards the goal) under constraints (e.g., collision avoidance and actuator limits). Due to large and multi-modal uncertainties in human motion prediction, collision avoidance in the mean or nominal case (e.g., constant velocity) may lead to collisions in practice. Many works therefore consider how to address this uncertainty.

One can consider the collision avoidance problem as a special case of optimization under uncertainty, for which two common approaches exist. Robust optimization (Ben-Tal and Nemirovski, 1998) requires the constraints to be satisfied for all possible realizations of the uncertainty, while stochastic optimization (see Mesbah (2016) for an overview) allows for a violation of the constraints, as long this happens with a probability smaller than an upper bound ϵ. Because the set of all possible realizations is often not available or too conservative, we focus in the remainder of this section on stochastic optimization. We refer to Kouvaritakis and Cannon (2016) for more details on both methods in the context of Model Predictive Control (MPC).

2.2. Collision-avoidance chance constraints

The probability of constraint violation in stochastic optimization is specified through chance constraints, which constrain the probability that a nominal constraint is satisfied. Exact evaluation of these chance constraints is however intractable in almost all applications. Existing works therefore focus on an approximation of the constraints. These can be divided into discrete-time and continuous-time methods. Discrete-time methods (e.g., Blackmore et al. (2006)) consider the CP at a fixed set of time intervals, usually at the discretization time of the robot dynamics. Continuous-time methods (e.g., Frey et al. (2020)) instead consider the CP over a continuous time interval, resulting in more accurate approximations but usually at a high computational cost. For our real-time application, we focus on the discrete-time methods. Existing discrete-time methods usually approximate chance constraints through additional assumptions on the probability distribution associated with the uncertainty, for example, by assuming Gaussian distributions in Zhu and Alonso-Mora (2019) and in Fisac et al. (2018) or through additional assumptions on the controlled robot, for example, by assuming linear dynamics in Blackmore et al. (2006). The recent works (Wang et al., 2020a; De Groot et al., 2021) have resolved many of the assumptions, making the framework applicable to nonlinear robot dynamics and arbitrary probability distributions. However, the chance constraints in these and many other works are not imposed on the robot’s planned trajectory, that is, the timed sequence of planned positions, but rather on each of its individual positions over the planning horizon. This fails to accurately bound the probability of colliding at any time. Similarly, chance constraints imposed per obstacle fail to estimate the probability of colliding with any obstacle. In this regard, three types of chance constraint formulations have been considered in previous work: Marginal, Conditional, and Joint.

2.2.1. Marginal chance constraints

Constraints on each position along the trajectory are referred to as marginal chance constraints. Let event A _k denote the case that no collisions occur at time k and $\mathbb{P}(A_k)$ therefore be the probability that the robot is safe at timestep k. Then the exact probability that a trajectory is safe over N steps is given by $\mathbb{P}(A)={\prod }_{k=1}^N$ $\mathbb{P}(A_k|A_{0:k-1})$ . That is, the CP for each position is conditional on the probability of avoiding collisions up until the position is reached at time k. The problem is simplified if we assume instead that this event is independent for all states ( $\tilde{\mathbb{P}}(A)\approx {\prod }_k\mathbb{P}(A_k)$ ). In Janson et al. (2018), these marginal methods are further divided into additive and multiplicative approaches.

Additive approaches impose constraints on each marginal probability (i.e., $\mathbb{P}(A_k)\le {ϵ}_k$ ). Using Boole’s inequality $(\mathbb{P}({\cup }_k{A}_k)\le {\sum }_k\mathbb{P}(A_k))$ the CP of the trajectory is bounded by the sum of the individual CPs. Under Gaussian uncertainty, Blackmore et al. (2006) reformulated the constraints as an analytical constraint on the 1D Cumulative Density Function (CDF). The same idea is used in Zhu and Alonso-Mora (2019) in an MPC framework to prevent collision between robot and obstacle volumes. In Masahiro Ono and Williams (2008), the bound on each marginal probability is updated, known as risk allocation, while maintaining the same total risk bound (i.e., ∑ _k ϵ _k = ϵ). In Luders et al. (2010) and Aoude et al. (2013), marginal chance constraints are applied to the Rapidly expanding Random Trees (RRT) algorithm such that each node in the tree is statistically safe. When the uncertainty is non Gaussian, the CDF of the probability distribution is typically not available. In Wang et al. (2020b, a), an MPC for motion planning is formulated where inequalities are posed on stochastic moments of the marginal probability distribution. A similar approach for linear dynamics is applied in Ren et al. (2023) where the conditional Variance-at-Risk (cVaR) is used to minimize constraint violation. In our previous work (De Groot et al., 2021), we ensure safety for a finite set of sampled chance constraints and generalize the associated safety properties using the scenario approach (Campi et al., 2018).

The multiplicative formulation explicitly constrains the product of the marginal probabilities. It was applied in van Den Berg et al. (2011) to plan motion under sensing and actuation uncertainty. An alternative marginal formulation is proposed in Fisac et al. (2018), which bounds the largest marginal constraint violation.

The limitation of marginal approaches is that the bound on the CP of the trajectory is inaccurate, as noted by Patil et al. (2012) and Janson et al. (2018). It is shown in Janson et al. (2018) that the trajectory CP approaches ∞ and 1 for the additive and multiplicative formulation, respectively, when the number of evaluations in the trajectory increases, regardless of the real CP. Marginal constraints only asses the risk correctly for the first time step and a single obstacle. The risk of the remainder is under- or overestimated. Overestimation of the risk and the associated unsafe space along the time horizon can cause the planning problem to become infeasible and may cause the robot to freeze. Due to these limitations, Patil et al. (2012) conditioned marginal chance constraints on being collision-free at prior times and evaluated them by truncating the part of the distribution in collision in each time instance. This formulation is more accurate but is limited to Gaussian distributions.

2.2.2. Joint chance constraint

Some authors formulate a joint chance constraint on the CP of the planned trajectory. Joint chance constraints estimate the open-loop risk over the time horizon more accurately, making it less likely that the problem becomes infeasible and improving performance. The joint CP can be evaluated by using sampling-based methods (Janson et al., 2018). In particular, Blackmore et al. (2010), Janson et al. (2018), and Schmerling and Pavone (2017) consider Importance Sampling Monte Carlo (ISMC) sampling to approximate the CP. An empirical estimate of the constraint violation is obtained by sampling the joint distribution over trajectories and evaluating the constraint for each sample. Since MC methods approximate the CP and its gradient with sampling, they can be computationally expensive to apply in trajectory optimization without further alterations. An alternative is to formulate a mixed-integer problem (Blackmore et al., 2010) to decide which samples may be violated, but these problems are hard to solve in real-time.

2.3. Scenario optimization

Rather than constraining each of the marginal CPs along the planned trajectory, we constrain the joint CP of the trajectory. We solve the resulting problem with Nonconvex Scenario Optimization (NSO) (Campi et al., 2018) under an explicit chance constraint on the joint CP. Scenario optimization is well established for convex optimization under uncertainty (Calafiore and Campi, 2006; Campi and Garatti, 2008, 2011; Schildbach et al., 2013) and has recently been extended to the general nonconvex case (Campi et al., 2018; Garatti and Campi, 2023). NSO solves the optimization problem under uncertainty by drawing a large set of samples from the uncertainty, that is, possible future motions of all obstacles, and formulating a deterministic constraint, for example, collision avoidance, for each sample. Our approach leverages a constraint formulation where many of the sampled constraints can be pruned, resulting in a deterministic program that can efficiently be solved online.

The NSO framework (Campi et al., 2018) cannot directly be applied to reformulate the joint collision avoidance chance constraint. To assess probabilistic safety, it traditionally needs to repeat the optimization problem once for each scenario and thus cannot be applied in real-time. In addition, unbounded distributions (e.g., Gaussian distributions) cannot be readily incorporated. We address these limitations in this work, including distributions with unbounded support (Sec. 5.1) and significantly reducing the computational complexity of the online safety assessment (Sec. 5.3) for our application to motion planning in dynamic environments. This allows us to apply the framework to online motion planning in real-time.

2.4. Contribution

The contributions of this paper are:

(1) A novel trajectory optimization method, Safe Horizon MPC, that explicitly constrains the collision probability over the full duration of the planned trajectory. This distinguishes our work from previous work, where the collision probability is constrained per planning time instance. The idea is that each sample we draw from the distribution of the uncertain obstacle trajectories corresponds to a single collision avoidance constraint over the horizon. We want to stress that each sample represents a possible trajectory for all obstacles. The probability of collision is controlled through the number of samples drawn. By relying on sampling, our planner is distribution agnostic.

(2) An approach that, under a convexity assumption on the iterations of the underlying optimization algorithm (that holds, for example, for Sequential Quadratic Programming), identifies the scenarios that hold the solution in place (known as the support) during optimization, in contrast with the general framework of Campi et al. (2018) where the support is computed after optimization. We leverage this information to certify the motion plan online.

We compare our approach in two simulation environments, on a mobile robot and a self-driving vehicle, with pedestrians against marginal baselines and show that our method achieves shorter task durations while maintaining the probabilistic safety specification. In addition, we show that the computation times scale well with respect to the number of obstacles and the type of distribution. Finally, we deploy our method on a mobile robot navigating among pedestrians. Our planner is implemented in C++/ROS and will be released open source.

2.5. Notation

Vectors and matrices are expressed in bold and capital bold notation, respectively. We use ⋀ to denote the “and” operation and ⋁ to denote the “or” operation. The notation $\mathcal{C}\backslash C_i$ refers to the set $\mathcal{C}$ from which the element C _i is removed.

3. Problem formulation

We consider a controlled robot with nonlinear discrete-time dynamics

${\mathit{x}}_{k+1}=f({\mathit{x}}_k,{\mathit{u}}_k),$

(1)

where ${\mathit{x}}_k\in {\mathbb{R}}^{n_x}$ and ${\mathit{u}}_k\in {\mathbb{R}}^{n_u}$ denote the states and inputs, respectively. The robot state is assumed to contain its x-y position $\mathit{p}=[x,y]\in {\mathbb{R}}^2\subseteq {\mathbb{R}}^{n_x}$ . Humans in the environment of the robot pose constraints on the navigation envelope. We model the robot area with the union of n _d discs and the area of each obstacle with a single disc. The position of the robot disc with index c at time step k is denoted ${\mathit{p}}_k^c$ . We model each obstacle region with a single disc for simplicity, our method trivially extends to the case of multiple discs. We consider that the motion prediction of the dynamic obstacles is uncertain by modeling the positions of at most M obstacles as random variables. In particular, we denote the uncertain position of Obstacle j at time step k as δ _k,j⊆ δ _k , where δ _k contains all obstacle positions at time k. The joint uncertainty $\mathit{\delta }={\left[{\mathit{\delta }}_1^{\top },\dots ,{\mathit{\delta }}_N^{\top }\right]}^{\top }\in \mathrm{\Delta }$ stacks the uncertainty over all N time steps. The probability space ¹ of the joint uncertainty $\mathrm{\Delta }={\mathbb{R}}^{\mathit{2MN}}$ therefore captures the future N positions of M obstacles and is endowed with a σ-algebra $\mathcal{D}$ and a probability measure $\mathbb{P}$ .

3.1. Chance constrained planning problem

The planning problem is visualized in Figure 2. The dynamic uncertainty of other road users affects the navigation envelope of the robot. To constrain the probability of a collision with any obstacle along the horizon, we formulate a single chance constraint for collision avoidance. This leads to the following Chance Constrained Problem (CCP)

Figure 2.

The chance constrained motion planning problem considered in this paper with the robot (yellow) navigating under probabilistic motion predictions of obstacles (blue/green). The distribution of the motion predictions can take any form but is visualized here with several modes and their variance (arrows/shaded regions).

Problem 1. CCP.

$\begin{array}{ll}\hfill {\min }_{\begin{array}{l}\mathit{u}\in \mathbb{U}\\ \mathit{x}\in \mathbb{X}\end{array}}\hfill & \sum _{k=0}^{N}J({\mathit{x}}_k,{\mathit{u}}_k)\hfill \end{array}$

(2a)

$\begin{array}{ll}\hfill \mathrm{s}.\mathrm{t}.& {\mathit{x}}_0={\mathit{x}}_{\text{init}}\hfill \end{array}$

(2b)

$\begin{array}{ll}\hfill & {\mathit{x}}_{k+1}=f({\mathit{x}}_k,{\mathit{u}}_k),k=0,\dots ,N-1\hfill \end{array}$

(2c)

$\begin{array}{ll}\hfill & \mathbb{P}\left[\underset{k=1}{\overset{N}{\bigwedge }}\underset{c=0}{\overset{n_d}{\bigwedge }}\underset{j=0}{\overset{M}{\bigwedge }}\left(\Vert {\mathit{p}}_k^c-{\mathit{\delta }}_{k,j}{\Vert }_2\ge r\right)\right]\ge 1-ϵ\hfill \end{array}$

(2d)

where x _init denotes the initial state of the robot, objective J is designed to achieve control objectives, $\mathbb{X}\subseteq {\mathbb{R}}^{N{n}_x}$ and $\mathbb{U}\subseteq {\mathbb{R}}^{N{n}_u}$ denote state and input constraints over the horizon, respectively, (2d) is the chance constraint for avoiding collisions in every time step with all obstacles and r is the summed radius of the robot and obstacle discs (for simplicity assumed the same for all obstacle and robot discs). This problem is in general nonconvex due to the nonlinear dynamics and collision avoidance chance constraint. Our goal is to compute a control input u and trajectory x under the uncertainty δ that is collision-free with a probability of at least 1 − ϵ, where ϵ denotes the maximum collision probability over the planned trajectory.

When $\mathbb{P}$ is estimated by a prediction model the collision avoidance constraint is formulated with respect to an estimate $\widehat{\mathbb{P}}$ of $\mathbb{P}$ and the chance constraint relates to the estimate of the probability distribution.

3.2. Scenario-based planning problem

Directly evaluating chance constraint (2d) is not computationally feasible in closed loop. Our goal is to formulate a sampled deterministic version of the CCP, known as a Scenario Program (SP) (Campi et al., 2018). The challenges for safe robot navigation within this framework are to determine the number of samples that must be drawn and, consequently, to identify the samples that affect the optimization.

3.3. Paper organization

In the following, we first consider a more general CCP formulation that can be solved by the proposed framework. We provide a brief summary of the nonconvex scenario optimization framework of Campi et al. (2018) to show how this general class of CCPs can be solved via its associated SP. We then present our main results in Sec. 5, which shows how this SP can be solved in closed loop as MPC. Finally, we apply our main results to probabilistic motion planning in dynamic environments, in Sec. 6.

4. Nonconvex scenario optimization

In the following, we summarize the main results of the NSO framework of Campi et al. (2018) that we use to build our motion planning framework. To this end, consider the following generalization of Problem 1,

Problem 2. General CCP.

$\begin{array}{ll}\hfill {\min }_{\begin{array}{l}\mathit{u}\in \mathbb{U}\\ \mathit{x}\in \mathbb{X}\end{array}}& \sum _{k=0}^{N}J({\mathit{x}}_k,{\mathit{u}}_k)\hfill \end{array}$

(3a)

$\begin{array}{ll}\hfill \mathrm{s}.\mathrm{t}.& {\mathit{x}}_0={\mathit{x}}_{\text{init}}\hfill \end{array}$

(3b)

$\begin{array}{ll}\hfill & {\mathit{x}}_{k+1}=f({\mathit{x}}_k,{\mathit{u}}_k),k=0,\dots ,N-1\hfill \end{array}$

(3c)

$\begin{array}{ll}\hfill & \mathbb{P}\left[g(\mathit{x},\mathit{\delta })\le 0\right]\ge 1-ϵ,\mathit{\delta }\in \mathrm{\Delta }.\hfill \end{array}$

(3d)

The nominal constraint g( x , δ ) ≤ 0 must be satisfied with a probability of at least 1 − ϵ. The main idea of scenario optimization is to solve Problem 2 by imposing deterministic constraints for a set of scenarios Ω = { δ ⁽¹⁾, …, δ ^(S)} ∈ Δ ^S , where each scenario is independently extracted from $\mathbb{P}$ (Δ ^S represents the S-fold Cartesian product of Δ associated with drawing S random samples). The number of sampled scenarios is known as the sample size S. Using scenarios, the SP for Problem 2 is formulated as

Problem 3

General SP.

$\begin{array}{ll}\hfill {\min }_{\begin{array}{l}\mathit{u}\in \mathbb{U}\\ \mathit{x}\in \mathbb{X}\end{array}}& \sum _{k=0}^{N}J({\mathit{x}}_k,{\mathit{u}}_k)\hfill \end{array}$

(4a)

$\begin{array}{ll}\hfill \mathrm{s}.\mathrm{t}.& {\mathit{x}}_0={\mathit{x}}_{\text{init}}\hfill \end{array}$

(4b)

$\begin{array}{ll}\hfill & {\mathit{x}}_{k+1}=f({\mathit{x}}_k,{\mathit{u}}_k),k=0,\dots ,N-1\hfill \end{array}$

(4c)

$\begin{array}{ll}\hfill & g(\mathit{x},{\mathit{\delta }}^{(i)})\le 0,{\mathit{\delta }}^{(i)}\in \mathrm{\Omega },i=1,\dots ,S,\hfill \end{array}$

(4d)

where chance constraint (3d) has been replaced by the deterministic constraints for each of the scenarios in (4d). To simplify notation, we define a decision as $\mathit{\theta }:={\left[\begin{array}{c}\hfill {\mathit{x}}^{\top },{\mathit{u}}^{\top }\hfill \end{array}\right]}^{\top }\in \mathrm{\Theta }$ , where $\mathrm{\Theta }=\mathbb{X}\times \mathbb{U}$ . Each scenario δ ⁽ⁱ⁾ imposes a constraint on the decision, denoted $\mathit{\theta }\in {\mathrm{\Theta }}_{{\mathit{\delta }}^{(i)}}$ , ${\mathrm{\Theta }}_{{\mathit{\delta }}^{(i)}}=\{\mathit{\theta }\in \mathrm{\Theta }:g(\mathit{x},{\mathit{\delta }}^{(i)})\le 0\}$ . Formally, to make a decision based on the scenarios, the decision algorithm $\mathcal{A}$ is defined, that maps the scenarios to a decision (i.e., solving Problem 3). This (sub)optimal decision ${\mathit{\theta }}^{*}=\mathcal{A}(\mathrm{\Omega })$ is called the scenario decision. The probabilistic guarantees derived from the SP depend on the following assumption.

Assumption 1. For any finite S = 1, 2, … and for any set of scenarios Ω ∈ Δ ^S , it holds that

$\mathcal{A}(\mathrm{\Omega })\in {\mathrm{\Theta }}_{{\mathit{\delta }}^{(i)}},i=1,\dots ,S.$

(5)

We focus our attention on the planning problem by considering only finite sample sizes S. Assumption 1 is not trivially satisfied for motion planning. It requires that a feasible trajectory exists for all possible extractions in the support, which is particularly problematic for unbounded distributions (e.g., Gaussians). We will consider a relaxed constraint formulation in Sec. 5.1 to address this limitation.

Since the scenario decision accounts for all sampled scenarios, each additional scenario makes the scenario decision more robust. Consequently, the more scenarios that are used to compute the scenario decision, the lower is the probability that the resulting decision will violate the constraints. Formally, the violation probability, V : Θ → [0, 1], given by

$V(\mathit{\theta }):=\mathbb{P}\left[\mathit{\delta }\in \mathrm{\Delta }:\mathit{\theta }\notin {\mathrm{\Theta }}_{\mathit{\delta }}\right],$

(6)

defines the probability that a decision θ violates a newly observed scenario. We also refer to this probability as the risk of the decision. Since the decision θ depends on the realization of the randomly sampled scenarios, the violation probability V( θ ) is in itself a random variable over the product probability measure, given by ${\mathbb{P}}^{\text{S}}$ = $\mathbb{P}\times \dots \times \mathbb{P}$ (S times). We are therefore interested in lower bounding the confidence, which is the probability that the scenario decision achieves a risk of at most ϵ. The key variable to obtain this bound is the support subsample, defined as follows.

Definition 1. Campi et al. (2018): Given a set of scenarios Ω = { δ ⁽¹⁾, …, δ ^(S)}, a support subsample $\mathcal{C}=\{{\mathit{\delta }}^{(i_1)},\dots ,{\mathit{\delta }}^{(i_n)}\}$ is a tuple of n elements extracted from the set of scenarios with i₁ < i₂ < … < i _n , which gives the same solution as with all scenarios in place, that is,

$\mathcal{A}({\mathit{\delta }}^{(i_1)},\dots ,{\mathit{\delta }}^{(i_n)})=\mathcal{A}({\mathit{\delta }}^{(1)},\dots ,{\mathit{\delta }}^{(S)}).$

(7)

The cardinality of the support subsample is referred to as the support size, that is, $n:=|\mathcal{C}|$ . In the context of this paper, a scenario is said to be of support if it is an element of the considered support subsample. A scenario that can be excluded from a support subsample without changing the solution is said to be not of support. For example, a sampled human trajectory δ ⁽ⁱ⁾ can be excluded from the support if it does not change the robot’s optimal behavior under the current set of human trajectory samples.

The support size captures the number of scenarios necessary to hold the solution of the SP in place and is strongly correlated with its risk. This correlation can be used to derive a probabilistic guarantee on the solution of the SP using only the support and sample size. For a confidence 1 − β, Theorem 1 in Campi et al. (2018) provides the bound

${\mathbb{P}}^{\text{S}}[V({\mathit{\theta }}^{*})>ϵ(n)]\le \beta ,$

(8)

where the risk is defined as a function of the support size, ϵ(n) : {0, …, S} → [0, 1], that must satisfy

$\sum _{v=0}^{S-1}\left(\genfrac{}{}{0ex}{}{S}{v}\right){\left[1-ϵ(v)\right]}^{S-v}=\beta ,ϵ(S)=1.$

(9)

Equation (8) therefore upper limits the probability that the scenario decision θ * exceeds the acceptable risk ϵ(n) by β. The risk is divided equally over all support values by the mapping

$ϵ(n)=\{\begin{array}{cc}1,\hfill & n=S,\hfill \\ 1-{\left(\frac{\beta }{S\left(\genfrac{}{}{0ex}{}{S}{n}\right)}\right)}^{\frac{1}{S-n}},\hfill & \mathrm{o}\mathrm{t}\mathrm{h}\mathrm{e}\mathrm{r}\mathrm{w}\mathrm{i}\mathrm{s}\mathrm{e}.\hfill \end{array}$

(10)

This mapping certifies the solution without further information on the problem. Other mappings are possible; for more details, see Campi et al. (2018). The interpretation of equation (8) is that when an algorithm computes a decision θ * along with the support size n*, then the risk is certified to be no larger than ϵ(n*) with confidence 1 − β. For example, with N = 1000 and 1 − β = 1 − 10⁻⁶, when the support size is n* = 6, then equation (10) gives ϵ(6) = 5.4% and we can certify that the risk is bounded by 5.4% with confidence 1 − 10⁻⁶.

A general algorithm to determine the support is the greedy algorithm of Campi et al. (2018). After solving Problem 3, this algorithm removes one scenario at a time, solving Problem 3 again. If the solution changes for a scenario, then that scenario is part of the support. The samples remaining after checking all scenarios constitute a support subsample.

4.1. Limitations of NSO for motion planning

An SP can be constructed from the CCP in Problem 1 by sampling scenarios from the distribution over obstacle trajectories and formulating collision avoidance constraints for each scenario. However, this strategy is not directly applicable for the motion planning problem because of two limitations.

Limitation 1. Assumption 1 is not always satisfied. Drawing samples from the probability distribution of future obstacle motion can lead to an infeasible problem (e.g., if samples are close to the robot). For unbounded distributions (e.g., Gaussians), there is always a nonzero probability of drawing such a sample.

Limitation 2. Certifying the trajectory requires solving S additional optimization problems of similar complexity to the original problem (the greedy support estimation), which is computationally intractable for real-time trajectory optimization.

Our goal is to address these limitations so that we can solve Problem 1 with an SP to bound the joint CP.

5. Safe horizon model predictive control

This section introduces our safe motion planning framework. First, we reformulate the SP to obtain a real-time solvable problem that satisfies Assumption 1. Then, we show how the support can be estimated during optimization. Finally, we derive the sample size of the SP.

5.1. Motion planning scenario program

Safe Horizon MPC bounds the joint CP by solving an SP. In the planning problem, the scenarios extracted from $\mathbb{P}$ represent realizations of the future motion of all obstacles in the near future (see Figure 3(a)). Using these scenarios, we can construct the following SP from the CCP in Problem 1

Figure 3.

Schematic illustration of Safe Horizon MPC applied to a mobile robot. (a) Scenarios are sampled from the trajectory distributions. Each time instance of SP (Equation (4)) is associated with a set of sampled obstacle positions as visualized by the green and blue circled pedestrians. (b) Halfspace constraints are constructed between sampled obstacles and the robot, and are reduced to a probabilistic safe polytope for each time instance and robot disc, depicted by the yellow lines. (c) Problem 5 is solved via Algorithm 1. The resulting trajectory is certified up to a probabilistic bound.

Problem 4. Motion Planning SP.

$\begin{array}{ll}\hfill {\min }_{\begin{array}{c}\mathit{u}\in \mathbb{U},\mathit{x}\in \mathbb{X}\\ d\in \mathbb{R}\end{array}}& \sum _{k=0}^{N}J({\mathit{x}}_k,{\mathit{u}}_k)+w_d\Vert d{\Vert }_2^2\hfill \end{array}$

(11a)

$\begin{array}{ll}\hfill \mathrm{s}.\mathrm{t}.& {\mathit{x}}_0={\mathit{x}}_{\text{init}}\hfill \end{array}$

(11b)

$\begin{array}{ll}\hfill & {\mathit{x}}_{k+1}=f({\mathit{x}}_k,{\mathit{u}}_k),k=0,\dots ,N-1\hfill \end{array}$

(11c)

$\begin{array}{ll}\hfill & \underset{i=0}{\overset{S}{\bigwedge }}\underset{k=1}{\overset{N}{\bigwedge }}\underset{c=0}{\overset{n_d}{\bigwedge }}\underset{j=0}{\overset{M}{\bigwedge }}\left(\Vert {\mathit{p}}_k^c-{\mathit{\delta }}_{k,j}^{(i)}{\Vert }_2\ge r+d\right),\hfill \end{array}$

(11d)

where (11d) represents the scenario constraints (bundled with the “and” operator) and $w_d>0,w_d\in \mathbb{R}$ weighs the slack variable. Compared to the general SP in Problem 3, Problem 4 adds a single joint slack variable $d\in \mathbb{R}$ over the collision constraints that denotes the minimum distance kept to all obstacles over the duration of the trajectory. Because of the slack introduced through d, there is always a solution to Problem 4 and Assumption 1 is satisfied (addressing Limitation 1). We utilize a single slack variable to allow the scenario constraints to be pruned for real-time performance. Less conservative trajectories may be obtained with S slack variables as proposed in Garatti and Campi (2023), but come at a high computational cost. The violation probability associated with Problem 4 is

$\mathrm{\mathbb{P}}\left[\mathit{\delta }\in \mathrm{\Delta }|\underset{k=1}{\overset{N}{ \bigwedge }}\underset{c=0}{\overset{n_d}{\bigwedge }}\underset{j=0}{\overset{M}{\bigwedge }}\left(\Vert {\mathit{p}}_k^c-{\mathit{\delta }}_{k,j}{\Vert }_2\ge r+d\right)\right] \ge 1-ϵ.$

(12)

For d = 0, the violation probability matches that of Problem 1 and is bounded through the NSO framework. When d ≤ 0, the optimization could not find a sufficiently safe solution. The robot can use this information to adjust its behavior and ensure safety (e.g., by slowing down or using a fallback plan).

5.2. Improved computational efficiency

While Problem 4 can be solved with off-the-shelf nonlinear program solvers, its problem statement is computationally inefficient. The collision-free space described by (11d) is nonconvex and can lead to high support (i.e., requiring many samples). We consider a more efficient overapproximation of the collision avoidance constraints with lower support.

We linearize the collision regions with respect to the previously planned robot trajectory (denoted $\widehat{\mathit{p}}$ ). After linearization, each scenario is associated with a linear constraint (depicted in Figure 3(b)). For a previous robot position ${\widehat{\mathit{p}}}_k$ and obstacle position δ _k , the constraints are given by

$\mathcal{H}({\widehat{\mathit{p}}}_k,{\mathit{\delta }}_k,d)=\{{\mathit{p}}_k|\mathit{A}{({\widehat{\mathit{p}}}_k,{\mathit{\delta }}_k)}^{\top }{\mathit{p}}_k\le b({\widehat{\mathit{p}}}_k,{\mathit{\delta }}_k,d)\},$

(13)

where

$\mathit{A}({\widehat{\mathit{p}}}_k,{\mathit{\delta }}_k)=\frac{{\mathit{\delta }}_k-{\widehat{\mathit{p}}}_k}{\Vert {\mathit{\delta }}_k-{\widehat{\mathit{p}}}_k\Vert },b({\widehat{\mathit{p}}}_k,{\mathit{\delta }}_k,d)={\mathit{A}}^{\top }{\mathit{\delta }}_k-(r+d).$

(14)

The linearized collision region contains the original collision region for zero slack, preserving the probabilistic guarantees when d = 0. In addition, the linearization is applied locally to each timestep k and robot disc c, resulting in a locally tight approximation of the original collision regions. For the linearized constraints, we obtain the following SP:

$\begin{array}{ll}\hfill {\min }_{\begin{array}{c}\mathit{u}\in \mathbb{U},\mathit{x}\in \mathbb{X}\\ d\in \mathbb{R}\end{array}}& \sum _{k=0}^{N}J({\mathit{x}}_k,{\mathit{u}}_k)+w_d\Vert d{\Vert }_2^2\hfill \end{array}$

(15a)

$\begin{array}{ll}\hfill \mathrm{s}.\mathrm{t}.& {\mathit{x}}_0={\mathit{x}}_{\text{init}}\hfill \end{array}$

(15b)

$\begin{array}{ll}\hfill & {\mathit{x}}_{k+1}=f({\mathit{x}}_k,{\mathit{u}}_k),k=0,\dots ,N-1\hfill \end{array}$

(15c)

$\begin{array}{ll}\hfill & \underset{i=0}{\overset{S}{\bigwedge }}\underset{k=1}{\overset{N}{\bigwedge }}\underset{c=0}{\overset{n_d}{\bigwedge }}\underset{j=0}{\overset{M}{\bigwedge }}\left({\mathit{p}}_k^c\in \mathcal{H}\left({\widehat{\mathit{p}}}_k^c,{\mathit{\delta }}_{k,j}^{(i)},d\right)\right).\hfill \end{array}$

(15d)

The linearization exploits the geometry of the motion planning problem to reduce the size of the support. Most of the linearized constraints become redundant (they are fully contained in other constraints) and can be excluded from the planning problem. We provide more details in Appendix A.

To drastically reduce the computational demand of this formulation, we reorder Constraints (15d) as

$\underset{k=1}{\overset{N}{\bigwedge }}\underset{c=0}{\overset{n_d}{\bigwedge }}\left[\underset{i=0}{\overset{S}{\bigwedge }}\underset{j=0}{\overset{M}{\bigwedge }}\left({\mathit{p}}_k^c\in \mathcal{H}\left({\widehat{\mathit{p}}}_k^c,{\mathit{\delta }}_{k,j}^{(i)},d\right)\right)\right],$

(16)

to pair constraints that apply to a single robot disc position ${\mathit{p}}_k^c$ . Because of the overlap between the constraints, each of these constraint pairings can be described by a small subset of the constraints for d = 0 (see Figure 3(b)). The constraints (15d) can, therefore, be reduced to free-space polytopes before optimization, which significantly reduces computation times. We denote these polytopes, for disc c and time step k as

${\mathcal{P}}_k^c(0)=\left\{{\mathit{p}}_k^c|\underset{(i,j)\in {\mathcal{I}}_k^c}{\bigwedge }{\mathit{p}}_k^c\in \mathcal{H}\left({\widehat{\mathit{p}}}_k^c,{\mathit{\delta }}_{k,j}^{(i)},0\right)\right\},$

(17)

where the indices of scenarios that form the boundary of the polytope are collected in the set ${\mathcal{I}}_k^c$ . For d > 0, the polytope is denoted ${\mathcal{P}}_k^c(d)$ and consists of the same linear constraints relaxed with a distance d. We derived an algorithm based on recursive search, detailed in Appendix B, that performs this pruning in less than 100 μs, while it typically reduces the number of constraints by a factor 10² − 10³. We verified, for an example in our simulations, that with 10,816 samples, the final polytope consists of 11 constraints on average.

The final SP that is solved online, after pruning, is given by

Problem 5. Safe Horizon MPC.

$\begin{array}{ll}\hfill {\min }_{\begin{array}{l}\mathit{u}\in \mathbb{U},\mathit{x}\in \mathbb{X}\\ d\in \mathbb{R}\end{array}}& \sum _{k=0}^{N}J({\mathit{x}}_k,{\mathit{u}}_k)+w_d\Vert d{\Vert }_2^2\hfill \end{array}$

(18a)

$\begin{array}{ll}\hfill \mathrm{s}.\mathrm{t}.& {\mathit{x}}_0={\mathit{x}}_{\text{init}}\hfill \end{array}$

(18b)

$\begin{array}{ll}\hfill & {\mathit{x}}_{k+1}=f({\mathit{x}}_k,{\mathit{u}}_k),k=0,\dots ,N-1\hfill \end{array}$

(18c)

$\begin{array}{ll}\hfill & {\mathit{p}}_k^c\in {\mathcal{P}}_k^c(d),\forall k,\forall c.\hfill \end{array}$

(18d)

The SH-MPC problem can be solved in real-time while bounding the joint CP of its trajectory (see Figure 3(c)).

5.3. Estimating the support

The joint CP that is bounded by SH-MPC depends on the sample size S and the support n. In the following, we propose an estimate of the support $\widehat{n}\ge n$ that can efficiently be computed during optimization.

For a convex SP, the support can easily be computed as its support constraints are active (Campi and Garatti, 2008; Garatti and Campi, 2019). Computing the support in the nonconvex case is much harder as this property does not hold. We show here that the support can be estimated through the active constraints after each iteration of the nonconvex optimization. An iteration refers to the procedure that is repeated to solve the nonconvex optimization, such that the decision algorithm can be described by a repeated sequence of iterations ${\mathcal{A}}^l:\mathrm{\Theta }\times {\mathrm{\Delta }}^S\to \mathrm{\Theta },l\in 0,\dots ,L$ as

$\mathcal{A}={\mathcal{A}}^L(\dots {\mathcal{A}}^1({\mathcal{A}}^0({\mathit{\theta }}^0,\mathrm{\Omega }),\mathrm{\Omega })\dots ,\mathrm{\Omega }).$

(19)

We define the support of an iteration as a set of scenarios that results in the same output as the output where all scenarios are considered. The support of the decision algorithm can be connected with that of its iterations by the following lemma.

Lemma 1. Consider a decision algorithm $\mathcal{A}$ , separated according to (19). Its support $\mathcal{C}$ satisfies

$\mathcal{C}\subseteq \mathrm{\Omega }\backslash {\mathrm{\Omega }}_{\text{ns}},{\mathrm{\Omega }}_{\text{ns}}=\bigcap _{l=0}^L{\mathrm{\Omega }}_{\text{ns}}^l,$

(20)

where

${\mathrm{\Omega }}_{\text{ns}}^l=\left\{{\mathit{\delta }}^{(i)}\in \mathrm{\Omega }|{\mathcal{A}}^l({\mathit{\theta }}^l,\mathrm{\Omega })={\mathcal{A}}^l({\mathit{\theta }}^l,\mathrm{\Omega }\backslash {\mathit{\delta }}^{(i)})\right\},$

(21)

is the set of scenarios not of support in iteration l.

Proof. Scenarios in Ω_ns can be excluded for all l without changing the solution, that is, by (20) and (21),

$\mathcal{A}({\mathit{\theta }}^0,\mathrm{\Omega }\backslash {\mathit{\delta }}^{(i)})=\mathcal{A}({\mathit{\theta }}^0,\mathrm{\Omega })={\mathit{\theta }}^{*},$

(22)

for all δ ⁽ⁱ⁾ ∈ Ω_ns. Therefore, by Definition 1 all scenarios in Ω_ns are not of support for $\mathcal{A}$ . The support of $\mathcal{A}$ is therefore in the complement of this set with respect to the set Ω and the result follows.

The support set obtained through Lemma 1 is an overestimation. It is possible that a scenario changes the solution of an intermediate iteration without changing the final solution.

To apply this lemma to SH-MPC, we note that a local optimum of Problem 5 can be computed by iteratively linearizing the problem and solving a convex optimization. In this case, each iteration of the solver is a convex scenario optimization for which the support constraints are active (Campi and Garatti, 2008). For example, in Sequential Quadratic Programming (SQP), each iteration ${\mathcal{A}}^l$ refers to the inner QPs. We explicitly impose this assumption on the solver.

Assumption 2. Each iteration ${\mathcal{A}}^l$ of decision algorithm $\mathcal{A}$ solves a convex optimization problem.

The active constraints can be identified by verifying which of the scenario constraints of the convex program are exactly satisfied. We therefore require the additional assumption that the constraints of the convex problem are satisfied in each iteration.

Assumption 3. The solution computed by each iteration ${\mathcal{A}}^l$ is feasible with respect to its inequality constraints.

The scenario constraints are in general nonlinear due to the robot dynamics. In each iteration, and as commonly done to solve nonlinear problems, the solver makes the problem convex, deriving constraints from the locally linearized dynamics. We only require that these constraints are satisfied. Using these assumptions, we propose the following support estimation.

Theorem 1. If iterations ${\mathcal{A}}^l$ of the decision algorithm satisfy Assumptions 2 and 3, then the support of Problem 3 satisfies

$\mathcal{C}\subseteq \bigcup _{j=0}^L{\mathrm{\Omega }}_{\text{active}}^j=\widehat{\mathcal{C}},\widehat{n}:=|\widehat{\mathcal{C}}|,$

(23)

where, with g ^l the inequality constraints of ${\mathcal{A}}^l$ ,

${\mathrm{\Omega }}_{\text{active}}^l=\{{\mathit{\delta }}^{(i)}\in \mathrm{\Omega }|\exists k{g}^l({\mathit{x}}_k^l,{\mathit{\delta }}_k^{(i)},d^l)=0\},$

(24)

denote the active constraints in iteration l.

Proof. Under Assumption 2, the support constraints of iteration l are in the set ${\mathrm{\Omega }}_{\text{active}}^l$ (all constraints are satisfied by Assumption 3). Under convexity, ${\mathrm{\Omega }}_{\text{active}}^l$ is therefore the complement of the set ${\mathrm{\Omega }}_{\text{ns}}^l$ . Invoking Lemma 1 and using De Morgans Law (Morgan, 1847), we obtain (23).

The support of SH-MPC can therefore be estimated by the aggregated set of active scenarios over all iterations, addressing Limitation 2. In practice, we use Sequential Quadratic Programming (SQP) (Nocedal and Wright, 2006) Chapter 18 to solve Problem 5, which satisfies Assumption 2 (convexity of iterates) and Assumption 3 (feasibility of intermediate iterates).

5.4. Determining the sample size

With the support of SH-MPC estimated through Theorem 1 and directly available after optimization, it remains to determine the sample size S for SH-MPC such that the joint CP of its trajectory is at most ϵ.

Problem 5 can only be solved once due to the strict real-time requirements on the planner. We therefore propose to find a sufficiently high S that certifies the joint CP almost always. To that end, we define a support limit $\overline{n}$ describing a support size that we expect not to exceed and use it to certify the optimized trajectory in practice.

Theorem 2. SH-MPC (Problem 5) with sample size S, computed from (10) with support limit $\overline{n}$ , does not exceed a specified risk of ϵ (with specified confidence 1 − β) if its support is lower than the support limit, that is, if $n\le \overline{n}$ .

Proof. SH-MPC satisfies Assumption 1 and with ϵ(n) as in (10), Theorem 1 in Campi et al. (2018) ensures that (8) holds. Since ϵ(n) in (10) is monotonically increasing in n (i.e., ϵ(n + 1) > ϵ(n), ∀n < S) and given that the computed S ensures that $ϵ(\overline{n})\le ϵ$ , we have that $ϵ(n)\le ϵ,\forall n\le \overline{n}$ .

Theorem 2 shows that SH-MPC solves the CCP in Problem 1 if its support is lower than the support limit (and if d = 0).

In practice, the support limit can be estimated by deploying the planner and collecting the support estimation of Theorem 1. In our approach, we keep track of the largest estimated support over a large number of iterations and use this empirical worst-case support as the support limit. An alternative could be to start with a large support limit, lowering it when it is not passed for many iterations. Since the support limit is likely higher than necessary for many iterations, the trajectories computed by SH-MPC become conservative. The support limit however allows the planner to incorporate the joint collision avoidance chance constraint in real-time. Conservatism can be reduced by running several instances of SH-MPC in parallel (along the lines of Mustafa et al. (2023)), which can lead to lower and more consistent support. Alternatively, the support limit could be continuously adapted based on its online estimation. We consider these future work.

5.5. Algorithm outline

Algorithm 1 summarizes the SH-MPC framework. Offline, we compute the sample size S based on the joint CP ϵ, confidence 1 − β and support limit $\overline{n}$ (Line 2). We provide a Jupyter notebook with this paper (De Groot, 2024) that performs this computation using a bisection of (10). Online, the predicted probability distribution is received from the perception module and scenarios are sampled from it (Line 4 − 5). We solve l iterations of Problem 5, determining the active scenarios and aggregating the support set in each iteration (Line 7 − 9). If the slack is zero and the support limit is not exceeded, then the final trajectory is certifiably safe and its first input is executed (Line 10 − 12). Otherwise, the robot executes a fallback action (e.g., braking).

6. Results

We evaluate SH-MPC in simulation, comparing against two MPC baselines, and in real-world experiments on a mobile robot among pedestrians. A video of the results accompanies this paper (De Groot et al., 2024a).

6.1. Simulation setup

We consider a mobile robot moving through an environment with pedestrians (see Figure 4) in which the robot is modeled by a kinematic unicycle model (Siegwart and Nourbakhsh, 2011). We assume throughout that the distribution of pedestrian motion is known in order to evaluate the performance of the planner in isolation (i.e., without prediction errors). We first validate on a Gaussian case, where the baselines may leverage the shape of the distribution to approximate the probabilistic collision-free space accurately. In this case, the pedestrian dynamics are given by

${\mathit{\delta }}_{k+1}={\mathit{\delta }}_k+(\mathit{v}+{\mathit{\delta }}_{w,k})dt,{\mathit{\delta }}_{w,k}\sim \mathcal{N}(\mathit{0},{\mathbf{\Sigma }}_w),$

(25)

where ${\mathbf{\Sigma }}_w\in {\mathbb{R}}^{2\times 2}$ is a diagonal covariance matrix (i.e., random variables in the x and y direction are independent). Its diagonal entries σ _wx = σ _wy = 0.3 are kept constant over the horizon. The nominal velocity is denoted by $\mathit{v}\in {\mathbb{R}}^2$ . The pedestrian and robot radius are 0.3 m and 0.325 m, respectively. Throughout our simulations and experiments, we set the maximum open-loop joint CP as ϵ = 0.05. This value can be handled computationally by the considered methods and is typically sufficient to prevent collisions in closed-loop without excessively reducing the probabilistic collision-free space.

Figure 4.

Planned trajectories for the different methods in the Gaussian simulation with eight pedestrians. The robot plan and associated collision areas are drawn in blue. For all methods, visualizations are shown in blue to green colors for stages 0, 5, 10, 15, and 19, respectively. (a) Circles show the predicted areas occupied by the obstacles following the mean of the Gaussian distribution. (b) Circles show the level sets of the Gaussian distribution at the specified ϵ. (c) Sampled pedestrian positions (excluding pruned samples in the center) are drawn as points with their collision area, borders of the safe polytopes are drawn as colored lines. (d) Similar to (c) but depicting sampled trajectories as dashed lines and highlighting support constraints. (a) Deterministic baseline. (b) Gaussian baseline at ϵ _k = 0.0003125. (c) S-MPCC at ϵ _k = 0.0025. (d) SH-MPC at ϵ = 0.05.

6.2. Implementation of SH-MPC

All baselines and SH-MPC use the Model Predictive Contouring Control formulation in Brito et al. (2019), which tracks a reference path and reference velocity while penalizing robot inputs. We solve Problem 5 using the Forces Pro SQP solver (Domahidi and Jerez, 2014) with at most 12 iterations. We empirically selected the support limit $\overline{n}=10$ . With ϵ = 0.05 and setting a confidence of 1 − β = 0.99, the sample size is S = 1351. If the support limit is exceeded or the slack is nonzero, we slow down the robot. The linearization of the constraints requires the previous plan to be feasible. We use of a projection step to ensure that this holds in all time steps. This projection step consists of a projection orthogonal to the direction of robot movement, which almost always results in a feasible plan. In the remaining cases, we solve a feasibility program using Douglas–Rachford Splitting (Aragón Artacho et al., 2020). SH-MPC is implemented in C++/ROS and will be released open source.

6.3. Baselines

We compare SH-MPC against a deterministic baseline and two probabilistic baselines that constrain the marginal CP, that is, the independent CP per time instance and/or obstacle.

(1) Deterministic MPC: Avoids the mean of each mode as a deterministic circular obstacle.

(2) S-MPCC (De Groot et al., 2021): Marginal CP per time instance.

(3) CC-MPC (Zhu and Alonso-Mora, 2019): Marginal CP per time instance and obstacle.

S-MPCC handles arbitrary distributions. It solves an SP where scenario constraints are posed on the marginal distributions. For each time k, it samples from the independent obstacle distribution at k to obtain a collision-free polygon. It is assumed that all constraints in the polygon are of support (set to 20 in these simulations), which requires S = 75946 for instance when ϵ _k = 0.0025. Samples in the center of the distribution are pruned in the Gaussian case to reduce the number of samples considered online.

CC-MPC is strictly applicable to Gaussian distributions. It approximates the collision probability via the Cumulative Density Function (CDF) of the Gaussian distribution. The linearized collision avoidance chance constraint

$\mathbb{P}\left[{\mathit{a}}_{k,j}^{\top }({\mathit{p}}_k-{\mathit{\delta }}_{k,j})\ge r\right]\ge 1-{ϵ}_k,{\mathit{a}}_{k,j}=\frac{{\mathit{p}}_k-{\mathit{\delta }}_{k,j}}{\Vert {\mathit{p}}_k-{\mathit{\delta }}_{k,j}\Vert },$

(26)

is equivalent, under a Gaussian distribution of δ _k,j, to

${\mathit{a}}_{k,j}^{\top }({\mathit{p}}_k-{\mathit{\delta }}_{k,j})-r\ge {\mathrm{e}\mathrm{r}\mathrm{f}}^{-1}(1-2{ϵ}_k)\sqrt{2{\mathit{a}}_{k,j}^{\top }\mathbf{\Sigma }{\mathit{a}}_{k,j}},$

(27)

where erf⁻¹ is the inverse standard error function and Σ is the covariance matrix of the uncertainty. This constraint is imposed separately for each time step and obstacle.

Since SH-MPC is characterized by a single bound ϵ on the trajectory CP, while the baselines specify bounds ϵ _k on the CP for each k and for each obstacle, we consider three versions of the baselines. The first sets ϵ _k = ϵ, which is not provably safe but relies on updates of the controller to remain safe. The second version sets ϵ _k = ϵ/N, accounting for the marginal approximation over time since ∑ _k ϵ _k = ϵ, but ignoring marginalization per obstacle. As S-MPCC inherently accounts for the obstacle marginalization, it matches SH-MPC’s safety specification (joint CP of ϵ) with this version. The third version accounts for both marginalizations, setting ϵ _k = ϵ/NM such that M∑ _k ϵ _k = ϵ. CC-MPC only attains the same safety specification as SH-MPC using this last version.

6.4. Weights and parameters

The only difference between the planners is their collision avoidance constraints. We use the same solver, weights and cost function for all methods. Weights of the MPC problem are given in Table 1. We define a horizon of N = 20 steps, with a discretization step of 0.2s, giving a time horizon of 4.0s. The control rate is 20 Hz, corresponding to a sampling time of 50 ms. The computer running the simulations is equipped with an Intel i9 CPU@2.4GHz.

Table 1.

Weights of the MPC problem (see Brito et al. (2019)).

Contour	Lag	Velocity	Acceleration	Ang. Velocity
0.005	0.1	0.05	0.05	0.05

6.5. Baseline comparison—Gaussian uncertainties

We consider an environment with multiple dynamic pedestrians following the dynamics in equation (25) where v describes a constant velocity. We validate the actual CP of the motion plan offline after the experiments through Monte Carlo sampling for all methods, where the dynamics in equation (25) are used to generate the samples. We compute the CP by dividing the number of samples where the robot and obstacle discs overlap at any stage by the total number of samples (set to 10⁵). The marginal CP (CP _k ) is computed without taking prior collisions into account. We validate in a scenario with 8 pedestrians.

Figure 4 depicts snapshots of the simulations. Quantitative results are summarized in Table 2. The deterministic baseline completes the task faster than the other methods but attains a high CP and consequently collides in total in 102 cases, whereas the other methods do not collide. S-MPCC is more conservative than the other methods because of its conservatively large sample size. CC-MPC, that uses the Gaussian CDF, bounds the marginal CP (CP _k ) per obstacle accurately. However, when ϵ _k = 0.05, its maximum joint CP is 0.2264, which exceeds 0.05. The trajectories are safe under ϵ _k = 0.0025 in practice with a maximum overall CP of 0.0159, although it is not theoretically accounting for the obstacle marginalization. This is evident in the marginal CP that, with 0.0047, exceeds its specification of 0.0025. For clarification, Figure 6 illustrates this error made by obstacle marginalization on a 1-dimensional example. The version of CC-MPC with the same safety specification as SH-MPC, constrains ϵ _k = 0.0025/8 = 0.0003125. The results indicate that SH-MPC moves through the environment faster than the safe baselines with the same safety specification as its joint CP (0.0092) is less conservative than that of the baselines (e.g., 0.0019 for CC-MPC).

Table 2.

Statistical results for the eight pedestrian environment under Gaussian uncertainty (sec. 6.5). Results compare the marginal CP (“CP _k ”) and joint CP (“CP”), the task duration, minimum distance to the pedestrians and computation times over 100 experiments. For the CPs we report the maximum observed over all experiments and compare it to the specified bound (a dash indicates that no bound is specified on the particular CP). Other results are reported as “average (standard deviation)” unless stated otherwise. The divider separates methods that do not bound the joint risk at ϵ = 0.05 (above) and methods that do (below). Our method (SH-MPC) is less conservative than the baselines with similar safety specifications.

Method	Max CP k /Spec. (%)	Max CP/Spec. (%)	Dur. [s]	Min Dist. [m]	Runtime (Max) [ms]
Deterministic MPC	0.9387/- (‐)	0.9989/- (‐)	10.1 (0.9)	‐0.03 (0.07)	13 (76)
S-MPCC (ϵ k = 0.05)	0.0037/0.0500 (7)	0.0049/- (‐)	15.6 (2.6)	0.36 (0.06)	47 (137)
CC-MPC (ϵ k = 0.05)	0.0929/0.0500 (186)	0.2264/- (‐)	10.3 (2.0)	0.12 (0.03)	10 (45)
CC-MPC (ϵ k = 0.0025)	0.0047/0.0025 (187)	0.0159/- (‐)	16.9 (4.5)	0.24 (0.07)	10 (39)
S-MPCC (ϵ k = 0.0025)	0.0002/0.0025 (7)	0.0002/- (‐)	19.2 (3.2)	0.55 (0.08)	83 (286)
CC-MPC (ϵ k = 0.0003125)	0.0008/0.0025 (30)	0.0019/- (‐)	18.4 (2.4)	0.32 (0.07)	11 (33)
SH-MPC (ϵ = 0.05)	0.0073/- (‐)	0.0092/0.0500 (18)	16.0 (1.9)	0.34 (0.03)	27 (66)

6.6. Baseline comparison: Gaussian mixture

We now modify the distribution to incorporate a probability that the pedestrians will cross. We encode this scenario with a Markov Chain (see Figure 5) that changes pedestrian movement from horizontal to diagonal in addition to the Gaussian process noise of the previous simulations. We define the pedestrian dynamics as

${\mathit{p}}_{k+1}={\mathit{p}}_k+(B\mathit{v}+{\mathit{\delta }}_{w,k})dt,{\mathit{\delta }}_{w,k}\sim \mathcal{N}(\mathrm{0},{\mathbf{\Sigma }}_{w,k}),$

(28)

where B is either $B_h={\left[\begin{array}{cc}\hfill 1\hfill & \hfill 0\hfill \end{array}\right]}^{\top }$ or $B_d={\left[\begin{array}{cc}\hfill \frac{1}{\sqrt{2}}\hfill & \hfill \frac{1}{\sqrt{2}}\hfill \end{array}\right]}^{\top }$ depending on the state of the Markov Chain. The uncertainties associated with this motion can be modeled as a Gaussian Mixture Model (GMM), where each possible state transition in the Markov Chain leads to a separate mode with an associated probability (in total 21 modes). For example, the mode where the pedestrian crosses after two steps occurs with probability p₂ = (1 − 0.025)0.025. We apply the deterministic baseline to the GMM distribution by avoiding the mean of each mode. For CC-MPC, we formulate the constraints at a risk of ϵ _k for all modes, which can lead to conservatism when multiple modes influence the plan. We validate in this environment with eight pedestrians.

Figure 5.

Markov chain modeling of a crossing pedestrian.

Figure 6.

A 1D illustration of the case where two obstacles constrain the robot. Even though the marginal probability of collision for each obstacle is less than ϵ _k (shaded tails) in the center region, the joint probability of collision in the feasible region (green shaded area) is larger than ϵ _k . (a) Example case. (b) Joint and marginalized risk in the direction of the black dashed arrow in (a).

Results are summarized in Table 3. SH-MPC outperforms the baselines on almost all metrics. We observed in total 25 collisions for the deterministic baseline. The computation times of CC-MPC become excessive due to the many modes to be considered, while the computation times of SH-MPC (and S-MPCC) are unaffected.

Table 3.

Statistical results for the eight pedestrian environment under multimodal uncertainty (sec. 6.5). Displayed results follow the notation in Table 2. Our method (SH-MPC) maintains a similar joint CP and similar computation times when the probability distribution describing pedestrian motion is non-Gaussian.

Method	Max CP k /Spec. (%)	Max CP/Spec. (%)	Dur. [s]	Min Dist. [m]	Runtime (Max) [ms]
Deterministic MPC	0.9426/- (‐)	1.0000/- (‐)	11.7 (1.8)	0.12 (0.20)	204 (483)
S-MPCC (ϵ k = 0.05)	0.0042/0.0500 (8)	0.0059/- (‐)	17.6 (4.3)	0.41 (0.16)	44 (123)
CC-MPC (ϵ k = 0.05)	0.1027/0.0500 (205)	0.3175/- (‐)	16.6 (4.7)	0.16 (0.18)	100 (340)
CC-MPC (ϵ k = 0.0025)	0.0053/0.0025 (213)	0.0196/- (‐)	18.2 (4.6)	0.35 (0.13)	106 (299)
S-MPCC (ϵ k = 0.0025)	0.0002/0.0025 (6)	0.0002/- (‐)	19.6 (3.0)	0.42 (0.22)	73 (131)
CC-MPC (ϵ k = 0.0003125)	0.0007/0.0025 (28)	0.0028\/- (‐)	18.8 (4.4)	0.45 (0.12)	106 (323)
SH-MPC (ϵ = 0.05)	0.0081/- (‐)	0.0107/0.0500 (21)	16.3 (4.3)	0.36 (0.14)	27 (67)

6.7. Empirical analysis and sensitivity

6.7.1. Support estimation

We compare our proposed support estimation in equation (23) with the default Greedy algorithm of Campi et al. (2018) in a scenario with one static pedestrian. We manually vary the sample size between 100 and 1000 and collect, for 100 iterations of each sample size, the runtime of the optimization with support estimation and the estimated support size. Figure 7 shows that, while the runtime of our support estimation is negligible compared to the optimization, the greedy algorithm takes roughly S + 1 times as long to solve.

Figure 7.

The mean and standard deviation of the runtime (top) and estimated support size (bottom).

6.7.2. Empirical risk distribution

We visualize the empirical distribution of the joint CP for each value of the support estimate in Figure 8 computed for the simulations of Sec. 6.5. In line with the theory, we observe that the trajectory CP is on average higher for higher support values. Our support limit (n = 10) is reached in only 2 out of 33564 cases and is never exceeded. In the other cases, our support limit is conservative. In the simulations of Sec. 6.6, the support limit is exceeded six times in total. This usually indicates that the planner’s intended trajectory is becoming increasingly risky and is a valid cause for slowing the robot down. The same situation can result in d > 0 (i.e., infeasible optimization problem) of which we detected 50 occurrences in the same simulations.

Figure 8.

The empirical distribution of the joint CP for SH-MPC for each estimated support over 33,564 planner iterations (in simulations of Sec. 6.5). Dots denote individual planner iterations. Boxplots denote the statistics per support value.

6.7.3. Sensitivity to ϵ and N

We validate the sensitivity of SH-MPC to varying risk specifications (ϵ) and horizon lengths (N) in the scenario of Sec. 6.5. Figure 9(a) shows that reducing the specified risk results in longer task durations and that the approach becomes slightly more conservative for lower risk specifications. In line with the theory, Figure 9(b) indicates that changing the horizon length does not significantly affect the CP. Instead, the trajectories become more cautious when the same risk must be guaranteed over a longer duration.

Figure 9.

Sensitivities of the empirical CP and task duration with respect to (a) the specified CP with constant N = 20 and (b) the horizon length with constant specified CP ϵ = 0.05, evaluated over 25 experiments in the setting of Sec. 6.5. (a) Risk sensitivity. (b) Horizon sensitivity.

6.8. Real-world experiments

We validate SH-MPC in the real world by applying the planner on a mobile robot navigating among pedestrians. The experimental setup consists of a Clearpath Jackal and up to three pedestrians in a 7 m × 9 m space. We detect the positions of all agents with a motion capture system. The robot is given a reference path along the center of the space and a reference velocity of 1.5 m/s. When the robot reaches the end of the reference path, it rotates without MPC control and is given the reversed reference path. The robot is controlled at 20 Hz by SH-MPC that models the dynamics as a second-order unicycle. We specify an acceptable risk of ϵ = 0.05 over a 20 step and 4s horizon with confidence 1 − β = 0.99 and support limit $\overline{n}=6$ . The pedestrian motion predictions follow Gaussian constant velocity dynamics (Equation (25)), where an Extended Kalman Filter (EKF) is used to estimate the velocity.

Figure 10 depicts three experiments with one pedestrian. SH-MPC keeps its distance from the pedestrian so that its motion remains smooth even when the pedestrian deviates from constant velocity behavior. In Figure 10(a) and 10(b), it keeps sufficient lateral distance when passing, while in Figure 10(c), it reacts to the pedestrian speeding up its pace. Figure 11 shows three experiments with three pedestrians. The robot evades the pedestrians smoothly in this more crowded environment. In Figure 11(a) and 11(b), the robot smoothly evades multiple pedestrians. In Figure 11(c), the planner first evades two pedestrians, then reverses to ensure the safety of the third, fast-moving pedestrian.

Figure 10.

Past trajectories of the robot and one pedestrian overlayed with top-view camera images. By accounting for the possible future motion of the pedestrian, SH-MPC remains safe and smooth even when the pedestrian deviates from constant velocity behavior. (a) The pedestrian turns in a straight encounter. (b) The pedestrian turns with the robot passing from behind. (c) The pedestrian speeds up.

Figure 11.

Past trajectories of the robot and three pedestrians overlayed with top-view camera images. The robot consistently avoids collisions with pedestrians. (a) The robot evades two pedestrians smoothly. (b) During a passing maneuver, the robot keeps a sufficient distance. (c) The robot passes two pedestrians, then reverses to ensure the safety of a third, fast-moving pedestrian.

With 1 pedestrian, the support limit was never exceeded. With three pedestrians, it was exceeded 30 times in total. This could be resolved by increasing the support limit when it is exceeded. In our case, slowing down led to safe behavior. We did not observe collisions in any of the experiments. Because the proposed planner is a local planner, it does sometimes get infeasible when its intended passing maneuver becomes impossible within the specified risk. This can be resolved by considering multiple distinct maneuvers in parallel (e.g., as considered in De Groot et al. (2023)) but this is outside of the scope of this paper.

6.9. Autonomous navigation in an urban environment

SH-MPC can be applied to different robot morphologies and scenarios. To demonstrate this, we deploy our approach on a simulated self-driving vehicle in Carla simulator (Dosovitskiy et al., 2017). The dynamics are modeled with a second-order bicycle model (Kong et al., 2015) and the collision region consists of three discs. We construct a collision-free polytope for each of the discs. The pedestrians are programmed to follow the same dynamics as in Sec. 6.6, that is, a GMM modeling crossing behavior. We do not model the interaction between the vehicle and the pedestrians. The control frequency is 10 Hz. We measured the computation time to be 88 ms on average and 135 ms maximum. Figure 12 visualizes snapshots of the simulations. In Case A (see Figure 12(b)), the planner keeps enough distance to let the pedestrians cross while driving as close to the path as is safe. In Case B (see Figure 12(c)), the vehicle passes behind the pedestrians while keeping its distance from the pedestrian that is not crossing.

Figure 12.

(a) Snapshot from Carla simulations. Visualization follows Figure 4(d) for the frontal vehicle disc. (b, c) Observed trajectories of the vehicle (dark blue) and pedestrians (green) with start positions as black dots in two cases. (a) A snapshot of the proposed approach in the Carla simulator. (b) Case A: Safe overtake. (c) Case B: Passing behind.

7. Discussion

As expected from the theoretical analysis, our experiments showed that SH-MPC can consistently bound the CP of the overall trajectory. Where methods that impose marginal constraints need to decide between performance (ϵ _k = ϵ) and safety (ϵ _k = ϵ/NM), SH-MPC makes this trade-off more explicit by ensuring that the CP remains consistent under different operating conditions, such as with regards to number of obstacles, probability distributions, and the horizon length.

The proposed method is widely applicable. It handles dynamic obstacles (e.g., cyclists, cars, or non-cooperative robots) and controls systems with nonlinear dynamics. In addition, the joint distribution of the uncertainty can capture interactions of dynamic obstacles with other obstacles or the robot (e.g., to predict that pedestrians evade other pedestrians and the vehicle). It cannot yet account for interaction during planning, where the dynamics of the robot and pedestrians directly influence each other, as the probability measure $\mathbb{P}$ cannot depend on the optimization variables in scenario optimization.

7.1. Reducing conservatism

Planning performance could be improved by reducing the gap between the guaranteed and obtained risk, for example, by assuming some knowledge of the distribution or by running multiple scenario programs in parallel as considered in Mustafa et al. (2023). Over-approximating Monte Carlo sampling may be less conservative than NSO and may reduce conservatism while maintaining safety. Recent work (Brudermüller et al., 2024) proposed such an approximation. Their method is less conservative, but also computationally more demanding than SH-MPC, achieving about an order of magnitude slower computation times than SH-MPC without considering non-holonomic constraints.

The guarantees provided in this paper rely on an accurate model of the uncertainty, which may be challenging to obtain, for example, in the case of human motion prediction. Nevertheless, the proposed method provides a planner that attains a desired level of risk with respect to the predicted probability distribution. The prediction model could also be replaced by recorded samples (see De Groot et al. (2024b)), in line with the typical scenario approach, to reduce modeling errors and provide formal guarantees with respect to the true motion of the obstacles.

7.2. Computational efficiency

We showed that our approach is online capable and scalable under typical operating conditions. For extremely low risk specifications (e.g., ϵ ≤ 5 ⋅ 10⁻³), computational requirements may become excessive as a result of the increase in sample size. This can be addressed, for example, by either pruning the samples, given that only the extreme samples are of interest (see, for example, De Groot et al. (2021)), or by solving an approximate scenario optimization (Sartipizadeh and Açikmeşe, 2020). In addition, most computations of SH-MPC are parallel linear computations (for each sample), which potentially leave room for further optimization, for example, by delegating computations to a Graphical Processing Unit (GPU).

The computational efficiency of our approach derives from the merging of linear constraints which is facilitated by modeling collision regions as discs. While this modeling can be used to represent several obstacles, we currently cannot efficiently handle more complex shaped obstacles, such as objects with sharp corners. Future work could explore if other linear constraints (e.g., towards vertices of obstacles) could be merged safely. The same comment holds for extension to planning problems in higher dimensions. Directly merging halfspaces in 3D may, for example, be more complicated than in the 2D case and may require constraints of different shapes to be merged effectively.

7.3. Support limit

We showed that the support limit could be estimated online by using a solver that convexifies the problem in each iteration, such as SQP. Although this limits the generality of the support estimation, SQP solvers are a common choice for robotics applications. SH-MPC furthermore requires the user to estimate a support limit online because the support depends on the complexity of the problem to be solved. We do provide the online estimation of this single parameter, so that the user can bound it conservatively based on real-world data, enabling real-world robotics applications. Future work could explore if the support limit could be known before optimization. For example, by adapting the constraint formulation so that the support is fixed by construction (see Appendix A in Campi et al. (2018)).

8. Conclusions

Safe Horizon Model Predictive Control (SH-MPC) enabled mobile robots to constrain the joint open-loop probability of collision over the planned trajectory and with respect to all obstacles in real-time for 2D dynamic environments. It does so without making simplifying assumptions on the robot dynamics or underlying probability distribution describing the motion of the obstacles. The method uses a scenario optimization formulation where samples of the involved uncertainty are used as constraints to limit the collision probability of the motion plan. The number of samples is the main indicator for the collision probability and, under our approach, could be computed before deploying the controller.

Our results, with a mobile robot and an autonomous vehicle, showed that SH-MPC better approximates the collision probability over the duration of the motion plan than existing methods that rely on the marginal probability of collision. The main baseline, which achieved tight evaluations of the risk for each time step, was shown to be conservative over the duration of its motion plan and there was significant variation in its overall risk when we varied the underlying distribution. The overall risk of SH-MPC remained less conservative and more consistent between different environments and distributions, which resulted in faster trajectories. In addition, we showed excellent scaling of the computation time with respect to the number of obstacles and under varying distributions.

The real-time performance of SH-MPC is currently tied to a 2D environment with disc-shaped collision regions, while the probabilistic safety certification poses assumptions on the solver (satisfied by SQP) and requires the support limit to be estimated online. Our future work will try to alleviate these assumptions to further generalize our approach.

Supplemental Material