# Hybrid Optimization Method for Safe Autonomous Navigation under Uncertainty

Emmanuel Alao, Lounis Adouane, Philippe Martinet

► To cite this version:

Emmanuel Alao, Lounis Adouane, Philippe Martinet. Hybrid Optimization Method for Safe Autonomous Navigation under Uncertainty. 12th IFAC Symposium on Intelligent Autonomous Vehicles (IAV 2025), May 2025, Phoenix, United States. pp.121-126, 10.1016/j.ifacol.2025.07.021. hal-05232803

HAL Id: hal-05232803

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

Submitted on 1 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
# Hybrid Optimization Method for Safe Autonomous Navigation under Uncertainty

Emmanuel Alao* Lounis Adouane* Philippe Martinet**

* Université de Technologie de Compiègne (UTC), CNRS, Heudiasyc
UMR 7253, CS 60319, 60203, Compiègne, France
 email: {emmanuel.alao, lounis.adouane} @hds.utc.fr).

** Centre Inria d'Université Côte d'Azur, INRIA, ACENTAURI,
06902, Sophia Antipolis, France (email: philippe.martinet@inria.fr).

**Abstract:** A fundamental difficulty that autonomous driving systems face in dynamic environments is uncertainty in the motion of the other traffic participants. Although predictive control algorithms have proven to be able to handle such scenarios, the multimodal motion of agents often increases the risk in traffic environments, leading to unsmooth motion and sometimes the failure of such risk assessment and management algorithms. This paper proposes to tackle such problems using a hybrid optimization method that finds a global safe trajectory with respect to the surrounding agents using a Fusion of stochastic Predictive Inter-Distance Profile (F-sPIDP) before following the safe path using a combination of stochastic reference points and an uncertainty-aware local trajectory optimizer. The results show that the proposed method guarantees safety even with varying levels of uncertainties.

**Keywords:** Predictive Navigation, Uncertainty, Risk Assessment, Risk Management, Optimal trajectory, Multimodal motion

## 1. INTRODUCTION

Any autonomous driving system must include safety, or more specifically, a risk assessment and management unit. This necessitates predicting the future of the environment surrounding the vehicle. Consequently, predicting the future behavior of the other agents in the environment has emerged as a widely researched issue. This work is inspired by recent motion prediction algorithms like Wang et al. (2024), Ivanovic and Pavone (2019), and Gupta et al. (2018) that predict a distribution of multimodal trajectories instead of predicting a deterministic trajectory. Multimodal predictions are essential for integrating uncertainties arising from sensors and the unobserved intentions of surrounding agents such as Personal Light Electric Vehicles (PLEVs).

This paper therefore adopts a stochastic method to predict the motion of the traffic participants and to assess the risk of collision. Two primary issues are addressed in this scenario. The first is the uncertainty in the predicted state (position, orientation, sizes) of the agents. For example, Zhu and Alonso-Mora (2019) assumes a Gaussian distribution for the noises in the obstacle trajectory, which is not the case in Wang et al. (2024). The second problem arises from the multimodal prediction of the agents' motion, which makes it difficult to apply the methods proposed in Fu et al. (2023) and Ferranti et al. (2019) to decide the appropriate control actions to be taken to manage the risk of collision.

Both issues are addressed in this work using a hybrid optimization method for risk assessment and management. The hybrid method combines sampling-based trajectory

<figure><img src="image_2.png" /><figcaption>Fig. 1. A scenario showing the evolution of the uncertainty of multiple PLEVs with multimodal motion uncertainties, the candidate trajectories of the AV, the selected safe trajectory, and the local control trajectory selection and control optimization methods. The proposed method builds on the concept of Predictive Inter-Distance Profile (PIDP) (Bellingard et al., 2023b), (Bellingard et al., 2023a) (Iberraken and Adouane, 2022), (Iberraken et al., 2019). We handle the underlying uncertainties in the motion of neighboring agents using a combination of safe global trajectory sampling and a Fusion of Stochastic Predictive Inter-Distance Profiles (F-sPIDP) for local trajectory control (cf. Fig. 1). In particular, an optimal trajectory is selected from the set of possible maneuvers the ego vehicle can perform. Using a safe reference F-sPIDP and local predictive control, we compute the set of control actions that reduces the risk of collision.</figcaption></figure>

## 2. RELATED WORKS

### 2.1 Multi-Agent Motion Prediction

Accurate prediction of the motion of multiple agents is essential for autonomous systems to navigate in dynamic
environments. Recent research has focused on multimodal prediction methods that account for the inherent uncertainties in the diverse behaviors of traffic agents. The Interacting Multiple Model Filter (IMM-F) (Gu et al., 2024) (Tran et al., 2020) and deep generative methods (Dendorfer et al., 2020) (Ivanovic et al., 2020) are two approaches that stand out in the field. Both methods represent a shift from the conventional deterministic prediction methods to probabilistic models that produce a distribution over possible predictions. IMM-F models the predictions based on the physical rules of motion, while deep generative methods try to learn the motion behaviors from data. This work embraces both methods by concentrating on the probability distribution derived from either approach.

## 2.2 Predictive Control Strategy

Most prominent control strategies for autonomous systems employ optimization techniques to satisfy various limitations on the predicted motion of the system. Model Predictive Control (MPC) as presented in the work of Charest-Finn and Pejhan (2024), stands out in this domain for its ability to optimize motion smoothness while handling system constraints. Diverse extensions of MPC have been explored to address uncertainties stemming from sensor noises and model mismatches.

For instance, Stochastic MPC (SMPC) (Nair et al., 2024) is a popular extension of MPC that models uncertainties in the system as probabilistic constraints. In Brüdigam et al. (2021), the system control was formulated as a SMPC problem with uncertainties in the predicted motion of the surrounding agents. Despite its robustness, SMPC approaches are often very conservative when applied to multimodal agents due to the need to consider all possible trajectories of the agents.

As mentioned in the work of Diehl et al. (2022), direct optimization methods generally compute local optimal solutions due to the nonlinear nature of most dynamical systems. Sampling-based methods are able to avoid such problems. For example, Ma et al. (2022) presented a Risk-based Rapidly-exploring Random Tree (Risk-RRT) algorithm to search for safe trajectories in a tight environment. Sharma et al. (2023) employed a sampling-based optimization method to generate a set of collision-free trajectories for an AV in the presence of multimodal obstacles. In general, to reduce complexity and computation time, sampling-based methods do not strictly respect the dynamic model of the agents.

Learning-based methods have also been employed to perform predictive controls. Schuurmans et al. (2023), introduced an uncertainty-aware decision transformer to estimate state uncertainties and improve the driving decision under uncertainties. However, ensuring the interpretability of the decision-making process remains an open research challenge.

This work combines the advantages of Sampling-based methods and optimization-based methods. Using a hybrid approach, we select the best trajectory that satisfies the desired safety constraints with uncertainties from a set of candidate trajectories. Then, we compute the control

actions to track the reference trajectory while respecting the predicted uncertainty.

# 3. STOCHASTIC RISK ASSESSMENT

## 3.1 Stochastic PIDP (sPIDP) Risk Assessment Metrics

The standard PIDP provides a good measure of the level of risk over a finite prediction time, assuming that the disturbances in the system are negligible or equal to zero. However, most systems are indeed perturbed by internal and external uncertainties in the driving environment. Hence, the true state trajectory of the traffic agents will vary from the prediction. Which when ignored may lead to dangerous performance of the system. This highlights the need for a more robust approach that improves the standard PIDP.

The basic idea of stochastic PIDP is to decompose the predicted inter-distance into (a) a deterministic component equivalent to the standard PIDP which uses the current best estimate of the agent's state trajectories, and (b) a stochastic component that models the disturbances in the system.

The stochastic PIDP is parameterized as:

$$sPIDP(t) = PIDP(t) + w(t) \quad (1)$$

the estimated PIDP(t) := E[sPIDP] ∈ ℝ+ is the estimated inter-distance of the position vectors of the interacting agents, w(t) denotes the Gaussian or non-Gaussian uncertainties in the system, over the prediction time interval 0 ≤ t ≤ t<sub>hor</sub>.

## 3.2 Deterministic Component of sPIDP

From (1), the deterministic component of sPIDP is the value of the inter-distance when the uncertainty is zero. This implies that given the estimated state trajectory of the position vectors $\tilde{\mathbf{p}}(t) = (\tilde{x}(t), \tilde{y}(t))$, the deterministic part of the sPIDP is:

$$E[sPIDP] = E[\|\tilde{\mathbf{p}}_a - \tilde{\mathbf{p}}_i\|] \quad (2)$$

where the current estimated position of the AV and the i-th traffic agents are $\tilde{\mathbf{p}}_a$ and $\tilde{\mathbf{p}}_i$, respectively. This formulation, shown in equation (2), captures the expected PIDP, consistent with the traditional PIDP frameworks outlined in (Bellingard et al., 2023b) and (Alao et al., 2024b).

## 3.3 Stochastic Component of sPIDP

As highlighted previously, the uncertainty in the agents' motion, denoted as $w(t) \in ℝ$ in (1), often heavily impact the final performance of the system. However, state-of-the-art motion prediction algorithms generally do not provide the $w(t)$ value. Therefore, a mapping from the predicted state covariance uncertainty of the agents to the predicted inter-distance uncertainty, $w(t): ℝ^{n \times n} \to ℝ$, is required. Let the variance of the inter-distance uncertainty be:

$$V[sPIDP] = E[d^2] - (E[d])^2 \quad (3)$$
where $\mathbb{E}[d] := \mathbb{E}[sPIDP]$ is the expected value of sPIDP given in (2), therefore we present below the derivation of $\mathbb{E}[d^2]$.

$$ \begin{aligned} \mathbb{E}[d^2] &= \mathbb{E}[(\tilde{x}_r - \tilde{x}_p)^2 + (\tilde{y}_r - \tilde{y}_p)^2] \\ &= \mathbb{E}[\tilde{x}_r^2] + \mathbb{E}[\tilde{x}_p^2] + \mathbb{E}[\tilde{y}_r^2] + \mathbb{E}[\tilde{y}_p^2] - 2(\mathbb{E}[\tilde{x}_r \tilde{x}_p] + \mathbb{E}[\tilde{y}_r \tilde{y}_p]) \end{aligned} \quad (4) $$

where the expected values of the squared position component of the agents are defined as $\mathbb{E}[x^2] = \mathbb{V}[x] + \mathbb{E}[x]^2$. Hence, $\mathbb{E}[x^2]$ can be computed using the variance of each of the position components $\mathbb{V}[\cdot]$ obtained from the uncertainty given by the motion prediction algorithm (Gu et al., 2024), (Dendorfer et al., 2020). Fig. 2 shows the stochastic PIDP of possible trajectories of a PLEV where the deterministic and stochastic components of sPIDP between an AV and the PLEV are represented by a surface.

## 4. MULTIMODAL AGENTS RISK ASSESSMENT USING FUSION OF STOCHASTIC PIDP

Pedestrians and PLEVs, unlike vehicles, exhibit multi-modal behaviors, meaning a single agent can have multiple predictions generated by one or more motion prediction algorithms. This adds to the complexity of the navigation problem as a single agent may be associated with more than one sPIDPs. In (Alao et al., 2024a) and (Alao et al., 2024b), we presented a fusion of PIDP (F-PIDP) for a deterministic scenario. However, in this paper, we propose a Fusion of the Stochastic PIDPs (F-sPIDP) to account for uncertainties in the estimated state of the agents. F-sPIDP is a curve derived from the weighted fusion of all the predicted sPIDP (cf. Fig. 2).

Each sPIDP curve is nonlinear, and prior to computing F-sPIDP, specific parameters must be extracted from each sPIDP. These parameters capture critical characteristics, emphasizing key points along the sPIDP curve:

(1) $sPIDP_j(t_0)$: the sPIDP value at time ($t_0$) has the same value for all the sPIDPs of a given agent.

(2) $sPIDP_j(t_{min})$: the sPIDP value at time ($t_{min}$), the minimum point of each sPIDP curve.

(3) $sPIDP_j(t_{hor})$: the sPIDP value at time ($t_{hor}$) the end point of each sPIDP curve.

Consequently, all sPIDP parameters are merged into a single quadratic polynomial curve, weighted by their probability $Pr(j)$. These parameters are integrated by calculating the probabilistic center of mass for all sPIDPs.

$$ \text{F-sPIDP}(t) = \sum_{j=1}^{N_{\text{traj}}} \text{Pr}(j) * \text{sPIDP}_j(t) \quad (5) $$

where, each maneuver $j \in \mathbb{N}^{N_{\text{traj}}}$ has a probability $Pr(j)$, and $sPIDP_j(t)$ represents the sPIDP at any of the feature points $t \in \{t_0, t_{min}, t_{horizon}\}$. $N_{\text{traj}}$ indicates the total number of maneuvers considered. The F-sPIDP computes a weighted average of the feature points based on the probability of the PLEV's maneuver. As a result, the F-sPIDP curve can be parameterized as:

$$ \begin{bmatrix} \text{F-sPIDP}(t_0) \\ \vdots \\ \text{F-sPIDP}(t_{hor}) \end{bmatrix} = \begin{bmatrix} 1 & t_0 & t_0^2 \\ \vdots & \vdots & \vdots \\ 1 & t_{hor} & t_{hor}^2 \end{bmatrix} \cdot \begin{bmatrix} q_0 \\ q_1 \\ q_2 \end{bmatrix} \quad (6) $$

<figure><img src="image_3.png" /><figcaption>Fig. 2. The multiple sPIDP and the corresponding F-sPIDP between an AV and the possible trajectories of an agent</figcaption></figure>

where the variables $[q_0, q_1, q_2]$ define the F-sPIDP curve. These variables are determined by solving the given linear equation in (6) at $t \in [t_0, t_{min}, t_{hor}]$.

## 5. HYBRID TRAJECTORY OPTIMIZATION

We propose a multi-level optimization in this paper. The overall navigation problem is divided into two main tasks: Safe global trajectory selection and a local trajectory optimization algorithm. This approach allows to assess and manage risk efficiently while accounting for unforeseen disturbances in the motion of surrounding agents in the traffic environment.

### 5.1 Safe Optimal Trajectory Selection

The objective of the optimization problem is, given a starting point, to find a safe trajectory $\mathbf{x}_{\text{ref}}[t:t_{hor}]$ that guides the AV to the desired destination while avoiding collision with other traffic agents. The optimal trajectory is computed in the road-aligned Frenet frame, this allows the treatment of curved roads as straight trajectories and implicitly accounts for motion primitives like lane change and lane keeping.

$$ \min_{\mathbf{x}_{\text{ref}}[t:t_{hor}]} \sum_{t=0}^{t_{hor}} (\ddot{x}(t) + \ddot{y}(t) + (\dot{x}(t) - v_{\text{ref}})) \quad (7) $$

subjected to:

$$ g(\ddot{x}(t), \ddot{y}(t), x_k, y_k) \le \mathbf{0} \quad \forall t \in [t, t_{hor}], \quad (8) $$

$$ \mathbf{x}(t) \in \mathcal{X}_{\text{safe}} \quad \forall t \in [t, t_{hor}], \quad (9) $$

The cost function (7) ensures the smoothness of the AV's motion and aims to drive the speed of the AV towards the reference velocity $v_{ref}$. The various constraints on the dynamics of the system are represented by the nonlinear function in (8) as proposed by Werling et al. (2010), the constraints in (9) represent the safety constraints to avoid the surrounding agents.
## 5.2 Uncertainty-aware Collision Check

To avoid collision with other traffic agents, the candidate trajectories are transformed from the Frenet frame to the inertia frame. Instead of considering multiple trajectories of each agent, we compute the F-sPIDP of each agent. As detailed in Section 4, computing the F-sPIDP of each candidate trajectory effectively reduces the multiple PIDs to a single prediction (the F-sPIDP). Hence, the set of trajectories that satisfy the constraints (9) are defined as:

$$ \mathcal{X}_{safe} := \{ \mathbf{x}_{traj} : \text{F-sPIDP} \geq d_{safe} + \mathbb{V}[\text{sPIDP}] \} \quad (10) $$

where, $\mathcal{X}_{safe}$ represents the set of safe trajectories. Like the conventional PIDP, safety is ensured when F-sPIDP $\geq d_{safe}$ for all the surrounding agents. We define $d_{safe} = R + r + \delta_v$, with $R$ and $r$ being the radii of the AV and PLEV, respectively, and $\delta_v$ denoting the braking distance. The optimal trajectory denoted as $\mathbf{x}_{ref}$ is the global trajectory that minimizes the objective function (7) and satisfies the constraints (cf. Fig. 1).

## 5.3 Local Trajectory Optimization

The local trajectory optimization is a tracking problem that integrates the kinematic constraints of the vehicle. The problem is formulated as a robust optimization problem:

$$ \min_{\mathbf{U}} \sum_{t=0}^{t_{hor}} \left( \|\mathbf{x}(t) - \mathbf{x}_{ref}(t)\|_{Q} + \|\mathbf{u}(t)\|_{R} + \delta^2 \right) \quad (11) $$

subjected to:

$$ \mathbf{x}(t+1) = f(\mathbf{x}(t), \mathbf{u}(t)) \quad \forall t \in [t, t_{hor}], \quad (12) $$

$$ \|\text{F-sPIDP}_{ref} - d(t)\| \le \delta \quad \forall t \in [t, t_{hor}], \quad (13) $$

$$ \mathbb{V}[\text{sPIDP}] \ge \delta \quad \forall t \in [t, t_{hor}], \quad (14) $$

$$ \mathbf{u}(t) \in \mathcal{U} \quad \forall t \in [t, t_{hor}], \quad (15) $$

where the current inter-distance $d(t) = \|\mathbf{x}(t) - \mathbf{x}_{ref}(t)\|_2$, and $\delta$ is the inter-distance error. The aim is to optimize the motion of the AV with respect to the reference trajectory $\mathbf{x}_{ref}$ while keeping the inter-distance error below the uncertainty $\mathbb{V}[\text{sPIDP}]$. $\|\mathbf{x}\|_A := \mathbf{x}^T A \mathbf{x}$, represents a weighted least square where $A = A^T$ is a positive definite matrix. The constraint in (12) denotes the motion dynamics of the AV, (13) and (14) represents the limits on the inter-distance error, such that, $\delta$ is bounded by the level of noise $\mathbb{V}[\text{sPIDP}]$ (cf. Fig 3). Limits on the AV control input $\mathbf{u}(t)$ are represented by (15). The above optimization problem can be solved by fast convex optimization packages.

# 6. VALIDATION AND RESULTS DISCUSSION

The proposed optimization problem in Section 5.3 is implemented using CasADi optimization framework (Andersson et al., 2019). The dynamic equation of the AV is:

$$ \dot{x} = v \cos(\phi + \beta) \quad (16) $$

$$ \dot{y} = v \sin(\phi + \beta) \quad (17) $$

$$ \dot{\phi} = v/l_r \sin(\beta) \quad (18) $$

<figure><img src="image_4.png" /><figcaption>Fig. 3. The local trajectory of the AV while respecting the uncertainty constraint</figcaption></figure>

where $\beta = \arctan(\frac{l_r}{l_r+l_f} \tan \delta)$, $\dot{x}$ and $\dot{y}$ are the speed of the AV in the x, y-axis, and $\dot{\phi}$ is the rate of change of the AV's direction. $v$ and $\delta$ represent the AV's forward velocity and steering angle, respectively. $l_r$ is the length of the rear wheel and $l_f$ is the length of the front wheels to the center of the AV. The simulations are executed at a sampling time $\Delta t = 0.1$ s with a prediction horizon $t_{hor} = 2$ s. Table 1, gives other parameters of the AV and the PLEV(s) in the simulations. The geometric shape of the AV is represented by three (3) circumscribed circles located at the rear wheel, front wheel, and center of the vehicle, respectively.

**Simulation video:** https://youtu.be/LdUf5fq4fPs

<table><caption>Table 1. Agents Parameters and initial states</caption><thead><tr><th>AV Parameters</th><th>Value</th><th>Unit</th></tr></thead><tbody><tr><td>R</td><td>1</td><td>m</td></tr><tr><td>d<sub>safe</sub></td><td>3</td><td>m</td></tr><tr><td>initial position</td><td>[5, -6]</td><td>m</td></tr><tr><td>initial velocity</td><td>[8, 0]</td><td>m/s</td></tr><tr><td>u<sub>max</sub></td><td>[8, 35]</td><td>[m/s, °]</td></tr><tr><td>u<sub>min</sub></td><td>[0, -35]</td><td>[m/s, °]</td></tr></tbody></table>

<table><thead><tr><th>PLEV Parameters</th><th>Value</th><th>Unit</th></tr></thead><tbody><tr><td>r</td><td>0.5</td><td>m</td></tr><tr><td>initial position</td><td>[17, -8]</td><td>m</td></tr><tr><td>initial velocity</td><td>[2, 0]</td><td>m/s</td></tr></tbody></table>

## 6.1 Handling the Trajectories of Multimodal Agents

First, we consider a scenario where the PLEV is moving at a constant speed with a maximum uncertainty of 0.2m in its tracked position. Fig. 4 shows that at $t = 0$ s the predicted inter-distance (or sPIDP) between the current trajectory of the AV and all the possible trajectories of the PLEV are below the safety distance $d_{safe}$. This implies a possible collision if the AV does not change its current motion. A trivial method requires computing a safe solution with respect to all the trajectories, which is very conservative (since not all trajectories would be taken by the agent). Instead, we compute the F-sPIDP of sample trajectories of the AV, the optimal trajectory $\mathbf{x}_{ref}$ that satisfies the optimization problem in Section 5.1 gives the reference F-sPIDP$_{ref}$ (cf. Fig. 5).

At $t = 1$ s, the AV is unable to accurately follow the reference trajectory (black curve in Fig. 5 (a)). This is due to the necessary model mismatch between the reference trajectory (Section 5.1) and the local trajectory (Section 5.3). However, the constraint in (13) & (14) guarantees safety by bounding the inter-distance error by the estimated uncertainty. Therefore, as shown in Fig. 6, at $t = 5$ s the safety distance is respected for the estimated (past) PIDP, the ground truth PIDP and the sPIDPs, even
<figure><img src="image_5.png" /><figcaption>Fig. 4. At $t = 0s$ (a) Shows the 2D scenario with the predicted (local) trajectory of the AV and PLEV, (b) Plot of inter-distance indicates that safety is not respected for the three (3) predicted maneuvers of the PLEV if AV maintains the same trajectory</figcaption></figure>

though the uncertainty variance ($V[sPIDP]$) falls below $d_{safe}$.

<figure><img src="image_6.png" /><figcaption>Fig. 5. At $t = 1s$ (a) Shows the 2D scenario with the reference trajectory ($\mathbf{x}_{ref}$, in black) and the predicted (local) trajectory (in red) of the AV, (b) Plot of F-sPIDP$_{ref}$ indicates that safety will be respected if AV follows the reference trajectory</figcaption></figure>

## 6.2 Quantitative Analysis

We consider varying levels of uncertainties in the position of the PLEV. The translation errors in the estimated position of the PLEV considered are $T_{error} = [0.01, 0.02, 0.05, 0.1, 0.2]m$. The plot in Fig. 7 shows the

<figure><img src="image_7.png" /><figcaption>Fig. 6. At $t = 5s$ (a) AV safely avoid collision with PLEV, (b) Plot of estimated (past) PIDP, the ground truth PIDP and sPIDPs all above safety distance</figcaption></figure>

<figure><img src="image_8.png" /><figcaption>Fig. 7. Plot of minimum PIDP between AV and PLEV</figcaption></figure>

$minPIDP_{true}$, the minimum inter-distance between the AV and the ground truth position of the PLEV. The result shows that for small values of uncertainty $T_{error} = [0.01, 0.02, 0.05]m$ the safety distance is the same since the uncertainty has little effect on the motion of the AV, whereas $minPIDP_{true}$ increases linearly for high levels of uncertainty $T_{error} = [0.1, 0.2]m$. This is because we consider that a linear relationship existing between $d_{safe}$ and $V[sPIDP]$ is sufficient, however, a more complex relationship between $d_{safe}$ and the uncertainty can be easily implemented.

## 7. CONCLUSION

This paper proposes a hybrid optimization framework for safe navigation in the presence of multimodal agents. Specifically, we presented stochastic PIDP (sPIDP) to estimate the deterministic and stochastic components of inter-distances between two agents. A fusion of sPIDP (F-sPIDP) is then proposed to fuse the multiple sPIDP of an agent into a single curve to reduce the ambiguity introduced by multimodal motion prediction algorithms. Using a hybrid optimization framework, we first select an optimal reference trajectory using a sampling-based method, after which a local optimal trajectory that respects the safety constraints and control limits of the AV is computed. Results show that the proposed approach can guarantee
safety given a reasonable estimation of uncertainty. Future
works will focus on more statistical comparison with the
state of the art and experimentation with a real vehicle.

ACKNOWLEDGEMENTS

This research is part of the ANNPOLIS project (https://project.inria.fr/annapolis/), funded by the French National Research Agency (ANR-21-CE22-0014). This work also received the support of the French government through the CPER RITMEA, Hauts-de-France region.

REFERENCES

Alao, E., Adouane, L., and Martinet, P. (2024a). Multi-risk assessment and management in the presence of personal light electric vehicles. In *21st International Conference on Informatics in Control, Automation and Robotics*, volume 1, 137–145. SCITEPRESS-Science and Technology Publications.

Alao, E., Adouane, L., and Martinet, P. (2024b). Reliable risk assessment and management using probabilistic fusion of predictive inter-distance profile for urban autonomous driving. In *European Control Conference (ECC)*.

Andersson, J.A., Gillis, J., Horn, G., Rawlings, J.B., and Diehl, M. (2019). Casadi: a software framework for nonlinear optimization and optimal control. *Mathematical Programming Computation*, 11, 1–36.

Bellingard, K., Adouane, L., and Peyrin, F. (2023a). Adaptive and reliable multi-risk assessment and management control strategy for autonomous navigation in dense roundabouts. In *IFAC World Congress 2023, Yokohama, Japan*.

Bellingard, K., Adouane, L., and Peyrin, F. (2023b). Safe and adaptive roundabout insertion for autonomous vehicle based limit-cycle and predicted inter-distance profiles. In *European Control Conference (ECC)*, 1–7. doi:10.23919/ECC57647.2023.10178129.

Brüdigam, T., Olbrich, M., Wollherr, D., and Leibold, M. (2021). Stochastic model predictive control with a safety guarantee for automated driving. *IEEE Transactions on Intelligent Vehicles*, 8(1), 22–36.

Charest-Finn, M. and Pejhan, S. (2024). Model predictive control used in passenger vehicles: An overview. *Machines*, 12(11), 773.

Dendorfer, P., Osep, A., and Leal-Taixé, L. (2020). Goal-gan: Multimodal trajectory prediction based on goal position estimation. In *Proceedings of the Asian Conference on Computer Vision*.

Diehl, C., Makarow, A., Rösmann, C., and Bertram, T. (2022). Time-optimal nonlinear model predictive control for radar-based automated parking. *IFAC-PapersOnLine*, 55(14), 34–39.

Ferranti, L., Brito, B., Pool, E., Zheng, Y., Ensing, R.M., Happee, R., Shyrokau, B., Kooij, J.F., Alonso-Mora, J., and Gavrila, D.M. (2019). Safevru: A research platform for the interaction of self-driving vehicles with vulnerable road users. In *2019 IEEE Intelligent Vehicles Symposium (IV)*, 1660–1666. IEEE.

Fu, J., Zhang, X., Jian, Z., Chen, S., Xin, J., and Zheng, N. (2023). Efficient safety-enhanced velocity planning for autonomous driving with chance constraints. *IEEE Robotics and Automation Letters*, 8(6), 3358–3365.

Gu, P., Jing, Z., and Wu, L. (2024). A novel robust interacting multiple model filter for manoeuvering target tracking. *Electronics Letters*, 60(6), e13165.

Gupta, A., Johnson, J., Fei-Fei, L., Savarese, S., and Alahi, A. (2018). Social gan: Socially acceptable trajectories with generative adversarial networks. In *Proceedings of the IEEE conference on computer vision and pattern recognition*, 2255–2264.

Iberraken, D. and Adouane, L. (2022). Safe navigation and evasive maneuvers based on probabilistic multi-controller architecture. *IEEE Transactions on Intelligent Transportation Systems*, 23(9).

Iberraken, D., Adouane, L., and Denis, D. (2019). Reliable risk management for autonomous vehicles based on sequential bayesian decision networks and dynamic intervehicular assessment. In *2019 IEEE Intelligent Vehicles Symposium (IV)*, 2344–2351. IEEE.

Ivanovic, B., Leung, K., Schmerling, E., and Pavone, M. (2020). Multimodal deep generative models for trajectory prediction: A conditional variational autoencoder approach. *IEEE Robotics and Automation Letters*, 6(2), 295–302.

Ivanovic, B. and Pavone, M. (2019). The trajectory: Probabilistic multi-agent trajectory modeling with dynamic spatiotemporal graphs. In *Proceedings of the IEEE/CVF international conference on computer vision*, 2375–2384.

Ma, H., Meng, F., Ye, C., Wang, J., and Meng, M.Q.H. (2022). Bi-risk-rrt based efficient motion planning for autonomous ground vehicles. *IEEE Transactions on Intelligent Vehicles*, 7(3), 722–733.

Nair, S.H., Lee, H., Joa, E., Wang, Y., Tseng, H.E., and Borrelli, F. (2024). Predictive control for autonomous driving with uncertain, multimodal predictions. *IEEE Transactions on Control Systems Technology*.

Schuurmans, M., Katriniok, A., Meissen, C., Tseng, H.E., and Patrinos, P. (2023). Safe, learning-based mpc for highway driving under lane-change uncertainty: A distributionally robust approach. *Artificial Intelligence*, 320, 103920.

Sharma, B., Sharma, A., Krishna, K.M., and Singh, A.K. (2023). Hilbert space embedding-based trajectory optimization for multi-modal uncertain obstacle trajectory prediction. In *2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, 7448–7455. IEEE.

Tran, A.T., Muraleedharan, A., Okuda, H., and Suzuki, T. (2020). Scenario-based stochastic mpc for vehicle speed control considering the interaction with pedestrians. *IFAC-PapersOnLine*, 53(2), 15325–15331.

Wang, K.L., Tsao, L.W., Wu, J.C., Shuai, H.H., and Cheng, W.H. (2024). Trajfine: Predicted trajectory refinement for pedestrian trajectory forecasting. In *Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition*, 4483–4492.

Werling, M., Ziegler, J., Kammel, S., and Thrun, S. (2010). Optimal trajectory generation for dynamic street scenarios in a frenet frame. In *2010 IEEE international conference on robotics and automation*, 987–993. IEEE.

Zhu, H. and Alonso-Mora, J. (2019). Chance-constrained collision avoidance for mavs in dynamic environments. *IEEE Robotics and Automation Letters*, 4(2), 776–783.