Robot path planning based on improved A* algorithm

To address the issues of excessive polyline paths and the increased number of search nodes in the A* algorithm, a weighted WA* algorithm is proposed based on the A* algorithm. Firstly, a new distance function is introduced to reduce computational resource usage and improve search efficiency. Then, the heuristic function is improved by searching all feasible paths to find the shortest path. When the lengths of feasible paths are very similar, an additional value is added to the heuristic function along with weights to minimize frequent searches. Lastly, Bezier curves are introduced to enhance the smoothness of the global path. Simulation results show that the improved WA* algorithm reduces the number of turns, time consumption, search space, and turning angles by 25.0%, 65.4%, 53.5%, and 18.7%, respectively, compared to the A* algorithm. Compared to the bidirectional A* algorithm, these reductions are 7.6%, 34.4%, 43.5%, and 7.1%, respectively. In addition, the WA* algorithm uses Bezier curves to curve-fit the generated path points, making the path trajectory smoother.


Introduction
With the improvement in the performance of external sensors and the continuous development of automation technology and intelligent control technology, mobile robots have been widely applied in various fields, such as intelligent logistics and smart factories.Path planning technology has become a research hotspot in the field of autonomous driving and mobile robots.It enables intelligent decisionmaking for the robot's motion based on the given target position and the current environmental model, ultimately generating an optimal path to fulfill its requirements.Path planning algorithms can be classified based on whether they have prior environmental information.Global path planning algorithms, which operate in a known environment map; local path planning algorithms, which operate in an unknown environment; each have their own advantages and disadvantages.Global path planning algorithms can find the best path when the environment map is known in advance.However, acquiring the environment map in advance is impractical in unknown environments.Furthermore, in practical applications, map information may be uncertain due to errors and update frequency.On the other hand, local path planning algorithms operate in unknown environments and have the advantage of quickly adapting to environmental changes.However, they cannot guarantee the globally optimal solution and may easily get trapped in local optima.Currently, commonly used local path planning algorithms include artificial potential field method [1] and dynamic window approach [2], while global path planning algorithms mainly include A* algorithm [3], random tree method [4].
In global path planning, the node-based A* algorithm is used in the field of path planning due to its high computational efficiency and the ability to generate short paths.However, the A* algorithm has some limitations, including generating paths with redundant points, lack of path smoothness, and excessive turning angles.To address these issues, many researchers have conducted studies on the A* algorithm and proposed various improved versions of A*.Guruji et al. [5] proposed a time-efficient A* algorithm that calculates heuristic values only when there is a collision risk at a node.This algorithm sacrifices a certain amount of path length and smoothness to significantly improve computational efficiency.Chen et al. [6] introduced an improved A* algorithm that reduces the search space by considering the angle relationship between the current grid and the starting grid, constructing a cross-product, and incorporating it into the heuristic function to effectively narrow the search direction of the current node.Zhao et al. [7] combined the A* algorithm with the JPS strategy, which greatly reduces the number of expanded nodes during the path search process and obviously improves the algorithm's speed.However, it does not deeply address the issue of excessive turning angles in the path.Cao et al. [8] adjusted the weights in the A* algorithm and optimized the turning angles, effectively improving algorithm efficiency while reducing the number of turning angles.
Aiming at the above problems, this paper proposes an improved A* algorithm for robot path planning.Based on the classical A* algorithm, optimizations are made to the distance function and heuristic function.Furthermore, high-order Bezier curves are utilized to achieve smooth optimization of the planned path.This approach aims to improve both planning efficiency and path quality, ultimately generating a path that complies with the robot's kinematics while ensuring planning efficiency.

A* algorithm
For grid-based maps, the goal of path planning algorithms is to find the optimal path from the starting node   ∈  to the goal node   ∈ , where  represents the set of all nodes contained in the two-dimensional grid.
Dijkstra's algorithm has low efficiency in path searching when there is a high search rate for the goal.The A* algorithm addresses this issue by incorporating a heuristic function to provide directional guidance in the search for the goal.Dijkstra's algorithm is essentially an undirected search that performs an indiscriminate search on all positions, resulting in high computational and memory costs.By introducing a heuristic function for differentiated search, A* algorithm eliminates less probable directions and positions.As a result, A* significantly reduces CPU computation and memory usage compared to Dijkstra's algorithm.
Represented as the A* algorithm as follows: () is the cost value associated with grid node s in path planning, which consists of two parts: the cost value from the starting node to the current grid node s is represented by (), which is the length of the path traversed through the grid nodes, and ℎ() represents the Manhattan distance from grid node s to the target grid node   .
The flowchart of the A* algorithm is shown in Figure 1.

End
Add the start point and obstacles to the open_list and close_list respectively

Check if the open_list is empty
Select the node with the minimum cost from the open_list and add it to the close_list.

Skip expanding other nodes and continue
Check if the current node is the goal node

Check if the current node is in the close_list
Add the child nodes of the current node to the open_list The choice of the heuristic function ℎ() plays a crucial role in determining the search behavior of the A* algorithm.Once the cost function () is constructed and the environment map model is imported, with the   and the   determined, the path search can begin.Before the A* algorithm starts searching for the path, two lists are defined: an _ , which stores the nodes to be expanded, and a _, which stores the expanded nodes and the obstacles in the environment map.By continuously updating these two lists, the algorithm selects the node with the minimum value of the cost function () from the open list for the next expansion step, until the path goal point   is reached, completing the path search and planning the shortest path.

Improved A* algorithm
The A* algorithm exhibits unnecessary turns and redundant nodes during the search, resulting in longer planning times, poor path smoothness, and a low level of safety due to paths crossing obstacle vertices.To address these issues, this paper proposes improvements to the A* algorithm in three main aspects.Firstly, the distance function is enhanced.Secondly, the weight ratio between estimated cost and actual cost in the evaluation function is adjusted, incorporating environmental information into the heuristic function.Lastly, path smoothing optimization is applied.

Improvement of the distance function
The Euclidean distance used in the A* algorithm is prone to underestimation compared to the actual minimum distance.Its calculation process consumes a significant amount of computational resources.
When the robot's working map is large and contains numerous obstacles, accessing a large number of nodes is required, resulting in increased algorithm execution time.On the other hand, the Manhattan distance tends to overestimate the distance and results in excessive path turns.Based on these observations, a Chebyshev-like distance is introduced to express the distance between the current node and the destination.The definition of this distance is as follows: in the equation, (  ,   ) and (  ,   ) represent the coordinates of two distinct nodes.If the cost function  for the current node corresponds to multiple paths, the path planning algorithm will search all feasible paths to find the shortest path.However, this situation is quite common in maps with few obstacles, as there are many feasible paths with similar lengths.To address this issue, an additional value can be added to the heuristic function.The magnitude of this additional value is calculated based on the cross-product of the vector from the current point to the target point and the vector from the initial point to the target point.This, in turn, modifies the value of the heuristic function h, making it more inclined to choose paths that align with the line from the initial point to the target point.The specific description is as follows:

Improvement of the heuristic function
in the equation,  represents the cross product between the vector from the initial point to the target point and the vector from the current point to the target point.Subscripts , , and  denote the current node (current), the target node (end), and the starting node (start), respectively.In actual scenarios, when starting the search, the most important aspect is to move quickly to any location.As the search approaches the end, the primary objective becomes finding the target.Therefore, in the heuristic function, a weight ( ≥ 1) is introduced.This weight reduces the essential of the heuristic function as the search approaches the target, while enhancement the relative essential of the actual cost of the path.
With the two aforementioned improvements, the ℎ function can be represented as follows: in the equation,  is a crossover operator that breaks the balance;  is the weight, and the specific size is set to  = () () + 1; () indicates the distance from the current node  to the end point, which is the improved Chebyshev-like distance.Similarly, () is the Chebyshev-like distance from the starting point  to the ending point.The closer it is to the target position, the smaller the weight value will be, which will reduce the significance of the heuristic function.This A* search algorithm using improved heuristic function is called weighted algorithm WA*.

Path smoothing based on Bezier curves
The WA* algorithm outputs the globally optimal path points for grid-based maps.Considering the sparse and non-smooth characteristics of the path points, in order to better control the robot, it is necessary to construct a trajectory in the state space that connects any given two states (without considering obstacles).This will transform the sparse path points into a smooth curve or dense trajectory points, which can then be transmitted to the lower-level driver for control.The Bezier curve [9] is well-suited for path smoothing tasks due to its endpoint properties and convex hull nature.It has become one of the most important methods in the field of global path optimization.Therefore, to address this issue, this paper puts a method that combines Bezier curves and WA* to optimize trajectories.
The equation for a second-order Bezier curve is as follows: The principle is as follows.For three points in a plane, they are connected sequentially as shown in Figure 2.Then, on segments AO and DO, points B and C are taken, respectively, such that AB/AO=OC/OD.Points B and C are connected to form line BC.On line BC, point P is taken such that BP/BC=AB/AO=OC/OD.All points that satisfy these conditions are connected to obtain the Bezier curve.
, () is the basis function of Bezier curve.The Bessel curve is optimized with  as  degree.Depending on the selection of control points, the shape and curvature of the Bezier curve will vary.When the distance between the start and target points is too large, using a single high-degree Bezier curve for smoothing may result in the possibility of the smoothed path crossing obstacles, as the curve may pass through control points near the obstacles.In this paper, we can divide the actual path into segments based on its shape and characteristics, and apply high-degree Bezier curves for smoothing individually to each segment.This approach can better avoid the problem of path crossing obstacles and ensure a higher quality of the smoothed path.We can use adaptive control point selection algorithms to select control points that better adapt to the characteristics and shape of the path.For example, the number and positions of control points can be dynamically adjusted based on parameters such as path curvature and length to achieve better smoothing effects and path quality.Figure 3 shows an enlarged grid, where the red polyline represents the original path, and the blue curve represents the smoothed path obtained using high-degree Bezier curves.
After implementing the WA* algorithm for path planning, we obtain the coordinates of multiple points along the path and display the planned path trajectory.At this point, we can integrate the Bezier function to optimize the original path.The flowchart of Bezier function integration optimization is shown in Figure 4.

Begin Parameter setting of Bezier curve
The WA* algorithm performs path planning and outputs a set of path points Q The set Q of path points is taken as the control point of Bezier curve.

End
Output the optimized path of Bezier curve

Algorithm simulation
To validate the effectiveness of the WA* algorithm, two other algorithms were simulated and compared with this algorithm.One is the A* algorithm, and the other is the Bidirectional A* algorithm [10] .The experiments were conducted in a Python 3.8 environment, using a grid with a side length of 1 meter and a grid map area of 35x35.The green dots represent the start nodes, and the red dots represent the target nodes.The black dots represent obstacles, and the white grid indicates the area without obstacles.The parameters used in the improved A* algorithm were a point step size of k=1 and a safety distance of D=0.5 meters.
Figure 5 shows the result of A* algorithm planning.Figure 6 shows the result of Bidirectional A* algorithm planning, and Figure 7 shows the result of improved WA* algorithm planning.The yellow and green crosses represent the number of nodes traversed by each algorithm, which indicates the search space.A smaller number of traversed nodes indicates a smaller search space and higher efficiency of the algorithm.Through the analysis of the simulation figures, it can be observed that the WA* algorithm optimizes the selection of child nodes and path smoothness, resulting in shorter path lengths, fewer turns, and better path smoothness.All three algorithms maintain a certain distance from obstacles to avoid collisions.Figure 6.Bidirectional A* algorithm.Figure 7. WA* algorithm.

Data analysis
To better demonstrate the superiority of the WA* algorithm, we have organized the experimental data of the traditional A* algorithm, Bidirectional A* algorithm, and WA* algorithm into a table.The specific performance metrics are shown in Table 2.We can observe that the WA* algorithm has significant advantages compared to the other two algorithms.Table 2. Experimental results of global path planning algorithm.
The data comparison in Table 3 shows that the WA* algorithm has optimized the child node selection method.Compared to the A* algorithm, the WA* algorithm reduces the search space by 53.5% and reduces the planning time by 65.4%.Compared to the Bidirectional A* algorithm, the WA* algorithm reduces the search space by 43.5% and reduces the planning time by 34.4%.This indicates that the WA* algorithm can search for paths faster.
Furthermore, the WA* algorithm reduces the number of turns and turn angles by 25.0% and 18.7%, respectively, compared to the A* algorithm.It also reduces the number of turns and turn angles by 7.6% and 7.1%, respectively, compared to the Bidirectional A* algorithm.This indicates that the paths planned by the WA* algorithm is more conducive to the movement of the mobile robot.
Although the improved A* algorithm slightly increases the path length by 2.5%, when combined with the path planning simulation, we can observe that the traditional A* algorithm is prone to collisions.In contrast, the improved A* algorithm avoids the vertices of obstacles and sets a safety distance, resulting in safer and more reasonable paths.In Figure 8, the blue curve represents the path after being smoothed by high-order Bezier curves.It can be seen that the path forms a smooth curve, which is more suitable for the walking of robots.The results demonstrate that the proposed WA* algorithm, integrated with Bezier curves, is feasible and robust.(2) The path points are fitted with high-order Bezier curves to connect the sparse path points into a smooth curve, which ensures the smoothness of the final path and conforms to the underlying drive control of the mobile robot.

Figure 4 .
Figure 4. Flowchart for the Bezier function integration optimization.
() represent the actual shortest distance between the current and the goal.A simple comparison reveals that different values of ℎ() will result in different search conditions, as shown in Table1.Table1.Compare () and ℎ()

Table 3 .
Comparison results of WA* with A* and bidirectional A* respectively.In this paper, some problems of A* algorithm in the process of robot path planning are targeted for improvement, and WA* algorithm is proposed.The generated path points are optimized for trajectory.After detailed comparison experiments, we reach the following conclusions.Compared with the A* algorithm and Bidirectional A* algorithm, WA* algorithm has obvious improvement in the number of turns, time consumption, search space and turn angle, and can plan a path of reasonable length.