# Time-To-Collision Constrained NMPC for Connected and Autonomous Vehicles

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

► To cite this version:

Charlotte Beaune, Elwan Héry, Vincent Frémont. Time-To-Collision Constrained NMPC for Connected and Autonomous Vehicles. IEEE International Conference on Intelligent Transportation Systems (ITSC 2025), IEEE, Nov 2025, Gold Coast, Australia. hal-05401349

HAL Id: hal-05401349

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

Submitted on 5 Dec 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
# Time-To-Collision Constrained NMPC for Connected and Autonomous Vehicles

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

¹Laboratoire des Sciences du Numérique de Nantes (LS2N, UMR 6004), École Centrale de Nantes, France

first-name.lastname@ls2n.fr

**Abstract**—This paper presents a Time-to-Collision (TTC) constrained Nonlinear Model Predictive Control (NMPC) framework for Connected and Autonomous Vehicles (CAVs) navigating in dynamic urban environments. The proposed method integrates V2X (Vehicle-to-Everything) communication and probabilistic obstacle modeling to enhance safety and reactivity in the presence of uncertain dynamic obstacles. Trajectories communicated by CAVs are directly incorporated into the NMPC optimization loop, while non-connected agents are modeled with uncertainty-aware predictions. The controller incorporates both geometric and time-based safety constraints, notably through TTC metrics and confidence ellipses. We validate our approach through a series of Hardware-in-the-Loop (HIL) real-world experiments using ROSBOT 2R platforms within a centralized ROS2 architecture.

***Index Terms***—Nonlinear Model Predictive Control, V2X, Time-to-Collision, Dynamic Obstacle Avoidance, Connected and Autonomous Vehicles

## I. INTRODUCTION

Connected and Autonomous Vehicles (CAVs) represent a significant advancement in transportation technology, where connectivity with surrounding infrastructure and other vehicles enhance situational awareness, safety, and operational efficiency. Ensuring safe and efficient obstacle avoidance while following a reference path in urban environments demands a seamless integration of perception, communication, prediction, and control.

<figure><img src="image_2.png" /><figcaption>Fig. 1: ROSBOT 2R is equipped with reflective markers used for the robot localization with Qualisys motion capture system. The robot must follow the global path (in dark blue) while avoiding virtual obstacles (in orange). Bounding circles of the obstacles and the ego-vehicle are represented in orange and blue respectively.</figcaption></figure>

## A. Related Works

Non Linear Model Predictive Control (NMPC) has been extensively employed for autonomous navigation tasks thanks to its ability to handle nonlinear dynamics, constraints, and multi-objective optimization in real-time [1]–[3]. For obstacle avoidance, the curvilinear (Frénet) frame is often adopted to simplify reference path tracking [4]–[7], although it complicates obstacle representation. Recent works [8] propose formulations where obstacle avoidance constraints are instead defined directly in the Cartesian frame, simplifying their parametrization and interpretation. Various strategies exist for obstacle modeling: simple geometric over-approximations for fast computation, or more detailed representations (e.g., triangle area criteria [9], convex hulls [10], hyperplane separating theorem or big-M method [11]) for better precision at the expense of computational complexity. When considering dynamic obstacles, trajectory prediction must be incorporated into NMPC constraints to ensure collision-free planning at each prediction step [12].

The use of connectivity in CAV navigation significantly enhances perception and trajectory planning capabilities. V2X communication allows vehicles to share intentions, predicted trajectories, and environmental information, providing a richer data set [13], [14] than relying solely on onboard sensors. Beyond individual vehicle navigation, CAVs offer the opportunity to deploy collaborative NMPC approaches [15]. Collaborative NMPC leverages V2X-shared information — such as intended trajectories — to anticipate interactions with other connected vehicles, improving safety margins and traffic efficiency without relying solely on conservative worst-case predictions. However, achieving scalable and reliable collaboration requires robust architectures that can handle asynchronous communications and partial information.

Chance-constrained formulations are widely used in motion planning to ensure probabilistic safety by bounding the collision risk along the planned trajectory [16]. Gaussian Processes (GPs) have been employed to predict dynamic obstacle motions under uncertainty [17], but real-time applications often require linearization and pruning to reduce computational load. In dynamic and uncertain environments, NMPC formulations integrate probabilistic constraints to formally guarantee a bound on the probability of constraint violations. Techniques such as chance-constrained optimization [16], uncertainty tubes, and Gaussian Processes [17] have been proposed to account for uncertainty in obstacle motion
predictions. However, balancing computational simplicity with accurate risk modeling remains challenging.

Time-based safety metrics, such as Time-To-Collision (TTC) [18]–[20], provide intuitive and practical tools for real-time risk assessment. TTC constraints can be integrated within NMPC frameworks either as hard constraints or cost function terms to enforce minimum safe reaction times to obstacles.

## B. Contributions

Our contributions are as follows :

1) We propose a NMPC formulation for uncertain dynamic obstacles avoidance problem in a V2X context, where connected vehicles can share their planned trajectories with infrastructure or other vehicles.

2) We conduct real-world experiments with a ROSbot 2R and simulated mobile robots obstacles (see Fig. 1) to validate our method.

3) Furthermore, detailed code implementations and experimental results can be accessed at https://github.com/CBeaune/nmpc-ttc-robot.git.

The rest of this work is organized as follows: In Section II, we present the problem statement and detail the proposed NMPC formulation, including vehicle modeling, dynamic obstacle prediction under uncertainty, and the centralized collaborative framework. Section III describes the experimental setup, covering the test environment, NMPC implementation details, and test scenarios as well as the experimental results. Finally, Section IV concludes the paper and outlines potential directions for future work.

## II. PROBLEM STATEMENT AND PROPOSED APPROACH

In this section, we define the ego-vehicle’s motion using a differential drive model in curvilinear coordinates for path-following and Cartesian for obstacle avoidance. Note that this model is used to better fit the kinematics of the experimental robots. We also formulate the NMPC problem in V2X context and detail our approach for predicting dynamic obstacles and their uncertainties for safe and efficient planning.

### A. Assumptions

We make the following assumptions for the path following and obstacles avoidance problem

H1) Detection and tracking of the ego vehicle and obstacles are handled by a holistic intelligent infrastructure,

H2) CAVs are controlled with a feedback velocity controller, that takes as input a desired 2D trajectory that the CAVs follow closely,

H3) Predictions of the non connected obstacles is sent via V2X communication or is computed onboard by the ego-vehicle when receiving obstacles’ detection,

H4) We do not consider communication cuts, delays or noise that could be added by the V2X communication process.

<figure><img src="image_3.png" /><figcaption>Fig. 2: Frenet states for a differential drive evolution model, the parameter $n$ is the closest distance from center of wheels pose $\mathbf{p}$ to the reference path pose $\mathbf{p}_{ref}(s)$. The vehicle must follow curve path $C(s)$ of curvature $\kappa(s)$.</figcaption></figure>

### B. Vehicle model in Frénet coordinates

We used the differential drive motion model [2] as the evolution function of the NMPC problem (see Fig 2). The evolution model for the Frenet states $\mathbf{x} = [s, n, \theta_e]^T$ is :

$$ \dot{\mathbf{x}} = f(\mathbf{x}, \mathbf{u}) = \begin{bmatrix} \dot{s} \\ \dot{n} \\ \dot{\theta}_e \end{bmatrix} = \begin{bmatrix} \frac{v \cos \theta_e}{1 - n\kappa(s)} \\ \frac{v \sin(\theta_e)}{\omega - \kappa(s) \cdot \dot{s}} \end{bmatrix} \quad (1) $$

where $\omega$ is the rotation speed of the ego-vehicle.

In order to get smoother velocity controls $(v, \omega)$, we extend the states to $\mathbf{x} = [s, n, \theta_e, v, \omega]^T$ with

$$ \dot{\mathbf{x}} = f(\mathbf{x}, \mathbf{u}) = \begin{bmatrix} \dot{s} \\ \dot{n} \\ \dot{\theta}_e \\ v \\ \omega \end{bmatrix} = \begin{bmatrix} \frac{v \cos \theta_e}{1 - n\kappa(s)} \\ \frac{v \sin(\theta_e)}{\omega - \kappa(s) \cdot \dot{s}} \\ a_{lon} \\ a_{\omega} \end{bmatrix} \quad (2) $$

where $a_{lon}$ is the longitudinal acceleration of the ego-vehicle.

Hence, the control inputs for the NMPC problem are $\mathbf{u} = [a_{lon}, a_{\omega}]^T$, while the controls given to the mobile robot would be $(v, \omega)$.

### C. Dynamic and uncertain obstacles model

As shown in [8], obstacle avoidance constraints can be directly formulated in the Cartesian frame. This approach allows for a straightforward modeling of such constraints.

Given the estimated obstacle pose $\bar{\mathbf{p}}^i$ and its associated covariance matrix $\Sigma^i$, we compute a confidence ellipse in $\mathbb{R}^2$ corresponding to a confidence level $\eta \in [0, 1]$. This ellipse will contain consistent values for $\bar{\mathbf{p}}^i$, with semi-axes $(\alpha_{\eta}, \beta_{\eta})$ and orientation angle $\phi_{\eta}$ (see Fig. 3.) Moreover, assuming that assume that each obstacle has a rectangular geometry
<figure><img src="image_4.png" /><figcaption>Fig. 3: 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 of the obstacle.</figcaption></figure>

characterized by a width $W$ and a length $L$, we associate $n_{\text{circles}}^i$ bounding circles of radius $r$. These circles approximate the obstacle shape while incorporating safety margins relevant for maneuvers such as overtaking. To each bounding circle we attach an extended ellipse increased by radius $r$. Similarly, ego-vehicle geometry is encapsulated by $n_{\text{circles}}^{\text{ego}}$ bounding ellipses. The free collision space with obstacle i is defined as:

$$ \mathcal{X}_{\text{FREE},i} = \left\{ \begin{array}{l} \mathbf{x} \mid (\mathbf{p}_j - \mathbf{p}_k^i)^T \mathcal{E}_i (\mathbf{p}_j - \mathbf{p}_k^i) > 1 \\ \quad \forall k \in \{1, n_{\text{circles}}^i\} \\ \quad \forall j \in \{1, n_{\text{circles}}^{\text{ego}}\} \end{array} \right\} \quad (3) $$

with $\mathcal{E}_i$ being the matrix representation of the extended confidence ellipse.

### D. Time-to-Collision constraints

We define $n_i$ free collision spaces attached to each obstacles i. We use the definition of the safe Time To Collision space:

$$ \mathcal{X}_{\text{TTC},i} = \left\{ \begin{array}{l} \mathbf{x} \mid \text{TTC}(\mathbf{p}_j, \mathbf{p}_k^i) > \text{TTC}_{\text{min}} \\ \quad \forall k \in \{1, n_{\text{circles}}^i\} \\ \quad \forall j \in \{1, n_{\text{circles}}^{\text{ego}}\} \end{array} \right\} \quad (4) $$

We consider the bounding ellipses defined Fig. 3, the distance $d_{ij}(t)$ between circle center $\mathbf{p}_j$ and the extended ellipse centered at $\mathbf{p}_k^i$ at time $t$. 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 (5) $$

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

$$ \text{TTC}(\mathbf{p}_j, \mathbf{p}_k^i) = \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 (6) $$

### E. Urban driving cost function formulation

In a urban setup, we want the controlled vehicle to avoid the obstacles while complying to the driving rules of the environment. For example we want to avoid the obstacles by the left side, we define the following term in our NMPC cost function:

$$ f_{n_+}(\mathbf{x}) = \begin{cases} 0.0 & \text{if } n \ge 0.0 \\ 1.0 & \text{else} \end{cases} \quad (7) $$

This way, tuning the weight of this cost function term, allows to nudge the avoidance maneuver to the left. The total cost function can then be defined as a non-linear least square function:

$$ J(\mathbf{x}, \mathbf{u}) = \|\mathbf{x} - \mathbf{x}_{\text{ref}}\|_{Q_x} + \|\mathbf{u} - \mathbf{u}_{\text{ref}}\|_{Q_u} + \|f_{n_+}\|_{q_{n_+}} \quad (8) $$

with $Q_x = \text{diag}(q_x)$ and $Q_u = \text{diag}(q_u)$. We define $\mathbf{u}_{\text{ref}} = 0.0$, to ensure that the velocity inputs remain as continuous as possible.

### F. NMPC Problem formulation

Given the previous definitions and details, the NMPC Problem can be formulated as:

$$ \min_{\mathbf{x},\mathbf{u}} J(\mathbf{x}, \mathbf{u}) \quad (7a) $$

$$ \text{s.t.} \quad \dot{\mathbf{x}} = f(\mathbf{x}, \mathbf{u}) \quad (7b) $$

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

$$ \mathbf{x}_{\text{min}} \le \mathbf{x} \le \mathbf{x}_{\text{max}} \quad (7d) $$

$$ \mathbf{u}_{\text{min}} \le \mathbf{u} \le \mathbf{u}_{\text{max}} \quad (7e) $$

$$ \mathbf{x} \in \bigcap_{i=1}^{n_i} \mathcal{X}_{\text{FREE},i} \quad (7f) $$

$$ \mathbf{x} \in \bigcap_{i=1}^{n_i} \mathcal{X}_{\text{TTC},i} \quad (7g) $$

### G. Framework for centralized communication and control

In this work, we consider a mixed environment comprising both connected and non-connected vehicles. Each connected vehicle is assumed to be equipped with a well-tuned trajectory tracking controller, such that its actual motion closely follows the desired trajectory with asymptotic convergence. In our framework, for each connected vehicle, the desired trajectory over the prediction horizon $T_f$ where $T_f$ denotes the common prediction horizon time used by the NMPC—is transmitted to the centralized NMPC solver. Although these trajectories are received asynchronously, they are assumed to start at the current NMPC solver time.

For non-connected vehicles, future motion is estimated using a differential drive kinematic model with constant or estimated velocities, propagated over the same prediction horizon $T_f$. This ensures temporal consistency across all predicted trajectories considered by the NMPC solver.

For all vehicles, the covariance of the predicted state is propagated over time using a linearized model of the form
$\Sigma_{t+dt} = A\Sigma_t A^T + \Sigma_\alpha$, with $\Sigma_0 = \text{diag}(\sigma_x^2, \sigma_y^2, \sigma_\theta^2)$, $A$ the Jacobian of the differential drive motion model with respect to the cartesian states and with $\Sigma_\alpha = \text{diag}(\sigma_\alpha^2, \sigma_\alpha^2, \sigma_\theta^2)$ a process noise matrix.

### III. RESULTS

The proposed method without communication is previously evaluated in simulations and a study on the selected confidence level $\eta$ is conducted. In this paper, we also extend the validation of the proposed method to a V2X context. We conducted both simulation-based evaluations and real-world experiments using a mobile robotics platform.

#### A. Study on noise levels and computational load

As the confidence level $\eta$ increases, our method generates more conservative trajectories, resulting in a more cautious ego-vehicle behavior that better satisfies safety constraints. In contrast, lower values of $\eta$ yield less conservative plans. This behavior underscores the relevance of adaptive planning frameworks that explicitly account for uncertainties arising from detection and tracking modules. Table I presents the influence of noise levels on performance and safety metrics across 100 crossroad scenarios. The results indicate that higher $\eta$ values lead to increased Time-to-Collision, reflecting enhanced safety margins, but at the cost of reduced task completion rates due to more cautious maneuvers.

Moreover we conducted experiment to understand the relation between the number of considered static obstacles in the NMPC framework and the computational time. This time includes pre-processing operations as well as the NMPC solver steps. Overall, if with an increasing number of simultaneously considered obstacles, the computational time stays under 1ms per control step. For each number of obstacles we conduct a loop on the desired path while avoiding the obstacles. Quantitative results are shown Fig. 4. A comparison of the proposed approach with the state-of-the-art methods has been conducted in simulation in our previous work [21].

#### B. Experimental setup

1) Test environment: The experiments were carried out in a controlled indoor environment equipped with a Qualisys Motion Capture system for high-accuracy state estimation. The motion capture system provides ground truth positions and orientations for all vehicles. A centralized computer collects states from all agents and handle trajectory predictions, NMPC optimization, and V2X communication. We conducted Hardware-in-the-Loop (HIL) experiments where the ego-vehicle operated physically within the environment while interacting with simulated dynamic obstacles, allowing validation of the control pipeline. The ego-vehicle and simulated mobile robots are ROSBOT 2R differential drive robots, each equipped with onboard computing resources.

Connected vehicles periodically broadcast their intended future trajectories as ROS2 messages, with a prediction horizon of $T_f$ seconds, corresponding to the NMPC prediction horizon.

For non-connected obstacles, the centralized computer predicts motion using a differential drive model.

<figure><img src="image_5.png" /><figcaption>Fig. 4: Influence of the number of simultaneously considered static obstacles in the NMPC framework.</figcaption></figure>

2) NMPC implementation: The NMPC controller was implemented using ACADOS libraries [22], with a solver frequency of 10 Hz using the Sequential Quadratic Programming Real-Time Iteration (SQP-RTI) scheme. The NMPC problem is solved on the centralized computer during real-world experiments and control inputs were sent to the ego-vehicle as ROS2 messages. Details on the NMPC cost functions weights and constraints can be found in Tab. II.

3) Test scenarios: Across all scenarios, the ego vehicle is required to follow a predefined reference path (see Fig.1). The proposed approach was evaluated in the following situations:

(1) Path following in the presence of static obstacles,

(2) Path following and obstacle avoidance in a two-lane road scenario with dynamic obstacles.

Details regarding the experimental setup, parameter configurations, and code implementation are available at https://github.com/CBeaune/nmpc-ttc-rorobot.git.

#### C. Experiment results and observations

1) Results: The proposed method was tested under multiple configurations and scenarios, demonstrating its successful implementation in an indoor environment with a motion system capable of detecting both obstacles and the ego-vehicle. Fig. 5 and 6 illustrate qualitative performance of the proposed collaborative NMPC approach in urban-like scenarios involving CAVs and dynamic obstacles. These qualitative results highlight key behaviors such as safe overtaking and proactive collision avoidance, demonstrating the controller's effectiveness in indoor environment settings. While a comprehensive quantitative evaluation using HIL testing was not conducted due to constraints in available resources and time, the qualitative results offer valuable insights into the method's capabilities and support its practical feasibility. Broadcasting the desired
<table><caption>TABLE I: Results of the quantitative tests for simulated scenario of unsignalized intersection: For different confidence levels η and at fixed noise level, 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>0.68</td><td>NMPC-TTC</td><td>0.261/0.005</td><td><strong>3.326</strong></td><td><strong>1%</strong></td><td>0.073/0.717</td><td>0.163</td><td><strong>99%</strong></td><td>1%</td></tr><tr><td>0.99</td><td>NMPC-TTC</td><td>0.262/0.005</td><td><strong>3.363</strong></td><td><strong>1%</strong></td><td>0.066/0.483</td><td>0.163</td><td>79%</td><td>0.8%</td></tr></tbody></table>

<figure><img src="image_6.png" /><figcaption>Fig. 5: Results of the proposed approach in a road scenario with static obstacles with TTC<sub>min</sub> = 1.0s, T<sub>f</sub> = 5.0s. The ego-vehicle (blue) is required to follow the reference path (green) while avoiding static obstacles (orange). Covariance ellipses are not shown for clarity. The figure shows three different snapshots of the HIL experiment at t= {2.2, 5.0, 9.0} s. With the proposed approach, the vehicle prefers the left side to overtake the obstacles and is able to follow the road driving constraints.</figcaption></figure>

<table><caption>TABLE II: NMPC parameters for the experimental setup</caption><thead><tr><th>Name (Variable)</th><th>Value</th></tr></thead><tbody><tr><td>Nber. of obst./ego-vehicle ext. ellipses (n<sup>obs</sup><sub>circles</sub>/n<sup>obs</sup><sub>circles</sub>)</td><td>4 / 3</td></tr><tr><td>Vehicle length/width (L/W)</td><td>0.200 / 0.233 [m]</td></tr><tr><td>Min lateral offset (n<sub>min</sub>)</td><td>-0.2 [m]</td></tr><tr><td>Max lateral offset (n<sub>max</sub>)</td><td>1.0 [m]</td></tr><tr><td>Prediction horizon (T<sub>f</sub>)</td><td>5.0 [s]</td></tr><tr><td>Discretization timestep (dt)</td><td>0.1 [s]</td></tr><tr><td>Reference velocity (v<sub>ref</sub>)</td><td>0.2 [m.s<sup>-1</sup>]</td></tr><tr><td>Position uncertainty (σ<sup>2</sup><sub>x</sub>)</td><td>0.001<sup>2</sup> [m<sup>2</sup>]</td></tr><tr><td>Position uncertainty (σ<sup>2</sup><sub>y</sub>)</td><td>0.001<sup>2</sup> [m<sup>2</sup>]</td></tr><tr><td>Orientation uncertainty (σ<sup>2</sup><sub>θ</sub>)</td><td>0.001<sup>2</sup>[rad<sup>2</sup>]</td></tr><tr><td colspan="2"><strong>Scenario (1)</strong></td></tr><tr><td>state weights (q<sub>x</sub>)</td><td>[10<sup>-1</sup>, 10<sup>-1</sup>, 10<sup>-3</sup>, 10<sup>-1</sup>, 10<sup>-5</sup>]</td></tr><tr><td>terminal state weights (q<sub>x<sub>e</sub></sub>)</td><td>[10<sup>-1</sup>, 10<sup>-2</sup>, 10<sup>-3</sup>, 10<sup>-1</sup>, 10<sup>-5</sup>]</td></tr><tr><td>control weights (q<sub>u</sub>)</td><td>[10<sup>-3</sup>, 10<sup>-3</sup>]</td></tr><tr><td>driving rule weight (q<sub>n<sub>+</sub></sub>)</td><td>1.0</td></tr><tr><td colspan="2"><strong>Scenario (2)</strong></td></tr><tr><td>state weights (q<sub>x</sub>)</td><td>[10<sup>-1</sup>, 10<sup>-2</sup>, 10<sup>-3</sup>, 10<sup>-1</sup>, 10<sup>-5</sup>]</td></tr><tr><td>terminal state weights (q<sub>x<sub>e</sub></sub>)</td><td>[10<sup>-1</sup>, 10<sup>-1</sup>, 10<sup>-3</sup>, 10<sup>-1</sup>, 10<sup>-5</sup>]</td></tr><tr><td>control weights (q<sub>u</sub>)</td><td>[10<sup>-3</sup>, 10<sup>-3</sup>]</td></tr><tr><td>driving rule weight (q<sub>n<sub>+</sub></sub>)</td><td>1.0</td></tr></tbody></table>

trajectories of CAVs consistently enhanced the qualitative performance of the collaborative NMPC controller—yielding smoother interactions and fewer trajectory conflicts—provided that each CAV already tracked its own reference with a small, asymptotic error.

The complementary TTC constraints allowed the system to retain a short, computationally affordable T<sub>f</sub> while still enforcing longer-range safety margins.

Moreover, a longer prediction horizon T<sub>f</sub> enables improved motion planning by providing greater foresight into the future trajectories of other agents through communication. Fig 7a presents snapshots of experiments conducted with a shorter prediction horizon. As observed, the resulting trajectory is noticeably bumpier compared to that in Fig 6a. This difference is due to the reduced anticipation of the future motion of dynamic agents when using a shorter horizon which results in less smooth ego-vehicle trajectories due to replanning triggered by received data. The following subsections discuss the limitations of the current approach and outline directions for future work, based on these experimental results.

2) Limitations: The current implementation treats trajectory messages as asynchronous: the NMPC solver simply uses the most recently received packet and lacks explicit fault detection for delayed or lost communications.
<figure><img src="image_7.png" /><figcaption>(a) Results of the proposed approach with dynamic communicated vehicles. The ego-vehicle is able to adapt its trajectory according to the desired trajectories of the connected vehicles</figcaption></figure>

<figure><img src="image_8.png" /><figcaption>(b) Results of the proposed approach with dynamic vehicles but without communication. The predicted trajectory of the obstacle is provided as input to the NMPC solver. The experiment results in a unfeasible NMPC problem at $t = 30.0s$. For clarity, the predicted poses of the obstacle in the non-communicated case are not shown; they correspond to either circular or planar motion, depending on the obstacle's steering angle at the initial prediction time $t$, under the assumption of constant velocity and steering.</figcaption></figure>

<caption>Fig. 6: Results of the proposed approach in a road scenario with dynamic obstacles and $TTC_{min} = 1.5s$, $T_f = 5.0s$. The ego-vehicle (blue) is required to follow the reference path (green) while avoiding static and dynamic obstacles (orange). Obstacle position uncertainties are set to ($\sigma_x$ = 0.001 m, $\sigma_y$ = 0.001 m, $\sigma_\theta$ = 0.001 m) and process noise is set to $\sigma_\alpha$ = 0.003 m. Output poses of the controller of the ego-vehicle are shown in blue and obstacle dynamic poses are shown in orange (with decreasing opacity indicating increasing prediction time steps). The planned positions of both the ego-vehicle and the dynamic obstacles are visualized using a color gradient from purple (near future) to orange (far future). Covariance ellipses are not shown for clarity.</caption>

In the absence of communication, having a longer prediction horizon $T_f$ increases model-mismatch errors between predicted and actual obstacle trajectories, particularly when relying on simplified motion models—such as differential-drive kinematics with constant velocity assumptions—for non-communicating agents. An example of this behaviour can be seen in Fig 7b, where the controller fails to adapt to the updated predicted trajectory of the dynamic agent—shifting from a circular motion to a straight line—and ultimately freezes due to violated collision-avoidance constraints and/or giving not enough continuous control inputs. Enlarging $T_f$ also lengthens optimisation time, pushing the computation closer to real-time limits. A systematic study of the relationship between

computation time and prediction horizon could inform the selection of an optimal $T_f$ value tailored to the specific hardware and scenario constraints. While TTC mitigates this trade-off, it does so at the cost of more conservative motion plans.

Obstacle shapes with probabilistic envelope help capture long-term positional uncertainty and communication errors, but can still be overly conservative.

Moreover, in our test case, ego-vehicle localization and obstacle detection/tracking are performed using a high-precision motion capture system, which does not fully reflect real-world conditions. Although the motion capture system is subject to some noise and occasional failures, these are sig-
<figure><img src="image_9.png" /><figcaption>(a) Results of the proposed approach with dynamic communicated vehicles. The ego-vehicle is able to adapt its trajectory according to the desired trajectories of the connected vehicles</figcaption></figure>

<figure><img src="image_10.png" /><figcaption>(b) Results of the proposed approach with dynamic vehicles but without communication. The predicted trajectory of the obstacle is provided as input to the NMPC solver. The experiment results in a unfeasible NMPC problem.</figcaption></figure>

<caption>Fig. 7: Results of the proposed approach in a road scenario with dynamic obstacles and TTC<sub>min</sub> = 1.5s, T<sub>f</sub> = 4.0s. Obstacle position uncertainties are set to (σ<sub>x</sub> = 0.001 m, σ<sub>y</sub> = 0.001 m, σ<sub>θ</sub> = 0.001 m) and process noise is set to σ<sub>α</sub> = 0.003 m. Readers are referred to Figure 6 for additional visual details and contextual information.</caption>

nificantly smaller compared to those encountered with off-the-
shelf sensor-based detection and tracking systems. Introducing
synthetic noise into the motion capture data can enhance the
realism of the tested scenarios.

3) *Potential Improvements:* The constant-velocity prediction model for non-communicating obstacles can be replaced with higher-fidelity or learned motion models. Additionally, when available, richer motion primitives may be transmitted to improve prediction accuracy and scene understanding.

To improve robustness against communication failures, the
system can integrate online fault-detection mechanisms. These
mechanisms would be capable of identifying faulty or missing
information, whether from the ego-vehicle's sensors or from
received trajectory data. Once a fault is detected, the system
can trigger appropriate safety fallback strategies to maintain
safe operation.

Probabilistic occupancy models such as weighted Gaussian
mixtures used to represent multiple motions possibilities and
communication uncertainty. This approach enables risk to be

bounded, but requires careful calibration to avoid introducing
excessive conservatism into the control strategy.

Finally, more realistic urban scenarios should be tested with
increasing number of dynamic and/or connected agents in
order to test the scalability of our method.

IV. CONCLUSION

In this work, we presented a TTC constrained NMPC
framework tailored to urban navigation scenarios involving
both connected and non-connected vehicles. Our approach
leverages V2X communication to integrate shared trajec-
tory information into a centralized NMPC controller. For
non-communicating agents, uncertainty-aware obstacle models
based on Gaussian confidence ellipses and Time-To-Collision
(TTC) constraints are used to preserve safety margins under
motion and communication uncertainty. Hardware-in-the-loop
(HIL) simulations and real-world experiments validate the
effectiveness of the proposed method in scenarios requiring
real-time collision avoidance and path following.
Although the qualitative results are promising, several di-
rections for future work remain. These include the integration
of online fault detection mechanisms for handling commu-
nication dropouts, improvements in probabilistic models for
obstacle position uncertainty, and the use of learned or higher-
fidelity models for more accurate motion and TTC predic-
tion. Furthermore, the development of multi-layered NMPC
architectures could enhance the optimality of motion planning.
A complete quantitative evaluation—particularly in terms of
reaction time, risk metrics and computational efficiency—is
planned as part of future work. Future works include vehicle
cooperation in urban environments, where multiple controllers
run simultaneously and share their predicted trajectories over a
V2X communication network, with a focus on scalability when
increasing the number of dynamic agents. Further research
will also explore scenarios with limited perception fields, as
opposed to the holistic detection system used in this work. In
this context, experiments with hybrid robotic platforms—such
as aerial or heterogeneous ground robots—are will be explored
in future studies, where the infrastructure and cooperative
robots may assist by sharing the positions of undetected
obstacles or the ego-vehicle’s pose in case of localization
faults.

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.

[11] B. Alrifaee, “Networked model predictive control for vehicle collision avoidance,” 2017.

[12] 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.

[13] T. Wang, S. Kim, W. Ji, E. Xie, C. Ge, J. Chen, Z. Li, and P. Luo, “Deepaccident: A motion and accident prediction benchmark for v2x autonomous driving,” 4 2023.

[14] R. Xu, H. Xiang, Z. Tu, X. Xia, M.-H. Yang, and J. Ma, “V2x-vit: Vehicle-to-everything cooperative perception with vision transformer,” 3 2022.

[15] A. L. Gratzer, M. M. Broger, A. Schirrer, and S. Jakubek, “Two-layer mpc architecture for efficient mixed-integer-informed obstacle avoidance in real-time,” *IEEE Transactions on Intelligent Transportation Systems*, vol. 25, pp. 13767–13784, 10 2024.

[16] 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.

[17] O. de Groot, B. Brito, L. Ferranti, D. Gavrilă, and J. Alonso-Morå, “Scenario-based trajectory optimization in uncertain dynamic environments,” *IEEE Robotics and Automation letters*, 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] C. Beaune, E. Hery, and V. Fremont, “Risk-aware nonlinear model predictive control for autonomous navigation: Confidence-based obstacle constraints and time-to-collision safe trajectory planning,” in *2025 IEEE Intelligent Vehicles Symposium (IV)*, 2025.

[22] 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*, vol. 14, 2022.

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] 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.

[5] 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.

[6] M. Brown and J. C. Gerdes, “Robust stabilization and collision avoid-
ance through minimizing open-loop velocity uncertainty,” *IEEE Intelli-
gent Vehicles Symposium, Proceedings*, 2020.

[7] 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.

[8] 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.

[9] 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.

[10] 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.