Driving Environment Uncertainty-Aware Motion Planning for Autonomous Vehicles

Abstract

Autonomous vehicles require safe motion planning in uncertain environments, which are largely caused by surrounding vehicles. In this paper, a driving environment uncertainty-aware motion planning framework is proposed to lower the risk of position uncertainty of surrounding vehicles with considering the risk of rollover. First, a 4-degree of freedom vehicle dynamics model, and a rollover risk index are introduced. Besides, the uncertainty of surrounding vehicles' position is processed and propagated based on the Extended Kalman Filter method. Then, the uncertainty potential field is established to handle the position uncertainty of autonomous vehicles. In addition, the model predictive controller is designed as the motion planning framework which accounts for the rollover risk, the position uncertainty of the surrounding vehicles, and vehicle dynamic constraints of autonomous vehicles. Furthermore, two edge cases, the cut-in scenario, and merging scenario are designed. Finally, the safety, effectiveness, and real-time performance of the proposed motion planning framework are demonstrated by employing a hardware-in-the-loop experiment bench.

1 Introduction

1.1 Motivation

Low-carbonization, electrification, intellectualization, and informatization are the future trend of the automotive industry [1–5]. Especially, autonomous driving is considered one of the revolutionary technologies shaping humanity’s future mobility and quality of life. Although the technology of autonomous vehicles (AVs) has achieved tremendous development in recent years, the safety of the intended functionality (SOTIF) issues remains one of the key challenges hindering its commercialization. SOTIF can be defined as the absence of unreasonable risk due to hazards resulting from functional insufficiencies of the intended functionality or reasonably foreseeable misuse [6, 7]. Generally, an autonomous system consists of an environment perception system, decision-making (DM) system, and a control execution system. Each of these sub-systems confronts SOTIF challenges. As for the DM system, its SOTIF issues can be assumed as caused by the unknown, uncertain driving environment and limitations of the algorithm itself [8]. In this paper, motion planning with driving environment uncertainty information is the focus. In Figure 1, the dynamic driving environment with uncertainties of the surrounding vehicles (SVs), blue vehicle, particularly its future position, is shown. It can be assumed that future behavior and trajectories of the surrounding participants are probabilistic. As such, it is difficult to predict them always with adequate precision. The shaded part in Figure 1 represents positions where the vehicle may appear. If the ego vehicle, the red vehicle, does not consider the uncertainty, it may adopt strategy 1 and suffer the risk of collision with the SV. However, if uncertainty is considered, the ego vehicle can adopt a safe strategy 2. Moreover, the perception system can also produce many physical state uncertainties due to sensor noise and algorithmic limitations. Therefore, a more reliable DM algorithm should be developed for the ego vehicle to cope with the accumulated uncertainties. Lastly, the algorithm must present a safe and anti-conservative plan.

Figure 1

How to generate a safe path to avoid a potential collision that can consider the position uncertainty of surrounding vehicles

1.2 Related Work

Motion planning plays a key role in guaranteeing driving safety. In recent years, many investigations have been conducted on motion planning issues for AVs [9, 10]. Those motion planning algorithms can be roughly categorized into three classes: graph-based (e.g., A∗ algorithm [11], rapidly random tree algorithm [12]), optimization-based (e.g., model predictive control [13–15]), and learning-based (e.g., deep reinforcement learning algorithm [16]) methods. Though most approaches can handle multiple scenarios, they are generally based on the assumption that traffic information can be accessed and predicted accurately. Nonetheless, this type of assumption may cause catastrophic consequences.

The partially observable Markov decision process (POMDP) is an effective way to deal with uncertainty [17]. Urban road situation model for proper environment representation was proposed in Ref. [18]. The Situation-aware DM problem was modeled as a POMDP and solved via online methods. Furthermore, a POMDP approach for a pedestrian collision avoidance system was presented for complex urban scenarios to handle situations with sensor occlusions [19]. Although POMDP planning is effective in processing uncertainties, most solvers are computationally expensive and have poor scalability, especially when the POMDP model changes during runtime and the problem dimension increases. In addition, since the accuracy of localization systems can be relatively low, the uncertainty of localization was taken into consideration by utilizing probabilistic occupancy grid-based approach to ensure the safety of AVs [20]. Similarly, the uncertainty of the predicted trajectory was processed based on gaussian propagation. Meanwhile, the uncertainty of localization, which is caused by AVs and other traffic participants [21], was also calculated based on a Linear-Quadratic Gaussian framework. Learning-based motion planning methods like deep reinforcement learning have been widely employed in recent years. However, current methods are developed with a strong reliance on black box predictions from deep neural networks. These methods tend to be overconfident in predictions of unknown scenarios, which may lead to dire consequences [22]. To avoid this problem, Monte Carlo dropout and bootstrapping were used to provide computationally tractable and parallelizable uncertainty estimates. This method was embedded in a model-based reinforcement learning framework to form uncertainty-aware navigation around pedestrians. However, the action space of this method is discrete, which is not suitable for AVs [23].

On the other hand, many researchers have designed rollover index (e.g., Load Transfer Ratio) to prevent rollover accidents. These indexes are used to describe and determine whether the vehicle has reached the critical rollover point [24]. Lane-keeping control of autonomous ground vehicles considering rollover prevention was studied in Ref. [25]. In this study, an enhanced state observer-based sliding mode control strategy was proposed to maintain lane-keeping errors and the roll angle within the prescribed performance boundaries. In addition, a nonlinear model predictive control approach, which combines braking and steering systems for AVs, was presented in Ref. [26]. A predictive control problem was formulated to track a given path at maximum velocity while satisfying roll angle, yaw rate, and physical constraints. Instead of improving vehicle stability and mitigating risk only from the tracking control perspective, a new local path planning approach was proposed. This method incorporated a time-to-rollover model for off-road autonomous driving on different road profiles with a predefined global route [27]. In addition, a new emergency steering control strategy based on hierarchical architecture was developed in Ref. [28]. It consists of a DM module and a motion control module which ensure the stability of the vehicle. Most of these methods generate a series of candidate trajectories. Then, the optimal one is selected according to a specific cost function rather than directly including the risk of the rollover to the motion planning algorithm.

1.3 Contribution

To the best of the authors’ knowledge, few studies considered both the rollover prevention of AVs and the position uncertainty of SVs from the motion planning perspective. Three contributions clearly distinguish this research from the aforementioned studies:

1) An index that is used to evaluate the risk of the rollover is presented based on a four-degree of freedom (4-DOF) vehicle dynamic model. Furthermore, the index is considered in the motion planning module to lower the risk of rollover.

2) Uncertainty propagation method is proposed to determine the probabilistic safety boundary (PSB) of SVs, and a potential field is designed to process position uncertainties.

3) A motion planning framework based on model predictive control (MPC), which considers the risk of rollover, position uncertainty, obstacle avoidance, and vehicle dynamics constraints, is presented. Moreover, its real-time performance is verified by employing the hardware-in-the-loop (HiL) platform.

1.4 Paper Structure

The rest of this paper is organized as follows. In Section 2, an index describing whether the vehicle has reached the critical rollover point is designed based on the 4-DOF vehicle dynamics model. In addition, an improved uncertainty propagation method is introduced and the PSB of SVs is determined. In Section 3, a potential field is proposed to consider the position uncertainty of SVs. Furthermore, rollover risk and obstacle avoidance which considers position uncertainty and vehicle dynamics are included based on the MPC algorithm. In Section 4, two challenging scenarios are designed, and the proposed method is deployed on the HiL platform to verify the feasibility and real-time performance of the proposed motion planning method. Finally, key conclusions and future work are summarized in Section 5.

2 Vehicle Dynamics Modeling and Position Uncertainty Propagation

In this section, a 4-DOF vehicle model of AVs is illustrated and the risk index of rollover (RIR) is deduced to determine whether the vehicle has reached the critical rollover point. Furthermore, a general model describing motion dynamics of SVs, which considers the uncertainty of SVs’ position and the noise of AVs’ perception system, is introduced. Moreover, the uncertainty propagation method is also proposed based on the extended Kalman filter (EKF). Finally, the PSB of SVs is calculated with 99% statistical confidence.

2.1 Vehicle Dynamics Modeling and Risk Index of Rollover

In this paper, the vehicle dynamics model is utilized for motion planning with consideration of rollover stability. Given that realistic vehicle dynamics are relatively complex, a tradeoff between model accuracy and computational cost is introduced. Therefore, the 4-DOF dynamics model (e.g., 2-DOF bicycle model, 1-DOF longitudinal model, and 1-DOF rollover model) is deduced with several assumptions [29]. In Figure 2a and b, the vehicle model with longitudinal, lateral, yaw and rollover dynamics is shown. The aforementioned dynamics are described by the following equations:

Figure 2

The 4-DOF vehicle dynamics model

$$\begin{array}{l}m\left(\dot{u}-v\dot{\varphi }\right)={F}_{xf}\mathrm{cos}{\delta }_{f}+{F}_{xr}-{F}_{r}={F}_{xT}-{F}_{r},\\ m\left(\dot{v}+u\dot{\varphi }\right)={m}_{s}h\ddot{\phi }+{C}_{f}{\alpha }_{f}+{C}_{r}{\alpha }_{r},\\ {I}_{z}\ddot{\varphi }={l}_{f}{C}_{f}{\alpha }_{f}-{l}_{r}{C}_{r}{\alpha }_{r},\\ {I}_{x}\ddot{\phi }={m}_{s}gh\mathrm{sin}\phi +{m}_{s}{a}_{y}h\mathrm{cos}\phi -{K}_{r}\phi -{B}_{r}\dot{\phi }.\end{array}$$

(1)

The vehicle’s position concerning global coordinates is expressed as:

$$\begin{array}{c}\dot{X}=u\mathrm{cos}\varphi -v\mathrm{sin}\varphi, \\ \dot{Y}=u\mathrm{sin}\varphi +v\mathrm{cos}\varphi, \end{array}$$

(2)

where ${\alpha }_{f}={\delta }_{f}-\frac{v+{l}_{f}\dot{\varphi }}{\dot{x}}$, ${\alpha }_{r}=\frac{{l}_{r}\dot{\varphi }-v}{\dot{x}}$ and ${a}_{y}=\frac{{C}_{f}}{m}({\delta }_{f}-\frac{\dot{y}+{l}_{f}\dot{\varphi }}{\dot{x}})+\frac{{C}_{r}}{m}\frac{{l}_{r}\dot{\varphi }-\dot{y}}{\dot{x}}$, $m$ denotes the total mass of the vehicle, ${m}_{s}$ represents the equivalent sprung mass, ${I}_{z}$ and ${I}_{x}$ depict the yaw inertia of the vehicle and the roll inertia of the spring-mass, respectively, ${l}_{f}$ and ${l}_{r}$ are longitudinal distances from the center of the mass to the front axle and the rear axle, respectively, ${C}_{f}$ and ${C}_{r}$ indicate front and rear tire cornering stiffness, ${\alpha }_{f}$ and ${\alpha }_{r}$ are wheel slip angles of the front and rear tire, respectively, $h$ is the height of the center of mass and $g$ is the gravitational acceleration, ${K}_{r}$ is the roll stiffness and ${B}_{r}$ is the roll damping, ${F}_{xf}$ and ${F}_{xr}$ is the longitudinal tire force of the front wheel and rear wheel, respectively, ${F}_{xT}$ means the generalized longitudinal tire force, ${F}_{r}$ denotes the tire resistance and wind drag, ${\delta }_{f}$ is the steering angle, $u$ is the longitudinal velocity and $v$ is the lateral velocity, $\varphi$ means the heading angle, $\phi$ depicts roll angle of the sprung mass, $X$ and $Y$ are the longitudinal displacement and lateral displacement, respectively.

Based on the vehicle dynamics model described in Eq. (1), $RIR$ that reflects the degree of stability of the vehicle can be obtained using the load transfer ratio:

$$RIR=\frac{{F}_{zr}-{F}_{zl}}{{F}_{zr}+{F}_{zl}}=\frac{2({m}_{s}gh\mathrm{sin}\phi +{m}_{s}{a}_{y}(h\mathrm{cos}\phi +{h}_{r})-{K}_{r}\phi -{B}_{r}\dot{\phi })}{mgL},$$

(3)

where $L$ is the wheelbase and ${h}_{r}$ is the height of the roll center. When $RIR$ is equal to zero, the vehicle exhibits a stable roll dynamic. On the contrary, when this indicator approaches $\pm 1$, the risk of rollover increases as well.

2.2 Uncertainty Propagation of Surrounding Vehicles

Accurate evaluation of current and future positions of SVs has a significant impact on the safety of the AV. Current positions of SVs can be measured through the perception system, like the radar. However, the measurement noise still exists and cannot be avoided. On the other hand, there are many effective methods to predict the future position, intentions, and behavior of AVs [30]. Nonetheless, these methods cannot ensure that the prediction results are always perfect and accurate. In realistic cases, the uncertainty of the predicted information is still one of the key factors resulting in SOTIF issues of DM. The uncertainty may cause the motion planner to compute dangerous trajectories and lead to serious consequences in some extreme cases [31]. Thus, it is necessary to develop a safe motion planning method and process these uncertainties. It should be noted that this paper does not focus on extracting uncertain information from the trajectory prediction algorithms. It mainly focuses on taking the uncertain information of SVs into account. More specifically, the goal of this paper is to propose a general motion planning framework and mitigate the risk resulting from the uncertainty of SVs’ future position and the noise of AVs' perception systems.

In this section, a motion dynamics model, which includes the uncertainty of SVs’ position and the noise of AVs’ perception system, is established. Here, the stochastic driver model of SVs is employed. The uncertainty of driver input prediction is modeled using a Gaussian probability distribution. Thus, the stochastic driver model of SVs can be modeled as follows:

$${{s}_{t}=f\left({s}_{t-1},{c}_{t}\right), \quad c}_{t}\sim N\left({\widehat{c}}_{t},{\mathcal{R}}_{t}\right),$$

(4)

where ${s}_{t}={[{x}_{s},{y}_{s},{\theta }_{s},{v}_{s}]}^\text{T}$ is state variables including global coordinates, heading angle, and velocity of SVs at time $t$. Parameter ${c}_{t}$ is the control input, $N$ indicates Gaussian distribution, ${\widehat{c}}_{t}$ is the mean value, ${\mathcal{R}}_{t}$ is the covariance, and $f$ denotes vehicle dynamics of SVs, which are presented in Ref. [21].

Furthermore, due to the measurement noise of AVs’ perception system, the position of SVs is obtained with uncertainty, which can also be modeled as Gaussian distribution according to Ref. [32]. Therefore, an observer considering measurement noise can be described as:

$$\begin{array}{cc}{z}_{t}=h({s}_{t} )+{q}_{t},& {q}_{t}\sim N(0,{\mathcal{Q}}_{t}),\end{array}$$

(5)

where ${z}_{t}$ is the measurement at time $t$, $h$ indicates the observer system, ${q}_{t}$ is the measurement noise of the perception system, and ${\mathcal{Q}}_{t}$ is the covariance of the measurement noise.

So far, two key factors that cause the uncertainty of SVs’ position have been included. A general model that describes the uncertainty of SVs is established as follows:

$$\begin{array}{l}{s}_{t}=f\left({s}_{t-1},{c}_{t}\right),\\ {z}_{t}=h({s}_{t} )+{q}_{t}.\end{array}$$

(6)

Then, an improved uncertainty propagation algorithm inspired by Ref. [21] using EKF is introduced. Priori state estimate ${s}_{t}$ and the priori uncertainty covariance matrix ${P}_{t}$ can be computed as follows:

$$\begin{array}{l}{s}_{t}=f({s}_{t-1},{c}_{t}),\\ {P}_{t}=\left(\frac{\partial f}{\partial s}\right){\overline{P} }_{t-1}{\left(\frac{\partial f}{\partial s}\right)}^\text{T},\end{array}$$

(7)

where ${\overline{P} }_{t-1}$ represents the posterior uncertainty covariance matrix at time $t-1$, and $\frac{\partial f}{\partial s}$ is the Jacobian operation.

Combined with the measurement information of the perception system, posterior state estimation ${\overline{s} }_{t}$ and posterior uncertainty covariance matrix ${\overline{P} }_{t}$ can be obtained through the following equations:

$$\begin{array}{l}{\overline{s} }_{t}={s}_{t}+{K}_{t}({z}_{t}-h({s}_{t})),\\ {\overline{P} }_{t}=(I-{K}_{t}\frac{\partial h}{\partial s}){P}_{t},\\ {K}_{t}={P}_{t}{\left(\frac{\partial h}{\partial s}\right)}^{\rm{T}}{\left(\left(\frac{\partial h}{\partial s}\right){P}_{t}{\left(\frac{\partial h}{\partial s}\right)}^{\rm{T}}+{\mathcal{Q}}_{t}\right)}^{-1},\end{array}$$

(8)

where ${K}_{t}$ is the Kalman gain and $\frac{\partial h}{\partial s}$ is the Jacobian operation. Based on Eqs. (7) and (8), uncertainty information of SVs can be propagated concerning time.

2.3 Probabilistic Safety Boundary of Surrounding Vehicles Position

According to the above-conducted analyses, uncertainty propagation of SVs is obtained. However, the uncertainty of SVs’ position should be quantified to ensure its adequate processing in the motion planning framework. In this section, to describe SVs’ position with statistical confidence, PSB of SVs’ position is calculated based on Eqs. (7) and (8).

First, let ${[{x}_{s},{y}_{s}]}^\text{T}$ represent the possible position of SVs. Then, optimal estimation coordinate ${[{\overline{x} }_{s},{\overline{y} }_{s}]}^\text{T}$ can be calculated based on Eqs. (7) and (8). Commonly employed motion planning approaches only consider optimal estimation coordinates in motion planning algorithms, which is one of the important differences between those methods and the proposed method in this paper.

Here, the possible position of surrounding vehicles is calculated with $99\mathrm{\%}$ confidence (${\chi }^{2}\text{ = 9.210}$ [33]) as follows:

$$\left[\begin{array}{l}{x}_{s}\\ {y}_{s}\end{array}\right]=\left[\begin{array}{c}{\overline{x} }_{s}\\ {\overline{y} }_{s}\end{array}\right]+\left[\begin{array}{cc}\mathrm{cos}\vartheta & \mathrm{sin}\vartheta \\ -\mathrm{sin}\vartheta & \mathrm{cos}\vartheta \end{array}\right]\left[\begin{array}{c}\sqrt{{\chi }^{2}{\lambda }_{1}}\mathrm{cos}k\\ \sqrt{{\chi }^{2}{\lambda }_{2}}\mathrm{sin}k\end{array}\right],\forall k\in \left[\mathrm{0,2}\pi \right],$$

(9)

where ${\lambda }_{1}=MaxEigval({\overline{P} }_{t})$,${\lambda }_{2}=MinEigval({\overline{P} }_{t})$,$\vartheta ={\mathrm{tan}}^{-1}(MaxEigvec(y)/MaxEigvec(x)),$ ${\overline{P} }_{t}$ is posterior uncertainty covariance matrix illustrated in Eq. (8), $MaxEigval$ and $MinEigval$ indicate maximum eigenvalue and minimal eigenvalue of the covariance matrix ${\overline{P} }_{t}$, respectively. $MaxEigvec(y)$ and $MaxEigvec(x)$ are eigenvectors of the covariance matrix ${\overline{P} }_{t}$ that corresponds to the largest eigenvalue in $Y$ and $X$ coordinate direction, respectively.

3 Uncertainty-Aware Motion Planning Framework Design

This section interprets a safe motion planning framework that considers RIR and uncertainty of SVs’ position. A potential field based on the value of position uncertainty of SVs is built. In addition, the road boundary is also processed. Then, multi-constraints of vehicle dynamics are described. Finally, MPC based motion planning algorithm is designed to consider these factors. The overall structure of the proposed motion planning framework is shown in Figure 3. In this paper, the AV controlled by the MPC method is also referred to as the ego vehicle.

Figure 3

The overall structure of an environment uncertainty-aware motion planning framework for autonomous vehicles

3.1 Potential Field for Surrounding Vehicles with Position Uncertainties Estimation

Vehicles and pedestrians are important for AVs to make decisions, which should be modeled as non-crossable obstacles. Collision with non-crossable obstacles could lead to damage or instability of the vehicle. Even worse, it might threaten human lives. In this paper, the focus is placed on non-crossable vehicles with considering the uncertainties of SVs' position. The non-crossable potential field is motivated by Ref. [34], and we include the position uncertainty in it. Based on Eq. (9), the non-crossable potential field for SVs with position uncertainty is defined as $NPF$:

$${NPF}_{i}\left(X,Y\right)={a}_{i}{e}^{{\beta }_{i}},$$

(10)

where

$${\beta }_{i}=-{\left\{\frac{{((X-{\overline{x} }_{s})\mathrm{cos}\vartheta +(Y-{\overline{y} }_{s})\mathrm{sin}\vartheta )}^{2}}{2{\left({L}_{xi}+\sqrt{{\chi }^{2}{\lambda }_{1}}\right)}^{2}}+\frac{{(-(X-{\overline{x} }_{s}\mathrm{sin}\vartheta +(Y-{\overline{y} }_{s})\mathrm{cos}\vartheta )}^{2}}{2{\left({L}_{yi}+\sqrt{{\chi }^{2}{\lambda }_{2}}\right)}^{2}}\right\}}^{{b}_{i}},$$

where ${a}_{i}$ and ${b}_{i}$ are intensity and shape factors of the potential field, respectively, $i$ denotes the ID of SVs, $e$ is the exponential function, $\vartheta$ means the heading angle of the SVs and ${L}_{xi}$ and ${L}_{yi}$ are factors related to vehicles’ shape, respectively. Other symbols are described in Section 2.

On the other hand, according to traffic rules, the ego vehicle should follow the road marker without violating road boundaries. Furthermore, the ego vehicle cannot depart from the road marker unless the lane change or overtaking command is executed. This issue can be solved by applying the method proposed by the authors' previous work [14]. Here, the quadratic function is applied to design the road boundary potential field ($RPF$):

$$RP{F}_{k}(X,Y)=\left\{\begin{array}{ll}{a}_{q}({S}_{Rq}(X,Y)-{D}_{a}{)}^{2}, &{S}_{Rq}(X,Y)<{D}_{a},\\ 0, &{ S}_{Rq}\left(X,Y\right)>{D}_{a},\end{array}\right.$$

(11)

where ${a}_{q}$ is the parameter of road boundaries $RPF$, ${S}_{Rq}$ indicates the distance from the ego vehicle to road boundaries, and ${D}_{a}$ is the safety threshold between the ego vehicle and road boundaries. Based on Eq. (11), $RPF$ will push the ego vehicle towards the center if it departs from the center of the lane.

Therefore, overall $APF$ includes non-crossable obstacles and road boundaries and it is denoted as:

$$APF=\sum_{i}NP{F}_{i}+\sum_{k}RP{F}_{k}.$$

(12)

It can be observed that $APF$ is nonlinear and non-convex, which increases the required time for solving the optimization problem. However, its approximated quadratic convex problem can be solved significantly faster. Consequently, the problem ought to be converted into a quadratic convex problem to reduce the calculated time. Usually, this type of problem can be solved with the Sequential Quadratic Programming (SQP) method. The main idea is to approximate a primal nonlinear and non-convex problem to a series of convex subproblems. A detailed proof of this process can be found in Ref. [13].

3.2 Motion Planning Framework Design

In this section, the model predictive control method is applied for the motion planning framework design. The presented RIR and $APF$ are included in the MPC controller objective to achieve road regulation and obstacle avoidance while simultaneously guaranteeing stability. First, the nonlinear system described by Eq. (1) is denoted as:

$$\dot{{\varvec{x}}}=g\left({\varvec{x}},{\varvec{u}}\right),$$

(13)

where $g$ represents vehicle dynamics, ${\varvec{x}}$ represents state variables of the system, and ${\varvec{u}}$ is the control input.

By linearizing the system at the operating point, the linear vehicle model can be expressed as:

$$\dot{{\varvec{x}}}={\varvec{A}}{\varvec{x}}+{\varvec{B}}{\varvec{u}},$$

(14)

where ${\varvec{x}}={\left[\begin{array}{cccccccc}u& v& \dot{\varphi }& \varphi & \dot{\varnothing }& \varnothing & X& Y\end{array}\right]}^\text{T}$, ${\varvec{u}}={\left[\begin{array}{cc}{\delta }_{f}& {F}_{xT}\end{array}\right]}^\text{T}$,

$${\varvec{A}}=\left[\begin{array}{cccccccc}0& \dot{\varphi }& v& 0& 0& 0& 0& 0\\ {a}_{1}& {a}_{2}& {a}_{3}& 0& 0& 0& 0& 0\\ {b}_{1}& {b}_{2}& {b}_{3}& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0\\ {c}_{1}& {c}_{2}& {c}_{3}& 0& -\frac{{B}_{r}}{{I}_{x}}& {c}_{4}& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ \text{cos}\varphi & -\text{sin}\varphi & 0& {d}_{1}& 0& 0& 0& 0\\ \text{sin}\varphi & \text{cos}\varphi & 0& {e}_{1}& 0& 0& 0& 0\end{array}\right],$$

$${\varvec{B}}={\left[\begin{array}{cccccccc}0& \frac{{C}_{f}}{m}& \frac{{(C}_{f}+{C}_{r})}{{I}_{z}}& 0& \frac{({C}_{f}h{m}_{s})}{{I}_{x}m}& 0& 0& 0\\ \frac{1}{m}& 0& 0& 0& 0& 0& 0& 0\end{array}\right]}^\text{T},$$

${a}_{1}=\frac{({C}_{f}+{C}_{r})}{(m{u}^{2})}v-\dot{\varphi }+\frac{\left({C}_{f}{l}_{f}-{C}_{r}{l}_{r}\right)}{\left(m{u}^{2}\right)}\dot{\varphi }, \quad$ ${a}_{2}=\frac{({C}_{f}+{C}_{r})}{(mu)}, \quad$ ${a}_{3}=u-\frac{\left({C}_{f}{l}_{f}-{C}_{r}{l}_{r}\right)}{\left(m{u}^{2}\right)}.$

${b}_{1}=\frac{({C}_{f}{l}_{f}-{C}_{r}{l}_{r}) }{{I}_{z}{u}^{2}}v+\frac{\left({C}_{f}{l}_{f}^{2}+{C}_{r}{l}_{r}^{2}\right) }{{I}_{z}{u}^{2}}\dot{\varphi }, \quad$ ${b}_{2}=-\frac{\left({C}_{f}{l}_{f}-{C}_{r}{l}_{r}\right)}{{I}_{z}u},{b}_{3}=-\frac{\left({C}_{f}{l}_{f}^{2}+{C}_{r}{l}_{r}^{2}\right) }{{I}_{z}u}$

${c}_{1}=\frac{h{m}_{s}({C}_{f}{v}_{y}+{C}_{r}{v}_{y}+{C}_{f}{l}_{f}\dot{\varphi }-{C}_{r}{l}_{r}\dot{\varphi }) }{{I}_{z}m{u}^{2}},$ ${c}_{2}=-\frac{h{m}_{s}\left({C}_{f}+{C}_{r}\right)}{{I}_{z}mu},$ ${c}_{3}=-\frac{h{m}_{s}\left({C}_{f}{l}_{f}-{C}_{r}{l}_{r}\right)}{{I}_{z}mu},$ ${c}_{4}=-\frac{\left({K}_{r}-gh{m}_{s}\right)}{{I}_{x}}.$

${d}_{1}=-v\text{cos}\varphi -u\text{sin}\varphi ,$ ${e}_{1}=-u\text{cos}\varphi -v\text{sin}\varphi ,$

The outputs of the system represented by Eq. (15) are defined as lateral position $Y$ and longitudinal velocity $u$, which are described as follows:

$${\varvec{y}}={\varvec{C}}{\varvec{x}},$$

(15)

where ${\varvec{y}}={\left[\begin{array}{cc}Y& u\end{array}\right]}^\text{T}$, $\varvec{C}=\left[\begin{array}{cccccccc}0& 0& 0& 0& 0& 0& 0& 1\\ 1& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$.

The desired outputs are denoted as:

$${{\varvec{y}}}_{{\varvec{d}}{\varvec{e}}{\varvec{s}}}={\left[\begin{array}{cc}{Y}_{des}& {u}_{des}\end{array}\right]}^\text{T}.$$

(16)

Next, the zero-order hold method is applied to discrete the system represented by Eqs. (14) and (15), which can be rewritten as:

$$\begin{array}{l}{\varvec{x}}(k+1)={{\varvec{A}}}_{d}{\varvec{x}}(k)+{{\varvec{B}}}_{d}{\varvec{u}}(k),\\ {\varvec{y}}(k)={{\varvec{C}}}_{d}{\varvec{x}}(k),\end{array}$$

(17)

where ${{\varvec{A}}}_{d}$, ${{\varvec{B}}}_{d}$ and ${{\varvec{C}}}_{d}$ means the discrete matrix of ${\varvec{A}}$, ${\varvec{B}},$ and ${\varvec{C}}$, respectively, $k$ represents the sample time.

Moreover, vehicle dynamic constraints should also be taken into account in the motion planning model to ensure that optimized trajectory can be executed by AVs. Specifically, the values of control variables should satisfy physical constraints of the actuator capacities:

$$\begin{array}{c}\left|{\delta }_{f}\right|\le {\delta }_{max},\\ {F}_{xT}\le \frac{{T}_{max}}{{R}_{w}},\end{array}$$

(18)

where ${\delta }_{max}$ is the maximum steering angle, ${T}_{max}$ is the maximum propelling torque, and ${R}_{w}$ is the radius of wheels.

Dynamic performance of the vehicle is not only restricted by the maximum propelling torque, but also by attachment conditions between tires and roads. Here, longitudinal and lateral accelerations should satisfy the following conditions:

$$\sqrt{{a}_{x}^{2}+{a}_{y}^{2}},$$

(19)

where ${a}_{x}$ and ${a}_{y}$ represent longitudinal acceleration and lateral acceleration, respectively, and $\mu$ is the friction coefficient between the tires and the road.

Furthermore, AVs should also obey the traffic rules. Here, the velocity of the vehicle is considered constrained within the allowable range. This constraint is represented as:

$${u}_{min}\le u\le {u}_{max},$$

(20)

where ${u}_{min}$ and ${u}_{max}$ indicate minimal and maximum allowed velocity, respectively.

Finally, the risk of rollover, potential field, as well as dynamic constraints are filled into the MPC framework:

$$\begin{array}{l}\underset{\Delta u,\varepsilon }{\mathrm{min}}\sum_{k=1}^{{N}_{p}}{(\Vert {\varvec{R}}{\varvec{I}}{\varvec{R}}\left(t+k,t\right)\Vert }_{{\varvec{Q}}}^{2}+{\Vert {\varvec{y}}\left(t+k,t\right)-{{\varvec{y}}}_{{\varvec{d}}{\varvec{e}}{\varvec{s}}}\left(t+k,t\right)\Vert }_{{\varvec{P}}}^{2}\\ +{\varvec{P}}{\varvec{F}}\left(t+k,t\right)+{\Vert {\varepsilon }_{k}\Vert }_{{\varvec{R}}}^{2})+\sum_{k=1}^{Nc}{\Vert \Delta {\varvec{u}}\left(t+k,t\right)\Vert }_{{\varvec{S}}}^{2},\\ \text{s.t.},\,{\varvec{x}}(t+k)={{\varvec{A}}}_{d}{\varvec{x}}(t+k-1)+{{\varvec{B}}}_{d}{\varvec{u}}(t+k-1),\\ {\varvec{y}}(t+k-1)={{\varvec{C}}}_{d}{\varvec{x}}(t+k-1),\\ {{\varvec{u}}}_{min}(t+k-1)\le {\varvec{u}}(t+k-1)\le {{\varvec{u}}}_{max}(t+k-1),\\ \Delta {{\varvec{u}}}_{min}(t+k-1)\le \Delta {\varvec{u}}(t+k-1)\le \Delta {{\varvec{u}}}_{max}(t+k-1),\\ {\varvec{u}}\left(t\text{+}k\right)={\varvec{u}}\left(t+k-1\right),\,\forall k\ge {N}_{c},\end{array}$$

(21)

where ${N}_{p}$ is the prediction horizon, ${N}_{c}$ is the control horizon, $t+k$ indicates the time step, ${\varvec{x}}(t+k)$ represents the predicted state values of the system, ${\varvec{y}}(t+k)$ denotes predicted outputs of the system over prediction horizon, ${{\varvec{u}}}_{min}$ and ${{\varvec{u}}}_{max}$ are the lower and upper bounds of the actuator, and $\boldsymbol{\Delta }{{\varvec{u}}}_{min}$ and $\boldsymbol{\Delta }{{\varvec{u}}}_{max}$ are various ranges of control variables at each moment.

Within the cost function shown in Eq. (21), matrices ${\varvec{Q}}$, ${\varvec{P}}$, ${\varvec{R}}$, and ${\varvec{S}}$ are weight matrices corresponding to the risk of rollover, tracking of the desired path, slack variables, and violation of the control input, respectively. So far, the motion planning problem has been converted into solving the optimal control issues over the prediction horizon.

4 Experimental Results and Analysis

In this section, the test environment and corresponding experimental procedures are presented. Two cases are designed via PreScan software: cut-in scenario and merging scenario. HiL experiment, whose system is shown in Figure 4, is implemented to validate the real-time performance of the framework. The system consisting of an AUDESSE FlexCase controller and a host computer is set up. Besides, the Simulink Desktop Real-Time environment is configured. The Simulink Desktop Real-Time provides a real-time kernel for executing Simulink models on Windows. The communication between the FlexCase controller and the host computer is enabled via CAN (PEAK device). Motion planning algorithm was deployed in FlexCase controller, and vehicle dynamics model is running in the host computer. Vehicle model parameters used in this experiment are listed in Table 1.

Figure 4

The hardware-in-the-loop experimental bench system

Table 1 Vehicle parameters of autonomous vehicles

Baselines: To demonstrate the feasibility of the proposed uncertainty-aware motion planning method that also considers the risk of rollover (subsequently referred to as UAMP-RR), two baselines are utilized. Firstly, a conventional motion planning (CMP) approach is developed, which does not consider the position uncertainty of SVs and the risk of rollover. Secondly, another baseline that considers the position uncertainty of SVs and does not consider the risk of rollover (called UAMP) is also adopted.

In addition, to quantify the benefits and disadvantages of the proposed decision-making algorithm, the safety-related evaluation indicator should be established. The following is the definition of the longitudinal safe distance:

$${X}_{s}={X}_{0}+{u}_{f}{t}_{0}+\frac{{({u}_{f}-{u}_{l})}^{2}}{2{a}_{max}},$$

(22)

where ${u}_{l}$ and ${u}_{f}$ are the leading and following vehicle’s current velocities, respectively, ${t}_{0}$ is the reaction time, ${a}_{max}$ is the maximum deceleration of the vehicles, and ${X}_{0}$ is the longitudinal standstill distance.

Similarly, the lateral safe distance is defined as follows:

$${Y}_{s}={v}_{f}{t}_{0}+{Y}_{0},$$

(23)

where ${v}_{f}$ is the following vehicle’s lateral velocity, and ${Y}_{0}$ is the lateral standstill distance.

The safety index (SI) is represented as follows.

$$SI=\left\{\begin{array}{ll}\Delta X/{X}_{s}, &\Delta X/{X}_{s}>1\, \&\, \Delta Y/{Y}_{s}<1,\\ {\Delta Y/Y}_{s},& \Delta X/{X}_{s}<1\, \&\, \Delta Y/{Y}_{s}>1,\\ min(\Delta X/{X}_{n},{\Delta Y/Y}_{s}),& \text{else},\end{array}\right.$$

(24)

where $\Delta X$, $\Delta Y$ mean absolute longitudinal and lateral Euclidean distance between the AV and SV, respectively.

It can be found that the lower the value of SI, the higher the risk. Finally, vehicle dynamic parameters are presented in Table 1.

4.1 Case 1: Evaluation of Obstacle Avoidance in the Cut-in Scenario

As aforementioned, the motion information of SVs plays a key role in the motion planning system of AVs. Therefore, a cut-in scenario is built to validate the effectiveness of the proposed method. In Figure 5, a two-lane scene is depicted where the ego vehicle lies in lane #2 (lower lane in Figure 5) with the initial velocity of 25 m/s and position [0 m, 1.75 m]. Four SVs are placed in the proximity of the ego vehicle: SV #1 with the velocity of 20 m/s and the initial position [20 m, 5.25 m] in lane #1 (upper lane in Figure 5), SV#2 with the velocity of 20 m/s, and initial position [60 m, 1.75 m] in lane #2, SV #3 with the velocity of 20 m/s and the initial position [− 40 m, 5.25 m] in lane #1, SV #4 with the velocity of 20 m/s and the initial position [− 30 m, 1.75 m] in lane #2. In this scenario, SV #1 is set to make an abrupt lane change from lane #1 to lane #2. This represents a challenge for the ego vehicle concerning making the correct decision and planning. Because the position of SVs is hard to predict accurately, even when applying some advanced prediction algorithms. In other words, for the ego vehicle, the future position of SVs has greater uncertainty.

Figure 5

Trajectory of ego vehicle and SVs in case 1

HiL experimental results are presented in Figures 5, 6, 7, 8, 9 and 10. In Figure 5, the trajectory of UAMP-RR, UAMP, and CMP algorithms are demonstrated, where only the key positions are marked. Based on the results, it can be concluded that the three methods behave differently. Specifically, UAMP-RR and UAMP choose to make a lane change at about 1 s to avoid collision with SV #1, whereas CMP selects to change lane at around 1.55 s, which is 0.55 s later than UAMP-RR and UAMP. In Figure 6, the comparison of SI values for these three methods is demonstrated. It can be found that the SI values of CMP for SV #1 are less than 1 from 2 s to 2.9 s, which represents a high collision risk. The reason why UAMP-RR and UAMP exhibit safely is that the position uncertainty of SVs is considered. Besides, the SI values of UAMP-RR for other SVs are also given in Figure 7, which means that UAMP-RR can make the ego vehicle keep a safe distance from other SVs. Moreover, the comparison of RIR values and corresponding roll angle are shown in Figures 8 and 9, respectively. From the results, it can be concluded that both UAMP-RR and UAMP can keep the ego vehicle at a low risk of rollover, but the RIR values of CMP are higher. Note that although the risk of rollover is not considered in UAMP, it chooses the safe trajectory approximate to UAMP-RR, and thus, the RIR values are lower. In case 2, the difference between UAMP-RR and UAMP will be displayed and highlighted. Finally, in Figure 10, the values of steering angle and longitudinal force are shown. It can be observed that constraints are well processed, i.e., the steering angel and longitudinal force are limited in reasonable ranges.

Figure 6

SI values for SV #1 in case 1

Figure 7

SI values of UAMP-RR for other SVs in case 1

Figure 8

RIR values of ego vehicle in case 1

Figure 9

Roll angle of ego vehicle in case 1

Figure 10

Steering angle and longitudinal force of ego vehicle with UAMP-RR method in case 1

4.2 Case 2: Evaluation of Obstacle Avoidance in the Merging Scenario

To further verify the effectiveness of the proposed framework, another challenging scenario represented by the merging scene is built. In Figure 11, a two-lane road with a ramp on the highway and ego vehicle in lane #2 (lower straight road in Figure 11) with the velocity 30 m/s and initial position [0 m, 1.75 m] is displayed. In addition, there are also four SVs placed in the proximity of the ego vehicle. More specifically, SV #1 is located in Ramp Road with a velocity of 32 m/s and an initial position [16 m, − 32.4 m], SV #2 is located in lane #1 (upper straight road in Figure 11) with the velocity of 30 m/s and the initial position [25 m, 5.25 m], SV #3 is located in lane #1 with the velocity of 30 m/s and initial position [− 35 m, 5.25 m], SV #4 is located in lane #2 with the velocity of 30 m/s and initial position [− 40 m, 1.75 m]. In this case, SV #1 is set to perform a merging behavior from ramp road to lane #2, which is also a challenging case for the ego vehicle to avoid the collision. Similarly, the experimental results are demonstrated in Figures 11, 12, 13, 14, 15 and 16.

Figure 11

Trajectory of ego vehicle and SVs in case 2

Figure 12

SI values for SV #1 in case 2

Figure 13

SI values of UAMP-RR for other SVs in case 2

Figure 14

RIR values of ego vehicle in case 2

Figure 15

Roll angle of ego vehicle in case 2

Figure 16

Steering angle and longitudinal force of ego vehicle with UAMP-RR method in case 2

Figure 11 depicts the trajectory of the UAMP-RR, UAMP, and CMP algorithms, with only the essential position highlighted. It can be seen that CMP adopts a different control strategy compared with UAMP-RR and UAMP. Specifically, CMP keeps the vehicle within its lane, whereas the others choose to change the lane to avoid collision with SV #1 at 2.4 s. The inherent reason why CMP made such a decision is that it predicts the future trajectory of SV #1 without uncertainty. In other words, the algorithm assumes that the prediction is accurate. Besides, the comparison of SI values for the three approaches is shown in Figure 12. From 3 s to 4 s, the SI values of CMP for SV #1 are less than 1, indicating that there is a high danger of collision. Because the position uncertainty of SVs is taken into account, UAMP-RR and UAMP exhibit securely. Figure 13 also shows the SI values of UAMP-RR for other SVs, implying that UAMP-RR can maintain the ego vehicle at a safe distance from other SVs. In addition, Figures 14 and 15 demonstrate the comparison of RIR values and roll angles, respectively. It can be found that the RIR values and roll angles of CMP are close to 0. The reason is that in this case, CMP adopts an unsafe strategy, i.e., keeping lanes (i.e., between 3 s and 4 s, the value of SI is less than 1 in Figure 12). Therefore, this does not show that the CMP method is better than the other two methods, despite low RIR values. Because, for the vehicle, crash safety is the priority. UAMP-RR and UAMP have in common that they both consider the position uncertainty of SVs, and the difference is that UAMP does not consider the risk of rollover. They both perform safer behaviors, as shown in Figure 12. But UAMP-RR has a lower RIR value, implying that it can increase vehicle rollover stability while guaranteeing collision safety. Finally, results for steering angle and longitudinal force are displayed in Figure 16. The steering angle and longitudinal force are both constrained in appropriate limits, indicating that constraints are effectively handled.

5 Conclusions

An environment uncertainty-aware motion planning approach for AVs with considering the risk of the rollover was proposed in this paper. First, the 4-DOF vehicle dynamic model was used to design the index describing the risk of rollover. Then, an extended Kalman filter was applied to process and propagate the uncertainty of SVs’ position. Furthermore, the PSB of SVs was determined, and a potential field was established to account for the uncertainty. To achieve obstacle avoidance while guaranteeing stability, the MPC was adopted to account for all of the previously mentioned items. Finally, two cases, i.e., the cut-in scenario, and merging scenario are designed on a hardware-in-the-loop experiment bench. The results demonstrate the effectiveness of the proposed method.

Future work may focus on extracting prediction uncertainty from advanced trajectory prediction methods like long short-term memory. In addition, more edge cases in the urban situation will be studied to improve its generalization and the real vehicle test will proceed.