1 准备工作
1.1 地图
\[\begin{bmatrix} 0 & 0 & 0 & 0 & 0 \\ 1 & 0 & 1 & 0 & 1 \\ 0 & 0 & 1 & 1 & 1 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \end{bmatrix} \]其中0表示路，1表示障碍，起点在(0,0)，终点在(4,4)
那么得到的路径应该是：
\[\begin{bmatrix} \textbf{A} & \textbf{B} & 0 & 0 & 0 \\ 1 & \textbf{C} & 1 & 0 & 1 \\ \textbf{E} & \textbf{D} & 1 & 1 & 1 \\ \textbf{F} & 1 & \textbf{J} & \textbf{K} & \textbf{L} \\ \textbf{G} & \textbf{H} & \textbf{I} & 1 & \textbf{M} \end{bmatrix} \]路径是：
(0, 0) -> (0, 1) -> (1, 1) -> (2, 1) -> (2, 0) -> (3, 0) ->
(4, 0) -> (4, 1) -> (4, 2) -> (3, 2) -> (3, 3) -> (3, 4) -> (4, 4)
1.2 一点代码
1.2.1 优先队列
因为A*算法每次需要找到代价最低的节点，然后进行探测。所以这里使用最小堆来维护Open表
import heapq
class PriorityQueue:
def __init__(self):
self._queue = []
self._index = 0
def push(self, item, priority):
"""
队列由 (priority, index, item) 形式组成
index 是为了当两个对象的优先级一致时，按照插入顺序排列
"""
heapq.heappush(self._queue, (priority, self._index, item))
self._index += 1
def pop(self):
return heapq.heappop(self._queue)[-1]
def empty(self):
return True if not self._queue else False
def __len__(self):
return self.qsize()
1.2.2 地图类
class GameMap:
def __init__(self):
self.game_map = None
self.rows = 0
self.cols = 0
def static_map(self):
self.game_map = [
[0, 0, 0, 0, 0],
[1, 0, 1, 0, 1],
[0, 0, 1, 1, 1],
[0, 1, 0, 0, 0],
[0, 0, 0, 1, 0]
]
self.rows = self.cols = 5
def is_obstacles(self, row: int, col: int) -> bool:
# 判断该点是否是障碍物
return self.game_map[row][col] == 1
1.2.3 节点类
该节点类的对象会保存到Open表中。关于如何保存最佳路径，这里采用的方式是让节点记住它的父节点。比如，当探测到终点后，我们从重点开始向前递归，不断递归父节点father，从而还原出整条路径。
from typing import Tuple
class Node:
def __init__(self, point: Tuple[int, int], g, h, father=None):
self.point = point # 坐标
self.father = father # 父节点, 用于生成路径使用
self.g = g # 实际代价
self.h = h # 未来代价
self.f = g + h # 总代价
def __eq__(self, __o: object) -> bool:
return __o.point == self.point
2 AStar
这里提供曼哈顿距离和欧式距离作为启发式函数
from typing import Tuple
import time
import math
from collections import defaultdict
class AStar:
def __init__(self, game_map: GameMap, start: Tuple[int, int], end: Tuple[int, int]):
# 进行一些检查, 起始点和结束位置不能是障碍物
assert len(start) == len(end) == 2
assert not game_map.is_obstacles(*start)
assert not game_map.is_obstacles(*end)
self.game_map = game_map
self.start = start
self.end = end
self.open_table = PriorityQueue()
self.close_table = defaultdict(set)
# 定义的行进方向
self.direction = ((-1, 0), (1, 0), (0, -1), (0, 1))
# 存储最短路劲
self.path = []
# 设置启发式函数
self.h_method = {
0: AStar.manhattan,
1: AStar.euclidean
}
# 记录一些状态
self.record = {
'open_size': [],
'close_size': []
}
@staticmethod
def manhattan(start: Tuple[int, int], end: Tuple[int, int]):
return abs(start[0] - end[0]) + abs(start[1] - end[1])
@staticmethod
def euclidean(start: Tuple[int, int], end: Tuple[int, int]):
dx = start[0] - end[0]
dy = start[1] - end[1]
return math.sqrt(dx ** 2 + dy ** 2)
def find_path(self, h_method_no: int = 0) -> None:
# 初始化
t1 = time.perf_counter()
hm = self.h_method
self.path = []
node = Node(self.start, 0, hm[h_method_no](self.start, self.end))
self.open_table.push(node, node.f)
flag = True
target = None
while flag:
# 从open表中获取代价最小的点
chosen_node: Node = self.open_table.pop()
cx, cy = chosen_node.point
# 以该节点为基准, 探测周围四步
for dx, dy in self.direction:
fx, fy = cx + dx, cy + dy
# 没有超出地图边界
if 0 <= fx < self.game_map.rows and 0 <= fy < self.game_map.cols:
# 判断探测到的位置是否在close表中
if fy in self.close_table[fx]:
continue
# 判断是否是障碍物
if self.game_map.is_obstacles(fx, fy):
continue
# 创建新节点, 并加入到open中
new_node = Node(point=(fx, fy),
g=chosen_node.g + 1,
h=hm[h_method_no](self.start, self.end),
father=chosen_node)
# 如果探测到终点
if (fx, fy) == self.end:
target = new_node
flag = False
break
self.open_table.push(new_node, new_node.f)
# 当前节点关闭
self.close_table[cx].add(cy)
# 记录
self.record['open_size'].append(len(self.open_table))
self.record['close_size'].append(sum(len(v) for v in self.close_table.values()))
if self.open_table.empty():
flag = False
t2 = time.perf_counter()
self.record['time'] = f'{t2 - t1}'
if target is None:
print(f'No available path from start:{self.start} to end: {self.end}')
else:
self.get_path(target)
self.path.reverse()
def get_path(self, node: Node) -> None:
if node is None:
return
self.path.append(node.point)
self.get_path(node.father)
3 测试
if __name__ == '__main__':
# 创建地图
game_map = GameMap()
game_map.static_map()
start, end = (0, 0), (4, 4)
# 创建算法对象
astar = AStar(game_map, start, end)
# 这里测试曼哈顿距离作为启发式函数
astar.find_path(0)
print(astar.path)
print(max(astar.record['time']))
print(max(astar.record['open_size']))
print(max(astar.record['close_size']))
测试结果得到路径与前文一致。
标签：__,end,int,self,路径,迷宫,start,查找,def From： https://www.cnblogs.com/silverbeats/p/16867602.html