Autonomous Lateral Maneuvers for Self-Driving Vehicles in Complex Traffic Environment

Autonomous driving functions have gained great interests from both academia and industry over the years. This paper proposes a Model Predictive Control based method to generate a safe and feasible trajectory for the ego vehicle to perform various lateral maneuvers and to produce optimized control inputs to guide the ego vehicle through a mixed traffic environment with both human drivers and autonomous vehicle. A novel reference speed generation function is proposed to automatically adjust the position of the ego vehicle before the initiation of any lateral maneuvers. After a proper gap is selected, the lateral maneuver initiation function with an add-on threshold function is introduced to ensure the safety of the maneuver and to enable the ego vehicle to automatically start or stop the lateral maneuver. A nonlinear and high fidelity tire model is utilized in the simulation to validate the algorithm and the simulation results show that various maneuvers can be successfully achieved without re-tuning the weights or changing the parameters of the controller.


Autonomous Lateral Maneuvers for Self-Driving
Vehicles in Complex Traffic Environment Zhaolun Li , Jingjing Jiang , Member, IEEE, Wen-Hua Chen , Fellow, IEEE, and Liang Sun , Member, IEEE Abstract-Autonomous driving functions have gained great interests from both academia and industry over the years. This paper proposes a Model Predictive Control based method to generate a safe and feasible trajectory for the ego vehicle to perform various lateral maneuvers and to produce optimized control inputs to guide the ego vehicle through a mixed traffic environment with both human drivers and autonomous vehicle. A novel reference speed generation function is proposed to automatically adjust the position of the ego vehicle before the initiation of any lateral maneuvers. After a proper gap is selected, the lateral maneuver initiation function with an add-on threshold function is introduced to ensure the safety of the maneuver and to enable the ego vehicle to automatically start or stop the lateral maneuver. A nonlinear and high fidelity tire model is utilized in the simulation to validate the algorithm and the simulation results show that various maneuvers can be successfully achieved without re-tuning the weights or changing the parameters of the controller.
Index Terms-Autonomous driving, dynamic environment, model predictive control, nonlinear system, optimal control.

I. INTRODUCTION
A CCORDING to [1], lateral maneuvers of vehicles are not only responsible for 4-10% of all traffic accidents, but also the reason for 10% of latency on roads and 7% of total crash fatalities. Thus, a lot of researchers have devoted their efforts on developing autonomous driving functions which could significantly reduce the accidents caused by drivers' misbehavior. To achieve this goal and make the vehicle fully autonomous, the system should be able to safely perform various kinds of lateral maneuvers without re-tuning parameters in the algorithm, and to follow certain traffic rules. An autonomous driving system normally has a hierarchical structure which may consist of two layers: path planning and trajectory following. Numerous types of curves have been used for path planning, such as line segments [2], harmonic curve [3], circular curve [4] and polynomial curve [5]. They can all be formulated as constrained optimization problems [6]. In [7], a comparison between different types of curves have been made while two planning methods have also been compared. From the study, the authors discovered that the trapezoidal acceleration path was the best among the others since it generated the least lateral acceleration. In addition, a path planning algorithm was proposed in [8] where constrained optimization method was used to generate and update the time-independent polynomial for different situations, by tuning the weights of the maneuver time and the lateral acceleration in the cost function. Potential field method is also commonly used for path planning for mobile robots and autonomous vehicles. [9] provided a robust path planning method for mobile robot by assigning potential fields based on their physical features and the optimal path is obtained by balancing the weights for target tracking and collision avoidance in the cost function. Reinforcement learning (RL) is another popular method used to handle complex traffic scenarios. In [10], a RL-based method using deep Q-learning is proposed to automatically perform lane change maneuver in unforeseen environment. It utilized a closed form greedy policy to improve the computation efficiency of its deep Q-learning algorithm. Moreover, [11] combines policy-based RL algorithm with post optimization to provide lane merge trajectory for autonomous vehicle. However, the key limitation for RL-based algorithm is that its performance may significantly degrade and even cause safety hazards if the operation environment is different from the training environment or there is uncertainty in operation environment.
On the other hand, a large number of control methods have also been applied to develop reliable trajectory followers, including PID control [12], sliding mode control [13] and adaptive control [14]. In [15] the stationary obstacles were treated as simple s-topes and safety constraints were also considered for the ego vehicle. However, the planned path was computed only once at the beginning of the maneuver. So it can not be used in the dynamic environment where speeds of surrounding vehicles vary with time. [13] has expanded the results of the research based on vehicle-to-vehicle communication, with which distances between the surrounding vehicles and the ego vehicle were able to be accurately measured. This helped to establish the dynamic safety constraints which guarantees collision-free movements of the ego vehicle.
Model Predictive Control (MPC) is an online optimization method which generates the optimal inputs for the vehicle, based on real-time sensor information. Thus, unlike offline planning methods mentioned before, it is able to deal with fast changing environment by recursively solving the online optimization problem with updated information. In [16], the path planning problem was mainly treated as a longitudinal planning problem and the MPC was first utilized to generate a longitudinal path that satisfy all the constraints. If such longitudinal path exists, then the algorithm will generate the corresponding lateral path. However, if the gap between the leading and the tailing vehicles in the target lane is not suitable for initiating the maneuver, the algorithm may be unable to find a proper longitudinal trajectory and the planning procedure may be stuck. To deal with this problem and accelerate the computational process, a systematic approach was taken to find an appropriate gap to start the maneuver as explained in [17]. To achieve fully autonomous maneuver for the ego vehicle, a decision making framework was built in [18] by creating a utility function that took different lane properties into consideration. All the studies mentioned above follows a two-layer structure, i.e., high-level path planning and low-level trajectory tracking. However, the mismatch between the models used in the two layers may results in unfeasible path planning by the higher layer which cannot be followed by the low-level controller. To overcome the limitations, an MPC-based control strategy for autonomous lane change was studied by us in [19]. This paper extends its idea and provides a generalized solution for autonomous vehicles to adapt to dynamic traffic environment in a safer way.
The main contributions of the paper are stated as follows. The proposed algorithm is capable of guiding the vehicle to perform various maneuvers in a dynamic traffic environment. Moreover, a gap selection function paired with a novel reference speed function are proposed to guide the ego vehicle to automatically adjust its states for the selected gap on the target lane, which enables the ego vehicle to place itself at the optimal position with proper speed before the initiation of the lateral maneuver. A maneuver initiation function is proposed to automatically decide whether to initiate or abandon the maneuver when sudden environmental changes occur. Last but not least, an add-on threshold function is designed to guarantee that the space between the ego vehicle and its front and rear vehicles on the target lane is enough for a safe maneuver, which ensures the proposed algorithm prioritizes safety over other factors in the maneuver.
The rest of the paper is organized as follows. The overall structure of the proposed algorithm, together with the vehicle model, is described in Section II. The gap selection and lateral maneuver initiation functions with add-on threshold capabilities are presented in Section III and IV, respectively. The problem, as well as the solution, of generating versatile control strategy for autonomous vehicles in a dynamic traffic environment based on MPC method is provided in Section V. To test the effectiveness of the proposed algorithm, four typical scenarios have been selected, the results of which are as shown in Section VI. In the last section, the conclusions and future work are discussed.

II. MODEL DESCRIPTION
The overall structure of the proposed algorithm is shown in Fig. 1, illustrating how the decision making framework In the MPC-based Controller design, a bicycle model from [20] is used to predict the behavior of the ego vehicle, the dynamics of which is described by the equationṡ where a x is the longitudinal acceleration of the vehicle and ψ denotes the yaw angle. v x (v y ) represents the longitudinal (lateral) speed in the vehicle body frame, while X (Y ) denotes the vehicle's x (y) position in the global frame. m and I Z are the vehicle mass and yaw inertia, respectively. l f (l r ) describes the distance from the vehicle's center of mass to the front (rear) axle, and F c,f (F c,r ) represents the lateral force generated by the front (rear) wheels. The linear tire model is used here and the tire forces F c,f and F c,r are calculated as where C a,f (C a,r ) is the cornering stiffness for the front (rear) tires, and a f (a r ) is the slip angle for the front (rear) tires. Note that the slip angles are calculated as where δ f is the front wheel steering angle andψ represents the yaw rate.
Although the linear tire model (2) is used in the prediction model (1), a high-fidelity nonlinear tire model [21] is used to represent the dynamics of the ego vehicle in the simulation. The tire forces of which are calculated as where a opt,f = 20 • (a opt,r = 11 • ) is the optimal slip angle for the front (rear) tires. F c,f,max (F c,r,max ) represents the maximum lateral tire force at the front (rear) wheels, in coordinate frames aligned with the wheels. They are calculated as:

III. GAP SELECTION FUNCTION
Before the initiation of any lateral maneuver, the ego vehicle needs to select a proper gap on the target lane. Three key aspects that affect the gap selection are: 1) The width of the gap; 2) The distance between the end point of the current lane and the center of the gap being evaluated on the target lane; 3) The time for the ego vehicle to reach the center of the gap being evaluated on the target lane. The combination of them quantifies the risk of merging into the gap. The increase of the first two elements will reduce the risk, while the increase of the third aspect will increase the risk. The detailed descriptions are given in the following three subsections.

A. Time Gap Density U g1
The time gap density U g1 is used to define the width of the gap on the target lane and is calculated by the equation where tg target = d gap v target . d gap is the width of the gap and v target is the average speed of the vehicles on the target lane. α g > 0 is a constant scaling factor and tg restr,g is the restriction time gap for the ego vehicle E. Note that the value of U g1 is directly linked to the distance of the gap d gap and is also bounded by α g tg restr,g .

B. Remaining Traveling Time U g2
The remaining traveling time U g2 describes how long the center of the evaluated gap on the target lane will reach the end point of the current lane. It is defined as: where d restr,g , the restriction travel distance, is defined as  (7), it is clear that U g2 increases as the distance d end − d center increases. In addition, the value of U g2 is bounded by the restriction travel distance d restr,g .

C. Estimated Time to Reach the Gap U g3
U g3 , is used to evaluate the time for the ego vehicle to reach the evaluated gap on the target lane. If the gap on the target lane is behind the ego vehicle, U g3 is defined as: where d relative , the longitudinal distance between the center of the gap and the ego vehicle, is defined as d relative = |d center − x k |. v x,k is the longitudinal velocity of the ego vehicle and the constant parameter γ g > 0 is used to avoid zero division for the function. If the gap is in front of the ego vehicle, U g3 is defined as: where v x,max represents the upper-bound for v x .

D. The Combination of the Gap Selection Function
As a combination of (6)-(9), the gap selection is given by where ω gi > 0, i = 1, 2, 3 are weighting parameters, and N g1 , N g2 and N g3 are normalization factors defined as A safety threshold function T g , for the gap selection function U g , is defined as where a = − 1 max(1,U g1 + 1 − tg min,g ) + 1, a 0 = − 1 max(1,U g1,res +1−tg min,g ) + 1. Note that U g1,res and tg min,g are user-selected constants. Fig. 2 illustrates how T g changes according to U g1 with selected parameters. The final gap selection function with the safety threshold T g is then defined as: where U g and T g are given in (10) and (12), respectively. Remark 2: Consistent with human-driving habits, the ego vehicle mainly targets on the gaps behind itself. So a smaller value is assigned to w g3 if the gap is ahead of the ego vehicle.
Remark 3: The extra safety threshold function T g is set to turn the whole gap selection function to zero if time gap density is too small. Hence, a gap that is not wide enough will never be selected.

IV. LATERAL MANEUVER INITIATION FUNCTION
In order to perform a safe and smooth maneuver, many aspects have been considered [22]- [24]. Inspired but different from the utility function in [18], the lateral maneuver initiation function U m not only helps the ego vehicle to choose the target lane, but also decides when the ego vehicle initiates or abandons the lateral maneuver. The lateral maneuver initiation function contains four parts which are detailed as follows.
The remaining traveling time U m1 is defined as where x k is the current longitudinal position of the ego vehicle. In addition, d restr,m , the restriction travel distance which guarantees that U m1 is bounded, is defined as d restr,m = β m v target with β m a sufficiently large number (eg. β m > 20) and v target the same as that given in (6). The average traveling time U m2 is defined as where γ m > 0 is a constant parameter and v u is the average vehicle speed on the current lane. The time gap density U m3 is defined as where α m > 0 is a constant scaling factor, tg restr,m is the restriction time gap which is selected as 2.1 s. Apart from that, tg f (tg r ) is the time gap between the ego vehicle and its preceding (tailing) vehicle on the considered lane. The traffic rule factor U m4 is defined as where n denotes the number of lanes to the left of the considered lane and ζ is a positive constant scaling factor. Note that this is suitable for left hand traffic rule. U m is defined as the combination of (14) - (17), i.e.
where ω mi > 0, i = 1, 2, 3, 4 are weighting parameters and N m1 , N m2 and N m3 are normalization factors which are chosen as To improve the safety and guarantee that a lane with small U m3 will never be selected, a novel safety threshold function T m is designed as where (1,U m3,res +1−tg min,m ) + 1 with U m3,res and tg min,m userselected positive constants. The final lateral maneuver initiation function with the safety threshold T m is defined as: Remark 4: In order to avoid oscillations, the maneuver will be initiated if and only if the value of the corresponding lateral maneuver initiation function is marginally larger than that of the original lane. In other words, the ego vehicle E will initiate the maneuver from the initial lane L I to the target lane L T , if and only if U T > (1 + ξ)U I . On the other hand, if U I > (1 + ξ)U T , the lateral maneuver will be abandoned with the ego vehicle being guided back to the initial lane. Note that U T and U I are the values of the lateral maneuver initiation function for lane L T and L I , respectively. In addition, ξ is a positive constant scaling factor.

V. MPC DESIGN
To design an MPC controller which automatically performs various maneuvers in a dynamic traffic environment, the cost function, the constraints, and the prediction horizon should be set properly.

A. Cost Function
The cost function for the MPC consists of three parts: reference tracking J 1 , control effort minimization J 2 and comfort maximization J 3 . They are defined as where Q i > 0, R i > 0 and M i > 0 are constant weighting parameters, for all i = 1, 2. v r x,k is the references for the longitudinal velocity v x,k of the ego vehicle. W denotes the width of the lane. a k,max and δ f,k,max represents the upper-bounds for the two inputs, vehicle acceleration a k and front wheel steering angle δ f,k , respectively. In addition, j k,max and Δδ f,k,max are used to describe the upper-bounds for the longitudinal jerk j k and the steering rate Δδ f,k , respectively. Note that, v 2 x,k,,max , W 2 , a 2 k,max , δ 2 f,k,max , j 2 k,max and Δδ 2 f,k,max are used to normalize each part. Therefore the cost function of the MPC controller can be defined as where J 1 , J 2 and J 3 are given in (22).

Remark 5:
The tuning of the parameters is a trade-off among maneuver efficiency (i.e., how fast the lateral maneuver can be finished), passengers' comfort and safety (i.e., aggressiveness). Specifically, weighting parameters on the MPC cost function can be adjusted to balance reference tracking, control effort minimizing and jerk minimizing. Other weighting parameters on the two decision making functions should be chosen to fit the traffic regulations. For instance, tg restr,m and U m3,res in the threshold function for lateral maneuver initiation function should be chosen to ensure that the function value is zero when the gap is smaller than the legal requirements. In addition, these parameters could be tuned to fit the passengers' preferences.

B. Longitudinal Reference Speed Generation
Different maneuvers require the ego vehicle to have different speed profiles. Some challenging maneuvers, such as lane merge and lane departure, require a more accurate and dynamically varying reference speed than a constant number (such as the average speed of the target lane). To develop a universal reference speed function which works for various maneuvers, we consider the following three principles: 1) The ego vehicle E should start from its current speed v x,k and gradually change to the average speed on the target lane v target as the relative distance between the center of the selected gap and E gets smaller.

Thus, the reference velocity is defined as
where v vari is calculated as e −µd relative (v target − v x,k−1 ). Note that μ > 0 is a parameter which is selected as μ = 0.1 for this paper. d f is the relative distance between the ego vehicle and its preceding vehicle. s m is the safety margin. Finally, v f is the longitudinal velocity of the preceding vehicle of E. Remark 6: According to United Kingdom highway driving code rule 126, the distance between two vehicles on the highway should be greater than the stopping distance of the tailing vehicle and there should also be a 2(s) time gap between the two vehicles. Thus, the safety margin s m is selected as and s d and s gap is defined as [25]: where s d is the stopping distance for the ego vehicle, and s gap is the minimum distance required for 2(s) time gap. Apart from that, t inc is a vehicle parameter denoting the time for increasing the braking deceleration. C ef f is coefficient of efficiency and d is the minimum allowed distance between two parked vehicles.

C. Constraints on the Vehicle States and the Control Inputs
For any control design of autonomous vehicles, safety requirements are essential to meet and they are modeled as state constraints. In addition, the inputs to the vehicle, just like the inputs to any other physical systems, have hard constraints due to the physical and power limitations. The details of which are given as follows.
1) The lateral speed should always stay within its limitation v y,limt , which is determined by two components: the lateral speed when the ego vehicle reaches its physical limitation v y,phy due to tire adhesion, and the lateral speed when the ego vehicle rolls over v y,roll , i.e. v y,limt = min(v y,phy , v y,roll ).
2) The ego vehicle should always maintain at least a safety margin s m from the surrounding vehicles to avoid potential collisions.
3) The longitudinal speed of the ego vehicle should always be kept below its upper-bound v x,max . It also should always be non-negative, which means backward motion of the ego vehicle is not allowed. 4) The acceleration a and the front wheel steering angle δ f should always be relatively small due to the power and the physical limitation of the ego vehicle, as well as the comfort of the passengers. 5) The ego vehicle should never hit the boundary of the road, i.e. the lateral position y k needs to be bounded between its maximum and minimum allowed value, y k,max and y k,min . To sum up, all the constraints can be summarized by the following inequalities limt v y,k v y,limt , ∀k = 1, 2, . . . , N,   x k,min x k x k,max , ∀k = 1, 2, . . . , N,   y k,min y k y k,max , ∀k = 1, 2, . . . , N,   a min a k a max , ∀k = 1, 2, . . . , N,   δ f,min δ f,k δ f,max , ∀k = 1, 2, . . . , N, (26) where N denotes the number of predicted time steps and is defined as N = T predict Δt = 10, with T predict representing the time length for the prediction horizon. Note that the upper and lower bounds for v x , v y , a and δ f are constants. However, the constraints for longitudinal position at time instant kΔt, x k , is time-varying because the surrounding vehicles are not stationary. The time-varying constraints x max,k and x min,k are defined corresponding to two different situations shown in Fig. 4 : within one lane (denoted as the Pre/Post Phase), and in the middle of two adjacent lanes (denoted as the Peri Phase).
r If the ego vehicle E belongs to a single lane, it should keep a safe distance from its front and rear vehicles on that lane, yielding that the upper and lower bounds for the longitudinal position of E are defined as: where x l,f and x l,r are the longitudinal position of the front vehicle S l,f and the rear vehicle S l,r of E on lane L l , where l ∈ {I, T } (see Fig. 4).
r If the ego vehicle E belongs to two lanes, it needs to keep a safe distance from all the surrounding vehicles on both lanes. This yields that, the upper and lower bounds for the longitudinal position for E are defined as

D. Prediction Model for the Surrounding Vehicles
We assume that the surrounding vehicles have constant speed during a prediction horizon which is chosen as 1 s. Note that such assumption is reasonable even in the dynamic environment, since the prediction horizon is relatively short. Hence the prediction model for the longitudinal position of surrounding vehicles is given by where x S i , 1 and v x,S i ,1 are the current longitudinal position and velocity of surrounding vehicle S i . Such information can be obtained from either vehicle to vehicle communication or the ego vehicle's on-board sensors. Note that, the prediction for the behavior of surrounding vehicles is relatively accurate, since it updates every Δt = 0.1(s) time period.

VI. SIMULATION RESULTS
The MPC based control strategy was designed and implemented in Matlab with Acado toolkit. To test the effectiveness of it, four different maneuvers have been selected: lane merge, lane change, overtaking, and lane departure. In the first two scenario, a comparison was made with the bench-marked control strategy used in [26]. The third scenario demonstrated that the proposed algorithm can also be applied to low-speed urban traffic environment, while the last scenario demonstrated the effectiveness of the add-on threshold function. Note that, all the simulations are run on a personal laptop with Intel(R) Core(TM) i7-10870H 2.21 GHz CPU. The peak time required for running the MPC based algorithm (i.e., from the state is measured to a reference is generated) is 0.0458(s) which is much less than the sampling time of 0.1(s). Thus, the propose algorithm is real-time implementable on devices with similar computational power. The parameters and constraints related to the ego vehicle can be found in [27] and the design parameters for the gap and lateral maneuver initiation functions are shown in Table I. Note that the high-fidelity nonlinear tire model has been used in the simulation. Furthermore, we assumed that all vehicles on the road have rectangular shape with length and width equal to 3(m) and 2.4(m), respectively.
Remark 7: The convergence of the algorithm depends on the convergence of the optimization solver involved in decision making. If there is no solution, the lateral maneuver won't be initiated.
In this subsection, we compared our control strategy with the bench-marked autonomous overtaking control strategy used in [26]. Simulation results are shown in Figs. 6 to 7. From Fig. 6(a), we can see that, compared with the bench-marked control strategy (orange dashed line), the vehicle with the propose algorithm has much smoother speed profile from the merge lane to the main lane. Note that, the sudden speed decrease in the bench-marked control strategy resulted from tracking the position of the selected gap G A . In contrast, the proposed algorithm was able to guide the ego vehicle to gradually accelerate to the average speed on the target lane without sudden speed drop. The control inputs of the system provided by the proposed algorithm and the bench-marked control strategy are shown in Fig. 6-(c) and (d), from which we can see that inputs provided by the proposed algorithm is smoother and smaller than the inputs provided by bench-marked control strategy. This is because an extra term J 3 was added in the cost function (22) to minimize   the jerks Δa and Δδ f compared with the bench-marked control strategy.
Finally, the time histories of the slip angles of the front and rear wheels are given in Fig. 7. It can be seen that, all tires, especially front ones, were operating in the nonlinear region for a certain period of time, demonstrating the necessity of testing the proposed algorithm with the nonlinear tire model.

B. Lane Change Maneuver
Consider a two-lane dynamic traffic environment as shown in Fig. 8. G 1 and G 2 represent two gaps in the target lane. This scenario is used to demonstrate the effectiveness of the proposed algorithm in dealing with dynamic traffic environment.
In the beginning of the simulation, the ego vehicle (represented by the blue rectangle in Fig. 8) wanted to change from the low speed lane, L I , to the high speed lane, L T . However, after the ego vehicle has initiated the lane change maneuver, the driver of vehicle S 2 decided to accelerate and close the gap G 1 . The initial states (X, Y, v x , v y ) for Vehicles E, S 1 , S 2 , S 3 , S 4 , S 5 were   Simulation results for this scenario are shown in Figs. 9 to 11. From Fig. 9-(a), it can be seen that the lane change maneuver was initiated in the beginning of the simulation and the ego vehicle attempted to move to its target lane L T since the ego vehicle wanted to increase its speed. However, due to the speed increase of S 2 , the gap G 1 was soon closed. Fig 10 shows that the bench-marked control strategy failed to terminate this maneuver when G 1 closed and led the ego vehicle into the unsuitable gap, resulting in the minimum relative distance d min , between E and the surrounding vehicles that are potentially to collide with, getting dangerously small. Note that, according to the position of the ego vehicle, the surrounding vehicles are defined in (27) and (28), respectively.
In contrast, the proposed MPC-based algorithm was able to catch this change in the environment. At time instant t = 0.05(s), the maneuver initiation function found the gap G 1 was closed, hence the ego vehicle headed back to lane L I and stayed there waiting for the next gap G 2 . With the help of the reference speed function, the maneuver was re-initiated at t = 12.9(s) when the gap G 2 was suitable for the maneuver. The bottom two subfigures in Fig 9 show that the inputs of the ego vehicle never violated the constraints during the whole process. With Fig. 10, we can conclude that the ego vehicle has always kept enough distance from all surrounding vehicles, demonstrating the safety of the vehicle with the proposed control algorithm. Note that there is a sudden change in d min at t = 14.1(s). This is because S 3 and S 4 are no longer regarded as surrounding vehicles of E as it has entered the target lane completely (i.e., in Post Phase).
Furthermore, from Fig. 9(b) and Fig. 11, we conclude that the ego vehicle followed its reference speed v r x,k . It dropped from 21(m/s) to avoid collision with its front vehicle S 3 on the initial lane at the first lane change attempt, and continued to drop its speed to 15(m/s) once the ego vehicle was guided back to the initial lane at t = 0.05(s). Then the reference speed v r x,k increased to lead the ego vehicle to reduce the distance between itself and the center of gap G 2 at t = 0.7(s). Note that v r x,k dropped back to 15(m/s) to avoid collision with its preceding vehicle S 3 on the initial lane at t = 4.2(s). After that its value increased again to make the ego vehicle closer to the center of G 2 and track the average speed 21(m/s) on the target lane. Such adaptive speed during the lane change maneuver coincides with typical human driver's behaviors and ensures that the proposed algorithm can guide the ego vehicle through a dynamic environment with unexpected changes.

C. Overtaking Maneuver
The third scenario, shown in Fig. 12, was designed to test the effectiveness of the proposed algorithm in low speed urban environment.
Suppose that the ego vehicle traveled at 10 m/s like all the other vehicles on lanes L 1 and L 2 , except for S 2 , at the beginning of the scenario. S 2 traveled at a constant speed of 5 m/s, so the ego vehicle wanted to overtake S 2 . The initial states (X, Y, v x , v y ) for Vehicles E, S 1 , S 2 , S 3 , S 4 , S 5 were selected as Simulation results are shown in Fig. 13. From Fig. 13(a), we can see that the ego vehicle was able to merge into lane L 2 , re-enter the lane L 1 and complete the overtaking maneuver. The speed increased at t=0.5 s and 11.8 s because the ego vehicle was trying to merge into other lanes and the sudden change of the lateral reference has rippling effects on the longitudinal speed of the ego vehicle through the vehicle dynamics. Compared with the high-speed lane change maneuver in the first scenario, we find that the proposed algorithm can also guide the ego vehicle to perform overtaking maneuver safely in low-speed  urban environment which proves that the proposed algorithm can adapt to traffic environment with a wide range of speed.

D. Lane Departure Maneuver
The forth case study is shown in Fig. 14  To demonstrate the importance of threshold functions, a comparison between results with and without the proposed threshold functions are presented in Figs. 15 to 17. From Fig. 15, it is obvious that, without the threshold functions, the simulation stuck in the middle, because the time to initiate the maneuver was not proper and safety constraints were violated in the middle of the maneuver. As can be seen from Fig. 16, there was not enough time gap between the ego vehicle and its front and rear vehicles at 2.6 s. Note that Fig. 17 shows that the lateral maneuver initiation function started to calculate values since 1.1 s, because the ego vehicle did not enter the longitudinal position belonging to the  selected gap G B at the beginning of the scenario. From Fig. 17, it can be seen that this inappropriate lateral maneuver initiation time can be corrected by the threshold function T m , as the time to initiate the maneuver was delayed from 2.6 s to 5.9 s. With the help of T m and T g , the lane departure initiation time is appropriate as there were sufficient long time gaps (see from Fig. 16) between the ego vehicle and its front vehicle (S 2 ), as well as its rear vehicle (S 3 ), on the departure lane. After the ego vehicle finished the lane departure procedure at 8.8 s, the ego vehicle still kept a safety distance from S 2 (77 m) and S 3 (50 m).

VII. CONCLUSION
This paper proposed an MPC-based algorithm for self-driving vehicles to automatically perform various maneuvers in a dynamic traffic environment. A nonlinear bicycle model has been considered in the algorithm to guarantee that the prediction of the behavior of the ego vehicle is close to the reality. In addition, a gap selection function combined with a reference speed function has been designed to lead the ego vehicle to the optimal position before the initiation of the lateral maneuver. A lateral maneuver initiation function with an add-on threshold function has been used to ensure the safety of the ego vehicle and achieve autonomy in various traffic scenarios. The proposed strategy were tested to accomplish four tasks (lane merge, lane change, overtaking and lane departure), and the results showed that the algorithm was able to guide the ego vehicle to initiate the lateral maneuver when it was safe to perform, and abandon it when the gap closed, in various traffic environment with a wide range of speed. Apart from that, the proposed algorithm also showed the capability to help the ego vehicle to choose an appropriate gap and guide it to enter the gap when the lateral maneuver is safe to perform.
To extend the capability of the proposed algorithm, future work will focus on the extension to curvy roads and the robustness of the algorithm with parameter uncertainties and sensor noises.