# Motion Planning and Tracking Control of Autonomous Vehicle Based on ImprovedA∗ Algorithm

Authors: Yunlong Bai; Gang Li; Ning Li
Journal: Journal of Advanced Transportation
 (2022)

---

## Abstract

The traditionalA∗ 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 A∗ 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 A∗ algorithm. The artificial potential field method is also incorporated into the proposed A∗ algorithm. The resultant force generated by each potential field is further introduced into the evaluation function of A∗ 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.


---

## Body

## 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,A∗ algorithm [1], artificial potential field method [2], genetic algorithm [3], RRT algorithm [4], etc, are commonly used in the current research. Among them, A∗ 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, A∗ 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 A∗ 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 A∗ algorithm. Tang et al. [6], aiming at the problem of large storage space of A∗ 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 A∗ algorithm by setting key points around the obstacle and designed a variable step size A∗ 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 A∗ 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 A∗ 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 byA∗ 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 ofA∗ algorithm is reduced by expanding the search neighborhood method, and the traditional algorithm’s 3∗3 search neighborhood is expanded to 9∗9 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 withA∗ 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 A∗ 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 improvedA∗ 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 Section2, the improved motion planning algorithm is introduced, which combines the improved A∗ 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

A∗ 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, A∗ 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 A∗ algorithm is difficult to meet the tracking requirements of autonomous vehicles. Therefore, relevant improvements should be made to the A∗ algorithm to plan the path suitable for autonomous vehicles. This paper studies the obstacle avoidance conditions as shown in Figure 1.Figure 1
Schematic diagram of obstacle avoidance conditions.


### 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 Figure2.Figure 2
Grid map.


### 2.2. TraditionalA∗ Algorithm

A∗ algorithm is one of the graph search algorithms. First, add the starting point to the Open List, search the grid of the surrounding 3∗3 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 3∗3 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 is(1)fn=gn+hn,where fn is the the evaluation function; gn is the real cost, and hn 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 is(2)hn=xn−xg+yn−yg,where xn,yn is the coordinate of the current node and xg,yg is the coordinate of the target point.It can be seen from the above formula that the traditionalA∗ 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. ImprovedA∗ Algorithm

#### 2.3.1. Expand the Search Neighborhood

The number of nodes included in the Close List ofA∗ 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 A∗ algorithm searches the grid of the surrounding 3∗3 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 π/4. 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 A∗ algorithm. The 3∗3 search neighborhood of the traditional algorithm is extended to 5∗5 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, 5∗5 search neighborhood is shown in Figure 4.Figure 3
3∗3 Search neighborhood.Figure 4
5∗5 Search neighborhood.As can be seen from Figure4, the number of adjacent nodes searched for 5∗5 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 5∗5 extended neighborhood A∗ algorithm, MATLAB software was used to verify the algorithm on 30∗30 grid map, and the planned path is shown in the Figures 5 and 6. The result of 3∗3 search neighborhood planning is shown in Figure 5, and the result of 5∗5 search neighborhood planning is shown in Figure 6.Figure 5
3∗3 Search neighborhood planning result.Figure 6
5∗5 Search neighborhood planning result.According to the planning results, the planned path length of 3∗3 search neighborhood is 31.31 m, and the planning time is 3.32 s, while the planned path length of 5∗5 search neighborhood is 28.36 m, and the planning time is 1.68 s. It can be concluded that the planning effect of 5∗5 search neighborhood is better, and the planning path of 5∗5 search neighborhood is smoother than that of 3∗3 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, 7∗7 search neighborhood and 9∗9 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.Figure 7
Comparison of paths planned in different search neighborhoods.Table 1
Different search neighborhood planning results.
Search neighborhoodAdjacent nodeSearch directionPath length/mPlanning time/s3∗38831.313.325∗5241628.361.687∗7483227.351.519∗9805627.211.49According to the simulation results, it can be seen that the length and time of the path planned by the 9∗9 search neighborhood A∗ algorithm are optimal, and the planned path has fewer turning points and is smoother. Therefore, this study adopts the method of expanding the 9∗9 search neighborhood as an improved A∗ algorithm. The improved A∗ algorithm is applied to the obstacle avoidance condition in this study, and the path planning is shown in Figure 8.Figure 8
ImprovedA∗ algorithm planning path.


#### 2.3.2. Combined with Artificial Potential Field Method

As can be seen from Figure8, the improved A∗ algorithm of 9∗9 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 A∗ 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 is(3)Uatt=12kattρ2q,qg,where katt is the action coefficient of gravitational field; ρq,qg 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ρq,qg, that is the negative gradient of the gravitational potential field:(4)Fatt=−∇Uatt=−12katt∗∇ρ2q,qg=−kattρq,qg.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 as(5)Urep=12krep1ρq,qo−1ρl2,0≤ρq,qo≤ρl,0,ρq,qo≥ρl,where krep is the action coefficient of repulsive field, ρq,qo is the distance between the obstacle and the vehicle, and ρl 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 Figure9.Figure 9
Improved potential field range.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 is(6)A=Dmin+ηv,where Dmin is the minimum obstacle avoidance distance between vehicle and obstacles, η is the speed correction coefficient, and v 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 as(7)B=ξL2,where ξ is the safety factor and L is the lane width.Therefore, the improved repulsive field function is(8)Urep=12krep1ρq,qo−1ρl2ρmq,qg,q∈X2A2+Y2B2=1,0,q∉X2A2+Y2B2=1,where m is the distance regulating factor; X2/A2+Y2/B2=1 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 Figure10. The repulsive force function after decomposition is(9)Frep=−∇Urep=Frep1+Frep2,q∈X2A2+Y2B2=1,0,q∉X2A2+Y2B2=1,Frep1=krep1ρq,qo−1ρl1ρ2q,qoρmq,qg,Frep2=m2krep1ρq,qo−1ρl2ρm−1q,qg,where the direction of Frep1 is that the obstacle points to the vehicle; the direction of Frep2 is that the vehicle points to the target point.Figure 10
Decomposition repulsion diagram.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:(10)Ur=12kr1ρq,qr−1ρr2,ρq,qr≤ρr,0,ρq,qr≥ρr,where kr is the boundary repulsive field action coefficient; ρq,qr is the distance between vehicles and the road boundary; ρr is the influence distance.The repulsive force corresponding to the road boundary is(11)Fr=kr1ρq,qr−1ρr1ρ2q,qr,ρq,qr≤ρr,0,ρq,qr≥ρr.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(12)F=Fatt+Frep+Fr,Combining the artificial potential field method with the improvedA∗ algorithm, the potential field force of each artificial potential field is introduced into the evaluation function of the A∗ algorithm. The improved evaluation function is(13)fn=gn+hn+F.The improvedA∗ 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.Figure 11
Planning path integrating artificial potential field method.


#### 2.3.3. Smoothing with B-Spline Curve

As can be seen from Figure11, the improved A∗ 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 aren + 1 control points in total, then B-spline curve of order K can be defined as(14)Pu=p0,p1,…,pnB0,kuB1,ku⋅⋅⋅Bn,ku=∑i=0nPiBi,ku,where P is the control points and Bi,ku is the basis function of the B-spline curve; The B-spline curve basis function is expressed by the deBoor−Cox recursive formula, and the expression is as follows:(15)Bi,ku=1,ui≤u<ui+10,Otherk=1,u−uiui+k−1−uiBi,k−1u+ui+k−uui+k−ui+1Bi+1,k−1u,k≥2,Define00=0.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:(16)P0,3t=16t1t2t31410−30303−630−13−31P0P1P2P3,where t is the position parameter, 0≤t≤1; P0−P3 are the control points.The planned path after cubic quasi-uniform B-spline curve processing is shown in Figure12.Figure 12
Path after smoothing.As shown in Figure12, the path planned by the improved A∗ algorithm has no sharp nodes and the path is smooth, which effectively improves the shortcomings of the traditional algorithm. The process of improving A∗ algorithm is summarized in Figure 13.Figure 13
ImproveA∗ algorithm flow.


## 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 Figure2.Figure 2
Grid map.


## 2.2. TraditionalA∗ Algorithm

A∗ algorithm is one of the graph search algorithms. First, add the starting point to the Open List, search the grid of the surrounding 3∗3 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 3∗3 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 is(1)fn=gn+hn,where fn is the the evaluation function; gn is the real cost, and hn 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 is(2)hn=xn−xg+yn−yg,where xn,yn is the coordinate of the current node and xg,yg is the coordinate of the target point.It can be seen from the above formula that the traditionalA∗ 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. ImprovedA∗ Algorithm

### 2.3.1. Expand the Search Neighborhood

The number of nodes included in the Close List ofA∗ 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 A∗ algorithm searches the grid of the surrounding 3∗3 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 π/4. 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 A∗ algorithm. The 3∗3 search neighborhood of the traditional algorithm is extended to 5∗5 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, 5∗5 search neighborhood is shown in Figure 4.Figure 3
3∗3 Search neighborhood.Figure 4
5∗5 Search neighborhood.As can be seen from Figure4, the number of adjacent nodes searched for 5∗5 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 5∗5 extended neighborhood A∗ algorithm, MATLAB software was used to verify the algorithm on 30∗30 grid map, and the planned path is shown in the Figures 5 and 6. The result of 3∗3 search neighborhood planning is shown in Figure 5, and the result of 5∗5 search neighborhood planning is shown in Figure 6.Figure 5
3∗3 Search neighborhood planning result.Figure 6
5∗5 Search neighborhood planning result.According to the planning results, the planned path length of 3∗3 search neighborhood is 31.31 m, and the planning time is 3.32 s, while the planned path length of 5∗5 search neighborhood is 28.36 m, and the planning time is 1.68 s. It can be concluded that the planning effect of 5∗5 search neighborhood is better, and the planning path of 5∗5 search neighborhood is smoother than that of 3∗3 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, 7∗7 search neighborhood and 9∗9 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.Figure 7
Comparison of paths planned in different search neighborhoods.Table 1
Different search neighborhood planning results.
Search neighborhoodAdjacent nodeSearch directionPath length/mPlanning time/s3∗38831.313.325∗5241628.361.687∗7483227.351.519∗9805627.211.49According to the simulation results, it can be seen that the length and time of the path planned by the 9∗9 search neighborhood A∗ algorithm are optimal, and the planned path has fewer turning points and is smoother. Therefore, this study adopts the method of expanding the 9∗9 search neighborhood as an improved A∗ algorithm. The improved A∗ algorithm is applied to the obstacle avoidance condition in this study, and the path planning is shown in Figure 8.Figure 8
ImprovedA∗ algorithm planning path.


### 2.3.2. Combined with Artificial Potential Field Method

As can be seen from Figure8, the improved A∗ algorithm of 9∗9 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 A∗ 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 is(3)Uatt=12kattρ2q,qg,where katt is the action coefficient of gravitational field; ρq,qg 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ρq,qg, that is the negative gradient of the gravitational potential field:(4)Fatt=−∇Uatt=−12katt∗∇ρ2q,qg=−kattρq,qg.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 as(5)Urep=12krep1ρq,qo−1ρl2,0≤ρq,qo≤ρl,0,ρq,qo≥ρl,where krep is the action coefficient of repulsive field, ρq,qo is the distance between the obstacle and the vehicle, and ρl 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 Figure9.Figure 9
Improved potential field range.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 is(6)A=Dmin+ηv,where Dmin is the minimum obstacle avoidance distance between vehicle and obstacles, η is the speed correction coefficient, and v 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 as(7)B=ξL2,where ξ is the safety factor and L is the lane width.Therefore, the improved repulsive field function is(8)Urep=12krep1ρq,qo−1ρl2ρmq,qg,q∈X2A2+Y2B2=1,0,q∉X2A2+Y2B2=1,where m is the distance regulating factor; X2/A2+Y2/B2=1 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 Figure10. The repulsive force function after decomposition is(9)Frep=−∇Urep=Frep1+Frep2,q∈X2A2+Y2B2=1,0,q∉X2A2+Y2B2=1,Frep1=krep1ρq,qo−1ρl1ρ2q,qoρmq,qg,Frep2=m2krep1ρq,qo−1ρl2ρm−1q,qg,where the direction of Frep1 is that the obstacle points to the vehicle; the direction of Frep2 is that the vehicle points to the target point.Figure 10
Decomposition repulsion diagram.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:(10)Ur=12kr1ρq,qr−1ρr2,ρq,qr≤ρr,0,ρq,qr≥ρr,where kr is the boundary repulsive field action coefficient; ρq,qr is the distance between vehicles and the road boundary; ρr is the influence distance.The repulsive force corresponding to the road boundary is(11)Fr=kr1ρq,qr−1ρr1ρ2q,qr,ρq,qr≤ρr,0,ρq,qr≥ρr.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(12)F=Fatt+Frep+Fr,Combining the artificial potential field method with the improvedA∗ algorithm, the potential field force of each artificial potential field is introduced into the evaluation function of the A∗ algorithm. The improved evaluation function is(13)fn=gn+hn+F.The improvedA∗ 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.Figure 11
Planning path integrating artificial potential field method.


### 2.3.3. Smoothing with B-Spline Curve

As can be seen from Figure11, the improved A∗ 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 aren + 1 control points in total, then B-spline curve of order K can be defined as(14)Pu=p0,p1,…,pnB0,kuB1,ku⋅⋅⋅Bn,ku=∑i=0nPiBi,ku,where P is the control points and Bi,ku is the basis function of the B-spline curve; The B-spline curve basis function is expressed by the deBoor−Cox recursive formula, and the expression is as follows:(15)Bi,ku=1,ui≤u<ui+10,Otherk=1,u−uiui+k−1−uiBi,k−1u+ui+k−uui+k−ui+1Bi+1,k−1u,k≥2,Define00=0.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:(16)P0,3t=16t1t2t31410−30303−630−13−31P0P1P2P3,where t is the position parameter, 0≤t≤1; P0−P3 are the control points.The planned path after cubic quasi-uniform B-spline curve processing is shown in Figure12.Figure 12
Path after smoothing.As shown in Figure12, the path planned by the improved A∗ algorithm has no sharp nodes and the path is smooth, which effectively improves the shortcomings of the traditional algorithm. The process of improving A∗ algorithm is summarized in Figure 13.Figure 13
ImproveA∗ algorithm flow.


## 2.3.1. Expand the Search Neighborhood

The number of nodes included in the Close List ofA∗ 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 A∗ algorithm searches the grid of the surrounding 3∗3 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 π/4. 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 A∗ algorithm. The 3∗3 search neighborhood of the traditional algorithm is extended to 5∗5 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, 5∗5 search neighborhood is shown in Figure 4.Figure 3
3∗3 Search neighborhood.Figure 4
5∗5 Search neighborhood.As can be seen from Figure4, the number of adjacent nodes searched for 5∗5 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 5∗5 extended neighborhood A∗ algorithm, MATLAB software was used to verify the algorithm on 30∗30 grid map, and the planned path is shown in the Figures 5 and 6. The result of 3∗3 search neighborhood planning is shown in Figure 5, and the result of 5∗5 search neighborhood planning is shown in Figure 6.Figure 5
3∗3 Search neighborhood planning result.Figure 6
5∗5 Search neighborhood planning result.According to the planning results, the planned path length of 3∗3 search neighborhood is 31.31 m, and the planning time is 3.32 s, while the planned path length of 5∗5 search neighborhood is 28.36 m, and the planning time is 1.68 s. It can be concluded that the planning effect of 5∗5 search neighborhood is better, and the planning path of 5∗5 search neighborhood is smoother than that of 3∗3 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, 7∗7 search neighborhood and 9∗9 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.Figure 7
Comparison of paths planned in different search neighborhoods.Table 1
Different search neighborhood planning results.
Search neighborhoodAdjacent nodeSearch directionPath length/mPlanning time/s3∗38831.313.325∗5241628.361.687∗7483227.351.519∗9805627.211.49According to the simulation results, it can be seen that the length and time of the path planned by the 9∗9 search neighborhood A∗ algorithm are optimal, and the planned path has fewer turning points and is smoother. Therefore, this study adopts the method of expanding the 9∗9 search neighborhood as an improved A∗ algorithm. The improved A∗ algorithm is applied to the obstacle avoidance condition in this study, and the path planning is shown in Figure 8.Figure 8
ImprovedA∗ algorithm planning path.


## 2.3.2. Combined with Artificial Potential Field Method

As can be seen from Figure8, the improved A∗ algorithm of 9∗9 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 A∗ 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 is(3)Uatt=12kattρ2q,qg,where katt is the action coefficient of gravitational field; ρq,qg 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ρq,qg, that is the negative gradient of the gravitational potential field:(4)Fatt=−∇Uatt=−12katt∗∇ρ2q,qg=−kattρq,qg.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 as(5)Urep=12krep1ρq,qo−1ρl2,0≤ρq,qo≤ρl,0,ρq,qo≥ρl,where krep is the action coefficient of repulsive field, ρq,qo is the distance between the obstacle and the vehicle, and ρl 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 Figure9.Figure 9
Improved potential field range.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 is(6)A=Dmin+ηv,where Dmin is the minimum obstacle avoidance distance between vehicle and obstacles, η is the speed correction coefficient, and v 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 as(7)B=ξL2,where ξ is the safety factor and L is the lane width.Therefore, the improved repulsive field function is(8)Urep=12krep1ρq,qo−1ρl2ρmq,qg,q∈X2A2+Y2B2=1,0,q∉X2A2+Y2B2=1,where m is the distance regulating factor; X2/A2+Y2/B2=1 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 Figure10. The repulsive force function after decomposition is(9)Frep=−∇Urep=Frep1+Frep2,q∈X2A2+Y2B2=1,0,q∉X2A2+Y2B2=1,Frep1=krep1ρq,qo−1ρl1ρ2q,qoρmq,qg,Frep2=m2krep1ρq,qo−1ρl2ρm−1q,qg,where the direction of Frep1 is that the obstacle points to the vehicle; the direction of Frep2 is that the vehicle points to the target point.Figure 10
Decomposition repulsion diagram.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:(10)Ur=12kr1ρq,qr−1ρr2,ρq,qr≤ρr,0,ρq,qr≥ρr,where kr is the boundary repulsive field action coefficient; ρq,qr is the distance between vehicles and the road boundary; ρr is the influence distance.The repulsive force corresponding to the road boundary is(11)Fr=kr1ρq,qr−1ρr1ρ2q,qr,ρq,qr≤ρr,0,ρq,qr≥ρr.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(12)F=Fatt+Frep+Fr,Combining the artificial potential field method with the improvedA∗ algorithm, the potential field force of each artificial potential field is introduced into the evaluation function of the A∗ algorithm. The improved evaluation function is(13)fn=gn+hn+F.The improvedA∗ 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.Figure 11
Planning path integrating artificial potential field method.


## 2.3.3. Smoothing with B-Spline Curve

As can be seen from Figure11, the improved A∗ 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 aren + 1 control points in total, then B-spline curve of order K can be defined as(14)Pu=p0,p1,…,pnB0,kuB1,ku⋅⋅⋅Bn,ku=∑i=0nPiBi,ku,where P is the control points and Bi,ku is the basis function of the B-spline curve; The B-spline curve basis function is expressed by the deBoor−Cox recursive formula, and the expression is as follows:(15)Bi,ku=1,ui≤u<ui+10,Otherk=1,u−uiui+k−1−uiBi,k−1u+ui+k−uui+k−ui+1Bi+1,k−1u,k≥2,Define00=0.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:(16)P0,3t=16t1t2t31410−30303−630−13−31P0P1P2P3,where t is the position parameter, 0≤t≤1; P0−P3 are the control points.The planned path after cubic quasi-uniform B-spline curve processing is shown in Figure12.Figure 12
Path after smoothing.As shown in Figure12, the path planned by the improved A∗ algorithm has no sharp nodes and the path is smooth, which effectively improves the shortcomings of the traditional algorithm. The process of improving A∗ algorithm is summarized in Figure 13.Figure 13
ImproveA∗ algorithm flow.


## 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 Figure14. 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 xoy is the vehicle coordinate system, XOY is the geodetic coordinate system, a, b is the distance from the center of mass to the front and rear axes, x˙ and y˙ are the speed of the vehicle in the x and y axis direction, φ˙ is the vehicle yaw rate, δf 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 αf and αr are the front and rear tire slip angle.Figure 14
Three degrees of freedom vehicle dynamics model.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 as(17)Flf=ClfSf,Flr=ClrSr,Fcf=Ccfαf=Ccfδf−y˙+aφ˙x˙,Fcr=Ccrαr=Ccrbφ˙−y˙x˙,where 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:(18)x¨=2mClfSf−Ccfδf−y˙+aφ˙x˙δf+ClrSr+y˙φ˙,y¨=2mCcfδf−y˙+aφ˙x˙+Ccrbφ˙−y˙x˙−x˙φ˙,φ¨=2IzaCcfδf−y˙+aφ˙x˙−bCcrbφ˙−y˙x˙,Y˙=x˙sinφ+y˙cosφ,X˙=x˙cosφ−y˙sinφ,where m is the mass of the vehicle, Iz is the moment of inertia of a vehicle, and X˙ and Y˙ 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:Figure 15
Tracking control strategy.Convert the three-degree-of-freedom model of the vehicle to a state-space representation:(19)ξ˙=fξ,u,λ=C·ξ.where ξ is the state quantity, ξ=y˙,x˙,φ,φ˙,Y,XT; u is the control quantity, u=δf; λ is the output, λ=φ,YT.The vehicle 3-DOF dynamics model is a nonlinear model, which is linearized. (19) is expanded at point ξ0,u0 using Taylor’s formula, retaining the first-order term yields a linear time-varying equation:(20)ξ˙=fξ0,u0+Atξ−ξ0+Btu−u0,where At is the fξ,u Jacobian matrix for ξ; Bt is the fξ,u Jacobian matrix for u.Using the first-order quotient difference method to process (19), the discrete state space equation can be obtained:(21)ξk+1=Akξk+Bkuk,where Ak=I+TAt; Bk=TBt; T is the sampling period; t is the sampling time; Np is the prediction time domain; K=t,t+1,……,t+Np; I 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:(22)ξ˜k+1|t=A˜k,tξ˜k|t+B˜k,tΔuk|t,λ˜k|t=C˜k,tξ˜k|t,where ξ˜k is the state matrix of state quantity at k moment and control quantity at k-1 moment, ξ˜k|t=ξk|tuk−1|t; A˜k,t=Ak,tBk,t0I, B˜k,t=Bk,tI; Δuk|t is the control increment, Δuk|t=uk|t−uk−1|t; λ˜k|t is the output of the system at time k.Assume that the prediction time domain of the MPC controller isNp, and the control time domain is Nc. The predicted output of the system at time k is obtained as(23)Yk|t=ψkξ˜k|t+ΘkΔUk|t,where Yk|t is the output matrix in the prediction time domain, Yk=λ˜k+1|tλ˜k+2|t⋮λ˜k+Np|t; ΔUk is the control increment in the control time domain Nc, ΔUk=Δuk|tΔuk+1|t⋮Δuk+Nc−1|t; ψk and Θk are coefficient matrices, ψk=C˜k,tA˜k,tC˜k,tA˜k,t2⋮C˜k,tA˜k,tNp, Θk=C˜k,tB˜k,t0⋯0C˜k,tA˜k,tB˜k,tC˜k,tB˜k,t⋯0⋮⋮⋮⋮C˜k,tA˜k,tNc−1B˜k,tC˜k,tA˜k,tNc−2B˜k,t⋯C˜k,tB˜k,tC˜k,tA˜k,tNcB˜k,tC˜k,tA˜k,tNc−1B˜k,t⋯C˜k,tA˜k,tB˜k,t⋮⋮⋱⋮C˜k,tA˜k,tNp−1B˜k,tC˜k,tA˜k,tNp−2B˜k,t⋯C˜k,tA˜k,tNp−Nc−1B˜k,t.The objective function is to ensure that the vehicle tracks the planned path with minimum error. The objective function of the controller is(24)J=∑i=1Npλk+i|t−λrefk+i|t2Q+∑i=1Nc−1Δuk+i|t2R+ρε2,where λk+i|t is the actual output, λrefk+i|t 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−8∘≤β≤8∘. Set the tire sideslip Angle constraint as −3∘≤α≤3∘. Set the front wheel Angle range as −12∘≤δf≤12∘, The angle change is constrained as −0.8∘≤Δδf≤0.8∘. At the same time, the attachment condition is constrained to ax2+ay2≤μg, 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: ay,min−ε≤ay≤ay,max+ε.The quadratic programming method is used to solve the objective function, and the objective function is converted into the standard type:(25)J=12ΔUk,εTHΔUk,ε+GΔUk,εT,where H, G is the coefficient matrix, H=2ΘkTQΘK+R002ρ, G=2ETkQΘk0; Ek 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:(26)minΔuk,ε12ΔUk,εTHΔUk,ε+GΔUk,εT,s.t.ΔUmin≤ΔUt≤ΔUmax,Umin≤Ut≤Umax,yhc,min≤yhc≤yhc,max,ysc,min−ε≤ysc≤ysc,max+ε.To solve (26), the optimal control increment of front wheel rotation angle can be obtained as follows:(27)ΔU∗k=Δu∗k,Δu∗k+1|t,…,Δu∗k+Nc−1|tT,where Δu∗k,Δu∗k+1|t,…,Δu∗k+Nc−1|t is the system control input at time k,k+1,…,k+Nc−1.The first element of the control sequence is input as the actual control increment to obtain the final control quantity:(28)uk=uk−1+Δu∗k.


## 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 Figure14. 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 xoy is the vehicle coordinate system, XOY is the geodetic coordinate system, a, b is the distance from the center of mass to the front and rear axes, x˙ and y˙ are the speed of the vehicle in the x and y axis direction, φ˙ is the vehicle yaw rate, δf 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 αf and αr are the front and rear tire slip angle.Figure 14
Three degrees of freedom vehicle dynamics model.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 as(17)Flf=ClfSf,Flr=ClrSr,Fcf=Ccfαf=Ccfδf−y˙+aφ˙x˙,Fcr=Ccrαr=Ccrbφ˙−y˙x˙,where 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:(18)x¨=2mClfSf−Ccfδf−y˙+aφ˙x˙δf+ClrSr+y˙φ˙,y¨=2mCcfδf−y˙+aφ˙x˙+Ccrbφ˙−y˙x˙−x˙φ˙,φ¨=2IzaCcfδf−y˙+aφ˙x˙−bCcrbφ˙−y˙x˙,Y˙=x˙sinφ+y˙cosφ,X˙=x˙cosφ−y˙sinφ,where m is the mass of the vehicle, Iz is the moment of inertia of a vehicle, and X˙ and Y˙ 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:Figure 15
Tracking control strategy.Convert the three-degree-of-freedom model of the vehicle to a state-space representation:(19)ξ˙=fξ,u,λ=C·ξ.where ξ is the state quantity, ξ=y˙,x˙,φ,φ˙,Y,XT; u is the control quantity, u=δf; λ is the output, λ=φ,YT.The vehicle 3-DOF dynamics model is a nonlinear model, which is linearized. (19) is expanded at point ξ0,u0 using Taylor’s formula, retaining the first-order term yields a linear time-varying equation:(20)ξ˙=fξ0,u0+Atξ−ξ0+Btu−u0,where At is the fξ,u Jacobian matrix for ξ; Bt is the fξ,u Jacobian matrix for u.Using the first-order quotient difference method to process (19), the discrete state space equation can be obtained:(21)ξk+1=Akξk+Bkuk,where Ak=I+TAt; Bk=TBt; T is the sampling period; t is the sampling time; Np is the prediction time domain; K=t,t+1,……,t+Np; I 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:(22)ξ˜k+1|t=A˜k,tξ˜k|t+B˜k,tΔuk|t,λ˜k|t=C˜k,tξ˜k|t,where ξ˜k is the state matrix of state quantity at k moment and control quantity at k-1 moment, ξ˜k|t=ξk|tuk−1|t; A˜k,t=Ak,tBk,t0I, B˜k,t=Bk,tI; Δuk|t is the control increment, Δuk|t=uk|t−uk−1|t; λ˜k|t is the output of the system at time k.Assume that the prediction time domain of the MPC controller isNp, and the control time domain is Nc. The predicted output of the system at time k is obtained as(23)Yk|t=ψkξ˜k|t+ΘkΔUk|t,where Yk|t is the output matrix in the prediction time domain, Yk=λ˜k+1|tλ˜k+2|t⋮λ˜k+Np|t; ΔUk is the control increment in the control time domain Nc, ΔUk=Δuk|tΔuk+1|t⋮Δuk+Nc−1|t; ψk and Θk are coefficient matrices, ψk=C˜k,tA˜k,tC˜k,tA˜k,t2⋮C˜k,tA˜k,tNp, Θk=C˜k,tB˜k,t0⋯0C˜k,tA˜k,tB˜k,tC˜k,tB˜k,t⋯0⋮⋮⋮⋮C˜k,tA˜k,tNc−1B˜k,tC˜k,tA˜k,tNc−2B˜k,t⋯C˜k,tB˜k,tC˜k,tA˜k,tNcB˜k,tC˜k,tA˜k,tNc−1B˜k,t⋯C˜k,tA˜k,tB˜k,t⋮⋮⋱⋮C˜k,tA˜k,tNp−1B˜k,tC˜k,tA˜k,tNp−2B˜k,t⋯C˜k,tA˜k,tNp−Nc−1B˜k,t.The objective function is to ensure that the vehicle tracks the planned path with minimum error. The objective function of the controller is(24)J=∑i=1Npλk+i|t−λrefk+i|t2Q+∑i=1Nc−1Δuk+i|t2R+ρε2,where λk+i|t is the actual output, λrefk+i|t 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−8∘≤β≤8∘. Set the tire sideslip Angle constraint as −3∘≤α≤3∘. Set the front wheel Angle range as −12∘≤δf≤12∘, The angle change is constrained as −0.8∘≤Δδf≤0.8∘. At the same time, the attachment condition is constrained to ax2+ay2≤μg, 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: ay,min−ε≤ay≤ay,max+ε.The quadratic programming method is used to solve the objective function, and the objective function is converted into the standard type:(25)J=12ΔUk,εTHΔUk,ε+GΔUk,εT,where H, G is the coefficient matrix, H=2ΘkTQΘK+R002ρ, G=2ETkQΘk0; Ek 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:(26)minΔuk,ε12ΔUk,εTHΔUk,ε+GΔUk,εT,s.t.ΔUmin≤ΔUt≤ΔUmax,Umin≤Ut≤Umax,yhc,min≤yhc≤yhc,max,ysc,min−ε≤ysc≤ysc,max+ε.To solve (26), the optimal control increment of front wheel rotation angle can be obtained as follows:(27)ΔU∗k=Δu∗k,Δu∗k+1|t,…,Δu∗k+Nc−1|tT,where Δu∗k,Δu∗k+1|t,…,Δu∗k+Nc−1|t is the system control input at time k,k+1,…,k+Nc−1.The first element of the control sequence is input as the actual control increment to obtain the final control quantity:(28)uk=uk−1+Δu∗k.


## 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 Table2.Table 2
Vehicle parameter list.
Parameters/unitsValueVehicle mass/Kg1270Distance from the center of mass to the front axis/mm1015Moment of inertia/Kg·m22875Front-wheel cornering stiffness/N/rad−56000Wheelbase of the front axle/mm1675Height of the center of mass/mm540Distance from the center of mass to the rear axis/mm1795Effective radius of wheel/mm350Rear wheel cornering stiffness/N/rad−34000Wheelbase of the rear axle/mm1675In 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 Figure16.Figure 16
Tracking control renderings. (a) Lateral position tracking comparison. (b) Lateral position tracking error. (c) Yaw angle tracking comparison. (d) Yaw angle tracking error. (e) Front-wheel angle diagram. (f) Lateral acceleration diagram. (g) Yaw rate diagram. (h) Center of mass slip angle diagram.
(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 Figures16(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 improvedA∗ algorithm and a tracking control strategy based on model predictive control theory. Aiming at dealing with the shortcomings of the traditional A∗ algorithm applied to the motion planning of autonomous vehicles, the traditional A∗ 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 A∗ 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 improvedA∗ algorithm can effectively plan the obstacle avoidance path under corresponding conditions. Compared with the traditional A∗ 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.



---

*Source: 1675736-2022-08-31.xml*