- 算法实现：

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 np
grid = np.array([[0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],[0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 0],[0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 1],[0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0],[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],[1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0],[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1],[0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1],[0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0],[0, 1, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1],[0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1],[0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1],[0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1],[0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 0],[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0],[0, 1, 1, 0, 1, 0, 1, 1, 0, 1, 0, 1, 0, 0, 1, 0],]
)start = (0, 0)  # 起点
goal = (15, 15)  # 终点path = a_star(start, goal, grid)
print("找到的路径:", path)

找到的路径: [(0, 0), (1, 0), (2, 0), (3, 0), (4, 0), (4, 1), (4, 2), (3, 2), (2, 2), (1, 2), (0, 2), (0, 3), (0, 4), (1, 4), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8), (1, 8), (0, 8), (0, 9), (0, 10), (0, 11), (0, 12), (0, 13), (0, 14), (1, 14), (2, 14), (3, 14), (4, 14), (4, 13), (4, 12), (4, 11), (4, 10), (4, 9), (4, 8), (4, 7), (4, 6), (5, 6), (6, 6), (6, 5), (6, 4), (7, 4), (8, 4), (8, 3), (8, 2), (9, 2), (10, 2), (11, 2), (12, 2), (12, 3), (12, 4), (12, 5), (12, 6), (11, 6), (10, 6), (9, 6), (8, 6), (8, 7), (8, 8), (7, 8), (6, 8), (6, 9), (6, 10), (6, 11), (6, 12), (7, 12), (8, 12), (8, 11), (8, 10), (9, 10), (10, 10), (10, 9), (10, 8), (11, 8), (12, 8), (12, 9), (12, 10), (13, 10), (14, 10), (14, 11), (14, 12), (13, 12), (12, 12), (11, 12), (10, 12), (10, 13), (10, 14), (11, 14), (12, 14), (13, 14), (13, 15), (14, 15), (15, 15)]

 - 可视化

def visualize_path(maze, path):fig, axis = plt.subplots(1, 2)mask = maze == 1maze[mask] = 0maze[~mask] = 1axis[0].imshow(maze, cmap="gray")axis[0].set_xticks([])axis[0].set_yticks([])axis[0].set_title("map")for position in path:maze[position[0]][position[1]] = 2axis[1].imshow(maze, cmap="gray")axis[1].set_xticks([])axis[1].set_yticks([])axis[1].set_title("shortest path")plt.show()visualize_path(grid.copy(), path)