Minimum-time lane changing problem of vehicle handling inverse dynamics based on adaptive mesh refinement and collocation optimization method

. In order to solve the problems of multiple constraints, many different calculations of nonlinear equations which lead to major errors in the process of vehicle lane changing with minimum time, an adaptive mesh refinement and collocation optimization method is proposed. Firstly, the problem of vehicle lane changing with minimum time has been divided into nonlinear programming problems in different grids. The Lagrange interpolation polynomial was used to approximate the solution of the optimization problem in the grid, and the absolute and relative errors were resolved. Then, a mesh was determined for the unsmooth part according to the curvature of the trajectory, and the location and number of meshes were further determined according to the relationship between the maximum relative error and the allowable error. At the same time, the solution accuracy was improved by adding an adaptive calculation to the smooth interval which does not meet the tolerance error. Finally, the simulation example of comparison with the traditional optimization method was proposed. The results showed that the algorithm presented in the paper had a higher solution efficiency under the same calculation accuracy.


Introduction
The rapid development of automobile technology has brought great convenience to human travel and life, and has also driven the rapid growth of automobile ownership in recent years. At present, automobile has become one of the pillar industries of national economy in various countries. However, due to some improper operations of drivers in complex traffic scenes, more and more traffic accidents seriously threaten the safety of human life. The longitudinal and lateral movements of vehicles are not completely independent, but there is a nonlinear coupling relationship. In some cases, the longitudinal and lateral coupling nonlinear characteristics will be very significant, and will affect the accuracy of vehicle motion control and even tracking, and even cannot ensure the vehicle stability in serious cases. For example, when an intelligent vehicle encounters sudden dangerous situations such as sudden braking in front of a static obstacle in the same lane or sudden emergency braking in front of the preceding vehicle, it can learn from an experienced driver to make a lateral lane change with a large range of steering operations. And at the same time, a certain braking operation is introduced to better increase the distance from the dangerous obstacles. At this time, the motion state of the intelligent vehicle traveling with large longitudinal and lateral accelerations may exceed the boundary of the power domain corresponding to the steady state condition, leading the vehicle to enter a strong transient condition with significant nonlinear dynamic characteristics of longitudinal and lateral coupling. Under such a strong transient condition, the longitudinal component of the lateral force of the vehicle will aggravate the impact on the longitudinal speed of the vehicle. And the change of the longitudinal speed will also cause the change of the centrifugal force and the vertical load, causing interference to the lateral motion state of the vehicle. With the action range of longitudinal and lateral acceleration further approaching to the limit boundary value of the dynamic domain provided by road adhesion, the vehicle will obtain a critical instability condition. In order to ensure the control margin of the vehicle, the intelligent driving system usually limits the lateral acceleration in its trajectory plan. Therefore, under the critical instability condition, the vehicle is often accompanied by a large longitudinal acceleration, which causes a large change in the longitudinal speed, leading to a more significant nonlinear dynamics of the vehicle longitudinal and lateral coupling. As the current control strategy considers the longitudinal and lateral designs of intelligent vehicle motion separately and lacks comprehensive consideration of the above longitudinal and lateral coupling nonlinear dynamic characteristics, the vehicle motion control performance may decline significantly under strong transient, critical instability and other working conditions, which lead to vehicle instability too [1].
In the field of intelligent vehicle research, the content related to autonomous lane changing has always been one of the research hotspots in the industry. According to incomplete statistics, vehicle traffic accidents when vehicles change lanes occur every year in the quantity of about 15 %-20 %. In addition, traffic jams caused by changing lanes account for about 25 % of the total. Lane changing is a complex vehicle behavior, which not only affects the safety, efficiency and comfort of the vehicle itself, but also greatly affects the performance of the entire traffic system as a part of the traffic flow. Therefore, it is of great significance to develop a set of intelligent vehicle automatic lane changing system to meet multi-objective optimization. From the current market situation, a very mature vehicle automatic lane change system that has reached the mass production level has not yet appeared [2].
The problem of vehicle lane change motion planning has been widely studied. A brief review is presented in the following.
Naskath et al. [3] analyzed the connectivity of high-speed mobility and lane changing based on discretionary lane changing approach for V2V environment. Muhamad et al. [4] proposed a yaw rejection control for a single-trailer truck using a steerable wheel located at the middle axle of the truck. Tolga et al. [5] analyzed autonomous electric vehicles and compared their potential environmental impacts with other public transport types, carpooling, walking, cycling, and various transportation policy applications. Cao et al. [6] designed a lane change algorithm based on a look-ahead concept for the vehicle driving in front of the emergency vehicle to avoid emergency situations. Jiang et al. [7] presented a lane-level vehicle counting system which was based on V2X communications and centimeter-level positioning technologies. Yang et al. [8] presented a road environment representation method based on a dynamic occupancy grid for the lane changing assistance strategy. Vaishnavi et al. [9] developed a lane optimization model for the CAV system for solving the complexities of multilane merging areas. Amit et al. [10] proposed a microscopic traffic flow model which was intended to explain accurately the lane changing activity using Cellular Automata. Xiao et al. [11] proposed a non-lane-discipline-based car-following model for solving the problem of vehicle lane changing. Huang et al. [12] proposed a neural network-based method to predict the future status of the local vehicle using the information from V2X, and estimate the future energy consumption of each lane. Panagiotis et al. [13] proposed a methodology for modeling the likelihood of lane changing at intersections to quantify the favorability of the surrounding environment towards lane changing and simple LSTM modeling structures. Wang et al. [14] adopted a generalized dynamic model for automatic lane-changing to solve the limitations of macroscopic evaluation. H. Hamedi et al. [15] studied the vehicles' lane change trajectories to present three novel prediction models for trajectory-grounded position. In order to investigate the characteristics of double fire sources, Guo et al. [16] performed four full-scale fire experiments in the Jiangpu road tunnel. Sharma et al. [17] proposed a novel hierarchical software architecture for the prediction of lane changing behavior on highways. Zhou et al. [18] proposed a hierarchical control strategy for the vehicle stability control. Wang et al. [19] proposed a two-stage crash causation analysis method based on the pre-crash scenarios and a crash causation derivation framework that systematically categorized and analyzed contributing 3 factors. Wei et al. [20] proposed a prediction model based on an attention-aided encoder-decoder structure and deep neural network (DNN) to solve the problems of low prediction accuracy, difficulty in long-term prediction. Zhang et al. [21] carried out a naturalistic driving study on a highway, from which a large amount of on-road data consisting of lane-keeping and lane-changing left and right maneuvers were collected. Tajalli et al. [22] presented a methodology for optimal control of connected automated vehicles in freeway segments with a lane drop. He et al. [23] proposed a novel observation adversarial reinforcement learning approach for robust lane change decision making of autonomous vehicles. Shi, et al. [24] proposed an integrated deep learningbased two-dimension trajectory prediction model which could predict combined behaviors. Li et al. [25] proposed a lane change decision-making framework based on deep reinforcement learning to find a risk-aware driving decision strategy with the minimum expected risk for autonomous driving to ensure the driving safety.
Some of the methods provided in the above documents have large solution errors, some cannot converge to the optimal solution, and some have low accuracy. In order to solve the above problems, the relative demerits were first eliminated in discrete grids based on the hp-adaptive pseudospectral method as a direct method. Then, the mesh to be refined is determined according to the curvature of state and control variables. Finally, according to the relationship between relative and tolerance error, the number of sub grids and the number of collocations in the grid are determined, and then a solution method for vehicle lane changing with minimum time is constructed. A typical example is used to verify the effectiveness of the method. In the process of solving minimum time lane changing problem of vehicle handling inverse dynamics by a direct method, a large number of nodes need to be added to improve the accuracy of solution. So, it will lead to a dense Jacobian matrix for solving nonlinear programming problems, high order of interpolation polynomial, large amount of required calculations. Moreover, the increase of nodes will also lead to the poor convergence of the algorithm. Therefore, it is necessary to optimize the mesh and collocation. For the problem of minimum time lane changing of vehicle handling inverse dynamics, the number of sub-grid divisions is provided by using the relative error, and the location of sub-grid divisions is determined by using the density function. The adaptive mesh and refinement and collocation optimization method can approach the optimal solution by densifying the mesh and adding collocation points nearby. In areas with high smoothness, fewer collocations can be used with the strong adaptability. In the process of optimization, the mesh should not be refined for the smooth parts that do not meet the requirements of allowable error, while the collocation should be added. The method can adaptively increase the number of collocation and reduce the number of meshes, so that the total number of collocations generated in the solution area is less, reducing the number of discrete nonlinear programming equations, improving the speed of solving the optimal trajectory by the SQP method.

Mathematical model of vehicle lane change problem
A nonlinear 7-DOF vehicle model shown in Fig. 1 is used to describe the vehicle lane change problem.
In the state space form, it is [26]: The state equation can be described as: where ( ) and ( ) are the state and the input which are denoted respectively as The parameters and the corresponding definitions can be found in Table 1.

Tire model
This article adopts the "Magic formula" tire model which expression is: The parameters and the corresponding definitions in Eqs. (3)-(5) can be found in Table 2.

Optimal control object of lane change motion planning problem
The minimum time problem of lane change motion planning can be regarded as the optimal control problem in the control theory. Therefore, the minimum time performance indicator is: where: is the initial time, ∈ ×( +1) is the final time.

Constrains
The initial and terminal states are described as: In order to accomplish the lane change motion planning maneuver successfully, the constraints set on the longitudinal and lateral lines as well as the lateral acceleration are 0 ≤ ≤ , Where is the longitudinal distance between vehicle and obstacle; 0 and 1 are the minimum lateral displacements required for completing lane change motion planning process and the left boundary of adjacent lane respectively [27].
When the braking maneuver is applied to decelerate the vehicle, the constraints on , can be rewritten in the following manner: ISSN PRINT 1392-8716, ISSN ONLINE 2538-8460

Problem description
For the sake of convenience, the problem of Lane Changing with Minimum Time is transformed into a Mayer problem.
The Bolza cost function is: In the discrete process using the HP method, the solution interval ∈ [−1,1] will be divided into grids, i.e.
. It is set that ( ) ( ) and ( ) ( ) represent the state and control variables of respectively. Then the above optimal control Bolza problem can be transformed as: Because the state is continuous at the end of the grid, ( −1 + ) = ( − ) should be met at the connection point.
The pseudo-spectral method can be divided into four categories according to different orthogonal polynomials. But the discrete principle is basically the same among all polynominals. The paper takes the Radau pseudo-spectral method as an example to illustrate discrete principle of the pseudo-spectral method.
Eq. (15) can be obtained by operating difference to : Approximation of the dynamic equation on the grid is: where: where ( ) is the × ( + 1)-dimensional LGR differential matrix on grid . Then Eq. (12) can be discretized as: where = 1,2, ⋯ , ; = 1,2, ⋯ , . According to the above transformation method, the optimal control problem is transformed into a constrained nonlinear programming problem (NLP), which can be solved by the Sequential Quadratic Programming (SQP) method.

Adaptive optimization of mesh refinement and collocation
When the smoothness of the state and control variables is low, the solution using the global interpolation polynomial will affect its accuracy greatly. In order to improve the efficiency and accuracy of the solution, it is necessary to solve the discrete error to determine whether it is necessary or not to mesh the interpolated nodes.

Discrete error solution
It is assumed that the nonlinear programming problem shown in Eq. (10)  (̂1 ( ) ) is the approximation of control variables in ( ) .
Additionally, the control in is approximated using Lagrange interpolating polynomials as: where So, the relative and absolute error can be defined according to (̂( ) ) and ̂(̂( ) ). The relative error ( ) (̂( ) ) and absolute error ( ) (̂( ) ) of the state variables in interval can be expressed as: where = 1,2, ⋯ , + 1, = 1,2, ⋯ , . And is number of the state variable. Then the maximum relative error can be defined as [29]:

Error range determination
Relationship between the estimated value (̂,̂) of LGR discrete interval and its analytical solution ( * , * ) is: where 1 is a constant; is number of the collocation point; ℎ is the interval width; is the minimum value of the order of consecutive derivatives solved and ; ‖•‖ ∞ is interval norm.

Mesh refinement method
The solution result of Section 4.1 is used to determine whether the maximum relative error exceeds the tolerance error or not. If there is a grid relative error exceeding the tolerance error , then the grid will be divided into smaller grids or the number of collocation point can be increased within the grid. When the state and control variables in the mesh are less smooth, the solution accuracy can be improved by refining the mesh. At the same time, when the state and control variables are smooth enough, the number of collocation points in the mesh is increased. Then the solution accuracy is improved by increasing the order of the interpolation polynomial.

Unsmooth mesh determination
It is assumed that the optimal solution on the th interval, i.e. , has been solved using the Radau pseudo-spectral method. And it is set that the solution of the state variable in th iteration is where ( ) is the curvature of the th state variable in . Since each state variable has a different curvature change in , the maximum value is taken as a judgment to determine whether the trajectory is smooth or not: If the curvature is satisfied with: where ̅ is the set curvature value, indicating that the trajectory is relatively smooth in the interval, and the interval needs to be further divided. Otherwise, the trajectory is smooth in the interval, and the accuracy of the solution can be improved by increasing the number of points. For control variables, the same method can be used to define the changes of the control variable.

Determination of sub-grids number
It is assumed that the curvature in grid in the th iteration meets Eq. (27), and it is known from the above analysis that the mesh needs to be refined. Taking equal sign for Eq. (24), then Eq. (28) can be obtained: In order to obtain the desired minimum relative error in the ( + 1)th iteration, Eq. (29) is obtained: Since the grid has been refined, let ( +1) = ( ) , that is, the interval of in the th interval and the iteration +1 in ( + 1)th iteration have the same number of collocation points. Eq. (30) can be obtained by combining Eq. (28) and Eq. (29): where is the number of sub-grids to be divided at the next step. The relative error in ( − 1)th iteration is: can be solved by combining Eq. (28). And then the number of sub-intervals can be determined according to Eq. (30). Eq. (24) demonstrates that the number of sub-intervals newly divided is at least to ensure that the relative error in the iterative process can meet the requirements. But for a specific problem, may be larger, so this will lead to a rapid increase in the amount of calculations, so it is necessary to set upper limit of . In the study of the paper, the number of the largest subinterval of is obtained based on the ratio of relative and set error: where [•] indicates taking an integer down. Eq. (32) gives the range of the largest subinterval. Its division process is as follows. When ( ) → , Eq. (32) may cause the value of to be too large, resulting in the sub-grid being redundancy. When ( ) → , decreases until it is 0 gradually. So always provides a reasonable number of subintervals. Then will be divided into: Eq. (33) gives the number of divided meshes but does not give the meshing position.

Determination of sub-grid position
In order to determine the location of the sub-grid, density function and cumulative distribution function are defined firstly. Let : [ , ] → + ( , ∈ ) be a non-negative Lebesgue integrable function, and ∫ ( ) = 1, as well as be a density function. Then the integral of the density function on the defined domain is defined as the cumulative distribution function: If collocation points { } =1 = 1( 1 = −1 , = ) are inserted in interval [ −1 , ], and if position of is determined, then the position of +1 is: The density function based on the curvature has a dense distribution at the position where the curvature is large, and the node distribution is sparse in a small curvature. Therefore, the cumulative distribution function can be used to determine the position of the newly added grid point. The density function can be defined by Eq. (25): where 2 is a selected constant and meeting the following constraint: Then the cumulative distribution function can be obtained: The position of the sub-grid is determined according to Eq. (33) and Eq. (34):

Distribution addition method
It is assuming that the solution in the grid does not satisfy the set error and Eq. (27). The default trajectory is smooth on interval . Collocation points should be added to increase the order of the polynomial and to reduce its solution error. It is set that ( ) denote an error of the th iteration on the interval , and the grid width in the ( + 1)th iteration is the same as in the th iteration, i.e. ℎ = ℎ ( +1) . Then ( +1) can be solved as: ]. (41) In order to ensure that the order of the polynomial on grid is not too high, is used to limit the order of the polynomial. If ( +1) > , the number of collocation points on the grid is .

Mesh refinement and collocation process updating
The process of grid refinement and collocation point number optimization is as follows: Step 1. Setting the initial grid and the number of collocation points in each grid.
Step 2. Resolving the nonlinear planning problem shown in Eq. (18) using the SNOPT software package.
Step 3. If the relative error resolved is less than the set relative error , the calculation is terminated, and the result is outputted; otherwise, Step 4 is to be done.
Step 4. Determining whether the curvature of the trajectory meets the set value , if it does not meet the requirement, go to step 4.1; otherwise, go to step 4.2.
Step 4.1. Dividing the grids according to the method in Section 4.3.2 and setting the number of collocation points.
Step 4.2. Adding the number of collocation points according to the method in Section 4.3.3 or dividing the grid.
Step 5. After all the grids and collocation points have been updated, the calculation result of previous step is taken as the initial value, and then go to Step 2.

Numerical simulations
In order to verify the effectiveness of the method proposed in this paper, the simulations are made in MATLAB. They show that the vehicle parameters are according to Ref. [28].

Double Lane Changing Condition
In the test of driving stability of ordinary vehicles as well as in the test of tracking ability of intelligent vehicles, the double lane changing condition is a test section used frequently. However, since the track function of standard double lane changing condition is a piecewise function with poor continuity, in order to restore the true driving track under the double lane changing condition, the hyperbolic function is used to perform nonlinear fitting. And the following results are obtained: where and are the longitudinal and lateral positions respectively. Fig. 2 shows the results of the lateral displacement, steering wheel angle, lateral acceleration, yaw rate and longitudinal speed under the double lane change road condition with the initial speed of 108 km/h. From Fig. 2(a), it can be seen that the vehicle can track the double lane change road basically with the control of the proposed algorithm.
From Fig. 2(b), it can be seen that a large steering wheel angle is required to enter and exit the corners of the double lane change road. Especially, when entering the first corner and exiting the fourth corner of the double lane change road, the steering wheel angle reaches its maximum values.
From Fig. 2(c), it also can be seen that the lateral acceleration is higher at 60 m, 100 m, 430 m and 490 m; that indicates that when entering and exiting the corners of the double lane change road, the vehicle will generate the highest lateral acceleration.
From Fig. 2(d), it also can be seen that the yaw rate is higher at 60 m, 100 m, 430 m and 490 m; that indicates that when entering and exiting the corners of the double lane change road, the vehicle will generate a greater yaw rate.
From Fig. 2(e), it also can be seen that to complete the process of passing the double lane change road with minimum time, the vehicle must establish continuous acceleration.

Overtaking condition
Overtaking is a common operation mode in the process of vehicle driving.  Fig. 3(b), it can be seen that a large steering wheel angle is required to enter the adjacent lane from the current lane. Especially, in order to maintain stability after entering the adjacent lane, a large steering wheel angle is still required.
From Fig. 3(c), it also can be seen that a high lateral acceleration is required when entering the adjacent lane from the current lane.
From Fig. 3(d), it also can be seen that the yaw rate is greater at 15 m, 36 m; that indicates that when entering and exiting the current lane, the vehicle will generate a greater yaw rate.

Accuracy and efficiency verification
A comparison is made in order to verify the superiority of this method over the AMRC and GPM methods. From Fig. 4(a), it can be seen that the trend of the curves of the steering wheel angle obtained by AMRC and GPM methods is basically consistent. However, it can be seen from Fig. 4(b) that the error of steering wheel angle calculated by the AMRC is smaller than that of the GPM method. This is because the method in this paper has fewer collocation and discrete equations and sparse Jacobian matrix, so it improves the speed of solving NLP and reduces the time of optimization. Fig. 4 indicates that under the same condition, it is required a less number of collocations and meshes, so the calculation accuracy is higher. This ensures the superiority of the proposed method. And also, it can be seen from Fig. 4(c) that the mean value of estimation error of steering wheel angle of the GPM method is 0.06 deg, while the one of the AMRC method is 0.028 deg, which is smaller than that of the GPM method. This ensures the higher calculation accuracy of the proposed method.

Overtaking condition
In order to analyze the computational efficiency of the proposed method, the calculation accuracy is compared between the proposed method and the GPM method. The comparison results are shown in Table 3. It can be seen from the table that for the same calculation accuracy, the number of iterations and the calculation time of the hybrid method were lower than that in the direct multiple shooting method. This demonstrated that the proposed hybrid method had better solving efficiency.

Experimental verification
A virtual test adopting the CarSim software is conducted to verify the feasibility of the simulated results.
CarSim vehicle dynamics software can quickly build a vehicle dynamics model and driving scene of the simulated vehicle by configuring parameters, and can also conduct a real-time simulation. The vehicle dynamics model is developed in the CarSim, which is a professional vehicle system simulation software. It is adopted by many automobile manufacturers and parts suppliers in the world, and it is the standard software of the vehicle industry. CarSim adopts a system-oriented parametric modeling method. Users only need to select each vehicle component module from the model database and configure the corresponding parameters as required to complete quickly the construction of the vehicle dynamics model. CarSim offers a variety of vehicle models including sedans, pickups, and SUVs. Carsim is the vehicle state research software that uses the Vehicle Sim technology based on the vehicle dynamics and takes the simulation as the technical method. During the project development and research, the use of Carsim can greatly reduce the time spent on development, testing and planning, and allow proposing better vehicle dynamics decision-making methods. For the research projects, such simulation technology can theoretically prove the expected results, which can provide a theoretical basis for a later research and development.
In this paper, the vehicle dynamics model of the driving simulation system is quickly built in the CarSim by configuring the parameters, and the control platform and motion simulation platform are developed by using its data interface. This improves the complexity of vehicle dynamics modeling in the process of the driving simulation system development, can reduce the difficulty of vehicle dynamics modeling in the driving simulation system, and simplify the development process.
From Fig. 5, it can be seen that for the lateral displacement and the steering wheel angle as well as longitudinal speed, there are discrepancies between the simulated and the virtual test values. This is because that the model in the virtual test ignores the nonlinearity of the steering system and suspension system. However, the trend of the curves in Figs

Conclusions
To solve the problem of minimum time lane changing problem of vehicle handling inverse dynamics, this paper proposes an adaptive mesh refinement and collocation optimization method on the basis of traditional pseudo-spectral method and based on a nonlinear 7-DOF vehicle dynamics model as well as the Magic formula tire model. The simulation results show that the adaptive mesh refinement and collocation optimization method proposed in this paper can solve the problem of minimum time lane changing problem of vehicle handling inverse dynamics with high accuracy by approximating the optimal solution by densifying the mesh and adding collocation nearby. In the process of optimization solution, the method proposed in this paper can adaptively increase the number of collocations and reduce the number of meshes which can generate fewer total collocation in the solution area reducing the number of nonlinear programming equations after discretization, and improving the solving speed of the SQP method.

Data availability
The datasets generated during and/or analyzed during the current study are available from the corresponding author on reasonable request.

Author contributions
Yingjie Liu: mathematical model and the simulation techniques. Dawei Cui: spelling and grammar checking as well as virtual validation. Wen Peng: virtual validation.