用python实现a*算法

以下是使用Python实现A*算法的示例代码：

import heapq

def astar(start, goal, graph):
    """
    :param start: 起点
    :param goal: 终点
    :param graph: 地图上的节点和边
    :return: 找到的路径和路径长度
    """
    # open_list中的每个元素都是一个元组，包括f值，g值和节点坐标
    open_list = [(0, 0, start)]
    # closed_list中存储已经遍历过的节点
    closed_list = set()
    # g值存储从起点到目前节点的距离
    g = {start: 0}
    # f值存储从起点到终点通过当前节点的总距离
    f = {start: heuristic(start, goal)}

    while len(open_list) > 0:
        # 从open_list中找到f值最小的节点
        current = heapq.heappop(open_list)[2]
        # 如果当前节点是终点，则返回路径和路径长度
        if current == goal:
            path = [current]
            while current in graph:
                current = graph[current]
                path.append(current)
            path.reverse()
            return path, f[goal]
        # 将当前节点加入closed_list
        closed_list.add(current)
        # 遍历当前节点的所有邻居节点
        for neighbor in graph[current]:
            # 如果邻居节点已经在closed_list中，则跳过
            if neighbor in closed_list:
                continue
            # 计算从起点到邻居节点的距离
            tentative_g = g[current] + heuristic(current, neighbor)
            # 如果邻居节点不在open_list中，则加入open_list
            if neighbor not in [i[2] for i in open_list]:
                heapq.heappush(open_list, (tentative_g + heuristic(neighbor, goal), tentative_g, neighbor))
            # 如果邻居节点已经在open_list中，则更新其g值和f值
            elif tentative_g < g[neighbor]:
                open_list.remove((f[neighbor], g[neighbor], neighbor))
                heapq.heappush(open_list, (tentative_g + heuristic(neighbor, goal), tentative_g, neighbor))
            # 更新邻居节点的g值和f值
            g[neighbor] = tentative_g
            f[neighbor] = tentative_g + heuristic(neighbor, goal)

    # 如果找不到路径，则返回None
    return None, None

def heuristic(node1, node2):
    """
    :param node1: 节点1的坐标
    :param node2: 节点2的坐标
    :return: 两个节点之间的距离
    """
    return abs(node1[0] - node2[0]) + abs(node1[1] - node2[1])

if __name__ == '__main__':
    graph = {
        (0, 0): [(0, 1), (1, 0)],
        (0, 1): [(0, 0), (0, 2), (1, 1)],
        (0, 2): [(0, 1), (1, 2)],
        (1, 0): [(0, 0), (2, 0), (1, 1)],
        (1, 1): [(1, 0), (0, 1), (2, 1), (1, 2)],
        (1, 2): [(1, 1), (0, 2), (2, 2)],
        (2, 0): [(1, 0), (2, 1)],
        (2, 1): [(2, 0), (1, 1), (2, 2)],
        (2, 2): [(2, 1), (1, 2)]
    }

    start = (0, 0)
    goal = (2, 2)

    path, length = astar(start, goal, graph)
    print("Path:", path)
    print("Length:", length)

在这个示例中，我们从起点(0,0)到终点(2,2)的路径通过A*算法进行搜索。我们的地图是一个3x3的网格，其中每个节点都有一些相邻节点。我们使用字典来表示节点和它的相邻节点。在实现中，我们使用了Python中的堆(heapq)数据结构来帮助我们查找f值最小的节点。最后，我们返回找到的路径和路径长度。