A* algorithm was published by Hart eta[23]. in 1968. It is a common path-finding and graph traversal algorithm, that combines the advantages of the Dijkstra algorithm and best-first search. It uses a heuristic function to guide the search direction and improves efficiency and accuracy. The core of the A* algorithm is the evaluation function f (n), which represents the estimated cost of the optimal path from the start point to the endpoint through node n. The evaluation function consists of two parts, one is g (n), which represents the actual cost from the start point to node n; the other is h (n), which represents the expected cost from node n to the end point, that is, the heuristic function. The choice of heuristic function has a great impact on the performance and accuracy of the A* algorithm. If the heuristic function h (n) is always 0, then the A* algorithm degenerates into the Dijkstra algorithm; if the heuristic function h (n) is always less than or equal to the actual cost from node n to the endpoint, then A* algorithm can guarantee to find the shortest path; if the heuristic function h (n) is exactly equal to the actual cost from node n to the endpoint, then A* algorithm will find the best path and be very fast; if the value of heuristic function h (n) is larger than the actual cost from node n to the endpoint, then A* algorithm cannot guarantee to find the shortest path, but it will be very fast.
ts search weight function is:
\(\:\text{f}\left(\text{n}\right)\:=\:\text{g}\left(\text{n}\right)\:+\:\text{h}\left(\text{n}\right)\) (Eq. 1)
where g(n) is the cost of the node from the start point, and h(n) is the estimated cost of the node n to the endpoint, that is, the heuristic function.
Common heuristic functions include Manhattan distance, diagonal distance, and Euclidean distance. Their calculation methods are as follows:
Manhattan distance:
\(\:\text{h}\left(\text{n}\right)\:=\:\text{D}\ast\:\left(\right|\text{n}.\text{x}\:-\:\text{g}\text{o}\text{a}\text{l}.\text{x}|\:+\:|\text{n}.\text{y}\:-\:\text{g}\text{o}\text{a}\text{l}.\left|\right)\) (Eq. 2)
Diagonal distance:
\(\:\text{h}\left(\text{n}\right)\:=\:\text{D}\ast\:\left(\right|\text{n}.\text{x}\:-\:\text{g}\text{o}\text{a}\text{l}.\text{x}|\:+\:|\text{n}.\text{y}\:-\:\text{g}\text{o}\text{a}\text{l}.\left|\right)+(\text{D}2\:-\:2\ast\:\text{D})\ast\:\:\text{m}\text{i}\text{n}\left(\right|\text{n}.\text{x}\:-\:\text{g}\text{o}\text{a}\text{l}.\text{x}|\:+\:|\text{n}.\text{y}\:-\:\text{g}\text{o}\text{a}\text{l}.\left|\right)\) (Eq. 3)
Euclidean distance:
\(\:\text{D}\ast\:\sqrt{(\text{n}.\text{x}\:-\:\text{g}\text{o}\text{a}\text{l}.\text{x})²+\:(\text{n}.\text{y}\:-\:\text{g}\text{o}\text{a}\text{l}.\text{y})²}\) (Eq. 4)
where D is the moving cost between adjacent nodes, and D2 is the moving cost between diagonally adjacent nodes.
The flowchart of the A* algorithm is shown in Fig. 1, and the specific steps are as follows:
Step 1: Initialize the open list and the closed list, and add the starting point as the root node to the open list.
Step 2: If the open list is not empty, select the node n with the smallest evaluation function f (n) from the open list as the current node: if node n is the end point, then: trace back the parent nodes from the end point to the starting point; return the found result path, and end the algorithm; if node n is not the end point, then: delete node n from the open list and add it to the closed list; traverse all the neighboring nodes m of node n : if the neighboring node m is in the closed list or not passable, then: skip it and select the next neighboring node; if the neighboring node m is not in the open list, then: set node n as the parent node of node m; calculate the evaluation function f (m), g (m) and h (m) of node m, where g (m) is the actual cost from the starting point to node m, h (m) is the estimated cost from node m to the end point, f (m) = g (m) + h (m) ; add node m to the open list; if the neighboring node m is already in the open list, then: check whether there exists a better path through node n to reach node m, that is, compare g (n) + cost (n, m) and g (m), where cost (n, m) is the moving cost from node n to node m ; if g (n) + cost (n, m) is smaller, then: set node n as the parent node of node m; update the evaluation function f (m) and g (m) of node m .
Although the heuristic A* algorithm improves some problems of traditional A* algorithm such as traversing many nodes and slow speed, its efficiency is still not high. If the configuration space is too large and there are many obstacles in between, the number of nodes traversed by it will still be very large, and its speed will not be much improved compared with the traditional A* algorithm.
Improved A* algorithm
Fixed sampling function
Li eta[17]. proposed an improved bidirectional A* (Bidirectional A*) algorithm based on the A* algorithm, which introduced a bidirectional alternating search strategy. Although this algorithm improved the search speed, it still needed to traverse a large number of nodes in the expansion process due to the lack of random sampling. Xin eta[24]. proposed a strategy of sampling a fixed point in an improved RRT algorithm, which specified the exploration direction and improved the path search efficiency.
There are two cases for determining the sampling point Qfix. If the midpoint of the line between Qstart(xs, ys) and Qgoal(xg, yg) does not collide with obstacles, use formula (Eq. 5) to calculate this midpoint Qcenter(xc, yc), and use it as the sampling fixed point.
$$\:\:\text{x}\text{c}\:=\:(\text{x}\text{s}\:+\:\text{x}\text{g})\:/\:\:2\:$$
\(\:\text{y}\text{c}\:=\:(\text{y}\text{s}\:+\:\text{y}\text{g})\:\:/\:\:2\) (Eq. 5)
When the midpoint collides with the obstacle, use the fixed sampling function to determine the fixed point. When Qcenter is in the obstacle area, first draw a circle with Qcenter as the center and R as the radius, initialize the radius Rs as 1, traverse from 0° to 360° every 10° on the circle, calculate the coordinates on the circle, if the node is safe, use this node as the sampling point. If no sampling point is found, increase the radius of the circle and continue to find a safe node. Its expression is:
The formula for the generated circle is:
\(\:(\text{x}\:-\:\text{x}\text{c})\:+\:(\text{y}\:-\:\text{y}\text{c})\:=\:\text{r}²\) (Eq. 6)
The formula for the sampling point is:
\(\:(\text{x}\text{n}\:,\:\text{y}\text{n})\:=\:(\text{x}\text{c}\:+\:\text{r}\text{c}\text{o}\text{s}\text{n}\:\times\:\:10^\circ\:,\text{y}\text{c}\:+\:\text{r}\text{s}\text{i}\text{n}\text{n}\:\times\:\:10^\circ\:),\text{n}\:=\:\text{1,2},\text{3,4}...35\) (Eq. 7)
The idea of the Sampling algorithm is shown in Algorithm 1.
Algorithm 1 Sampling |
Input: Map, k; Output: x, y; 1 : while True do 2 : x ← Random(min_x, max_x); 3 : y ← Random(min_y, max_y); 4 : if k % 10 = = 0 then 5 : x ← gx; 6 : y ← gy; 7 : if verify_node(x, y) then 8 : return x, y; |
Improvement of search method
A* algorithm has a heuristic function, which explores by estimating the cost from the starting point to the target point, but the results are not satisfactory through experimental verification. Therefore, this section recommends how to improve the search method. Similar to the basic A* algorithm, it finds a midpoint Qc from the line between the starting point Qs and the target point Qg, and uses a sampling function to find a fixed point Qf, and then explores from the starting point and target point to the fixed point at the same time. The improved A* algorithm is shown in Fig. 3.
The steps of the A* algorithm using the fixed samplingfunction are:
Step1, The midpoint Qc of the connection between the initial point Qs and the target point Q is calculated by Eq. (4), and it is determined whether this point is located in the barrier area and performs 3 if this point is not in the obstacle area, otherwise perform Step2.
Step 2. Use the sampling function to determine whether the sampling point is in the obstacle or not. If not, determine the sampling point. If it is in the obstacle, enlarge the radius of the search circle and resample until a fixed sampling point Qf is determined.
Step 3. The starting point Qs and target point Qg explore towards the fixed sampling point at the same time. When in some special obstacle environments, if starting point Qs or target point Qg can directly point to target or when starting point Qs and target point Qg traverse to the same node during the exploration process, discard fixed sampling point Qf and directly generate path. Otherwise, execute Step 4.
Step 4. Starting point Qs and target point Qg find fixed sampling point Qf, connect two paths, generate a final path, and the algorithm ends.
The complete idea of the Improved A* algorithm is shown inAlgorithm 2.
Algorithm 2 Improved A* |
Input: Map, sx, sy, gx, gy, xp, yp; Output: path1, path2; 1 : start_node ← new Node(calc_xy_index(sx, min_x), calc_xy_index(sy, min_y), 0.0,−1); 2 : goal_node ← new Node(calc_xy_index(gx, min_x), calc_xy_index(gy, min_y), 0.0,−1); 3 : state_node ← new Node(calc_xy_index(xp, min_x), calc_xy_index(yp, min_y), 0.0,−1); 4: open_set ← new dict(); 5: closed_set ← new dict(); 6: open_set1 ← new dict(); 7: closed_set1 ← new dict(); 8: open_set[calc_grid_index(start_node)] ← start_node; 9: open_set1[calc_grid_index(goal_node)] ← goal_node; 10: while True do 11: if open_set is empty then 12: c_id ← min(open_set, key = lambda o: open_set[o].cost + calc_heuristic(state_node, open_set[o])); 13: current ← open_set[c_id]; 14: if current.x = = state_node.x and current.y = = state_node.y then 15: path1 ← calc_final_path(state_node, closed_set); 16: if open_set1 is empty then 17: c_id1 ← min(open_set1, key = lambda o: open_set1[o].cost + calc_heuristic(state_node, open_set1[o])); 18: current1 ← open_set1[c_id1]; 19: if current1.x = = state_node.x and current1.y = = state_node.y then 20: path2 ← calc_final_path(state_node, closed_set1); 21: return path1, path2; |