Autonomous land vehicle path planning algorithm based on improved heuristic function of A-Star

The path planning of autonomous land vehicle has become a research hotspot in recent years. In this article, we present a novel path planning algorithm for an autonomous land vehicle. According to the characteristics of autonomous movement towards the autonomous land vehicle, an improved A-Star path planning algorithm is designed. The disadvantages of using the A-Star algorithm for path planning are that the path planned by the A-Star algorithm contains many unnecessary turning points and is not smooth enough. Autonomous land vehicle needs to adjust its posture at each turning point, which will greatly waste time and also will not be conducive to the motion control of autonomous land vehicle. In view of these shortcomings, this article proposes a new heuristic function combined with the artificial potential field method, which contains both distance information and obstacle information. Our proposed algorithm shows excellent performance in improving the execution efficiency and reducing the number of turning points. The simulation results show that the proposed algorithm, compared with the traditional A-Star algorithm, makes the path smoother and makes the autonomous land vehicle easier to control.


Introduction
In recent years, autonomous land vehicle (ALV) has been widely used in emergency rescue, port freight, logistics distribution, and other fields. The development of ALVs is based on path planning, vehicle control, environment perception, and so on. 1 Path planning occupies a core position in the fields of ALV, the efficiency of path planning algorithms directly affects the efficiency of ALV's path finding, and implementation of planning capabilities determines whether the ALV can accomplish the tasks efficiently and safely. 2,3 On the basis of path planning, there are many extended studies. For example, fully sharing the environmental information between different vehicles in autonomous driving is not only conductive to the realization of more adequate path planning but also can ensure higher security level. 4 In addition, the trajectory prediction of dynamic obstacles is also a hot topic of extended research. 5 All these studies have expanded the application and development direction of path planning. In the last decades, lots of path planning algorithms are proposed, such as Floyd algorithm, 6 Dijkstra algorithm, 7 A-Star algorithm, 8 colony algorithm, 9 particle swarm algorithm, 10 and artificial potential field., 11,12 Due to the characteristics of fast search speed, the A-Star algorithm has been widely applied in solving the shortest path problem of the static road network. 13 Derived from Dijkstra algorithm, A-Star introduced heuristic function to enable the fast node search speed. However, the heuristic function of the A-Star algorithm only considered distance information, which leads to redundant expansion nodes in the pathfinding process. In view of the shortcomings of the A-Star algorithm, a large number of scholars have proposed corresponding improved algorithms. Hongbin et al. 14 proposed a hybrid algorithm combining an improved A-Star algorithm and a dynamic window method, which has good performance in complex dynamic environment. Chen et al. 15 defined the valuation function as an exponential decay method to reduce redundant expansion. Wang et al. 16 proposed an efficient full area coverage path planning algorithm based on grid mapping. Zhang et al. 17 considered the actual application scenarios of unmanned boats and improved the evaluation function by adding the corner loss term, so that the final path has the smallest corner and reduces the loss on the path. Wu et al. 18 improved the evaluation function of the A-Star by establishing a taboo table to quickly and effectively implement path planning. Lin et al. 19 improved the real-time performance of the A-Star by introducing the influence of the parent node but did not consider the influence of the value of weight when weighting the evaluation function.
However, the above algorithms only consider the position of the obstacle in the global map but not consider the number of obstacles and distance around the ALV in the path planning process, which is not sufficient for the use of map information. Aiming at the problem of unsmooth path of the A-Star algorithm, we propose a new heuristic function, which is combined with the artificial potential field to optimize the path and improve search efficiency. When calculating heuristic items, our method considers not only the distance from the current position to the target position but also the obstacle information around the current position. The simulation experiment shows that our algorithm performances better than A-Star. Comparing the A-Star algorithm and proposed algorithm in the grid map environment, the result verifies that our method is more conducive to motion control and the planned path is smoother.

Establishment of environment model for ALV path planning
For path planning, environment modeling is a crucial step. According to establish environment model, the real environmental scene is converted into an abstract scene that can be processed by the robot, which is convenient for the robot to carry out path planning. Common map representation methods include topology modeling method, outline map method, and grid map method.
We use the grid map method to establish a grid map to record the obstacle grids and path information of ALV. We define the attributes of the grid, store information such as the parent node of the track and whether the grid is an obstacle and use a matrix to map the information of each node in the grid map. Then binarize the grid map, define 0 as the passable grids and 1 as the obstacle grids to obtain the binarized map. Figure 1 shows an example of the grid map. The black grid represents obstacles, whereas the white grid is the passable area. In the environment map of rasterization model, the map is divided into several closely connected small grids, and each grid represents a node of the map. Each node in the map only constitutes a path with eight adjacent nodes. Therefore, in the grid map, the movement direction of the ALV is generally only 8, which are the directions of the 8 neighboring nodes of the grid map. 20

A-Star algorithm
A-Star algorithm was introduced by Hart et al. 21 and now is widely used in the fields of the shortest path problem. As a typical heuristic search algorithm, the A-Star algorithm evaluates nodes through heuristic search, which improves the efficiency of node search and has better performance and accuracy. 21 The heuristic search using the valuation function, and the valuation function is expressed as follows f ðnÞ ¼ gðnÞ þ hðnÞ (1) where f ðnÞ denotes the total estimated cost from the start position through the current position n to the target position, gðnÞ denotes the actual cost of the path from the starting position to the current position, and hðnÞ denotes the estimated cost from position n to the target position. 22 The pathfinding process of the A-Star algorithm is shown in Figure 2.

Artificial potential field
Khatib introduced the artificial potential field method in 1986. 11 The artificial potential field method constructs a gravitational potential field with the target position as the center. 23 The value of the attractive potential field increases with the increase of the distance between the ALV and the target, and the direction is from the autonomous vehicle to the target. The mathematical expression of the attractive potential field function is as follows F a ðx; yÞ ¼ e Ã ½ðgoal x À xÞ 2 þ ðgoal y À yÞ 2 (2) f a ðx; yÞ is the attractive potential field function and e is a scaling factor. With the current position of the ALV as the center, a repulsive force field is constructed, which decreases as the distance between the obstacle and the ALV increases, and the direction is from the obstacle to the ALV. The mathematical expression of the repulsive force field function is as follows where rðx; yÞ is the distance from the current point ðx; yÞ to the obstacle and d 0 is a distance threshold. When the distance from the current point to the nearest obstacle is greater than d 0 , no repulsive force is generated. h is also a simple scaling factor. These two potential fields together form an abstract artificial potential field. The calculation of the potential field force of the artificial potential field method is shown in Figure 3 In the artificial potential field, for a certain point in space, the ALV will be subjected to a force with a certain magnitude and direction. The artificial potential field method abstracts the motion of the ALV as the motion dragged by this virtual force.

Method
In application scenarios, the ALV first plans a path in the environment map and then proceeds along with the path. However, when using the traditional A-Star algorithm in path planning, the planned path contains too many turning points and the path is not smooth enough. As a result, the ALV needs to make many unnecessary posture adjustments during the path tracking process to adjust the moving  direction of the ALV. When the vehicle moves along the winding path, it will slow the speed of the ALV, and the excessive posture adjustment can easily disrupt the balance of the ALV. To solve those problems mentioned above, this article combines the idea of artificial potential field method to improve the A-Star algorithm and proposes a new heuristic function construction method. In our method, the influence of surrounding obstacles and target points on the ALV is also included in the estimated cost. The new valuation function is expressed as follows where f ðnÞ denotes the total estimated cost from the start position through the current position to the end position, gðnÞ denotes the movement cost of the path from the starting position to the current position [22], we use hðnÞ ¼ h 0 ðnÞ þ vðnÞ as improved heuristic function, h 0 ðnÞ denotes the distance cost in traditional A-Star, and vðnÞ denotes the potential field cost and contains information about surrounding obstacles.

Improved distance heuristics h 0 ðnÞ
The heuristic term h 0 ðnÞ of the heuristic function has an important impact on the computational efficiency of the algorithm. Its weight in the total estimated cost is positively related to the search speed of the A-Star. The heuristic function is informative. The so-called informativeness of the heuristic function is the constraint condition when evaluating a node. The more information (that is, the more constraint conditions), the more nodes will be excluded. For the selection of heuristic term h 0 ðnÞ, there are the following conclusions, where HðnÞ is the actual path length from node n to the target. If h 0 ðnÞ < HðnÞ, the algorithm can find the optimal path but there are many search nodes, and the search efficiency is very low; if h 0 ðnÞ ¼ HðnÞ, this is the most ideal situation, and the algorithm can search for the optimal path with the highest efficiency; and if h 0 ðnÞ > HðnÞ, at this time, although the algorithm searches faster, it is difficult to obtain the optimal path. 24,25,22,26 In addition, there are two extreme cases: 1) If h 0 ðnÞ ) HðnÞ, the A-Star evolves into a breadthfirst algorithm. 2) If h 0 ðnÞ ( HðnÞ, the A-Star can be approximately regarded as the Dijkstra algorithm.
If a heuristic function that is closer to the actual loss can be found, this will improve the search efficiency. The function to estimate the path cost of the A-Star algorithm generally takes the Manhattan distance; in addition, there are Euclidean distance and diagonal distance.
Suppose there are two points P and Q in rectangular coordinates, P point coordinates are ðx P ; y P Þ, and Q point coordinates are ðx Q ; y Q Þ, the Manhattan distance between P and Q can be expressed as follows Euclidean distance can be expressed as follows The diagonal distance can be expressed as follows Considering that the square grid map has eight moving directions, there is a diagonal movement, and the diagonal distance is closer to the actual path cost. To simply verify the influence of these three distance selection methods on the A-Star algorithm, we compared the search speed and the number of search grid nodes of the A-Star algorithm under different distance selection methods through experimental simulations. A verification experiment was carried out on a grid map of size 40 Â 40 and the results are shown in Figure 4 and Table 1. As the experimental results shown, because the estimated cost calculated by using the Manhattan distance method is greater than the cost in the actual path, the search time is the fastest and the number of searched grids is the least. However, the search range is the smallest and failed to search to the shortest path, the length of the path planned in this way is the longest. The estimated cost that is calculated using the Euclidean distance method is smaller than the cost in the actual path, so the number of grid nodes is the largest among the three; the shortest path can be searched, but the search time is the largest. The diagonal distance is closer to the actual length. Therefore, as the experiment shown, the number of grid nodes that are searched using Diagonal distance is moderate, compared to Manhattan distance and Euclidean distance.
The diagonal distance can find the shortest path while the running time is moderate. Therefore, we use the diagonal distance as the distance heuristic h 0 ðnÞ. The formula of h 0 ðnÞ is as follows where x n denotes the abscissa of the current position n, y n denotes the ordinate of the current position n, x e denotes the abscissa of the target, and y e denotes the ordinate of the target.

Obstacle heuristics vðnÞ
According to the principle of artificial potential field method, obstacles will produce repulsive force, which is decreases as the distance between obstacles and nodes increases, and the target position generates attraction, which is proportional to the distance from the node to the target. In this article, the environment map is constructed as a grid map, and the position of each grid is represented by the coordinates. Therefore, the calculation method of the potential field force is different from the artificial potential field method.
In the rasterized map environment, the calculation formula of repulsion is as follows, and the direction is from the obstacle i to the node k F r ðk; iÞ ¼ where k denotes the current position and the coordinates are ðx k ; y k Þ; i represents the i th grid containing obstacles and the coordinates are ðx i ; y i Þ,ñ r denotes the vector from the obstacle i to the node k, andẽ x andẽ y , respectively, represent the unit vector in the X-axis direction and the Y-axis direction. Considering that only those obstacles close to the ALV will affect the ALV's working, and to reduce the amount of calculation, this article limits the scope of obstacles. Where rðx i ; y i is the distance from the current node k to the i th obstacle and d 0 is a distance threshold, in this article, d 0 ¼ 10.
The formula for calculating the magnitude of attraction is shown in Eq. (11), and the direction is from the node k to the target point d where k denotes the current position and the coordinates are ðx k ; y k Þ, d represents the target point and the coordinates are ðx d ; y d Þ, andñ r denotes the vector from the node k to the target point d.
After the attraction and repulsion are obtained, the combined force of the attraction and repulsion can be calculated to obtain the potential field force experienced by the current grid node k. The calculation formula is as follows here a and b, respectively, represent the weight of attraction and repulsion. In this article, a ¼ 5 and b ¼ 4.
The problem of local minima is inevitable in the artificial potential field method. When the calculatedF ðkÞ is 0, to avoid the algorithm return to the traditional A-Star algorithm, we will give up repulsion at that point. At this time, the calculation formula ofF ðkÞ is as follows Since the A-Star algorithm is based on rasterized map information, the eight-neighbor search method is used when searching for nodes, and only grids in eight directions are considered when searching for paths. The potential field force calculated in the above section is any direction. So, when calculating the vðnÞ, we project the potential field force into the eight directions of the neighborhood node, which are front, left front, left, rear left, back, right back, right, and front right. The calculation process is as follows: First calculate the unit vector from the point k to the adjacent node n, as shown in the following equatioñ e n ¼ x n À x k j j e x þ y n À y k j j e y ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ffi Then calculate the angle betweenF ðkÞ andẽ n , as shown in the following equation q n ¼ẽ n ;F ðkÞ (15) The calculation formula of vðnÞ is as shown in the following equation vðnÞ ¼ jF ðkÞjcosq n (16) Figure 5 shows the calculation diagram of the current position k when expanding its adjacent node n. First, calculate the repulsionF r ðk; S i Þ of the obstacle S 1 *S 3 and the attractionF a ðkÞ of the target position d and then calculate the resultant forceF ðkÞ of all repulsive forces and attraction. When calculating the vðnÞ of the node n, which is on the right of current position k, first calculate the direction vectorẽ n and then calculate the projection of the resultant force on the direction vector to obtain vðnÞ. According to this method, calculate the vðn i Þ of the eight adjacent expansion nodes of node k (excluding nodes that are obstacles). Then calculate h 0 ðnÞ, gðnÞ, and f ðnÞ to obtain the cost function of the extended node and perform a path search.
The algorithm steps are as follows: Step 1: Create Open List O and Close List C, the O is used to record the nodes that have been generated but have not been visited, and the C is used to record the nodes that have been visited; add the starting point to the O; Step 2: Add the node k with the smallest cost f in the O to the C and check whether the node is the target node, if it is the target, path planning ends; otherwise, go to the next step; Step 3: Expand all adjacent nodes of node k. Calculate the vðnÞ, gðnÞ, h 0 ðnÞ and f ðnÞ of all adjacent node n. If node n is neither in the O nor in the C, add the node n to the O; if n is in the O, but calculated f ðnÞ is smaller than the old one in the O, update the f ðnÞ of node n in the O, then reorder all nodes in the O according to the value of f ðnÞ; if n is in the C, skip this point. Repeat these steps until all adjacent nodes of node k are expanded; Step 4: Check whether the target point is in the C and whether the O is empty. If the target point is in the C or the O is empty, the algorithm ends; otherwise, repeat Step 2* 4.

Path smoothing
To obtain a smooth path, commonly used methods include spline function and Bezier function. B-spline is an extension of Bezier curve. The polynomial degree of the B-spline can be independent of the number of control points, while the degree of the Bezier curve and the control points are closely related. Therefore, we use the quartic B-spline curves to smooth the planned path. Given n þ 1 control points, the equation of the quartic B-spline curve is as follows The mathematical expression of the basis function is as follows

Experiment
In this section, three sets of grid maps are designed for experimental verification, and each grid in the grid map represents the actual environment of 10 cm Â 10 cm. The computer configuration used in our simulation is i7-6700HQ with 16 GB RAM.
In the first map, the given start position coordinates are ð1; 1Þ, and the target position coordinates are ð20; 17Þ. The original planned path is shown in Figure 6(a), where the red is the path planned by our algorithm and the blue is the path planned by the A-Star algorithm. Figure 6(b) shows the smoothed path. The comparison between A-Star and our proposed algorithm planning path is presented in Table 2. Among them, the time consumed by the algorithm is the average value obtained after 30 repeated experiments.
As shown in Figure 6, the A-Star algorithm passes between a piece of obstacles close to the endpoint. The improved A-Star algorithm avoids crossing obstacles and chooses to detour from the side. There is almost no unnecessary turning point in the planned path. From Table 2, we can see that the path lengths are both 323.1 cm, but the path planned by the A-Star requires a total turn of 540 , while our proposed algorithm only needs a turn of 315 , which greatly reduces turning costs. After the path smoothing, the path of A-Star is shortened to 294.4 cm, and the turning angle is reduced to 219.09 . The path planed by our method is shortened to 303.3 cm, and the turning angle is reduced to 140.86 . Since our algorithm is more complex, the calculation time is longer than that of A-Star. The A-Star algorithm took 3.8 ms, while our algorithm took 5.2 ms.
In the second set of experiments, we selected a more complex environment. The ALV needs to bypass obstacles many times to reach the given target point. The given start position coordinates are (24,17) and the target position coordinates are (4,16).
The original planned path is shown in Figure 7(a). Figure 7(b) shows the smoothed path. The comparison between A-Star and our proposed algorithm planning path is presented in Table 3.
As shown in Figure 7, the path planned by the improved A-Star is very straight, and there are few of unnecessary inflection points. Table 4 presents that the path lengths planned by both are 906.7 cm, but the path planned by the A-Star requires a total turn of 1080 , while our proposed algorithm only needs a turn of 540 . After the path smoothing, the path of A-Star is shortened to 851.6 cm, and the turning angle is reduced to 450.64 . The path planned by our method is shortened to 863.8 cm, and the turning angle is reduced to 270.16 . From the experimental results, we can see that our algorithm has better performance than A-Star. Since our algorithm is more complex, the calculation time is longer than that of A-Star. The A-Star algorithm took 99.6 ms, while our algorithm took 110.1 ms.
We also experimented our algorithm on reactive oxygen species. Use the Gmapping method to create a grid map as shown in Figure 8, and the map resolution is 5 cm=pixel and the puffing safety distance is set to 15 cm. We use a four-wheel drive car based on the Ackerman model and the size is 20 cm Â 18 cm Â 13 cm. The given start position coordinates are (3.6 m, 2.15 m) and the target position coordinates are (4 m, 15 m).
The original planned path is shown in Figure 8(a), where the red are the path planned by our algorithm and the blue are the path planned by the A-Star algorithm. Figure 8(b) shows the smoothed path. The comparison between A-Star  and our proposed algorithm planning path is presented in Table 4. As shown in Figure 8, the path planned by the improved A-Star is very straight and there are few of the unnecessary inflection points. Table 4 presents that the path length planned by ours is 1558.9 cm but the path planned by the A-Star requires a total turn of 1755 , while our proposed algorithm only needs a turn of 495 . After the path smoothing, the path of A-Star is shortened to 1516.5 cm, and the turning angle is reduced to 687.46 . The path planed by our method is shortened to 1534.9 cm, and the turning angle is reduced to 237.84 . From the experimental results, we can see that our algorithm has better performance than A-Star. Since our algorithm is more complex, the calculation time is longer than that of A-Star. The A-Star algorithm took 3332.7 ms, while our algorithm took 6657.4 ms. As the complexity of the map increases, the calculation time consumed also increases. As shown in Figure 8, the path planned by A-Star has many jagged turning points. This makes the path not smooth enough, which is not conducive to the motion control of ALV. Because the ALV has to adjust the angle at each inflection point, this will inevitably reduce the speed and affect the operating efficiency.
As the experimental simulation results shown that, compared with the traditional A-Star algorithm, our improved algorithm has lower turning loss, straighter path. The path length planned by our algorithm is similar to the A-Star algorithm. But, the total turning angle was reduced by 4060% compared with A-Star. However, our algorithm is more complex, and the calculation time will be longer than A-Star. Especially in the environment with many obstacles, the calculation time will increase more obviously, and the efficiency of the algorithm still needs to be further improved. Our algorithm reduces the inflection points, allowing the ALV to easier adjust its posture and realize the autonomous movement of the ALV. When the ALV encounters a turn during the operation, it will reduce the speed and adjust the angle to ensure safe operation. In this process, the efficiency of ALV will be reduced, the control difficulty will increase, and a large part of the time will be consumed. Although our method increases the cost of time spent in planning, it improves the efficiency of operation and reduces the running time and control difficulty of ALV.

Conclusion
In this article, a new heuristic function design method is proposed to improve the widely used A-Star algorithm. The influence of surrounding obstacles on the operation of the trolley is considered. The artificial potential field force is added to the heuristic function to strengthen the search direction information of the algorithm, which can improve the performance of the A-Star algorithm. Compared with the traditional heuristic function that only considers distance, our method takes into account the surrounding obstacle information and is more suitable for environments with many obstacles; the improved algorithm plans a path that is straighter, effectively reducing unnecessary inflection points, and making vehicle can adjust the angle as little as possible, which reduces path loss and is more conducive to the motion control of ALV.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.