Dynamic Trajectory Planning for Automated Lane Changing Using the Quintic Polynomial Curve

. 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 efcient LC trajectory for the AV. Tis paper proposes a novel dynamic LTP algorithm based on the quintic polynomial curve. Tis algorithm is capable of adjusting LC trajectory according to the state changes of the surrounding driving environment. Te 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. Te problem of solving the parameters of the corresponding equation is then transformed into an optimization problem, which takes driver’s safety, comfort, and efciency into account. After that, the SQP algorithm is employed to solve this optimization problem. Finally, both numerical simulation and feld-data validation are used to verify the efectiveness of our proposed algorithm. We anticipate that the research could provide certain valuable insights for developing more reliable LC algorithms for AVs.


Introduction
Numerous research studies indicate that the advent of AVs (autonomous vehicles) could signifcantly enhance trafc safety, improve trafc efciency, alleviate trafc congestion, and reduce fuel consumption [1][2][3][4][5][6][7][8].California, as the frst city to formulate regulations for road testing of AVs, has attracted world's top companies to conduct research, development, and road testing of AVs.Te 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.Te 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][15][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.Te 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.
Te 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 eforts has been made.Existing research on LTP could be mainly divided into the analytical method [1][2][3][4][5][6][7][8], artifcial potential feld method [2,4,5], and data-driven method [18,19].Te 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].Te 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 artifcial potential feld into the LTP algorithm.Te artifcial potential feld method regards the various elements of driving environment, such as road edges, static obstacles, and moving obstacles as a potential energy feld [2,4,5].Te vehicle tries to fnd a trajectory with the lowest total potential feld.Tis paper will focus on reviewing and employing the frst method since it has high reliability and fexibility 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 frst 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 efciency, 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.Te time-based quintic polynomial function is introduced to model the reference trajectory, which satisfes the safety, comfort, and efciency of the automated vehicle.Finally, simulation and experiments results demonstrate the efectiveness 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 verifed under diferent 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 diferent drivers' personalized LC needs, Huang et al. [25] incorporated personalized driving style into the automated LC trajectory planning algorithm.Te 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.Diferent 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].Te collision area of the preceding vehicle is defned to tolerate the disturbances and uncertainties.Tereafter, the generated trajectory is not allowed to cross the collision area and thus obtaining the fnal optimal trajectory.Although the abovementioned research have provided valuable insights into developing the LTP algorithm, these existing studies all share a common faw.Te 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.Te 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 trafc 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.Te results demonstrate that the proposed DLTP algorithm provide safer trajectory for AV than the traditional SLTP algorithm.
Although existing studies have achieved signifcant 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 difcult 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  Journal of Advanced Transportation a more general equation and has been widely used in the existing literature [1,22,24].Te 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 feld-data validation are designed to verify the efectiveness of the proposed algorithm.Te 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.Te felddata validation of our proposed algorithm is presented in Section 4. Te summary of this paper is provided in Section 5.

Model Framework
A typical LC schematic involves fve vehicles, including the ego vehicle and the surrounding four vehicles.For the convenience of subsequent discussion, we denote these vehicles as S ego , S 1 , S 2 , S 3 , and S 4 .Te 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).Tis paper mainly focuses on the second region, and the LC decision-making process is beyond the research scope of this study.Te 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 timebased quintic polynomial function is introduced to model the trajectory equation.Te problem of solving the parameters of the corresponding equation is then transformed into an optimization problem, which takes driver's safety, comfort, and efciency into account.After that, the SQP algorithm is employed to solve this optimization problem.

Modeling the LC Trajectory.
Te parameter planning step size t pss is introduced for replanning the LC trajectory every t pss second interval, thus enabling the AV to adjust its LC trajectory in real time proactively.For the k th planning step, the time-based quintic polynomial function is introduced, which exhibits better performance in ftting the LC trajectory and has been widely used in the existing studies [1,25].
Te longitudinal and lateral trajectory with respect to time is defned as follows: where x k (t), y k (t), and θ k (t) denote the longitudinal position, lateral position, and course angle of the ego vehicle.a i,k , i � 0, 1, ..., 5 and b j,k , j � 0, 1, ..., 5 are the corresponding coefcients.
At the frst 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.Tereafter, we could simplify the equation ( 1) and obtain the trajectory at the frst planning step.
where t ini denotes the initial time of LC, _ x(t ini ) denotes the initial speed, and D 0 denotes the lateral distance.It can be found that the simplifed formula contains only two unknown parameters, namely, t fin,1 and x fin,1 , which refer to the fnal lane change duration and lateral move distance left after the frst 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 (t fin,k and x fin,k ).

Optimization Objective Function.
Te 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 defned as the rate of acceleration change.As mentioned above, for the k th planning step, the trajectory formula could be reduced to contain only two parameters, i.e., t fin,k and x fin,k .Te corresponding formula of comfort cost function 3), which is composed of longitudinal and lateral comfort parts.Te numerator is the sum of the discomfort felt by the driver Journal of Advanced Transportation within the remaining LC duration, and the denominator is the product of the maximum acceleration and jerk of the AV.
where k � 1, 2, 3, ..., m denotes the current planning step.m denotes the total planning steps.j x,k and j y,k denote the longitudinal and lateral jerks of the AV.j x, max and j y, max denote the maximum and minimum jerks.a x, max and a y, max 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 D 0 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 efciency of the autonomous vehicle.Te higher the ratio, the lower the efciency of the AV to complete the LC.
Te formula of efciency cost function J efficiency k (t fin,k , x fin,k ) is given in the following equation, which is composed of the remaining longitudinal and lateral distance of the autonomous vehicle.
where x k−1 (t pss ) and y k−1 (t pss ) indicate the longitudinal and lateral positions of the previous planning step.
x fin,k − x k−1 (t pss ) denotes the remaining longitudinal distance.D 0 − y k−1 (t pss ) denotes the remaining lateral distance.
Terefore, we defne J total k (t fin,k , x fin,k ) as the total cost function of the AV at the k th planning step.
where ω 1 and ω 2 are the corresponding weight coefcients, which refect the tradeof between the comfort and efciency part.It is worth noting that these two values may vary diferently among drivers and driving environments.Diferent settings for these two values may lead to diferent optimal objective values.If there are no surrounding vehicles, we could choose a higher value of ω 1 .When the trafc is in a congested state, to avoid the excessive infuence of LC behavior on trafc fow, we could choose a larger value of ω 2 .
For the k th planning step, we transform the problem of solving these coefcients into an optimization problem.Te AV needs to minimize the discomfort and inefciency for the current trajectory planning step.
where t * fin,k and x * fin,k are optimal corresponding parameters when the total cost function is the lowest.Te corresponding constrains will be elaborated in the next subsection.

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.Te formula of the speed constraint is given in the following equation: where v min represents the minimum speed limit and v max represents the maximum speed limit.(2) Stability and comfort constraints: the acceleration and the jerk should be within the reasonable range.Te corresponding constraint formula is given in the following equation: where a min , a max , j min , and j max 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.Te defnition of the collision boundary area is shown in Figure 3. Te l a , l b , C a , and C b are defned 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 t, let P j (t) � (x j (t), y j (t)), j � S ego , S 1 , S 2 , S 3 , S 4 denotes the center position of the vehicle j.Te real-time boundary of the collision area of each vehicle is defned as G j (x, y).
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 G S ego (x, y) and G S 1 (x, y) represent the collision boundary of vehicle S ego and S 1 , respectively.Also, the two closet points are (x 1 , y 1 ) and (x 2 , y 2 ).Te point (x 1 , y 1 ) is on the G S ego (x, y) curve and the point (x 2 , y 2 ) is on the G S 1 (x, y) curve.Te minimum distance d S 1 S ego could be derived through Lagrange multiplier as shown as follows. 2 .λ 1 and λ 2 are Lagrange multipliers.When the distance between two points is the shortest, we need to meet the following formulas: Trough solving the abovementioned six equations, the value of the abovementioned six variables could be obtained.Tus, the real-time minimum distance between S ego and S 1 can be calculated.Te real-time minimum distance between two collision boundary ellipses should be greater than the corresponding threshold MSS 0 (the minimum safe space).Te reason for setting this threshold is to avoid collision if an emergency situation occurs.If the value MSS 0 is too small, it is difcult for the ego vehicle to cope with unexpected situations; otherwise, the efciency of LC may be afected.

SQP Algorithm.
To fnd the solution of the abovementioned optimization objective function, we introduce the SQP algorithm.Te idea of the SQP algorithm is to transform the nonlinear optimization problem with equality and inequality constraints into a quadratic programming problem.Te optimization problem of general nonlinear constraints can be expressed as follows: where f(x) refers to the equation ( 5) and x denotes the corresponding variables.c i (x) denotes the equality constraints, and c j (x) denotes the inequality constraints.

Journal of Advanced Transportation
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 H k denotes the positive defnite approximation of ∇ 2 xx L(x k , λ k ).k denotes the current number of iterations.Te 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 fnal solution set.

Summary of Our Model.
Te core part of our algorithm is the setting of the planning step parameter t pss .Te 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.Te constructed optimization objective function considers the comfort and efciency, while the safety is determined by the crash boundary model.Finally, the SQP algorithm is introduced to solve our optimization problem.

Numerical Simulation
In this section, numerical simulation is conducted to verify the efectiveness of the proposed algorithm.Te simulated parameters adopted in this section are given in Table 1, which mainly refer to existing literature [3,17,24].Te initial speed of the S ego , S 1 , S 2 , S 3 , and S 4 vehicles are assumed to be 20 m/s, the planning step size t pss is set as 1 s, and the value of comfort weight coefcient is set as 0.5.Te initial longitudinal relative position between the surrounding vehicles and the ego vehicle is all 50 m.Te 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 4 s.At the end of the frst-step trajectory planning, the AV arrives at (17.98 m, 0.21 m).Ten, the autonomous vehicle replans its second-step trajectory planning and arrives at (37.85 m, 1.51 m).After the third-step and fourth-step trajectory plannings, the AV fnally reaches the center line of the target lane.On the other hand, the lateral speed gradually increases to 1.7 m/s and then gradually decreases to 0 m/s.Te longitudinal speed drops slightly, but it is always above 20 m/s.Te cost value (J total , J comfort , J efficiency ) k of the AV at each planning step is (15.22,5.09, 25.34) k�1 , (13.43, 7.18, 19.69) k�2 , (13.2, 5.84, 20.56) k�3 , and (22.58, 8.05, 37.1) k�4 , respectively.Tis preliminary numerical simulation verifes the efectiveness of the proposed DLTP algorithm.Tis algorithm indeed achieves the efect of dynamically planning the LC trajectory.Tere are total four steps of trajectory planning in this simulation scenario.
Tereafter, we vary the value of t pss 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 t pss , AV needs to plan more steps.When t pss equals to 0.5 s, there are eight steps of trajectory planning.When t pss equals to 2 s, there are only two steps of trajectory planning.Te corresponding total cost value, comfort cost value, and efciency cost value at each planning step are also detailed presented in Table 2.When we compare the performance of the algorithm with diferent t pss , we can analyze the average, maximum, and minimum values of each cost.When t pss is 0.5 and 0.8, the average value of comfort cost is 11.26 and 7.30, the average value of efciency cost is 29.03 and 26.70, and the total cost is 20.12 and 17.00, respectively.Terefore, it may be more appropriate to choose t pss as 0.8.An excessively small t pss may lead to the increase of the discomfort cost.A smaller value of t pss 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 t pss .In essence, this may relate to the tradeof between our desire for safer LC trajectory and our desire for more comfortable LC experience.Tis may involve the further development of our algorithm in selecting the optimal t pss 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 _ x(t ini ) increasing from 15 m/s to 30 m/s, the LC duration gradually decreases from 4 s to 3.6 s.Meanwhile, the fnal 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 efciency cost gradually increases from 19.91 to 34.48.

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 feld data.[26].Tis dataset contains 16.5 hours of measurement, 45,000 kilometers of total driven distance, and over 11,500 vehicles.Tese trajectories are recorded in 4k (4096 * 2160) resolution from six diferent locations near Cologne, Germany.Te 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 fve surrounding vehicles.Te process of extracting the LC trajectories is summarized as follows.

Journal of Advanced Transportation
Step 1: we mainly focus on researching the LC of the private vehicles.We manually flter out the LC trajectories in which some of the abovementioned fve 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 fnal LC trajectory one by one.Tis is because the trajectory information may be missing at some time in the process of LC.

8
Journal of Advanced Transportation 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 fuctuate around zero as shown in Figure 6.Each specifc trajectory needs specifc 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 fve vehicles.It is worth noting that no matter what the initial input is, this algorithm could still ensure the safe completion of LC.Tis is guaranteed by the vehicle constraints (the real-time speed, acceleration, jerk, and distance constraints).Terefore, we will not adopt all trajectories for verifcation.Te comprehensive comparative analysis of the cost value between the real trajectory and the simulated trajectory is the follow-up research of this paper.Under diferent LC scenarios, the distribution of the comfort and efciency costs of feld-data LC trajectories and the approximation of the weight coefcient between these two types of cost will be studied in the future research.Tis may guide us to improve the proposed algorithm.

Simulation Results.
Te state of the surrounding vehicles of the LC trajectory is utilized as the input of the proposed DLTP algorithm, and t pss is set as 1 second.Under diferent sizes of the weight coefcient, simulation results are presented in Figure 8 (the corresponding tracks' segments id and vehicle id are also presented).When ω 1 equals to 0.1, AV is most concerned about LC efciency compared with comfort.Take the vehicle 376 as an example, the corresponding average comfort and efciency cost is about 42.31 and 15.93, respectively.Tereafter, with the gradually increase of ω 1 from 0.1 to 0.9, the driver becomes more concerned about LC comfort.Te corresponding average comfort cost gradually decreases from 42.31 to 3.43, and the average efciency cost gradually increases from 15.93 to 27.36.Meanwhile, the LC duration gradually increase from 2.9 s to 4.8 s.
Te comparison with the real trajectory data and the trajectory simulated by the SLTP algorithm is also conducted.ω 1 and t pss are set as 0.5 and 1s, respectively.Te right part of Figure 9

Conclusion and Future Work
Tis paper focuses on developing the DLTP algorithm.Specifcally, for each planning step, the quintic polynomial curve is introduced as the underlying form of the trajectory equation.Te problem of determining the corresponding parameter is then transformed into an optimization problem, which takes driver's safety, comfort, and efciency into account.Tereafter, to verify the efectiveness of our proposed algorithm, preliminary numerical simulation has been conducted at frst.Te corresponding numerical results demonstrate that our proposed algorithm could achieve the efect of dynamically replanning its LC trajectory.Te 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, feld-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.Te feld-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 feld-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.Te 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.Te corresponding LTT algorithm will be accomplished in the future research of this study.Second, the exploration of the distribution of comfort and efciency cost of feld-data LC trajectories and the comparison between the feld-data trajectories and the simulated trajectories may guide us to improve our proposed algorithm.Tird, the incorporation of the prediction algorithm into the LTP algorithm could also have been imbedded into our proposed algorithm.Tis 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.Tey are proprietary or confdential in nature and may only be provided with restrictions.

Figure 1 :
Figure 1: Te schematic diagram of LC of the autonomous vehicle.

Figure 3 :
Figure 3: Te boundary of the collision area of the ego vehicle.

Figure 4 :
Figure 4: Preliminary numerical result of the proposed algorithm (four planning steps in this scenario).

Figure 5 :
Figure 5: Sensitivity analysis of the initial speed of the AV.

Figure 6 :Figure 7 :
Figure 6: Example of determining the starting and ending points of the LC trajectory.

Figure 8 :
Figure 8: Sensitivity analysis of the weight coefcient of the objective function.

Figure 9 :
Figure 9: Comparison between the real trajectory with the simulated trajectory.

Table 1 :
Te numerical parameters used in this section.

Table 2 :
Sensitivity analysis result of the planning step size.

Table 3 :
Comparison results of the cost between the feld data with simulated data.

Table 3 .
Te results demonstrate that our proposed algorithm could provide more efcient and more comfort LC trajectories than original trajectories.