Python A星算法实现及路径可视化

A星算法是一种常用的路径规划算法，它结合了Dijkstra算法的优点和启发式搜索的思想，能够高效地找到最优路径。本文将介绍A星算法的Python实现，并结合示例程序演示如何在网格地图中进行路径规划和可视化。

A星算法代码

以下是一个适用于网格地图的A*路径算法示例代码：pythonimport heapqimport numpy as np

def a_star(start, goal, obstacles, grid): # 定义节点类 class Node: def init(self, position, parent=None, g=0, h=0): self.position = position self.parent = parent self.g = g # 从起始点到当前节点的实际代价 self.h = h # 从当前节点到目标点的估计代价 self.f = g + h # 总代价

    def __lt__(self, other):            return self.f < other.f

# 定义估计代价函数（这里使用曼哈顿距离）    def heuristic(position, goal):        return abs(position[0] - goal[0]) + abs(position[1] - goal[1])

# 定义移动操作    moves = [(0, 1), (0, -1), (1, 0), (-1, 0)]  # 上、下、左、右

# 初始化起始节点和目标节点    start_node = Node(start, None, 0, heuristic(start, goal))    goal_node = Node(goal)

# 初始化开放列表和关闭列表    open_list = []    closed_list = set()

# 将起始节点添加到开放列表    heapq.heappush(open_list, start_node)

# 开始A*算法    while open_list:        current_node = heapq.heappop(open_list)

    # 如果当前节点是目标节点，则返回路径        if current_node.position == goal_node.position:            path = []            while current_node is not None:                path.append(current_node.position)                current_node = current_node.parent            path.reverse()            return path

    # 将当前节点添加到关闭列表        closed_list.add(current_node.position)

    # 处理当前节点的邻居节点        for move in moves:            new_position = tuple(np.add(current_node.position, move))

        # 如果邻居节点在障碍物中或在关闭列表中，则跳过            if new_position in obstacles or new_position in closed_list:                continue

        # 计算邻居节点的代价            g = current_node.g + 1  # 假设每个移动操作的代价都是1            h = heuristic(new_position, goal_node.position)            f = g + h

        # 创建邻居节点            neighbor_node = Node(new_position, current_node, g, h)

        # 如果邻居节点已经在开放列表中并且有更小的f值，则跳过            if any(neighbor_node.position == node.position and neighbor_node.f >= node.f for node in open_list):                continue

        # 将邻居节点添加到开放列表            heapq.heappush(open_list, neighbor_node)

        # 标注路径点为红色            grid[new_position[0]][new_position[1]] = 'R'

# 如果无法找到路径，则返回空列表    return []

示例程序def main(): # 创建示例网格 grid = [['' for _ in range(10)] for _ in range(10)] obstacles = {(2, 2), (3, 2), (4, 2), (5, 2), (6, 2), (6, 3), (6, 4), (5, 4)} start = (1, 1) goal = (8, 8)

# 执行A*路径规划    path = a_star(start, goal, obstacles, grid)

# 打印网格，标注路径点    for i, row in enumerate(grid):        for j, value in enumerate(row):            if (i, j) in path:                grid[i][j] = '�33[31m' + value + '�33[0m'  # 使用终端控制字符设置红色文本            print(value, end=' ')        print()

if name == 'main': main()

代码说明

a_star(start, goal, obstacles, grid) 函数: * 接受起始点、目标点、障碍物列表和网格地图作为输入。 * 使用 Node 类表示节点，包含位置、父节点、代价等信息。 * 使用 heuristic 函数计算估计代价，这里使用曼哈顿距离。 * 使用 moves 列表定义移动操作，表示上下左右四个方向。 * 使用 open_list 和 closed_list 分别存储待探索节点和已探索节点。 * 通过循环遍历 open_list，直到找到目标节点或 open_list 为空。 * 在循环中，每次取出 open_list 中代价最小的节点进行扩展，并将邻居节点加入 open_list。 * 最后，如果找到目标节点，则通过回溯父节点的方式构建路径并返回。
main() 函数: * 创建示例网格地图、障碍物、起始点和目标点。 * 调用 a_star() 函数进行路径规划。 * 使用终端控制字符将路径点标记为红色并打印网格地图。

总结

本文介绍了A星算法的Python实现，并结合示例程序演示了如何在网格地图中进行路径规划和可视化。A星算法是一种高效的路径规划算法，可以应用于各种场景，例如游戏开发、机器人导航等。