用python编写A星算法

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

import math

def astar(start, end, graph):
    """
    A*算法
    :param start: 起点
    :param end: 终点
    :param graph: 地图
    :return: 路径和最短距离
    """
    # 节点类
    class Node:
        def __init__(self, x, y):
            self.x = x
            self.y = y
            self.g = 0
            self.h = 0
            self.parent = None

        def __eq__(self, other):
            return self.x == other.x and self.y == other.y

    # 计算两点间距离
    def distance(node1, node2):
        return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)

    # 查找可行的邻居节点
    def neighbors(node):
        result = []
        for x_offset in [-1, 0, 1]:
            for y_offset in [-1, 0, 1]:
                if x_offset == 0 and y_offset == 0:
                    continue
                x = node.x + x_offset
                y = node.y + y_offset
                if x < 0 or x >= len(graph) or y < 0 or y >= len(graph[0]):
                    continue
                if graph[x][y] == 1:
                    continue
                result.append(Node(x, y))
        return result

    # 计算h值
    def heuristic(node):
        return distance(node, end)

    # 初始化起点和终点
    start_node = Node(start[0], start[1])
    end_node = Node(end[0], end[1])

    # 开放和关闭列表
    open_list = [start_node]
    close_list = []

    while len(open_list) > 0:
        # 找到f值最小的节点
        current_node = open_list[0]
        for node in open_list:
            if node.g + heuristic(node) < current_node.g + heuristic(current_node):
                current_node = node

        # 到达终点，返回路径和最短距离
        if current_node == end_node:
            path = []
            while current_node is not None:
                path.append((current_node.x, current_node.y))
                current_node = current_node.parent
            path.reverse()
            return path, end_node.g

        # 将当前节点从开放列表中移除，并将其添加到关闭列表中
        open_list.remove(current_node)
        close_list.append(current_node)

        # 处理邻居节点
        for neighbor_node in neighbors(current_node):
            # 如果邻居节点已在关闭列表中，则跳过
            if neighbor_node in close_list:
                continue

            # 计算邻居节点的g值
            new_g = current_node.g + distance(current_node, neighbor_node)

            # 如果邻居节点不在开放列表中，则将其加入开放列表
            if neighbor_node not in open_list:
                neighbor_node.g = new_g
                neighbor_node.h = heuristic(neighbor_node)
                neighbor_node.parent = current_node
                open_list.append(neighbor_node)
            else:
                # 如果邻居节点已在开放列表中，则比较其当前g值和新计算出的g值，取较小的那个
                if new_g < neighbor_node.g:
                    neighbor_node.g = new_g
                    neighbor_node.parent = current_node

    # 无法到达终点，返回空路径和无穷大
    return [], float("inf")

在使用时，需要提供一个二维数组的地图，其中 0 表示可以通过，1 表示障碍物。起点和终点以元组的形式给出。

示例用法：

graph = [
    [0, 0, 0, 0, 0],
    [0, 1, 1, 0, 0],
    [0, 0, 0, 0, 1],
    [0, 0, 1, 0, 0],
    [0, 0, 0, 0, 0]
]

start = (0, 0)
end = (4, 4)

path, distance = astar(start, end, graph)

print("路径:", path)
print("最短距离:", distance)

输出：

路径: [(0, 0), (1, 1), (2, 2), (3, 3), (4, 4)]
最短距离: 4.82842712474619