Automated Vehicle’s Overtaking Maneuver with Yielding to Oncoming Vehicles in Urban Area Based on Model Predictive Control

Abstract

The rapid development of automated driving technology has brought many emerging technologies. The collision avoidance (CA) function by braking and/or steering maneuver of advanced driver assistance systems (ADAS), which contributes to the improvement of the safety of automated vehicles, has been playing an important role in recent modern passenger cars and commercial vehicles. When an automated vehicle needs to avoid multiple obstacles at the same time, consuming travel time and safety assurance of CA need to be carefully considered especially in the case related to unpredictable motion of obstacles. This paper proposes a feasible solution to this situation by controlling speed and the steering wheel angle. The proposed motion re-planning based on post-encroachment time (PET) provides a judgment of a function which calculates the possibility of unavoidable road accidents. Then the path re-planning layer of a novel two-layer model predictive control (TL-MPC) will re-plan a local trajectory and give a reference acceleration. Finally, the path tracking layer outputs steering wheel angle to follow the trajectory under the premise of ensuring safety constraints. The proposed control system is evaluated by co-simulations of MATLAB/Simulink and CarSim software. The results show that for various conditions of post-encroachment time, the ego vehicle adopting the proposed strategy will conduct reasonable behavior re-planning and consequently successfully avoid obstacles.

1. Introduction

As the rapid innovation of microprocessor technology continues, more investment in the research and development of vehicle electronic control technology in the context of automated driving has been conducted [1,2,3,4]. Road fatalities has been one of the major death causes globally [5], and it is projected to become the fifth leading cause of death by 2030 [6]. In the same report, it is noted that the human factor is the main reason for road accidents globally, with Hamlet et al. [7] suggesting that 93% of road accidents were caused by human error and driver inattentiveness. Nevertheless, road accidents can be prevented by assimilating automation components into vehicle driving systems, thus reducing human error by controlling the vehicle in ways similar to skillful and careful drivers [8,9]. Therefore, vehicle active safety control technologies, such as advanced driver assistance systems (ADAS) that help the driving process automation are playing an important role in modern passenger cars and commercial vehicles. One of the main features of ADAS is the collision avoidance (CA) system. A sufficient CA architecture usually encompasses threat assessment, path planning and path tracking strategies [10]. As road collisions might happen in various scenarios, a successful CA system should have the ability to guide the avoidance navigation of the host vehicle in multi-scenario hazards. For example, collisions at intersections, high-speed highway collisions, CA in crowded urban areas as well as collision mitigation of a priori unknown obstacles [11,12]. There are at least two common types of CA design control architecture which have been proposed and implemented. For example, a multi-layer CA system [13], which contains both trajectory re-planning layer and tracking layer [14]. This is the usual type of CA implementation and is also known as a guidance and navigation control system [15].

In addition, optimization algorithms based on different mathematical theories have been proposed and applied in the CA system. Threat assessment strategies provide an online evaluation of the potential threat to assist drivers in avoiding or evading collisions [16]. According to Gray et al. [17], threat assessment can be defined as a real-time judgment of a function which calculates the possibility of an unavoidable road accident. A path planning (PP) strategy as part of the full CA architecture can be widely used in the field of road vehicles and mobile robots. It is formulated to acquire a collision-free path while the vehicle independently navigates itself [12]. In the condition of the avoidance of single or multiple static obstacles, many studies have been carried out. Inspired by viscous fluid flowing between two parallel plates or around a cylinder, Cheng et al. proposed virtual fluid-flow-model based lane-keeping integrated with a collision avoidance control system to realize the function of both lane-keeping and collision avoidance [18]. The predictive nature and constraint handling capabilities of model predictive control (MPC) make it an attractive framework for leveraging these new technologies, and it is also adopted in CA [15,19]. A machine learning approach, such as the artificial neural network [20,21,22] and collision cone approach [23], depends on the driver behavior and the environment instead of the mapping information alone, making it reliable in the context of dynamic CA. The advantage of this method is, once a solution is found, it will be a shorter path. However, getting stuck in local minima is also an issue that should be addressed [24]. At the same time, its drawback is it requires a large trial to obtain the results, thus the computational time is also an issue [25].

However, the literatures mentioned above mostly focuses on the fixed obstacles, like [19], or moving obstacles in the same direction as the ego vehicle, like [26], and the speed of the ego vehicle and obstacles remains a constant. Most studies are based on the fact that the ego vehicle can safely avoid obstacles while maintaining the current speed. However, in the actual obstacle avoidance process, obstacles are usually dynamic. In this situation, the safest obstacle avoidance strategy is to require the ego vehicle to decelerate or even stop. Considering the travel time cost or convenience, we hope that the ego vehicle can avoid obstacles in accordance with the predetermined requirements under the premise of ensuring safety. In other words, if the vehicle can avoid obstacles at a constant speed or by acceleration, deceleration is unnecessary, which requires that the speed of the ego vehicle and trajectory re-planning is real-time controllable during obstacle avoidance. Focusing on such problems, this research proposes a novel obstacle avoidance strategy that contains behavior re-planning based on post-encroachment time (BR-PET) and trajectory re-planning and tracking based on a two-layer MPC system. The BR-PET module will indicate the ego vehicle’s speed change based on the initial in-formation of ego vehicle and the static obstacle and the dynamic obstacle and the speed and acceleration limits of the ego vehicle and the uncertain dynamic obstacle. At the same time, after selecting different dynamic vehicle models of different complexity for two different MPCs used in the path re-planning layer and tracking layer, the real-time performance is also guaranteed.

The remainder of this paper is organized as follows: the scenario targeted by the strategy and the challenges of this research are presented in Section 2. Section 3 gives the principle of behavior re-planning and a detailed description about the design of the TL-MPC on trajectory re-planning layer and tracking layer. The performance of the proposed strategy is tested by CarSim-Matlab/Simulink co-simulation in Section 4. Section 5 is the conclusions.

2. Scenario

In the proposed scenario, shown as Figure 1, the dotted line 3 is the trajectory of the oncoming vehicle and the dotted line 1 is the initial reference trajectory (also known as the global reference trajectory) of the ego vehicle. Due to the existence of the parked vehicle and oncoming vehicle, the ego vehicle needs to alternate the local trajectory planning (shown as the dotted line 2) at this stage. Considering the different initial position and the speed change in the process of the oncoming vehicle’s motion, it may be not safe for the ego vehicle if it still maintains the current speed or accelerates within the allowable speed boundaries or handling stability requirements. Therefore, it is inevitable for the ego vehicles to decelerate in order to handle the driving situation with guaranteed safety. Considering the dynamic obstacles like oncoming vehicles in the opposite lane, especially when future speed of the obstacle is unpredictable, it is difficult for the controlled ego vehicle to behave in a similar way of judgment as a skillful and careful human driver. Therefore, how to re-plan the motion behavior and local reference trajectory is a question worth pondering.

Before introducing a collision avoidance strategy, it is necessary to refine the scenario and establish some restrictions. This research aims to propose an upper-level control algorithm to improve the safe avoidance of unpredictable obstacles. By default, the relative position relationship between the vehicles and the real-time speed of the oncoming vehicle can be accurately measured by automotive radar.

As shown in Figure 2, A represent the ego vehicle, B is the parked vehicle and C is the oncoming vehicle. A1, C1 are the positions when the ego vehicle and the oncoming vehicle meet with each other (the same for A2, C2). L_a and L_b are defined as the distance between the ego vehicle and the front/rear side of the parked vehicle, respectively. L_c is defined as the distance between the oncoming vehicle and the front side of the parked vehicle. $D_{safety}$ is a safe distance which is used as a reference for speed control of the ego vehicle. Unless otherwise stated, the units of measurement for distances is meter in this paper. We consider that:

(1)

L_a, L_b are global fixed parameters for the scenario, that means the initial relative position of the ego vehicle and the parked vehicle are not changed in any simulation;

(2)

L_c is treated as a local invariant, that means the initial relative position of the ego vehicle and the parked vehicle will only be changed as the initial condition of a simulation;

(3)

$0\mathrm{m}/\mathrm{s}\le v_a\le 15\mathrm{m}/\mathrm{s}$, $10\mathrm{m}/\mathrm{s}\le v_c\le 15\mathrm{m}/\mathrm{s}$;

(4)

$-3\mathrm{m}/{\mathrm{s}}^2\le a_{sc}\le 1\mathrm{m}/{\mathrm{s}}^2$, $-3\mathrm{m}/{\mathrm{s}}^2\le a_{OnC}\le 1\mathrm{m}/{\mathrm{s}}^2$.

Where $v_a$, $a_{sc}$ represent the speed and acceleration/deceleration of the ego vehicle respectively. $v_c$, $a_{OnC}$ represent the speed and acceleration/deceleration of the oncoming vehicle respectively. $D_{safety}$ is a distance used to determine a safe passing position. It is obvious that such a scene often occurs in crowded urban traffic conditions.

The behavior of the ego vehicle will be re-planned by the calculated passing position, shown as follows:

(1)

If the passing position is far enough away from the parked vehicle, as shown in Figure 2a at passing position 1, the ego vehicle will choose to keep speed unchanged or accelerate according to the oncoming vehicle speed $v_{c1}$ and position $L_{c1}$;

(2)

Due to excessive speed $v_{c1}$ or short initial distance $L_{c2}$ of the oncoming vehicle, even if the ego vehicle accelerates to the maximum allowable speed, it cannot guarantee that their passing position is at a safe position, as shown in Figure 2b at passing position 2. The ego vehicle will decelerate until oncoming vehicle has already passed, shown as Figure 3, then it restarts and overtakes the parked vehicle.

The problem describes how to calculate the passing position, control speed and re-plan the trajectory in real time. The position can be simply obtained by the initial state of three vehicles when their speed remains constant during the whole process. However, if the future speed of the oncoming vehicle is unpredictable, the passing position will not be defined as a fixed position. Thus, it is necessary to adopt a more intuitive safety indicator. The problem of balancing the consuming travel time and ensuring the safety need be solved in the behavior re-planning part. In addition, the module that conducts path re-planning and tracking tasks needs to adopt a vehicle dynamics model with reasonable boundary constraints in terms of physical capability to ensure real-time computation ability.

3. Collision Avoidance Strategy

The control flow of the proposed CA system is shown in Figure 4. First, the oncoming vehicle’s predictive state is calculated based on its real-time position and velocity. Then the recognition layer will determine the movement (i.e., acceleration, constant speed or deceleration) of the ego vehicle in the whole obstacle avoidance process based on the PET calculated from the initial state of the ego vehicle and obstacles. Based on calculated PET, the vehicle control unit (VCU) will determine the action (keeping speed unchanged or acceleration or deceleration) of the next step. With the MPC controller 1 of the re-planning layer, the local reference path will be passed to the tracking layer in the form of fitted curve parameters and reference longitudinal acceleration is obtained and output to the tracking layer at the same time. Finally, in the operation layer, which is also named the tracking layer, the optimal front wheel steering angle and reference speed are calculated by the vehicle dynamics model and MPC controller 2. Then they are output to Carsim module. In the next step, the state of the ego vehicle output by Carsim will update the data in the recognition layer. More detailed instructions will be expanded in the following subsections.

3.1. Behavior Replanning Based on Post-Encroachment Time (PET)

Based on a safety indicator, this module is used to make behavior planning for the entire obstacle avoidance process for the ego vehicle. A widely used safety indicator of obstacle avoidance is time to collision (TTC) [27]. It indicates the time span left before two vehicles collide, if nobody takes evasive action. However, this method is based on the speed and heading angle of the vehicle at the current time, and does not take into account the acceleration of the vehicle, resulting in large judgment error. PET is another indicator that is used to estimate the criticality of a certain traffic situation. It has been defined as the elapsed time between the front of the lead vehicle passing a point on the roadway and the front of the following vehicle passing the same point [28]. This research adopts PET as a safety indicator for behavior re-planning due to the variability of the oncoming vehicle’s speed.

The site for calculating PET is set at the front side of the parked vehicle, shown as Line 1 in Figure 5. Here the PET represents the time difference between A and C passing through Line 1. As the scenario is the congested urban road with unpredictable future speed of oncoming vehicle, the safety of the ego vehicle in the process of obstacle avoidance is the highest priority. Therefore, we assume that the oncoming vehicle will accelerate to the allowable maximum speed without deceleration even when it is close to the ego vehicle. The safe distance $D_{safety}$ is set as $20\mathrm{m}$.

The equivalent PET is defined as:

$$PE{T}_{safe}^{max}=D_{safety}/v_{a\_ini}+D_{safety}/v_{c\_max}\approx 3.33s\to 3.5\mathrm{s}$$

(1)

$$PE{T}_{safe}^{min}=D_{safety}/v_{a\_max}+D_{safety}/v_{c\_max}\approx 2.66s\to 3\mathrm{s}$$

(2)

where $PE{T}_{safe}^{max}$ defines the maximum time difference between two vehicles passing through the set site while $PE{T}_{safe}^{min}$ defines the minimum time difference. These two items are close to each other so that the safe PET is set as 3.5 s for convenience. In order to visualize the result, the case where the initial speed of the ego vehicle is 11 m/s is selected. The result can be easily extended to the entire allowed speed span of the ego vehicle.

As Figure 6 shows, the behavior re-planning module is obtained by the following steps. Firstly, after traversing all allowable speeds and allowable positions of the oncoming vehicle, the PET under the condition of the ego vehicle keeping the initial velocity unchanged ($PE{T}_{s1}$) and accelerating ($PE{T}_{s2}$) will be calculated separately. Then, the initial data of oncoming vehicle and calculated PET will be stored as two different arrays according to whether the ego vehicle needs to accelerate. In Figure 6, the blue surface consists of point set $S1$ while the red surface consists of point set $S2$. Finally, the logic of behavior re-planning is as follows:

Case 1: If $PE{T}_{s2}$ > $PE{T}_{safe}$, then ego vehicle will keep speed as initial speed unchanged in the whole avoidance process.

Case 2: If $PE{T}_{s2}$ < $PE{T}_{safe}$ and $PE{T}_{s1}$ > $PE{T}_{safe}$ which means the ego vehicle cannot avoid obstacles by an unchanged speed unless acceleration, then ego vehicle will be required to accelerate.

Case 3: If $PE{T}_{s2}$ < $PE{T}_{safe}$ and $PE{T}_{s1}$ < $PE{T}_{safe}$ which means even if the ego vehicle accelerates to maximum allowable speed, it cannot avoid obstacles safely, then the ego vehicle will be required to decelerate.

3.2. Two-Layer Model Predictive Control (MPC) Modeling

After behavior is re-planned, the global reference path should be re-planned based on the real-time relative position of the ego vehicle and obstacles. Then the re-planned path will be tracked.

When building a vehicle mathematical model, it is necessary to comprehensively consider factors such as performance and calculation speed. Due to the large amount of calculation bring by the algorithm of the re-planning layer, the main purpose of introducing MPC is to ensure the re-planning results meet the constraints of vehicle dynamics and kinematics. Therefore, in order to reduce the calculation, there is no advantage when choosing a complex model. A comparison conducted proved that adopting a relatively lower precision model at the re-planning layer and a relatively higher precision model at the tracking layer can balance control performance and calculation speed. Therefore, a bicycle model and a point mass model are adopted at the re-planning layer and the tracking layer, respectively.

The core work of this layer is to design a reasonable evaluation function to realize the obstacle avoidance and reduce the deviation between the re-planned path and global reference path while satisfying the constraints. Then, the local reference path fitting function and the reference longitudinal acceleration are output to the tracking layer. In this layer, we define the cost function as follows:

$$J_1={\displaystyle \sum }_{i=1}^{N_{p,1}}\Vert {\eta }_1\left(t+i|t\right)-{\eta }_{ref,global}{\left(t+i|t\right)\Vert }_{Q_1}^2+{\displaystyle \sum }_{i=1}^{N_{c,1}}\Vert U_i\left(t+i|t\right){\Vert }_{R_1}^2+J_{ob}$$

(3)

where ${\eta }_1\left(t+i|t\right)=Y\left(t+i|t\right)$ is the output vector. ${\eta }_{ref,global}\left(t+i|t\right)$, with $i=1,\dots ,N_{p,1}$, is the initial trajectory reference, for the output tracking variable.$U_i={\left[a_x{a}_y\right]}^T$ is control vector.$N_{p,1}$, $N_{c,1}$ are the prediction horizon in replanning layer and the control horizon in re-planning layer, respectively. $Q_1\in {ℝ}^{p_{y,1}\times p_{y,1}},R_1\in {ℝ}^{m_1\times m_1}$ are weighting matrices. $J_{ob}=J_{Pariking}+J_{road}+J_{OnC}$ is the total cost of obstacles avoidance. The above variables and constraints will be explained in detail in the following subsections. In this function, the first term suppresses the lateral deviation and heading deviation of the ego vehicle relative to the reference trajectory. The second item requires ego vehicles to keep speed unchanged as far as possible, which also ensures the comfort of the vehicle.

In this layer, the vehicle is treated as a point with a given mass m, i.e., no orientation is defined and the yaw dynamics is neglected [29], shown as Figure 7.

The state of motion and the state of force can be expressed as:

$$\dot{X}=\dot{x}\cos \phi -\dot{y}\sin \phi$$

(4)

$$\dot{Y}=\dot{x}\sin \phi +\dot{y}\cos \phi$$

(5)

$$m{a}_y=F_y$$

(6)

$$m{a}_x=F_x-F_d$$

(7)

where subscript $\left[x,y\right]$ is the [longitudinal, lateral] axle in the vehicle body frame. $X,Y$ are the Cartesian coordinates that belong to the world axis frame. $F_x$,$F_y$ are the resultant force in longitudinal and lateral axles in the vehicle body frame, respectively.$F_d=m{a}_d$ represents the resultant force of resistance in the longitudinal axle.

The maximum tire forces should be constrained by the maximum adhesion provided by ground. In particular, for a given friction coefficient $\mu$ and the tire normal force $F_z$, the lateral and longitudinal forces are constrained as follows:

$$F_x^2+F_y^2\le {\left(\mu F_z\right)}^2$$

(8)

Therefore the vehicle dynamics described by the Equations (4)–(7) can be rewritten in the following compact from:

$$\begin{array}{c}\dot{\kappa }\left(t\right)=f^{pm}\left(\kappa \left(t\right),u\left(t\right)\right)\\ s.t.\\ u_{min}\le u\left(t\right)\le u_{max}\\ a_x^2+a_y^2\le {\left(\mu g\right)}^2\end{array}$$

(9)

where the state vector is $\kappa ={\left[\dot{y}\dot{x}\phi YX\right]}^T$ and the input vector is $u={\left[a_x{a}_y\right]}^T$.

Since the point-mass vehicle model ignores the information of body size, the obstacles should be expanded. At the same time, the obstacle needs to be segmented according to a certain resolution to prevent the vehicles passing through the obstacle when its size is too large, shown as Figure 8.

The principles of processing of road boundary and the parked vehicle are as follows:

(1)

The actual boundary of the road should be preprocessed according to the width of the ego vehicle;

(2)

The parked vehicle is decomposed into several discrete small obstacles;

(3)

The cost of the function is adjusted by the vehicle speed and the distance deviation between the obstacle points and the vehicle.

The cost function of the distance between the ego vehicle and parked vehicle is established as:

$$J_{Parking}={\displaystyle \sum }_{i=1}^{N_{p,1}}{\displaystyle \sum }_{n=1}^{N_1}\frac{S_1}{{\left(X_i-X_{0,i}\right)}^2+{\left(Y_i-Y_{0,i}\right)}^2+{\zeta }_1}$$

(10)

where $S_1$ is the weight coefficient. ${\zeta }_1$ is a small positive number that prevents the denominator from being zero.$N_1$ is total number of copies after the parked vehicle is segmented.$X_{0,i}$ is the predictive position of the ego vehicle at step $i$.

Moreover, we consider that the road is divided into two opposite lanes and the global reference path is in one of them, shown as Figure 9. Therefore, in order to eliminate the deviation between the actual trajectory and the reference trajectory, the cost function of the road part is adjusted by the relative position between the ego vehicle and the road bounds.

$$J_{road}=\{\begin{array}{c}0,D_{sc2Road,i}\ge D_{\min };\\ {\displaystyle {\displaystyle \sum }_{i=1}^{N_{p,1}}}\frac{S_2}{D_{sc2Road,i}+{\zeta }_2},D_{sc2Road,i}<D_{\min }\end{array}$$

(11)

where $S_2$ is the weight coefficient.${\zeta }_2$ is a small positive number that prevents the denominator from being zero. $D_{\min }$ is the allowable minimum distance between the ego vehicle and the road bounds.$D_{sc2Road,i}$ is defined as:

$$D_{s\mathrm{c}2Road,i}=\{\begin{array}{c}\left|Y_{sf,i}-R_{ub}\right|,Y_{sf}-R_{ub}\le Y_{sf}-R_{lb};\\ \left|Y_{sf,i}-R_{lb}\right|,Y_{sf}-R_{ub}\ge Y_{sf}-R_{lb}\end{array}$$

(12)

where $Y_{sf}$ is the longitudinal coordinate of the ego vehicle in world axis frame. $R_{\left[ub,lb\right]}$ is the [upper, lower] bound constraint of the road.

Similar to a parked vehicle and taking the position prediction of the oncoming vehicle into account, the cost function of oncoming vehicle part is defined as:

$$J_{OnC}={\displaystyle \sum }_{i=1}^{N_{p,1}}{\displaystyle \sum }_{n=1}^{N_2}\frac{S_3}{D_{sc2mo}{|}_{n,T=i}+{\zeta }_3}$$

(13)

where $S_3$ is the weight coefficient. ${\zeta }_3$ is a small positive number that prevents the denominator from being zero. $N_2$ is total number of copies after the oncoming vehicle is segmented. $D_{sc2mo}{|}_{n,T=i}$ is defined as:

$$D_{sc2mo}{|}_{n,T=i}={\left(X_{n,i}-X_{0,i}\right)}^2+{\left(Y_{n,i}-Y_{0,i}\right)}^2$$

(14)

where $X_{n,i}$ is the predictive position of oncoming vehicle’s $n$th block after being segmented at step $i$.

When combining road, parked vehicle and the oncoming vehicle, shown as the Figure 10, we solve the following optimization problem at each time step:

$$\begin{array}{c}\underset{{\mathrm{U}}_{\mathrm{t}}}{\min }{J}_1\left(\kappa \left(t\right),u\left(t\right)\right)\\ s.t.\\ u_{min}\le u\left(t\right)\le u_{max}\\ a_x^2+a_y^2\le {\left(\mu g\right)}^2\end{array}$$

(15)

With the non-linear problem solver, the solution $U_t^{*}={\left[u_{t,t}^{*},\dots ,u_{t+N_{c,2},t}^{*}\right]}^T$ is obtained. Then, with the $u_{t,t}^{*}={\left[a_{x,t}^{*}{a}_{y,t}^{*}\right]}^T$, the predictive state of the ego vehicle from the time $t$, that is $\left[X_{0,t+i|t}{Y}_{0,t+i|t}{\phi }_{0,t+i|t}\right]$ with $i=1,\dots ,N_{p,2}$, is obtained by Equations (4)–(7).

3.2.1. Polynomial Fitting

The re-planned path is given by discrete points in the prediction time domain. If these points are directly input into the tracking layer, input interfaces will be greatly occupied. As the prediction horizon changes, the number of points will also be changed, which is not conducive to the standardized design of the controller. Besides, the control period of re-planning layer is not the same as that of tracking layer, which is difficult for the controller of the tracking layer to track discrete reference trajectory points [30].

In order to reduce the calculation of the controller, meet the continuity requirements of position change, yaw angle change and acceleration change and reduce impact, a fifth-order polynomial is adopted as the fitting curve.

$$\begin{array}{c}{Y}_{ref}=a_0{t}^5+a_1{t}^4+a_2{t}^3+a_3{t}^2+a_{4}t+a_5\\ {\phi }_{ref}=b_0{t}^5+b_1{t}^4+b_2{t}^3+b_3{t}^2+b_{4}t+b_5\end{array}$$

(16)

The output of the re-planning layer are coefficients of the fifth-order polynomial, $a_p=\left[a_0{a}_1{a}_2{a}_3{a}_4{a}_5\right]$ and $b_p=\left[b_0{b}_1{b}_2{b}_3{b}_4{b}_5\right]$, and the reference longitudinal acceleration $a_{x,ref}$.

3.2.2. Reference Trajectory Tracking Layer

The target of the MPC controller in this layer is tracking the local reference trajectory re-planned by the MPC controller of trajectory re-planning layer. Similar to the re-planning layer, the cost function is defined as:

$$\begin{array}{c}\underset{\Delta U_t,\epsilon }{\min } J_2\left(\xi \left(t\right),u\left(t-1\right),∆U\left(t\right),\epsilon \right)=\\ {\displaystyle {\displaystyle \sum }_{i=1}^{N_{p,2}}}\Vert {\eta }_2\left(t+i|t\right)-{\eta }_{ref,local}{\left(t+i|t\right)\Vert }_{Q_2}^2+{\displaystyle {\displaystyle \sum }_{i=1}^{N_{c,2}-1}}\Vert \Delta U{\left(t+i-1|t\right)\Vert }_{R_2}^2+\rho {\epsilon }^2\end{array}$$

(17)

where ${\eta }_{ref,local}\left(t+i|t\right)$, with $i=1,\dots ,N_{p,2}$, is the reference, which is generated by re-planning layer, for the output tracking variable. $U={\delta }_f$ is control vector.$Q_2\in {ℝ}^{p_{y,2}\times p_{y,2}},R_2\in {ℝ}^{m_2\times m_2}$ are weighting matrices. $\epsilon$ is a slack variable. The above variables and constraints will be explained in detail in the following. In this function, the first term suppresses the lateral deviation and heading deviation of the ego vehicle relative to the reference trajectory while the second item requires the smooth change of the control quantity.

Since this study considers the low-to-medium-speed urban conditions where there is no emergency, and this MPC contains two layers, this layer uses a bicycle model (Figure 11) that is advantageous to reduce the calculation.

In this scenario, the vehicle tires operate in the linear region under small lateral acceleration, so vehicle lateral forces can be calculated based on the simple linear tire model, which adopts the equivalent tire cornering stiffness ${\overline{C}}_{\alpha f}$ and ${\overline{C}}_{\alpha r}$:

$$F_{yf}={\overline{C}}_{\alpha f}{\alpha }_f$$

(18)

$$F_{yr}={\overline{C}}_{\alpha r}{\alpha }_r$$

(19)

where ${\alpha }_f,{\alpha }_r$ denote slip angles of the front and rear axle, respectively. Moreover, due to the small tire deflection, the tire slip angles,${\alpha }_f,{\alpha }_r$, can be expressed as:

$${\alpha }_f=\arctan \left(\frac{v_y+l_{f}r}{v_x}\right)-{\delta }_f\approx \frac{v_y+l_f\dot{\phi }}{v_x}-{\delta }_f$$

(20)

$${\alpha }_r=\arctan \left(\frac{v_y-l_{r}r}{v_x}\right)\approx \frac{v_y-l_r\dot{\phi }}{v_x}$$

(21)

where $v$ is the velocity.$r$ is the yaw rate. $l_f$ and $l_r$ are the distances from the center of gravity to the front and rear axles, respectively. $\phi$ and ${\delta }_f$ are the yaw angle and the front wheel steering angle, respectively.

Thus, based on small deflection angle of front wheels and the linear tire model, the vehicle dynamics can be described by the following mathematical equations

$$m{\dot{v}}_y=-m{v}_x\dot{\phi }+2\left[{\overline{C}}_{\alpha f}\left({\delta }_f-\frac{v_y+l_f\dot{\phi }}{v_x}\right)+{\overline{C}}_{\alpha r}\frac{l_r\dot{\phi }-v_y}{v_x}\right]$$

(22)

$$m{\dot{v}}_x=m{v}_y\dot{\phi }+2\left[C_{lf}{s}_f+{\overline{C}}_{\alpha f}\left({\delta }_f-\frac{v_y+l_f\dot{\phi }}{v_x}\right){\delta }_f+C_{lr}{s}_r\right]$$

(23)

$$I_{zz}\ddot{\phi }=2\left[l_f{\overline{C}}_{\alpha f}\left({\delta }_f-\frac{v_y+l_f\dot{\phi }}{v_x}\right)-l_r{\overline{C}}_{\alpha r}\frac{l_r\dot{\phi }-v_y}{v_x}\right]$$

(24)

$$\dot{\phi }=r$$

(25)

$$\dot{Y}=v_x\sin \phi +v_y\cos \phi$$

(26)

$$\dot{X}=v_x\cos \phi -v_y\sin \phi$$

(27)

where $m$ is the vehicle mass. The yaw moment of inertia and vehicle mass are represented by $m$ and $I_{zz}$. $s_{\left[f,r\right]}$ is the slip rate of [front, rear] tire.${\overline{C}}_{\alpha \left[f,r\right]}$ is the longitudinal stiffness of [front, rear] tire. The state vector is $\xi ={\left[\dot{y}\dot{x}\phi \dot{\phi }YX\right]}^T$ and the input vector is $u={\delta }_f$. And we define that $-10°\le {\delta }_f\le 10°,-0.85\le \mathsf{\Delta }{\delta }_f\le 0.85$.

The vehicle nonlinear dynamic model is:

$$\dot{\xi }=f\left(\xi ,u\right)$$

(28)

Firstly, to form an MPC problem, the bicycle model is linearized by Taylor formula at the reference point and discretized by the Forward-Euler method as follows:

$$\xi \left(k+1\right)=A_k\xi \left(k\right)+B_{k}u\left(k\right)+d\left(k\right)$$

(29)

$${\eta }_2\left(k\right)=C_k\xi (k)$$

(30)

where $A_k=I+A_t,B_k=T_s{B}_t$ and $d\left(k\right)={\xi }_{ref}\left(k+1\right)-A_k{\xi }_{ref}\left(k\right)-B_{k}u\left(k\right)$. $A_t={\frac{\partial f}{\partial \xi }|}_{{\xi }_t,u_t}$,$B_t={\frac{\partial f}{\partial u}|}_{{\xi }_t,u_t}$.$T_{s,2}=0.01\mathrm{s}$ is the simulation step.${\eta }_2\left(k\right)={\left[\phi Y\right]}^T$ is the output vector and $C_k$ is a output selection matrix.

To introduce the increment of control quantity $\mathsf{\Delta }U$ and simplify the calculation, the state space is reconstructed as:

$$\tilde{\xi }\left(k\right)=\left[\begin{array}{c}\xi (k)\\ u(k-1)\end{array}\right]$$

(31)

$${\tilde{\eta }}_2\left(k\right)={\tilde{C}}_k\tilde{\xi }\left(k\right)$$

(32)

The predictive state is defined as:

$$\tilde{\xi }\left(k+1\right)={\tilde{A}}_k\tilde{\xi }\left(k\right)+{\tilde{B}}_k\mathsf{\Delta }u\left(k\right)+\tilde{d}\left(k\right)$$

(33)

where ${\tilde{A}}_k=\left[\begin{array}{cc}{A}_k& B_k\\ 0_{m\times n}& I_m\end{array}\right];{\tilde{B}}_k=\left[\begin{array}{c}{B}_k\\ I_m\end{array}\right];{\tilde{C}}_k=\left[C_{k}0\right];\tilde{d}\left(k\right)=\left[\begin{array}{c}d\left(k\right)\\ 0_m\end{array}\right];\mathsf{\Delta }u\left(k\right)=u\left(k\right)-u\left(k-1\right)$ $m$ is the dimension of control amount while $n$ is the dimension of state amounts.

Therefore, the final output expression can be expressed as:

$$\gamma \left(k+1\right)=\psi \xi \left(k\right)+\Theta \mathsf{\Delta }u\left(k\right)+\Gamma \Phi \left(k\right)$$

(34)

where

$$\begin{array}{c}\gamma \left(k+1\right)=\left[\begin{array}{c}{\eta }_2(k+1)\\ ⋮\\ {\eta }_2(k+N_{c,2})\\ ⋮\\ {\eta }_2(k+N_{p,2})\end{array}\right],\psi =\left[\begin{array}{c}{\tilde{C}}_k{\tilde{A}}_k\\ ⋮\\ {\tilde{C}}_k{\tilde{A}}_k^{N_{c,2}}\\ ⋮\\ {\tilde{C}}_k{\tilde{A}}_k^{N_{p,2}}\end{array}\right],\mathsf{\Delta }U\left(k\right)=\left[\begin{array}{c}\mathsf{\Delta }u(k)\\ \mathsf{\Delta }u(k+1)\\ ⋮\\ \mathsf{\Delta }u(k+N_{c,2}-1)\end{array}\right],\\ \Theta =\left[\begin{array}{cccc}{\tilde{C}}_k{\tilde{B}}_k& 0& 0& 0\\ ⋮& ⋮& ⋮& ⋮\\ {\tilde{C}}_k{\tilde{A}}_k^{N_{c,2}-1}{\tilde{B}}_k& {\tilde{C}}_k{\tilde{A}}_k^{N_{c,2}-2}{\tilde{B}}_k& \cdots & {\tilde{C}}_k{\tilde{B}}_k\\ ⋮& ⋮& \ddots & ⋮\\ {\tilde{C}}_k{\tilde{A}}_k^{N_{p,2}-1}{\tilde{B}}_k& {\tilde{C}}_k{\tilde{A}}_k^{N_{p,2}-2}{\tilde{B}}_k& \cdots & {\tilde{C}}_k{\tilde{A}}_k^{N_{p,2}-N_{c,2}}{\tilde{B}}_k\end{array}\right],\\ \Gamma =\left[\begin{array}{ccc}{\tilde{C}}_k& \cdots & 0\\ ⋮& \ddots & ⋮\\ {\tilde{C}}_k{\tilde{A}}_k^{N_{p,2}-1}& \cdots & C\end{array}\right],\Phi \left(k\right)=\left[\begin{array}{c}\tilde{d}\left(k\right)\\ ⋮\\ \tilde{d}\left(k+N_{p,2}-1\right)\end{array}\right],\end{array}$$

and $N_{p,2}$, $N_{c,2}$ are the prediction horizon in tracking layer and the control horizon in tracking layer, respectively.

At each time step we solve the following optimization problem:

$$\begin{array}{c}\underset{\Delta U_t,\epsilon }{\min }{J}_2\left(\xi \left(t\right),u\left(t\right),∆U\left(t\right),\epsilon \right)\\ s.t.\\ \mathsf{\Delta }{U}_{\min }\le \mathsf{\Delta }{U}_t\le \mathsf{\Delta }{U}_{\max }\\ U_{\min }\le A\mathsf{\Delta }{U}_t+U_t\le U_{\max }\\ y_{hc.\min }\le y_{hc}\le y_{hc.\max }\\ y_{sc.\min }-\epsilon \le y_{sc}\le y_{sc.\max }+\epsilon \\ \epsilon >0\end{array}$$

(35)

where $y_{hc.\left[\min ,\max \right]}$ is the [upper, lower] bound on output variables to be hard constrained;$y_{sc.\left[\min ,\max \right]}$ is the [upper, lower] bound on output variables to be soft constrained. As it is a convex optimization problem, the function can be reconstructed as a standard quadratic form:

$$J_2\left(\xi \left(t\right),u\left(t-1\right),∆U\left(t\right),\epsilon \right)={\left[\Delta U{\left(t\right)}^T,\epsilon \right]}^T{H}_t\left[\Delta U{\left(t\right)}^T,\epsilon \right]+G_t\left[\Delta U{\left(t\right)}^T,\epsilon \right]+{{\rm P}}_t$$

(36)

where $H=\left(\begin{array}{cc}{\Theta }^T{Q}_e\Theta +R_e& 0\\ 0& \rho \end{array}\right)$,$G_t=\left[\begin{array}{cc}2E^T{Q}_e\Theta & 0\end{array}\right]$,${{\rm P}}_t=E^T{Q}_{e}E$ and $E$ is defined as a tracking error between the free response of the linearized system and the reference trajectory, i.e.,$E={\gamma }_{ref}-\psi \xi \left(t\right)-\Gamma \Phi \left(t\right)$ with ${\gamma }_{ref}={\left[{\eta }_{ref,local}\left(t+1|t\right),\dots ,{\eta }_{ref,local}\left(t+N_p|t\right)\right]}^T$. The matrices $Q_e$ and $R_e$ are block diagonal matrices of appropriate dimensions with $Q$ and $R$ on the main diagonal, respectively.

With QP problem solver, the solution $U_t^{*}={\left[\Delta u_{t,t}^{*},\dots ,\Delta u_{t+N_{c,1}-1,t}^{*}\right]}^T$ at time $t$ for the current state $\xi \left(t\right)$ and the previous input $u\left(t-1\right)$ are obtained. The resulting feedback control law is given by:

$$u\left(t,\xi \left(t\right)\right)=u\left(t-1\right)+\Delta u_{t,t}^{*}\left(\xi \left(t\right)\right)$$

(37)

At time $t+1$ the optimization problem is formulated and solved over a shifted horizon. Controller parameters and weights are shown as

Table 1. Controller parameters and weights. A. MPC parameters in motion re-planning layer; B. MPC parameters in the motion tracking layer.

A
Symbol	Value	Unit
$N_{p,1}$	60	(none)
$N_{c,1}$	2	(none)
$Q_1$	$100\times I_{N_{p,1}}$	(none)
$R_1$	$10\times I_{N_{c,1}}$	(none)
$T_{s,1}$	0.02	s
$S_1$	$900+k_1\times v_a$	(none)
$S_2$	$2000+k_2\times v_a$	(none)
$S_3$	$900+k_3\times \left(v_a+v_{OnC}\right)$	(none)
B
Symbol	Value	Unit
$N_{p,2}$	30	(none)
$N_{c,2}$	3	(none)
$Q_2$	$\left[\begin{array}{cc}2000& 0\\ 0& 10,000\end{array}\right]$	(none)
$R_2$	$5\times {10}^5\times I_{N_{c,2}}$	(none)
$\rho$	1000	(none)
$T_{s,2}$	0.01	s

Where $I_{*}$ is an identity matrix of dimension $*$. $k_1=k_3=3$ and $k_2=1$ are the weights of speed. $S_1$, $S_2$ and $S_3$ are the weight coefficients of avoiding the parked vehicle, road bounds and the oncoming vehicle, respectively. They are velocity dependent due to the demands of ensuring the safety and lateral stability of driving. Therefore, cost of collision avoidance will increase as the speed of the car increases especially in relatively high speed conditions.

.

4. Simulation Results

To verify the effectiveness of the obstacle avoidance strategy which is based on PET and two-layer MPC, CarSim-MATLAB co-simulation is conducted. The vehicle used in the simulation is based on the front wheel driving C-class hatchback model in Carsim software. The sample time is 0.01 s. The three cases are based on different PET, which is established in Section 2, should be discussed in this section. Therefore, to get different PET, we assume that the distance between the ego vehicle and parked vehicle is a constant while the initial position of the oncoming vehicle is variable. At the same time, the initial speed of the ego vehicle is set as 11 m/s in all three simulations.

In case 1, the calculated $PE{T}_{s2}$ is larger than the set $PE{T}_{safe}$, so the ego vehicle keeps the initial speed unchanged in the whole avoidance process. The acceleration, the steering wheel angle and vehicle longitudinal speed under case 1 are shown in Figure 12.

Figure 13 shows the CA process of ego vehicle under case 1. The CA process is simulated by Simulink/Matlab, and items among CA process, i.e., the parking vehicle, the oncoming vehicle, the re-planned local reference and so on, are marked in Figure 13c. The local reference path, which is shown as a short gray curve out of actual trajectory (green curve), is generated by MPC controller 1 in decision making layer (refer to Figure 4) in this time step to indicate the reference trajectory in the next time step.

In case 2, the calculated $PE{T}_{s1}$ is larger than $PE{T}_{safe}$, so the ego vehicle will then be required to accelerate. The acceleration, the steering wheel angle and vehicle longitudinal speed under case 2 are shown as Figure 14.

Figure 15 shows the CA process of the ego vehicle under case 2. If there is a risk of collision when the ego vehicle keeps speed unchanged during the CA process, and this risk can be avoided if ego vehicle accelerates to allowable maximum speed, then the ego vehicle will be required to accelerate. Although deceleration and waiting for oncoming vehicle passing the parked vehicle is another option for ego vehicle, considering the time cost and convenience, acceleration is an optimal solution with the promise of safety.

In case 3, the calculated $PET$ is smaller than $PE{T}_{safe}$, so the ego vehicle will then be required to decelerate. The acceleration, the steering wheel angle and vehicle longitudinal speed under case 3 are shown as Figure 16.

Figure 17 shows the CA process of ego vehicle under case 3. When the calculation shows that this is not a safe time under uniform speed or acceleration for avoiding both parked vehicle and oncoming vehicle in the same time, the ego vehicle will be required to decelerate to wait for the oncoming vehicle passing the parked vehicle firstly, as shown in Figure 17c. After the oncoming vehicle has passed, the ego vehicle then restarts and continues the CA process.

Figure 12a,c, Figure 14a,c and Figure 16a,c show the acceleration and velocity of the vehicle during the whole obstacle avoidance process, respectively. Based on different$PET$, the vehicle will be required to choose corresponding behavior, which is shown by figures of acceleration and velocity. As shown in Figure 12b, Figure 14b and Figure 16b, due to the low-speed urban condition, the vehicle front wheel steering angle under the three cases is small. When the ego vehicle begins to turn, the steering wheel angle also fluctuates lightly, and the maximum value of the steering wheel is about 8°.

The comparison of the longitudinal speed under three cases is shown in Figure 18.

From Figure 18, the comparison of speed under three different cases, in case 1, the velocity still maintains at about 11m/s despite fluctuations, which meets the requirement of keeping the speed as reference when the situation permits. In case 2, in order to reduce the time waste on the premise of ensuring safety, the vehicle chooses to accelerate to avoid obstacles, and its speed and acceleration are all within the preset boundaries. After passing obstacles, the vehicle decelerates to the reference speed. In case 3, the vehicle cannot avoid obstacles safely even if it accelerates to the maximum allowable speed. Therefore, in this case, the ego vehicle decelerates until the oncoming vehicle has already passed, then it restarts and avoids the parked vehicle. The final speeds of ego vehicle in three cases will be controlled to return to the reference speed after obstacle avoidance.

At the same time, the time history of TTC between the ego vehicle and the oncoming vehicle is also established, as Figure 19 shows. These two cars go towards each other, so finally the TTC will be zero. Smaller initial TTC will bring higher safety risk during the CA process. If the ego vehicle can safely avoid collision by acceleration, then deceleration is not necessary. This is to ensure the rapid passing of the ego vehicle to prevent congestion and save passengers’ time.

Time consumption of the simulation calculation is an effective indicator used to reflect the real-time performance of the proposed algorithm. As Figure 20 shows, in case 1 and case 2 in which the ego vehicle is not required to decelerate, the calculation time cost generally stays around 8 ms except for some fluctuations. Although some simulation calculations in case 2 are relatively time-consuming, they are no bigger than 100 ms. In addition, long time-consuming calculations mainly occur between the 7th and 10th seconds of the simulation in case 2. During this time period, the ego vehicle is parked and waiting for the oncoming vehicle to pass first (referring to Figure 18). Thus these long time-consuming calculations will not affect the safety of obstacle avoidance and the real-time decision making when the ego vehicle is running.

Table 2. Related parameters while the ego vehicle is running. A. Parameters in case1; B. Parameters in case2; C. Parameters in case3.

A
Symbol	Range	Unit
Longitudinal speed	10.8~11.8	m/s
Steering wheel angle	−3.1~3.2	°(degree)
Longitudinal acceleration	−1.2~2.2	m/s2
Lateral acceleration	−2.4~2.5	m/s2
B
Symbol	Range	Unit
Longitudinal speed	11~15.2	m/s
Steering wheel angle	−2.3~2.8	°(degree)
Longitudinal acceleration	−1.9~3.1	m/s2
Lateral acceleration	−2.5~3.3	m/s2
C
Symbol	Range	Unit
Longitudinal speed	0~11	m/s
Steering wheel angle	−8.3~4.8	°(degree)
Longitudinal acceleration	−2.1~3.3	m/s2
Lateral acceleration	−4.1~3.1	m/s2

shows changes in related parameters while the ego vehicle is running. The data in the table proves that the ego vehicle can avoid obstacles stably and safely within the speed and acceleration limits and further confirms the reliability of the proposed algorithm.

Figure 21 shows the real trajectories of the ego vehicle in three cases. And the minimum distance between ego vehicle and obstacle are 0.52 m (in case 1), 0.24 m (in case 2) and 0.5 m (in case 3), which verify the effectiveness of the proposed algorithm as the functions of obstacle avoidance and lane keeping in the urban traffic condition.

In addition, a comparison simulation is also established to prove the superiority of TL-MPC system. In order to promise real-time performance, reference [31] give a method of linear MPC based on a triple integrator chain. The original method which cannot adjust the speed after overtaking the obstacles is modified to adapt to this research.

Figure 22 shows the comparison in case 1 (collision avoidance at uniform speed). Even in this case under which there is no relative high requirement of speed control for the ego vehicle, the compared method cannot easily converge due to lack of model accuracy. At the same time, it also has great fluctuations in controlling the steering wheel angle. The same problem, that it is difficult to converge, can also be found in the results of reference [18]. This is basically due to the same vehicle model being selected in both re-planning and tracking layers. Since the comparison results in the other two cases are very similar, they will not be repeated in the paper. The results, shown by Figure 20 and Figure 22, illustrates that the TL-MPC proposed in this paper can guarantee real-time performance and converge.

5. Conclusions

Based on a normal urban driving scenario, this paper proposed an effective vehicle motion control strategy for automated vehicles which contains TL-MPC and BR-PET to realize the function of both lane-keeping and collision avoidance. The difficulty of solving this motion planning problem is how to deal with the unpredictable or uncertain future speed of oncoming vehicle and to control the speed of an autonomous vehicle in the whole process. Firstly, the BR-PET is proposed to solve the problem of oncoming vehicle’s unpredictable speed. Then, based on TL-MPC system, the vehicle’s speed and steering wheel angle are controlled in real time, so that both trajectory re-planning and tracking are realized. Finally, the proposed control system is verified by co-simulations of MATLAB/Simulink and CarSim. The obtained simulation results showed that under three different cases, BR-PET successfully re-plans a collision-free movement of the vehicle while the TL-MPC ensures trajectory re-planning and tracking performance under the premise of a safe collision avoidance.

Future work will focus on obstacle avoidance under extreme adhesion or at relative high speed conditions. More issues such as considering vehicle stability and passenger comfort could also be taken into account in future.