# Risk-Aware Nonlinear Model Predictive Control for Autonomous Navigation: Confidence-Based Obstacle Constraints and Time-To-Collision Safe Trajectory Planning

Charlotte Beaune, Elwan Héry, Vincent Frémont

► To cite this version:

Charlotte Beaune, Elwan Héry, Vincent Frémont. Risk-Aware Nonlinear Model Predictive Control for Autonomous Navigation: Confidence-Based Obstacle Constraints and Time-To-Collision Safe Trajectory Planning. 2025 IEEE Intelligent Vehicles Symposium (IV), IEEE, Jun 2025, Cluj-Napoca, Romania. pp.1633-1639, 10.1109/IV64158.2025.11097563. hal-05282985

HAL Id: hal-05282985

https://hal.science/hal-05282985v1

Submitted on 25 Sep 2025

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# Risk-Aware Nonlinear Model Predictive Control for Autonomous Navigation: Confidence-Based Obstacle Constraints and Time-To-Collision Safe Trajectory Planning

Charlotte Beaune<fnref n="1" />, Elwan Héry<fnref n="1" /> and Vincent Frémont<fnref n="1" />

*Abstract*—Nonlinear Model Predictive Control (NMPC) has established itself as a robust framework for autonomous navigation problems by effectively handling nonlinear dynamics and complex constraints in real-time. Despite its success in various applications, challenges remain in integrating robust obstacle avoidance while navigating in dynamic and uncertain environment. To enhance safety, risk assessment metrics like Time-To-Collision (TTC) are widely used for evaluating collision risks. Extensions to these metrics allow their application across diverse road scenarios, making them valuable as cost function components or constraints within risk-aware NMPC formulations. In this work, we propose novel obstacle constraint formulations that account for uncertainties in obstacle positions using confidence thresholds, along with TTC-based constraints to ensure safe trajectory planning. Our approaches are validated through extensive simulations in urban scenarios, including overtaking maneuvers and intersections, demonstrating their effectiveness in mitigating risks. Detailed implementations and results are publicly available to support further research and development.

## I. INTRODUCTION

Ensuring safe and efficient obstacle avoidance while following a path in urban environments requires a seamless integration of collision prediction and control. In such dynamic settings, intelligent vehicles must navigate through complex scenarios, where perception is affected by sensor noise, uncertainties induced by communication in Vehicle-to-Everything (V2X) systems, and the inherent unpredictability of moving obstacles. An approach to addressing these challenges is to use risk-aware planning strategies, where risk constraints help balance safety and performance. However, integrating such metrics into real-time control introduces additional constraints, necessitating methods that can handle both safety requirements and navigation objectives in uncertain environments.

### A. Related works

The prominent use of Nonlinear Model Predictive Control (NMPC) formulations for autonomous navigation problems can be explained by several factors. The path-following problem in autonomous navigation can be easily modelized as an Optimal Control Problem (OCP) [1] with quadratic objectives functions. Moreover, kinematic and dynamic models such as the bicycle model or unicycle model [2] are quite simple and accurate, yet nonlinear, and are used to predict the motion of most mobile robots. NMPC solvers

can handle these nonlinear dynamics as well as complex constraints, while operating in real-time [3]. These have been successfully deployed in various autonomous vehicles related problems, such as global path planning [4][5] or path following time-optimal control for race-cars [6].

The use of NMPC for path following is often coupled with the problem of obstacles avoidance. While the use of the curvilinear frame (or Frénet frame) has been widely used in order to simplify the characterization of the reference path [7][8][9][10], using this frame has made the definition of the obstacles avoidance constraints more complex. However, the authors in [11] have shown that it is possible to characterize obstacle avoidance constraints in the Cartesian frame, allowing the problem to be easily understandable and parametrized.

Various formulations exist for modeling obstacle avoidance constraints. Over-approximated geometries provide a straightforward implementation but result in overly conservative maneuvers. Less conservative approaches, including the triangle area criterion [12], convex hulls [4] or separating hyperplanes, offer greater flexibility but are more challenging to implement, as they require augmenting the NMPC problem with nested optimization problems. For dynamic obstacles, these formulations are often paired with obstacle motion prediction to ensure the safety of the planned trajectory at each prediction step. In [5], authors proposed a method that guarantees safety even between adjacent collocation points.

<figure><img src="image_2.png" /><figcaption>Fig. 1: Solution of the NMPC solver for ego-vehicle trajectory along with predicted obstacles trajectories for an overtaking maneuver. The predicted vehicle poses, bounding circles, and bounding boxes transition from orange (initial prediction at step 0) to purple (final prediction step).</figcaption></figure>

<footnote n="1">¹Authors are with Laboratoire des Sciences du Numérique de Nantes (LS2N), Ecole Centrale de Nantes name.surname@ls2n.fr</footnote>
Moreover, integrating uncertain dynamics of the obstacles in the obstacle avoidance problem is a key challenge and is typically done by using probabilistic approaches including Monte-Carlo methods [13], Tubes or Gaussian Processes. Chance constraints that are integrated into planning problems ensure probabilistic safety by setting an upper limit on the collision probability of the planned trajectory [14]. While Gaussian Processes (GP) can manage multiple realizations of obstacle motions [15], the processes must be linearized and pruned to mitigate excessive computational load. A challenge is to develop formulations that balance computational simplicity with accurate obstacle representation while maintaining real-time performance in highly dynamic environments. Current approaches tend to focus on either accurate definition of the obstacles or probabilistic safety constraints, but not necessarily both in a unified framework. In [16], authors proposed an approximation of the confidence region for the ego-vehicle position to enforce track constraints. We will use an analogous methodology to define our obstacle avoidance constraints.

Several risk indicators can assess collision risk by analyzing planned trajectories and key factors such as vehicle velocity, overlap, or simultaneous occupancy probability, providing critical information for mitigating potential collisions [17]. Time-based metrics like Time-To-Collision (TTC) and Time-To-React (TTR) are widely used, offering insights into the time remaining to avoid a collision or the time available for a corrective action [18]. In [19], TTC metric was extended to accommodate various road scenarios, moving beyond its traditional application in platooning maneuvers. TTC can also be utilized to identify the safest maneuver for an autonomous vehicle, making it a valuable component as either a cost function factor or a constraint in risk-aware NMPC formulations [20]. Yet, challenges remain in further leveraging TTC as a safety constraint. Hybrid approaches that combine TTC with other risk assessment metrics, such as minimum distance to obstacles, is required for a more comprehensive safety evaluation in highly dynamic environments.

## B. Contributions

Our contributions are as follows:

1) We propose obstacle constraint formulations that account for uncertainties in obstacle positions by incorporating a confidence threshold, based on Gaussian distribution,

2) We develop Time-To-Collision constraint formulations aimed at ensuring the safety of planned trajectories,

3) We conduct extensive simulation experiments in urban scenarios, including overtaking maneuvers and unsignalized crossroad interactions, to evaluate autonomous vehicle safety. Furthermore, we provide an in-depth discussion on problem parametrization.

Additionally, detailed code implementations and results are available at https://github.com/CBeaune/mpc-ttc.git.

## II. PROBLEM STATEMENT

In this section, we define the ego-vehicle's motion using a dynamic bicycle model in curvilinear coordinates for path-following and Cartesian for obstacle avoidance. While we adopt this model, others can be used depending on the chosen vehicle or robot. We also formulate the NMPC problem and detail our approach to predicting dynamic obstacles and their uncertainties for safe, efficient planning.

<figure><img src="image_3.png" /><figcaption>Fig. 2: Frenet states for bicycle evolution model, the parameter $n$ is the closest distance from center of mass $\mathbf{p}$ to the reference path point $\mathbf{p}_{ref}(s)$. The ego-vehicle must follow curve path $\mathcal{C}(s)$</figcaption></figure>

### A. Vehicle Evolution model

The evolution function of the ego-vehicle's Cartesian states is based on the dynamic bicycle model with lateral forces neglected. However, since the task of path following involves following a curve, using a curvilinear coordinate system is a more practical choice [6]. The curve is described by its arc length $s$, its curvature along the path $\kappa(s)$ and associated orientation to the global frame $\theta_{ref}(s)$. We describe a point in this coordinate system by its position along the curvilinear abscissa $s$, its lateral offset relative to the center of the curve $n$, and its relative orientation with respect to the curve tangent $\theta_e = \theta - \theta_{ref}$. Hence, the evolution model for the Frenet states $\mathbf{x}$ is:

$$ \dot{\mathbf{x}} = g(\mathbf{x}, \mathbf{u}) = \begin{bmatrix} \dot{s} \\ \dot{n} \\ \dot{\theta}_e \\ \dot{v} \end{bmatrix} = \begin{bmatrix} \frac{v \cos(\theta_e + \beta)}{1 - n \kappa(s)} \\ \frac{v \sin(\theta_e + \beta)}{\dot{\theta}_e - \kappa(s) \cdot \dot{s}} \\ \frac{F_{xd}}{m} \cos(\beta) \end{bmatrix} \quad (1) $$

where $F_{xd}$ is the longitudinal forces on the tires (as a function of the throttle $D$), $m$ is the mass of the vehicle and the slide-slip angle $\beta \approx \frac{l_r}{l_r + l_f} \delta$, assuming small steering angle $\delta$. The control inputs of the system are $\mathbf{u} = (D, \delta)$.

For the rest of the paper, the formulation $\mathbf{x}$ will be used to denote Frenet states of the ego-vehicle, whereas $\mathbf{p}$ and $\mathbf{p}^{obs}$ will denote the Cartesian positions of the ego-vehicle and the obstacle respectively. Moreover, it is assumed that the Cartesian position of the ego-vehicle is perfectly known.
## B. Dynamic Obstacles model and covariance estimation

In order to define the free space for the obstacle avoidance constraints, we need to predict the motion of the obstacles during look-ahead time $T = Ndt$. At each time step $k \in [0, N]$, we evaluate the distributions of the obstacles positions in Cartesian frame $\mathbf{p}_k^{\text{obs}} = (x_k^{\text{obs}}, y_k^{\text{obs}})^T$ as Gaussian. For a given obstacle at time step $k$, $\bar{\mathbf{p}}_k^{\text{obs}} \sim \mathcal{N}(\bar{\mathbf{p}}_{\text{obs}}^k, \Sigma_k)$. The dynamics for mean $\bar{\mathbf{p}}_k^{\text{obs}}$ and covariance matrix $\Sigma_k \in \mathbb{R}^2$ are given as [21][16]:

$$ \bar{\mathbf{p}}_{k+1}^{\text{obs}} = f(\bar{\mathbf{p}}_k^{\text{obs}}, u_0^{\text{obs}}) \quad (2) $$

$$ \Sigma_{k+1} = A_k \Sigma_k A_k^T + \Sigma_{\alpha_k} \quad (3) $$

with $\Sigma_{\alpha_k}$ being the covariance matrix of a white Gaussian noise, $A_k = \frac{\partial f}{\partial \mathbf{p}_k^{\text{obs}}}(\bar{\mathbf{p}}_k^{\text{obs}}, u_0^{\text{obs}})$ and controls $u_0^{\text{obs}} = (v_0^{\text{obs}}, \omega_0^{\text{obs}})$. The evolution function $f(\bar{\mathbf{p}}_k^{\text{obs}}, u_0^{\text{obs}})$ is:

$$ \begin{bmatrix} \bar{\mathbf{p}}_{k+1}^{\text{obs}} \\ \theta_{k+1}^{\text{obs}} \end{bmatrix} = \begin{bmatrix} \bar{x}_{k+1}^{\text{obs}} \\ \bar{\theta}_{k+1}^{\text{obs}} \end{bmatrix} = \begin{bmatrix} \bar{x}_k^{\text{obs}} + v_0^{\text{obs}} dt \cdot \cos \theta_0^{\text{obs}} \\ \bar{y}_k^{\text{obs}} + v_0^{\text{obs}} dt \cdot \sin \theta_0^{\text{obs}} \\ \theta_k^{\text{obs}} + \omega_0^{\text{obs}} dt \end{bmatrix} \quad (4) $$

We make the assumption that the motion of the obstacles centers during $[0, T]$ is a straight motion with perfectly known and constant orientation $\omega_0^{\text{obs}} = 0$ and $\theta_k^{\text{obs}} = \bar{\theta}_k^{\text{obs}} = \theta_0^{\text{obs}}$.

# III. PROPOSED APPROACH

## A. Problem Formulation

To ensure smoothness of the vehicle control, we extend the NMPC states $(\mathbf{x}, \mathbf{u})^T = (s, n, \theta_e, v, D, \delta)^T$ and controls to $\mathbf{u}_{\Delta} = (\dot{D}, \dot{\delta})^T$. We also note $\mathbf{X}_N = (\mathbf{x}_0, \dots, \mathbf{x}_N)$, $\mathbf{U}_N = (\mathbf{u}_0, \dots, \mathbf{u}_N)$ and $\mathbf{U}_{\Delta,N} = (\mathbf{u}_{\Delta,0}, \dots, \mathbf{u}_{\Delta,N})$. Note that the effective controls of the vehicles are $(D, \delta)$, the vehicle throttle and the steering angle respectively. Given the previous definitions, the NMPC Problem can be formulated as:

$$ \min_{\mathbf{X}_N, \mathbf{U}_N, \mathbf{U}_{\Delta,N}} J(\mathbf{X}_N, \mathbf{U}_N, \mathbf{U}_{\Delta,N}) \quad (5a) $$

$$ \text{subject to} \quad \dot{\mathbf{x}} = g(\mathbf{x}, \mathbf{u}) \quad (5b) $$

$$ \dot{\mathbf{u}} = \mathbf{u}_{\Delta} \quad (5c) $$

$$ \mathbf{u}_{\Delta, \text{min}} \le \mathbf{u}_{\Delta, k} \le \mathbf{u}_{\Delta, \text{max}} \quad | \ k \in [0, N] \quad (5d) $$

$$ \mathbf{x}_{\text{min}} \le \mathbf{x}_k \le \mathbf{x}_{\text{max}} \quad | \ k \in [0, N] \quad (5e) $$

$$ \mathbf{x}_0 = \mathbf{x}_{\text{init}}, \quad u_0 = u_{\text{init}} \quad (5f) $$

$$ \mathbf{x}_k \in \bigcap_{i=1}^{n_{\text{obs}}} \mathcal{X}_{\text{FREE},i}(k) \quad | \ k \in [0, N] \quad (5g) $$

$$ \mathbf{x}_k \in \bigcap_{i=1}^{n_{\text{obs}}} \mathcal{X}_{\text{TTC},i}(k) \quad | \ k \in [0, N] \quad (5h) $$

1) States and controls constraints formulation: To ensure road-boundaries constraints and that overtaking is done with

respect to the driving rules, we formulate the following states (6) and controls (7) constraints:

$$ \begin{bmatrix} n_{\text{min}} \\ v_{\text{min}} \\ D_{\text{min}} \\ \delta_{\text{min}} \\ a_{\text{min}}^{\text{lat}} \\ a_{\text{min}}^{\text{long}} \end{bmatrix} < \begin{bmatrix} n \\ v \\ D \\ \delta \\ a_{\text{lat}}(\mathbf{x}) \\ a_{\text{long}}(\mathbf{x}) \end{bmatrix} < \begin{bmatrix} n_{\text{max}} \\ v_{\text{max}} \\ D_{\text{max}} \\ \delta_{\text{max}} \\ a_{\text{max}}^{\text{lat}} \\ a_{\text{max}}^{\text{long}} \end{bmatrix} \quad (6) $$

$$ \begin{bmatrix} \dot{D}_{\text{min}} \\ \dot{\delta}_{\text{min}} \end{bmatrix} < \begin{bmatrix} \dot{D} \\ \dot{\delta} \end{bmatrix} < \begin{bmatrix} \dot{D}_{\text{max}} \\ \dot{\delta}_{\text{max}} \end{bmatrix} \quad (7) $$

The constraints on $D, \dot{D}, \delta, \dot{\delta}, a_{\text{lat}}(\mathbf{x})$, and $a_{\text{long}}(\mathbf{x})$ represent the operational limitations of the vehicle. Given that we are operating in an environment similar to the one depicted in Fig. 1:

$$ n_{\text{min}} = \frac{-w + W}{2}, \quad n_{\text{max}} = \frac{2w - W}{2} \quad (8) $$

With $w$ being the width of the lane and $W$ the width of the car. Additionally, $v_{\text{max}}$ is parametrized as:

$$ v_{\text{max}} = \begin{cases} v_0^{\text{obs}} & \text{if } v < (v_0^{\text{obs}} + v_{\text{allow}}^{\text{max}} + dv)/2 \\ v_{\text{max}}^{\text{allow}} & \text{else} \end{cases} \quad (9) $$

with $v_{\text{max}}^{\text{allow}}$ being the maximum allowed velocity. Equations 8 and 9 define the constraints applicable to an urban environment, corresponding to lane-keeping and overtaking conditions, respectively.

2) Cost function formulation: We discretize our problem in N collocation points and we use a quadratic cost function

$$ J(\mathbf{x}_0, \mathbf{x}_1, \dots, \mathbf{x}_{N-1}, \mathbf{x}_N, u_0, u_1, \dots, u_{N-1}) = \sum_{0}^{N-1} (\|\mathbf{x}_k - \mathbf{x}_k^{\text{ref}}\|_{\mathbf{Q}}^2 + \|u_k\|_{\mathbf{R}}^2) + \|\mathbf{x}_N - \mathbf{x}_N^{\text{ref}}\|_{\mathbf{Q}_e}^2 \quad (10) $$

with $\mathbf{Q} = \text{diag}(q)$, $\mathbf{Q}_e = \text{diag}(q_e)$ and $\mathbf{R}$ being the weight matrices of the state, terminal state and control constraints respectively.

In the following sections, we will present our methodology to take into account obstacle uncertainties (Eq. 5g) in the trajectory planning problem as well as our approach to define TTC constraints (Eq. 5h).

## B. Risk-aware obstacle constraints formulation

In the following section, indices $k$ are omitted for clarity. As shown in [11], we can formulate the obstacle avoidance constraints directly in Cartesian frame. This allows us to simply modelize the obstacles avoidance constraints. We make the assumption that the geometry of the obstacles is rectangular with width and length $W$ and $L$ respectively. For each obstacle pose $(\mathbf{p}_k^{\text{obs}}, \theta^{\text{obs}})$ in the Cartesian frame, we can attach $n_{\text{circles}}^{\text{obs}}$ bounding circles of radius $r = \frac{W^2 + L^2}{4L}$ (see Fig. 3a for reference). This allows us to encapsulate the geometries of the obstacles along with road safety consideration for overtaking.
<figure><img src="image_4.png" /></figure>

(a) Bounding circles parametrization for obstacles. Circles centers $\mathbf{p}_i^{\text{obs}}$ are located at $\{(-\frac{L}{2}, 0), (-r, 0), (0, 0), (r, 0), (\frac{L}{2}, 0)\}$ in the vehicle frame.

<figure><img src="image_5.png" /></figure>

(b) Bounding ellipses parametrization for obstacles with confidence ellipses $\mathcal{E}_i$ for a given confidence level $\eta$. The red-filled ellipse represent the position confidence ellipse of the center of mass $\mathbf{p}_i^{\text{obs}}$ of the obstacle. Rear ellipses are omitted for clarity.

<caption>Fig. 3: Bounding circles and ellipses representations for uncertainties-aware obstacle constraints formulation</caption>

We have access to the pair $(\bar{\mathbf{p}}_k^{\text{obs}}, \Sigma^{\text{obs}})$, we can associate a confidence ellipse in $\mathbb{R}^2$ that will contain for a confidence level $\eta \in [0, 1]$ consistent values for $x^{\text{obs}}, y^{\text{obs}}$, with semi-axes $(\alpha_\eta, \beta_\eta)$ orientation $\phi_\eta$. The Appendix provides additional mathematical details for further clarification and reference. We can attach to each bounding circle an extended ellipse increased by radius $r$ (see Fig. 3b). In the same way, ego-vehicle geometry is encapsulated by $n_{\text{circles}}^{\text{ego}}$ bounding circles of radius $r$, so that the extended ellipse matrix representation is:

$$ \mathcal{E}_i = \mathbf{R}_{\phi_\eta} \begin{bmatrix} \frac{1}{(\alpha_\eta + 2r + d_{\text{safe}})^2} & 0 \\ 0 & \frac{1}{(\beta_\eta + 2r + d_{\text{safe}})^2} \end{bmatrix} \mathbf{R}_{\phi_\eta}^T \quad (11) $$

with $\mathbf{R}_{\phi_\eta}$ being the rotation matrix associated with the yaw angle $\phi_\eta$.

The free collision space at time step $k$ is defined as:

$$ \mathcal{X}_{\text{FREE}} = \{ \mathbf{x} \mid (\mathbf{p}_j - \mathbf{p}_i^{\text{obs}})^T \mathcal{E}_i (\mathbf{p}_j - \mathbf{p}_i^{\text{obs}}) \ge 1 \} \\ \quad \forall i \in \{1, n_{\text{circles}}^{\text{obs}}\} \\ \quad \forall j \in \{1, n_{\text{circles}}^{\text{ego}}\} \quad (12) $$

Thus, we define $n_{\text{obs}}$ free collision spaces attached to each obstacles in the ego-vehicle surroundings.

### C. Time-to-Collision constraints formulation

Considering the bounding ellipses defined in III-B, the distance $d_{ij}(t)$ between circle center $\mathbf{p}_j$ and the extended ellipse centered at $\mathbf{p}_i^{\text{obs}}$ at time $t$ follows:

$$ d_{ij}^2(t) = (\mathbf{p}_j(t) - \mathbf{p}_i^{\text{obs}}(t))^T \mathcal{E}_i(\mathbf{p}_j(t) - \mathbf{p}_i^{\text{obs}}(t)) \quad (13) $$

with $\mathcal{E}_i$ being the matrix representation of the extended ellipse attached to bounding circle $i$. In the same manner as in [19], we differentiate both sides of Eq (13) with respect to time:

$$ \frac{\partial d_{ij}}{\partial t}(t) = (\mathbf{p}_j(t) - \mathbf{p}_i^{\text{obs}}(t))^T \mathcal{E}_i(\mathbf{v}(t) - \mathbf{v}^{\text{obs}}(t)) / d_{ij}(t) \quad (14) $$

where, $\forall t \in [0, T]$:

$$ \mathbf{v}^{\text{obs}} = \begin{bmatrix} \mathbf{v}^{\text{obs}} \cos(\theta_0^{\text{obs}}) \\ \mathbf{v}^{\text{obs}} \sin(\theta_0^{\text{obs}}) \end{bmatrix} $$

and

$$ \mathbf{v} = \begin{bmatrix} \mathbf{v}(t) \cos \theta(t) \\ \mathbf{v}(t) \sin \theta(t) \end{bmatrix} $$

Using the first-order Taylor series approximation, we approximate the function $d_{ij}(t)$:

$$ d_{ij}(t) = \frac{\partial d_{ij}(0)}{\partial t} \cdot t + d_{ij}(0) \quad (15) $$

Thus, solving this to find time where $d_{ij}(t) = 1$:

$$ \text{TTC}(\mathbf{p}_j, \mathbf{p}_i^{\text{obs}}) = \begin{cases} \frac{1 - d_{ij}(0)}{\frac{\partial d_{ij}(0)}{\partial t}} & \text{if } \dot{d}_{ij} < 0 \\ +\infty & \text{else} \end{cases} \quad (16) $$

with the case $\frac{\partial d_{ij}(0)}{\partial t} = 0$ corresponding to the case when the vehicles move along the same direction with the same velocity, $\frac{\partial d_{ij}(0)}{\partial t} > 0$ when vehicles are moving away from each other. The safe Time To Collision space at timestep $k$ is defined as:

$$ \mathcal{X}_{\text{TTC}} = \{ \mathbf{x} \mid \text{TTC}(\mathbf{p}_j, \mathbf{p}_i^{\text{obs}}) \ge \text{TTC}_{\text{min}} \} \\ \quad \forall i \in \{1, \dots, n_{\text{circles}}^{\text{obs}}\} \\ \quad \forall j \in \{1, \dots, n_{\text{circles}}^{\text{ego}}\} \quad (17) $$

## IV. RESULTS

### A. Experimental setup

We transform the NMPC problem presented in (14) into a discretized system with $N$ shooting nodes and a discretization timestep $dt$ (see Table I for problem parameters). We use the ACADOS [3] solver with HPIPIM and RTI iterations. Vehicles are modelized by miniature race-cars [6] inside a miniature urban track. To evaluate the performance of our approach and the addition of the TTC constraints in the problem formulation, we simulate an urban environment with 3 scenarios: (S1) Overtaking a dynamic obstacle while a vehicle is coming from the other lane, (S2) an unsignalized intersection and turning right, and (S3) an unsignalized Intersection and turning left. We made the following assumptions
to simplify our driving problem. Firstly the pose of the ego-vehicle is perfectly known and that the evolution model used in the NMPC problem perfectly matches the one used for the simulation. Then, the obstacles expected poses and covariance are all retrieved in a synchronous manner every *dt*. Their position is perturbed with Gaussian white noise of a known covariance $Σ_α$. Moreover the obstacles are non-reactive and the simulation ends when a collision is detected. For each scenario, we run 100 experiments and evaluate them on several performances and safety metrics. Safety metrics are the minimum estimated TTC and average minimum distance to obstacles, computed offline after the simulation run, as well as the total percentage of collision. Performances metrics include average speed, goal completion as well as the percentage of iterations the controller returns an error when the problem becomes unfeasible. We compare our proposed *NMPC-TTC* method with two other approaches: (1) a baseline that incorporates only distance constraints to obstacles without accounting for obstacle uncertainties [11], using a safety distance of $d_{safe} = 0.3r$, and (2) the same baseline augmented by the uncertainty ellipse (as detailed in Sec. III-B) that we will refer as *NMPC-dist-extended*. In contrast, the *NMPC-TTC* method sets the safety distance to $d_{safe} = 0.0m$ while enforcing a minimum time-to-collision constraint of $TTC_{min} = 0.75s$. We implemented a relaunch of the solver every time there are freeze steps during the simulation.

<table><caption>TABLE I: NMPC parameters for the simulation setup</caption><thead><tr><th>Name</th><th>Variables</th><th>Value</th></tr></thead><tbody><tr><td>Number of obstacles</td><td>n<sub>circles</sub><sup>obs</sup></td><td>5</td></tr><tr><td>extended ellipses</td><td></td><td></td></tr><tr><td>Number of ego-vehicle</td><td>n<sub>circles</sub><sup>ego</sup></td><td>3</td></tr><tr><td>extended ellipses</td><td></td><td></td></tr><tr><td>Vehicle length</td><td>L</td><td>0.25 [m]</td></tr><tr><td>Vehicle width</td><td>W</td><td>0.15 [m]</td></tr><tr><td>Road width</td><td>w</td><td>0.3 [m]</td></tr><tr><td>Prediction horizon</td><td>N</td><td>5.0 [s]</td></tr><tr><td>Discretization timestep</td><td>dt</td><td>0.1 [s]</td></tr><tr><td>reference velocity</td><td>v<sub>ref</sub></td><td>0.25 [m/s]</td></tr><tr><td></td><td>nh</td><td>45</td></tr><tr><td>Num. of hard constraints</td><td>(baseline)</td><td></td></tr><tr><td></td><td>nh</td><td>96</td></tr><tr><td></td><td>(NMPC-TTC)</td><td></td></tr><tr><td></td><td>ns</td><td>5</td></tr><tr><td>Num. of soft constraints</td><td>(baseline)</td><td></td></tr><tr><td></td><td>ns</td><td>5</td></tr><tr><td></td><td>(NMPC-TTC)</td><td></td></tr><tr><td>state weights</td><td>q ∈ ℝ<sup>6</sup></td><td>[5, 5, 1, 1, 1, 1]</td></tr><tr><td>terminal state weights</td><td>q<sub>e</sub> ∈ ℝ<sup>6</sup></td><td>[1, 10, 1, 1, 1, 1]</td></tr><tr><td>control weights</td><td>R ∈ ℝ<sup>2×2</sup></td><td>I</td></tr></tbody></table>

## B. Simulation Results

1) **Robustness:** To test the robustness of our method against obstacle pose uncertainty, we simulate Gaussian white noise for the obstacles positions to emulate sensor noise. Table III shows the results of the different methods when covariance matrix of the added noise is set to $Σ_α = 0$. In

<table><caption>TABLE II: Initial parameters values for obstacles position ($p^{obs1}$, $p^{obs2}$) and initial position $p_0$ are uniformly chosen in the given intervals, in SI units for the tested scenarios.</caption><thead><tr><th>Scenario</th><th>Variable</th><th>Value</th></tr></thead><tbody><tr><td rowspan="4">(S1)</td><td>p<sub>0</sub><sup>obs1</sup></td><td>[-0.1, -0.1] - [0.1, 0.1]</td></tr><tr><td>p<sub>0</sub><sup>obs2</sup></td><td>[0.3, 0.3] - [1.0, 0.3]</td></tr><tr><td>p<sub>0</sub></td><td>[-1.2, 0.0] - [-1.0, 0.0]</td></tr><tr><td>v<sub>0</sub><sup>obb</sup></td><td>[0.0, 0.1]</td></tr><tr><td rowspan="4">(S2)</td><td>p<sub>0</sub><sup>obs1</sup></td><td>[1.2, 0.5] - [1.3, 1.0]</td></tr><tr><td>p<sub>0</sub><sup>obs2</sup></td><td>[1.57, -1.0] - [1.6, -0.5]</td></tr><tr><td>p<sub>0</sub></td><td>[-0.1, 0.0] - [0.1, 0.0]</td></tr><tr><td>v<sub>0</sub><sup>obb</sup></td><td>0.15</td></tr><tr><td rowspan="4">(S3)</td><td>p<sub>0</sub><sup>obs1</sup></td><td>[1.45, 0.5] - [1.55, 1.0]</td></tr><tr><td>p<sub>0</sub><sup>obs2</sup></td><td>[1.7, 0.5] - [1.8, 1.0]</td></tr><tr><td>p<sub>0</sub></td><td>[-0.1, 0.0] - [0.1, 0.0]</td></tr><tr><td>v<sub>0</sub><sup>obb</sup></td><td>0.15</td></tr></tbody></table>

this case, the baseline and the NMPC-dist-extended method, gives the same results. Covariance matrix of the added noise is set to $Σ_α = 0.05^2I$.

2) **TTC consideration:** Results of the quantitative tests for 100 simulated scenes per scenario are presented Table IV. This shows the results for the baseline method (distance constraints to obstacles) and for our proposed approach that include TTC consideration into the NMPC problem. Overall the percentage of collision is reduced with NMPC-TTC method and longitudinal velocity tends to be lower, as the vehicle will be more cautious around obstacles. However, our method leads to more freezing steps, as the problem is more constrained and thus, leads to unfeasible solutions when being close to the constraints limits. Computation time tends also to be higher, because number of constraints is larger in our proposed approach, but in average still acceptable for real-time control of the vehicle. Our method appears to align more effectively with safety and performance criteria in Scenario (S3), the most critical case, where the vehicle must navigate while avoiding two moving obstacles. Relaunch of the solver leads to higher computational overload, because the NMPC problems need to be re-created when doing so. This explains high computational time when the problem is over-constrained, especially for scenario (S1) and (S3). Moreover, NMPC-dist-extended proposed method frequently resulted in collisions with the lead vehicle during simulations causing poor results on scenario (S3). Snapshots of the experiments on scenario (S3) are presented Fig 4c.

3) **Confidence levels:** At higher confidence levels, the planned trajectories of NMPC-dist-extended and NMPC-TTC become more conservative, leading to a more cautious ego-vehicle behavior that ensures constraint satisfaction. Conversely, at lower confidence levels, the trajectories are less conservative. This highlights the potential for an adaptive planning framework that accounts for uncertainties introduced by detection and tracking modules.

4) **Limitations and future works:** While the proposed method guarantees that Time-to-Collision (TTC) constraints in urban scenarios are respected when planning trajectories,
<table><caption>TABLE III: Results of the quantitative tests for fixed confidence level $\eta = 0.9545$ and for scenario (S3): the safety metrics and the performance metrics are evaluated on 100 scenarios for obstacles' position covariance $\Sigma_0^{\text{obs}} = 0$. Safety metrics are : the average minimum distance to obstacles, average minimum TTC to obstacles and the collision rate. The performance metrics are : the average/maximum computation time for the solver step, the average longitudinal velocity, the completion rate and the freeze steps rate. For this case, the baseline and the approach MPC-dist extended gives the same results.</caption><thead><tr><th>Method</th><th>Avg. min.<br>Min. dist [m]</th><th>Avg. Min.<br>TTC [s]</th><th>Coll.</th><th>Avg. tcomp /<br>Max. tcomp [s]</th><th>Avg.<br>vel. [m/s]</th><th>Compl.</th><th>Freeze<br>steps</th></tr></thead><tbody><tr><td>baseline</td><td>0.125/0.063</td><td>1.135</td><td>0%</td><td>0.056 / 0.388</td><td>0.166</td><td>72%</td><td>0%</td></tr><tr><td>NMPC-dist extended</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr><tr><td>NMPC-TTC</td><td><b>0.338/0.279</b></td><td><b>3.687</b></td><td>0%</td><td>0.065 / 0.583</td><td>0.125</td><td>0%</td><td>0%</td></tr></tbody></table>

<table><caption>TABLE IV: Results of the quantitative tests for fixed confidence level $\eta = 0.9545$: For each scenario (S1), (S2), (S3), the safety metrics and the performance metrics are evaluated on 100 scenarios. Safety metrics are : the average minimum distance to obstacles, average minimum TTC to obstacles and the collision rate. The performance metrics are : the average/maximum computation time for the solver step, the average longitudinal velocity, the completion rate and the freeze steps rate.</caption><thead><tr><th></th><th>Method</th><th>Avg. min. /<br>Min. dist [m]</th><th>Avg. Min.<br>TTC [s]</th><th>Coll.</th><th>Avg. tcomp /<br>Max. tcomp [s]</th><th>Avg.<br>vel. [m/s]</th><th>Compl.</th><th>Freeze<br>steps</th></tr></thead><tbody><tr><td rowspan="3">(S1)</td><td>baseline</td><td>0.066 / 0.005</td><td>1.174</td><td>20%</td><td>0.057 / 0.15</td><td>0.096</td><td>72%</td><td>12%</td></tr><tr><td>NMPC-dist-extended</td><td>0.066 / 0.005</td><td>1.191</td><td>19%</td><td>0.06 / 0.199</td><td>0.095</td><td>71%</td><td>14%</td></tr><tr><td>NMPC-TTC</td><td>0.081 / 0.005</td><td><b>1.664</b></td><td><b>13%</b></td><td>0.078 / 0.378</td><td>0.084</td><td>66%</td><td>18%</td></tr><tr><td rowspan="3">(S2)</td><td>baseline</td><td>0.070 / 0.057</td><td>0.888</td><td>0%</td><td>0.057 / 0.619</td><td>0.149</td><td>100%</td><td>0%</td></tr><tr><td>NMPC-dist-extended</td><td>0.071 / 0.017</td><td>0.894</td><td>0%</td><td>0.06 / 0.645</td><td>0.150</td><td>100%</td><td>0%</td></tr><tr><td>NMPC-TTC</td><td><b>0.279 / 0.22</b></td><td><b>3.426</b></td><td>0%</td><td>0.079 / 0.702</td><td>0.131</td><td>100%</td><td>&gt;0.1%</td></tr><tr><td rowspan="3">(S3)</td><td>baseline</td><td>0.125 / 0.063</td><td>1.135</td><td>0%</td><td>0.056 / 0.841</td><td>0.166</td><td>100%</td><td>0%</td></tr><tr><td>NMPC-dist-extended</td><td>0.026 / 0.005</td><td>0.168</td><td>76%</td><td>0.058 / 0.723</td><td>0.193</td><td>76%</td><td>0%</td></tr><tr><td>MPC-TTC</td><td><b>0.254 / 0.01</b></td><td><b>3.306</b></td><td>1%</td><td>0.074 / 0.50</td><td>0.163</td><td>100%</td><td>0.2%</td></tr></tbody></table>

<figure><img src="image_6.png" /><figcaption>Fig. 4: Snapshots of experiments for scenario (S3) for three steps of simulation for method NMPC-TTC. The predicted vehicle poses, bounding circles, and bounding boxes transition from orange (initial prediction step) to purple (final prediction step).</figcaption></figure>

<table><caption>TABLE V: Results of the quantitative tests for scenario (S3): For different confidence levels $\eta$, the safety metrics and the performance metrics are evaluated on 100 scenarios. Safety metrics are : the average minimum distance to obstacles, average minimum TTC to obstacles and the collision rate. Performance metrics are : the average/maximum computation time for the solver step, the average longitudinal velocity, the completion rate and the freeze steps rate.</caption><thead><tr><th>η</th><th>Method</th><th>Avg. min./<br>Min. dist. [m]</th><th>Avg. Min.<br>TTC [s]</th><th>Coll.</th><th>Avg. /<br>Max. tcomp. [s]</th><th>Avg.<br>vel. [m/s]</th><th>Compl.</th><th>Freeze<br>steps</th></tr></thead><tbody><tr><td rowspan="2">0.68</td><td>NMPC-dist-extended</td><td>0.026/0.005</td><td>0.169</td><td>76%</td><td>0.057/0.475</td><td>0.193</td><td>75%</td><td>0</td></tr><tr><td>NMPC-TTC</td><td>0.261/0.005</td><td><b>3.326</b></td><td><b>1%</b></td><td>0.073/0.717</td><td>0.163</td><td><b>99%</b></td><td>1%</td></tr><tr><td rowspan="2">0.99</td><td>NMPC-dist-extended</td><td>0.051/0.005</td><td>0.855</td><td>76%</td><td>0.055/0.612</td><td>0.182</td><td>85%</td><td>0%</td></tr><tr><td>NMPC-TTC</td><td>0.262/0.005</td><td><b>3.363</b></td><td><b>1%</b></td><td>0.066/0.483</td><td>0.163</td><td>79%</td><td>0.8%</td></tr></tbody></table>
it has limitations that require further investigation. First, the approach exhibits a computational overhead compared to the baseline, which may impact real-time applicability. Second, due to strict constraints, the optimization occasionally encounters infeasible solutions. Addressing this issue requires constraint handling strategies to enhance feasibility and recovery steps in case the solver fails. Ongoing works focus on improving computational efficiency and real-time performance in both simulation environments and real experiments.

## V. CONCLUSION

In this paper, we presented a methodology for path following in urban environments using a NMPC problem formulation, integrating probabilistic obstacle avoidance constraints to account for dynamic obstacles. Additionally, we incorporated Time-to-Collision (TTC)-based constraints to enhance risk-aware trajectory planning. The proposed approach demonstrated its effectiveness in critical scenarios, though further improvements are required to reduce and manage infeasibility issues and improve computational efficiency.

Future work will focus on real-time implementation and the use of more accurate motion and TTC prediction methods to include vehicle's dynamics, as well as the development of collaborative strategies to enhance safety and robustness, ensuring more reliable and adaptive trajectory planning in complex urban settings.

## APPENDIX

### A. Threshold Calculation for Confidence Ellipse with Gaussian Probabilistic Interpretation

For a bivariate normal distribution with covariance matrix $Σ$, the confidence ellipse's semi-axes ($α_η, β_η$) and angle $φ_η$ are:

$$ α_η = ||Av_1||, \quad β_η = ||Av_2||, \quad φ_η = \tan^{-1}(\frac{v_{1,y}}{v_{1,x}}) \quad (18) $$

where $v_1$ and $v_2$ are the eigenvectors of the positive definite matrix $A = a_η \cdot \sqrt{Σ} \in R^{2 \times 2}$ and $(v_{1,y}, v_{1,x})$ the components of eigenvector $v_1$. Matrix A defines the scaling of the ellipse based on the desired confidence level $η$. Here, $\sqrt{Σ}$ represents the symmetric matrix square root. Given that for a centered, unit Gaussian random vector $s$, the quadratic form $z = s^T s = (p^{\text{obs}} - \bar{p}^{\text{obs}})^T (Σ^{\text{obs}})^{-1} (p^{\text{obs}} - \bar{p}^{\text{obs}})$ follows a $χ^2$ distribution. The probability that $s$ falls within the confidence ellipse is given by:

$$ \eta = P(\|s\| \le a) = \begin{aligned}[t] & \mathrm{prob}(z \le a^2) \\ & \int_0^{a^2} \frac{1}{2} \exp\left(-\frac{z}{2}\right) dz = 1 - e^{-\frac{a^2}{2}} \end{aligned} \quad (19) $$

Giving the confidence scaling factor $a_η = \sqrt{-2\ln(1-η)}$

## ACKNOWLEDGMENT

This work was carried out in the framework of the NExT Senior Talent Chair DeepCoSLAM, which were funded by the French Government, through the program Investments for the Future managed by the National Agency for Research

(ANR-16-IDEX-0007), and with the support of Région Pays de la Loire and Nantes Métropole.

## REFERENCES

[1] A. Bemporad and M. Morari, "Control of systems integrating logic, dynamics, and constraints," *Automatica*, vol. 35, 1999.

[2] P. Morin and C. Samson, "Motion control of wheeled mobile robots," *Springer Handbook of Robotics*, 2008.

[3] R. Verschueren, G. Frison, D. Kouzoupis, J. Frey, N. van Duijkeren, A. Zanelli, B. Novoselnik, T. Albin, R. Quirynen, and M. Diehl, "acados – a modular open-source framework for fast embedded optimal control," *Mathematical Programming Computation*, 2021.

[4] T. Schoels, P. Rutquist, L. Palmieri, A. Zanelli, K. O. Arras, and M. Diehl, "Ciao*: Mpc-based safe motion planning in predictable dynamic environments," *IFAC-PapersOnLine*, vol. 53, 2020.

[5] B. Li, Y. Zhang, T. Zhang, T. Acarman, Y. Ouyang, L. Li, H. Dong, and D. Cao, "Embodied footprints: A safety-guaranteed collision-avoidance model for numerical optimization-based trajectory planning," *IEEE Transactions on Intelligent Transportation Systems*, vol. 25, 2024.

[6] D. Kloeser, T. Schoels, T. Sartor, A. Zanelli, G. Frison, and M. Diehl, "Nmpc for racing using a singularity-free path-parametric model with obstacle avoidance," *IFAC-PapersOnLine*, vol. 53, 2020.

[7] R. Reiter and M. Diehl, "Parameterization approach of the frenet transformation for model predictive control of autonomous vehicles," in *2021 European Control Conference (ECC)*, IEEE, 2021.

[8] N. Kawaguchi, K. Hashimoto, I. Okawa, and K. Nonaka, "Stochastic model predictive control for relative-velocity based multimodal risk avoidance under a vehicle coming scenarios," *IFAC-PapersOnLine*, vol. 56, 1 2023.

[9] M. Brown and J. C. Gerdes, "Robust stabilization and collision avoidance through minimizing open-loop velocity uncertainty," *IEEE Intelligent Vehicles Symposium, Proceedings*, 2020.

[10] F. Gao, Y. Han, S. E. Li, S. Xu, and D. Dang, "Accurate pseudospectral optimization of nonlinear model predictive control for high-performance motion planning," *IEEE Transactions on Intelligent Vehicles*, vol. 8, 2023.

[11] R. Reiter, A. Nurkanović, J. Frey, and M. Diehl, "Frenet-cartesian model representations for automotive obstacle avoidance within nonlinear mpc," *European Journal of Control*, vol. 74, 2023. 2023 European Control Conference Special Issue.

[12] B. Li and Z. Shao, "A unified motion planning method for parking an autonomous vehicle in the presence of irregularly placed obstacles," *Knowledge-Based Systems*, vol. 86, 2015.

[13] A. Lambert, D. Gruyer, and G. Saint Pierre, "A fast monte carlo algorithm for collision probability estimation," in *2008 10th International Conference on Control, Automation, Robotics and Vision*, 2008.

[14] K. A. Mustafa, O. D. Groot, X. Wang, J. Kober, and J. Alonso-Mora, "Probabilistic risk assessment for chance-constrained collision avoidance in uncertain dynamic environments," *Proceedings - IEEE International Conference on Robotics and Automation*, vol. 2023-May, 2 2023.

[15] O. de Groot, B. Brito, L. Ferranti, D. Gavrilà, and J. Alonso-Mora, "Scenario-based trajectory optimization in uncertain dynamic environments," *IEEE Robotics and Automation Letters*, 2021.

[16] L. Hewing, A. Liniger, and M. N. Zeilinger, "Cautious nmpc with gaussian process dynamics for autonomous miniature race cars," *2018 European Control Conference (ECC)*, June 2018.

[17] D. D. Fan, K. Otsu, Y. Kubo, A. Dixit, J. Burdick, and A.-A. Agha-Mohammadi, "Step: Stochastic traversability evaluation and planning for risk-aware off-road navigation," 2021.

[18] S. Lefèvre, D. Vasquez, and C. Laugier, "A survey on motion prediction and risk assessment for intelligent vehicles," *ROBOMECH Journal*, vol. 1, 12 2014.

[19] J. R. Ward, G. Agamennoni, S. Worrall, A. Bender, and E. Nebot, "Extending time to collision for probabilistic reasoning in general traffic scenarios," *Transportation Research Part C*, 2015.

[20] H. D. Nguyen, M. Choi, and K. Han, "Risk-informed decision-making and control strategies for autonomous vehicles in emergency situations," *Accident Analysis Prevention*, vol. 193, 2023.

[21] L. Jaulin, "Mobile robotics: Kalman filter." Online Course, 2021. ENSTA Bretagne, Available: https://www.ensta-bretagne.fr/kalmooc/.