本文以这篇博主的文章为基础【精选】A*算法（超级详细讲解，附有举例的详细手写步骤）-CSDN博客
这篇文章的博主做了一个UI界面，但我感觉，这样对新手关注算法和代码本身反而不利，会被界面的代码所干扰。所以笔者在他的基础上舍去了界面的内容，只关注算法本身。
A星算法的作用就是：已知起点和终点坐标，将地图离散化为网格，可以使用A star算法寻路。
A star算法简单来说三个步骤：一是准备两个列表，开列表和闭列表，开列表将节点移进移出，闭列表只进不出。二是在每一步探索的时候，计算三个“花费”，起点走到当前点的实际花费 G，从当前点到目标点的预估花费 H， 总花费F = G + H。三是计算父节点，父节点的作用是推算花费以及到达终点后倒推路径。
具体来说，我们称现在所处的网格为 checking point k，检查checking point k周围所有的下一步可到达点，并『临时』将这些可到达点的父节点记为checking point k。可到达点是没有障碍物且不在闭列表中的网格（near point 1, near point 2, ......, near point i, ......, near point n），对于near point i，计算起点到near point i的实际花费：
起点到near point i的实际花费 = 起点到checking point k的实际花费 + checking point k到near point i的实际花费。
计算near point i到终点的预估花费：
near point i到终点的预估花费 = near point i到终点的曼哈顿距离。
near point i的总花费 = 实际花费 + 预估花费。这里只是“花费“的一种定义形式，也可以用其他的定义形式。
每个点都是我们建立的点类的类实例，在点类中储存这三个花费，以及储存父节点。
如果near point i不在开列表中，将其加进去。注意我们前面『临时』标记的父节点，这里正式标记checking point k为父节点。
如果near point i已经在开列表中，则说明，near point i在我们到达checking point k之前，处于另一个checking point j时，作为checking point j的可到达点被计算过了。这里我们比较一下near point i旧的总花费和新的总花费，如果新的总花费小，则更新花费，并把near point i的父节点更新为checking point k；如果旧的总花费小，则保持旧的花费，以及保持旧的父节点。
当checking point k的所有可到达点都被检查完后，将其移进闭列表。
之后，从开列表中选一个总花费最小的点作为新的checking point，重复上述的检查可达点操作，直到找到终点，起始时，将起点加入开列表，如果在途中开列表空了，则不存在可达路径。
完整代码如下：
import numpy as np
import json
import matplotlib.pyplot as plt
class Map:
def __init__(self):
# 设置起点，终点所在的行列数，左上角为0，0
start_row = 17
start_col = 2
end_row = 9
end_col = 16
with open('map.txt', 'r') as f:
my_map = json.loads(f.read())
my_map = np.array(my_map)
self.map = np.where(my_map == 0, 1, np.where(my_map == 1, 0, my_map))
self.map[start_row, start_col] = -100 # 起点
self.map[end_row, end_col] = 100 # 终点
# self.map = np.array([[1, 1, 1, 1, 0, 100],
# [1, 0, 0, 1, 0, 1],
# [1, -100, 0, 1, 0, 1],
# [1, 1, 0, 1, 0, 1],
# [1, 1, 1, 1, 1, 1]])
def get_start_point(self):
indices = np.where(self.map == -100)
return indices[0][0], indices[1][0]
def get_end_point(self):
indices = np.where(self.map == 100)
return indices[0][0], indices[1][0]
def check_grid(self, point):
return self.map[point.x, point.y]
class Point:
def __init__(self, x_, y_):
self.x = x_
self.y = y_
self.father = None
self.G = 0 # 起点到当前节点所花费的消耗
self.H = 0 # 到终点的预估消耗
self.F = 0
def get_x_y(self):
return self.x, self.y
def set_GHF(self, G, H, F):
self.H = H
self.G = G
self.F = F
class Astar:
def __init__(self):
self.openlist = []
self.closelist = []
self.map = Map()
self.start_x, self.start_y = self.map.get_start_point()
self.start_position = None
self.end_x, self.end_y = self.map.get_end_point()
self.find_path = False
self.path = []
def cal_GHF(self, checkpoint):
if checkpoint.father is not None:
G = checkpoint.father.G + 1 # 起点到父节点的花费加上父节点到本节点的花费
else:
G = 0
H = abs(checkpoint.x - self.end_x) + abs(checkpoint.y - self.end_y)
F = G + H
return G, H, F
def add_near_point(self, check_point):
x, y = check_point.get_x_y()
tmp_list = [Point(x-1, y-1), Point(x-1, y), Point(x-1, y+1),
Point(x, y-1), Point(x, y+1),
Point(x+1, y-1), Point(x+1, y), Point(x+1, y+1)]
near_list = []
for pi in tmp_list:
if self.map.map.shape[0] > pi.x >= 0 and self.map.map.shape[1] > pi.y >= 0: # 在地图范围内
if self.map.check_grid(pi) == 100:
return [pi]
elif self.map.check_grid(pi) == 1 and self.not_in_closelist(pi):
near_list.append(pi)
return near_list
def choose_min_F_point(self):
minF = 1e10
choosed_point = None
for pi in self.openlist:
if pi.F < minF:
minF = pi.F
choosed_point = pi
return choosed_point
def not_in_openlist(self, pi):
not_in = True
for pii in self.openlist:
if pii.x == pi.x and pii.y == pi.y:
not_in = False
return not_in
def not_in_closelist(self, pi):
not_in = True
for pii in self.closelist:
if pii.x == pi.x and pii.y == pi.y:
not_in = False
return not_in
def run(self):
self.start_position = Point(self.start_x, self.start_y)
G, H, F = self.cal_GHF(self.start_position)
self.start_position.set_GHF(G, H, F)
self.openlist.append(self.start_position)
while True:
checking_point = self.choose_min_F_point()
if checking_point is None or self.find_path:
print("End!")
break
self.openlist.remove(checking_point)
self.closelist.append(checking_point)
near_list = self.add_near_point(checking_point)
for pi in near_list:
if self.map.check_grid(pi) == 100:
self.find_path = True
# print("find path:\n{}".format(checking_point.get_x_y()))
self.path.append([checking_point.get_x_y()[0], checking_point.get_x_y()[1]])
reverse_point_father = checking_point.father
while reverse_point_father.father is not None:
# print(reverse_point_father.get_x_y())
self.path.append([reverse_point_father.get_x_y()[0], reverse_point_father.get_x_y()[1]])
reverse_point_father = reverse_point_father.father
break
if self.not_in_openlist(pi):
pi.father = checking_point
G, H, F = self.cal_GHF(pi)
pi.set_GHF(G, H, F)
self.openlist.append(pi)
else:
G_old = pi.G + 1
G_new = checking_point.G + 1
if G_new < G_old:
pi.father = checking_point
G, H, F = self.cal_GHF(pi)
pi.set_GHF(G, H, F)
# 打印路性
print("path: ")
print(self.path)
self.path = self.path[::-1]
with open('my_path.txt', 'w') as file:
for item in self.path:
file.write(str(item) + '\n')
def check(self):
for pi in self.path:
self.map.map[pi[0], pi[1]] = 2
fig = plt.figure("A Star Algorithm")
cmap = plt.cm.colors.ListedColormap(['yellow', 'black', 'white', 'blue', 'green'])
# 创建一个离散的归一化器，根据不同数值映射到不同颜色
bounds = [-101, -99, 0, 1, 2, 99, 101]
norm = plt.cm.colors.BoundaryNorm(bounds, cmap.N)
# 显示二维数组
plt.imshow(self.map.map, cmap=cmap, norm=norm)
# 添加颜色条，以便查看数值与颜色的对应关系
cb = plt.colorbar()
# 显示图
plt.show()
if __name__ == "__main__":
astar = Astar()
astar.run()
astar.check()
使用我的代码前，可以先运行文章开头提到的那个博主的代码，生成一个地图保存，我的代码加载他的地图（也可以使用我注释掉的那个地图），我的地图中，1表示可行网格，0表示障碍物网格。如果我们足够幸运的话，我们两篇文章的结果应该是一致的。