A Universal Trajectory Planning Method for Automated Lane-Changing and Overtaking Maneuvers

Lane-changing and overtaking are conventional maneuvers on roads, and the reference trajectory is one of the prerequisites to execute these maneuvers. ,is study proposes a universal trajectory planning method for automated lane-changing and overtaking maneuvers, in which the trajectory is regarded as the combination of a path and its traffic state profiles. ,e twodimensional path is represented by a suitable curve to connect the initial position with final position of the ego vehicle. Based on the planned path, its traffic state profiles are generated by solving a nonlinear mathematical optimization model. Moreover, the study discretizes the time horizon into several time intervals and determines the parameters to obtain the continuous and smooth profiles, which guarantees the safety and comfort of the ego vehicle. Finally, a series of simulation experiments are performed in the MATLAB platform and the results show the feasibility and effectiveness of the proposed universal trajectory planning method.


Introduction
Automated driving system can significantly alleviate traffic congestion, ensure traffic safety, and improve road capacity [1][2][3][4]. In order to realize the intelligent vehicle as soon as possible, some automated driving technologies are being tested on roads. Among the test items, the lane-changing and overtaking maneuvers are included [5][6][7][8]. To execute these maneuvers automatically, a feasible reference trajectory is needed. e ego vehicle will drive along the planned trajectory to finish the relevant maneuvers.
is paper proposes a novel universal trajectory planning method for the automated lane-changing and overtaking maneuvers. With the development of advanced sensors and vehicle-tovehicle (V2V) communications, real-time traffic information of vehicles, such as position, speed, and acceleration, can be obtained conveniently and accurately [9][10][11][12], and the trajectory is planned based on these traffic state information.
Ziegler et al. [13] employed a quantic spline curve to generate the trajectory. ey divided the space into multiple geometric graphs and adopted the shortest path algorithm to search for the feasible trajectory for each graph. Rousseau et al. [17] utilized the trajectory planning method based on the B-spline curve. e parameters of curve function were determined by minimizing the driving time. Yin et al. [18] described a trapezoidal acceleration motion planning model. e planned trajectory could avoid the potential collisions with obstacles by analyzing the variation law of the actuator. Chen et al. [22] presented a trajectory planning method using the 3D Bezier curve, which enhanced the flexibility of trajectory and guaranteed the conformity to the realistic driving maneuver.
Among the commonly used geometric curves, the polynomial curve is the most widely used in trajectory planning. is type of curve can plan the smooth trajectory with a low computational cost, and the order of polynomial can be tuned for obtaining the desired trajectory performance. Nelson et al. [27] were the first to propose the polynomial method. ey used the polynomial curve to replace the arc curve for planning the continuous-curvature trajectory, which guaranteed the traceability of the trajectory. Nilsson et al. [29] presented a trajectory planning model using the discrete quadratic polynomial curve. ey divided the planning area into three regions (i.e., preregion, periregion, and postregion) and solved the optimal positions of the ego vehicle in each region to determine the trajectory function. Zhang et al. [30] adopted two time-dependent cubic polynomial functions to describe the longitudinal and lateral motions, respectively. ey concluded that the continuity of the polynomial curve could ensure the robustness of trajectory. Yang et al. [32] proposed a trajectory planning model based on the cubic polynomial curve. ey focused on the trajectory replanning problem in dynamic driving environments. Wei et al. [33] increased the polynomial function to the fifth order and solved the optimal trajectory by treating the driving time and movement distance as free variables.
However, there are still several common disadvantages existing in the previous studies. First, these trajectory planning models usually only focus on the automated lanechanging or overtaking maneuver, while neglecting the universality of the model for lane-changing and overtaking maneuvers. Second, the traffic states of the ego vehicle solved by the previous model usually were discretized, which might result in hard tracking or even crashes. ird, most of the studies designed the lane-changing or overtaking scenarios simply, in which they assumed that the surrounding environment was static, the speeds of other vehicles were constants, or only one or two vehicles were around the ego vehicle. ese assumptions were inconsistent with the realworld traffic characteristics.
Aiming at these problems, a trajectory planning method based on the mathematical optimization framework is proposed in this paper. e contributions can be summarized as follows: (1) A universal framework of trajectory planning for the automated lane-changing and overtaking maneuvers is proposed. e study considers that the combination of a path and its traffic state profiles determines a complete spatiotemporal trajectory. e suitable curve is employed to represent the two-dimensional path. And then a nonlinear optimization model is established to generate the traffic state profiles, whose computational cost is low since the path has been planned.
(2) e traffic state profiles are continuous and smooth.
In previous studies, the traffic states of the ego vehicle solved by the nonlinear model usually were discretized. Our proposed model discretizes the time horizon into several time intervals and solves the unknown parameters of the function to generate the continuous and smooth traffic state profiles. e rest of the paper is organized as follows. Section 2 introduces the framework of the universal trajectory planning method. Section 3 describes the technical details of the proposed method, including the generation process of the path and traffic state profiles. Section 4 presents the simulation results and several typical numerical examples. Section 5 concludes the study and discusses the future work.

Framework of the Universal Trajectory
Planning Method Figure 1 presents the framework of the proposed universal trajectory planning method, which plans the trajectory through four stages. In stage 1, the current traffic states of the vehicles are collected via GPS, digital maps, and sensors. In stage 2, based on the known boundary conditions, a suitable curve is employed to generate the two-dimensional path connecting the initial position and the final position. In stage 3, the objective function and relevant constraints are established to solve the traffic state profiles along the path, such as speed, acceleration, and jerk profiles. e combination of the path and its traffic state profiles determines a complete spatiotemporal trajectory. In stage 4, the generated trajectory parameters are used as inputs for the actuators to execute the desired control.

The Universal Trajectory Planning Method
Planning a reference trajectory before changing lane or overtaking is necessary and important. e quality of the trajectory has a direct impact on the performance of the automated driving behaviors. is study considers that the combination of a path and its traffic state profiles determines a complete spatiotemporal trajectory. erefore, in the following sections, we will describe the universal generation method of path and traffic state profiles in detail.

Path Generation.
A suitable curve should be selected to plan the lane-changing or overtaking path. Here, we choose the quintic polynomial, which has the advantages of a continuous third derivative and smooth curvature, as shown in the following equation: where x and y are the longitudinal and lateral positions of the ego vehicle, respectively. a 0 , . . ., a 5 are unknown parameters that need to be calculated. As for the lane-changing maneuver, the boundary traffic information can be collected with the help of sensors, GPS, 2 Mathematical Problems in Engineering and digital maps. Based on such information, the following equations can be obtained: where x 0 , y 0 , c 0 , and k 0 represent the longitudinal position, the lateral position, the derivative, and the curvature of the lane curve at current moment, respectively. x f , y f , c f , and k f represent the longitudinal position, the lateral position, the derivative, and the curvature of the lane curve at final moment, respectively. By solving equations (2)- (7), the values of a 0 , . . ., a 5 are obtained, and then the lane-changing path can be generated. As for overtaking maneuver, the same method as lanechanging is adopted to generate the path. A complete overtaking path is composed of two lane-changing paths, as shown in Figure 2. In the figure, vehicle 0 is the ego vehicle and vehicles 1, 2, and 3 are the surrounding vehicles.
After collecting the boundary traffic information of the ego vehicle in lane-changing or overtaking, the unknown parameters of the path function can be solved and then the suitable path is generated. e study chooses the polynomial curve to represent the path, which has the advantages of a continuous derivative and smooth curvature. Other types of curve can also be employed according to the traffic scenario, while the framework of the path generation is unchanged.

Traffic State Profile Generation.
In this section, a nonlinear programming model is constructed to generate the traffic state profiles (speed, acceleration, and jerk profiles) for the automated lane-changing or overtaking maneuver. Since the discrete traffic states of the ego vehicle may result in crashes, our model discretizes the time horizon and solves the unknown parameters of the function to generate the continuous and smooth traffic state profiles. e driving distance profile is shown in the following equation: e corresponding speed, acceleration, and jerk profiles are shown in the following equations: Six unknown parameters b 0 , . . ., b 5 need to be determined. Based on the known initial traffic states of the ego vehicle, three equations are derived as follows: where s 0,0 , v 0,0 , and a 0,0 are the current driving distance, speed, and acceleration of the ego vehicle, respectively. e values of b 0 , b 1 , and b 2 can be obtained by solving equation (12). However, the unknown parameters b 3 , b 4 , and b 5 and total driving time t f are not easy to determine, and the arbitrary values may affect the solution efficiency of the model [29,34]. erefore, we treat b 3 , b 4 , b 5 , and t f as free variables and establish a nonlinear model to search for the optimal traffic state profiles. e study divides t f into I time intervals and uses t i (i � 1, . . . , I) to denote the end moment of each time interval. Let t 0 � 0 and set constraint t i > t i−1 . As shown in Figure 3, s i is the driving distance of the ego vehicle at t i . x 0,i and y 0,i are the longitudinal and lateral positions of the ego vehicle at t i , respectively. s 0 � 0 and s i is calculated as . . , I are treated as free variables. e unknown parameters b 3 , b 4 , and b 5 and driving time t f can be represented as functions of t i . erefore, the planning traffic state profile problem can be converted into the optimization problem.

Optimal Traffic State Profile Planning.
e objective function of the constrained nonlinear optimization model is shown in equation (13). a 0,i and j 0,i denote the acceleration and jerk of the ego vehicle at t i , respectively, and t f denotes the total driving time: Mathematical Problems in Engineering 3 During the lane-changing or overtaking process, the passengers will feel comfortable when the accelerations and jerks of the ego vehicle are small, so a 0,i and j 0,i are used to reflect the comfort in equation (13). e total driving time t f can represent the efficiency. Short time indicates high efficiency.

Collision-Avoidance Constraint.
To avoid the potential collisions, the ego vehicle should maintain sufficient distance from the surrounding vehicles during lanechanging or overtaking maneuver. For illustrative purposes, the study assumes that the vehicles drive along the straight roads. is limitation can be easily removed by introducing a curved road function. As shown in Figure 4, there are three vehicles around the ego vehicle (vehicle 0). e leading vehicle on the current lane is indexed as vehicle 1, the following vehicle on the target lane is indexed as vehicle 2, and the leading vehicle on the target lane is indexed as vehicle 3. e safe distance between the ego vehicle and vehicle 1 is discussed first. As shown in Figure 5, the ego vehicle is simplified a geometry of several circles with diameter m. Following Ziegler et al. [35], the Euclidean distance between the center of each circle of the ego vehicle and the center of the first circle of vehicle 1 should be greater than diameter m of the circle. erefore, the distance constraint between the ego vehicle (vehicle 0) and vehicle 1 can be shown in the following equation: where k is used to denote the index of circles;  en, the constraints of the collision avoidance between the ego vehicle and the other two vehicles are discussed. As shown in equations (15)- (16), the Euclidean distance between the ego vehicle and vehicles 2 and 3 should be greater than the diagonal length r of the vehicle: where x 0,i , x 2,i , and x 3,i are the longitudinal central positions of the ego vehicle, vehicle 2, and vehicle 3 at t i , respectively. y 0,i , y 2,i , and y 3,i are the lateral central positions of the ego vehicle, vehicle 2, and vehicle 3 at t i , respectively.
x 0,i and y 0,i are the variables that need to be solved in the model. x 2,i and x 3,i can be calculated as x 2,0 + v 2,0 t i + 1/2 * a 2,0 t 2 i and x 3,0 + v 3,0 t i + 1/2 * a 3,0 t 2 i , respectively; y 2,i and y 3,i are the lane width 3.75 m. Here, the initial traffic states of relevant vehicles are directly derived from the sensors.

Acceleration Constraint Based on Car-following
Maneuver. In real-world traffic environment, the ego vehicle usually shifts to a car-following maneuver at the end of the lane-changing or overtaking process. To guarantee a comfort driving behavior, the planned final acceleration a 0,I of the ego vehicle should be smaller than the car-following acceleration, as shown in the following equation: where A(v 0,I , v 3,I , d 0,3 ) denotes the car-following acceleration of the ego vehicle at t I ; v 0,I and v 3,I denote the speeds of the ego vehicle and vehicle 3 at t I , respectively; and d 0,3 denotes the distance between the ego vehicle and vehicle 3 at t I .
Here, v 0,I is solved by the proposed nonlinear model; v 3,I and d 0,3 are calculated as follows, where l is the length of the vehicle: When a 0,I is smaller than the car-following acceleration, the ego vehicle will shift from a lane-changing or overtaking maneuver to a car-following maneuver comfortably, i.e., no accelerate or decrease immediately.
We choose a linear dynamic model [36] as the underlying car-following model. e car-following acceleration of the ego vehicle A(v 0,I , v 3,I , d 0,3 ) can be given as follows: where w 1 and w 2 denote the variation rate of the distance gap and the variation rate of the speed gap between the ego vehicle and vehicle 3, respectively; Δs * denotes the ideal distance gap between the ego vehicle and vehicle 3. e function of the three parameters is to help the ego vehicle reach the equilibrium condition as soon as possible by adjusting its acceleration. Following Pariota et al. [36], the parameters in the model are set as w 1 � 0.0343, w 2 � 0.948, and Δs * � 30.

Traffic State Constraint.
e speed v 0,i , acceleration a 0,i , and jerk j 0,i of the ego vehicle at t i can be obtained according to equations (9)- (11). To ensure the driving safety and comfort, the total lane-changing or overtaking time and traffic states of the ego vehicle should be simultaneously constrained as follows: a min < a 0,i < a max , for i � 1, . . . , I, where t f,max , v max , a max , and j max denote the allowable maximum total time, speed, acceleration, and jerk, Mathematical Problems in Engineering respectively. v min , a min , and j min denote the allowable minimum speed, acceleration, and jerk, respectively. In the above constraints, the speed of the ego vehicle should always be positive and not exceed the maximum allowable speed, i.e., no stop or backward motion. Moreover, the smaller the jerk, the smaller the acceleration variation, and the higher the driving comfort degree [37].

Trajectory Planning Based on the Nonlinear Model.
After receiving the lane-changing or overtaking decision, our proposed trajectory planning model will work. e steps are as follows: (1) Traffic information sensing Collecting the current traffic information of the relevant vehicles through the advanced sensors, GPS, and digital maps. If a feasible solution is found for the proposed nonlinear optimization model, the lane-changing or overtaking trajectory of the ego vehicle can be planned. However, if there is no feasible solution, the current lane will continue to be used, and the planner will iterate the above three steps until a feasible trajectory is planned. e sequence quadratic programming (SQP) algorithm is selected to solve the nonlinear optimization model. Previous studies [19,33,34] prove that this algorithm can find the feasible solution for the nonlinear problem well. rough multiple iterations, the optimal traffic state profiles of the ego vehicle are acquired based on the SQP algorithm. erefore, the optimal lane-changing or overtaking trajectory can be planned.

Simulation and Discussion
In this section, the types of test scenarios are described first; then, the performance characteristics of the simulation results are shown; finally, three detailed trajectory planning cases are demonstrated.

Scenario Description and Simulation Result Analysis.
Various test scenarios should be considered to evaluate the proposed model. Yang et al. [32] have proposed that lanechanging maneuver in real-world traffic can be roughly categorized into two types: the ego vehicle is located in front of vehicle 2 and the ego vehicle is located behind vehicle 2 at the initial moment. Similar to Yang et al. [32], we also consider that the overtaking maneuver includes two types according to the initial relative positions between the ego vehicle and vehicle 2 (see Figures 6(a) and 6(b)). In the figure, the solid vehicles indicate their current positions and the dashed vehicle indicates the final position of the ego vehicle.
As shown in Figure 6(a), under the first type of scenario, the ego vehicle is located in front of vehicle 2 at the initial moment. e ego vehicle can change lane to reach the target gap between vehicle 2 and vehicle 3 directly. Under the second type of scenario (see Figure 6(b)), the ego vehicle is located behind vehicle 2 at the initial moment. e ego vehicle needs to overtake vehicle 2 to reach the target gap between vehicle 2 and vehicle 3.
Furthermore, 100 random overtaking test cases for each type are generated to verify the feasibility and effectiveness of the proposed trajectory planning method. MATLAB is employed as the simulation platform. Table 1 presents the relevant parameters of the model, including the lower and upper boundaries of the traffic states, and the constant quantities. Inputting the current traffic information, the planner can automatically plan a comfort and safe trajectory for the ego vehicle. rough our proposed model, 82 of the 100 test cases can find the feasible overtaking trajectory under the first type of scenario; 76 of the 100 test cases can find the feasible overtaking trajectory under the second type of scenario, which proves that this method is applied to most traffic conditions. e distribution of the overtaking time is shown in Figure 7. From the figure, it can be indicated that the driving time of most cases is between 6 and 7 s for the first type of overtaking scenario. e shortest time is 4.72 s, the longest time is 10.24 s, and the average time is approximately 6.36 s. For the second type of overtaking scenario, most driving time is between 7 and 8 s, the shortest time is 4.68 s, the longest time is 11.56 s, and the average time is approximately 7.71 s.
A comprehensive presentation of the simulation results is presented in Table 2. Using the current traffic states of vehicles as the input variables, the overtaking time t f can be determined automatically by the proposed model. Since the ego vehicle needs to overtake vehicle 2 first to reach the target gap under the second type of scenario, the mean value of t f (7.71 s) is larger than the mean value of t f (6.36 s) under the first type of scenario. Moreover, the variations in the trajectory performance parameters (speed, acceleration, and jerk) in Table 2 are small, which guarantees the ride comfort.

Numerical Examples.
In this section, two special cases are demonstrated in detail. Table 3 shows the initial traffic states of vehicles under two detailed scenarios. In scenario 1, the ego vehicle (111.59 m) is located in front of vehicle 2 (100.00 m), which belongs to the first type of scenario. In scenario 2, the ego vehicle (89.17 m) is located behind vehicle 2 (100.00 m), which belongs to the second type of scenario.   Figure 8, it is shown that the overtaking longitudinal distance x f is 115 m and the total driving time t f is 7.14 s. Figure 9 indicates that the ego vehicle first trends to decelerate slightly to avoid the collisions with surrounding vehicles and then to accelerate to a maximum value and finally to decrease the speed slowly to a stable state. It can be concluded that the ego vehicle can dynamically adjust its speed for adapting the speed change in surrounding vehicles. Figures 10 and 11 display the overtaking trajectories and the variations in speeds of vehicles under scenario 2, respectively. e overtaking longitudinal distance x f is 147 m, and the total driving time t f is 9.02 s. From Figure 11, it can be seen that the ego vehicle trends to decrease the speed from 0 s to 1.69 s and then to change its speed lightly from 1.69 s to 7.35 s. After 7.35 s, the ego vehicle accelerates to a value that is larger than the final speed of vehicle 1. Figure 12 shows the speed comparison of the ego vehicle under scenarios 1 and 2. Under scenario 1, the ego vehicle decreases its speed lightly at beginning because the ego vehicle has enough initial speed gap (4.87 m/s) to adjust its position. However, since the initial speed gap (0.95 m/s) between the ego vehicle and vehicle 2 is small under scenario 2, the ego vehicle needs to decelerate quickly at the beginning for avoiding a collision with vehicle 2.     Figures 13 and 14 show the planned accelerations and jerks of the ego vehicle under two scenarios, respectively. From the figures, it can be indicated that different initial traffic states of vehicles will have a significant impact on acceleration and jerk profiles. ese trajectory performance variables of the ego vehicle under scenarios 1 and 2 are all small, which ensures the comfort of passengers during the overtaking process.

Trajectory Correction Scenario.
If the traffic states of surrounding vehicles change suddenly, the planned trajectory of the ego vehicle needs to be corrected to prevent the potential collision. e corrected trajectory can still be generated using our proposed model. Next, we will describe a trajectory correction scenario in detail. Table 4 shows the initial traffic states of the vehicles.
In this scenario, the ego vehicle originally intends to change lanes. However, vehicle 3 suddenly decelerates at 2.62 s. e ego vehicle perceives that the distance between itself and vehicle 3 will become very short, and there may be a collision. erefore, the lane-changing trajectory is corrected, and the ego vehicle returns to the original lane. Figure 15 presents the trajectories of the relevant vehicles in this scenario. e simulation result indicates that the total          longitudinal distance of the ego vehicle is 91.73 m, and the driving time is 6.64 s. During the trajectory correction process, we assume that vehicle 2 follows the leading vehicle politely, so the motion of vehicle 2 after 2.62 s is not discussed. In Figure 15, the ego vehicle successfully returns to the original lane tracking the corrected trajectory and does not collide with the surrounding vehicles. It can be concluded that the proposed model is effective for emergent traffic condition.
Furthermore, the real-time distance of the ego vehicle and surrounding vehicles is shown in Figure 16. Here, the distance denotes the driving distance from the origin point of the lane to the current position of vehicle. In Figure 16, vehicle 3 decelerates at 2.62 s, and then its speed decreases rapidly. If the ego vehicle does not correct the trajectory, it is likely to collide with vehicle 3.

Conclusion
is paper proposes a universal trajectory planning method that is suitable for the automated lane-changing and overtaking maneuvers. e proposed method first generates a two-dimensional path in the Cartesian coordinate system to connect the initial position with the final position, and then a nonlinear mathematical optimization model is established to obtain the traffic state profiles along the path. e combination of a path and its traffic state profiles determines a spatiotemporal trajectory. Moreover, considering the safety and comfort of the ego vehicle during the driving process, the study discretizes the total time horizon into several time intervals and solves the unknown parameters of the function to generate the continuous and smooth traffic state profiles. Furthermore, a series of numerical examples are conducted under the typical scenarios. e simulation results demonstrate that the intelligent vehicle can avoid the potential collisions effectively when tracking the planned trajectory. Future works will concentrate on the trajectory replanning and tracking issues for proposing a more realistic lanechanging and overtaking model.

Data Availability
e data used to support the findings of this study are included within the article. In this paper, the scenarios traffic data are simulated by MATLAB platform, and this study focuses on the establishment of the universal model.