Abstract
The traditional algorithm, applied to the motion planning of autonomous vehicles, easily causes high computational costs and excessive turning points generated in the planning path. In addition, the vehicle cannot track the path due to the unsmooth inflection point. To overcome these potential limitations, an improved algorithm-based motion planning algorithm and a tracking control strategy based on model predictive control theory were proposed in this work. The method of expanding the search neighborhood is adopted to improve the planning efficiency of algorithm. The artificial potential field method is also incorporated into the proposed algorithm. The resultant force generated by each potential field is further introduced into the evaluation function of algorithm to plan the driving path, which could be suitable for autonomous vehicles. The sharp nodes in the path are smoothed by cubic quasi-uniform B-spline curve. The tracking control strategy is designed based on model predictive control theory to realize the accurate tracking of the planned path. Typical obstacle avoidance conditions were selected for co-simulation test verification. The experimental results show that the proposed motion planning algorithm and tracking control strategy can effectively plan the obstacle avoidance path and accurately track the path in different environments.
1. Introduction
With the rapid growth of car ownership, traffic safety, and urban traffic congestion problems becoming more and more serious, and research and development of safe and reliable autonomous vehicles have become an inevitable development trend. Motion planning and tracking control are two key technologies of autonomous vehicles, which solve the problem of how autonomous vehicles drive. Motion planning is to plan a drivable safe path for the autonomous vehicle while tracking control is to track the planned path on the premise of satisfying the vehicle kinematics and dynamics constraints.
For the motion planning and tracking control of the autonomous vehicle, a large number of scholars have conducted in-depth research. In terms of motion planning, algorithm [1], artificial potential field method [2], genetic algorithm [3], RRT algorithm [4], etc, are commonly used in the current research. Among them, algorithm is the most widely used in the motion planning of autonomous vehicles because of its simple working principle and strong robustness. However, with the increase of the map, algorithm also presents problems such as high computational cost and the inability to find the optimal path. In order to solve this kind of problems, a large number of scholars have carried out research on it. Fu et al. [5] proposed an improved algorithm. When exploring the neighborhood, local paths from the current node to the target point are selected and planned according to the current environment, and the drivable local paths are optimized as part of the global path, improving the smoothness of the path planned by algorithm. Tang et al. [6], aiming at the problem of large storage space of algorithm, by using three methods: bidirectional search, guideline, and key point list, the amount of calculation of the algorithm is optimized and the planning time of the algorithm is reduced. Erke et al. [7] improved the obstacle avoidance performance of the algorithm by setting key points around the obstacle and designed a variable step size algorithm to reduce the calculation time of the algorithm. Liu et al. [8] selected and recorded the nodes in the Open List and Close List through the method of jump point search, optimized the algorithm planning time, and combined the dynamic window method to select the optimal path. Zhong et al. [9] combined the adaptive window method with the algorithm, which simplified the calculation of the risk cost function and the distance cost function, and realized the motion planning of the mobile robot in a dynamic environment. Xiong et al. [10] smoothed the planned path through the Bezier curve, which improved the applicability of the algorithm.
In terms of tracking control, the commonly used tracking control theories include pure tracking control [11], fuzzy control [12], optimal control [13], sliding mode variable structure control [14], model predictive control [15], etc. Since model predictive control (MPC) has a good ability to deal with multiple constraints and has a good control effect on the path tracking of nonlinear systems, a large number of scholars have applied MPC to the tracking control of autonomous vehicles. Novi et al. [16] proposed a hierarchical control method for nonlinear model predictive control (NMPC), in which high-order MPC calculated the curve of optimal speed and low-order NMPC constrained motion curve, thus improving the real-time performance of the control algorithm. Zhang et al. [17] proposed an adaptive MPC control strategy based on the recursive least square method, which can achieve better control effects under different driving conditions. Xu et al. [18] proposed a tracking control strategy based on the preview-follower theory (PFT) and MPC theory, in which PFT updated the reference quantity according to the preview-point and controlled the vehicle movement through MPC, effectively reducing the tracking error of the vehicle. Berntorp et al. [19] proposed an adaptive nonlinear model predictive control with variable tire model, which has a good control effect on path tracking on different roads.
To sum up, scholars have conducted a lot of research on the motion planning and tracking control of autonomous vehicles, but the current research mainly focuses on the optimization of the performance parameters of the algorithm itself, the actual running environment of the vehicle and whether the planned path can meet the requirements of vehicle tracking are less considered. Therefore, this study analyzed the motion planning and tracking control of autonomous vehicles in actual obstacle avoidance conditions, proposes a motion planning algorithm improved by algorithm, and shows a tracking control strategy based on model predictive control theory. The main contributions of this study are as follows:(i)In this study, the planning time of algorithm is reduced by expanding the search neighborhood method, and the traditional algorithm’s 33 search neighborhood is expanded to 99 search neighborhood, which reduces the included nodes in the Close List and increases the searchable direction of paths, effectively improving the planning efficiency of the algorithm.(ii)In this study, artificial potential field method is combined with algorithm. In the process of motion planning, the minimum safe distance between the planned path and the obstacle is considered. The resultant force generated by each potential field is introduced into the evaluation function of algorithm for obstacle avoidance, and the sharp nodes in the path are smoothed by a cubic quasi-uniform B-spline curve.(iii)The tracking control strategy is designed based on the model predictive control theory. The vehicle center of mass slip angle constraint is added to the model predictive control to improve the vehicle driving stability. Through the path tracking test under different vehicle speeds, the feasibility of the improved algorithm for path planning and the effectiveness of the tracking control performance are separately verified.
The rest of the study is organized as follows: In Section 2, the improved motion planning algorithm is introduced, which combines the improved algorithm by expanding the search neighborhood with the artificial potential field method, and uses a cubic quasi-uniform B-spline curve to smooth the path. In Section 3, the path tracking controller is designed based on model predictive control theory. In Section 4, the planning algorithm and control strategy are verified by co-simulation. Finally, the conclusions and future research directions of this study are discussed in Section 5.
2. Motion Planning
algorithm was first applied to the motion planning of mobile robots. Most mobile robots are capable of omnidirectional movement, so the curvature of the planned path is not strictly required. With the rapid development of autonomous vehicles, algorithm has also been widely used in the motion planning of autonomous vehicles. However, due to the constraints of autonomous vehicles such as minimum turning radius and maximum front-wheel Angle, the path planned by the traditional algorithm is difficult to meet the tracking requirements of autonomous vehicles. Therefore, relevant improvements should be made to the algorithm to plan the path suitable for autonomous vehicles. This paper studies the obstacle avoidance conditions as shown in Figure 1.
2.1. Environmental Map Modeling
In the motion planning of autonomous vehicles, it is necessary to establish an appropriate map model to represent the current environmental information, this study selects the commonly used grid map to construct the environment map of obstacle avoidance conditions. In the grid map, a series of squares of the same size are used to represent the current environment, and the binary method is used to assign values to each square to divide the map into occupied and unoccupied areas. 0 means that the current grid is not occupied, that is, the driving area of the vehicle, which is shown in white in the grid map, 1 indicates that the current grid is occupied and vehicles cannot pass through this area, as shown in black on the map. The grid map established in this study according to obstacle avoidance conditions is shown in Figure 2.
2.2. Traditional Algorithm
algorithm is one of the graph search algorithms. First, add the starting point to the Open List, search the grid of the surrounding 33 neighborhood from the starting point, and traverse the adjacent nodes. Then the direction to be included is selected by calculating the evaluation function value of each adjacent node, and the node with the smallest evaluation function value is selected as the optimal node for inclusion, and Add the current node to the Close List [20]. Continue to explore the 33 neighborhood around the optimal node, and repeat the above process to traverse the map until the target point is included in the Close List. Finally, find the parent node of each included node from the target point to the starting point to form a path. Its evaluation function iswhere is the the evaluation function; is the real cost, and is the estimated cost. Since autonomous vehicles cannot achieve omnidirectional movement, Manhattan distance is used to represent the estimated cost in order to reduce the planning error, then the estimated cost iswhere is the coordinate of the current node and is the coordinate of the target point.
It can be seen from the above formula that the traditional algorithm is simple in principle, easy to implement, and can effectively plan the path existing on the map. However, with the expansion of the environmental map, there are also some problems such as long planning time, inability to plan the optimal path, and many sharp nodes in the planning path, so it is necessary to improve the traditional algorithm to plan the optimal path.
2.3. Improved Algorithm
2.3.1. Expand the Search Neighborhood
The number of nodes included in the Close List of algorithm is proportional to the planning time of the algorithm. The more nodes included, the longer the planning time. Therefore, the algorithm planning time can be reduced by reducing the nodes included in the Close List. The traditional algorithm searches the grid of the surrounding 33 neighborhood every time, as shown in Figure 3. Then, one of the surrounding eight adjacent nodes is selected as the optimal node for inclusion, that is, one of every eight adjacent nodes will be included in the Close List. In this way, only eight adjacent nodes can be explored at a time, and the planned path direction is limited to an integer multiple of . The planned path has many turning points and is not smooth enough. To solve this problem, this study adopts the method of expanding search neighborhood to improve the traditional algorithm. The 33 search neighborhood of the traditional algorithm is extended to 55 search neighborhood to reduce the nodes included in the Close List and increase the searchable direction of the planned path to improve the smoothness of the path, 55 search neighborhood is shown in Figure 4.
As can be seen from Figure 4, the number of adjacent nodes searched for 55 search neighborhood increases from 8 to 24, that is, one of every 24 nodes is included in the Close List. Compared with the traditional algorithm, it can effectively reduce the nodes included in the Close List. At the same time, the searchable directions of each traversal increase from 8 to 16, and the turning angle of the path within the search step becomes smaller. In order to verify the effectiveness of 55 extended neighborhood algorithm, MATLAB software was used to verify the algorithm on 3030 grid map, and the planned path is shown in the Figures 5 and 6. The result of 33 search neighborhood planning is shown in Figure 5, and the result of 55 search neighborhood planning is shown in Figure 6.
According to the planning results, the planned path length of 33 search neighborhood is 31.31 m, and the planning time is 3.32 s, while the planned path length of 55 search neighborhood is 28.36 m, and the planning time is 1.68 s. It can be concluded that the planning effect of 55 search neighborhood is better, and the planning path of 55 search neighborhood is smoother than that of 33 search neighborhood. Therefore, it can be concluded that the method of expanding the search neighborhood can effectively reduce the search time of the algorithm and improve the smoothness of the path. Due to the large driving environment map of autonomous vehicles and the uniform distribution of obstacles, a larger search neighborhood can be expanded to plan the path to reduce the planning time. Similarly, 77 search neighborhood and 99 search neighborhood were used to conduct experiments respectively. The planned path is shown in Figure 7, and the experimental results are summarized in Table 1.
According to the simulation results, it can be seen that the length and time of the path planned by the 99 search neighborhood algorithm are optimal, and the planned path has fewer turning points and is smoother. Therefore, this study adopts the method of expanding the 99 search neighborhood as an improved algorithm. The improved algorithm is applied to the obstacle avoidance condition in this study, and the path planning is shown in Figure 8.
2.3.2. Combined with Artificial Potential Field Method
As can be seen from Figure 8, the improved algorithm of 99 search neighborhood only considers the length of the path, resulting in a small distance between the planned path and the obstacle, which cannot meet the tracking requirements of vehicles under normal driving conditions. The normal planned path should consider the minimum safe distance between the obstacle and the vehicle. Therefore, the artificial potential field method is combined with algorithm to construct a repulsive potential field around the obstacle, so that the planned path is far away from the obstacle. At the same time, to prevent the distance between the planned path and the road boundary from being small or beyond the road boundary, repulsive force potential field of the road boundary is created on both sides of the road to ensure that vehicles will not exceed the road boundary. Due to the long road, the gravitational potential field is constructed around the target point in order to make the planned path approach the target point more quickly.
In the traditional artificial potential field method, there is a repulsive force field around the obstacle and a gravitational field around the target point, and autonomous vehicles plan the driving path under the combined action of the repulsive force field and gravitational field [21]. The traditional gravitational potential field changes with the position of the vehicle. In this study, the gravitational potential field of the target point is established based on the traditional gravitational potential field function, and the target point gravitational potential field function iswhere is the action coefficient of gravitational field; is the euclidean distance between the target point and the vehicle.
The gravitational function can be obtained from the gravitational potential field function, which is the negative derivative of the gravitational potential field with respect to , that is the negative gradient of the gravitational potential field:
The repulsion potential field mainly repulses the vehicle. When the autonomous driving vehicle enters the repulsive potential field, the obstacle will generate repulsive force on the vehicle. When the vehicle is outside the range of the repulsive potential field, the repulsive potential field will not work. The repulsive field function is expressed aswhere is the action coefficient of repulsive field, is the distance between the obstacle and the vehicle, and is the influence distance of the potential field.
In the traditional artificial potential field method, the repulsion and attraction may appear equal in magnitude and opposite in direction, resulting in the autonomous vehicle into local optimization, unable to continue to plan the path. Also, when the vehicle is about to reach the target point, the resultant force of the potential field at the target point is not zero, resulting in the unreachable target point and other problems. In this study, the repulsive field function is improved, and the influence factor of distance between vehicle and target point is added into the repulsive field function. At the same time, the influence range of the traditional repulsive field is improved, and the traditional repulsive force field is circular, but for the autonomous vehicle, the length of the lane is much larger than the width, and the longitudinal speed of the vehicle is much larger than the lateral speed of the vehicle. In order to plan a safer path, this study changes the influence range of the repulsive force field to ellipse, the influence range of the improved repulsion field is shown in Figure 9.
In order to plan the path safely, the major axis A and the minor axis B of the ellipse should be appropriately valued. Where the value of A has an important relationship with the speed, meanwhile, there is a minimum safe distance between the vehicle and the obstacle, so the value of the long axis of the elliptic scope A iswhere is the minimum obstacle avoidance distance between vehicle and obstacles, is the speed correction coefficient, and is the speed of the vehicle.
According to the obstacle avoidance safety rules, the value of the short axis B of the ellipse scope is set aswhere is the safety factor and is the lane width.
Therefore, the improved repulsive field function iswhere is the distance regulating factor; is the elliptic region of action.
To prevent the vehicle from falling into the local optimum, the repulsive force generated by the repulsive potential field is decomposed, and the repulsive force of the obstacle on the vehicle is decomposed into two directions. The schematic diagram of the decomposition repulsion force is shown in Figure 10. The repulsive force function after decomposition iswhere the direction of is that the obstacle points to the vehicle; the direction of is that the vehicle points to the target point.
To prevent the planned path from exceeding the road boundary due to the large repulsive force of obstacles on vehicles, the repulsive force potential field is created at the road boundary. The repulsive field function is as follows:where is the boundary repulsive field action coefficient; is the distance between vehicles and the road boundary; is the influence distance.
The repulsive force corresponding to the road boundary is
Then the resultant force of the vehicle under the joint action of the target gravitational potential field, the obstacle repulsive potential field and the road boundary repulsive potential field is
Combining the artificial potential field method with the improved algorithm, the potential field force of each artificial potential field is introduced into the evaluation function of the algorithm. The improved evaluation function is
The improved algorithm integrated with the artificial potential field method is applied to the obstacle avoidance condition in this study, and the path planning is shown in Figure 11.
2.3.3. Smoothing with B-Spline Curve
As can be seen from Figure 11, the improved algorithm combined with the artificial potential field method still has problems such as sharp nodes and curvature discontinuity in motion planning. Therefore, a cubic quasi-uniform B-spline curve is used to smooth the sharp nodes in the path to making the planned path smoother.
Suppose there are n + 1 control points in total, then B-spline curve of order K can be defined aswhere is the control points and is the basis function of the B-spline curve; The B-spline curve basis function is expressed by the recursive formula, and the expression is as follows:
According to the basis function of B-spline curve, we know that B-spline curve of higher order can be obtained by two B-spline curves of lower order. The cubic quasi-uniform B-spline curve retains the properties of the Bessel curve at two endpoints. The tangent directions of the start and end points are tangent to the first and last sides of the characteristic polygon, respectively, and pass through the starting and ending points, which is more suitable for the motion planning of autonomous vehicles. In this study, cubic quasi-uniform B-spline curve were selected to smooth the planned path, and the repeatability of nodes at both ends was set as 3. After derivation, the expression of the cubic quasi-uniform B-spline curve is obtained as:where is the position parameter, ; are the control points.
The planned path after cubic quasi-uniform B-spline curve processing is shown in Figure 12.
As shown in Figure 12, the path planned by the improved algorithm has no sharp nodes and the path is smooth, which effectively improves the shortcomings of the traditional algorithm. The process of improving algorithm is summarized in Figure 13.
3. Tracking Control
3.1. Vehicle Dynamics Model
In order to reduce the amount of calculation in the process of solving the vehicle dynamics model, a three-degree of freedom vehicle dynamics model considering longitudinal, lateral, and yaw motion is established, as shown in Figure 14. The following simplifications are made: the road surface is assumed to be smooth and the vertical motion of the vehicle is ignored; the vehicle suspension system and aerodynamics are ignored; the lateral load transfer of the tires is ignored.where is the vehicle coordinate system, is the geodetic coordinate system, a, b is the distance from the center of mass to the front and rear axes, and are the speed of the vehicle in the x and y axis direction, is the vehicle yaw rate, is the front wheel angle, Flf and Flr is the longitudinal force of the front tire and rear tire, Fcf and Fcr are the lateral force of the front tire and rear tire, and and are the front and rear tire slip angle.
In this study, the simplified “magic formula” tire model is selected to analyze the tire force, and the small angle assumption is adopted. The lateral force and longitudinal force of the front and rear tires are expressed aswhere Clf and Clr are the longitudinal stiffness of front and rear tires, Ccf and Ccr is the lateral stiffness of the front and rear tires, and Sf and Sr are the slip rate of the front and rear tires.
According to Newton’s second law, the transformation between vehicle coordinate system and geodetic coordinate system, and the tire force analysis, the vehicle dynamics equation is as follows:where is the mass of the vehicle, is the moment of inertia of a vehicle, and and are the speed of the vehicle in the X and Y axis directions of the inertial coordinate system.
3.2. Model Predictive Control
The tracking control strategy is designed based on MPC theory. The model predictive control can predict the output of the system in the future according to the prediction model, the current state quantity of the system and the future control quantity, and can solve problems with various constraints in a rolling manner [22]. The tracking control strategy designed in this study is shown in Figure 15. The planned path and vehicle state are input into the model predictive controller, and the front wheel angle of the vehicle at the next time can be obtained through rolling optimization. The model predictive control flow is as follows:
Convert the three-degree-of-freedom model of the vehicle to a state-space representation: where is the state quantity, ; is the control quantity, ; is the output, .
The vehicle 3-DOF dynamics model is a nonlinear model, which is linearized. (19) is expanded at point using Taylor’s formula, retaining the first-order term yields a linear time-varying equation:where is the Jacobian matrix for ; is the Jacobian matrix for .
Using the first-order quotient difference method to process (19), the discrete state space equation can be obtained:where ; ; is the sampling period; is the sampling time; is the prediction time domain; ; is the unit matrix.
In order to ensure that the vehicle can track the path stably, use the increment of the front wheel angle as the output of the MPC controller, and a new state space expression is obtained:where is the state matrix of state quantity at k moment and control quantity at k-1 moment, ; , ; is the control increment, ; is the output of the system at time k.
Assume that the prediction time domain of the MPC controller is Np, and the control time domain is Nc. The predicted output of the system at time k is obtained aswhere is the output matrix in the prediction time domain, ; is the control increment in the control time domain Nc, ; and are coefficient matrices, , .
The objective function is to ensure that the vehicle tracks the planned path with minimum error. The objective function of the controller iswhere is the actual output, is the reference output, Q,R is the weight coefficient matrix, is the relaxation factor weight coefficient, and is the relaxation factor.
In order to track the planned path more accurately, set the center of mass slip angle constraint as . Set the tire sideslip Angle constraint as . Set the front wheel Angle range as , The angle change is constrained as . At the same time, the attachment condition is constrained to , in order to prevent the constraint condition from being too small to cause no solution, a relaxation factor can be introduced to define the attachment condition constraint as a soft constraint: .
The quadratic programming method is used to solve the objective function, and the objective function is converted into the standard type:where H, G is the coefficient matrix, , ; is the output deviation matrix in the prediction time domain.
The optimal control increment of the system is obtained by solving the following constrained problems:
To solve (26), the optimal control increment of front wheel rotation angle can be obtained as follows:where is the system control input at time .
The first element of the control sequence is input as the actual control increment to obtain the final control quantity:
4. The Simulation Verification
CarSim and MATLAB/Simulink are used to conduct co-simulation experiments to verify the effectiveness of the planning algorithm and tracking control strategy proposed in this study. The main parameters of the vehicle are shown in Table 2.
In order to verify the feasibility of the planned path and the effectiveness of the tracking control strategy, part of the planned path is selected for the path tracking test. Select the path from the starting point to bypass the first obstacle for the tracking test. The adhesion coefficient of dry and good asphalt pavement can reach 0.7–0.8, in this study, the road surface adhesion coefficient is set as 0.75. The tracking control test of the planned path is carried out at high, medium, and low speed respectively, and the simulation results are shown in Figure 16.
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
The simulation results show that the planned path can meet the tracking requirements of autonomous vehicles and achieve good tracking effects at different speeds. As can be seen from Figures 16(a) and 16(b), as the vehicle speed increases, the tracking error of the lateral position also increases. When the vehicle speed is 90 Km/h, the maximum lateral position error is 0.31 m. When the vehicle speed is 60 Km/h, the maximum lateral position error is 0.19 m. When the speed is 30 Km/h, the lateral position error of the vehicle can be controlled within 0.11 m, and the errors are all within the allowable range. As can be seen from Figures 16(c) and 16(d), the yaw angle tracking error at different vehicle speeds can be controlled within 0.02 rad and the difference is not large, indicating that the tracking process of the vehicle is relatively stable. When the vehicle speed is 30 Km/h, the yaw angle tracking effect is the best, and the yaw angle error is within 0.01 rad. As can be seen from Figures 16(e) and 16(f), the higher the vehicle speed, the greater the change of the front wheel angle, but there is no step change in the front wheel angle and lateral acceleration at different vehicle speeds, indicating that the tracking process is relatively stable. It can be seen from Figure 16(g) that during the turning process of the vehicle, the yaw rate changes greatly, when the vehicle speed is 90 Km/h, the maximum yaw rate reaches 11°/s, within the permissible limits. It can be seen from Figure 16(h) that after adding the vehicle center of mass slip angle constraint, the change of the center of mass slip angle can be controlled within 1°, and the change is smooth, indicating that the driving stability of the vehicle tracking process is better.
5. Conclusions
To solve the problem of motion planning and tracking control of autonomous vehicles in obstacle avoidance conditions, this study proposes a motion planning algorithm by improved algorithm and a tracking control strategy based on model predictive control theory. Aiming at dealing with the shortcomings of the traditional algorithm applied to the motion planning of autonomous vehicles, the traditional algorithm is improved. By expanding the search neighborhood method, the planning efficiency of the algorithm is improved, and the improved artificial potential field method is combined with the algorithm for obstacle avoidance and improving the driving safety of the vehicle. At the same time, the cubic quasi-uniform B-spline curve is used to smooth the planned path. The tracking control strategy is designed based on the model predictive control theory, and related constraints are considered to realize the accurate tracking of the planned path. By analyzing the simulation results, the following conclusions can be drawn as follows:(1)The improved algorithm can effectively plan the obstacle avoidance path under corresponding conditions. Compared with the traditional algorithm, the algorithm reduces the planning time and improves the planning efficiency. The planning path considers the minimum safe distance between obstacles and vehicles. The planned paths are smoother and more suitable for autonomous vehicles to track.(2)The designed tracking control strategy reflect a good control effect on the path tracking at different vehicle speeds. The average lateral position tracking error can be controlled within 0.2 m, the yaw Angle tracking error is controlled within 0.02 rad, and the change of center of mass slip angle is controlled within 1°, which verifies the feasibility of planning path and effectiveness of tracking control strategy.
The simulation results demonstrate great reference value for the subsequent real vehicle test. In the future work, the proposed motion planning algorithm and tracking control strategy will be deployed into the real vehicle testing, and the longitudinal speed of the vehicle will be adaptively adjusted according to the paths, and the tracking error of the vehicle would be reduced through the changeable longitudinal speed.
Data Availability
The data used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This work was supported by the National Science Foundation of China (51675257, 51305190) ,General program of Natural Science Foundation of Liaoning Province in 2022 (2022-MS-376) and ”Liaoning BaiQianWan Talents Program.