a星算法python路径规划

A*算法是一种常用于路径规划的算法，它利用启发式函数来预估从起点到终点的最短路径距离，并采用优先队列来实现节点的扩展和排序，从而得出最优路径。以下是一个简单的Python实现：

import heapq

def heuristic(node, goal):
    # 曼哈顿距离作为启发式函数
    return abs(node[0] - goal[0]) + abs(node[1] - goal[1])

def astar(start, goal, graph):
    # 优先队列的元素为 (f, g, node)
    queue = [(0, 0, start)]
    visited = set()
    while queue:
        _, cost, node = heapq.heappop(queue)
        if node in visited:
            continue
        visited.add(node)
        if node == goal:
            return cost
        for neighbor in graph[node]:
            if neighbor in visited:
                continue
            # 代价为走到邻居节点的距离加上从起点到邻居节点的曼哈顿距离
            f = cost + graph[node][neighbor] + heuristic(neighbor, goal)
            heapq.heappush(queue, (f, cost + graph[node][neighbor], neighbor))
    return -1 # 无法到达终点

# 示例：寻找从起点到终点的最短路径代价
start = (0, 0)
goal = (4, 4)
graph = {
    (0, 0): {(0, 1): 1, (1, 0): 1},
    (0, 1): {(0, 0): 1, (1, 1): 1},
    (1, 0): {(0, 0): 1, (1, 1): 1},
    (1, 1): {(0, 1): 1, (1, 0): 1, (2, 1): 1},
    (2, 1): {(1, 1): 1, (3, 1): 1},
    (3, 1): {(2, 1): 1, (3, 2): 1},
    (3, 2): {(3, 1): 1, (4, 2): 1},
    (4, 2): {(3, 2): 1, (4, 3): 1},
    (4, 3): {(4, 2): 1, (4, 4): 1},
    (4, 4): {(4, 3): 1}
}
print(astar(start, goal, graph)) # 输出：14