【算法 python A*算法的实现】

 - 算法实现：

import heapqclass Node:def __init__(self, position, g=0, h=0):self.position = position  # 节点的位置self.g = g  # 从起点到当前节点的成本self.h = h  # 从当前节点到终点的启发式估计成本self.f = g + h  # 总成本self.parent = None  # 父节点def __lt__(self, other):return self.f < other.fdef heuristic(a, b):# 使用曼哈顿距离作为启发式函数return abs(a[0] - b[0]) + abs(a[1] - b[1])def a_star(start, goal, grid):open_list = []closed_list = set()start_node = Node(start, 0, heuristic(start, goal))goal_node = Node(goal)heapq.heappush(open_list, start_node)while open_list:current_node = heapq.heappop(open_list)if current_node.position == goal_node.position:path = []while current_node:path.append(current_node.position)current_node = current_node.parentreturn path[::-1]  # 返回反转后的路径closed_list.add(current_node.position)# 获取相邻节点neighbors = [(0, 1), (1, 0), (0, -1), (-1, 0)]  # 右，下，左，上for new_position in neighbors:node_position = (current_node.position[0] + new_position[0],current_node.position[1] + new_position[1],)# 检查节点是否在网格内if (0 <= node_position[0] < len(grid)) and (0 <= node_position[1] < len(grid[0])):# 检查节点是否是障碍物if grid[node_position[0]][node_position[1]] == 1:continue# 创建新的节点neighbor_node = Node(node_position)if neighbor_node.position in closed_list:continue# 计算 g, h, f 值neighbor_node.g = current_node.g + 1neighbor_node.h = heuristic(neighbor_node.position, goal_node.position)neighbor_node.f = neighbor_node.g + neighbor_node.hneighbor_node.parent = current_node# 检查节点是否在开放列表中if add_to_open(open_list, neighbor_node):heapq.heappush(open_list, neighbor_node)return None  # 如果没有找到路径def add_to_open(open_list, neighbor):for node in open_list:if neighbor.position == node.position and neighbor.g >= node.g:return Falsereturn True

 - 测试：

# 示例使用
import numpy as npgrid = np.array([[0, 0, 0, 0, 0],[0, 1, 1, 1, 0],[1, 0, 0, 0, 0],[0, 0, 1, 1, 1],[0, 0, 0, 0, 0],]
)start = (0, 0)  # 起点
goal = (4, 4)  # 终点path = a_star(start, goal, grid)
print("找到的路径:", path)

找到的路径: [(0, 0), (0, 1), (0, 2), (0, 3), (0, 4), (1, 4), (2, 4), (2, 3), (2, 2), (2, 1), (3, 1), (4, 1), (4, 2), (4, 3), (4, 4)]

 - 可视化

import matplotlib.pyplot as pltdef visualize_path(maze, path):plt.title("map")mask = maze==1maze[mask] = 0maze[~mask] = 1plt.imshow(maze, cmap="gray")plt.xticks([]), plt.yticks([])  # 去掉坐标轴plt.show()for position in path:maze[position[0]][position[1]] = 2  # 用2表示路径plt.title("shortest path")plt.imshow(maze, cmap="gray")plt.xticks([]), plt.yticks([])  # 去掉坐标轴plt.show()