# Stochastic and Safe Multi-Risk Fusion for Autonomous Navigation in the presence of PLEVs

Emmanuel Alao, Lounis Adouane, Philippe Martinet

► To cite this version:

Emmanuel Alao, Lounis Adouane, Philippe Martinet. Stochastic and Safe Multi-Risk Fusion for Autonomous Navigation in the presence of PLEVs. 36th IEEE Intelligent Vehicles Symposium (IV 2025), Jun 2025, Cluj - Napoca, Romania. pp.1942-1949, 10.1109/IV64158.2025.11097408. hal-05076678

HAL Id: hal-05076678

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

Submitted on 21 May 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
# Stochastic and Safe Multi-Risk Fusion for Autonomous Navigation in the presence of PLEVs

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

**Abstract**— Risk assessment and management in urban scenarios are difficult for automated vehicles due to perception uncertainties and the latent stochastic and high-dynamic behaviors of other agents e.g., Personal Light Electric Vehicles (PLEVs). Although the Predictive Inter-Distance Profile (PIDP) provides a continuous assessment of the risk between multiple agents, it fails when there are significant uncertainties in the estimated states of the agents. In this paper, we propose a Stochastic PIDP (sPIDP) to handle the uncertainties in the motion of the agents. sPIDP projects the uncertainties to the inter-distance between the agents. Furthermore, an Uncertainty-aware MPC is proposed to perform risk management. Statistical results considering multiple traffic scenarios show that our method is efficient for safe navigation.

## I. INTRODUCTION

Modern transportation is being transformed by autonomous vehicles (AVs), which promise better safety, efficiency, and convenience. These vehicles use complex algorithms to observe their environment, make judgments, and control their mobility in real time. A key component of algorithms for AVs is the risk assessment and management units [1] that focus on analyzing the traffic scenario for potential hazards, especially towards other road users and computing the best control inputs for the safe navigation of the AV. This requires predicting and simulating multiple road scenarios. It has led to the development of multiple hypotheses and prediction algorithms for estimating the future states of road users [2]. This uncertainty has further escalated due to the introduction of Personal Light Electric Vehicles (PLEVs) like electric scooters and bikes. These devices can exhibit multimodal behaviors with low speeds like walking pedestrians and sudden high speeds that are multiple times faster (cf. Fig. 1), their true sizes (or radius) are also difficult to detect.

To overcome this problem, it is proposed in this paper to perform risk assessment using a Fusion of stochastic PIDPs (F-sPIDP) to reduce the possible multi-trajectories of the PLEV to the most probable one. Risk management is performed using an Uncertainty-aware MPC (U-MPC) algorithm. U-MPC uses the sPIDP as a safety constraint.

Among the main contributions of this paper, we can cite:

- Accounts for uncertainties in the multimodal motion of high dynamic agents like PLEVs by considering multiple predictions for the agents (cf. Section II & III).

<figure><img src="image_2.png" /><figcaption>Fig. 1. Urban traffic scenario with AV (in red) and a PLEV (in purple): with multiple hypotheses on PLEV maneuvers and predicted state uncertainty ellipse (purple)</figcaption></figure>

- It is proposed to project the other agents' motion uncertainties into the expected safety distance between the AV and the considered PLEV, which corresponds to a more generic approach for risk assessment and management (cf. Section III).

- Finally, it is demonstrated through several realistic simulations that the proposed approach reduces the complexity of the problem while being less conservative with the possibility of adjusting the desired risk parameter based on the scenario (cf. section V).

The paper is organized as follows: Section II introduces the Predictive Inter-Distance Profile (PIDP). Section III presents the proposed F-sPIDP for multi-risk assessment of the future motion of multiple PLEVs. Section IV then describes the proposed risk management strategy using U-MPC. In Section V, several evaluations of the proposed method are presented. Finally, Section VI gives some remarks that sum up the main contributions of the paper and gives some prospects.

### A. Related Works

Various solutions exist to control autonomous vehicles [3]. Some authors concentrate on vehicle-to-vehicle interactions [4] [5] [6], while others also take into account other road users, including pedestrians and PLEVs [7]. The dynamic model of the agents in the environment is required to integrate their predicted behavior into the control process. However as noted in [7], walking pedestrians and agents using PLEVs can exhibit multiple intentions and high dynamic behaviors that are not directly observable, thereby increasing the uncertainties and the difficulty of computing safe trajectories for the AV. Therefore, the motion of a PLEV is better modeled as probabilistic functions with multimodal

<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>
predictions. The state of the art for modeling the multimodal
behavior of agents are: The Interacting Multiple Model
Filter (IMM-F) [8] [9] and Generative Adversarial Networks
(GANs) [10] [11]. IMM-F is a model-based approach, while
GANs methods are machine learning-based. The method
presented in this paper is independent of the multimodal
prediction method, hence both approaches are applicable
while having the means to get the probability of each
potential PLEV's trajectories.

Nominal risk measures that ignore uncertainties, such as the popular Time to Collision (TTC) [12] and the Predictive Inter-Distance Profile (PIDP) [13], [14], [15], often lead to dangerous situations. Therefore, recent risk-based navigation implements robust approach that formalizes the risk using probabilistic functions [16] [14] [17].

Risk-based navigation methods can broadly be divided into
optimization methods and sampling-based methods. Model
Predictive Control (MPC) [18] [19] method is a prominent
solution in the optimization domain, known for its capacity
to predict future states and make real-time adjustments.
Considering the worst-case scenario using the nominal MPC
can be very conservative, hence MPC has been extended to
Stochastic MPC (SMPC) [20] to account for uncertainties.
SMPC models the uncertainties as chance constraints in the
optimization process. It was employed in [21] [7] [22] to
address the interaction between a vehicle and a single pedes-
trian with low dynamics. In [23], the authors presented an
approach to tackle the problem introduced by the multimodal
motion of PLEVs with high dynamics using a Fusion of
PIDPs (F-PIDP) and SMPC. Motion uncertainties can also
be modeled as a Value at Risk (VaR) constraint [24] [25].
However, VaR methods are more conservative than chance
constraint methods, leading to infeasible solutions that often
violate the constraints or hinder performance.

This paper extends the approach we proposed in [23]
[26] to handle uncertainties in the estimated state of the
traffic agents. Indeed, the approach proposed in [23] [26]
only handles uncertainties in the possible trajectories that
the agent can follow. In addition, we present in this paper
a method that also handles the uncertainties in the predicted
states (positions and velocities) of the agents along each of
the predicted trajectories. Furthermore, the proposed method
also accounts for multiple agents.

Sampling-based methods include the popular Risk-RRT
[27] and Monte Carlo methods [28] [29]. They give good es-
timates of the collision probabilities for various uncertainties
with little or no approximations on the type of uncertainty.
However, they are very expensive in terms of computa-
tion time and memory required by the samples [17]. To
reduce the computation time, in [16] a sampling-based risk-
averse trajectory optimization method was applied to account
for epistemic and aleatoric uncertainties. Nonetheless, this
method was shown to scale linearly to the sample size for a
single pedestrian interaction in the best-case scenario.

## II. RISK ASSESSMENT USING PREDICTIVE INTER-DISTANCE PROFILE (PIDP)

PIDP is a risk assessment metric for continuous assess-
ment of the motion of an ego-vehicle with respect to all other
entities in the traffic environment. As demonstrated in [30],
[13] and [31], the PIDP metric can evaluate the possibility
of a collision occurring between multiple traffic participants.
Given a specific time horizon $t_{horizon}$ (cf. Fig. 2), the PIDP
measures the progress of the future distance between two
entities based on their predicted future trajectories. To con-
tinuously monitor and detect potentially harmful situations
between traffic participants, PIDP computes the Euclidean
distance between the predicted trajectories of the traffic
participants. This gives both the time scale and distance scale
measure of the risk over the specified time horizon. As such,
PIDP does not depend on the geometry of the trajectories of
any of the target participants [13], which makes it suitable
for most, if not all, traffic scenarios. When the minimum
PIDP is smaller than the combined radius of the ego-vehicle
and PLEV, $\min\text{PIDP}(t) \le R + r$ and the two considered
entities do not modify their dynamics, then a collision may
happen (cf. Fig. 2).

<figure><img src="image_3.png" /><figcaption>Fig. 2. Predictive Inter-Distance Profile (PIDP) between two traffic participants with the severity of the situations (SR: Safety Respected, SNR: Safety Not Respected, and Collision) w.r.t. the safety distance $d_{safe}$ over the time horizon</figcaption></figure>

In [23], [26], we proposed the Fusion of PIDPs (F-PIDP)
method to handle PLEVs with high dynamic multimodal
motion. However, both PIDP and F-PIDP assume that the
states (positions, velocities, etc.) of the traffic participants
are deterministic, which is not true in a real-world scenario
since only an estimate of the current state of the traffic
participants is known during motion. Furthermore, there can
be multiple hypotheses on the future trajectories of a PLEV
[32], which implies multiple PIDP for a single PLEV (cf.
Fig. 1). The problem becomes a multi-risk problem, which
is addressed in the following section by computing a Fusion
of stochastic PIDPs (F-sPIDP), which explicitly incorporates
the estimated uncertainties in the predicted instantaneous
states of the agents, rather than neglecting these uncertainties
as done using PIDP and F-PIDP (cf. Fig. 3).
### III. MULTI-RISK ASSESSMENT BASED ON F-sPIDP

Fusion of stochastic PIDP (F-sPIDP) is a multi-risk metric that measures the risk of collision between an ego-vehicle and PLEVs while accounting for the estimation uncertainties in the state of the PLEVs and their possible maneuvers. It computes the stochastic PIDP of all the predicted maneuvers that the PLEVs can make, together with their prediction uncertainties. Then, a weighted fusion of the sPIDPs is computed using a quadratic fit. The subsequent sections present a comprehensive explanation of the procedures involved in calculating the F-sPIDP. In sub-section A, the mean of the stochastic PIDP (sPIDP) is computed. Sub-section B evaluates the variance of the uncertainty of the sPIDP. The probabilistic Fusion of sPIDPs (F-sPIDP) is shown in sub-section C.

#### A. Estimation of the mean sPIDP

Consider the state space form of the dynamics and measurement equation of the PLEV.

$$ \tilde{x}_j(t+1) = A_j(t) \cdot \tilde{x}_j(t) + w_j \quad (1) $$

$$ z(t+1) = C(t) \cdot \tilde{x}_j(t) + \varepsilon \quad (2) $$

where $\tilde{x}_j(t+1)$ denotes the predicted state of the PLEV's jth maneuver at time step t, $A_j(t)$ is the state transition matrix, the measurement $z(t+1)$ is same for all maneuvers, and $C(t)$ is the measurement matrix. Both $w_j \sim \mathcal{N}(0, \Sigma_j^w)$ and $\varepsilon \sim \mathcal{N}(0, \Sigma^\varepsilon)$ are assumed to be Gaussian additive noises with covariance $\Sigma_j^w$ and $\Sigma^\varepsilon$ respectively. This assumption is reasonable for small deviations in the agent's instantaneous motion, whereas significant changes in motion are typically captured by a different mode within the multimodal prediction framework.

We propose to estimate the expected state of the PLEVs and their uncertainties during the prediction of the PLEVs' motion. Such uncertainties can be obtained from an appropriate estimation algorithm like [33], which gives the predicted state of the PLEV and its covariance. Therefore, only an estimate of the state of the PLEV is known as expected in a real-world scenario, unlike the traditional PIDP that assumes a deterministic state of the PLEV [13], [23]. To compute the sPIDP, let the estimated position of the ego-vehicle be $\tilde{p}(t) = [\tilde{x}_r(t), \tilde{y}_r(t)]^T$ and the estimated position of the jth maneuver of the PLEV be $\tilde{p}_j(t) = [\tilde{x}_j(t), \tilde{y}_j(t)]^T$. The mean sPIDP of the jth maneuver of the PLEV is

$$ sPIDP_j(t) = \{\tilde{d}_j(t) | j \in \mathcal{P}, 0 \le t \le t_{\text{horizon}}\} \quad (3) $$

$$ \tilde{d}_j(t) = \|\tilde{p}(t) - \tilde{p}_j(t)\| \quad (4) $$

where the stochastic inter-distance profile $sPIDP_j \in \mathbb{R}^+$ at time t is evaluated by computing the Euclidean distance $||\cdot||$ between the AV and each possible trajectory $j \in \mathcal{P}$ of the PLEV. Fig. 3 shows the mean sPIDP of three possible maneuvers (red, blue, and green curves) of a PLEV.

#### B. Inter-distance Uncertainty Propagation

As earlier mentioned, there are uncertainties in the predicted state of the PLEVs which implies that the computed sPIDP is also an estimation of the true PIDP$_{true}$ with some degree of uncertainty. Hence there is a mapping from the covariance of the motion uncertainty of the PLEV to the inter-distance uncertainty: $\tilde{d}: \mathbb{R}^{n \times n} \to \mathbb{R}$.

For brevity, we temporarily drop the jth term and time t. Let the mean sPIDP be

$$ \mathbb{E}[sPIDP] = \mathbb{E}[d] = \mathbb{E}[(\tilde{x}_r - \tilde{x}_j)^2 + (\tilde{y}_r - \tilde{y}_j)^2]^{1/2} \quad (5) $$

Then the variance of the sPIDP is

$$ \mathbb{V}[sPIDP] = \mathbb{V}[d] = \mathbb{E}[d^2] - (\mathbb{E}[d])^2 \quad (6) $$

where

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

here $\mathbb{E}[\cdot]$ are the expected values, $\mathbb{E}[\tilde{x}^2] = \mathbb{V}[\tilde{x}] + \mathbb{E}[\tilde{y}^2]$. Since the complex relationship between the AV and PLEV's motion are unknown, it is assumed that the motion of the ego-vehicle and PLEV are independent, then $\mathbb{E}[\tilde{x}\tilde{y}] = \mathbb{E}[\tilde{x}]\mathbb{E}[\tilde{y}]$. Note that the variance of each of the position components $\mathbb{V}[\cdot]$ can be obtained from the covariance matrix $P_j(t+1)$ as shown in [33], defined in (8):

$$ P_j(t+1) = A_j(t) \cdot P_j(t) \cdot A_j^T(t) + \Sigma_j^w \quad (8) $$

Where $A_j(t)$ and $\Sigma_j^w$ are defined in (2). Therefore, putting (5) and (7) in the variance equation (6) will return the uncertainty of the sPIDP. The value of $\mathbb{V}[sPIDP]$ is required to compute the dynamic safety distance in Section (IV).

#### C. Probabilistic weight Fusion of sPIDPs

Having obtained the expected sPIDP and an estimate of the prediction uncertainty, the next step is to perform a fusion of the sPIDPs to obtain a single curve hereby referred to as the Fusion of sPIDP (F-sPIDP). This extension of the traditional PIDP solves the ambiguity introduced into the system by the multimodal model [32] of the PLEVs behavior.

Nevertheless, before computing the F-sPIDP, it is necessary to extract some parameters of each sPIDP. These are features of the sPIDP that represent the important points on the sPIDP curve, as presented in [23]:

1) The start point of the sPIDP denoted as $sPIDP_j(t_0)$ which has the same value for all the sPIDPs of a specific PLEV.

2) The minimum point of each sPIDP curve denoted as $sPIDP_j(t_{min})$.

3) The end point of each sPIDP curve denoted as $sPIDP_j(t_{horizon})$.

Since the sPIDP is derived from the Euclidean distance between the ego-vehicle and the PLEV, the F-sPIDP is modeled as a quadratic polynomial curve (cf. Equation 10). As a result, all of the sPIDP parameters are combined into a single quadratic polynomial curve depending on the magnitude $Pr(j)$ of their probability.
The fusion of the parameters is obtained by computing the probabilistic center of mass (pCOM) of all the PIDs. The definition of the pCOM for each of the feature points is given by (9)

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

where $Pr(j)$ is the probability of maneuver $j$, $sPIDP_j(t)$ represents the sPIDP at any of the feature points $t \in \{t_0, t_{min}, t_{horizon}\}$ and $N_{traj} \in \mathbb{N}$ is the number of maneuvers being considered. The pCOM gives a weighted average of the feature points based on the likelihood of the PLEV maneuver. Therefore, the resulting parameters for the F-sPIDP are:

* $F\_sPIDP(t_0) := pCOM(t_0)$ fusion of the start points $sPIDP_j(t_0)$

* $F\_sPIDP(t_{min}) := pCOM(t_{min})$ fusion of all the minimum points $sPIDP_j(t_{min})$

* $F\_sPIDP(t_{horizon}) := pCOM(t_{end})$ fusion of all the end points $sPIDP_j(t_{horizon})$

The above parameters are utilized to compute the variables of a quadratic curve that passes through the points. From the equation of a quadratic curve

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

where $[q_0, q_1, q_2]$ are unknown variables that describe the F-sPIDP curve and are obtained by solving the above linear equation by taking the matrix (or pseudo) inverse at $t \in [t_0, t_{min}, t_{horizon}]$. As illustrated in Fig. 3, the outcome is a singular, cohesive F-sPIDP curve that encompasses the information from all of the sPIDPs. The definition of the appropriate F-sPIDP and a coherent F-sPIDP setpoint required by the risk management algorithm to guarantee safety (cf. Section IV) can be derived as shown in [23].

## IV. RISK MANAGEMENT BASED ON UNCERTAINTY-AWARE MPC (U-MPC)

The risk management problem is formulated as an Uncertainty-aware MPC (U-MPC) problem. The proposed risk management approach aims to compute the set of control actions that the AV must execute for a safe and smooth motion, even in the presence of uncertainties and constraints caused by PLEVs and other vehicles. The proposed U-MPC is similar to the nominal MPC since they both apply optimization techniques to evaluate the sequence of optimal or sub-optimal control actions except that U-MPC focuses more on safety by accounting for the influence of the ego-vehicle's speed and the PLEV's uncertainties on the risk of collision. At each sampling time, the U-MPC solves the equation:

<figure><img src="image_4.png" /><figcaption>Fig. 3. Fusion of stochastic PIDs (F-sPIDP) of an AV and a PLEV with three (3) possible multimodal maneuvers and their uncertainties (variance)</figcaption></figure>

$$\min_{\mathbf{U}} \mathcal{F}(\tilde{\mathbf{x}}(t), \mathbf{x}_{ref}, \mathbf{U}, \mathbf{U}_{ref}) \quad (11)$$

$$\text{such that} \quad \begin{cases} \forall t & t \in \{0, \dots, N\} \\ \tilde{\mathbf{x}}(t+1) & = f(\tilde{\mathbf{x}}(t), \mathbf{u}(t)) \\ \tilde{\mathbf{x}}_j(t+1) & = f(\tilde{\mathbf{x}}_j(t), w) \\ \text{PIDP}(t) & \geq \mathcal{D}_{\text{safe}}(\delta_v, d_w(t)) \\ \mathbf{U}_{\text{min}} & \leq \mathbf{U}(t) \leq \mathbf{U}_{\text{max}} \end{cases} \quad (12)$$

based on the current estimation of the state of the ego-vehicle $\tilde{\mathbf{x}}(t+1)$ and the PLEV most probable trajectory $\tilde{\mathbf{x}}_j(t+1)$. The lower and upper bounds of the acceptable control inputs are denoted as $\mathbf{U}_{min}$ and $\mathbf{U}_{max}$, respectively. While $\mathbf{U} = [\mathbf{u}(t), \dots, \mathbf{u}(N-1)]$ are the desired safe control inputs for the ego-vehicle, $\mathcal{D}_{\text{safe}}(\cdot)$ denotes the safety distance constraint function. Therefore, the output of the U-MPC algorithm is the value of $\mathbf{U}$ that minimizes the objective function $\mathcal{F}$, defined in the following sub-section IV-A. Sub-Section IV-B defines the uncertainty and speed safety constraint.

### A. Definition of U-MPC Objective Function

It is required that the motion of the ego-vehicle must be as smooth as possible and also safe as it optimizes its tracking of the defined F-sPIDP setpoint (cf. Section III and Fig. 3). Therefore, the U-MPC optimizes a combination of multiple objective functions. For simplicity, the multi-objective functions are considered to be least-squares functions:

1) The stage cost to reach the target destination $\mathbf{x}_{ref}$: $f_x = \frac{1}{2} \|\tilde{\mathbf{x}}(t) - \mathbf{x}_{ref}\|^2$

2) The terminal cost to reach the target destination $\mathbf{x}_{ref}$ of the ego-vehicle: $f_{x_N} = \frac{1}{2} \|\tilde{\mathbf{x}}(N) - \mathbf{x}_{ref}\|^2$

3) The energy cost to reach the reference control action $\mathbf{U}_{ref}$: $f_u = \frac{1}{2} \|\mathbf{U}(t) - \mathbf{U}_{ref}\|^2$

4) The deviation cost from the reference F-sPIDP setpoint: $f_p = \frac{1}{2} \|\text{PIDP}(t) - F\_s\text{PIDP}(t)\|^2$
Therefore, the overall multi-objective function $\mathcal{F}$ is given by (13)

$$ \mathcal{F} = \gamma_1 f_x + \gamma_2 f_{x_N} + \gamma_3 f_u + \gamma_4 f_p; \quad \sum_{i=1}^{4} \gamma_i = 1 \quad (13) $$

with $\gamma_i \in \mathbb{R}^+$ representing global weights to balance the influence of the cost functions on the overall multi-objective function.

## B. Uncertainty-Based Safety Constraint

The risk of collision with other traffic agents is assumed to be proportional to the linear velocity of the AV and the uncertainty in the inter-distance between the AV and PLEV. This is because the faster the motion of the AV, the more cautious it needs to be around other road users. Similarly, the more the level of uncertainty in the predicted state of the PLEV, the higher the risk of collision. Therefore, the safety distance constraint function is assumed to be dynamic and defined as

$$ \mathcal{D}_{safe}(\delta_v, d_w(t)) = R + r + \delta_v + d_w(t) \quad (14) $$

The braking distance $\delta_v$ depends on the vehicle velocity and $d_w(t) := \mathcal{N}(0, \mathbb{V}ar[sPIDP])$ (6) is the predicted uncertainty of the current most probable sPIDP. This implies that the safety distance constraint function $\mathcal{D}_{safe}$ in (12) is linear with respect to the sum of the radii of the AV ($R$) and PLEV ($r$), the AV's braking distance, and the sPIDP uncertainty.

1) Collision Avoidance as a Chance constraint: We employ sPIDP to guarantee safety between the AV and the surrounding road users. The approach is similar to the traditional PIDP (c.f Section II), here the collision constraint is expressed as

$$ \mathcal{D}_{safe}(\delta_v, d_w(t)) \le sPIDP(t) \quad (15) $$

The uncertainty due to estimation errors in the AV and PLEVs' position (and radius) is addressed through the addition of the noise in the sPIDP to the safety distance and by formulating (15) as a chance-constraint problem with a desired probability $\alpha$ to be satisfied,

$$ \mathrm{Pr} \left( d_{safe} + d_{w}(t) \le sPIDP(t) \right) \ge \alpha \quad (16) $$

with $0.5 \le \alpha \le 1$, $d_{safe} = R + r + \delta_v$

$\alpha$ is a tunable risk parameter. When $\alpha$ is large, the behavior of the AV is conservative, which can make the chance-constraint optimization problem infeasible. However, a small value of $\alpha$ may lead to a high risk of collision.

2) Collision Avoidance Constraint Tightening: Directly adding uncertainty to the safety distance may lead to highly conservative behavior, since the estimated uncertainty may be very large. We address this problem by performing constraint tightening using the desired risk parameter $\alpha$. Hence, the constraint in (16) is separated into a deterministic and stochastic component with a limit on the inter-distance, such

that $d_{safe}$ will have a high probability of being below the PIDP. This implies that:

$$ \begin{gathered} d_{safe} + \sigma(t) \le d(t) \\ \mathrm{Pr} \left( d_w(t) \le \sigma(t) \right) \ge \alpha \end{gathered} \quad (17) $$

since the uncertainty in $d_w(t) \sim \mathcal{N}(0, \mathbb{V}ar[sPIDP])$ is known (cf. Section III-B) and $d(t)$ is the current inter-distance, the value of $\sigma(t)$ can be computed explicitly during every iteration from the quantile function of a Gaussian distribution as, $\sigma(t) = \sqrt{2 * \mathbb{V}ar[sPIDP]} \, erf^{-1}(2\alpha - 1)$. where the function $erf^{-1}(\cdot)$ denotes the inverse error function.

## V. SIMULATION RESULTS AND DISCUSSIONS

In the following, the proposed risk assessment and management approach using F-sPIDP and U-MPC, is applied to simulated autonomous driving scenarios that characterize urban areas. The studied scenarios generally involve an AV trying to avoid collision in a narrow space with other vehicles.

(cf. video link https://youtu.be/mzyxZoojUB8)

The presented results have been implemented in MATLAB using the CasADi optimization framework [34] as base implementation. The AV is modeled as a rectangle inscribed in a circle with a diameter of 4 m. The PLEV is modeled as a multi-circular shaped dynamic obstacle with an overall diameter of 2 m. Simulations are executed at a sampling time step of $\Delta t = 0.05$ s. The model of the vehicle is based on the single-track bicycle kinematic model

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

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

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

where, $\beta = \arctan(\frac{l_r}{l_r+l_f} \tan \delta)$, $\dot{x}$ and $\dot{y}$ represent the longitudinal and lateral speeds of the AV, respectively, and $\dot{\phi}$ represents its angular speed. The input commands of the AV are the linear velocity $v$ and the steering angle $\delta$. The geometric length of the rear and front wheels to the center of mass of the ego-vehicle are $l_r$ and $l_f$, respectively. The AV's dynamic motion is later discretized using the explicit Runge-Kutta method of order 4. The motion of the AV is subjected to the following constraints in addition to the safety constraint

$$ y \in [0, -8]m, \delta \in [-35, 35], v \in [0, 8]m/s, d_{safe} = 3m $$

The minimum safety distance $d_{safe}$ between the AV and a PLEV is set to 3 m, with values below this indicating a collision. The PLEV has three possible maneuvers with their associated probabilities for slow forward motion (red), fast forward motion (green) and turning right (blue) (cf. Fig. 5). The predicted trajectories of the PLEV and their associated probabilities are assumed to be provided by an appropriate perception/prediction algorithm such as the one proposed in [32], hence out of the scope of this paper. However, for simplicity, the evolution of the probability of the trajectories
<figure><img src="image_5.png" /><figcaption>Fig. 4. Varying Risk Factor $\alpha \in [0.5, 1]$: (a) Velocity, (b) Inter-Distance, and (c) Safety Distance</figcaption></figure>

<figure><img src="image_6.png" /><figcaption>Fig. 5. Scenario with multiple agents: Left, shows the 2D scenario of the AV trying to avoid collision with the surrounding vehicles. Right, shows the plot of the estimated stochastic PIDP (sPIDP) for each agent and their corresponding variance ($\mathcal{V}[sPIDP]$), all stochastic PIDP above $d_{safe}$</figcaption></figure>

is assumed to be the same for the tested scenarios depending on the PLEV trajectory.

## A. Chance Constraint Risk Factor Test

The first scenario involves an AV and a PLEV crossing the road at constant velocity. The results show that even lower values of the risk factor $\alpha \in [0.5, 1]$ yield safe trajectories for the AV, either by decelerating or performing a lane change (cf. Fig. 6). The velocity plot shows a deceleration to low velocity of about 1 m/s for high values of the risk factor to create more distance, while for lower values of the risk factor the AV decelerates to an average speed of 2 m/s (cf. Fig. 4 (a) & (b)). The difference between the safety distance and the actual inter-distance for varying parameters of the risk factor is shown in Fig. 4 (c), with large values when the risk factor is high which indicates a conservative behavior.

We set $\alpha = 0.7$, and simulated multiple agents with two PLEVs and two parked vehicles. The F-sPIDP of each PLEV is computed independently. The result shows that safety is respected: all stochastic PIDs and the ground truth PIDP<sub>true</sub> of the agents are always above $d_{safe}$ without constraint violations (cf. Fig. 5).

<figure><img src="image_7.png" /><figcaption>Fig. 6. Test Scenario of the AV and a single PLEV crossing the road: showing the different trajectories of the AV for Varying Risk Factor $\alpha \in [0.5, 1]$</figcaption></figure>

## B. Statistical Comparison with SMPC

In this subsection we compare the proposed method with Stochastic MPC. The scenario involves a PLEV that suddenly enters the traffic environment at an average speed of 4 m/s as it tries to overtake two packed vehicles. This scenario is very challenging because the PLEV starts from a position only a few meters from the AV. Furthermore, different levels of multi-object detection and tracking errors were considered, starting from low translation errors for LiDAR-based tracking algorithms to high levels of uncertainty in camera-based tracking algorithms ($\Sigma_T \in [0.02, 0.1, 0.5, 1]m$) [35].
The following performance metrics are considered for comparison of the results (cf. Table I):

1) Minimum inter-distance (min PIDP): this measures the smallest safety distance or risk of collision between the AV and all the other agents. A high min PIDP indicates a safer motion.

2) Cost to reach the reference position (Cost to Ref.): measures the algorithm's efficiency to reach the desired goal quickly. It measures the distance between the final position of the AV and the reference position (||$x_N$ - $x_{ref}$||, where $x_{ref}$ = 100m) A small value of Cost to Ref. implies an efficient motion towards the reference position (AV's destination).

3) Computation Time: measures the time it takes for the algorithm to compute the control actions required by the AV. In general, low computation times are desirable.

Report of the mean (over 20 runs) of the minimum inter-distance (min PIDP), cost to reach the reference position (Cost to Ref.), and Computation Time are shown in Table I.

To ensure clarity, just the outcomes of the interaction between the AV and PLEV are studied. Due to the high rate of constraint violation using SMPC with only the most probable trajectory, we considered SMPC with all three (3) trajectories as collision constraints. This approach reduces the collision rate, but have a significant impact on the efficiency of the method. Therefore, when the uncertainty is low ($\Sigma_T$ ≤ 0.1m), the resulting motion is smooth using all the methods considered. SMPC gives high values of minimum PIDP due to its conservative behavior since it tries to avoid all three predicted trajectories of the agents. Whereas, the proposed approach using only the most probable trajectory and the F-sPIDP$_{setpoints}$ maintains a safety distance above $d_{safe} = 3m$, with the lowest Cost to Ref. and computation time, which can be further optimized for real-time application. A low Cost to Ref. of 43.80 m using the proposed approach as compared to 67.54 m using SMPC shows that the final position of the AV using the proposed approach is about 24 m ahead of the SMPC-based method (cf. Fig. 7).

For high levels of translation errors ($\Sigma_T$ ≥ 0.5m) often observed in camera-based object detection algorithms [35], SMPC becomes too conservative while trying to increase the safety distance (min PIDP) between the AV and PLEVs. This behavior in such a narrow space often results in poor performance that violates the constraints and keeps the AV far from the reference position as indicated by the high value of the Cost to Ref. (cf. Table I). Moreover, when the object tracking error is 1 m, SMPC couldn't find a safe solution, thereby violating the constraint by moving too close to the road boundary (cf. Fig. 7). A limitation of the proposed method, common to most MPC-based approaches, is its high computational time. This can be improved by strategies such as increasing the sampling interval $\Delta t$, reducing the number of prediction steps $N$, or leveraging sampling-based optimization techniques that are capable of utilizing GPU acceleration for faster computation.

<figure><img src="image_8.png" /><figcaption>Fig. 7. Final Position of AV at Final Time $N$ for all methods with tracking error of 1m: SMPC violates the constraint by moving too close to the boundary, U-MPC maintains a safe distance without being too conservative</figcaption></figure>

<table><caption>TABLE I COMPARISON WITH VARYING OBJECT TRACKING ERRORS. REPORT OF THE MEAN (OVER 20 RUNS) OF (MIN PIDP ↑, COST TO REF. ↓, AND COMPUTATION TIME ↓)</caption><thead><tr><th>Method</th><th>Metric / Σ<sub>T</sub></th><th>0.02 m</th><th>0.1 m</th><th>0.5 m</th><th>1 m</th></tr></thead><tbody><tr><td rowspan="3">SMPC</td><td>min PIDP [m]</td><td><strong>4.64</strong></td><td><strong>4.67</strong></td><td><strong>4.15</strong></td><td><strong>5.50</strong></td></tr><tr><td>Cost to Ref. [m]</td><td>67.54</td><td>67.83</td><td>68.72</td><td>86.47</td></tr><tr><td>Computation Time [s]</td><td>0.17</td><td>0.17</td><td>0.20</td><td>0.26</td></tr><tr><td rowspan="3">Proposed Approach</td><td>min PIDP [m]</td><td>3.28</td><td>3.32</td><td>3.46</td><td>3.45</td></tr><tr><td>Cost to Ref. [m]</td><td><strong>43.80</strong></td><td><strong>43.90</strong></td><td><strong>51.64</strong></td><td><strong>55.07</strong></td></tr><tr><td>Computation Time [s]</td><td><strong>0.11</strong></td><td><strong>0.13</strong></td><td><strong>0.16</strong></td><td><strong>0.16</strong></td></tr></tbody></table>

## VI. CONCLUSIONS

This paper has proposed a fusion of stochastic PIDs (F-sPIDP) for Multi-risk assessment and an Uncertainty-aware MPC (U-MPC) as a Multi-risk management strategy for autonomous vehicles moving in urban areas. Stochastic PIDP accounts for uncertainties in the possible maneuvers that the traffic participants can take and the errors in tracking their instantaneous motion using the estimated state of the PLEVs and their uncertainties. The proposed F-sPIDP is a weighted fusion of the stochastic PIDs of the possible multimodal maneuvers that the PLEV can execute while considering their motion probability. The resulting fusion is then used as a reference by the proposed U-MPC algorithm. U-MPC computes the appropriate control actions for the AV with a dynamic safety distance constraint on the motion of the AV and the uncertainties from F-sPIDP. The performance of the proposed multi-risk assessment and management strategy has been evaluated and shown to perform better compared to the nominal SMPC method. Validation of the proposed method through experiments on an autonomous vehicle will be taken into consideration in future work.

## ACKNOWLEDGMENT

This research is part of the ANNAPOLIS project (link 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] W. M. D. Chia, S. L. Keoh, C. Goh, and C. Johnson, "Risk assessment methodologies for autonomous driving: A survey," *IEEE transactions on intelligent transportation systems*, vol. 23, no. 10, pp. 16923-16939, 2022.

[2] S. Lefèvre, D. Vasquez, and C. Laugier, "A survey on motion prediction and risk assessment for intelligent vehicles," *ROBOMECH journal*, vol. 1, no. 1, pp. 1-14, 2014.

[3] B. Jafary, E. Rabiei, M. A. Diaconea, H. Masoomi, L. Fiondella, and A. Mosleh, "A survey on autonomous vehicles interactions with human and other vehicles," in *14th PSAM International Conference on Probabilistic Safety Assessment and Management*, 2018.

[4] J. Hu, Y. Zhang, and S. Rakheja, "Path planning and tracking for autonomous vehicle collision avoidance with consideration of tire-road friction coefficient," *IFAC-PapersOnLine*, vol. 53, no. 2, pp. 15524–15529, 2020.

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

[6] H. Taghavifar, L. Taghavifar, C. Hu, C. Wei, and Y. Qin, "Optimal reinforcement learning and probabilistic-risk-based path planning and following of autonomous vehicles with obstacle avoidance," *Proceedings of the Institution of Mechanical Engineers, Part D: Journal of Automobile Engineering*, 2023.

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

[8] N. Schneider and D. M. Gavrila, "Pedestrian path prediction with recursive bayesian filters: A comparative study," in *german conference on pattern recognition*. Springer, 2013, pp. 174–183.

[9] Y.-L. Lee, "Using a new circular prediction algorithm to design an imm filter for low update rate radar system," *Sensors*, vol. 20, no. 18, p. 5035, 2020.

[10] A. Sadeghian, V. Kosaraju, A. Sadeghian, N. Hirose, H. Rezatofighi, and S. Savarese, "Sophie: An attentive gan for predicting paths compliant to social and physical constraints," in *Proceedings of the IEEE/CVF conference on computer vision and pattern recognition*, 2019, pp. 1349–1358.

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

[12] M. M. Minderhoud and P. H. Bovy, "Extended time-to-collision measures for road traffic safety assessment," *Accident Analysis & Prevention*, vol. 33, no. 1, pp. 89–97, 2001.

[13] K. Bellingard, L. Adouane, and F. Peyrin, "Risk assessment and management based on neuro-fuzzy system for safe and flexible navigation in unsignalized intersection," in *IEEE Intelligent Vehicles Symposium (IV)*, 2023, pp. 1–7.

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

[15] K. Bellingard, L. Adouane, and F. Peyrin, "Adaptive and reliable multi-risk assessment and management control strategy for autonomous navigation in dense roundabouts," in *IFAC World Congress 2023, Yokohama, Japan*, 2023.

[16] T. Lew, R. Bonalli, and M. Pavone, "Risk-averse trajectory optimization via sample average approximation," *IEEE Robotics and Automation Letters*, 2023.

[17] K. M. Frey, T. J. Steiner, and J. P. How, "Collision probabilities for continuous-time systems without sampling [with appendices]," *arXiv preprint arXiv:2006.01109*, 2020.

[18] K.-D. Kim and P. R. Kumar, "An mpc-based approach to provable system-wide safety and liveness of autonomous ground traffic," *IEEE Transactions on Automatic Control*, vol. 59, no. 12, pp. 3341–3356, 2014.

[19] E. Fiasché, P. Martinet, and E. Malis, "Towards autonomous robot navigation in human populated environments using an universal sfm and parametrized mpc," in *IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, 2023.

[20] T. A. N. Heirung, J. A. Paulson, J. O'Leary, and A. Mesbah, "Stochastic model predictive control—how does it work?" *Computers & Chemical Engineering*, vol. 114, pp. 158–170, 2018.

[21] M. Castillo-Lopez, P. Ludivig, S. A. Sajadi-Alamdari, J. L. Sanchez-Lopez, M. A. Olivares-Mendez, and H. Voos, "A real-time approach for chance-constrained motion planning with dynamic obstacles," *IEEE Robotics and Automation Letters*, vol. 5, no. 2, pp. 3620–3625, 2020.

[22] B. Škugor, J. Deur, V. Ivanovic, and H. E. Tseng, "Stochastic model predictive control of an autonomous vehicle interacting with pedestrians at unsignalized crosswalks," in *European Control Conference (ECC)*, 2023, pp. 1–7.

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

[24] A. Hakobyan, G. C. Kim, and I. Yang, "Risk-aware motion planning and control using cvar-constrained optimization," *IEEE Robotics and Automation Letters*, vol. 4, no. 4, pp. 3924–3931, 2019.

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

[26] 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 (ICINCO)*, vol. 1, 2024, pp. 137–145.

[27] 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, pp. 722–733, 2022.

[28] L. Janson, E. Schmerling, and M. Pavone, "Monte carlo motion planning for robot trajectory optimization under uncertainty," in *Robotics Research: Volume 2*. Springer, 2017, pp. 343–361.

[29] L. J. Hong, Y. Yang, and L. Zhang, "Sequential convex approximations to joint chance constrained programs: A monte carlo approach," *Operations Research*, vol. 59, no. 3, pp. 617–630, 2011.

[30] 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, pp. 1–7.

[31] D. Iberraken, L. Adouane, and D. Denis, "Reliable risk management for autonomous vehicles based on sequential bayesian decision networks and dynamic inter-vehicular assessment," in *IEEE 2019 IEEE Intelligent Vehicles Symposium (IV'19)*, Paris-France, 9-12 June 2019.

[32] L. Li, B. Zhou, J. Lian, X. Wang, and Y. Zhou, "Multi-pptp: Multiple probabilistic pedestrian trajectory prediction in the complex junction scene," *IEEE Transactions on Intelligent Transportation Systems*, vol. 23, no. 8, pp. 13758–13768, 2021.

[33] Q. Li, R. Li, K. Ji, and W. Dai, "Kalman filter and its application," in *2015 8th International Conference on Intelligent Networks and Intelligent Systems (ICINIS)*. IEEE, 2015, pp. 74–77.

[34] 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*, vol. 11, pp. 1–36, 2019.

[35] 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. 20113–20123.