若当前处理节点的相邻格子已经在Open List中,则检查这条路径是否更优,即计算经由当前处理节点到达那个方格是否具有更小的 G值。如果没有,不做任何操作。相反,如果G值更小,则把那个方格的父节点设为当前处理节点 ( 我们选中的方格 ) ,然后重新计算那个方格的 F 值和 G 值。
若当前处理节点的相邻格子不在Open List中,那么把它加入,并将它的父节点设置为该节点。
按照上述规则我们继续搜索,选择起点右边的方格作为当前处理节点。它的外框用蓝线打亮,被放入了close list 中。然后我们检查与它相邻的方格。它右侧的3个方格是墙壁,我们忽略。它左边的方格是起点,在 close list 中,我们也忽略。其他4个相邻的方格均在 open list 中,我们需要检查经由当前节点到达那里的路径是否更好。我们看看上面的方格,它现在的G值为14 ,如果经由当前方格到达那里, G值将会为20( 其中10为从起点到达当前方格的G值,此外还要加上从当前方格纵向移动到上面方格的G值10) ,因此这不是最优的路径。看图就会明白直接从起点沿对角线移动到那个方格比先横向移动再纵向移动要好。
当把4个已经在 open list 中的相邻方格都检查后,没有发现经由当前节点的更好路径,因此不做任何改变。接下来要选择下一个待处理的节点。因此再次遍历open list ,现在open list中只有 7 个方格了,我们需要选择F值最小的那个。这次有两个方格的F值都是54,选哪个呢?没什么关系。从速度上考虑,选择最后加入 open list 的方格更快。因此选择起点右下方的方格,如下图所示。
接下来把起点右下角F值为54的方格作为当前处理节点,检查其相邻的方格。我们发现它右边是墙(墙下面的一格也忽略掉,假定墙角不能直接穿越),忽略之。这样还剩下 5 个相邻的方格。当前方格下面的 2 个方格还没有加入 open list ,所以把它们加入,同时把当前方格设为他们的父亲。在剩下的 3 个方格中,有 2 个已经在 close list 中 ( 一个是起点,一个是当前方格上面的方格,外框被加亮的 ) ,我们忽略它们。最后一个方格,也就是当前方格左边的方格,检查经由当前方格到达那里是否具有更小的 G 值。没有,因此我们准备从 open list 中选择下一个待处理的方格。
不断重复这个过程,直到把终点也加入到了 open list 中,此时如下图所示。注意在起点下方 2 格处的方格的父亲已经与前面不同了。之前它的G值是28并且指向它右上方的方格。现在它的 G 值为 20 ,并且指向它正上方的方格。这是由于在寻路过程中的某处使用新路径时G值更小,因此父节点被重新设置,G和F值被重新计算。
function A*(start, goal)// The set of nodes already evaluated.
closedSet :={}// The set of currently discovered nodes still to be evaluated. Initially, only the start node is known.
openSet :={start}// For each node, which node it can most efficiently be reached from.// If a node can be reached from many nodes, cameFrom will eventually contain the most efficient previous step.
cameFrom := the empty map
// For each node, the cost of getting from the start node to that node.
gScore := map with default value of Infinity
// The cost of going from start to start is zero.
gScore[start]:=0// For each node, the total cost of getting from the start node to the goal// by passing by that node. That value is partly known, partly heuristic.
fScore := map with default value of Infinity
// For the first node, that value is completely heuristic.
fScore[start]:=heuristic_cost_estimate(start, goal)while openSet is not empty
current := the node in openSet having the lowest fScore[] value
if current = goal
returnreconstruct_path(cameFrom, current)
openSet.Remove(current)
closedSet.Add(current)for each neighbor of current
if neighbor in closedSet
continue// Ignore the neighbor which is already evaluated.// The distance from start to a neighbor
tentative_gScore := gScore[current]+dist_between(current, neighbor)if neighbor not in openSet // Discover a new node
openSet.Add(neighbor)elseif tentative_gScore >= gScore[neighbor]continue// This is not a better path.// This path is the best until now. Record it!
cameFrom[neighbor]:= current
gScore[neighbor]:= tentative_gScore
fScore[neighbor]:= gScore[neighbor]+heuristic_cost_estimate(neighbor, goal)return failure
function reconstruct_path(cameFrom, current)
total_path :=[current]while current in cameFrom.Keys:
current := cameFrom[current]
total_path.append(current)return total_path
import numpy
from heapq import heappush,heappop
defheuristic_cost_estimate(neighbor, goal):
x = neighbor[0]- goal[0]
y = neighbor[1]- goal[1]returnabs(x)+abs(y)defdist_between(a, b):return(b[0]- a[0])**2+(b[1]- a[1])**2defreconstruct_path(came_from, current):
path =[current]while current in came_from:
current = came_from[current]
path.append(current)return path
# astar function returns a list of points (shortest path)defastar(array, start, goal):
directions =[(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)]# 8个方向
close_set =set()
came_from ={}
gscore ={start:0}
fscore ={start:heuristic_cost_estimate(start, goal)}
openSet =[]
heappush(openSet,(fscore[start], start))# 往堆中插入一条新的值 # while openSet is not emptywhile openSet:# current := the node in openSet having the lowest fScore value
current = heappop(openSet)[1]# 从堆中弹出fscore最小的节点 if current == goal:return reconstruct_path(came_from, current)
close_set.add(current)for i, j in directions:# 对当前节点的 8 个相邻节点一一进行检查
neighbor = current[0]+ i, current[1]+ j
## 判断节点是否在地图范围内,并判断是否为障碍物if0<= neighbor[0]< array.shape[0]:if0<= neighbor[1]< array.shape[1]:if array[neighbor[0]][neighbor[1]]==1:# 1为障碍物continueelse:# array bound y wallscontinueelse:# array bound x wallscontinue# Ignore the neighbor which is already evaluated.if neighbor in close_set:continue# The distance from start to a neighbor via current
tentative_gScore = gscore[current]+ dist_between(current, neighbor)if neighbor notin[i[1]for i in openSet]:# Discover a new node
heappush(openSet,(fscore.get(neighbor, numpy.inf), neighbor))elif tentative_gScore >= gscore.get(neighbor, numpy.inf):# This is not a better path.continue# This path is the best until now. Record it!
came_from[neighbor]= current
gscore[neighbor]= tentative_gScore
fscore[neighbor]= tentative_gScore + heuristic_cost_estimate(neighbor, goal)returnFalseif __name__ =="__main__":
nmap = numpy.array([[0,0,0,0,0,0,0,0,0,0,0,0,0,0],[1,1,1,1,1,1,1,1,1,1,1,1,0,1],[0,0,0,0,0,0,0,0,0,0,0,0,0,0],[1,0,1,1,1,1,1,1,1,1,1,1,1,1],[0,0,0,0,0,0,0,0,0,0,0,0,0,0],[1,1,1,1,1,1,1,1,1,1,1,1,0,1],[0,0,0,0,0,0,0,0,0,0,0,0,0,0],[1,0,1,1,1,1,1,1,1,1,1,1,1,1],[0,0,0,0,0,0,0,0,0,0,0,0,0,0],[1,1,1,1,1,1,1,1,1,1,1,1,0,1],[0,0,0,0,0,0,0,0,0,0,0,0,0,0]])
path = astar(nmap,(0,0),(10,13))for i inrange(len(path)):
nmap[path[i]]=100
# -*- coding: utf-8 -*-import numpy as np
from heapq import heappush,heappop
defheuristic_cost_estimate(neighbor, goal):
x = neighbor[0]- goal[0]
y = neighbor[1]- goal[1]returnabs(x)+abs(y)defdist_between(a, b):return(b[0]- a[0])**2+(b[1]- a[1])**2defreconstruct_path(came_from, current):
path =[current]while current in came_from:
current = came_from[current]
path.append(current)return path
# astar function returns a list of points (shortest path)defastar(array, start, goal):
directions =[(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)]# 8个方向
close_set =set()
came_from ={}
gscore ={start:0}
fscore ={start:heuristic_cost_estimate(start, goal)}
openSet =[]
heappush(openSet,(fscore[start], start))# 往堆中插入一条新的值 # while openSet is not emptywhile openSet:# current := the node in openSet having the lowest fScore value
current = heappop(openSet)[1]# 从堆中弹出fscore最小的节点 if current == goal:return reconstruct_path(came_from, current)
close_set.add(current)for i, j in directions:# 对当前节点的 8 个相邻节点一一进行检查
neighbor = current[0]+ i, current[1]+ j
## 判断节点是否在地图范围内,并判断是否为障碍物if0<= neighbor[0]< array.shape[0]:if0<= neighbor[1]< array.shape[1]:if array[neighbor[0]][neighbor[1]]==0:# 0为障碍物continueelse:# array bound y wallscontinueelse:# array bound x wallscontinue# Ignore the neighbor which is already evaluated.if neighbor in close_set:continue# The distance from start to a neighbor via current
tentative_gScore = gscore[current]+ dist_between(current, neighbor)if neighbor notin[i[1]for i in openSet]:# Discover a new node
heappush(openSet,(fscore.get(neighbor, np.inf), neighbor))elif tentative_gScore >= gscore.get(neighbor, np.inf):# This is not a better path.continue# This path is the best until now. Record it!
came_from[neighbor]= current
gscore[neighbor]= tentative_gScore
fscore[neighbor]= tentative_gScore + heuristic_cost_estimate(neighbor, goal)returnFalseif __name__ =="__main__":from PIL import Image
import matplotlib.pyplot as plt
img = Image.open('C:\Users\Administrator\Desktop\map.bmp')map= np.array(img)# 图像转化为二维数组
path = astar(map,(0,0),(260,260))# 绘制路径
img = np.array(img.convert('RGB'))for i inrange(len(path)):
img[path[i]]=[0,0,255]
plt.imshow(img)
plt.axis('off')
plt.show()