A*寻路算法 python

A*寻路算法是一种常用的路径规划算法，它可以在地图中找到最短路径。在Python中，我们可以使用以下步骤实现A*寻路算法：

1. 定义节点类，包括节点坐标、父节点、G值、H值和F值等属性。
2. 定义启发函数，计算当前节点到目标节点的估价值H。
3. 定义搜索函数，使用open列表和closed列表来存储待扩展的节点和已扩展的节点。
4. 在搜索函数中，首先将起点加入open列表，然后重复以下步骤：
- 从open列表中选取F值最小的节点作为当前节点。
- 如果当前节点是终点，则返回路径。
- 否则，将当前节点从open列表中删除，并将其加入closed列表。
- 对当前节点的相邻节点进行遍历，如果相邻节点不可通过或已在closed列表中，则跳过。
- 如果相邻节点不在open列表中，则将其加入open列表，并计算其G值、H值和F值，并将当前节点作为其父节点。
- 如果相邻节点已在open列表中，则比较当前路径是否更优，如果更优，则更新其父节点和G值。

下面是一个简单的A*寻路算法实现示例：

import heapq

class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.parent = None
        self.g = 0
        self.h = 0
        self.f = 0

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

def heuristic(node, goal):
    return abs(node.x - goal.x) + abs(node.y - goal.y)

def astar(start, goal, grid):
    open_list = []
    closed_list = []

    heapq.heappush(open_list, start)

    while open_list:
        current = heapq.heappop(open_list)

        if current == goal:
            path = []
            while current:
                path.append((current.x, current.y))
                current = current.parent
            return path[::-1]

        closed_list.append(current)

        for dx, dy in [(0, 1), (0, -1), (1, 0), (-1, 0)]:
            x, y = current.x + dx, current.y + dy
            if x < 0 or x >= len(grid) or y < 0 or y >= len(grid[0]) or grid[x][y] == 1:
                continue

            neighbor = Node(x, y)
            neighbor.g = current.g + 1
            neighbor.h = heuristic(neighbor, goal)
            neighbor.f = neighbor.g + neighbor.h
            neighbor.parent = current

            if neighbor in closed_list:
                continue

            if neighbor in open_list:
                index = open_list.index(neighbor)
                if neighbor.g < open_list[index].g:
                    open_list[index].g = neighbor.g
                    open_list[index].parent = current
            else:
                heapq.heappush(open_list, neighbor)

    return None

# 示例用法
grid = [[0, 0, 0, 0],
        [0, 1, 1, 0],
        [0, 0, 0, 0],
        [0, 0, 0, 0]]

start = Node(0, 0)
goal = Node(3, 3)

path = astar(start, goal, grid)
print(path)
# 输出：[(0, 0), (1, 0), (2, 0), (2, 1), (2, 2), (2, 3), (3, 3)]