Abstract
As one of the key algorithms in supporting AV (autonomous vehicle) to complete the LC (lane changing) maneuver, the LTP (LC trajectory planning) algorithm generates safe and efficient LC trajectory for the AV. This paper proposes a novel dynamic LTP algorithm based on the quintic polynomial curve. This algorithm is capable of adjusting LC trajectory according to the state changes of the surrounding driving environment. The formulation of our proposed algorithm mainly consists the underlying form of trajectory equation, the optimization objective function, the corresponding constrains, and the SQP (sequential quadratic programming) algorithm. For each planning step, the time-based quintic polynomial function is introduced to model the trajectory equation. The problem of solving the parameters of the corresponding equation is then transformed into an optimization problem, which takes driver’s safety, comfort, and efficiency into account. After that, the SQP algorithm is employed to solve this optimization problem. Finally, both numerical simulation and field-data validation are used to verify the effectiveness of our proposed algorithm. We anticipate that the research could provide certain valuable insights for developing more reliable LC algorithms for AVs.
1. Introduction
Numerous research studies indicate that the advent of AVs (autonomous vehicles) could significantly enhance traffic safety, improve traffic efficiency, alleviate traffic congestion, and reduce fuel consumption [1–8]. California, as the first city to formulate regulations for road testing of AVs, has attracted world’s top companies to conduct research, development, and road testing of AVs. The 2020 autonomous driving road test data released by DMV [9] showed that Waymo and Crusie have conducted nearly 630,000 and 770,000 miles of testing. The corresponding MPI (miles per intervention, the average number of miles traveled between every two manual interventions) is about 0.033 and 0.035, respectively. It is foreseeable that high-level AV will soon appear in daily life.
One of the indispensable components of autonomous vehicle technology is the lateral maneuver research, which is a challenging undertaking that requires the exploration of solution spaces to achieve optimal safety, mobility, and environmental factors [10]. Generally speaking, the research on lateral maneuver research can be roughly divided into modeling the decision-making process of LC [4, 11, 12], the impact of LC on surroundings [13], the duration of LC [14–16], and the LC trajectory planning [1–5, 7, 8, 17–19]. Since this paper concentrates on LTP (LC trajectory planning), we will mainly focus on reviewing the literature which are closely related to our research theme. The LTP (LC trajectory planning) algorithm is one of the most important algorithms in supporting the autonomous vehicle to complete the LC maneuver. When the AV has decided and is about to execute LC maneuver, AV needs to specify an LC trajectory in advance and then drive along this trajectory until it arrives the center line in the target lane. As shown in Figure 1, the center position of the autonomous vehicle at each time step forms the entire shape of the LC trajectory. The LTP algorithm is employed to address several questions related to this reference trajectory as follows: (1) what is the mathematical equation form of the LC trajectory curve? (2) what factors should we consider to obtain the corresponding parameters? and (3) how to control the vehicle to accurately track the planned trajectory?
Over the past decades, a considerable amount of efforts has been made. Existing research on LTP could be mainly divided into the analytical method [1–8], artificial potential field method [2, 4, 5], and data-driven method [18, 19]. The analytical method sets the trajectory equation in advance, takes the needs of the LC vehicle as the optimization objective, and solves the optimal lane-changing trajectory [1–5, 7, 8, 16]. The data-driven method usually refers to the method of using the machine learning or deep learning algorithm, which aims to extract LC dynamics from massive data instead of describing the nature of things [18, 19]. Recently, many scholars have tried to introduce the artificial potential field into the LTP algorithm. The artificial potential field method regards the various elements of driving environment, such as road edges, static obstacles, and moving obstacles as a potential energy field [2, 4, 5]. The vehicle tries to find a trajectory with the lowest total potential field. This paper will focus on reviewing and employing the first method since it has high reliability and flexibility without being limited by various situations [20].
If we adopt the analytical method, the approximate curve equation of LC trajectory has to be determined in the first place. Up to now, the most commonly used mathematical equations are the quintic polynomial equation [1, 21, 22], cubic polynomial equation [17], sine (cosine and trapezoidal) curve equation [23], etc. After determining the curve equation, the problem of determining the values of corresponding parameters are generally transformed into an optimization problem, which usually takes driving comfort, driving efficiency, and driving safety into account. Subsequently, the optimization algorithm (i.e., sequential quadratic programming [24], the interior-point algorithm [1, 25], etc.) is introduced to solve this optimization problem. After that, we could acquire the mathematical equation of the LC trajectory of the autonomous vehicle, and the autonomous vehicle will then drive along with this trajectory until it arrives at the center line of the target lane.
Based on the vehicle-to-vehicle communication, the dynamic automated LC maneuver algorithm was proposed [1], which is composed of the trajectory planning algorithm and the trajectory tracking algorithm. The time-based quintic polynomial function is introduced to model the reference trajectory, which satisfies the safety, comfort, and efficiency of the automated vehicle. Finally, simulation and experiments results demonstrate the effectiveness of the proposed algorithm. Using the same mathematics equation, Bai et al. [21] introduced the quintic polynomial function to model the accelerated lane-changing characteristics, which considers the collaboration with the following vehicle in the target lane. Furthermore, Bai et al. [21] established the rectangular collision boundary of the subject vehicle so as to analyze the possible collision points. Finally, this algorithm is verified under different accelerated lane-changing scenarios. Yue et al. [22] introduced the time-based quintic polynomial function for the implementation of trajectory planning and develops a robust tube-based model predictive control method for an underactuated tractor-trailer vehicle system.
In order to satisfy different drivers’ personalized LC needs, Huang et al. [25] incorporated personalized driving style into the automated LC trajectory planning algorithm. The time-based quintic polynomial function is also introduced to model the LC trajectory. Simulation results demonstrate that the driving style adaptive LC trajectory model can meet the driver’s personalized LC needs very well. On the other hand, in order to develop the cooperative LTP algorithm, a multivehicle cooperative automated LTP algorithm is proposed in Luo et al. [24]. Due to the involvement of multiple cars at the same time, the cooperative safety spacing model is proposed to guarantee and improve the safety of the vehicles. Different from previous research, the prediction of the states of surrounding vehicles is integrated into the LTP algorithm so as to reduce the risk of possible collisions [3]. The collision area of the preceding vehicle is defined to tolerate the disturbances and uncertainties. Thereafter, the generated trajectory is not allowed to cross the collision area and thus obtaining the final optimal trajectory. Although the abovementioned research have provided valuable insights into developing the LTP algorithm, these existing studies all share a common flaw. The algorithms adopted in these studies could all be viewed as the SLTP (static LTP) algorithm [17], which fails to react to the changes of the states of surroundings vehicles. The AV plans the LC trajectory only once, which is based on the state of surroundings before the execution of LC. Nevertheless, in the actual driving environment, the states of the surrounding vehicles may change randomly (i.e., suddenly accelerates or decelerates), and if the AV drives along with the original LC trajectory, there may be a traffic accident or even death or injury [17]. In order to bridge this research gap, the DLTP (dynamic LTP) algorithm is proposed by Yang et al. [17], which is capable of replanning the LC trajectory at certain frequency. The results demonstrate that the proposed DLTP algorithm provide safer trajectory for AV than the traditional SLTP algorithm.
Although existing studies have achieved significant results, these algorithms cannot adapt to changes in the surrounding environment. If the speed of surrounding vehicles changes greatly during the LC process, it is difficult to adjust the trajectory planned by itself in real time, which is accompanied by greater driving risks. To address this gap, this paper proposes a novel DLTP algorithm based on the quintic polynomial function. Compared with the cubic polynomial function, this kind of polynomial function is a more general equation and has been widely used in the existing literature [1, 22, 24]. The formulation of our proposed algorithm mainly consists the underlying form of trajectory equation, the optimization objective function of the trajectory, the constrains of the vehicle, and the SQP algorithm. Finally, both numerical simulation and field-data validation are designed to verify the effectiveness of the proposed algorithm. The subsequent sections of this paper are organized as follows. Section 2 presents the structure of the proposed LTP algorithm. Section 3 presents the numerical simulations on the proposed algorithm. The field-data validation of our proposed algorithm is presented in Section 4. The summary of this paper is provided in Section 5.
2. Model Framework
A typical LC schematic involves five vehicles, including the ego vehicle and the surrounding four vehicles. For the convenience of subsequent discussion, we denote these vehicles as . The typical LC process is divided into two regions as shown in Figure 2. (1) Preparation region: when the AV has decided to perform LC, it will still drive forward until there is a safe gap distance in the target lane. (2) Execution region: when the AV is about to execute LC, it will gradually move from the current lane to the target lane (from the starting point to the ending point as shown in Figure 2). This paper mainly focuses on the second region, and the LC decision-making process is beyond the research scope of this study. The input of this algorithm is the real-time state of surrounding vehicles, and the output is the state of the ego vehicle. For each planning step, the time-based quintic polynomial function is introduced to model the trajectory equation. The problem of solving the parameters of the corresponding equation is then transformed into an optimization problem, which takes driver’s safety, comfort, and efficiency into account. After that, the SQP algorithm is employed to solve this optimization problem.
2.1. Modeling the LC Trajectory
The parameter planning step size is introduced for replanning the LC trajectory every second interval, thus enabling the AV to adjust its LC trajectory in real time proactively. For the planning step, the time-based quintic polynomial function is introduced, which exhibits better performance in fitting the LC trajectory and has been widely used in the existing studies [1, 25]. The longitudinal and lateral trajectory with respect to time is defined as follows:where denote the longitudinal position, lateral position, and course angle of the ego vehicle. and are the corresponding coefficients.
At the first planning step, it is reasonable to assume that the velocity and acceleration of AV are desired to be zero at the start and end position in the lateral direction. Also, we assume the ego vehicle has the same speed at the starting and ending point in the longitudinal direction. Thereafter, we could simplify the equation (1) and obtain the trajectory at the first planning step.where denotes the initial time of LC, denotes the initial speed, and denotes the lateral distance. It can be found that the simplified formula contains only two unknown parameters, namely, and , which refer to the final lane change duration and lateral move distance left after the first planning step. As for the other planning steps, it is also reasonable for us to assume that the position and speed should be continuous at the intersection of the adjacent LC trajectory curves. Note that since the starting and ending state of the vehicle could be obtained, the trajectory formula at any planning step can be reduced to only two parameters ( and ).
2.2. Optimization Objective Function
The AV needs to make driver feel comfortable during the process of LC, so the acceleration of the AV should change smoothly rather than abruptly. Hence, we introduce the variable, jerk, to serve as a measure of driver’s comfort/discomfort, which is defined as the rate of acceleration change. As mentioned above, for the planning step, the trajectory formula could be reduced to contain only two parameters, i.e., and . The corresponding formula of comfort cost function is presented in equation (3), which is composed of longitudinal and lateral comfort parts. The numerator is the sum of the discomfort felt by the driver within the remaining LC duration, and the denominator is the product of the maximum acceleration and jerk of the AV.where denotes the current planning step. denotes the total planning steps. and denote the longitudinal and lateral jerks of the AV. and denote the maximum and minimum jerks. and denote the maximum and minimum accelerations.
On the other hand, the AV hopes to complete the LC maneuver as soon as possible to minimize the impact of LC on surroundings or avoid excessive occupation of the surrounding road resources. At the initial point of trajectory planning, the lateral movement distance is known, while the longitudinal movement distance will vary with the parameters of the trajectory curve. Hence, we introduce the ratio of the longitudinal movement distance and lateral movement distance as a measure of LC efficiency of the autonomous vehicle. The higher the ratio, the lower the efficiency of the AV to complete the LC.
The formula of efficiency cost function is given in the following equation, which is composed of the remaining longitudinal and lateral distance of the autonomous vehicle.where and indicate the longitudinal and lateral positions of the previous planning step. denotes the remaining longitudinal distance. denotes the remaining lateral distance.
Therefore, we define as the total cost function of the AV at the planning step.where and are the corresponding weight coefficients, which reflect the tradeoff between the comfort and efficiency part. It is worth noting that these two values may vary differently among drivers and driving environments. Different settings for these two values may lead to different optimal objective values. If there are no surrounding vehicles, we could choose a higher value of . When the traffic is in a congested state, to avoid the excessive influence of LC behavior on traffic flow, we could choose a larger value of .
For the planning step, we transform the problem of solving these coefficients into an optimization problem. The AV needs to minimize the discomfort and inefficiency for the current trajectory planning step.where and are optimal corresponding parameters when the total cost function is the lowest. The corresponding constrains will be elaborated in the next subsection.
2.3. Constrains for the Vehicle
When solving this optimization problem, it is inevitable to consider the corresponding constraints: speed constraint, stability and comfort constraints, and safety constraint.(1)Speed constraint: the speed of the AV should not exceed the maximum speed but should be greater than the minimum speed. The formula of the speed constraint is given in the following equation:where represents the minimum speed limit and represents the maximum speed limit.(2)Stability and comfort constraints: the acceleration and the jerk should be within the reasonable range. The corresponding constraint formula is given in the following equation:where , , , and denote the maximum acceleration, minimum acceleration, minimum jerk, and maximum jerk, respectively.(3)Safety constraint: the AV must not collide with surrounding vehicles at any time. The definition of the collision boundary area is shown in Figure 3. The are defined as the vehicle length, vehicle width, ellipse long radius, and ellipse short radius, respectively.
Taking the starting point as the origin of coordinates, suppose at time , let denotes the center position of the vehicle . The real-time boundary of the collision area of each vehicle is defined as .
It is worth noting that the four corners of the smallest circumscribed rectangle of the vehicle outline should fall on the ellipse or within the ellipse.
We assume that and represent the collision boundary of vehicle and , respectively. Also, the two closet points are and . The point is on the curve and the point is on the curve. The minimum distance could be derived through Lagrange multiplier as shown as follows.where . and are Lagrange multipliers. When the distance between two points is the shortest, we need to meet the following formulas:
Through solving the abovementioned six equations, the value of the abovementioned six variables could be obtained. Thus, the real-time minimum distance between and can be calculated. The real-time minimum distance between two collision boundary ellipses should be greater than the corresponding threshold (the minimum safe space). The reason for setting this threshold is to avoid collision if an emergency situation occurs. If the value is too small, it is difficult for the ego vehicle to cope with unexpected situations; otherwise, the efficiency of LC may be affected.
2.4. SQP Algorithm
To find the solution of the abovementioned optimization objective function, we introduce the SQP algorithm. The idea of the SQP algorithm is to transform the nonlinear optimization problem with equality and inequality constraints into a quadratic programming problem. The optimization problem of general nonlinear constraints can be expressed as follows:where refers to the equation (5) and denotes the corresponding variables. denotes the equality constraints, and denotes the inequality constraints.
After the initial point is given, the Lagrange equation of the objective optimization function is approximated quadratically and then the subproblem of quadratic programming is obtained as follows:where denotes the positive definite approximation of . denotes the current number of iterations. The quasi-Newton method can be used to approximate the solution of the quadratic programming subproblem as the search direction of the next iteration, thus converging to the final solution set.
2.5. Summary of Our Model
The core part of our algorithm is the setting of the planning step parameter . The change in the magnitude of its value helps us to adjust the planned lane change trajectory at regular intervals. Equation (2) is obtained by simplifying the speed and acceleration at the starting and ending points in equation (1) so that the objective function contains only two variables. The constructed optimization objective function considers the comfort and efficiency, while the safety is determined by the crash boundary model. Finally, the SQP algorithm is introduced to solve our optimization problem.
3. Numerical Simulation
In this section, numerical simulation is conducted to verify the effectiveness of the proposed algorithm. The simulated parameters adopted in this section are given in Table 1, which mainly refer to existing literature [3, 17, 24]. The initial speed of the vehicles are assumed to be , the planning step size is set as , and the value of comfort weight coefficient is set as 0.5. The initial longitudinal relative position between the surrounding vehicles and the ego vehicle is all . The preliminary numerical result is given in Figure 4.
Under this simulation scenario, there are total four steps of trajectory planning, and the total duration is about . At the end of the first-step trajectory planning, the AV arrives at . Then, the autonomous vehicle replans its second-step trajectory planning and arrives at . After the third-step and fourth-step trajectory plannings, the AV finally reaches the center line of the target lane. On the other hand, the lateral speed gradually increases to and then gradually decreases to . The longitudinal speed drops slightly, but it is always above . The cost value of the AV at each planning step is , , , and , respectively. This preliminary numerical simulation verifies the effectiveness of the proposed DLTP algorithm. This algorithm indeed achieves the effect of dynamically planning the LC trajectory. There are total four steps of trajectory planning in this simulation scenario.
Thereafter, we vary the value of from 0.5 s to 2 s, and the corresponding results is given in Table 2. It could be found that with the decrease of the , AV needs to plan more steps. When equals to 0.5 s, there are eight steps of trajectory planning. When equals to 2 s, there are only two steps of trajectory planning. The corresponding total cost value, comfort cost value, and efficiency cost value at each planning step are also detailed presented in Table 2. When we compare the performance of the algorithm with different , we can analyze the average, maximum, and minimum values of each cost. When is 0.5 and 0.8, the average value of comfort cost is 11.26 and 7.30, the average value of efficiency cost is 29.03 and 26.70, and the total cost is 20.12 and 17.00, respectively. Therefore, it may be more appropriate to choose as 0.8. An excessively small may lead to the increase of the discomfort cost. A smaller value of could be set in a more complicated LC environment. On the other hand, if the LC conditions are comfortable (i.e., no surrounding vehicles or too far apart), the AV could choose a large . In essence, this may relate to the tradeoff between our desire for safer LC trajectory and our desire for more comfortable LC experience. This may involve the further development of our algorithm in selecting the optimal value for the AV. How to calculate this value will be accomplished in future research. Figure 5 presents the sensitivity analysis results of the initial speed of the ego vehicle. It can be found that with increasing from 15 m/s to 30 m/s, the LC duration gradually decreases from 4 s to 3.6 s. Meanwhile, the final longitudinal position also increases from 69.69 m to 120.68 m, the corresponding average comfort cost gradually increases from 4.00 to 8.33, and the corresponding average efficiency cost gradually increases from 19.91 to 34.48.
4. Field Data Validation
In this section, the highD dataset is employed to validate our proposed algorithm. First, the processing process of this dataset is presented. Second, we compare and analyze the simulation results with the field data.
4.1. highD Dataset
The highD dataset is a new dataset of naturalistic vehicle trajectories recorded on German highways during 2017 and 2018 [26]. This dataset contains 16.5 hours of measurement, 45,000 kilometers of total driven distance, and over 11,500 vehicles. These trajectories are recorded in 4k (4096 2160) resolution from six different locations near Cologne, Germany. The total driven distance of this dataset is about 45,000 kilometers, and the total recorded hour is about 16.5. At the same time, the positioning error of each trajectory is typically less than ten centimeters. For the details of this new dataset, please refer to reference [26]. As shown in Figure 1, we mainly focus on extracting the LC trajectory which involves five surrounding vehicles. The process of extracting the LC trajectories is summarized as follows. Step 1: we mainly focus on researching the LC of the private vehicles. We manually filter out the LC trajectories in which some of the abovementioned five vehicles are missing. Step 2: we determine the beginning and ending points of LC according to the lateral speed of the subject vehicle. It is reasonable for us to assume that the lateral speed of the subject vehicle equals to zero at these two points. Step 3: after the abovementioned two steps, we roughly got a preliminary data processing result. Finally, we manually verify the final LC trajectory one by one. This is because the trajectory information may be missing at some time in the process of LC.
Figure 6 exhibits an example of the process of determining the starting and ending points of the LC trajectory. It is worth noting that when determining the starting and ending points, we cannot rely solely on the variable of speed. It is also necessary to consider the variable of acceleration and position since the speed and acceleration of the vehicle fluctuate around zero as shown in Figure 6. Each specific trajectory needs specific analysis and determination. Finally, we have obtained a total of 560 LC trajectories. Figure 7 presents three examples of LC trajectory, and each LC trajectory involves five vehicles. It is worth noting that no matter what the initial input is, this algorithm could still ensure the safe completion of LC. This is guaranteed by the vehicle constraints (the real-time speed, acceleration, jerk, and distance constraints). Therefore, we will not adopt all trajectories for verification. The comprehensive comparative analysis of the cost value between the real trajectory and the simulated trajectory is the follow-up research of this paper. Under different LC scenarios, the distribution of the comfort and efficiency costs of field-data LC trajectories and the approximation of the weight coefficient between these two types of cost will be studied in the future research. This may guide us to improve the proposed algorithm.
4.2. Simulation Results
The state of the surrounding vehicles of the LC trajectory is utilized as the input of the proposed DLTP algorithm, and is set as 1 second. Under different sizes of the weight coefficient, simulation results are presented in Figure 8 (the corresponding tracks’ segments id and vehicle id are also presented). When equals to 0.1, AV is most concerned about LC efficiency compared with comfort. Take the vehicle 376 as an example, the corresponding average comfort and efficiency cost is about 42.31 and 15.93, respectively. Thereafter, with the gradually increase of from 0.1 to 0.9, the driver becomes more concerned about LC comfort. The corresponding average comfort cost gradually decreases from 42.31 to 3.43, and the average efficiency cost gradually increases from 15.93 to 27.36. Meanwhile, the LC duration gradually increase from 2.9 s to 4.8 s.
The comparison with the real trajectory data and the trajectory simulated by the SLTP algorithm is also conducted. and are set as 0.5 and 1s, respectively. The right part of Figure 9 represents the actual speed of the surrounding vehicles. The left part is the comparison of the lateral trajectory of the ego vehicle. In these three examples, the speed of the target-front vehicle decelerates and the speed of the target-rear vehicle gradually increases. The results demonstrate that the proposed DLTP algorithm could adjust its LC trajectory according to the change of surroundings, while the SLTP algorithm fails to adapt to this change. Furthermore, the corresponding average comfort and efficiency cost for each LC trajectory are presented in Table 3. The results demonstrate that our proposed algorithm could provide more efficient and more comfort LC trajectories than original trajectories.
5. Conclusion and Future Work
This paper focuses on developing the DLTP algorithm. Specifically, for each planning step, the quintic polynomial curve is introduced as the underlying form of the trajectory equation. The problem of determining the corresponding parameter is then transformed into an optimization problem, which takes driver’s safety, comfort, and efficiency into account. Thereafter, to verify the effectiveness of our proposed algorithm, preliminary numerical simulation has been conducted at first. The corresponding numerical results demonstrate that our proposed algorithm could achieve the effect of dynamically replanning its LC trajectory. The detailed trajectory information for each planning step has been provided, including the speed, acceleration, position, and the corresponding cost values. At the same time, sensitivity analysis on the planning step size and the initial speed has been carried out.
Along with numerical simulation, field-data validation has also been conducted in this paper. Since no actual automated LC trajectory is available at this point, it is at least appropriate to employ the naturalistic LC trajectories to examine the performance of our proposed algorithm. Several LC trajectories are extracted from the highD dataset. The field-data validation results demonstrate that our proposed algorithm could provide safer LC trajectory than the traditional SLTP algorithm. Meanwhile, our proposed algorithm exhibits better performance in terms of the average total cost than the field-data trajectory. To some extent, this indicates that the AV could provide more ideal trajectory than the human-drive vehicle.
Undoubtfully, many aspects of this paper need further research. First, an issue that remains to be explored is to develop the corresponding LTT (LC trajectory tracking) algorithm. The LTT algorithm focuses on controlling the autonomous vehicle to follow a given trajectory, which is also an indispensable part in supporting automated LC. However, due to page limit, we only focus on developing the LTP algorithm in this paper. The corresponding LTT algorithm will be accomplished in the future research of this study. Second, the exploration of the distribution of comfort and efficiency cost of field-data LC trajectories and the comparison between the field-data trajectories and the simulated trajectories may guide us to improve our proposed algorithm. Third, the incorporation of the prediction algorithm into the LTP algorithm could also have been imbedded into our proposed algorithm. This may further reduce the risk of collisions, thus enhancing the safety of LC.
Data Availability
Some or all data, models, or code generated or used during the study are available in a repository online in accordance with funder data retention policies (highD dataset). Some or all data, models, or code generated or used during the study are provided by the National Key R&D Program. They are proprietary or confidential in nature and may only be provided with restrictions.
Disclosure
An earlier version of this paper has been presented as conference paper in Transportation Research Board 2021 Annual Meeting.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Authors’ Contributions
Yang Li was responsible for the methodology and writing the original draft. Linbo Li was responsible for conceptualization, data curation, funding acquisition, and writing the original draft. Daiheng Ni was investigated the study and reviewed and edited the manuscript.
Acknowledgments
This work was supported by National Key R&D Program of China (grant no. 2019YFB1600703), the National Natural Science Foundation of China (grant no. 52172331), and the Fundamental Research Funds for the Central Universities (grant no. 22120230310).