A*算法（三）算法实现
作者：互联网
A*算法（三）算法实现
1. Array2D类
通用类Array2D用于描述地图的宽和高，并且存储地图的数据
class Array2D:
"""
1.构造方法需要两个参数，即二维数组的宽和高
2.成员变量w和h是二维数组的宽和高
3.使用：‘对象[x][y]’可以直接取到相应的值
4.数组的默认值都是0
"""
def __init__(self, w, h):
self.w = w # 地图的宽
self.h = h # 地图的高
self.data = [] # 地图的存储数据
self.data = [[0 for y in range(h)] for x in range(w)] # 数据初始化赋0
def __getitem__(self, item):
return self.data[item] # 设置获取所存储的数据
2. Point类
通用类Point用于描述地图结点的坐标
并且重载等号运算符，可以判断两个Point坐标是否相等
最后设置了打印时的格式
class Point:
"""
表示一个结点
"""
def __init__(self, x, y):
self.x = x # 结点的x坐标
self.y = y # 结点的y坐标
def __eq__(self, other): # 判断引用是否相等
if self.x == other.x and self.y == other.y:
return True
return False
def __str__(self): # 定义打印时的格式
return "x:" + str(self.x) + ",y:" + str(self.y)
3. AStar类
AStar算法，启发式函数默认曼哈顿距离
class AStar:
# 描述AStar算法中的结点数据
class Node:
def __init__(self, point, goalPoint, g=0, hef='MD'):
self.point = point # 自己的坐标
self.father = None # 父结点
self.g = g # g值，当前已产生的代价
self.D = 10 # D倍
# h值，未来可能产生的代价
if hef == 'DD': # 对角线距离
D2 = np.sqrt(2) * self.D
h_diagonal = min(abs(point.x - goalPoint.x), abs(point.y - goalPoint.y))
h_straight = (abs(point.x - goalPoint.x) + abs(point.y - goalPoint.y))
self.h = D2 * h_diagonal + self.D * (h_straight - 2 * h_diagonal)
elif hef == 'ED': # 欧几里得距离
self.h = np.sqrt(pow(point.x - goalPoint.x, 2) + pow(point.y - goalPoint.y, 2))
else: # 曼哈顿距离
self.h = (abs(point.x - goalPoint.x) + abs(point.y - goalPoint.y)) * self.D
def __init__(self, map2d, startPoint, goalPoint, passTag=0, hef='MD'):
# 启发式函数
if hef != 'MD' and hef != 'DD' and hef != 'ED':
hef = 'MD'
print("启发式函数输入有误，应为MD DD ED\n默认设置曼哈顿距离")
self.hef = hef
# 开启表，保存已产生而未访问的结点
self.openList = []
# 关闭表，保存已访问过的结点
self.closeList = []
# 寻路地图
self.map2d = map2d
# 起点终点
if isinstance(startPoint, Point) and isinstance(goalPoint, Point):
self.startPoint = startPoint
self.goalPoint = goalPoint
else:
self.startPoint = Point(*startPoint)
self.goalPoint = Point(*goalPoint)
# 可行走标记
self.passTag = passTag
def getMinNode(self):
currentNode = self.openList[0]
for node in self.openList:
if node.g + node.h < currentNode.g + currentNode.h:
currentNode = node
return currentNode
def pointInCloseList(self, point):
for node in self.closeList:
if node.point == point:
return True
return False
def pointInOpenList(self, point):
for node in self.openList:
if node.point == point:
return node
return None
def goalPointeInCloseList(self):
for node in self.closeList:
if node.point == self.goalPoint:
return node
return None
def searchNear(self, minF, offsetX, offsetY):
# 越界检测
if minF.point.x + offsetX < 0 or minF.point.x + offsetX > self.map2d.w - 1 or \
minF.point.y + offsetY < 0 or minF.point.y + offsetY > self.map2d.h - 1:
return
# 如果是障碍，就忽略
if self.map2d[minF.point.x + offsetX][minF.point.y + offsetY] != self.passTag:
return
# 如果在关闭表中，就忽略
currentPoint = Point(minF.point.x + offsetX, minF.point.y + offsetY)
if self.pointInCloseList(currentPoint):
return
# 设置单位代价
if offsetX == 0 or offsetY == 0:
step = 10
else:
step = 14
# 如果不再openList中，就把它加入openlist
currentNode = self.pointInOpenList(currentPoint)
if not currentNode:
currentNode = AStar.Node(currentPoint, self.goalPoint, g=minF.g + step, hef=self.hef)
currentNode.father = minF
self.openList.append(currentNode)
return
# 在openList中，判断minF到当前点的g值是否更小
if minF.g + step < currentNode.g: # 如果更小，就重新计算g值，并且改变father
currentNode.g = minF.g + step
currentNode.father = minF
def start(self):
# 判断起始点是否是障碍
if self.map2d[self.startPoint.x][self.startPoint.y] != self.passTag:
return None
# 判断目标点是否是障碍
if self.map2d[self.goalPoint.x][self.goalPoint.y] != self.passTag:
return None
# 1.将起点放入开启列表
startNode = AStar.Node(self.startPoint, self.goalPoint, hef=self.hef)
self.openList.append(startNode)
# 2.主循环逻辑
while True:
# 找到F值最小的点
minF = self.getMinNode()
# 把这个点加入closeList中，并且在openList中删除它
self.closeList.append(minF)
self.openList.remove(minF)
# 判断这个目标点的上下左右结点，默认不允许对角运动
self.searchNear(minF, 0, -1)
self.searchNear(minF, 0, 1)
self.searchNear(minF, -1, 0)
self.searchNear(minF, 1, 0)
# 若启发式函数非曼哈顿距离，允许对角运动
if self.hef != 'MD':
self.searchNear(minF, 1, 1)
self.searchNear(minF, 1, -1)
self.searchNear(minF, -1, 1)
self.searchNear(minF, -1, -1)
# 判断是否终止
point = self.goalPointeInCloseList()
if point: # 如果终点在关闭表中，就返回结果
cPoint = point
pathList = []
while True:
if cPoint.father:
pathList.append(cPoint.point)
cPoint = cPoint.father
else:
return list(reversed(pathList))
if len(self.openList) == 0:
return None
下面来详细了解代码：
3.1 Node类
首先创建一个类Node，包含自己的坐标point ，父结点father ，g值，h值
不同的启发式函数，对应不同的h值运算
默认MD：曼哈顿距离，DD：对角线距离，ED：欧几里得距离
class Node:
def __init__(self, point, goalPoint, g=0, hef='MD'):
self.point = point # 自己的坐标
self.father = None # 父结点
self.g = g # g值，当前已产生的代价
self.D = 10 # D倍
# h值，未来可能产生的代价
if hef == 'DD': # 对角线距离
D2 = np.sqrt(2) * self.D
h_diagonal = min(abs(point.x - goalPoint.x), abs(point.y - goalPoint.y))
h_straight = (abs(point.x - goalPoint.x) + abs(point.y - goalPoint.y))
self.h = D2 * h_diagonal + self.D * (h_straight - 2 * h_diagonal)
elif hef == 'ED': # 欧几里得距离
self.h = np.sqrt(pow(point.x - goalPoint.x, 2) + pow(point.y - goalPoint.y, 2))
else: # 曼哈顿距离
self.h = (abs(point.x - goalPoint.x) + abs(point.y - goalPoint.y)) * self.D
3.2 初始化处理
然后初始化处理
先确保正确的启动式函数hef
然后创建开启表openList ，保存已产生而未访问的结点
然后创建关闭表closeList ，保存已访问过的结点
初始化寻路地图map2d、起点startPoint、终点goalPoint、可行走标记passTag
def __init__(self, map2d, startPoint, goalPoint, passTag=0, hef='MD'):
# 启发式函数
if hef != 'MD' and hef != 'DD' and hef != 'ED':
hef = 'MD'
print("启发式函数输入有误，应为MD DD ED\n默认设置曼哈顿距离")
self.hef = hef
# 开启表，保存已产生而未访问的结点
self.openList = []
# 关闭表，保存已访问过的结点
self.closeList = []
# 寻路地图
self.map2d = map2d
# 起点终点
if isinstance(startPoint, Point) and isinstance(goalPoint, Point):
self.startPoint = startPoint
self.goalPoint = goalPoint
else:
self.startPoint = Point(*startPoint)
self.goalPoint = Point(*goalPoint)
# 可行走标记
self.passTag = passTag
定义函数来获得openlist中F值最小的结点
def getMinNode(self):
currentNode = self.openList[0]
for node in self.openList:
if node.g + node.h < currentNode.g + currentNode.h:
currentNode = node
return currentNode
3.3 判断函数
定义一些用于判断的函数，从字面上也比较容易理解
pointInCloseList ：判断结点是否在CloseList，也就是判断节点是否已经访问过
pointInOpenList ：判断结点是否在OpenList，也就是判断节点是否产生，且未访问过
goalPointeInCloseList：判断目标点是否在CloseList，也就是判断目标点是否已经访问过，如果存在则返回目标结点
def pointInCloseList(self, point):
for node in self.closeList:
if node.point == point:
return True
return False
def pointInOpenList(self, point):
for node in self.openList:
if node.point == point:
return node
return None
def goalPointeInCloseList(self):
for node in self.closeList:
if node.point == self.goalPoint:
return node
return None
3.4 搜索结点周围的点
创建函数用于搜索结点周围的点
def searchNear(self, minF, offsetX, offsetY):
# 越界检测
if minF.point.x + offsetX < 0 or minF.point.x + offsetX > self.map2d.w - 1 or \
minF.point.y + offsetY < 0 or minF.point.y + offsetY > self.map2d.h - 1:
return
# 如果是障碍，就忽略
if self.map2d[minF.point.x + offsetX][minF.point.y + offsetY] != self.passTag:
return
# 如果在关闭表中，就忽略
currentPoint = Point(minF.point.x + offsetX, minF.point.y + offsetY)
if self.pointInCloseList(currentPoint):
return
# 设置单位代价
if offsetX == 0 or offsetY == 0:
step = 10
else:
step = 14
# 如果不再openList中，就把它加入openlist
currentNode = self.pointInOpenList(currentPoint)
if not currentNode:
currentNode = AStar.Node(currentPoint, self.goalPoint, g=minF.g + step, hef=self.hef)
currentNode.father = minF
self.openList.append(currentNode)
return
# 在openList中，判断minF到当前点的g值是否更小
if minF.g + step < currentNode.g: # 如果更小，就重新计算g值，并且改变father
currentNode.g = minF.g + step
currentNode.father = minF
值得注意的是：
因为搜索有先后循序之分，所以可能存在同一批结点拓散搜索周围结点时
而周围结点的父结点可能不一定是距离最近的结点
如果该结点恰好为寻路路径，则可能会造成多余的路径存在
所以，如果周围的点已经存在openlist，需要判断minF到当前点的g值是否更小
如果更小，就重新计算g值，并且改变父结点father
这样就 消除了之前结点存在非最小g值的父结点隐患，从而避免 生成多余的路径
举个简单的例子：
7 逆时针顺序搜索到8、5、4，假如5的f值最小，逆时针顺序搜索到9、6、3、2、1
如果 5 方向道路存在障碍切换到 4 作为路径，逆时针顺序搜索到 2、1等
1、2就已经存在openlist中
重新搜索时 1 的父结点就需要改变成 4，因为 1 离 4 结点距离更短
而 2 仍保留 5 作为父结点，因为 2 离 5 结点距离更短
3.5 寻路
创建函数用于寻路
开始是判断起始点startPoint和目标点goalPoint是否合理
然后将起点startPoint放入开启列表openList
接着开始主循环逻辑：
1、在openlist中找到F值最小的点
2、把这个点加入closeList中，并且在openList中删除它
3、搜索的周围结点，默认4个，允许对角运动时，为8个
4、如果目标点在关闭表中，就中止循环返回结果，否则返回循环起点
def start(self):
# 判断起始点是否是障碍
if self.map2d[self.startPoint.x][self.startPoint.y] != self.passTag:
return None
# 判断目标点是否是障碍
if self.map2d[self.goalPoint.x][self.goalPoint.y] != self.passTag:
return None
# 1.将起点放入开启列表
startNode = AStar.Node(self.startPoint, self.goalPoint, hef=self.hef)
self.openList.append(startNode)
# 2.主循环逻辑
while True:
# 找到F值最小的点
minF = self.getMinNode()
# 把这个点加入closeList中，并且在openList中删除它
self.closeList.append(minF)
self.openList.remove(minF)
# 判断这个结点的上下左右结点，默认不允许对角运动
self.searchNear(minF, 0, -1)
self.searchNear(minF, 0, 1)
self.searchNear(minF, -1, 0)
self.searchNear(minF, 1, 0)
# 若启发式函数非曼哈顿距离，允许对角运动
if self.hef != 'MD':
self.searchNear(minF, 1, 1)
self.searchNear(minF, 1, -1)
self.searchNear(minF, -1, 1)
self.searchNear(minF, -1, -1)
# 判断是否终止
point = self.goalPointeInCloseList()
if point: # 如果目标点在关闭表中，就返回结果
cPoint = point
pathList = []
while True:
if cPoint.father:
pathList.append(cPoint.point)
cPoint = cPoint.father
else:
return list(reversed(pathList))
if len(self.openList) == 0:
return None
下面是简约的流程图：
4. 地图显示
用于显示在地图上A*算法计算的轨迹
def Display_map(map, start=None, goal=None, title=None):
plt.rcParams['font.sans-serif'] = ['SimHei'] # 设置正常显示中文
plt.xlim(- 1, map.w)
plt.ylim(- 1, map.h)
plt.xticks(np.arange(0, map.w, 1))
plt.yticks(np.arange(0, map.h, 1))
plt.grid(lw=2)
obstaclesX, obstaclesY = [], []
pathx, pathy = [], []
for x in range(map.w):
for y in range(map.h):
if map[x][y] == 1:
obstaclesX.append(x)
obstaclesY.append(y)
elif map[x][y] == 'o':
pathx.append(x)
pathy.append(y)
if obstaclesX != []:
plt.plot(obstaclesX, obstaclesY, 'xr', markersize=10, label='障碍')
if pathx != []:
plt.plot(pathx, pathy, 'og', markersize=10, label='路径')
if start != None:
plt.plot(start[0], start[1], 'or', markersize=10, label='起始')
if goal != None:
plt.plot(goal[0], goal[1], 'ob', markersize=10, label='目标')
if title != None:
plt.title(title) # 设置标题
plt.legend() # 设置图例
plt.show()
5. 计算测试
if __name__ == '__main__':
# 创建一个10*10的地图
mapw, maph = 10, 10
map2d = Array2D(mapw, maph)
# 设置障碍
obstacle = [[4, 9], [4, 8], [4, 7], [4, 6], [4, 5]]
for i in obstacle:
map2d[i[0]][i[1]] = 1
# 显示地图设置障碍后的样子
Display_map(map2d, title="设置障碍")
# 设置起点，终点
startx, starty = 0, 0
goalx, goaly = 9, 8
# 显示地图设置起点和终点后的样子
Display_map(map2d, [startx, starty], [goalx, goaly], title="规划准备")
# 创建AStar对象
aStar = AStar(map2d, Point(startx, starty), Point(goalx, goaly), hef='MD')
# 开始寻路
pathList = aStar.start()
# 遍历路径点,在map2d上以'o'表示
for point in pathList:
map2d[point.x][point.y] = 'o'
# 再次显示地图
Display_map(map2d, [startx, starty], [goalx, goaly], title="轨迹规划")
下面来详细了解代码：
5.1 创建地图
创建一个10*10的地图
mapw, maph = 10, 10
map2d = Array2D(mapw, maph)
5.2 设置障碍
在 [4, 9], [4, 8], [4, 7], [4, 6], [4, 5] 结点设置障碍
然后显示此时设置障碍后的地图
obstacle = [[4, 9], [4, 8], [4, 7], [4, 6], [4, 5]]
for i in obstacle:
map2d[i[0]][i[1]] = 1
Display_map(map2d, title="设置障碍")
5.3 设置起点和终点
设置起点（0, 0）和终点（9, 8）
然后显示此时设置起点和终点后的地图
startx, starty = 0, 0
goalx, goaly = 9, 8
Display_map(map2d, [startx, starty], [goalx, goaly], title="规划准备")
5.4 曼哈顿距离
启动式函数采用曼哈顿距离，开始寻路
# 创建AStar对象
aStar = AStar(map2d, Point(startx, starty), Point(goalx, goaly), hef='MD')
# 开始寻路
pathList = aStar.start()
# 遍历路径点,在map2d上以'o'表示
for point in pathList:
map2d[point.x][point.y] = 'o'
# 再次显示地图
Display_map(map2d, [startx, starty], [goalx, goaly], title="曼哈顿距离轨迹规划")
5.5 对角线距离
启动式函数采用对角线距离，开始寻路
# 创建AStar对象
aStar = AStar(map2d, Point(startx, starty), Point(goalx, goaly), hef='DD')
# 开始寻路
pathList = aStar.start()
# 遍历路径点,在map2d上以'o'表示
for point in pathList:
map2d[point.x][point.y] = 'o'
# 再次显示地图
Display_map(map2d, [startx, starty], [goalx, goaly], title="对角线距离轨迹规划")
5.6 欧几里得距离
启动式函数采用欧几里得距离，开始寻路
# 创建AStar对象
aStar = AStar(map2d, Point(startx, starty), Point(goalx, goaly), hef='ED')
# 开始寻路
pathList = aStar.start()
# 遍历路径点,在map2d上以'o'表示
for point in pathList:
map2d[point.x][point.y] = 'o'
# 再次显示地图
Display_map(map2d, [startx, starty], [goalx, goaly], title="欧几里得距离轨迹规划")
[1] python的代码地址:
https://github.com/JoveH-H/A-simple-explanation/blob/master/A%20star.py
[2] jupyter notebook的代码地址:
https://github.com/JoveH-H/A-simple-explanation/blob/master/ipynb/A%20star.ipynb
相关推荐：
A* 算法（二）启发式算法
A* 算法（一）算法导言
谢谢！
标签：结点,point,实现,self,minF,goalPoint,算法,map2d 来源： https://blog.csdn.net/qq_32618327/article/details/100112075