# Reliable Multi-Level Optimization for Safe Predictive Control of Autonomous Vehicles to Avoid Uncertain Multimodal PLEVs

Emmanuel Alao, Lounis Adouane, Philippe Martinet

► To cite this version:

Emmanuel Alao, Lounis Adouane, Philippe Martinet. Reliable Multi-Level Optimization for Safe Predictive Control of Autonomous Vehicles to Avoid Uncertain Multimodal PLEVs. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2025), Oct 2025, Hang Zhou, China, China. 10.1109/IROS60139.2025.11246468. [https://doi.org/10.1109/IROS.2025.11246468](https://doi.org/10.1109/IROS.2025.11246468)

HAL Id: hal-05243754

https://inria.hal.science/hal-05243754v1

Submitted on 7 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
# Reliable Multi-Level Optimization for Safe Predictive Control of Autonomous Vehicles to Avoid Uncertain Multimodal PLEVs

Emmanuel Alao<fnref n="1" />,<fnref n="2" />, Lounis Adouane<fnref n="1" /> and Philippe Martinet<fnref n="2" />

**Abstract**— Safety assurance using all perceptual information to predict the motion of dynamic agents is critical in urban environments and remains an open challenge. For Autonomous Vehicles (AV) operating around vulnerable road users, the risk assessment strategy often needs to address stochastic uncertainties in the multiple possible trajectories (or multimodal motion) of the surrounding traffic agents. However, this increases the complexity of the navigation problem using the existing planners. To address this issue, this paper presents a multi-level optimization strategy that combines sampling-based and direct optimization methods for decision-making and control with improved safety and trajectory smoothness. In the primary stage, a sampling-based optimization framework systematically identifies safe candidate trajectories by employing the Fusion of stochastic Predictive Inter-Distance Profile (F-sPIDP). F-sPIDP encapsulates the multimodal dynamics of traffic agents and explicitly computes the uncertainties in their estimated or tracked states. From the set of trajectories, a reference optimal trajectory and its F-sPIDP setpoints are selected, adhering to stringent safety constraints and motion smoothness. Subsequently, a secondary local control optimization refines the optimal trajectory to ensure compliance with the AV's kinematic and dynamic constraints while accounting for the quantified uncertainty within the F-sPIDP framework. The performance of the proposed method was assessed through simulations and statistical analyses, evaluating its robustness to diverse levels of uncertainty.

## I. INTRODUCTION

A safety component, or more precisely a risk assessment and management unit, is a prerequisite for all autonomous driving systems moving in urban environments. This involves avoiding risky situations between the motion of the Autonomous Vehicle (AV) and other traffic agents. To this end, predicting the future motion of the surrounding traffic agents has been a topic of much research. In addition, vulnerable road users using electric vehicles like scooters, exhibit multimodal behaviors that make it difficult to accurately track their motion using state-of-the-art deterministic trajectory prediction algorithms. Hence, this work adopts the recent motion prediction algorithms like [1], [2], and [3] that predict a distribution of multimodal trajectories as opposed to single trajectory predictors (cf. Fig. 1). Predicting the multimodal behavior of traffic agents is very important for integrating uncertainties arising from perception noises and sudden motions of other traffic participants like the rapid change in velocity of Personal Light Electric Vehicles (PLEVs).

<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 global safe trajectory, and the local control trajectory</figcaption></figure>

Two primary issues arise in such a scenario. First is the uncertainty in the predicted state (position, orientation, sizes) of the surrounding agents. The second issue is caused by the multimodal prediction of the agents' motion, which makes it difficult to apply methods such as [4] and [5] to decide the appropriate control actions to be taken to manage the risk of collision without being too conservative.

This paper proposes a decision-making and control strategy using a multi-level motion optimization method to address the above problems. More specifically, below are the key contributions of this work:

- A multi-level motion optimization architecture is proposed to combine sampling-based planning with local control refinement, enabling robust and smooth motion under uncertainty.

- The Fusion of stochastic Predictive Inter-Distance Profile (F-sPIDP) is integrated into the motion planning process, extending the concept of Predictive Inter-Distance Profile (PIDP) [6], [7], [8] to enable a structured assessment of the stochastic uncertainties and multimodal behaviors of surrounding agents.

- A sampling-based optimization planner is developed to evaluate candidate trajectories using F-sPIDP-derived safety constraints. And a subsequent local control optimization method is presented to refine the selected trajectory by generating dynamically feasible control actions that remain consistent with the underlying uncertainty profile. The overall multi-level framework is validated through simulations, demonstrating enhanced safety, robustness, and motion smoothness under varying levels of uncertainty.

## II. RELATED WORKS

Recent research in autonomous systems has shifted toward multimodal prediction methods to handle the unpredictable behaviors of traffic agents in dynamic environments. Unlike

<footnote n="1">¹Université de Technologie de Compiègne (UTC), Heudiasyc UMR-CNRS 7253, Compiègne, France
{emmanuel.alao, lounis.adouane}@hds.utc.fr</footnote>

<footnote n="2">²Centre Inria d'Université Côte d'Azur, (INRIA), ACENTAURI, Sophia Antipolis, France philippe.martinet@inria.fr</footnote>
<figure><img src="image_3.png" /><figcaption>Fig. 2. The multi-level motion optimization architecture showing the inputs and outputs of each module. Red shows the sampling-based trajectory selection procedure, and blue shows the local control optimization step</figcaption></figure>

traditional deterministic approaches [9], these newer probabilistic models generate a range of possible outcomes, capturing uncertainties in agent motion. Two notable methods in this field produce distributions over potential predictions, focusing on physical principles [10] [11] or data-driven methods [12] [13]. The key outputs of interest in this work are the predicted multimodal trajectories of agents, their associated probabilities, and the state covariance representing uncertainties, enabling more robust navigation in complex settings.

An important part of control strategies for autonomous systems employs optimization techniques to satisfy various limitations on the predicted motion of the system. Model Predictive Control (MPC) [14] is unique for its ability to optimize motion smoothness while handling system constraints. Diverse extensions of MPC have been explored to handle uncertainties stemming from sensor noises and model inaccuracies.

For instance, Stochastic MPC (SMPC) [15] is a popular extension of MPC that models uncertainties in the system as probabilistic constraints. In [16], the system control was formulated as an SMPC problem with uncertainties in the predicted motion of the surrounding agents. Despite their robustness, SMPC approaches are often very conservative when applied to multimodal agents due to the need to examine all possible trajectories of the agents.

Furthermore, most MPC approaches employ direct optimization methods that generally compute local optimal solutions due to the nonlinear nature of most dynamical systems [17]. Sampling-based methods can avoid such problems by searching for multiple solutions to the problem. For example, [18] presented a Risk-based Rapidly-exploring Random Tree (Risk-RRT) algorithm to search for safe trajectories in a tight environment. [19] also employed a sampling-based optimization method to generate a set of collision-free trajectories for an ego-vehicle 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. In [20], the authors introduced an uncertainty-aware decision transformer to estimate state uncertainties and improve the driving decision under uncer-

tainties. Similarly, [21] proposes to generate kinematic-aware trajectories for the AV using neural stochastic differential equations (SDE). However, ensuring the interpretability of the decision-making process remains an open research challenge.

In this paper, we present an approach that combines the benefits of sampling-based methods and direct optimization-based methods. Using a hybrid approach, we select the best trajectory from a set of candidate trajectories that satisfy the desired safety constraints with uncertainties. Then, we compute the control actions to track the reference trajectory while respecting the predicted uncertainty.

### III. MAIN BACKGROUND

The proposed control architecture for the AV (cf. Fig. 2), like most architectures, requires inputs from the perception and localization modules to get information about the state of the surrounding agents and the environment (cf. block P in Fig. 2). The multi-level motion optimization and control comprises a sequence of five important modules. Beginning with the Multimodal Prediction Module (block 1), which uses perception data to predict multiple possible trajectories of surrounding agents ($\{x_i(t)\}_{j=1}^{N_{traj}}$ where $x_i(t) = (x_i(t), y_i(t), \theta_i(t))$ represents the position and orientation over time $t$ for the $j$-th mode of the $i$-th agent), their probabilities $Pr(j)$, and uncertainties $\mathcal{V}[x_i, y_i, \theta_i]$, providing a probabilistic view of their potential movements. This module is taken from the literature assumptions [10], [13]. Simultaneously, the Vehicle Candidate Trajectories Generation Module (block 2, cf. Section VI-A) computes a set of candidate lane-disciplined trajectories ($x_{traj} \in \mathcal{X}_{traj}$) for the autonomous vehicle in the road-aligned Frenet frame as proposed in [22]. This ensures that the motion of the vehicle does not only depend on the motion of the surrounding agents (PLEVs) but respects the expected lane change and lane-keeping behaviors.

Then, F-sPIDP collision checks (block 3, cf. Section V) performs risk assessment to filter unsafe candidate trajectories of the autonomous vehicle without being too conservative by considering the uncertainty in the multimodal motion of the surrounding agents. The safe set of candidate trajectories ($x_{traj} \in \mathcal{X}_{safe}$) is then sent to the optimal trajectory selection module (block 4, cf. Section VI-A) that
selects the optimal trajectory from the set of safe trajectories ($x_{ref} \in \mathcal{X}_{safe}$) based on the desired smoothness criteria. Finally, the local control optimization module (block 5, cf. Section VI-C) performs robust optimization to compute the local controls ($u(t)$) that follow the reference trajectory while satisfying various constraints on the control inputs of the vehicle and the uncertainty in the F-sPIDP reference trajectory.

The following sections present the formulation of the F-sPIDP, which is the core of the proposed architecture.

# IV. STOCHASTIC RISK ASSESSMENT

## A. Stochastic PIDP (sPIDP) Risk Assessment Metric

The conventional Predictive Inter-Distance Profile (PIDP) [8], [6] offers a reliable indicator of the degree of risk over a finite prediction period, assuming that the system's disturbances are minimal or equal to zero. PIDP measures the Euclidean distance between interacting agents, such that collision may occur when the minimum point of the PIDP curve is less than the set safety distance. For instance, in Fig. 3 the minPIDP₁ and minPIDP₂ are above the $d_{safe}$ indicating that the current motion of the AV is safe with regards to the PLEVs' trajectories 1 & 2, whereas minPIDP₃ $\le d_{safe}$ indicating a possible collision. Nonetheless, it is true that both internal and external uncertainties in the driving environment influence the majority of autonomous driving systems. As a result, the true state trajectory of the traffic agents will differ from the prediction. Which, if disregarded, may lead to the dangerous performance of the system. This highlights the need for a more robust approach that improves the standard PIDP.

The fundamental idea of stochastic PIDP is to break down 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)$$

where the estimated PIDP($t$) := $E[sPIDP] \in \mathbb{R}^+$ is the estimated inter-distance of the position vectors of the interacting agents, $w(t)$ denotes the uncertainties in the system, over the prediction time interval $0 \le t \le t_{hor}$.

## B. Deterministic Component of sPIDP

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

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

where $\tilde{p}_a$ and $\tilde{p}_i$ are the current estimates of the position of the AV and the i-th traffic participant. Indeed, (2) is the expected PIDP that corresponds to the traditional PIDP in [8] and [6].

## C. Stochastic Component of sPIDP

The uncertainty in the motion of the agents represented as $w(t) \in \mathbb{R}$ in (1) often has a significant impact on the behavior of the AV. Nevertheless, the value of $w(t)$ is typically not provided by state-of-the-art motion prediction algorithms. Therefore, we project the predicted state covariance uncertainty of the agents to obtain the predicted inter-distance uncertainty, $w(t) : \mathbb{R}^{n \times n} \to \mathbb{R}$. Let the variance of the inter-distance uncertainty be:

$$V[sPIDP] = E[d^2] - (E[d])^2 \quad (3)$$

where $E[d] =: E[sPIDP]$ is the expected value of sPIDP given in (2), therefore we present below the derivation of $E[d^2]$.

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

The formula for defining the expected values of the agents' squared position component is $E[x^2] = V[x] + E[x]^2$. The variance of each position component $V[\cdot]$ derived from the uncertainty provided by the motion prediction algorithm [10], [12] can be utilized to compute $E[x^2]$. The stochastic PIDP of possible PLEV trajectories is depicted in Fig. 3, where a 2D surface represents the deterministic and stochastic components of sPIDP between an AV and the PLEV. The proposed approach does not rely on a specific assumption about the distribution of the uncertainty, which makes it applicable to most motion prediction algorithms.

# V. MULTIMODAL AGENTS RISK ASSESSMENT USING FUSION OF STOCHASTIC PIDP (F-sPIDP)

Unlike vehicles, pedestrians and PLEVs are known to exhibit multimodal behaviors. This implies that each agent may have multiple predictions given by the same or multiple motion prediction algorithms. This increases the complexity of the navigation problem since a single agent may have multiple sPIDP. In [23], 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. 3).

Before calculating the F-sPIDP, it is essential to extract certain parameters from each sPIDP. These parameters represent key features of each sPIDP that highlight significant points on 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, at each time step, all sPIDP parameters are merged into a single quadratic polynomial curve, weighted by their probability Pr(j). These parameters are integrated by
<figure><img src="image_4.png" /><figcaption>Fig. 3. The multiple sPIDP and the corresponding F-sPIDP between an AV and the possible trajectories of an agent</figcaption></figure>

calculating the probabilistic center of mass (pCOM) for all sPIDPs:

$$pCOM(t) = \sum_{j=1}^{N_{traj}} Pr(j) \times sPIDP_j(t) \quad (5)$$

where, each maneuver $j \in \mathbb{N}^{N_{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_{traj}$ indicates the total number of maneuvers considered. The F-sPIDP($t$) = $pCOM(t)$, 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)$$

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

## VI. MULTI-LEVEL TRAJECTORY OPTIMIZATION

In this paper, we propose to ensure safety using a multi-level motion optimization strategy. The overall motion and control optimization problem is divided into two main tasks: Safe global trajectory selection and local control optimization. This approach efficiently assesses and manages risk while accounting for unforeseen disturbances in the motion of surrounding agents in the traffic environment. Fig. 2 shows the steps of the multi-level motion optimization procedure, which are presented in the following subsections.

### A. Sampling-based Safe Optimal Trajectory Selection

Given a starting position, the optimization problem aims to find a safe trajectory $\mathbf{x}_{ref}[t, t_{hor}]$ from a set of trajectories samples $\mathcal{X}_{traj}$ that leads the AV to the target destination without colliding with other traffic agents. The trajectory samples are 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. We want to optimize the following:

$$\min_{\mathbf{x}_{ref}[t, t_{hor}]} \sum_{t=0}^{t_{hor}} (K_1\ddot{x}(t) + K_2\ddot{y}(t) + K_3(v(t) - v_{ref})) \quad (7)$$

subjected to:

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

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

where $\mathbf{x}(t) = (x(t), y(t), \theta(t))$ is the state vector of the AV, the cost function (7) ensures the smoothness of the AV's motion and aims to drive the speed of the AV $v(t) = \sqrt{\dot{x}^2(t) + \dot{y}^2(t)}$ towards the reference velocity $v_{ref}$. $K_1$, $K_2$, and $K_3$ are gains for the cost functions. The various constraints on the dynamics of the system are represented by the nonlinear function in (8) as proposed in [22]. The following subsection presents the proposed method to satisfy the safety constraint in (9) to avoid the surrounding agents.

### B. Uncertainty-aware Collision Check

The candidate trajectories are transformed from the Frenet frame to the Cartesian coordinates to prevent collisions with other traffic agents. Instead of considering multiple trajectories of each agent, we compute the F-sPIDP of all PLEVs. The F-sPIDP of each candidate trajectory is computed as shown in Section V, effectively reducing the multiple PIDs to a single prediction (the F-sPIDP). The set of trajectories that satisfy constraint (9) for all i-th PLEVs are defined as:

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

where $\mathcal{X}_{safe}$ is the safe set. Similar to the traditional PIDP, safety is guaranteed when $F-sPIDP_i \ge d_{safe}$ for all the agents. We define $d_{safe} = R+r+\delta_v$, where the radius of the AV and PLEV are $R$ and $r$ respectively, $\delta_v$ is a braking distance. The optimal trajectory $\mathbf{x}_{ref}$ and its F-sPIDP$_{ref}$ is the global trajectory that minimizes the objective function (7) and satisfies the constraints (8) & (10) (cf. Fig. 4). Algorithm 1 gives a summary of the optimal trajectory selection process.

### C. Local Control Optimization

The local control 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}[t, t_{hor}]} \sum_{t=0}^{t_{hor}} (\|\mathbf{x}(t) - \mathbf{x}_{ref}(t)\|_Q + \|\mathbf{u}(t)\|_R + \delta^2) \quad (11)$$
<figure><img src="image_5.png" /><figcaption>Fig. 4. The candidate trajectories and the corresponding F-sPIDP between an AV and the possible trajectories of an agent: each F-sPIDP curve represent the fusion of inter-distances between AVs' candidate trajectory and all the maneuvers of the agent</figcaption></figure>

<figure><img src="image_6.png" /><figcaption>Fig. 5. The local trajectory of the AV while respecting the uncertainty constraint $V[sPIDP(t)]$</figcaption></figure>

**Algorithm 1 Safe Optimal Trajectory Selection**

**Given** set of AV trajectory samples $\mathcal{X}_{traj}$

**Initialize** empty set of safe trajectory $\mathcal{X}_{safe}$

**for** each trajectory $\mathbf{x}_{traj} \in \mathcal{X}_{traj}$ **do**

**Compute** F-sPIDP between $\mathbf{x}_{traj}$ and all trajectories of the i-th Agent

if $\mathbf{x}_{traj}$ satisfy Equation (8) & (10) **then**

**Add** $\mathbf{x}_{traj} \to \mathcal{X}_{safe}$

**Compute** objective cost $\mathcal{J}(\mathbf{x}_{traj})$ in Equation (7)

end if

end for

**Return** $\mathbf{x}_{ref} \leftarrow \arg \min \mathcal{J}(\mathbf{x}_{traj})$, where $\mathbf{x}_{ref} \in \mathcal{X}_{safe}$

subjected to:

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

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

$$V[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 $\delta$ is the inter-distance error, the current inter-distance $d(t) = \|\tilde{\mathbf{p}}_a(t) - \mathbf{p}_i(t)\|_2$, here $\mathbf{p}_i = (\tilde{x}_i(t), \tilde{y}_i(t))$ denotes the predicted positions of the PLEV's most probable trajectory (with highest probability). The uncertainties in the other multimodal trajectories are integrated in the reference F-sPIDP$_{ref}$. The aim is to optimize the motion of the AV with respect to the reference trajectory $\mathbf{x}_{ref}$ and its F-sPIDP$_{ref}$ obtained in Section VI-B while keeping the inter-distance error below the uncertainty $V[sPIDP]$ (cf. Fig. 5). $\|\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 $V[sPIDP]$ (cf. Fig. 5). 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.

# VII. SIMULATION RESULTS AND DISCUSSIONS

## A. Implementation Details

The proposed method was implemented in MATLAB leveraging built-in functions for the sampling-based reference trajectory computation, while the direct optimization

problem was solved using the CasADi optimization framework [24]. The AV is modeled using the bicycle model, such that the rate of change of its position $x, y$ and orientation $\phi$ in the Cartesian coordinates are defined as:

$$\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)$$

Here $\beta = \arctan(\frac{l_r}{l_r+l_f} \tan \gamma)$ denotes the heading of its center of gravity, $(v, \gamma) \in \mathbf{u}$ are the AV's control inputs. The lengths of the rear wheel and front wheels of the vehicle are $l_r$ and $l_f$, respectively. At each time step $\Delta t = 0.1$, we predict the motion of the traffic agents over a time horizon, $t_{hor}$. (cf. video shown through this link https://youtu.be/S4uAOAuYsGU)

## B. Comparison between F-PIDP and F-sPIDP

The proposed F-sPIDP was evaluated against the earlier proposed F-PIDP [8] in a dangerous scenario, where a PLEV was suddenly detected in front of the AV. Fig. 6 (a) shows the current state of the agents, the predicted motion of the AV, and the predicted multimodal motion that the PLEV can execute in this scenario. The plot of the inter-distance between their future motion shows a possibility of violating the safety constraint since both the F-sPIDP and F-PIDP curves are similar and below the safety distance ($d_{safe}$) (cf. Fig. 6 (b)). This implies that both methods can account for the multimodal uncertainties in the motion of the detected agent (PLEV). F-PIDP assumes a deterministic state estimate, which is a liability in the presence of highly dynamic agents. As shown in Fig. 6 ((c) & (d)), the reference trajectory using F-PIDP (F-PIDP$_{ref}$) is invariant to the degree of noise $\Sigma_T$ in the estimated inter-distance, as compared to the reference F-sPIDP trajectories (F-sPIDP$_{ref}$) that varies with respect to the magnitude of the estimated state uncertainty. As a result, F-PIDP risks generating paths that are overly optimistic and close to the safety distance, potentially resulting in dangerous situations in real-world applications. In contrast, F-sPIDP incorporates the stochastic uncertainty ($V[sPIDP]$), resulting in safer trajectories.

## C. Comparative Analysis of Proposed method

We simulate a cut-in scenario, where a PLEV swiftly enters the road as it tries to overtake another vehicle (cf.
<figure><img src="image_7.png" /></figure>

<figure><img src="image_8.png" /><figcaption>Fig. 6. Comparison between F-PIDP and F-sPIDP: (a) the current state of the agents, the predicted motion of the AV, and the predicted multimodal motion of the PLEV, (b) the PIDPs, F-PIDP, F-sPIDP, and estimated inter-distance uncertainty plot, showing possible collision with PLEV (all PIDPs < $d_{safe}$), (c) & (d) sampled trajectories for varying levels of uncertainty, where F-PIDP$_{ref}$ is invariant to the level of uncertainty</figcaption></figure>

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

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

Fig. 7). This scenario emulates a realistic challenge for autonomous navigation, characterized by sudden and unpredictable maneuvers from vulnerable road users. We focus on PLEV's motion modeled with a spectrum of multi-object detection and tracking errors, reflecting translation inaccuracies typical of Lidar-based and camera-based systems. These errors could reach ($\Sigma_T$ = 1.0 m) [25], introducing significant uncertainty into the PLEV's state estimates. The average computation time of the sampling algorithm is 75 ms, while the direct local control refinement optimization takes 20 ms; the sampling-based algorithm can have significant speed-ups through parallel processing of the sampled trajectories.

**Trajectory Performance:** The proposed algorithm, utilizing the multi-level optimization with F-sPIDP was benchmarked against the Chance Constraint Optimization (CC-Opt) method, a well-established baseline for obstacle avoidance under uncertainty [15]. Both methods successfully avoid collision with the PLEV by computing a local optimal trajectory that accounts for all possible PLEV trajectories within the uncertainty bounds. However, the baseline approach results in a reactive AV motion, excessively influenced by the PLEV's uncertain behavior. The AV's trajectory exhibits pronounced oscillations (wavy motion cf. Fig. 7 (a)) as it adjusts to the broad envelope of potential PLEV trajectories, compromising ride comfort and lane discipline.

In contrast, the proposed method employs sampling-based optimization to generate collision-free trajectories that inherently respect desired lane-keeping and lane-changing maneuvers.

By integrating stochastic uncertainty into the F-sPIDP model, the algorithm selects an optimal trajectory and F-sPIDP$_{ref}$ that balances safety and smoothness, which is then refined via local control optimization to adhere to vehicle dynamics. This results in a significantly less erratic AV motion, maintaining smoother transitions despite the sudden cut-in and uncertainties in the motion of the PLEV (cf. Fig. 7 (b)).

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

<figure><img src="image_12.png" /><figcaption>Fig. 7. Comparison of the trajectory performance: (a) Baseline (CC-Opt) results in oscillatory AV motion, excessively influenced by the PLEVs' uncertain behavior, (b) Proposed method shows little oscillation by generating collision-free trajectories that respect desired lane-changing and lane-keeping maneuvers while accounting for uncertainties in the PLEV motion</figcaption></figure>
**Velocity Profile:** The above behavior is also observed in the velocity profile of the AV. As seen in Fig. 8 ((a) & (b)), both methods decelerate strongly upon initial detection of the parked vehicle and the PLEV as it enters the road, reflecting an immediate response to the perceived collision risk, however the baseline method makes frequent deceleration actions to maintain a safe distance to the evolving set of multimodal trajectories of the PLEV. Therefore, using the baseline method results in a jerky velocity profile that is too conservative, potentially compromising passenger comfort and energy efficiency.

<figure><img src="image_13.png" /><figcaption>Fig. 8. Velocity profile over time for varying level of uncertainty $Σ_T = [0, 0.5]m$: (a) Baseline (CC-Opt) approach showing multiple pronounced decelerations, (b) Proposed method with a single deceleration upon PLEV detection</figcaption></figure>

**Safety Distance Metrics:** A critical metric for assessing safety is the minimum inter-distance (Min Dist.) between the AV and the other vehicles. For clarity, we focus on the interaction between the AV and PLEV. We performed multiple simulation runs of 10s and set the desired safety distance to 3 meters. Fig. 9 ((a) & (b)) shows the minimum inter-distance between the AV and the PLEV for different levels of uncertainty across multiple simulation runs. For the baseline method, the AV maintains an average safety clearance around the 3-meter threshold across most runs, however, the minimum inter-distance occasionally drops below the set 3-meter threshold (cf. Fig. 9(a)). This breach occurs because the chance constraints are optimized locally against a broad uncertainty, resulting in reactive adjustments to the AV motion. Increasing the desired safety distance (e.g., to 5 meters) to mitigate this issue often renders the problem infeasible due to the narrow road geometry in the cut-in scenario, highlighting a limitation in the baseline's adaptability (cf. Table I).

In contrast, the proposed method (cf. Fig. 9(b)) consis-

tently achieves a robust safety margin, maintaining a minimum inter-distance that exceeds the 3-meter threshold by up to 1 meter across the varying uncertainty levels. This large clearance stems from the sampling-based optimization stage, which leverages the F-sPIDP to select a reference trajectory that inherently respects safety constraints while accounting for uncertainties in the motion of all the PLEVs (cf. Fig. 6). By sampling trajectories aligned with lane-keeping and lane-changing intents, the proposed method avoids the reactive corrections when using the baseline approach, ensuring a stable and safe path even in narrow conditions. The subsequent local optimization further refines this trajectory to maintain vehicle dynamics without compromising the established clearance. As defined in (10), **multiple PLEVs** are easily accounted for as shown in Fig. 9(c).

<figure><img src="image_14.png" /><figcaption>Fig. 9. Minimum inter-distance between AV and PLEV over time for varying level of uncertainty $Σ_T = [0, 1]m$: (a) Baseline (CC-Opt) showing occasional drops below ($d_{safe} = 3$ m), (b) Proposed method maintaining clearance up to 1 m above desired $d_{safe}$, (c) Proposed method easily consider multiple PLEVs by selecting safe trajectories for all agents</figcaption></figure>

**Cost to Reference Metrics:** An additional metric for evaluating the proposed algorithm's performance is the Cost to Reference Position (Cost to Ref.), which quantifies the efficiency of the AV in reaching its intended destination. Defined as the Euclidean distance between the AV's final position $x_N$ and the reference position $x_{ref}$, i.e., Cost to Ref. := $||x_N - x_{ref}||$, we set $x_{ref} = 100m$. This metric complements safety considerations by assessing how well the AV balances obstacle avoidance with progress toward its destination. A lower Cost to Ref. indicates a more efficient trajectory toward the goal.

Table I shows the results of 20 simulation runs with varying uncertainty levels; the proposed method yields an average Cost to Ref. of approximately 6 meters less than the baseline. This implies that the proposed F-sPIDP-based method is always closer to the goal compared to the baseline method while simultaneously maintaining a large safety clearance from the PLEV. The reduced Cost to Ref. underscores the proposed method's potential for real-world AV deployment, where efficiency is critical for operational timelines and energy use.
<table><caption>TABLE I
COMPARISON WITH VARYING OBJECT TRACKING ERRORS. REPORT OF
THE MEAN OF (MIN DIST. ↑, COST TO REF. ↓)</caption><thead><tr><th>Method</th><th>Metric / Σ<sub>T</sub></th><th>0.1 m</th><th>0.3 m</th><th>0.5 m</th><th>1.0 m</th></tr></thead><tbody><tr><td rowspan="2">Baseline</td><td>Min Dist. [m]</td><td>3.06</td><td>2.84</td><td>3.02</td><td>3.34</td></tr><tr><td>Cost to Ref. [m]</td><td>39.82</td><td>49.63</td><td>44.44</td><td>49.10</td></tr><tr><td rowspan="2">Proposed</td><td>Min Dist. [m]</td><td>3.69</td><td>3.55</td><td>3.90</td><td>3.45</td></tr><tr><td>Cost to Ref. [m]</td><td>33.33</td><td>43.34</td><td>36.22</td><td>29.83</td></tr></tbody></table>

VIII. CONCLUSION

This paper presented an algorithm for autonomous navigation designed to ensure safe and efficient motion in the presence of dynamic entities, such as Personal Light Electric Vehicles (PLEVs), under significant uncertainty. The proposed methodology leverages a multi-level motion optimization framework that integrates sampling-based optimization with local control optimization. At its core, the Fusion of stochastic Predictive Inter-Distance Profile (F-sPIDP) captures the multimodal maneuvers and stochastic uncertainties of traffic agents, enabling the sampling stage to generate candidate trajectories that inherently respect safety constraints and lane discipline. An optimal trajectory that satisfies the F-sPIDP constraint is selected for its balance of safety and smoothness. The trajectory is subsequently refined through a local optimization process to adhere to the AV's dynamic constraints while preserving the stochastic safety margins. This dual-stage approach with F-sPIDP distinguishes itself from traditional methods by proactively embedding uncertainty into the global planning phase rather than relying solely on reactive local adjustments. Several results in PLEV cut-in scenarios affirm the efficacy of this methodology. Future work will address computation time limitations, by leveraging GPU parallel processing to accelerate the sampling process, targeting real-time experimentation.

ACKNOWLEDGMENT

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

[1] K.-L. Wang, L.-W. Tsao, J.-C. Wu, H.-H. Shuai, and W.-H. Cheng, "Trajfine: Predicted trajectory refinement for pedestrian trajectory forecasting," in Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, 2024, pp. 4483-4492.

[2] B. Ivanovic and M. Pavone, "The trajectron: Probabilistic multi-agent trajectory modeling with dynamic spatiotemporal graphs," in Proceedings of the IEEE/CVF international conference on computer vision, 2019, pp. 2375-2384.

[3] A. Gupta, J. Johnson, L. Fei-Fei, S. Savarese, and A. Alahi, "Social gan: Socially acceptable trajectories with generative adversarial networks," in Proceedings of the IEEE conference on computer vision and pattern recognition, 2018, pp. 2255-2264.

[4] J. Fu, X. Zhang, Z. Jian, S. Chen, J. Xin, and N. Zheng, "Efficient safety-enhanced velocity planning for autonomous driving with chance constraints," IEEE Robotics and Automation Letters, vol. 8, no. 6, pp. 3358-3365, 2023.

[5] L. Ferranti, B. Brito, E. Pool, Y. Zheng, R. M. Ensing, R. Happee, B. Shyrokau, J. F. Kooij, J. Alonso-Mora, and D. M. Gavrila, "Safevru: A research platform for the interaction of self-driving vehicles with vulnerable road users," in IEEE Intelligent Vehicles Symposium, 2019.

[6] K. Bellingard, L. Adouane, and F. Peyrin, "Safe and adaptive roundabout insertion for autonomous vehicle based limit-cycle and predicted inter-distance profiles," in European Control Conference (ECC), 2023.

[7] D. Iberraken and L. Adouane, "Safe navigation and evasive maneuvers based on probabilistic multi-controller architecture," IEEE Transactions on Intelligent Transportation Systems, vol. 23, no. 9, 2022.

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

[9] A. Alahi, K. Goel, V. Ramanathan, A. Robicquet, L. Fei-Fei, and S. Savarese, "Social lstm: Human trajectory prediction in crowded spaces," in Proceedings of the IEEE conference on computer vision and pattern recognition, 2016, pp. 961-971.

[10] P. Gu, Z. Jing, and L. Wu, "A novel robust interacting multiple model filter for manoeuvering target tracking," Electronics Letters, vol. 60, no. 6, p. e13165, 2024.

[11] A.-T. Tran, A. Muraleedharan, H. Okuda, and T. Suzuki, "Scenario-based stochastic mpc for vehicle speed control considering the interaction with pedestrians," IFAC-PapersOnLine, vol. 53, no. 2, pp. 15 325-15 331, 2020.

[12] P. Dendorfer, A. Osep, and L. Leal-Taixé, "Goal-gan: Multimodal trajectory prediction based on goal position estimation," in Proceedings of the Asian Conference on Computer Vision, 2020.

[13] B. Ivanovic, K. Leung, E. Schmerling, and M. Pavone, "Multimodal deep generative models for trajectory prediction: A conditional variational autoencoder approach," IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 295-302, 2020.

[14] M. Charest-Finn and S. Pejhan, "Model predictive control used in passenger vehicles: An overview," Machines, vol. 12, no. 11, 2024.

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

[16] T. Brüdigam, M. Olbrich, D. Wollherr, and M. Leibold, "Stochastic model predictive control with a safety guarantee for automated driving," IEEE Transactions on Intelligent Vehicles, vol. 8, no. 1, pp. 22-36, 2021.

[17] C. Diehl, A. Makarow, C. Rösmann, and T. Bertram, "Time-optimal nonlinear model predictive control for radar-based automated parking," IFAC-PapersOnLine, vol. 55, no. 14, pp. 34-39, 2022.

[18] H. Ma, F. Meng, C. Ye, J. Wang, and M. Q.-H. Meng, "Bi-risk-rrt based efficient motion planning for autonomous ground vehicles," IEEE Transactions on Intelligent Vehicles, vol. 7, no. 3, 2022.

[19] B. Sharma, A. Sharma, K. M. Krishna, and A. K. Singh, "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). IEEE, 2023, pp. 7448-7455.

[20] M. Schuurmans, A. Katriniok, C. Meissen, H. E. Tseng, and P. Patri nos, "Safe, learning-based mpc for highway driving under lane-change uncertainty: A distributionally robust approach," Artificial Intelligence, vol. 320, p. 103920, 2023.

[21] R. Jiao, Y. Wang, X. Liu, S. S. Zhan, C. Huang, and Q. Zhu, "Kinematics-aware trajectory generation and prediction with latent stochastic differential modeling," in 2024 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2024.

[22] M. Werling, J. Ziegler, S. Kammel, and S. Thrun, "Optimal trajectory generation for dynamic street scenarios in a frenet frame," in 2010 IEEE international conference on robotics and automation. IEEE, 2010, pp. 987-993.

[23] E. Alao, L. Adouane, and P. Martinet, "Multi-risk assessment and management in the presence of personal light electric vehicles," in 21st International Conference on Informatics in Control, Automation and Robotics, vol. 1, 2024, pp. 137-145.

[24] J. A. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, "Casadi: a software framework for nonlinear optimization and optimal control," Mathematical Programming Computation, 2019.

[25] Z. Li, S. Lan, J. M. Alvarez, and Z. Wu, "Bevnext: Reviving dense bev frameworks for 3d object detection," in Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, 2024, pp. 20 113-20 123.