Automatic Lane Change Maneuver in Dynamic Environment Using Model Predictive Control Method

The lane change maneuver is one of the typical maneuvers in various driving situations. Therefore the automatic lane change function is one of the key functions for autonomous vehicles. Many researches have been conducted in this field. Most existing work focused on the solutions for the static environment and assume that the surrounding vehicles are running at constant speeds. However, in reality, if not all the vehicles on the road are fully autonomous, the situation could be much more complicated and the ego vehicle has to deal with the dynamic environment. This paper proposes a Model Predictive Control (MPC)-based method to achieve automatic lane change in a dynamic environment. A two-wheel dynamic bicycle model, which combines the longitudinal and lateral motion of the ego vehicle, together with a utility function, which helps to automatically determine the target lane have been used in the algorithm. The simulation results have demonstrated the capability of the proposed algorithm in a dynamic environment.


I. INTRODUCTION
Driving on the highway is always a stressful and tedious task. That is why many accidents have occurred on highways, and a lot of researchers have devoted their effort on developing autonomous vehicles which achieves safe driving on highways. One of the key aspects of driving on highway is the lane change maneuver, as it is one of the riskiest maneuvers on highways. According to [1], the maneuver is not only responsible for 4-10% of all the traffic accidents, but also the reason for 10% of latency on the roads and 7% of the total crash fatalities. In Netherlands, lane change maneuver was responsible for 12.6% of all the traffic accident [2]. In Canada, 9.8% fatalities related to car crash was caused by lane change [3]. In the United States of American, 13,939 traffic accidents result from lane change and 24565 were killed during 1994 to 2005 [4]. The inappropriate lane change causes even more problems in developing countries, such as China and India. According to the report from the Chinese High Traffic Safety Administration, 60% of the large traffic accidents on highway resulted from or were related to lane changes [5]. Due to the great proportion of traffic accidents caused by the lane change, traffic safety administration agencies, funding bodies and vehicle companies have invested a lot in developing safe autonomous lane change systems.
In order to improve the safety of the lane change maneuver, many researches have been contributed to this area over the years. One key aspect in this area is trajectory planning. To reduce the computation complexity for trajectory planning, numerous types of curves have been used such as line segments in [6], harmonic curve in [7], circular curve in [8] and polynomial curve in [9]. In [10], 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 trajectory is the best trajectory among the others since it generates the least lateral acceleration for lane changes. They also proved that it is better to use the sliding mode control than PID control to stabilize the system. During the analysis, they assumed that the vehicle was placed in an isolated environment with no surrounding vehicles and obstacles. The stationary obstacles were treated as simple s-topes and the dynamic constraints were also considered for the ego vehicle. However only a single coefficient was used as the criteria to evaluate the achievable trajectories and to make the choice among them. In addition, the planned trajectory was computed 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. [11] has expanded the results of the research by using two parameters, the lane change time and the lane change distance, as the criteria to evaluate and generate the path. The research was based on vehicle-to-vehicle communication, so with low latency conditions, highly accurate safety spaces between the surrounding vehicles and the ego vehicle were able to be computed. This helped to establish the dynamic constraints for collision avoidance. However all the simulation was carried out with a single surrounding vehicle on the target line and it did not consider the case when the lane change maneuver needed to be abandoned after initiated due to environmental changes. A similar research was carried out in [12]. They used the same lane type while the main difference between [11] and [12] is that [12] used a series of dynamic circles to represent the geometry of the ego vehicle and the surrounding vehicles in the collision avoidance analysis. A dynamic path planning algorithm was proposed in [13] where it used constrained optimization method to generate and update the time-independent polynomial for different lane change situations, such as the existence of gap between the vehicles on the target lane and the sudden acceleration of the surrounding vehicles which closed the gap, by adjusting the weights of the lane change time and the lateral acceleration in the cost function. All the previous mentioned path planning methods can be grouped into two categories: one assumed that the surrounding vehicles have constant speed, such as [11], [12] and [14], the other divided the whole lane change process into different situations and the cost function needed to be tuned for each situation, for example in [13]. However, none of them provides a fully automated algorithm that can generate a trajectory which adapts to the changing surrounding environment.
MPC is an online method which can generate the optimal input for the vehicle based on the real-time sensor information. Thus, unlike the offline methods mentioned above, it is suitable for trajectory planning in a dynamic traffic environment. In [15], the trajectory planning problem was mainly treated as a longitudinal planning problem and the MPC was first utilized to generate a longitudinal trajectory that satisfy all the constraints related to it. If such longitudinal trajectory exists, then the algorithm will generate the corresponding lateral trajectory. However, if the gap between the leading and tailing vehicles in the target lane is not suitable for the lane change to be initiated, the algorithm is unable to find a proper longitudinal trajectory and a large amount of computation time will be wasted. In order to deal with this problem and accelerate the computation process, a programmatic approach was taken to decide the appropriate traffic gap to start the lane change by estimating whether there exists a longitudinal trajectory for the automated ego vehicle to safely perform the lane change maneuver or not, as explained in [16]. To achieve fully autonomous lane change, a decision making frame work was built in [17] by creating a utility function that took different lane properties into consideration. However, the vehicle model used in the paper was double-integrator for both longitudinal and lateral dynamics. This results in two consequences: Firstly, to merge the two separately planned trajectories, the constraints have to be set tighter for trajectories on each individual axis, which results in a much conservative lane change trajectory. Secondly, the double-integrator model is a simplified model of the ego vehicle which sacrifices a lot of model fidelity. In addition, since most researches on autonomous lane change maneuvers are hierarchical structure with two layers, the mismatch of the model used in path planning and path following layers may result in unfeasible path planned by the higher layer which cannot be followed by the low level controller.
The main contributions of the paper are stated as follows. An MPC-based lane change algorithm is proposed to integrate the path planning and path following layers together with an automatic decision making frame work to achieve automatic lane change maneuver. Such framework is suitable for the dynamic traffic environment. A two-wheel dynamic bicycle model with high fidelity is used for the ego vehicle, so the simulation results do not have much deviation from real applications.
The paper is organized as follows. The description of the model used for the ego vehicle is provided in Section II. In Section III, the utility function used to make decisions on lane change is presented. The automatic lane change problem is formulated in Section IV based on the MPC structure, while the simulation results by the proposed algorithm are presented in Section V. In the last section, the conclusions and future work are discussed.

II. MODEL DESCRIPTION
For this paper, a dynamic bicycle model is used for the model of the ego vehicle which is defined by the equations where x and y represent the longitudinal and the lateral positions of the mass center for the vehicle, respectively. ψ denotes the yaw angle. m and I Z are the vehicle mass and yaw inertia, respectively. l f and l r describe the distance from the mass center of the vehicle to the front and rear axles, respectively, while F c,f and F c,r represent the lateral tire force at the front and rear wheels, respectively, in coordinate frames aligned with the wheels. In the studied lane change maneuver, the slip angles for all wheels are relatively small. Hence 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, respectively, and a f (a r ) is the slip angle for the front (rear) tires. Note that the slip angle can be calculated as where δ f is the steering angle andψ represents the yaw rate. In addition, v x and v y denotes the longitudinal and lateral velocity of the vehicle. Remark 1: To keep the high fidelity of the model, the lateral acceleration of the vehicle a y must be maintained at a relatively small value i.e. the maximum magnitude of a y , denoted by a y,max , must satisfy the constraints |a y,max | 0.4g where g is the gravitational acceleration.
Remark 2: The inputs of the system (1)-(2)-(3) are the longitudinal acceleration a x at the vehicle body frame and the front wheel steering angle δ f .
Since the algorithm is designed to guide the ego vehicle to perform the lane change maneuver, the vehicle models for the surrounding vehicles are set as simple double-integrator.

III. UTILITY FUNCTION
In order to perform a safe and smooth lane change maneuver, many aspects have to be considered, such as average speeds of different lanes, time gap between surrounding vehicles and the ego vehicle's remaining traveling time in each lane [18]- [20]. Aside from that, the traffic rule is another important factor that needs to be considered. In order to construct a utility function which accounts for the lane properties and traffic rules and guides the ego vehicle through the lane change maneuver, four aspects are taken into consideration: U lv , average traveling time in L l , U lg , time gap density in L l , U ld , remaining traveling time in L l and U ln , traffic rule factor in L l . Note that l ∈ L where L describes the set of lanes the ego vehicle will be traveled on.
A. Average traveling time U lv U lv is used to measure the deviation of the average velocity v lu from v des . Its value is determined by the average longitudinal speed on L l , v lu . It is also related to the desired longitudinal speed v des which the ego vehicle is aiming to achieve by the end of the lane change process. It is defined as: where d max is the maximum travel distance and is defined as d max = βv des with β is a small positive constant parameter. The constant parameter γ > 0 is used to avoid zero division for the function (i.e. γ is chosen such that 0 < γ < v des ).
Note that the value of U lv has the maximum value 0 when v lu is equal to v des .

B. Time gap density U lg
The time gap density U lg is used to quantify traffic density in the lane L l and is defined by the equation where α > 0 is a constant scaling factor, tg des is the desirable time gap for the ego vehicle E and tg f (tg r ) is time gap between E and its preceding (tailing) vehicle S f (S r ). From (5), it is clear that U lg increases as both tg f and tg r increase until both of them reaches the value of αtg des . Note that according to the property of the min function given in (5), the value of U lg is bounded even for an empty lane.

C. Remaining traveling time U ld
The remaining traveling time of U ld describes how long the ego vehicle can stay on the lane L l . Thus, U ld is defined as where d end is the distance between the current position of the ego vehicle to the end point of the lane L l (eg. an exit point, a land drop or the end of an entry ramp). Note that the min function is used to bound U ld so that its value will not go to infinity for a normal (i.e. without ending point) lane.

D. Traffic rule factor U ln
To cope with the left hand traffic rule all the vehicle need to be travel in the leftmost lane if possible. So extra traffic rule factor U ln is added to the utility function. Note that U ln is defined as where n denotes the number of lanes to the left of L l and ζ is a positive constant scaling factor. Similarly, right hand traffic rule can be coped with by using n to denote the number of lane to the right of L l .
E. The combination of the utility function As a combination of (4), (5), (6) and (7), the final utility function for L l is defined as where ω i > 0, i = 1, 2, 3, 4 are weighting parameters and N lv , N lg and N ld are normalization factors which are defined as Remark 3: With the normalization of the first three components in the utility function, they have been brought to the same scale. This is very important for the weight tuning process.

IV. MPC PROBLEM FORMULATION
To design the MPC controller for the lane change maneuver, the constraints for the ego vehicle, the cost function and the prediction horizon of the MPC should be set properly to ensure the safety of the maneuver and the comfort for the passengers. And more importantly, it needs to adapt to a dynamic traffic environment.

A. Cost function
The cost function for the MPC consists of three parts: reference tracking, J 1 , control effort minimization, J 2 and inputs rate minimization, J 3 . They are defined as where Q i > 0, R i > 0 and M i > 0 are constant weighting parameters, for all three parameters i = 1, 2. r 1 and r 2 are the references for the longitudinal velocity v x,k and the lateral position y k of the ego vehicle, respectively. v x,max and W denote the maximum longitudinal speed of the ego vehicle and the width of the lane, respectively. a max and δ f,max represents the maximum values for the two inputs: vehicle acceleration a and front wheel steering angle δ f , respectively. In addition, ∆a max and ∆δ f,max are used to describe the maximum values for the alteration rates of a and δ f , respectively. Note that, v 2 x,max , W 2 , a 2 max , δ 2 f,max , ∆a 2 max and ∆δ 2 f,max are used to normalize each part. Therefore, the cost function for the MPC is defined as where J 1 , J 2 and J 3 are given in (10). Remark 5: Weighting parameters Q i , R i and M i can be adjusted to shift the balance between reference tracking, control effect minimizing and jerk minimizing. For instance, increasing the values of Q i , we can get a swifter lane merge/lane change. However, the inputs of the model and the jerk will be enlarged during the process, vice versa.

B. Constraints on the vehicle states and the control inputs
For any autonomous vehicle, safety requirements have the highest priority and they are modeled as the states constraints in the paper. 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 ego vehicle should always maintain enough safety distance s m from the surrounding vehicles to avoid potential collision.
where N denotes the number of predicted time steps and is defined as N = T predict ∆t , with T predict representing the time length for the prediction horizon and ∆t representing the sampling time. Note that the upper and lower bounds for v x , v y , a and δ f are constants. However the constraints on the longitudinal position x k of the ego vehicle is timevarying because the surrounding vehicles S i are not stationary. The time-varying constraints x max,k and x min,k are defined corresponding to three different phases of the lane change maneuver shown in Fig. 1: before the lane change procedure (denoted as the Pre-phase), during the lane change procedure (denoted as the Peri-phase) and after the lane change procedure (denoted as the Post-phase).
where x I,f and x I,r are the longitudinal position of S I,f and S I,r of E on L I . In addition, s m represents the safety distance. • In the Peri-Phase, the ego vehicle E changes its lane from L I to L D . Therefore, it needs to keep a safe distance from all surrounding vehicles in both lanes. This yields that, the upper and lower bounds for the longitudinal position for E are defined as x k,min = max(x I,r + s m , x D,r + s m ), where x D,f and x D,r are the longitudinal position of S D,f and S D,r on the destination lane L D (see Fig. 1). • In the Post-phase, the ego vehicle E is in the Post region and it belongs to the destination lane L D . Therefore, we pay attention to S D,f and S D,r on L D , yielding that the upper and lower bounds for the longitudinal position for E are defined as

C. Prediction Horizon
Nonlinear MPC requires large computational power, so the prediction time needs to be set relatively short to reduce computational time and make the controller suitable for real time implementation. Therefore, the prediction horizon is chosen as 1s. We assume that the surrounding vehicles do not change lanes in the considered scenario. Hence the prediction model for the longitudinal position of the surrounding vehicles is given by where x Si, 1 and v x,Si,1 are the current longitudinal position and velocity of surrounding vehicle S i . Such information can be given by either vehicle to vehicle communication or the ego vehicle's onboard sensors. Note that the constant speed model is used in the prediction. However, as the control horizon is set to 1 second and the prediction of surrounding vehicles updates every ∆t = 0.1s time period, the prediction on the behavior of surrounding vehicles is relatively accurate. The prediction horizon N is calculated as N = 1 ∆t = 10.

V. SIMULATION RESULTS
The MPC based autonomous driving function was designed based on standard Matlab function fmincon. To test the effectiveness of it, the ego vehicle was placed in a two-lane dynamic traffic environment as shown in Fig. 2.
, π 6 }rad, m = 1575kg, I Z = 2875kg * m 2 , C a,f = −19000N/rad, C a,r = −33000N/rad, l f = 1.2m, l r = 1.6m, W = 3.5m, ξ = 0.02, α = 2, β = 30s, γ = 2, ζ = 0.1, v des = 21m/s and tg des = 1.5m. In addition, we also assume that all vehicles on the road have rectangular shape with length and width equal to 3m and 2.4m, respectively. Simulation results are shown in Fig. 3 to Fig. 6. From Fig. 3, it can be seen that the lane change maneuver was initiated in the beginning of the simulation and the ego vehicle moved to its destination lane L D , since the ego vehicle wanted to increase its speed, (i.e. U D (t) > (1 + ξ)U I (t), when t = 0). However, due   to the speed increasement of S 2 , the gap G 1 was closed and the utility function was able to catch the situation. At time instant t = 0.4s, U I (t) > (1 + ξ)U D (t), hence the ego vehicle headed back to lane L I and stayed there waiting for the next gap G 2 . When t=6.4s, the gap G 2 was detected and U D (6.4) > (1 + ξ)U I (6.4). Therefore, the lane change maneuver for the ego vehicle was re-initiated automatically. Fig. 3 also shows that it took about 1.2s to change from lane L I to lane L D . Thus, we can conclude that the MPC controller successfully made E to follow different references at different lanes (V x = 15, y = −1.75 at L I and V x = 21, y = 1.75 at L D ). Clearly, the speed of the ego vehicle did not achieve the average speed in Lane L D , 21m/s, when it entered the destination lane. Therefore it accelerated for another 2.4s until its speed reaches 21m/s. This phenomenon also coincides with typical human driver's behaviors. The whole lane change maneuver takes around 3.6s which can be seen from Fig. 4. Take the size information of the vehicles in the beginning of this section and the lane width into consideration. We know that the ego vehicle belonged to the initial lane L I when t < 0.2s and 1.3s < t < 6.6s, while it belonged to the destination lane L D when t > 7.4s. In addition, when 0.2s t 1.3s and 6.6s t 7.4s, the ego vehicle belonged to both lanes, i.e. part of the ego vehicle entered the destination lane L D , while the others still belonged to the initial lane L I . From Fig. 5, we know that when t < 0.2s and 1.3s < t < 6.6s, E has kept enough distance from S 3 and S 4 on lane L I . When 0.2s t 1.3s, E has kept enough distance from S 1 , S 2 , S 3 and S 4 on both lane L I and L D . When 6.6s t 7.4s, E has kept enough distance from S 2 , S 5 , S 3 and S 4 on both lane L I and L D . When t > 7.4s, E has kept enough distance from S 2 , S 5 on lane L D . So we can draw the conclusion that the ego vehicle E did not collide with any surrounding vehicles during the entire lane change process. Fig. 6 shows that in the whole lane change process the inputs of E never violated the constraints.

VI. CONCLUSION
This paper proposed an MPC-based algorithm to achieve the automatic lane change maneuver. A high fidelity ego vehicle model has been considered in the algorithm to guarantee that the prediction of the behavior of the ego vehicle is close to the reality according to its dynamics. In addition, the utility function is used to make automatic decisions on which lane the ego vehicle should head to. It was tested in a dynamic traffic scenario and the results showed that the proposed algorithm was capable to guide the ego vehicle to initiate or continue the lane change when the maneuver was safe to perform, or abandon the lane change moreover when there was potential risk to collide with the surrounding vehicles. To extend the capability of the proposed algorithm, future work will focus on the extension to curvy roads and the robustness of the algorithm on parameter uncertainties and sensor noises.