A*

一 简介

与 Dijkstra 不同的是，A*算法是source 到 target 的最短路径，Dijkstra 是source 到所有点的最短路径。A*算法的核心是评估函数 f(n) = g(n) + h(n)。

• g(n) 从起点到节点 n 的实际代价
• h(n) 从节点 n 到终点的估计代价（启发函数）
• f(n) 经过节点 n 到终点的估计总代价

 

二、流程

• 维护 Open Set （开放列表）：待处理的节点集合
• 维护 Closed Set （关闭列表）：已处理完毕的节点，即不再访问的节点

1. 将起始点放入开放列表
2. 初始化关闭列表为空
3. 每次从开放列表取 f(n) 最小的节点
4. 如果当前节点是目标节点，则算法结束
5. 否则将当前节点加入关闭列表并处理它的邻居节点
6. 如果邻居节点不在开放列表中，或找到一条更优路径，则更新邻居节点的 g和 f，将邻居节点的父节点设为当前节点，如果邻居节点不在开放列表中，则加入。

 

三、代码

import heapq
import math
from typing import List, Tuple, Optional


class Node:
    """表示网格中的节点"""

    def __init__(self, x: int, y: int, walkable: bool = True):
        self.x = x
        self.y = y
        self.walkable = walkable
        self.g = 0  # 从起点到当前节点的实际代价
        self.h = 0  # 从当前节点到目标点的预估代价
        self.f = 0  # 总代价 f = g + h
        self.parent = None  # 路径中的父节点

    def __lt__(self, other):
        # 用于优先队列比较，按f值排序
        return self.f < other.f


class AStar:
    """A* 路径查找算法实现"""

    def __init__(self, grid_width: int, grid_height: int):
        self.width = grid_width
        self.height = grid_height
        self.grid: List[List[Node]] = []
        self._initialize_grid()

    def _initialize_grid(self):
        """初始化网格"""
        self.grid = [[Node(x, y) for y in range(self.height)] for x in range(self.width)]

    def set_obstacles(self, obstacles: List[Tuple[int, int]]):
        """设置障碍物"""
        for x, y in obstacles:
            if 0 <= x < self.width and 0 <= y < self.height:
                self.grid[x][y].walkable = False

    def heuristic(self, node: Node, end_node: Node, method: str = 'manhattan') -> float:
        """计算启发式距离"""
        if method == 'manhattan':
            # 曼哈顿距离
            return abs(node.x - end_node.x) + abs(node.y - end_node.y)
        elif method == 'euclidean':
            # 欧几里得距离
            return math.sqrt((node.x - end_node.x) ** 2 + (node.y - end_node.y) ** 2)
        elif method == 'chebyshev':
            # 切比雪夫距离
            return max(abs(node.x - end_node.x), abs(node.y - end_node.y))
        else:
            return 0

    def get_neighbors(self, node: Node) -> List[Node]:
        """获取相邻节点"""
        neighbors = []
        directions = [(0, 1), (1, 0), (0, -1), (-1, 0)]  # 上下左右

        for dx, dy in directions:
            x, y = node.x + dx, node.y + dy
            # 检查是否在网格范围内
            if 0 <= x < self.width and 0 <= y < self.height:
                neighbor = self.grid[x][y]
                # 如果邻居不可行走，跳过
                if not neighbor.walkable:
                    continue
                neighbors.append(neighbor)

        return neighbors

    def find_path(self, start: Tuple[int, int], end: Tuple[int, int], heuristic_method: str = 'manhattan') -> Optional[List[Tuple[int, int]]]:
        # 获取起点和终点节点
        start_node = self.grid[start[0]][start[1]]
        end_node = self.grid[end[0]][end[1]]
        # 检查起点和终点是否可通行
        if not start_node.walkable or not end_node.walkable:
            return None

        # 初始化开放列表和关闭列表
        open_list = []
        closed_set = set()

        # 将起点加入开放列表
        start_node.g = 0
        start_node.h = self.heuristic(start_node, end_node, heuristic_method)
        start_node.f = start_node.g + start_node.h
        heapq.heappush(open_list, (start_node.f, start_node))

        while open_list:
            # 获取f值最小的节点
            current_f, current_node = heapq.heappop(open_list)
            # 如果到达终点，重构路径
            if current_node.x == end_node.x and current_node.y == end_node.y:
                return self._reconstruct_path(current_node)
            # 将当前节点加入关闭列表
            closed_set.add((current_node.x, current_node.y))
            # 检查所有邻居
            for neighbor in self.get_neighbors(current_node):
                # 如果邻居已在关闭列表中，跳过
                if (neighbor.x, neighbor.y) in closed_set:
                    continue
                move_cost = 1
                tentative_g = current_node.g + move_cost
                # 检查邻居是否在开放列表中
                neighbor_in_open = any(node[1] == neighbor for node in open_list)
                if not neighbor_in_open or tentative_g < neighbor.g:
                    # 更新邻居节点信息
                    neighbor.parent = current_node
                    neighbor.g = tentative_g
                    neighbor.h = self.heuristic(neighbor, end_node, heuristic_method)
                    neighbor.f = neighbor.g + neighbor.h

                    # 如果不在开放列表中，加入
                    if not neighbor_in_open:
                        heapq.heappush(open_list, (neighbor.f, neighbor))
                    else:
                        # 如果在开放列表中，更新优先级
                        for i, (f, node) in enumerate(open_list):
                            if node == neighbor:
                                open_list[i] = (neighbor.f, neighbor)
                                heapq.heapify(open_list)
                                break

        # 开放列表为空，未找到路径
        return None

    def _reconstruct_path(self, node: Node) -> List[Tuple[int, int]]:
        """从终点节点重构路径"""
        path = []
        current = node

        while current is not None:
            path.append((current.x, current.y))
            current = current.parent

        # 反转路径（从起点到终点）
        path.reverse()
        return path