脱离迷宫的利器——Astar算法与你携手寻路

2023-09-01 14:37:21

算法原理

Astar算法是一种启发式搜索算法，它使用启发函数来估计从当前节点到目标节点的距离。启发函数的选择对算法的性能有很大的影响，一个好的启发函数可以使算法更快地找到最短路径。

Astar算法的基本原理如下：

将起点加入到开放列表中，并将其距离设置为0。
从开放列表中选择距离最小的节点，并将其加入到封闭列表中。
将该节点的相邻节点加入到开放列表中，并计算它们的距离。
重复步骤2和步骤3，直到目标节点被加入到封闭列表中。
从封闭列表中反向追踪，即可得到从起点到目标节点的最短路径。

代码示例

下面是一个使用Astar算法寻找最短路径的代码示例：

import heapq

class Node:
    def __init__(self, x, y, g, h):
        self.x = x
        self.y = y
        self.g = g  # 从起点到该节点的距离
        self.h = h  # 从该节点到目标节点的估计距离
        self.f = self.g + self.h

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

class Astar:
    def __init__(self, grid, start, end):
        self.grid = grid
        self.start = start
        self.end = end

    def find_path(self):
        open_list = []
        closed_list = []

        # 将起点加入到开放列表中
        heapq.heappush(open_list, Node(self.start[0], self.start[1], 0, self.heuristic(self.start, self.end)))

        while open_list:
            # 从开放列表中选择距离最小的节点
            current_node = heapq.heappop(open_list)

            # 将该节点加入到封闭列表中
            closed_list.append(current_node)

            # 如果目标节点被找到，则返回最短路径
            if current_node.x == self.end[0] and current_node.y == self.end[1]:
                return self.reconstruct_path(current_node)

            # 将该节点的相邻节点加入到开放列表中
            for neighbor in self.get_neighbors(current_node):
                if neighbor in closed_list:
                    continue

                # 计算相邻节点的距离
                g = current_node.g + 1
                h = self.heuristic(neighbor, self.end)
                f = g + h

                # 如果相邻节点不在开放列表中，则将其加入到开放列表中
                if neighbor not in open_list:
                    heapq.heappush(open_list, Node(neighbor[0], neighbor[1], g, h))
                # 如果相邻节点已经在开放列表中，则更新其距离
                else:
                    for node in open_list:
                        if node.x == neighbor[0] and node.y == neighbor[1]:
                            if f < node.f:
                                node.g = g
                                node.h = h
                                node.f = f

        # 如果没有找到目标节点，则返回空列表
        return []

    def reconstruct_path(self, node):
        path = []
        while node:
            path.append((node.x, node.y))
            node = node.parent
        return path[::-1]

    def get_neighbors(self, node):
        neighbors = []
        # 向上移动
        if node.y > 0 and self.grid[node.y - 1][node.x] != 1:
            neighbors.append((node.x, node.y - 1))
        # 向下移动
        if node.y < len(self.grid) - 1 and self.grid[node.y + 1][node.x] != 1:
            neighbors.append((node.x, node.y + 1))
        # 向左移动
        if node.x > 0 and self.grid[node.y][node.x - 1] != 1:
            neighbors.append((node.x - 1, node.y))
        # 向右移动
        if node.x < len(self.grid[0]) - 1 and self.grid[node.y][node.x + 1] != 1:
            neighbors.append((node.x + 1, node.y))
        return neighbors

    def heuristic(self, node1, node2):
        # 曼哈顿距离
        return abs(node1[0] - node2[0]) + abs(node1[1] - node2[1])

if __name__ == "__main__":
    # 定义迷宫地图
    grid = [
        [0, 0, 0, 0, 0],
        [0, 1, 1, 0, 0],
        [0, 1, 0, 0, 0],
        [0, 1, 0, 1, 0],
        [0, 0, 0, 0, 0],
    ]

    # 定义起点和终点
    start = (0, 0)
    end = (4, 4)

    # 创建Astar对象
    astar = Astar(grid, start, end)

    # 寻找最短路径
    path = astar.find_path()

    # 打印最短路径
    print(path)