Computational Efficient Motion Planning Method for Automated Vehicles Considering Dynamic Obstacle Avoidance and Traffic Interaction

Abstract

In complex driving scenarios, automated vehicles should behave reasonably and respond adaptively with high computational efficiency. In this paper, a computational efficient motion planning method is proposed, which considers traffic interaction and accelerates calculation. Firstly, the behavior is decided by connecting the points on the unequally divided road segments and lane centerlines, which simplifies the decision-making process in both space and time span. Secondly, as the dynamic vehicle model with changeable longitudinal velocity is considered in the trajectory generation module, the C/GMRES algorithm is used to accelerate the calculation of trajectory generation and realize on-line solving in nonlinear model predictive control. Meanwhile, the motion of other traffic participants is more accurately predicted based on the driver’s intention and kinematics vehicle model, which enables the host vehicle to obtain a more reasonable behavior and trajectory. The simulation results verify the effectiveness of the proposed method.

1. Introduction

Due to the complex driving scenarios and difficulty in accurately predicting the behavior of the surrounding vehicles, the automated vehicles need to adapt to great complexity and dynamics in real traffic [1,2]. Therefore, advanced motion planning algorithms should help the agent behave reasonably and respond adaptively in dynamic and complex driving scenarios with computational efficient and reliable control [3].

1.1. State-of-the-Art Review and Challenges

The performance of motion planning methods is closely related to the trajectory prediction of other traffic participants, behavior planning, and trajectory generation. As automated vehicles will frequently interact with other traffic participants, trajectory prediction will influence behavior planning and trajectory generation modules. The motion of other environment vehicles is predicted with constant longitudinal velocity or acceleration [4]. Such a prediction is inaccurate and will decrease the reasonability of behavior planning and trajectory generation modules [5,6]. Thus, more accurately predicted trajectories will promote the performance of motion planning.

In behavior planning aspects, the machine learning-based or model-based methods usually decide a human-like behavior, like lane-change and obstacle avoidance, which contains a wide range of trajectories. To ensure absolute safety, decision-making methods are always conservative. Thus, more complex behaviors should be generated in decision-making. POMDP can generate a more abundant behavior [7,8]. The road is divided into several segments and the points are connected to represent different behavioral decisions [9,10].

Regarding trajectory generation, current methods can be divided into graph-search-based methods [11], incremental search [12], interpolating curve methods [13] and numerical optimization [14,15]. Model Predictive Control (MPC) is widely used because it can explicitly deal with constraints to ensure safety with consideration of traffic interaction not only at the current time step but also in the predictive horizon [16,17]. As for the control model in MPC, the dynamic vehicle model is added to the kinematic model to realize stable motion control and enrich driving behaviors [18,19].

The high-performance motion planning methods should be computationally efficient and enable automated vehicles to behave reasonably and respond adaptively in dynamic and complex scenarios. First, the host vehicle will interact with the surrounding vehicles while driving on the road inevitably. The motion prediction of environment vehicles needs to be predicted in the planning horizon, which enables the host vehicle to behave reasonably and adapt to the dynamic traffic environment. Meanwhile, the driving process contains a large span in both time and space, which means precise driving behavior will cause huge calculations. To balance the calculation time and planning performance, the problem in the model formulation aspect should be simplified without losing reasonability.

To further promote the reasonability of motion planning, except for the dynamic vehicle model, variational velocity can be considered in the trajectory generation. Thus, the control model will change from the linear model to the nonlinear model [20]. Such a control model increases the time for online solving, which needs an additional fast solving algorithm to realize online calculation [21,22,23].

1.2. Work and Contributions

As shown in Figure 1, this paper proposes a computational efficient motion planning method for autonomous vehicles, which can behave reasonably and adaptively in dynamic and complex driving scenarios. After predicting the trajectory of environment vehicles, the behavior planning and trajectory generation will be done sequentially. The following improvements simplify the problem and decrease calculation to realize computational efficiency. In the behavior planning module, the road is divided into several segments with road points and different behaviors are represented with different connections between road points in each segment. Firstly, rather than only set road points in the center of the lane, the road points are also distributed on the lane line, which enables the behavior planner to generate more complex behaviors. Meanwhile, in predictive control, short-term behavior is much more complex and also should be paid more attention to. Besides, the prediction is not accurate while lengthening the predictive horizon. Thus, rather than equally dividing the road, unequal segments that the distance between these road segments is gradually increasing can further decrease calculation and raise reasonability. Secondly, based on the analysis of different traffic participants, the motion of static environment vehicles is much more simple, and can directly exclude corresponding behaviors. Therefore, static environment vehicles are used to narrow the feasible region of the solution and further decrease online calculation. Thirdly, variational longitudinal velocity and dynamic vehicle models are considered to raise the reasonability of trajectory generation. It can also speed up the trajectory following process with the resulting acceleration and steering wheel angle. We use the C/GMRES algorithm to realize on-line calculations in NMPC. The main contributions are summarized as follows

• The complex and reasonable behavior of the host vehicle is efficiently realized by connecting different points located on unequally divided road segments and lane centerlines.
• Trajectory prediction of surrounding vehicles is considered during trajectory planning. And the trajectory planning is based on both driver’s intention and the kinematics vehicle model, which can increase the accuracy and rationality.
• C/GMRES is used to realize online calculation and raise the reasonability of trajectory generation and trajectory following.

The remainder of this paper is organized as follows. In Section 2, the coordinate systems and the trajectory prediction are introduced. Section 3 introduces the behavioral planning module. In Section 4, the trajectory generation module is introduced. In Section 5, the simulation process is shown, and the results are given and analyzed in detail. The simulation results verify the effectiveness of the proposed method. Section 6 is the conclusion of this paper.

2. Coordinate Systems Conversion and Trajectory Prediction

In this section, first, the conversion between the Cartesian coordinate system and the Frenet coordinate system is introduced to simplify the planning process. Then, three ways of trajectory prediction are illustrated and compared.

2.1. Coordinate Systems Conversion

To describe the relation between two coordinates, as shown in Figure 2, the Cartesian coordinate of $\overrightarrow{x}$ is $\overrightarrow{x}(x,y)$ while the Frenet coordinate of $\overrightarrow{x}$ is $\overrightarrow{x}(s,l)$, where l is the distance from $\overrightarrow{x}$ to the reference point [24]. In coordinate systems conversion, the lane centerline is extracted with a third-degree polynomial equation as the reference curve of the Frenet coordinate system, e.g., $y=a{x}^3+b{x}^2+cx+d$. For the conversion from Frenet coordinate $C_a:(s_a,l_a)$ to Cartesian coordinate $C_a:(x_a,y_a)$, a nearest point $p_0$ works as the reference point to convert $p_a$, whose Frenet coordinate can be written as $C_0:(x_0,a{x}_0^3+b{x}_0^2+c{x}_0+d)$. Since the arc length $s_a$ is already known, $x_0$ can be calculated by dividing the curve and sampling from the starting point $p_s$. The integral value of the sampling point $s_0^{\prime }$ can similarly be calculated. Compare $s_0^{\prime }$ with $s_a$ to determine whether the $x_0^{\prime }$ is the desired coordinate point $x_0$, and finally find the coordinates of $p_0$. And the Cartesian coordinate of $p_a,C_a:(x_a,y_a)$ can be calculated with

$$\begin{array}{cc}\hfill x_a& =x_0-l·sin(arctan\left(k\right)),\hfill \end{array}$$

(1a)

$$\begin{array}{cc}\hfill y_a& =y_0-l·cos(arctan\left(k\right)).\hfill \end{array}$$

(1b)

where $k=3a{x}_0^2+2b{x}_0+c$ is the curvature of the reference curve at point $p_0$.

For the conversion from Cartesian coordinate $C_a:(x_a,y_a)$ to Frenet coordinate $C_a:(s_a,l_a)$, the reference point $p_0$ is also needed. We use $D_2$ to represent the square of the distance from $p_a$ to the reference point $p_0$. The horizontal coordinate value of the reference point $x_0^{*}$ that minimizes $D_2$ satisfies $D_2^{\prime }\left(x_0^{*}\right)=0$, which can be solved by Newton’s method [25]. By calculating $D_2$ and $D_2^{\prime }$, the iteration formula can be expressed as

$$\begin{array}{c}\hfill x_0^{*,m+1}=x_0^{*,m}-\frac{D_2^{\prime }\left(x_0^{*,m}\right)}{D_2^{″}\left(x_0^{*,m}\right)},m=0,1,2,\dots .\end{array}$$

(2)

The iteration stops while $|x_0^{*,m+1}-x_0^{*,m}|⩽\delta$ and $x_0^{*,m+1}$ is the target value. The Frenet coordinate $C_a:(s_a,l_a)$ can be solved as

$$\begin{array}{cc}\hfill s_a& ={\int }_{x_0}^{x_0^{*,m+1}}\sqrt{1+{(3a{x}^2+2bx+c)}^2}dx,\hfill \end{array}$$

(3a)

$$\begin{array}{cc}\hfill l_a& =D_2\left(x_0^{*,m+1}\right)=\sqrt{{(x_a-x_0^{*,m+1})}^2+{(y_a-y_0^{*,m+1})}^2}.\hfill \end{array}$$

(3b)

2.2. Trajectory Prediction

The information about the surrounding vehicles and the trajectories of the surrounding vehicles in a period of time in the future is essential for motion planning. Such trajectory prediction can be done based on the driver’s intention, vehicle kinematics model, or both driver’s intention and vehicle kinematics model, which will be compared in this section.

2.2.1. Trajectory Prediction Based on Driver’s Intention

We consider lane change and lane-keeping operations. For an operation intention, countless driving trajectories can be realized. Based on the driver of the vehicle, the actual driving trajectory may be very gentle or aggressive. In addition, the geometric environment of the road will also affect the actual trajectory. Therefore, the trajectory prediction based on the operation intention can generate a set of predicted trajectories based on the current state of the vehicle, the operation intention, and the road parameters, and then select the optimal one based on the information. Since the shape of the road has a great influence on the predicted trajectory, the predicted trajectory cluster is firstly generated in the Frenet coordinate system, then converted into the Cartesian coordinate system.

In Frenet coordinate system, $s\left(t\right)$ and $l\left(t\right)$ represent longitudinal distance and lateral distance, respectively. We use $F_0=(s_0,{\dot{s}}_0,{\ddot{s}}_0,l_0,{\dot{l}}_0,{\ddot{l}}_0)$ and $F_1=(s_1,{\dot{s}}_1,{\ddot{s}}_1,l_1,{\dot{l}}_1,{\ddot{l}}_1)$ to represent the initial state and the end state of the vehicle trajectory. To ensure the continuity of the trajectory and provide unique expressions for different trajectories, high-order polynomials are used for fitting to represent the trajectory of longitudinal $s\left(t\right)$ and lateral distance $l\left(t\right)$ over time t. For the initial state $F_0$, each state variable can be obtained by converting the current kinematic parameters in the Cartesian coordinate system, which can be expressed as

$$\left\{\begin{array}{c}{l}_0=l_0^{*},{\dot{l}}_0=v_{0}sin({\theta }_0-{\theta }_{T_0}),\hfill \\ {\ddot{l}}_0=\sqrt{(a_{0^2}+{\gamma }_0{v}_{0^2})}sin({\theta }_0-{\theta }_{T_0}),\hfill \\ s_0=0,{\dot{s}}_0=v_{0}cos({\theta }_0-{\theta }_{T_0}),\hfill \\ {\ddot{s}}_0=\sqrt{(a_{0^2}+{\gamma }_0{v}_{0^2})}cos({\theta }_0-{\theta }_{T_0}),\hfill \end{array}\right.$$

(4)

where $l_0^{*}$ is the distance from the initial position to the centerline of the lane. ${\theta }_{T_0}$ is the orientation of the tangent vector ${\overrightarrow{T}}_0$. ${\gamma }_0{v}_0^2$ is the current value of the normal acceleration of the vehicle and $\gamma$ is the curvature of the reference point.

We assume that the vehicle is on the center line of its target lane after finishing the intended operation and it remains the same longitudinal acceleration throughout the operation. Therefore, some of the operation termination state $F_1$ can be calculated as

$$\left\{\begin{array}{c}{l}_1=l_1^{*},{\dot{l}}_1=0,\hfill \\ {\ddot{l}}_1=0,{\ddot{s}}_1=a_0.\hfill \end{array}\right.$$

(5)

$|l_1^{*}|=d$, where d is the width of the lane while the vehicle is changing the current lane and the sign is determined by the lane change direction. For lane keeping operation, $l_1^{*}=0$.

For a complete lane change operation, the time to complete the operation $t_{end}$ is about 6 s, and the length of $t_{end}$ can be adjusted according to the driver’s driving style. For lane-keeping operations, the time $t_{end}$ is significantly shorter. Use $t_1\in [0,t_{end}]$ to represent the end time of the operation, that is, take a fixed step size and sample start from 0 to $t_{end}$ with K steps. Since it is assumed that the longitudinal acceleration of the vehicle remains unchanged, the longitudinal speed at the termination state is ${\dot{s}}_1=v_0+a_0·t_1$. For the lateral distance at the termination state, a fifth-degree polynomial fit for time t can be used, which is calculated as

$$\begin{array}{c}\hfill l\left(t\right)=c_5{t}^5+c_4{t}^4+c_3{t}^3+c_2{t}^2+c_{1}t+c_0,\end{array}$$

(6)

where $c_0,c_2,\dots ,c_5$ are the parameters of the curve. Then, $\dot{l}\left(t\right)$ and $\ddot{l}\left(t\right)$ can be calculated respectively. Therefore, the lateral state values at the initial state ($t=0$) and the terminal lateral state ($t=t_1$) can be written as

$$\left\{\begin{array}{c}{l}_0=l\left(0\right),{\dot{l}}_0=\dot{l}\left(0\right),{\ddot{l}}_0=\ddot{l}\left(0\right),\hfill \\ l_1=l\left(t_1\right),{\dot{l}}_1=\dot{l}\left(t_1\right),{\ddot{l}}_1=\ddot{l}\left(t_1\right).\hfill \end{array}\right.$$

(7)

And the parameters $c_i$ can be obtained by solving the following matrix

$$\begin{array}{cc}\hfill \left[\begin{array}{cccccc}0& 0& 0& 0& 0& 1\\ t_1^5& t_1^4& t_1^3& t_1^2& t_1& 1\\ 0& 0& 0& 0& 1& 0\\ 5t_1^4& 4t_1^3& 3t_1^2& 2t_1& 1& 0\\ 0& 0& 0& 2& 0& 0\\ 20t_1^3& 12t_1^2& 6t_1& 2& 0& 0\end{array}\right]·\left[\begin{array}{c}{c}_5\\ c_4\\ c_3\\ c_2\\ c_1\\ c_0\end{array}\right]& =\left[\begin{array}{c}{l}_0\\ l_1\\ {\dot{l}}_0\\ {\dot{l}}_1\\ {\ddot{l}}_0\\ {\ddot{l}}_1\end{array}\right]\hfill \\ & =\left[\begin{array}{c}{l}_0^{*}\\ l_1\\ v_{0}sin({\theta }_0-{\theta }_{T_0}),\\ \sqrt{(a_{0^2}+{\gamma }_0{v}_{0^2})}sin({\theta }_0-{\theta }_{T_0}),\\ 0\\ 0\end{array}\right].\hfill \end{array}$$

(8)

Since the longitudinal displacement changes according to $t_1$, a fourth-degree polynomial with respect to time t is used to fit the longitudinal kinematic, which can be expressed as

$$\begin{array}{c}\hfill s\left(t\right)=c_4^{\prime }{t}^4+c_3^{\prime }{t}^3+c_2^{\prime }{t}^2+c_1^{\prime }t+c_0^{\prime },\end{array}$$

(9)

where $c_0^{\prime },c_2^{\prime },\dots ,c_5^{\prime }$ are the parameters. The initial states and the terminal states can be represented by replacing t in the above formula with 0 and $t_1$, respectively.

$$\left\{\begin{array}{c}{s}_0=s\left(0\right),{\dot{s}}_0=\dot{s}\left(0\right),{\ddot{s}}_0=\ddot{s}\left(0\right),\hfill \\ {\dot{s}}_1=\dot{s}\left(t_1\right),{\ddot{s}}_1=\ddot{s}\left(t_1\right).\hfill \end{array}\right.$$

(10)

Therefore, the parameters $c_i^{\prime }$ can be obtained by solving the following matrix

$$\begin{array}{cc}\hfill \left[\begin{array}{ccccc}0& 0& 0& 0& 1\\ 0& 0& 0& 1& 0\\ 4t_1^3& 3t_1^2& 2t_1& 1& 0\\ 0& 0& 2& 0& 0\\ 12t_1^2& 6t_1& 2& 0& 0\end{array}\right]·\left[\begin{array}{c}{c}_4^{\prime }\\ c_3^{\prime }\\ c_2^{\prime }\\ c_1^{\prime }\\ c_0^{\prime }\end{array}\right]& =\left[\begin{array}{c}{s}_0\\ {\dot{s}}_0\\ {\dot{s}}_1\\ {\ddot{s}}_0\\ {\ddot{s}}_1\end{array}\right],\hfill \\ & =\left[\begin{array}{c}0\\ v_{0}cos({\theta }_0-{\theta }_{T_0}),\\ \sqrt{(a_{0^2}+{\gamma }_0{v}_{0^2})}cos({\theta }_0-{\theta }_{T_0}),\\ v_0+a_0·t_1\\ 0\end{array}\right].\hfill \end{array}$$

(11)

Different $t_1\in [0,t_{end}]$ corresponds to different fitting parameters c and different driving trajectories. When selecting the optimal trajectory, first convert the trajectory to the Cartesian coordinate system. The principles to be followed in the selection process include: $t_1$ is as short as possible, the driving process is comfortable, and the lateral displacement is reduced as much as possible during the lane change operation. Therefore, for the selection of the optimal predicted trajectory based on the driver’s intention, the maximum normal acceleration value in the driving trajectory and the time to complete the operation $t_1$ are mainly considered

$$\begin{array}{c}\hfill C\left(tra{j}_i\right)={\mathrm{w}}_1\max \left(\overline{a}\left(t\right)\right)+{\mathrm{w}}_2{t}_i,\end{array}$$

(12)

where $tra{j}_i$ represents the $i_{th}$ trajectory and $C\left(tra{j}_i\right)$ represents the cost of the $i_{th}$ trajectory. $\overline{a}\left(t\right)$ is the normal acceleration at time t. $t_i$ is the total time of the $i_{th}$ trajectory. ${\mathrm{w}}_1$ and ${\mathrm{w}}_2$ are two coefficients. The resulting trajectory with the smallest cost value is used as the optimal prediction trajectory based on the operation intention prediction

$$\begin{array}{c}\hfill T_{\mathrm{man}}=\arg \min {\left(C\left(tra{j}_i\right)\right)}_{i=1,\dots ,K}.\end{array}$$

(13)

2.2.2. Trajectory Prediction Based on Vehicle Kinematics Model

The trajectory predicted based on the driver’s intention is more accurate at a longer time horizon, but the accuracy is lower on a shorter time horizon. The trajectory predicted using the current kinematic parameters of the vehicle is more accurate in a shorter time. So, it is necessary to put both the intention and kinematics model into consideration. We assume that the vehicle acceleration and yaw rate remain unchanged. Therefore, in the Cartesian coordinate system, the vehicle speeds along the x and y axes at time t can be represented as

$$\begin{array}{cc}& v_x\left(t\right)=v\left(t\right)·cos(w_{0}t+{\theta }_0),\hfill \end{array}$$

(14a)

$$\begin{array}{cc}& v_y\left(t\right)=v\left(t\right)·sin(w_{0}t+{\theta }_0),\hfill \end{array}$$

(14b)

where the velocity at time t is $v\left(t\right)=a_{0}t+v_0$. The predicted trajectory based on the kinematic model can be obtained by integrating the vehicle velocities,

$$tra{j}_{mdl}:\left\{\begin{array}{c}x\left(t\right)=\frac{a_0}{w_0^2}cos\left(\theta \left(t\right)\right)+\frac{v\left(t\right)}{w_0}sin\left(\theta \left(t\right)\right)+c_x,\hfill \\ y\left(t\right)=\frac{a_0}{w_0^2}sin\left(\theta \left(t\right)\right)-\frac{v\left(t\right)}{w_0}cos\left(\theta \left(t\right)\right)+c_y,\hfill \end{array}\right.$$

(15)

where $c_x$ and $c_y$ are two parameters determined by the initial state and can be expressed as

$$\begin{array}{cc}& c_x=x_0-\frac{v_0}{w_0}cos\left({\theta }_0\right)-\frac{a_0}{w_0^2}sin\left({\theta }_0\right),\hfill \end{array}$$

(16a)

$$\begin{array}{cc}& c_y=y_0+\frac{v_0}{w_0}sin\left({\theta }_0\right)-\frac{a_0}{w_0^2}cos\left({\theta }_0\right).\hfill \end{array}$$

(16b)

In particular, when the initial yaw rate $w_0=0$, the predicted trajectory changes to

$$tra{j}_{mdl}:\left\{\begin{array}{c}x\left(t\right)=(\frac{1}{2}·a_0·t^2+v_0)cos\left({\theta }_0\right)+x_0,\hfill \\ y\left(t\right)=(\frac{1}{2}·a_0·t^2+v_0)sin\left({\theta }_0\right)+y_0.\hfill \end{array}\right.$$

(17)

2.2.3. Trajectory Prediction Based on Both Driver’s Intention and Vehicle Kinematics Model

Since the predicted trajectory based on the kinematics model is more accurate only in a shorter prediction time, and the trajectory based on the driver’s intention has higher accuracy in a longer period of time, the predicted trajectory obtained by combining the two will be more accurate. Let the coefficient of the predicted trajectory based on the kinematics model be $\mathrm{w}\left(t\right)$, and the predicted trajectory model can be changed to

$$\begin{array}{c}\hfill traj\left(t\right)=\mathrm{w}\left(t\right)tra{j}_{mdl}\left(t\right)+(1-\mathrm{w}\left(t\right))tra{j}_{man}\left(t\right),\end{array}$$

(18)

where $\mathrm{w}\left(t\right)\in [0,1]$ is a time-varying variable that is designed to predict trajectory with high accuracy. Here, $\mathrm{w}\left(t\right)$ is designed and tuned with multiple simulations as

$$\mathrm{w}\left(t\right)=\left\{\begin{array}{cc}\frac{2}{27}{t}^3-\frac{1}{3}{t}^2+1\hfill & \mathrm{for}0\le \mathrm{t}<3,\hfill \\ 0\hfill & \mathrm{for}\mathrm{t}\ge 3.\hfill \end{array}\right.$$

(19)

Combined with the driver’s intention and the vehicle kinematics model, a more accurate predicted trajectory can be obtained. The results of trajectory prediction are shown in Figure 3. The trajectory that is predicted based on both operation intention and the vehicle kinematics model is more accurate than the other two methods.

3. Behavioral Planning

In the behavioral planning, by comprehensively considering the host vehicle information, road information, and surrounding environment information, an optimal behavioral trajectory is selected, which will be used for trajectory generation.

3.1. Generation of Candidate Paths

To reduce the size of the search space for trajectory planning and speed up the calculation, a series of candidate paths need to be generated first. The optimal path that does not collide is selected from the candidate paths. The candidate path is a candidate set that can represent the behavior that the vehicle can take in the planning process, and the road space of the entire prediction time is reasonably divided into multiple road segments. Comprehensively considering the prediction length and calculation time, the road space in the prediction time is divided into three road segments. The candidate points at the same s coordinates are called a layer of candidate point sets. The diagram of this division is shown in Figure 4. The road space occupied by each road segment can be represented as

$$s_i=\left\{\begin{array}{cc}{N}_i·\Delta s\hfill & \mathrm{for}\mathrm{v}>{\mathrm{v}}_{\min },\hfill \\ N_i·\Delta s_{min}\hfill & \mathrm{for}\mathrm{v}\le {\mathrm{v}}_{\min },\hfill \end{array}\right.$$

(20)

where $\Delta s$ refers to the distance traveled by the vehicle in 1 s, $s_i$ refers to the length of the $i_{th}$ road space. To prevent the predicted length from being too short, a minimum interval $\Delta s_{min}$ needs to be set so that the vehicle can plan the trajectory even if the current vehicle speed is too slow. The value of $N_i$ is set based on the comprehensive consideration of the calculation time and the predicted length. The step length near the current position is short and the one farther away from the current position is large.

All candidate roads together constitute a candidate set of trajectories in the future. Each layer of candidate point sets contains the center point of both the current lane and the adjacent lane and the lane change point of the two lanes. By connecting the points in the candidate point set, a series of path candidate sets can be generated. To further narrow the search range, considering that the vehicle has a high risk of completing the lane change operation in a short period of time, the set of candidate points of the first layer contains only the road center point of the current lane and the lane change point with the adjacent lane.

The path candidate set is a connection of a series of points in the search space, but not every road can be driven in the path candidate set. There will inevitably be static and dynamic obstacles on the road. Regardless of the dynamic obstacles, the paths that pass through the static obstacles are firstly removed. When driving along these paths, the vehicle will inevitably collide with a static obstacle at any time. After removing, all paths in the remaining set of paths candidates will become candidate paths in the behavior decision layer. Speed planning is needed for these candidate paths to select the optimal one.

3.2. Speed Profile

The selection of vehicle speed needs comprehensive consideration of traffic and road information and restrictions, vehicle restrictions, dynamic obstacles, and other information. Traffic road information mainly includes traffic lights, traffic signs, stop lines, maximum and minimum speed limits, etc, which is simply shown in Figure 5. When selecting the optimal speed sequence, traffic road information must be extracted as the first constraint. Since the influence of road traffic information on vehicle speed mainly acts on the road driving direction, that is, the S direction, the limit curve based on the traffic road information on the vehicle speed can be represented in a v-s profile.

The maximum lateral acceleration of the vehicle while driving needs to consider the physical limitations of the vehicle and the impact on comfort. Based on the maximum lateral acceleration and the road curvature, the speed limit based on the lateral acceleration can be calculated as

$$\begin{array}{c}\hfill v_{a_{y,max}}=\sqrt{\frac{a_{y,max}}{C_{Lane}}}.\end{array}$$

(21)

Since the road curvatures $C_{Lane}$ of adjacent lanes are the same, the limitation of the maximum lateral acceleration on the vehicle speed still only exists in the S direction. By comparing the value of the above two speeds, the limit curve of the vehicle speed in the S direction can be obtained as shown in Figure 5.

For dynamic obstacles, if time and space are considered at the same time, the problem of speed selection is a problem of optimal selection in S-L-T three-dimensional space, which is extremely high in calculation complexity. To reduce the dimension, each of the above candidate paths can be subjected to speed planning once. The coordinates L can be ignored to reduce the difficulty of calculation. An S-T profile is used to plan speed.

In an S-T profile, the T axis represents the time axis for predicting the future along the candidate road, and the S axis represents the space axis extending from the origin along the candidate road. We assume that the dynamic obstacles have constant acceleration within the prediction. The information on dynamic obstacles can be displayed in blue blocks and a safe distance is reserved too. The vehicle needs to travel in the space-time area corresponding to the blank grid. The origin of the S-T map is the current position of the host vehicle, and how to get to the target S position is the goal of speed planning.

Some restrictions and objective functions are necessary. First, the speed of the vehicle should be as fast as possible, which is calculated as

$$\begin{array}{c}\hfill f_v=|v_{max}\left(s_i\right)-v_i|.\end{array}$$

(22)

In addition, large acceleration will reduce driving comfort, so acceleration needs to be limited as

$$\begin{array}{c}\hfill f_a=\left|a_i\right|=\frac{|v_i-v_{i-1}|}{\Delta t}.\end{array}$$

(23)

Finally, it is necessary to avoid frequent acceleration and deceleration of the vehicle during driving, which is

$$\begin{array}{c}\hfill f_{jerk}=\frac{|a_i-a_{i-1}|}{\Delta t}=\frac{|v_i-2v_{i-1}+v_{i-2}|}{\Delta t^2}.\end{array}$$

(24)

In summary, the cost function is

$$\begin{array}{c}\hfill f_{speed}=w_v{f}_v+w_a{f}_a+w_j{f}_{jerk},\end{array}$$

(25)

where $w_v$, $w_a$ and $w_j$ are coefficients corresponding to $f_v$, $f_a$ and $f_{jerk}$. The optimal speed sequence for a certain behavior can be obtained through the cost function as shown in Figure 6.

3.3. Optimal Behavioral Trajectory Selection

Each candidate trajectory in the behavior planning candidate set represents a behavioral operation, and it is especially important to select the optimal one. It is necessary to consider efficiency, comfort, energy consumption, and other aspects.

First, the number of lane changes needs to be considered. $C_{LC}$ is a cost function factor affected by the number of lane changes. Since lane changing greatly increases energy consumption and greatly reduces comfort, it is necessary to avoid unnecessary and frequent lane changing operations. Thus,

$$\begin{array}{c}\hfill C_{LC}=\sum _{k=1}^{N_{layer}}|Lane\left(n_k\right)-Lane\left(n_{k-1}\right)|,\end{array}$$

(26)

where $N_{layer}$ represents the layer number, $n_k$ represents the $k_{th}$ candidate point. $Lane\left(n_{(·)}\right)$ represents the lane at $n_{(·)}$ candidate point.

In addition, since the S coordinate of the endpoint of each candidate route is the same, the shorter the travel time, the higher the efficiency. $C_T=T$ is only determined by the time. Frequent changes in behavior can reduce comfort and increase control difficulty. To reduce unnecessary changes in behavior planning, a consistency coefficient is introduced to indicate the difference between the candidate behaviors at the current time and the previously executed behavior. Thus, $C_{con}$ can be formulated as

$$\begin{array}{c}\hfill C_{con}=\sum _{j=1}^{N_b-k}[{(S_t^j-S_{t-\Delta t}^{j+k})}^2+{(L_t^j-L_{t-\Delta t}^{j+k})}^2].\end{array}$$

(27)

When generating the speed profile in the previous step, the candidate trajectory has been discretized. The step size after discretization is $\Delta t_{ST}$, and the total number of discrete points is $N_b=T/\Delta t_{ST}+1$. $\Delta t_{re}$ is the discrete step size of the model used for resampling performed. Then, $k=\Delta t_{re}/\Delta t_{ST}$. Therefore, $S_t^j$ and $L_t^j$ represent the coordinate values of the $j_{th}$ point on the candidate trajectory in Frenet coordinate system at time t. $S_{t-\Delta t}^{j+k}$ and $L_{t-\Delta t}^{j+k}$ represent the coordinate values of the $j_{th}$ point on the candidate trajectory in Frenet coordinate system at time t, which are the same corresponding point with $S_t^j$ and $L_t^j$. In this way, by constraining the difference between the same corresponding points in the two behavior planning paths taken at adjacent moments, the consistency and continuity of the behavior planning path can be constrained.

Last but not least, to make the vehicle location in the center of the road as much as possible while lane-keeping, $C_{boun}=\sum n_{boun}$ is used to represent the number of nodes where the vehicle is on the road boundary in the planned behavior trajectory, where $n_{boun}$ represents nodes where the vehicle is at the road boundary in the behavior trajectory.

In summary, a behavior path that is optimal in terms of efficiency, comfort, and energy consumption is selected according to the following cost function, and this behavior path is used as a reference for motion planning.

$$\begin{array}{c}\hfill C\left(t\right)=w_{LC}{C}_{LC}\left(t\right)+w_T{C}_T\left(t\right)+w_{con}{C}_{con}\left(t\right).\end{array}$$

(28)

3.4. Resampled Behavioral Trajectory

The step length of the behavior trajectory is longer. After selecting an optimal behavior trajectory, the trajectory needs to be converted from the Frenet coordinate system to the Cartesian coordinate system. The diagram of this process is shown in Figure 7. The step length of the candidate trajectory is $\Delta t_{ST}$, and the time sequence is $\Delta t_{ST}^0$, $\Delta t_{ST}^1$, …. The step size used in resampling is $\Delta t_{re}$, and the time sequence is $\Delta t_{re}^0$, $\Delta t_{re}^1$, …. Since $\Delta t_{ST}$ is larger than $\Delta t_{re}$, the optimal behavior trajectory needs to be interpolated to shorten the step size. In the process of interpolating, it is assumed that the vehicle speed remains unchanged within $\Delta t_{ST}$, and the number of $\Delta t_{re}$ contained in each $\Delta t_{ST}$ is $N=\Delta t_{ST}/\Delta t_{re}$. The time sequence of resampled trajectory can be written as $\Delta t_{ST}^0$, $\Delta t_{re}^{01}$, …, $\Delta t_{re}^{0(N-1)}$, $\Delta t_{ST}^1$, $\Delta t_{re}^{11}$, …, where $\Delta t_{ST}^i$ represents the time at the $i_{th}$ step, and $\Delta t_{re}^{ij}$ represents the $j_{th}$ resampled point in the $i_{th}$ step.

4. Trajectory Generation

With selected behavior from the behavioral planning module, nonlinear model predictive control is used in the trajectory generation module, which considers variational longitudinal velocity and dynamic vehicle model, and uses the C/GMRES algorithm to speed up the calculation process.

4.1. Vehicle Dynamic Model

To describe the vehicle dynamics characteristics more accurately, this paper uses a vehicle dynamics model. For the vehicle system, the coordinate system is the right-handed coordinate system, and the origin is the position of the center of mass of the vehicle. According to Newton’s second law, the dynamic characteristics can be represented as

$$\begin{array}{cc}\hfill 2(F_{yf}+F_{yr})& =m·a_y,\hfill \end{array}$$

(29a)

$$\begin{array}{cc}\hfill \dot{w}& =2\frac{F_{yf}·l_f-F_{yr}·l_r}{I_z},\hfill \end{array}$$

(29b)

where m is the mass of the vehicle and $I_z$ is the inertia of the vehicle. $F_{yf}$ and $F_{yr}$ are the lateral forces of the front wheel and rear wheel, respectively. $a_y$ is the lateral acceleration. $l_f$ and $l_r$ are the lengths from the center of mass to the front axle and the rear axle, respectively. w is the yaw rate. $\beta$ is used to represent the ratio of lateral speed to the longitudinal speed,

$$\begin{array}{c}\hfill \beta =\frac{v_y}{v_x}.\end{array}$$

(30)

From the geometric relationship, the slip angles of the front and rear wheels can be represented as

$$\begin{array}{cc}\hfill {\alpha }_f& =arctan\left(\frac{v_y+l_{f}w}{v_x}\right)\approx -\delta +\frac{w{l}_f}{v_x}+\beta ,\hfill \end{array}$$

(31a)

$$\begin{array}{cc}\hfill {\alpha }_r& =arctan\left(\frac{v_y-l_{r}w}{v_x}\right)\approx \beta -\frac{w{l}_r}{v_x}+\beta .\hfill \end{array}$$

(31b)

where $\delta$ is the steering angle. When the lip angle is small, the tire characteristics can be regarded as linear, which is calculated as

$$\begin{array}{cc}\hfill F_{yf}& =k_f{\alpha }_{yf},\hfill \end{array}$$

(32a)

$$\begin{array}{cc}\hfill F_{yr}& =k_r{\alpha }_{yr}.\hfill \end{array}$$

(32b)

The derivative of longitudinal acceleration is the acceleration of the vehicle.

Using $X={[x,y,\varphi ,v_x,v_y,w]}^T$ as the state vector of the vehicle dynamics system and $U={[a,\delta ]}^T$ as the input vector of the vehicle dynamics system, the state equation of the vehicle dynamics system is

$$\begin{array}{cc}\hfill \dot{X}& =f(X,U)=\left[\begin{array}{c}{v}_{x}cos\phi -v_{y}sin\phi \\ v_{x}sin\phi +v_{y}cos\phi \\ w\\ a\\ \frac{2k_f(v_f+w{l}_f-\delta v_x)+2k_r(v_y-w{l}_r)}{m{v}_x}-v_{x}w\\ \frac{2l_r{k}_f(v_y+w{l}_f-\delta v_x)-2l_r{k}_r(v_y-w{l}_r)}{I_z{v}_x}\end{array}\right].\hfill \end{array}$$

(33)

Transform the state equation of the continuous form vehicle dynamics system into a discrete form

$$\begin{array}{cc}& X_{k+1}=X_k+f(X_k,U_k)\Delta t.\hfill \end{array}$$

(34)

4.2. Controller Design

To complete the task of trajectory planning, it is necessary to rationally design the objective functions and constraints of model predictive control. The input value of the vehicle system cannot exceed the limit of the physical structure, so the upper and lower limits of the input variable need to be limited as

$$\begin{array}{cc}& |a_k|\le a_{max},\hfill \end{array}$$

(35a)

$$\begin{array}{cc}& |{\delta }_k|\le {\delta }_{max}.\hfill \end{array}$$

(35b)

To ensure that the planned trajectory does not collide with obstacles, it is necessary to maintain a certain safety distance between the trajectory at each moment and the obstacle at the corresponding moment. Since the longitudinal speed of the vehicle is often much higher than the lateral speed, a larger safe distance is needed in the longitudinal direction than in the lateral direction. The safety range around the host autonomous vehicle is designed as an ellipse, where the long axis of the ellipse is the longitudinal direction so that the safety constraint can be expressed as

$$\begin{array}{c}\hfill {\left(\frac{x_{host}-x_{obs}}{r_x}\right)}^2+{\left(\frac{y_{host}-y_{obs}}{r_y}\right)}^2\ge 1,\end{array}$$

(36)

where $r_x$ and $r_y$ represent safety distance in the longitudinal direction and the lateral direction, respectively. The larger the safety distance is, the further the host vehicle acts.

To make the final trajectory planning result close to the behavioral planning path, the following objective function is needed

$$\begin{array}{c}\hfill J_{coor}=w_1{(x_k-x_{ref,k})}^2+w_2{(y_k-y_{ref,k})}^2,\end{array}$$

(37)

where $x_k$ and $y_k$ are the coordinates of the trajectory at time-step k. $x_{ref,k}$ and $y_{ref,k}$ are the coordinates of the resampling behavioral planning path at time-step k.

As mentioned before, excessive acceleration and a small turning radius will greatly reduce driving comfort, and so it’s necessary to control their value of them.

$$\begin{array}{c}\hfill J_{com}=w_3{a}_k^2+w_4{\delta }_k^2.\end{array}$$

(38)

A terminal constraint is added to ensure the final trajectory matches the planned behavioral path better.

$$\begin{array}{c}\hfill J_{end}=\frac{1}{2}{(X_N-X_{ref,N})}^T{S}_f(X_N-X_{ref,N}).\end{array}$$

(39)

When there are two environment vehicles, the expression of the controller for motion planning is

$$\begin{array}{cc}\underset{{\lambda }_{M\times N}}{min}{J}_{U_{i-N}}\hfill & =\frac{1}{2}{(X_N-X_{ref,N})}^T{S}_f(X_N-X_{ref,N})+\sum _{k=1}^N\left(w_1{(x_k-x_{ref,k})}^2\right.\hfill \\ \hfill & \left.+w_2{(y_k-y_{ref,k})}^2+w_3{a}_k^2+w_4{\delta }_k^2\right)\Delta t\hfill \end{array}$$

(40a)

$$\begin{array}{cc}\hfill s.t.& {\dot{X}}_{k+1}=X_k+f(X_k,U_k)\Delta t,\hfill \end{array}$$

(40b)

$$\begin{array}{cc}& \left|a\right|\le a_{max},\left|\delta \right|\le {\delta }_{max},\hfill \end{array}$$

(40c)

$$\begin{array}{cc}& {(\frac{x_k-x_{obs,k}^j}{r_x})}^2+{(\frac{y_i-y_{obs,k}^j}{r_y})}^2\ge 1,j=1,2.\hfill \end{array}$$

(40d)

Since the vehicle dynamics model used is a non-linear model, a suitable solving algorithm is necessary. In this paper, a Continuation/GMRES algorithm is used to solve the nonlinear model predictive control problem.

4.3. C-GMRES

In the nonlinear model predictive control problem, the small sampling period of mechanical systems will bring a great burden to the computing platform. Based on Generalized Minimum Residual Method (GMRES), Ohtsuka introduced the concept of the Continuation Generalized Minimum Residual Method (C/GMRES), which solves the linear equations involved in the differential equations at each sampling instant, thereby solving the control input sequence [26]. The detailed C/GMRES algorithm that is used in this paper is depicted in Algorithm 1.

Algorithm 1 C/GMRES Algorithm

//Initialize $t=0$, $l=0$, initial state $x_0=x\left(0\right)$ and find $U_0$ analytically or numerically such that $\left|\right|F(U_0,x_0,0)\left|\right|\le \delta$ for some positive δ, maximum iteration number $k_{max}$. 1. For $t^{\prime }\in [t,t+\Delta t]$, compute the real control input by $u\left(t^{\prime }\right)=P_0\left(U_l\right)$. 2. At next sampling instant $t+\Delta t$, measure the state $x_{l+1}=x(t+\Delta t)$, set $\Delta x_l=x_{l+1}-x_l$. 3. $\dot{U_l}=FDGMRES(U_l,x_l,\Delta x_l/\Delta t,t,{\widehat{\dot{U}}}_l,h,k_{max})$, where ${\widehat{\dot{U}}}_l={\widehat{\dot{U}}}_{l-1}$ with ${\widehat{\dot{U}}}_{-1}=0$. 4. $U_{l+1}=U_k+{\widehat{U}}_k+\Delta t$. 5.  Update $t=t+\Delta t$, $k=k+1$.

To solve the trajectory planning problem, first, the dummy inputs are used to convert inequality constraints into equality constraints.

$$\begin{array}{cc}& ({\delta }_k^2+u_{d1,k}^2-{\delta }_{max}^2)/2=0,\hfill \end{array}$$

(41a)

$$\begin{array}{cc}& (a_k^2+u_{d2,k}^2-a_{max}^2)/2=0,\hfill \end{array}$$

(41b)

$$\begin{array}{cc}& {(\frac{x_k-x_{obs,k}^1}{r_x})}^2+{(\frac{y_k-y_{obs,k}^1}{r_y})}^2-1-u_{d3,i}^2=0,\hfill \end{array}$$

(41c)

$$\begin{array}{cc}& {(\frac{x_k-x_{obs,k}^2}{r_x})}^2+{(\frac{y_k-y_{obs,k}^2}{r_y})}^2-1-u_{d4,i}^2=0.\hfill \end{array}$$

(41d)

Meanwhile, to prevent multiple solutions of dummy inputs, adding a small dummy penalty to the objective function

$$\begin{array}{cc}\hfill \underset{{\lambda }_{M\times N}}{min}{J}_{U_{i-N}}=& \frac{1}{2}{(X_N-X_{ref,N})}^T{S}_f(X_N-X_{ref,N})+\sum _{k=1}^N\left(w_1{(x_k-x_{ref,k})}^2\right.\hfill \\ & \left.+w_2{(y_k-y_{ref,k})}^2+w_3{a}_k^2+w_4{\delta }_k^2-w_{U_d}{U}_{d,k}\right)\Delta t\hfill \end{array}$$

where $w_{U_d}$ is a small positive constant.

Then, define the Hamiltonian by

$$\begin{array}{c}\hfill H(x,\lambda ,u,\mu ,p)=L(x,u,p)+{\lambda }^{T}f(x,u,p)+{\mu }^{T}C(x,u,p),\end{array}$$

(42)

where $\lambda \in {\mathbb{R}}^n$ represents costate and $\mu \in {\mathbb{R}}^{m_c}$ represents language multiplier. $m_c$ represents the dimension of constraints.

For an optimal control ${\left\{u_k^{*}\right\}}_{k=0}^{N-1}$, it exists ${\left\{{\lambda }_k^{*}\right\}}_{k=0}^N$ and ${\left\{{\mu }_k^{*}\right\}}_{k=0}^{N-1}$, the following conditions should be satified

$$\begin{array}{cc}& x_{k+1}^{*}=x_k^{*}+f(x_k^{*},u_k^{*},p_k)\Delta t,\hfill \end{array}$$

(43a)

$$\begin{array}{cc}& {\lambda }_k^{*}={\lambda }_{k+1}^{*}+H_x^T(x_k^{*},{\lambda }_{k+1}^{*},u_k^{*},{\mu }_k^{*},p_k)\Delta t,\hfill \end{array}$$

(43b)

$$\begin{array}{cc}& {\lambda }_N^{*}={\phi }_x^T(x_N^{*},p_N),\hfill \end{array}$$

(43c)

$$\begin{array}{cc}& x_k^{*}=x\left(t_0\right),\hfill \end{array}$$

(43d)

$$\begin{array}{cc}& H_u(x_k^{*},{\lambda }_{k+1}^{*},u_k^{*},{\mu }_k^{*},p_k)=0,\hfill \end{array}$$

(43e)

$$\begin{array}{cc}& C(x_k^{*},u_k^{*},p_k)=0.\hfill \end{array}$$

(43f)

Here, Equaton (43) can be summarized as

$$\begin{array}{cc}\hfill F\left(U\right(t),x(t),t)& =\left[\begin{array}{c}{H}_u^T(x_0^{*},{\lambda }_1^{*},u_0^{*},{\mu }_0^{*},p_0)\\ C(x_0^{*},u_0^{*},p_0)\\ \dots \\ H_u^T(x_{N-1}^{*},{\lambda }_N^{*},u_{N-1}^{*},{\mu }_{N-1}^{*},p_{N-1})\\ C(x_{N-1}^{*},u_{N-1}^{*},p_{N-1})\end{array}\right],\hfill \\ & =0,\hfill \end{array}$$

(44)

Then, $\dot{F}(U,x,t)$ can be expressed as

$$\begin{array}{c}\hfill \dot{F}(U,x,t)=A_{s}F(U,x,t).\end{array}$$

(45)

Here, $A_s$ is an introduced stable matrix that stabilizes $F(U,x,t)$ at the origin. Then, $\dot{U}$ can be computed with

$$\begin{array}{c}\hfill \dot{U}=F_U^{-1}(A_{s}F-F_x\dot{x}-F_t).\end{array}$$

(46)

The solution curve $U\left(t\right)$ is approximated by forward difference if an initial solution $U\left(0\right)$ satisfying $F\left(U\right(0),x(0),0)=0$ can be found. Here, generalized minimal residual (GMRES) method is applied to solve the linear equation $F_U\dot{U}=A_{s}F-F_x\dot{x}-F_t$. The combination of forward difference approximation and GMRES is called FDGMRES.

5. Simulation

In this section, the proposed computational efficient motion planning method is verified in three different environments. According to the information of the obstacle and the predicted trajectory, the behavior is selected, and the trajectory is optimized using NMPC, which is fast solved by the C/GMRES algorithm.

5.1. Obstacle Avoidance on Straight Lane

As shown in Figure 8, the host vehicle travels on a straight lane with an initial speed of 10 m/s. The maximum speed limit of the road is 15 m/s and each lane width is 4m. An obstacle vehicle is 30 m ahead of the host vehicle at speed of 5 m/s. The trajectory of the host vehicle and the obstacle vehicle is shown in Figure 8a. The speed profile of two vehicles and the steering wheel angle of the host vehicle is shown in Figure 8b,c. In this driving scenario, the host vehicle executes two consecutive lane-changing operations smoothly and quickly to avoid dynamic obstacles and keep the speed under the maximum speed.

The processor of the computer is Intel (R) Core (TM) i7-6700hq CPU@ 2.60GHZ. The time step size $\Delta t$ for simulation is 0.05s and the nonlinear model predictive control problem using the C/GMRES algorithm, which is also compared with fmincon function in MATLAB. As shown in Figure 9, the calculating time of solving is much less than the time step size $\Delta t$, and also much less than the calculating time of the fmincon function in MATLAB which is more than 10 min. It shows that the local path planning module solved by C/GMRES can better meet the requirements of solving speed.

5.2. Obstacle Avoidance on Winding Lane

In the scenario of a winding road, the center line of the initial lane is $y={10}^{-6}{x}^3+{10}^{-5}{x}^2$ The obstacle vehicle travels at a speed of 5 m/s at 50 m in front of the host vehicle. The trajectory and speed profile of the two vehicles and the steering wheel angle of the host vehicle is shown in Figure 10. The host vehicle chooses to accelerate to overtake the preceding vehicle and avoid the obstacle.

5.3. Lane-Changing Obstacle Avoidance

A much more complex scenario is verified in the third simulation, in which the intention of the obstacle changed. The trajectory of the obstacle vehicle is selected from the open data set, which executes a lane change to the left lane. The trajectory and speed profile of the two vehicles and the steering wheel angle of the host vehicle is shown in Figure 11.

As shown in Figure 11a, at first, the obstacle vehicle chooses lane-keeping before changing lanes to the left lane. In this process, the host vehicle tries to change lanes. After the obstacle vehicle decides to change to the target lane of the host vehicle, the host vehicle decides to change back to its original lane. From the above simulation results, the proposed motion planning method considers safety, computational efficiency, and comfort simultaneously and obtains good system performance.

6. Conclusions

This paper proposes a computationally efficient motion planning method for autonomous vehicles, which considers dynamic obstacle avoidance and traffic interaction. The decision process for complex behavior is reasonably simplified in both time and space span. Different points located on unequally divided road segments and lane centerlines are connected to represent behavior. And C/GMRES algorithm is used to accelerate the calculation of the NMPC problem in the trajectory generation module. The trajectories of other traffic participants are more accurately predicted with known intention and vehicle models, which enables the movement to be more reasonably planned. Finally, three groups of simulation experiments are carried out to verify the rationality and superiority of the algorithm.

In future works, the interactive intention prediction will be considered in the intention predictive layer, which can extend the motion planning method from reacting adaptively to predicting adaptively. By considering the interactions between the ego vehicle and surrounding drivers socially via implicit and/or explicit communications, the behavior of the autonomous vehicle can be more human-like and facilitate safety performance under complex and dynamic environments [27].