Courteous MPC for Autonomous Driving with CBF-inspired
Risk Assessment

Consider a dynamical system in control-affine form:

where $x\in\mathbb{R}^{d}$ and $u\in\mathbb{R}^{m}$ are the system state vector and control input vector, respectively. The vector fields $f:\mathbb{R}^{d}\mapsto\mathbb{R}^{d}$ and matrix-valued function $g:\mathbb{R}^{d}\mapsto\mathbb{R}^{d\times m}$ are assumed to be locally Lipschitz continuous.

Given the control affine system in Eq. (1) and the safe set $\mathcal{H}$ defined as the zero-superlevel set of a continuously differentiable function $h:\mathbb{R}^{d}\mapsto\mathbb{R}$, Safety Barrier Certificates [11] has been used to enforce the safety set forward invariance, as defined in the following lemma.

This defines the set of controllers enforcing the system state $x$ staying in $\mathcal{H}$ over time if the initial system state $x(t=0)$ is inside the set $\mathcal{H}$.

However, when the system states are updated discretely and so does the controller, such controller implemented under a discrete-time system may not satisfy the safety condition using the continuous constraints defined in Eq. (2) between sample time steps [21]. To this end, the Sampled-data Control Barrier Functions (Sampled-data CBFs) [22] are introduced to enforce the forward invariance of safe set using Controller Margin¹¹1An additional term compare with Eq. (2) added to the right-hand side in Eq. (2) is called the controller margin., which is summarized in the following lemma.

This defines the set of controllers for the system whose states are observed discretely to stay in $\mathcal{H}$ over time, especially during $[t_{k},t_{k+1})$ where $t_{k}=kdt$.

By assuming that the tire slip ratios and slip angles are small, the vehicle’s orientation and position evolve based on its velocity, steering angle, and wheelbase. The kinematics of ego vehicle $\mathcal{V}_{e}$ as an AV can be described by the Kinematic Bicycle Model [23] as follows:

where $(X_{e},Y_{e})$ represents the vehicle’s longitude and lateral positions. $\phi_{e}$ is the vehicle heading angle and $v_{e}$ is the velocity of the vehicle. $\beta_{e}$ is the side slip angle at the center of gravity and $a_{e}$ the acceleration. $\delta_{e}$ is the front wheel steering angle.

When the vehicle travels on a highway, the environment and the vehicle dynamics may evolve rapidly, requiring quick responses from the control system  [24]. Discretizing the kinematic bicycle model allows the computation to be performed efficiently. In this case, the system in Eq. (4) can be discretized as a linear parameter-varying system using Euler’s method. To simplify the discussion, we use the mapping $\tilde{f}$ to represent the system:

where $\mathbf{s}_{e,k}=[X_{e,k},Y_{e,k},v_{e,k},\phi_{e,k}]^{\text{T}}\in\mathbb{R}^{4}$ and $\mathbf{u}_{e,k}=[a_{e,k},\delta_{e,k}]^{\text{T}}\in\mathbb{R}^{2}$ are the state and control input of the vehicle $e$ at time step $k$, respectively. In the following sections, we use $\mathbf{P}_{e,k}$ to represent the position $(X_{e,k},Y_{e,k})$ of the ego vehicle $\mathcal{V}_{e}$ at time step $k$.

We consider the scenario where an AV operates on the road and can observe the neighboring vehicles’ positions and velocities. For the observed vehicle $\mathcal{V}_{i}$ at time step $k$:

where $\mathbf{P}_{i,k},\mathbf{V}_{i,k}$ represent the position and velocity, respectively. $v^{x}_{i,k},v^{y}_{i,k}$ are the velocities of the vehicle $i$ in the $x$ and $y$ direction. Both $\mathbf{P}_{i,k},\mathbf{V}_{i,k}$ are considered as Gaussian random variables with $\mathbf{P}_{i,k}\sim(\hat{\mathbf{P}}_{i,k},\Sigma^{P}_{i})$ and $\mathbf{V}_{i,k}\sim(\hat{\mathbf{V}}_{i,k},\Sigma^{V}_{i})$, where $\hat{\mathbf{P}}_{i,k}\in\mathbb{R}^{2}$ and $\hat{\mathbf{V}}_{i,k}\in\mathbb{R}^{2}$ are the observed location and velocity of the neighboring vehicle, respectively, and $\Sigma^{P}_{i}\in\mathbb{R}^{2\times 2}$ and $\Sigma^{V}_{i}\in\mathbb{R}^{2\times 2}$ are the diagonal covariance matrix.

Motivated by CBF-inspired risk evaluation framework in [17], we seek to quantify the risk when there is both position and velocity uncertainty and exploit the potential of this risk evaluation framework in safe and courteous driving.

The vehicles operating on roads are assumed to be covered by an ellipse [25, 26], hence motivated by [27] the safety set between any pairwise vehicles at time step $k$ can be formulated as follows:

where $\mathbf{P}_{k}$ is the joint position space of the ego vehicle $e$ and its neighboring vehicle $\forall i$ at the time step $k$, $\tau$ is the scaling factor and $\mathcal{D}_{s}$ is the safety distance.

Inspired by Lemma 2, we use the function²²2In this paper, we select the class $\kappa$ function $\alpha(h)=\gamma h$ as the same setting in [28, 17], where $\gamma\in\mathbb{R}\geq 0$. $H_{e,i}(\mathbf{P}_{k},\dot{\mathbf{P}}_{k})$ to represent the severity of the potential pairwise collision, which is defined as:

where $l_{H}=l_{L_{f}h_{e,i}}+l_{L_{g}h_{e,i}}u_{max}+l_{\alpha}(h_{e,i})$ with $\delta_{H}=\sup_{\mathbf{P}_{e,k}\in\mathcal{H}^{P_{k}}_{e,i}}|\dot{\mathbf{P}}_{e,k}|$ and $dt$ is the duration of one time step (the dependence of $h_{e,i}$ is omitted for notation simplicity.).

With the Gaussian distributed observed positions and velocities of the neighboring vehicles, $H_{e,i}(\mathbf{P}_{k},\dot{\mathbf{P}}_{k})$ is distributed as a noncentral chi-square distribution. Using the Delta method [29], we can approximate the distribution of $H_{e,i}(\mathbf{P}_{k},\dot{\mathbf{P}}_{k})$ as a Gaussian distribution where the mean and variance are estimated with the first-order Taylor series expansion, i.e., $H_{e,i}(\mathbf{P}_{k},\dot{\mathbf{P}}_{k})\sim\mathcal{N}(\boldsymbol{\mu}_{H_{e,i}},\Sigma_{H_{e,i}})$ with:

where $\Sigma_{e,i}\in\mathbb{R}^{8\times 8}$ is the covariance of the $[\mathbf{P}_{e,k},\mathbf{P}_{i,k},\dot{\mathbf{P}}_{e,k},\dot{\mathbf{P}}_{i,k}]$.

We employ conditional value-at-risk³³3With a user-defined confidence level $\alpha$, $\mathrm{CVaR}$ is the expected cost in the worst $\alpha$-percentile of the distribution function $H_{e,i}(\mathbf{P}_{k},\dot{\mathbf{P}}_{k})$. (CVaR) [8] to quantify the ego vehicle’s perceived risk from potential pairwise collision and observation uncertainty:

where $\mathrm{V}\mathrm{a}\mathrm{R}_{\alpha}(H_{e,i}):=\text{inf}\{H\in\mathbb{R}|\mathrm{Pr}(H_{e,i}\geq H)\leq\alpha\}$, and $\alpha$ is a user-defined confidence level. $\mathrm{Pr}(\cdot)$ indicates the probability of an event. To simplify the notation, we use $H_{e,i}$ to represent $H_{e,i}(\mathbf{P}_{k},\dot{\mathbf{P}}_{k})$ by omitting the dependence on $\mathbf{P}_{k}$ and $\dot{\mathbf{P}}_{k}$.

Based on the pairwise risk evaluation defined in Eq. (III-A), we further define the resultant perceived risk $R_{e,k}$ of the ego vehicle $\mathcal{V}_{e}$ on the highway at time step $k$ as:

Using the CBF-inspired risk evaluation framework, we can construct a comprehensive risk map that enables deeper insights into how perceived risk is aggregated, enhancing the decision-making processes for the ego vehicle.

We employ a three-lane highway as a running example with two vehicles running on the road where the red vehicle is the ego vehicle $\mathcal{V}_{e}$ and the blue one $\mathcal{V}_{b}$ is the observed neighboring vehicle. To construct the risk map depicted in Fig. 2, we counterfactually place the ego vehicle at various points across the map and evaluate the perceived risk at each point using Eq. (14). This evaluated risk is then attributed to the corresponding point on the map and visualized using a gradient color scheme to explicitly depict risk variations.

Using the risk maps depicted in Fig. 2, we illustrate how risk is aggregated based on various velocity configurations and their influence on the ego-perceived risk map. In all three subfigures, the positions of the ego vehicle $\mathcal{V}_{e}$ and the neighboring vehicle $\mathcal{V}_{b}$ are the same. This consistent positioning allows us to isolate and analyze the effects of velocity changes on the perceived risk without the confounding factor of varying spatial relationships. The state and velocity noise covariance are both set as diag$\{0.1,0.1\}$ and the confidence level is set as $\alpha=0.1$. The dashed blue contour in every subfigure delineates the area where the evaluated risk exceeds $0$. In Fig. 2(a), both $\mathcal{V}_{e}$ and $\mathcal{V}_{b}$ are traveling at a velocity of $(15,0)$, and it is obvious that the closer the ego vehicle is to the neighboring vehicle, the larger the perceived risk. Fig. 2(b) illustrates a shift in risk perception when $\mathcal{V}_{b}$’s velocity changes to $(10,0)$, showing a noticeable bias. This is reasonable given that the ego vehicle’s velocity exceeds that of the leading vehicle.

Furthermore, we adjust $\mathcal{V}_{b}$’s velocity to $(15,1.5)$, which significantly reduces the safe region in the third lane as shown in Fig. 2(c). If $\mathcal{V}_{e}$ intends to overtake $\mathcal{V}_{b}$, although there appears to be sufficient space in Lane three, the risk map suggests that overtaking in Lane one is a preferable strategy. This decision not only offers a safer alternative but also exhibits courteous behavior towards $\mathcal{V}_{b}$.

Based on these observations, it is reasonable to conclude that: 1) the risk map based on the risk evaluation method is applicable and illustrates varying performance under different positions and motions, and 2) it shows the potential for aiding AV’s decision-making for safe and courteous driving.

Inspired by the analysis of the CBF-inspired risk map and Theorem 3, to realize safe and courteous driving performance, we integrate the perceived risk by ego vehicle into the MPC framework.

For a MPC framework, a general task-related cost function $J(\mathbf{s}_{e,k})$ is defined as below:

where $N$ is the prediction horizon, $l_{\text{S}}(\mathbf{s}_{e,k+t|k},\mathbf{u}_{e,k+t|k})$ and $l_{\text{T}}(\mathbf{s}_{e,k+N|k},\mathbf{u}_{e,k+N|k})$ represent the stage cost and terminal cost, respectively. $\mathbf{s}_{e,k+t|k}$ denotes the state vector at time step $t+k$ predicted at time step $k$ obtained by starting from the current state $\mathbf{s}_{e,k}$ by applying the input sequence $\mathbf{u}_{k:k+N-1|k}$. To realize safe and courteous driving, we further integrate the risk perceived by the ego vehicle to the cost function and constraints to synthesize the MPC controller, which is defined as:

where $P_{\text{S}}$ is a user-defined weight parameter and $R_{e,k+t|k}$ is the predicted risk at time step $k$. $\mathbf{s}_{min}$ and $\mathbf{s}_{max}$ represent the lower and upper bounds of the state constraints. $\mathbf{u}_{min}$ and $\mathbf{u}_{max}$ represent the lower and upper bounds of the control inputs. By the constraint of Eq. (16a), we can guarantee safety with at least $1-\alpha$ probability. By integrating the risk evaluation into the cost function, the ego vehicle may make decisions to reduce the perceived risk to yield more space for the neighboring vehicles.

To exploit the influence of the weight parameter $P_{\mathrm{S}}$ on the ego vehicle performance, we conducted a cruise driving task with varying $P_{\mathrm{S}}$ in the highway-env [33] simulation environment, where the behavior of surrounding vehicles is controlled by IDM [19]. The state and velocity noise covariances are both set as diag$\{0.1,0.1\}$ with the confidence level set as $\alpha=0.1$. The target speed of the ego vehicle is $15\;m/s$. As depicted in Fig. 3, with the increase of $P_{\mathrm{S}}$ in Eq. 16, the minimum distance between the ego vehicle and neighboring vehicles also increases. This indicates that the penalty of the risk term allows more space around the ego vehicle, enhancing comfort for surrounding vehicles. Additionally, we can observe that even with a lower $P_{\mathrm{S}}$, a reasonable space is still maintained to ensure safe driving.

To further validate the performance of the proposed Courteous MPC, three methods including Courteous MPC, Risk-aware MPC and Batch MPC [26] are conducted for the cruise control task in the highway-env simulation environment [33]. For clarity, the controller is designated as ”Risk-aware MPC” when the risk term is removed from the cost function. For Courteous MPC, we use the same parameter setting as Section. IV-A and set $P_{\mathrm{S}}=0.25$.

As depicted in Fig. 4, we provide snapshots demonstrating two examples of overtaking behavior for each method. Both Risk-aware MPC and Courteous MPC initiate overtaking maneuvers earlier than Batch MPC, which supports the efficacy of our CBF-inspired risk evaluation framework in preventing collisions proactively. Compared to Risk-aware MPC, Courteous MPC initiates overtaking maneuvers even earlier. This suggests that integrating the risk term into the cost function allows more space around neighboring vehicles, thereby enhancing comfort and safe margins. Additionally, Fig. 5 shows the minimum distance maintained by the ego vehicle from IDM-controlled vehicles over time. This analysis also reveals that Courteous MPC consistently allows greater space around the ego vehicle, enhancing comfort for surrounding vehicles⁴⁴4The simulation details can be found at https://youtu.be/s5aeT6MPhio .

To prove the effectiveness of Courteous MPC, we compare the cruise task performance of the three methods as shown in Table I. Performance metrics include the average speed of the ego vehicle and the total travel distance in the longitudinal direction. The data reveals that Courteous MPC significantly outperforms the other two methods on both metrics. Notably, the relatively lower accuracy of Batch MPC aligns with the findings reported in [34]. These improvements suggest that providing additional space does not detrimentally affect overall task performance. Instead, it likely enables the ego vehicle to seek more open areas, allowing it to achieve higher speeds rather than confining itself within narrow spaces.

To further validate the robustness of the Courteous MPC, we conduct $10$ trials for the three methods and plot the average minimum distances with error bars, as shown in Figure 6. For every trial, the positions of the vehicles are randomly initiated. The results corroborate our earlier findings, indicating that Courteous MPC consistently allows for greater spacing around surrounding vehicles, thereby facilitating more courteous interactions on the road.

To further verify the performance of the Courteous MPC on real-world data set, we conduct the simulation experiment in the NGSIMENV simulation environment [30] with Next Generation Simulation (NGSIM) US101 dataset [20], where a human-driven vehicle is replaced by the intelligent vehicle empowered by the Courteous MPC. As we can observe from Fig. 7, the Courteous MPC can successfully travel through heterogeneous vehicles, which is realized by setting different scaling factors $\tau$ based on the vehicles’ sizes in Eq. (7). Note that in Fig. 7(b), the ego vehicle is running on the dashed white lane due to the neighboring two vehicles are running too close to the lane. The best way for the ego vehicle to show courtesy to both two vehicles is to maintain its position on the dashed white lane ⁵⁵5The simulation details can be found at https://youtu.be/9Cekw5xafWw .