Stochastic Model-Predictive Control with Uncertainty Estimation for Autonomous Driving at Uncontrolled Intersections

Abstract

This paper presents an uncontrolled intersection-passing algorithm with an integrated approach of stochastic model-predictive control and prediction uncertainty estimation for autonomous vehicles. The proposed algorithm is designed to utilize information from sensors mounted on the autonomous vehicle and high-definition intersection maps. The proposed algorithm is composed of two modules, namely target state prediction and a motion planner. The target state prediction module has predicted the future behavior of intersection-approaching vehicles based on human driving data. The recursive covariance estimator has been utilized to estimate the prediction uncertainty for each approaching vehicle. The desired driving mode has been determined based on the uncontrolled intersection theory. The estimated prediction uncertainty has been used to define the probability distribution of the stochastic model-predictive controller to cope with time-varying uncertainty characteristics of the perception algorithm. The constrained stochastic model-predictive controller based on safety indexes has determined the desired longitudinal acceleration. The proposed robust intersection-passing algorithm has been evaluated via computer simulation based on Monte Carlo simulation with a sensor model. The simulation results showed that the proposed algorithm guarantees the minimum safety constraints and improves the ride comfort at uncontrolled intersections by estimating the uncertainty of sensors and prediction.

1. Introduction

Autonomous or self-driving vehicles are attracting attention as a solution for transportation due to their various advantages such as improved safety, increased traffic efficiency, decrease in pollution, and equitable access to mobility. The coverage of autonomous driving has been extended from the motorway to urban roads to spread these advantages. In this process, uncontrolled intersections are one of the most dangerous driving situations because there is no traffic signal to control the traffic participants. In particular, the accident rate and fatality rate at uncontrolled intersections are higher than in other driving environments [1,2]. Several studies tried to figure out the important factors related to the geometric and traffic-related elements that affect the accident rates of intersections [3,4]. Based on the achievements of accident analysis in intersections, situation awareness, decision making, and motion planning become important issues to pass an uncontrolled intersection safely and comfortably. Various algorithms have been proposed for autonomous driving at uncontrolled intersections.

Various approaches have been used for decision making and motion planning for uncontrolled intersections. In many previous studies, decision making at intersections has been designed based on pre-defined policies. The Markov Decision Process (MDP) has been utilized as a policy-based motion planning algorithm. In particular, the partially observable Markov decision process was introduced to overcome the limitations of unmeasurable variables from the sensors and communication [5,6,7,8]. In these studies, the uncertainty of the surrounding vehicles was considered at the maneuver level, which affects the driving policy decisions of the autonomous vehicles. The Bayesian network was also utilized for probabilistic decision making at intersections. A time window for observation was considered to improve the performance of the Bayesian network [9]. The rule-based approach was applied to select the optimal rule from the pre-defined rule sets [10]. The MDP and the deep Q-network algorithm were established to obtain the optimal driving policy based on traffic image analysis using convolution neural networks [11].

In addition to the decision-making problem, there have been studies focused on determining the driving path and acceleration command for the autonomous vehicle. The combined approach of Gaussian processes regression and a rapidly exploring random tree was used to generate a desired local path to guide the autonomous vehicle through the intersection [12]. The robust Model Predictive Control (MPC) was used for optimal velocity planning under the assumption that all target information is available [13]. The loosely coupled low-complexity MPC was formulated for automated yield maneuvers [14]. The linear MPC with target vehicle motion prediction and driving mode decision was employed to achieve human-like behavior at uncontrolled intersections [15]. In addition, the cooperative scheduling method of all intersection-approaching vehicles was proposed under the assumption of a connected autonomous vehicle [16,17,18,19]. In particular, virtual platooning was introduced, in which vehicles traveling on each branch are transferred to the same road [17].

In order to improve the performance of the decision-making and motion-planning algorithm, various studies have been conducted on traffic modeling and predictions on surrounding vehicles at intersections. The prediction can be classified as maneuver-level [5,6,7,8,19,20] and trajectory-level prediction [9,21] based on the output of the predictor. A game theory was used to model the vehicle interactions in unsignalized intersections [20]. The map-based prediction was used to provide the lane-level prediction results [9]. A learning-based approach, such as the support vector machine and the long short-term memory-based recurrent neural network [21,22], was also utilized to predict the future behavior of surrounding vehicles. However, predictions based on the perception results inevitably have uncertainties in perception information. Many studies have assumed that Vehicle to Vehicle (V2V) can provide the true values of target states without uncertainty [10,16,17,18,19,23,24].

According to a careful review of the previous studies, various approaches have been introduced to develop the prediction and motion-planning algorithm for intersections. Many studies focused on the maneuver decision of the autonomous vehicle among the pre-defined policies. In particular, MDP-based approaches were frequently utilized to select the optimal policy. However, the computational burden of MDP is an obstacle to achieving real-time implementation. MPC was also employed to calculate the desired acceleration to pass the intersection. When using MPC, the target states were often provided by V2V communication to overcome the limitations of perception caused by blind spots and sensor uncertainty. A robust motion planner that can manage the perception and prediction uncertainty has not been addressed much in previous studies.

In this paper, the motion planner based on SMPC with prediction uncertainty estimation is proposed and evaluated. The objectives of the proposed algorithm are ensuring safety, robustness to sensor uncertainty, and minimizing control efforts. Prediction uncertainty of the prediction algorithm is estimated using the recursive covariance estimator to improve the Intelligent Driver Model (IDM)-based prediction algorithm. The estimated uncertainty is used to define the probability distribution of the chance constraint for the SMPC problem, which improves the performance under changes in sensor noise. The desired longitudinal acceleration is decided by solving the SMPC problem. The proposed algorithm has been evaluated via computer simulation using MATLAB/Simulink and FORCES solver.

2. Overall Architecture of Proposed Algorithm

The core functions of autonomous driving can be categorized into four categories, namely perception, localization, motion planning, and control, with the interactions between the autonomous vehicle and the environments depicted in Figure 1. The perception module has a role to detect the surrounding objects around the autonomous vehicle. In this study, a geometric model-free approach [25] has been used to process the LiDAR point clouds from the six IBEO Lux sensors. This algorithm provides the local position, heading angle, and velocity of the moving objects with a grid-base static obstacle map. The localization module estimates the current global position and heading angle of the autonomous vehicle with respect to the high-definition map. A map-matching algorithm with a multi-rate Kalman filter has been used based on the measurement of GPS/INS sensors, LiDAR, and an around-view camera. The control module determines the steering-wheel angle, throttle, and brake inputs to track the desired path and acceleration.

The last module, motion planning, is mainly discussed in this article. The architecture of the proposed motion-planning algorithm for an uncontrolled intersection is shown in Figure 2. The motion-planning algorithm consists of two parts with three sub-modules. The first part, target state prediction with uncertainty estimation, is composed of the three sub-modules intention inference, state prediction, and the recursive covariance estimator. These modules have been utilized to predict the future trajectory of the approaching targets and estimate the prediction uncertainty based on the result of driving intention inference and a recursive covariance estimator. The second part, the robust motion planner for intersection passing, consists of the three sub-modules driving mode decision, defining MPC constraint decision, and solving SMPC. The driving mode of the autonomous vehicle has been determined among ‘Cross’, ‘Yield’, and ‘Stop’ modes according to the uncontrolled intersection theory. Depending on the driving mode, predicted states of the target, and estimated prediction uncertainty, the hard and chance constraints for the SMPC problem have been defined to satisfy the safety index thresholds, which are derived from the human driving data. Finally, the desired acceleration is calculated by the SMPC.

Before proceeding further with the discussion in this paper, the safety indexes for the intersection scenario have been defined as shown in Figure 3. From both a safety and performance perspective, the clearance and time to collision (TTC) have been widely used as an important index for the research of advanced driving-assistant systems (ADAS) and autonomous vehicles. However, the driving situation of an intersection is quite different from nominal driving conditions. Therefore, the clearance and TTC have been modified to consider the conflict point between intersection-approaching vehicles as follows:

$$\begin{array}{l}{C}_{conf}=d_{DTC,ego}+d_{DTC,target}\\ TT{C}_{conf}=\frac{d_{DTC,ego}}{v_{ego}}+\frac{d_{DTC,target}}{v_{target}}\end{array}$$

(1)

where ‘conf’ and ‘DTC’ are the abbreviation of ‘conflict point’ and ‘distance to conflict point’. d_DTC,ego and d_DTC,target represent the remaining distance to the conflict point of ego and the target vehicle. v_ego and v_target are the velocity of the ego and target vehicle, respectively. In addition, the proposed safety index considers the trade-off of cross priority between two vehicles encountered at an intersection. The trade-off relationship of each term in TTC_conf applies equally to C_conf. Therefore, C_conf and TTC_conf is the suitable safety index that can be utilized for all driving situations. In addition, the remaining distance to the stop line of the intersection is denoted as d_DTI, where ‘DTI’ means the distance to the intersection. d_DTI is used to depict an evaluation result in the distance domain.

3. Target State Prediction with Uncertainty Estimation

The state prediction of the surrounding objects is generally used in autonomous driving research for motion planners because autonomous vehicles should act like human drivers, who have intuition to be aware of the driving situation. In this research, the objective of state prediction is to provide the information for the driving mode and desired acceleration decision, which are robust to sensor and prediction uncertainty. Therefore, the prediction algorithm has the rule to infer the driving intention and predict the future states of the intersection-approaching vehicles with uncertainty prediction.

3.1. Intention Inference and State Prediction

Because the behavior of the approaching vehicles can vary widely, it is difficult to express the future behavior with a few models without compromising the continuity of behavior. However, if too many models are used to overcome this problem, the computational load becomes too burdensome to achieve real-time performance. This study utilized the interacting multiple model (IMM) filter to estimate the probability of each behavior model, which is used to fuse each behavior model. A block diagram of the proposed IMM-based target intention inference algorithm is described in Figure 4. The purpose of the IMM filter is to define the fused behavior model by integrating the behavior model of the local extended Kalman filter (EKF) based on probability.

The first step of designing the IMM filter is defining the behavior model for local EKF. The ‘Cross’, ‘Yield’, and ‘Stop’ models are defined based on the collected driving data. The data were collected at Midan City, Incheon, South Korea. The velocity profiles of the collected driving data are described in Figure 5. As shown in Figure 5, am asymmetric characteristic of the acceleration usage has been observed, which is biased to braking. Therefore, the IDM has been selected to model the asymmetric acceleration profile [26]. The IDM is composed of two terms, namely the desired velocity tracking term and the car-following behavior term, as follows:

$$\begin{array}{l}\dot{x}=v\\ \dot{v}=a_{max}\left(1-{\left(\frac{v}{v_{des}}\right)}^{\delta }-{\left(\frac{s^{*}(v,\hspace{0.17em}\Delta v)}{s_{des}}\right)}^2\right)\\ \mathrm{with}\hspace{0.17em}\hspace{0.17em}{s}^{*}(v,\hspace{0.17em}\Delta v)=s_0+v\cdot T+\frac{v\cdot \Delta v}{2\sqrt{a_{max}b}}\end{array}$$

(2)

where a_max is the maximum acceleration. v and v_des are the vehicle velocity and desired velocity. s, s^*, and Δv are the clearance, desired clearance, and relative velocity to the preceding vehicle. Parameters T, b, and δ are the desired time headway, comfortable braking deceleration, and acceleration exponent.

The collected velocity profiles should be converted to the desired velocity profiles to use the IDM as a process update model of the local EKF. The desired velocity tracking term of the IDM has been rearranged to calculate v_des from the v profile. The rearranged IDM is as follows:

$$v_{des,i}(s)=\frac{v_i(s)}{\sqrt[\delta ]{1-{\dot{v}}_i(s)/a_{max}}}$$

(3)

where i is the data index of each profile. The generated desired velocity profiles are presented in Figure 5 as a red dotted line. Parameters a_max of 5.0 m/s² and δ of 4 have been used to calculate the v_des,i(s). The derived v_des,i(s) profiles have been clustered by K-means clustering to define the behavior model as depicted in Figure 6. As mentioned before, these three v_des,i(s) profiles have been named ‘Cross’, ‘Yield’, and ‘Stop’ models and used to determine the process update model of local EKF.

The local filters have been designed as an IDM-based EKF to use ‘Cross’, ‘Yield’, and ‘Stop’ models. The state vector x and measurement vector z of the IDM-based EKF has been defined as follows:

$$x={\left[\begin{array}{ccc}p& v& a\end{array}\right]}^T,\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}z={\left[\begin{array}{cc}{p}_{meas}& v_{meas}\end{array}\right]}^T$$

(4)

where p, v, and a are the d_DTC, velocity, and acceleration, respectively. Parameters p_meas and v_meas represent the measurement of the d_DTC and velocity from the sensor of the autonomous vehicle. The process update of IDM-based EKF is defined as follows:

$$\begin{array}{cc}{\widehat{x}}_{k|k-1}& =f({\widehat{x}}_{k-1|k-1},u_k)\\ & ={\left[\begin{array}{ccc}{p}_{k|k-1}& v_{k|k-1}& a_{k|k-1}\end{array}\right]}^T\\ \mathrm{where},& \\ p_{k|k-1}& =p_{k-1|k-1}+v_{k-1|k-1}\cdot dt\\ v_{k|k-1}& =v_{k-1|k-1}+a_{k-1|k-1}\cdot dt\\ a_{k|k-1}& =a_{k-1|k-1}\left(1-\frac{dt}{{\tau }_x}\right)+a_{max}\left[1-{\left(\frac{v_{k-1|k-1}}{v_{des}(p_{k-1|k-1})}\right)}^{\delta }\right]\cdot \frac{dt}{{\tau }_x}\end{array}$$

(5)

$$\begin{array}{l}{P}_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k{}^T+Q_k\\ \mathrm{where},\\ F_k=\left[\begin{array}{ccc}1& dt& 0\\ 0& 1& dt\\ 0& F_{k,(3,2)}& 1-dt/{\tau }_x\end{array}\right]\\ F_{k,(3,2)}=-a_{max}\delta \left(\frac{v_{k-1|k-1}{}^{\delta -1}}{v_{des}{\left(p_{k-1|k-1}\right)}^{\delta }}\right)\cdot \frac{dt}{{\tau }_x}\end{array}$$

(6)

where the parameters dt of 0.1 s and τ_x of 0.5 s are the sampling time and acceleration delay constant of the IDM-based EKF. The probabilities of each local filter are estimated by using the probability update process of the IMM filter. First, the mixing probability of the i-th model is calculated as follows:

$${\mu }_{i|j}(k-1|k-1)=\frac{1}{c_j}{p}_{ij}{\mu }_i(k-1)$$

(7)

where c_j is the normalizing factor and μ_i is the i-th model probability. p_ij is the transition probability from the i-th model to the j-th model under the assumption of the Markov chain. The mixed state is calculated as follows:

$${\widehat{x}}_{oj}(k-1|k-1)={\displaystyle \sum _{i=1}^m{\widehat{x}}_i(k-1|k-1) \times }{\mu }_{i|j}(k-1|k-1))$$

(8)

Based on the mixed state, filtering using the IDM-based EKF is performed. The model probability is updated with the likelihood ratio Λ_j,k as follows:

$$\begin{array}{l}{\mu }_{j,k}=\frac{1}{c}{\Lambda }_{j,k}{\displaystyle \sum _i{p}_{i,j}{\mu }_{i,k-1}}\\ \mathrm{where},\hspace{0.17em}\hspace{0.17em}{\Lambda }_{j,k}=N(r_{j,k};\hspace{0.17em}0,\hspace{0.17em}{S}_{j,k})\end{array}$$

(9)

In Equation (9), r_j,k is the residual of the j-th model, and S_j,k is the covariance matrix of the residual. The remaining parts and details of the IMM filter can be found in [27].

The process update of each IDM-based EKF has been conducted within the prediction horizon to predict the future states of the target vehicle. In this study, the fused behavior model has been derived by using the individual behavior model based on the estimated probability of each model. Therefore, the desired velocity profile of the fused behavior model has been defined as follows:

$$v_{des,fused}(s)={\displaystyle \sum _j{\mu }_j\cdot }{v}_{des,j}(s)$$

(10)

The fused behavior model, v_des,fused(s), has been used as a desired velocity profile of the IDM to predict the future state of the target vehicle based on the sensor measurements. The example of state prediction with an estimated probability is summarized in Figure 7. As shown in Figure 7, the proposed predictor provides more accurate prediction results than the single model prediction case.

3.2. Recursive Uncertainty Estimation

In the previous chapter, the target state prediction with the intention inference algorithm has been proposed based on the IMM approach. Because the future states are predicted based on the present and past information, the prediction uncertainty inevitably exists regardless of the prediction methods. In addition, the level of uncertainty is dependent on the prediction algorithm and the quality of the object data from the perception algorithm. Therefore, a proper algorithm for uncertainty estimation is required to increase the safety and control performance of the prediction-based motion planner. In this study, a recursive covariance estimator has been used to estimate the prediction uncertainty of the prediction results [28]. Because the prediction algorithm is based on the process update model of the EKF using IDM, the recursive covariance estimator is applicable to estimate the process noise of each state, such as the position and velocity. The basic concept of the prediction uncertainty estimation is a comparison between the current true states and predicted states derived from the true states of the previous step. The graphical representation of the relationships is described in Figure 8.

The recursive covariance estimator assumes that the measurement noise is stationary. As mentioned in the previous chapter, the position of the target vehicle is defined in a fixed global coordinate, not a local coordinate of the autonomous vehicle. In other words, d_DTC, the position of the state vector, is defined with respect to the conflict point on the intersection map. Therefore, the proposed prediction algorithm satisfies the assumption of the recursive covariance estimator. Since EKF is already used, the process update and measurement can be written in the linearized equation as follows:

$$\begin{array}{l}{x}_k=A_{k-1}{x}_{k-1}+B_{k-1}{w}_{k-1}\\ y_k=C{x}_k+v_k\end{array}$$

(11)

where w_k−1 and v_k are the covariances of the prediction model and perception module. A_k−1, B_k−1, and C are the system matrix, input matrix, and output matrix, respectively. In this study, A_k−1 is the same matrix F_k in the previous section. In Equation (11), x_k−1 is the vehicle state at (k−1), and x_k is the predicted state using the prediction model. This means that x_k is the first-step prediction results from the true state x_k−1 using the above-linearized equation.

y_k can be represented in terms of x_k−1 as follows:

$$y_k=C{A}_{k-1}{x}_{k-1}+C{B}_{k-1}{w}_{k-1}+v_k$$

(12)

x_k−1 is represented in terms of y_k−1 as follows:

$$\begin{array}{l}{x}_{k-1}={[C^{T}C]}^{-1}{C}^T{y}_{k-1}-{[C^{T}C]}^{-1}{C}^T{v}_{k-1}\\ \hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}=M\hspace{0.17em}{y}_{k-1}-M\hspace{0.17em}{v}_{k-1}\\ \mathrm{where},\hspace{0.17em}\hspace{0.17em}M={[C^{T}C]}^{-1}{C}^T\end{array}$$

(13)

y_k can be represented as follows by substituting (13) into (12):

$$y_k=C{A}_{k-1}M\hspace{0.17em}{y}_{k-1}-C{A}_{k-1}M\hspace{0.17em}{v}_{k-1}+C{B}_{k-1}{w}_{k-1}+v_k$$

(14)

In Equation (14), the only unknown term is w_k−1. Because y_k and y_k−1 are the measurement from the perception algorithm, v_k and v_k−1 are the same measurement noise under the assumption of stationary. Therefore, the recursive estimator for w_k−1 can be defined by multiplying M to (14) and introducing the new notations ξ_k and V_k:

$$\begin{array}{l}M{y}_k-AM{y}_{k-1}=B{w}_{k-1}+M{v}_k+AM{v}_{K-1}\\ {\xi }_k=B{w}_{k-1}+V_k\\ \mathrm{where},\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}{\xi }_k=M{y}_k-AM{y}_{k-1}\\ \hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}{V}_k=M{v}_k-AM{v}_{k-1}\end{array}$$

(15)

The covariance of w_k−1 is estimated as a recursive form as follows:

$$\begin{array}{l}Co{v}_k(\xi )=\frac{k-1}{k}Co{v}_k(\xi )+\frac{1}{k}{\xi }_k{\xi }_k{}^T\\ Co{v}_k(Bw)=Co{v}_k(\xi )+Cov(V)\end{array}$$

(16)

Using (16), the covariance of the process noise w_k is estimated.

4. Target State Prediction with Uncertainty Estimation

It is necessary to consider the uncertainty of the prediction in order to improve the safety and performance of autonomous vehicles on urban roads. Therefore, this study integrates the SMPC approach with the prediction uncertainty estimator to improve the performance of SMPC by updating the probability distribution of chance constraints every moment. Before discussing SMPC, the driving mode and constraints are properly defined based on the predicted states of intersection-approaching vehicles. Therefore, the robust motion planner for intersection passing is composed of three sub-modules: The driving mode decision, the defining prediction-based constraint, and solving SMPC. The details of each module will be discussed in the following sections.

4.1. Driving Mode Decision

Constraints for SMPC are highly related to the driving mode because intersection cross priority decides the boundaries of constraints. Therefore, the driving mode should be determined before defining the constraints for SMPC. In this study, driving modes are composed of three modes: ‘Approach’, ‘Cross’, and ‘Yield’. The approach mode is prepared to provide safe approaching behavior at the uncontrolled intersection until the target is detected. It means that the driver’s deceleration characteristics are reflected in autonomous driving as an approach mode to avoid the inevitable collision with targets appearing from a blind spot. The deceleration command of the approach mode is determined based on the shape of the occluded region, the possible target-appearing position, and the nominal braking characteristics of the human driver.

Meanwhile, the cross and yield modes are declared when the autonomous vehicles detect other intersection-approaching targets. In these driving modes, the risk caused by the target vehicle should be properly managed by the interaction between the autonomous vehicle and targets. Then, an SMPC-based motion planner is used to determine the desired acceleration. Particularly, yield mode is the replacement of the stop mode of the previous studies to cover most driving situations except when the ego vehicle has a cross priority. Therefore, an autonomous vehicle in yield mode does not simply stop but determines various behaviors according to the driving situation.

A mode transition condition between the cross and yield modes is defined based on the uncontrolled intersection theory. The uncontrolled intersection theory has been used to analyze the driving behavior of human drivers who are interacting with each other in uncontrolled intersections. The concepts of critical gap and follow-up gap have been used to define the mode transition condition between the cross and yield modes. In the uncontrolled intersection theory, the critical gap is the threshold for the time gap between higher-rank stream vehicles. It means that no driver will split the vehicle stream from a higher-rank road unless the gap between the vehicle in a higher priority stream is at least equal to the critical gap. The follow-up gap is the threshold to follow the preceding vehicle that enters the intersection. The driver will enter the intersection if the headway time with a preceding vehicle is less than the follow-up gap.

This concept has been modified to define the mode transition condition for the proposed driving-mode decision algorithm. First, the approaching targets have been classified into three groups, primary, secondary, and in-lane targets, as shown in Figure 9. The first vehicle entering the intersection from other branches of the ego vehicle is denoted as a primary target. Then, the next vehicle immediately following the primary target is classified as a secondary target. The in-lane target means the preceding target of the ego vehicle. Based on the target classification, a time gap between primary and secondary targets is compared to the critical gap. A headway time between the in-lane target and ego vehicle is compared to the follow-up gap. When calculating the time gap and headway time, the predicted states of the ego and the target vehicle have been used to overcome the limitations of the constant velocity assumption.

The driving mode is changed to yield mode if one of the following conditions is satisfied: (1) The remaining time to the intersection is larger than the primary target, (2) the time gap between primary and secondary target is smaller than the critical gap threshold, or (3) the headway time to the preceding vehicle is larger than the follow-up gap threshold. On the contrary, the cross mode is only declared if the opposites of the aforementioned conditions are all satisfied simultaneously. This hard condition for the cross mode is introduced to prevent a dangerous situation caused by misjudgment of the driving mode. In this study, a critical gap threshold of 4 s and a follow-up gap threshold of 2 s has been used [29,30,31]. In addition, the transition margin of a 1s base has been added to avoid chattering between driving modes.

4.2. Stochastic Model Predictive Controller

The first step of designing the SMPC-based motion planner is defining the plant model [32]. The state-space model of the longitudinal dynamics is defined by considering the characteristics of the longitudinal actuator. The delay of the acceleration and braking can be modeled as a first-order delay model. Therefore, the first-order input delay model has been employed to define a longitudinal model. The state and input vectors are defined as follows:

$$x_{[k]}={\left[\begin{array}{ccc}{p}_{[k]}& v_{[k]}& a_{[k]}\end{array}\right]}^T,\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}{u}_{[k]}=a_{des,[k]}$$

(17)

where p_[k], v_[k], and a_[k] are the position, velocity, and acceleration of the ego vehicle on the curvilinear coordinate along the desired trajectory of the ego vehicle. a_des,[k] is the longitudinal acceleration input, which is determined by solving the SMPC problem. Based on the state and input vectors, the longitudinal dynamics are defined as follows:

$$\left[\begin{array}{c}{p}_{[n+1]}\\ v_{[n+1]}\\ a_{[n+1]}\end{array}\right]=\left[\begin{array}{ccc}1& dt& 0\\ 0& 1& dt\\ 0& 0& 1-dt/{\tau }_x\end{array}\right]\left[\begin{array}{c}{p}_{[n]}\\ v_{[n]}\\ a_{[n]}\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ dt/{\tau }_x\end{array}\right]u_{[k]}$$

(18)

where dt and τ_x are the sampling time of the SMPC solver and the acceleration delay constant of the actuator.

The next step is defining the constraints for the SMPC problem. The concept of intersection motion planning is described in Figure 10. The example scenario is the primary target-only case where a single conflict point region is predicted inside the intersection as shown in Figure 10a. This conflict point region can be defined based on the prediction time vs. the predicted station plane as shown in Figure 10b. When defining the conflict point region, the prediction results of the ego vehicle under the assumption of the cross mode and the predicted states of the approaching target using the proposed predictor have been used to find the most probable conflict position and time. The constraints have been determined to avoid the conflict point region while considering the prediction uncertainty.

Constrains for the SMPC problem are categorized as chance constraints and deterministic constraints. The constraints related to the predicted states of the targets are defined as chance constraints. Meanwhile, the dynamics, actuator limitation, and initial condition correspond to the deterministic constraints. As mentioned before, the predicted states of the target are used to define the conflict point region. Therefore, the position constraint should consider the uncertainty of target state prediction. In other words, the position constraint is designed as a chance constraint. The inequalities of the position constraint are defined as follows:

$$\begin{array}{l}Yield\hspace{0.17em}Mode\hspace{0.17em}:\hspace{0.17em}\hspace{0.17em}0\le p_{[k]}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}and\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}{p}_{[k]}\le P_{ego,[k]}-c_{[k]}\\ Cross\hspace{0.17em}Mode\hspace{0.17em}:\hspace{0.17em}{P}_{ego,[k]}+c_{[k]}\le p_{[k]}\\ \mathrm{where},\\ c_{[k]}=\left(TT{C}_{conf,\min }-\frac{{\overline{x}}_{[k],\hspace{0.17em}1}}{{\overline{x}}_{[k],\hspace{0.17em}2}}\right)\cdot v_{[k]}+C_{conf,\min }\end{array}$$

(19)

where TTC_conf,min and C_conf,min are the minimum value of TTC_conf and C_conf from the human driving data analysis. $\overline{x}$_[k],1 and $\overline{x}$_[k],2 are the predicted position and velocity of the target vehicle, which have uncertainty. P_ego is the base constraint that follows the predicted states of the ego vehicle, which is represented as a green dotted line in Figure 10b. c_[k] is the coupled term between the optimal state of the ego vehicle and the predicted state of the target vehicle. Therefore, (19) has a stochastic variable as a form of an inequality constraint. In other words, (19) is the chance constraint of the SMPC problem. Therefore, the position constraints are redefined as follows:

$$\begin{array}{l}Yield\hspace{0.17em}\hspace{0.17em}Mode\hspace{0.17em}:\mathrm{Pr}(p_{[k]}\le P_{ego,[k]}-c_{[k]})>\beta \\ Cross\hspace{0.17em}Mode\hspace{0.17em}:\hspace{0.17em}\mathrm{Pr}(P_{ego,[k]}+c_{[k]}\le p_{[k]})>\beta \end{array}$$

(20)

where β is the lower boundary for the probability of the inequality equations. The standard deviation of the probability distribution is defined as an estimation result of the recursive covariance matrix. It means that the inequality constraints are adaptively adjusted according to the perception performance and the accuracy of the behavior predictor.

The position constraint is defined differently for the corresponding driving mode. For the yield mode, the ego vehicle avoids the collision using braking, which means an upper limit is assigned. For the cross mode, the ego vehicle has a priority to pass the conflict point first then the target vehicle. In this case, a lower limit has been applied to the position constraint.

The constraints for v_[k], a_[k], and $\dot{a}$_[k] are defined as a deterministic constraint as follows:

$$\begin{array}{l}0\le v_{[k]}\le \min \left(\left[\begin{array}{cc}{v}_{max}& v_{curve}\end{array}\right]\right)\\ a_{min}\le a_{[k]}\le a_{max}\\ \frac{\left|a_{[k+1]}-a_{[k]}\right|}{dt}\le {\dot{a}}_{max}\end{array}$$

(21)

where v_max and v_curve are the velocity limit and velocity derived from the maximum lateral acceleration and road curvature. a_min, a_max, and $\dot{a}$_max are the minimum acceleration, maximum acceleration, and maximum jerk for longitudinal motion, respectively. The parameters used for the SMPC problem are summarized in

Table 1. Parameters for SMPC problem.

Symbol	Values	Symbol	Values
dt	0.2 s	vmax	50 km/h
τx	0.5 s	amax	1 m/s2
Np	25	amin	−5 m/s2
TTCconf,min	2.0 s	$\dot{a}$max	2 m/s3
Cconf,min	5.0 m	β	0.95

.

The initial conditions are defined as the current states of the ego vehicle.

$$p_{[1]}=0,\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}{v}_{[1]}=v_{ego},\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}{a}_{[1]}=a_{ego}$$

(22)

Based on these constraints, the SMPC problem has been designed to calculate the desired longitudinal acceleration. The cost function for the SMPC problem is formulated as follows:

$$J={\displaystyle \sum _{k=1}^{N_p}{\rm E}}\left({\Vert x_{[k]}-x_{ref,[k]}\Vert }_Q^2+{\Vert u_{[k]}\Vert }_R^2\right)$$

(23)

where x_ref,[k] is the ego vehicle prediction results, which are depicted as a green dotted line in Figure 10b. Q and R are the weighting matrices of the cost function. N_p is the size of the prediction horizon of the SMPC problem. The cost function is designed to minimize the deviation from the ego vehicle prediction results and control effort. Therefore, the SMPC problem calculates the control inputs that minimize the change of behavior of the cross mode and the acceleration usage. The solver FORCES is utilized to solve the SMPC problem in the MATLAB/Simulink environment [33].

5. Results

The proposed motion planner based on SMPC with a recursive uncertainty estimator has been evaluated through computer simulation for a Left-Turn Across-Path–Opposite Direction (LTAP-OD) scenario and the arbitrary scenario with multi-traffic. For simulation under the arbitrary scenario, the Monte Carlo simulation approach has been used to randomly generate the initial condition and driving route of target vehicles. The simulation environment has been constructed based on MATLAB/Simulink R2021a and CarSim 8.02.

5.1. Case Study

The simulation scenario for the case study is described in Figure 11. The LTAP-OD scenario has been used for the evaluation of the proposed and base algorithms. The SMPC without an uncertainty estimator is denoted as a base algorithm in this chapter. In the conventional LTAP-OD scenario, the subject vehicle tries to turn left while oncoming targets are going straight. In this study, LTAP-OD has been modified to swap the driving route of the ego and target vehicles. In other words, the target vehicle turns left while the ego vehicle attempts to pass straight through the intersection. The values of initial d_DTI,ego of 80 m, initial d_DTI,target of 60 m, the initial velocity of 45 km/h, and the maximum velocity of 50 km/h have been used as an initial condition. The difference in the initial condition of d_DTI was intended to make the target vehicle enter the intersection first, which creates the situation where the ego vehicle will yield. Under the same initial condition, the simulation has been performed three times for each algorithm with different levels of sensor noise. In each of the three simulation runs, the measurement noise of the environment sensor and perception algorithm has been assumed to be a normal distribution. The standard deviation of the sensor noise has been set to 0.5-times, 1 time, and 2 times the reference value from the quantitative analysis of the previous study [25].

The simulation results of the proposed and base algorithms have been summarized in Figure 12 and Figure 13, respectively. A history of the variables has been represented with respect to the d_DTI to prevent the misinterpretation caused by a different driving time. As shown in Figure 12 and Figure 13, the behavior changes of the ego vehicle occurred at d_DTI = 30 m, where the target vehicle was detected by the sensor model for the first time. The driving mode of all simulations was transitioned to yield mode after target detection. As a result of the simulation, it can be confirmed that the TTC_conf and C_conf were maintained above the TTC_conf,min and C_conf,min in all simulation cases. However, the difference in behavior shown in the simulation results was significant.

From the simulation results of the proposed algorithm shown in Figure 12, it can be confirmed that the response to the perception and prediction uncertainty has been effectively made. The magnitude of the deceleration command is proportional to the sensor noise level because the probability distribution of the chance constraint is adapted to the uncertainty estimate from the recursive covariance estimator. Specifically, the amount of the deceleration is increased to respond to large uncertainty of the σ_sensor = 2 σ_ini case. Then, in a situation where the influence of uncertainty is reduced due to the decrease in C_conf, the acceleration command is generated earlier than the σ_sensor = σ_ini case because the safety margin is already secured. In the case of σ_sensor = 0.5 σ_ini, the SMPC can trust the prediction results more than other cases, because the prediction algorithm estimates the small uncertainty. Therefore, the risk caused by approaching targets can be managed at a point close to the conflict point region. The braking command is generated at a later moment than the other two cases. As the proposed algorithm can actively respond to the uncertainty of the sensor, the trajectories of TTC_conf vs. C_conf show a similar shape while complying with the constraints for TTC_conf and C_conf.

The simulation result of the base algorithm shows the opposite tendency to the proposed algorithm, and it can be seen that more deceleration is utilized to manage the risk. Because the covariance of the chance constraint is set to σ_ini, the simulation result of the σ_sensor = σ_ini case is the same as the proposed algorithm. However, the sensor noise differed from the σ_ini, and different behavior was observed as shown in Figure 13. In the case of σ_sensor = 2 σ_ini, the predicted states of the target are overly trusted, which means the delayed braking is close to the conflict point region. On the other hand, the uncertainty is excessively considered in the σ_sensor = 0.5 σ_ini case, which causes deceleration to occur earlier. In both simulation cases, the minimum safety specifications TTC_conf,min and C_conf,min were satisfied, but the base algorithm was unable to respond to changes of perceived uncertainty and showed discomfort behavior for passengers and surrounding vehicles. In short, the simulation results for the LTAP-OD scenario have shown that the proposed algorithm can manage the uncertainty of perception and prediction when passing through the uncontrolled intersection.

5.2. Monte Carlo Simulation

Monte Carlo simulation has been conducted to evaluate the proposed algorithm in various situations. The test scenario has been generated by randomly selecting the sensor noise level, initial position, initial velocity, maximum velocity, and the driving route of the target vehicles. Five target vehicles have been used when configuring the test scenarios. Sets of the sensor noise, initial position, initial velocity, and maximum velocity are generated by using a normal distribution as follows:

$$\begin{array}{l}{\sigma }_{sensor}=N\left(0.3 \mathrm{m}, 0.05 \mathrm{m}\right)\\ d_{DTI,ini}=N(100 \mathrm{m}, 20 \mathrm{m})\\ v_{ini}=N(40 \mathrm{k}\mathrm{m}/\mathrm{h},\hspace{0.17em}\hspace{0.17em}10\hspace{0.17em}\mathrm{k}\mathrm{m}/\mathrm{h})\\ v_{\max }=N(50\hspace{0.17em}\mathrm{k}\mathrm{m}/\mathrm{h},\hspace{0.17em}\hspace{0.17em}5\hspace{0.17em}\mathrm{k}\mathrm{m}/\mathrm{h})\end{array}$$

(24)

The example case of the Monte Carlo simulation scenario is shown in Figure 14.

A total of 100 simulations were performed for both algorithms. The distributions of the simulation result on the TTC_conf vs. C_conf plane are depicted in Figure 15. As the two vehicles approach the intersection, the TTC_conf and C_conf decrease, and trajectories are headed to the origin of the TTC_conf vs. C_conf plane. Then, the motion planner applied braking inputs to manage the risks caused by approaching targets and prevent the reduction of TTC_conf. After the target vehicle passing through the intersection, TTC_conf and C_conf increase to the right top of the TTC_conf vs. C_conf plane. From this point of view, it can be seen that the proposed algorithm maintains its control performance robustly against changes in sensor uncertainty from the distribution toward the top right side of the TTC_conf vs. C_conf plane. On the other hand, the results of the base algorithm show that few trajectories are heading to the top right. This shows that the base algorithm does not have the ability to respond to changes in sensor uncertainty. In particular, since the risk was not properly assessed, the braking input was increased only after TTC_conf,min and C_conf,min were exceeded. In other words, the results of the proposed algorithm were maintained above the safety constraints TTC_conf,min and C_conf,min. However, the base algorithm cannot guarantee the safety constraints, even though more deceleration was used.

In order to analyze the ride comfort of both algorithms, the histogram of the acceleration usage is illustrated in Figure 16. The acceleration usage of the proposed algorithm was concentrated near 0 m/s² and most of it is in the range of −3 to 1 m/s², which is the typical range of human drivers. However, the histogram of the base algorithm is evenly distributed between −3 to 1 m/s². In addition, hard braking below −3 m/s² is often used. This difference can be explained based on the shape difference of the trajectory on the TTC_conf vs. C_conf plane shown in Figure 15. As mentioned before, the base algorithm does not guarantee the minimum constraints, and in this situation, maximum deceleration is generated. As a result, hard braking below −3 m/s² occurs frequently as shown in Figure 16b. Meanwhile, the proposed algorithm proactively applies braking before the risk increases, and risk is appropriately managed with minimal hard braking. Therefore, the proposed algorithm achieves both safety and ride comfort compared to the base algorithm.

From the perspective of computational time, the proposed SMPC-based motion planner was able to determine the desired acceleration within 30 ms even though the algorithm was implemented in MATLAB/Simulink environments. When implemented on an autonomous vehicle, a velocity controller calculates the throttle and brake inputs with much faster sampling to track the target acceleration determined by the proposed algorithm. In addition, since the speed of the vehicle is not fast in the uncontrolled intersection driving situation, the error caused by the calculation time of 30 ms is not significant. Furthermore, since the 30 ms is shorter than the typical LiDAR or camera sensor processing time, a sampling time of 30 ms is fast enough to determine the target motion based on environmental information. For example, the LiDAR-based environment perception system used to collect driving data in this study has a sampling time of 40 ms. Therefore, the proposed algorithm can be applied to an autonomous vehicle.

6. Conclusions

A stochastic model predictive control-based motion-planning algorithm for uncontrolled intersections has been developed and evaluated with computer simulation. The proposed SMPC problem has been improved by integrating it with a prediction uncertainty estimator. The future states of target vehicles have been predicted using an interacting multiple-model filter based on human behavior models. The intelligent driver model has been used to define the behavior model and process update of the IMM filter. The uncertainty of the sensor measurement and prediction has been estimated by using the recursive covariance estimator. The driving mode has been determined using the estimated time to arrive at the intersection, the critical gap, and the follow-up gap. The desired acceleration of autonomous vehicles has been determined using the SMPC approach considering the safety indexes, predicted states of ego and target vehicles, prediction uncertainty, and driving mode. Simulation studies have been conducted to evaluate the proposed algorithm. The results of the left-turn across-path–opposite direction scenario showed that the integrated approach of an uncertainty estimator and SMPC improve the robustness of perception performance. The results of the Monte Carlo simulation showed the proposed algorithm to guarantee the minimum safety level and improve the ride comfort compared to the base algorithm without uncertainty estimation.

The proposed integrated approach can be extended to situations where prediction uncertainty can affect the performance of the motion planning of autonomous driving. For example, decision and motion planning for lane changing, overtaking, and roundabout scenarios can improve the safety and performance of an autonomous vehicle driving with human drivers. In future studies, the proposed motion planner for intersections will be extended to the path-planning algorithm within the intersection as well as the prediction error estimation for lateral motion. In other words, the algorithm for an uncontrolled intersection will cover the prediction of the surrounding vehicles and motion planning in both directions, longitudinally and laterally. In addition, the model-based predictor will be improved by introducing learning-based approaches to effectively reflect the driving characteristics of the drive.