Collision Probability Estimation for Optimization-based Vehicular Motion Planning

TL;DR

The paper tackles the challenge of estimating the probability of collision (POC) for optimization-based motion planning under uncertainty. It introduces a deterministic, over-approximating POC estimator using multi-circle footprint coverings for all actors, including a Gaussian-uncertainty treatment with wrapped heading, and an analytic integration strategy. The authors demonstrate that a 3-circle approximation often suffices, yielding substantial computational gains over Monte Carlo methods while preserving safety guarantees, and integrate the estimator into a path-following stochastic SMPC to achieve reproducible, feasible trajectories under varying uncertainty. The approach significantly improves planning robustness and computational efficiency, enabling real-time, safety-focused motion planning for automated vehicles. Finally, numerical case studies validate the method against MCS and show improved feasibility and conservatism handling in an overtaking scenario.

Abstract

Many motion planning algorithms for automated driving require estimating the probability of collision (POC) to account for uncertainties in the measurement and estimation of the motion of road users. Common POC estimation techniques often utilize sampling-based methods that suffer from computational inefficiency and a non-deterministic estimation, i.e., each estimation result for the same inputs is slightly different. In contrast, optimization-based motion planning algorithms require computationally efficient POC estimation, ideally using deterministic estimation, such that typical optimization algorithms for motion planning retain feasibility. Estimating the POC analytically, however, is challenging because it depends on understanding the collision conditions (e.g., vehicle's shape) and characterizing the uncertainty in motion prediction. In this paper, we propose an approach in which we estimate the POC between two vehicles by over-approximating their shapes by a multi-circular shape approximation. The position and heading of the predicted vehicle are modelled as random variables, contrasting with the literature, where the heading angle is often neglected. We guarantee that the provided POC is an over-approximation, which is essential in providing safety guarantees. For the particular case of Gaussian uncertainty in the position and heading, we present a computationally efficient algorithm for computing the POC estimate. This algorithm is then used in a path-following stochastic model predictive controller (SMPC) for motion planning. With the proposed algorithm, the SMPC generates reproducible trajectories while the controller retains its feasibility in the presented test cases and demonstrates the ability to handle varying levels of uncertainty.

Figures (7)

• Figure 1: Schematic of the problem statement.
• Figure 2: The ego estimates and predicts the uncertainty about the object's current and future configurations. Note, the PDFs $p_{\mathbf{y}_{o,n|k}}$ may have a different functional form for each time-step, thus accommodating different possible object behaviors.
• Figure 3: (a) Problem setup for some $(\phi_o, \rho_o, \theta_o)$ and shifting the polar frame by distance $L_e$ to $\rho', \phi'$. (b) Upper-bounding $\rho'$. (c) Bounding $\theta_o$ for a given $\rho'$ and $\phi'$.
• Figure 4: Resulting POCs for varying amount of circles in three scenarios; the ego vehicle is depicted in blue and the object vehicle in red.
• Figure 5: POC over-approximation error (left) and average runtime (right). In the right figure, the red circles and triangles relate to the number of circles on the $x$-axis, whereas MCS relates to the number of samples on the $x$-axis.

Theorems & Definitions (8)

• Definition 1
• Definition 2
• Remark 1
• Remark 2
• Theorem 1
• proof
• Remark 3
• Remark 4