basic concept
The A* algorithm was first proposed in 1964 in the paper "A Formal Basis for the Heuristic Determination of Minimum Cost Paths" in IEEE Transactions on Systems Science and Cybernetics. It is a classic heuristic search method. The so-called heuristic search means that when selecting the next node from the current search node, a heuristic function can be used to select the node with the least cost as the next search node. node and jump to it.
In traditional algorithms, depth-first search (DFS) and breadth-first search (BFS) are blind searches when expanding child nodes. That is to say, they will not choose which node is better in the next search and jump to it. Go to this node for the next step of search. In the case of bad luck, the entire solution set space needs to be explored. Obviously, it can only be applied to search problems with a small problem size. What is different from DFS and BFS is that a carefully designed heuristic function can often obtain the optimal solution to a search problem in a very short time.
In the original paper, the steps of the A* algorithm are designed as follows:
1、标记起点s为open，计算起点的估计代价
2、选择open点集中估计代价最小的点
3、如果选中的点∈目标集合T，即到达目标，标记该点closed，算法结束
4、否则，还未到达目标，标记该点closed，对其周围直达的点计算估计代价，如果周围直达的点未标记为closed，将其标记为open；如果已经标记为closed的，如果重新计算出的估计代价比它原来的估计代价小，更新估计代价，并将其重新标记open。返回第2步。
Give a chestnut:
Example 1:
As shown in the figure below, the initial starting point is position 5, and you need to go to position 20. The value on each path represents the cost of passing that path.
According to the above formula, first set point 5 as the starting point, then it has two paths downward: to point 2 and to point 7. The cost values of the two paths are 4 and 5 respectively. Then according to the cost function, we choose to go to the point with a smaller cost value, that is, go to point 2:
Then based on the third step, we judge whether we have reached the end point. It can be seen that 2 is not the end point. Jump to the fourth step, mark the position of this point, and calculate the cost value from it to surrounding points: from point 2, you can go to point 7, the cost value is 9; you can go to point 22, the cost value is 16; you can go to 18 No. 10, the value is 18; you can go to No. 10, the value is 6. Then switch back to step two.
According to the second step, select the point with the smallest value among the above points, which is point 10:
Then according to the third step, determine whether the end point has been reached. It can be seen that 10 is not the end point. Jump to the fourth step, mark the position of the point, and calculate the cost value from it to the surrounding points: from point 10 to point 66, the cost value is 20.
According to the second step, select the point with the smallest value among the above points, which is point 66:
Then according to the third step, determine whether the end point is reached. It can be seen that 66 is not the end point. Jump to the fourth step, mark the position of the point, and calculate the cost value from it to the surrounding points: from point 66 to point 20, the cost value is 6.
According to the second step, select the point with the smallest cost among the above points, which is point 20:
Then according to the third step, judge whether the end point is reached. It can be seen that No. 20 is the end point. At this point, the algorithm ends. The optimal path 5->2->10->66->20 is obtained. It can be seen that A is very high in search speed and accuracy, significantly better than DFS and BFS. But noticed a problem. Different from Dijkstra, although Dijkstra is based on BFS and the search speed is relatively low, its search results must be optimal. Although A can quickly find a path, it is not necessarily optimal. For example, as shown in Example 2:
Example 2:
Change the cost value from 2 to 10 in Example 1 from 6 to 10:
At this time, the path obtained according to the above method should be:
that is, through 5->2->18->66->20, the total cost value is 42. But in fact, the optimal path from 5->20 should be 5->2->10->66->20, and the total generation value is 40. So from here we can also see that although A* is much faster than Dijkstra in terms of search speed, its results may not be optimal.
A* and heuristic functions
After reading the examples, I also have a preliminary understanding of the A* algorithm. So for the A* algorithm, what are its advantages over other algorithms? The advantage of depth-first search is that it is fast, but it can rarely find the optimal solution; while breadth-first search can indeed find the optimal solution, but because breadth-first search searches layer by layer, every point must be expanded. So the time efficiency and space efficiency are not high. The A* algorithm can solve these two shortcomings: it can not only find the optimal solution with great probability, but also reduce redundant time.
So what should we pay attention to when using A*? The most critical point is to determine its heuristic function, that is, to determine the cost value from one point to another point in the above example. In the original algorithm, a cost function is defined:
f (n) = g (n) + h (n) f(n)=g(n)+h(n)f(n)=g(n)+h ( n )
where f[n] is the cost estimate from the initial state to the target state via state n, g(n) is the actual cost from the initial state to n, and h(n) is the estimate from n to the target state cost.
In the estimation of g and h, the estimation method of h is relatively broad, it can be a straight line distance, or it can be other (such as coordinate sum, etc.), but the author mentioned that the estimated h must be smaller than the real h to ensure the algorithm is acceptable, that is, the optimal solution can be found. If the estimated h is larger than the real h, the solution may not be the optimal one.
So set h to 0? Yes, but it will become similar to Dijkstra's algorithm, which will find the optimal route to each point on the map. Eventually, the nodes in the target set T will be reached, and the algorithm will be marked closed, but more unnecessary nodes will be expanded in the process, aimlessly. Therefore, a very important point in the A* algorithm lies in the choice of h.
A* algorithm and path planning
A* is also widely used in path planning of mobile robots. It is a very suitable algorithm when it is necessary to find an effective path from the starting point to the end point.
To give a very classic example:
moving from the green square to the red square in the picture, blue is the obstacle. First, we rasterize the map and call the center of each square a node; this special method simplifies our search area into a 2-dimensional array. Each item in the array represents a grid, and its status is walkable (walkalbe) or unwalkable (unwalkable). By calculating which squares need to be walked from A to the target point, the path is found. Once the path is found, move from the center of one square to the center of another until you reach your destination.
Once we have reduced the search area to a set of quantifiable nodes, the next step is to find the shortest path. In A*, we start from the starting point, check its adjacent squares, and then expand around until we find the goal:
1. Starting from the starting point A, define A as the parent node and add it to openList. Now there is only the starting point A in the openList, and more items will be added later.
2. As shown in the figure above, there are 8 nodes around the parent node A, which are defined as child nodes. Put the reachable or walkable child nodes into openList and become the objects to be inspected.
3. If a node is neither in openList nor closeList, it means that the node has not been searched yet.
4. Initially, the distance between node A and itself is 0, and the path is completely determined. Move it into closeList; each square in closeList does not need to be paid attention to now.
5. The basis for judging the quality of the path is the movement cost. The single-step movement cost adopts the Manhattan calculation method (i.e. d = ∣ x 1 − x 2 ∣ + ∣ y 1 − y 2 ∣ d=|x_1-x_2|+|y_1-y_2 | d=∣x1−x2∣+∣y1−y2∣), that is, the cost of moving a node horizontally and vertically is defined as 10. The cost of diagonal movement is 14
6. Now openList = {B,C,D,E,F,G,H,I}, closeList = {A}
Next we need to select the node with the smallest movement cost f among the adjacent child nodes of node A. The following takes the calculation of node I as an example.
The movement cost evaluation function is: f (n) = g (n) + h (n). f (n) is the cost estimate from the initial state to the goal state via state n, g (n) is the actual cost from the initial state to state n in the state space, h (n) is the minimum cost from state n to the goal state. The estimated cost of the best path.
First, examine g. Since the movement from A to the grid is diagonal, the single-step movement distance is 14, so g = 14.
Let’s examine the estimated cost h again. The meaning of estimation is to ignore whether the remaining path contains obstacles (not walkable), and calculate the cumulative cost of only moving horizontally or vertically according to the Manhattan calculation method: move 3 steps to the right horizontally, and move 1 step vertically. There are 4 steps in total, so h = 40.
Therefore, the total movement cost from node A to node I is: f = g + h = 54
By analogy, the movement costs f of the remaining seven child nodes in the current openList are calculated respectively, and the minimum cost node F is selected and moved to closeList.
Now openList = {B,C,D,E,G,H,I}, closeList = {A,F}
Then continue searching:
Select the (square) node I (square) with the smallest f value from openList (the f value of node D is the same as I. You can choose either one. However, considering the speed, it is faster to select the square that is finally added to openList. This leads to During the pathfinding process, when approaching the goal, the preference of using the newly found square is given priority. Different treatment of the same data will only cause the two versions of A* to find different paths of equal length), taken from openList, Put it in closeList.
Check all child nodes adjacent to it, ignore unwalkable nodes, and ignore nodes that already exist in closeList; if the squares are not in openList, add them to openList and use them as node I child node.
If an adjacent node (assumed to be X) is already in opeLlist, check whether this path is better, that is, whether reaching node X via the current node (the node we selected) has a smaller g value. If not, do nothing. Otherwise, if the g value is smaller, set the parent node of X to the current square, and then recalculate the f value and g value of X.
After judging all child nodes, now openList = {B,C,D,E,G,H,J,K,L}, closeList = {A,F,I}
And so on, and repeat. Once the target node T is found, the path search is completed and the algorithm ends.
After completing the path search, start from the end point and move to the parent node, so that you are brought back to the starting point. This is the path after the search. As shown below. Moving from the starting point A to the end point T is simply to move from the center of one square on the path to the center of another square until the target.
Code
import os
import sys
import math
import heapq
import matplotlib.pyplot as plt
class AStar:
"""AStar set the cost + heuristics as the priority
"""
def __init__(self, s_start, s_goal, heuristic_type,xI, xG):
self.s_start = s_start
self.s_goal = s_goal
self.heuristic_type = heuristic_type
self.u_set = [(-1, 0), (0, 1),
(1, 0), (0, -1)] # feasible input set
self.obs = self.obs_map() # position of obstacles
self.OPEN = [] # priority queue / OPEN set
self.CLOSED = [] # CLOSED set / VISITED order
self.PARENT = dict() # recorded parent
self.g = dict() # cost to come
self.x_range = 51 # size of background
self.y_range = 31
self.xI, self.xG = xI, xG
self.obs = self.obs_map()
def update_obs(self, obs):
self.obs = obs
def animation(self, path, visited, name):
self.plot_grid(name)
self.plot_visited(visited)
self.plot_path(path)
plt.show()
def plot_grid(self, name):
obs_x = [x[0] for x in self.obs]
obs_y = [x[1] for x in self.obs]
plt.plot(self.xI[0], self.xI[1], "bs")
plt.plot(self.xG[0], self.xG[1], "gs")
plt.plot(obs_x, obs_y, "sk")
plt.title(name)
plt.axis("equal")
def plot_visited(self, visited, cl='gray'):
if self.xI in visited:
visited.remove(self.xI)
if self.xG in visited:
visited.remove(self.xG)
count = 0
for x in visited:
count += 1
plt.plot(x[0], x[1], color=cl, marker='o')
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if count < len(visited) / 3:
length = 20
elif count < len(visited) * 2 / 3:
length = 30
else:
length = 40
#
# length = 15
if count % length == 0:
plt.pause(0.001)
plt.pause(0.01)
def plot_path(self, path, cl='r', flag=False):
path_x = [path[i][0] for i in range(len(path))]
path_y = [path[i][1] for i in range(len(path))]
if not flag:
plt.plot(path_x, path_y, linewidth='3', color='r')
else:
plt.plot(path_x, path_y, linewidth='3', color=cl)
plt.plot(self.xI[0], self.xI[1], "bs")
plt.plot(self.xG[0], self.xG[1], "gs")
plt.pause(0.01)
def update_obs(self, obs):
self.obs = obs
def obs_map(self):
"""
Initialize obstacles' positions
:return: map of obstacles
"""
x = 51
y = 31
obs = set()
for i in range(x):
obs.add((i, 0))
for i in range(x):
obs.add((i, y - 1))
for i in range(y):
obs.add((0, i))
for i in range(y):
obs.add((x - 1, i))
for i in range(10, 21):
obs.add((i, 15))
for i in range(15):
obs.add((20, i))
for i in range(15, 30):
obs.add((30, i))
for i in range(16):
obs.add((40, i))
return obs
def searching(self):
"""
A_star Searching.
:return: path, visited order
"""
self.PARENT[self.s_start] = self.s_start
self.g[self.s_start] = 0
self.g[self.s_goal] = math.inf
heapq.heappush(self.OPEN,
(self.f_value(self.s_start), self.s_start))
while self.OPEN:
_, s = heapq.heappop(self.OPEN)
self.CLOSED.append(s)
if s == self.s_goal: # stop condition
break
for s_n in self.get_neighbor(s):
new_cost = self.g[s] + self.cost(s, s_n)
if s_n not in self.g:
self.g[s_n] = math.inf
if new_cost < self.g[s_n]: # conditions for updating Cost
self.g[s_n] = new_cost
self.PARENT[s_n] = s
heapq.heappush(self.OPEN, (self.f_value(s_n), s_n))
return self.extract_path(self.PARENT), self.CLOSED
def get_neighbor(self, s):
"""
find neighbors of state s that not in obstacles.
:param s: state
:return: neighbors
"""
return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set]
def cost(self, s_start, s_goal):
"""
Calculate Cost for this motion
:param s_start: starting node
:param s_goal: end node
:return: Cost for this motion
:note: Cost function could be more complicate!
"""
if self.is_collision(s_start, s_goal):
return math.inf
return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
def is_collision(self, s_start, s_end):
"""
check if the line segment (s_start, s_end) is collision.
:param s_start: start node
:param s_end: end node
:return: True: is collision / False: not collision
"""
if s_start in self.obs or s_end in self.obs:
return True
if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
else:
s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
if s1 in self.obs or s2 in self.obs:
return True
return False
def f_value(self, s):
"""
f = g + h. (g: Cost to come, h: heuristic value)
:param s: current state
:return: f
"""
return self.g[s] + self.heuristic(s)
def extract_path(self, PARENT):
"""
Extract the path based on the PARENT set.
:return: The planning path
"""
path = [self.s_goal]
s = self.s_goal
while True:
s = PARENT[s]
path.append(s)
if s == self.s_start:
break
return list(path)
def heuristic(self, s):
"""
Calculate heuristic.
:param s: current node (state)
:return: heuristic function value
"""
heuristic_type = self.heuristic_type # heuristic type
goal = self.s_goal # goal node
if heuristic_type == "manhattan":
return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
else:
return math.hypot(goal[0] - s[0], goal[1] - s[1])
def main():
s_start = (5, 5)
s_goal = (45, 25)
astar = AStar(s_start, s_goal, "euclidean",s_start,s_goal)
path, visited = astar.searching()
astar.animation(path, visited, "A*") # animation
if __name__ == '__main__':
main()
The effect is as follows:
briefly analyze:
After obtaining the starting point, the algorithm maintains two dictionaries:
self.PARENT[self.s_start] = self.s_start
self.g[self.s_start] = 0
self.g[self.s_goal] = math.inf
What is stored in the PARENT dictionary is the point from which this point extends, that is, who is its previous node, which is used for the return of the final path; what is stored in the dictionary g is the cost value of each coordinate, that is, g[ s].
The algorithm then maintains a stack through the concept of stack:
heapq.heappush(self.OPEN,(self.f_value(self.s_start), self.s_start))
Store the initial value of f[s] here, and then do a while loop:
First remove the top element from the stack:
_, s = heapq.heappop(self.OPEN)
Notice the two functions heapq.heappop and heapq.heappush in the heapq stack function used here. Heappop will take out the top element of the stack and delete the original data from the stack, while heappush will sort the inserted data by size and store it in the stack. Therefore, each traversed point will be put into the stack according to its cost value, and the one with the smallest cost value will be taken out every time.
Then determine whether the top element of the stack is the target point. If it is the target point, exit:
if s == self.s_goal: # stop condition
break
If not, update the cost values of points near the point:
for s_n in self.get_neighbor(s):
for s_n in self.get_neighbor(s):
new_cost = self.g[s] + self.cost(s, s_n)
if s_n not in self.g:
self.g[s_n] = math.inf
if new_cost < self.g[s_n]: # conditions for updating Cost
self.g[s_n] = new_cost
self.PARENT[s_n] = s
heapq.heappush(self.OPEN, (self.f_value(s_n), s_n))
get_neighbor is to get the coordinates of points around the point. The calculation method of the cost value of the point that needs to be stored when heappush is pushed onto the stack is:
def f_value(self, s):
"""
f = g + h. (g: Cost to come, h: heuristic value)
:param s: current state
:return: f
"""
return self.g[s] + self.heuristic(s)
Among them, self.g[s] is g in algorithm A, and self.heuristic(s) is h in algorithm A. As a heuristic function, the calculation of h can be selected according to the actual situation. For example, in a raster map, the calculation methods of h can generally be divided into two types: Manhattan distance and Euclidean distance.
Euclidean distance is what we usually use more often, that is, to find the length of a straight line between two points:
else:#sqrt(x^2+y^2)
return math.hypot(goal[0] - s[0], goal[1] - s[1])
The Manhattan distance is relatively not that common. Simply put, it is to find the number of grids that need to be walked in the X direction from the current point to the end point and the number of grids that need to be walked in the Y direction. :
if heuristic_type == "manhattan":
return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
Applying these two different calculation methods will yield different results:
The result calculated using Manhattan distance:
The result calculated using Euclidean distance:
It can be seen that there will be certain differences in the results of traversing using different h methods. In addition, the selection of points when searching for surrounding points will also affect the traversal of the algorithm. For example, in the above code, u_set is set to:
self.u_set = [(-1, 0), (0, 1),
(1, 0), (0, -1)] # feasible input set
That is, each point can only search the four points before, after, left and right. But if you change it to search for 8 points, the search results will become:
At this point, the simple principles of the A* algorithm have been sorted out.
reference:
1. Research and summary of fifteen classic algorithms (1)-A* search algorithm