A* 算法讲解与简单示例

A* 算法讲解与简单示例

1. A* 算法是什么

A* 算法是一种常用的路径搜索算法，常见于栅格地图寻路、游戏地图导航、二维地图路径规划等场景。

它的目标是：在一张栅格地图中，从起点 Start 找到一条到终点 Goal 的代价尽可能小的路径，同时避开障碍物。

A* 的核心公式是：

f(n) = g(n) + h(n)

其中：

• g(n)：从起点走到当前节点 n 的真实代价。
• h(n)：从当前节点 n 到终点的估计代价，也叫启发函数。
• f(n)：当前节点的综合评分，A* 每次优先搜索 f(n) 最小的节点。

简单理解：
g(n) 代表“已经走了多远”，h(n) 代表“估计还要走多远”，f(n) 代表“这条路整体看起来有多划算”。

2. A* 需要维护的数据结构

A* 通常需要下面几个重要结构：

名称	作用
open_list	待搜索节点集合，保存还没有正式扩展的节点
closed_set	已经搜索过的节点集合，避免重复搜索
g_score	记录起点到每个节点的真实代价
came_from	记录每个节点是从哪个父节点走来的，用于最后回溯路径

在代码中，open_list 通常使用优先队列实现。Python 中可以使用 heapq，这样每次都能快速取出 f(n) 最小的节点。

3. 启发函数 h(n)

如果地图只允许上下左右四个方向移动，常用曼哈顿距离：

h(n) = abs(current_x - goal_x) + abs(current_y - goal_y)

例如当前点是 (2, 3)，目标点是 (8, 6)：

h(n) = abs(2 - 8) + abs(3 - 6)
     = 6 + 3
     = 9

曼哈顿距离适合网格地图，因为它只统计横向和纵向需要移动多少格。

4. A* 算法流程

1. 把起点加入 open_list。
2. 从 open_list 中取出 f(n) 最小的节点，记为当前节点。
3. 如果当前节点就是终点，搜索结束。
4. 把当前节点加入 closed_set。
5. 遍历当前节点的所有相邻节点。
6. 如果相邻节点是障碍物，跳过。
7. 如果相邻节点已经在 closed_set 中，跳过。
8. 计算从起点经过当前节点到相邻节点的新代价。
9. 如果这条新路径更短，就更新该相邻节点的 g_score 和父节点。
10. 把相邻节点加入 open_list。
11. 重复上述过程，直到找到终点或 open_list 为空。

如果 open_list 为空还没有找到终点，说明起点到终点之间不存在可行路径。

5. 前两步的更新解释

下面用本示例中的起点 (0, 0)、终点 (9, 7) 来说明 A* 前两步是怎么更新的。这里使用四方向移动，每移动一格代价为 1，启发函数使用曼哈顿距离。

第 1 步：初始化起点

起点为：

start = (0, 0)
goal = (9, 7)

起点的真实代价：

g(start) = 0

起点到终点的估计代价：

h(start) = abs(0 - 9) + abs(0 - 7)
         = 9 + 7
         = 16

所以起点的总代价：

f(start) = g(start) + h(start)
         = 0 + 16
         = 16

此时各数据结构更新为：

数据结构	更新结果
open_list	加入 (0, 0)，优先级 f=16
closed_set	仍为空
g_score	{(0, 0): 0}
came_from	{(0, 0): None}

这一步的含义是：先把起点作为第一个待搜索节点放入 open_list。

第 2 步：扩展起点并更新邻居

A* 从 open_list 中取出 f 最小的节点。此时只有起点，所以当前节点为：

current = (0, 0)

然后把起点加入 closed_set：

closed_set = {(0, 0)}

从 (0, 0) 出发，四方向邻居理论上是：

(1, 0), (-1, 0), (0, 1), (0, -1)

其中 (-1, 0) 和 (0, -1) 超出地图范围，所以跳过。有效邻居为：

(1, 0), (0, 1)

对邻居 (1, 0)：

g(1, 0) = g(0, 0) + 1 = 1
h(1, 0) = abs(1 - 9) + abs(0 - 7) = 15
f(1, 0) = 1 + 15 = 16

更新：

came_from[(1, 0)] = (0, 0)
g_score[(1, 0)] = 1
open_list 加入 (1, 0)

对邻居 (0, 1)：

g(0, 1) = g(0, 0) + 1 = 1
h(0, 1) = abs(0 - 9) + abs(1 - 7) = 15
f(0, 1) = 1 + 15 = 16

更新：

came_from[(0, 1)] = (0, 0)
g_score[(0, 1)] = 1
open_list 加入 (0, 1)

第 2 步结束后：

数据结构	更新结果
open_list	包含 (1, 0) 和 (0, 1)，二者 f 都是 16
closed_set	{(0, 0)}
g_score	{(0, 0): 0, (1, 0): 1, (0, 1): 1}
came_from	{(0, 0): None, (1, 0): (0, 0), (0, 1): (0, 0)}

后续循环会继续从 open_list 中取出 f 最小的节点，重复“扩展当前节点、计算邻居代价、更新更短路径”的过程。

6. 地图符号说明

下面的代码使用字符地图来展示路径：

符号	含义
S	起点
G	终点
#	障碍物
*	A* 找到的最终路径
.	可通行空白区域

7. 完整 Python 示例代码

可以把下面代码保存为 astar_demo.py 后运行：

#!/usr/bin/env python3
"""A* 算法二维网格路径规划示例。"""

from __future__ import annotations

import heapq
from typing import Dict, FrozenSet, Iterable, List, Optional, Tuple


GridPoint = Tuple[int, int]


class GridMap:
    """二维网格地图。"""

    def __init__(self, width: int, height: int, obstacles: FrozenSet[GridPoint]):
        self.width = width
        self.height = height
        self.obstacles = obstacles

    def in_bounds(self, point: GridPoint) -> bool:
        """判断点是否在地图范围内。"""

        x, y = point
        return 0 <= x < self.width and 0 <= y < self.height

    def passable(self, point: GridPoint) -> bool:
        """判断点是否不是障碍物。"""

        return point not in self.obstacles

    def neighbors(self, point: GridPoint) -> Iterable[GridPoint]:
        """返回当前点上下左右四个可通行邻居。"""

        x, y = point
        candidates = (
            (x + 1, y),
            (x - 1, y),
            (x, y + 1),
            (x, y - 1),
        )

        for candidate in candidates:
            if self.in_bounds(candidate) and self.passable(candidate):
                yield candidate


def heuristic(current: GridPoint, goal: GridPoint) -> int:
    """曼哈顿距离，作为 A* 的启发函数 h(n)。"""

    return abs(current[0] - goal[0]) + abs(current[1] - goal[1])


def reconstruct_path(
    came_from: Dict[GridPoint, Optional[GridPoint]],
    goal: GridPoint,
) -> List[GridPoint]:
    """从终点沿父节点回溯，恢复完整路径。"""

    path: List[GridPoint] = []
    current: Optional[GridPoint] = goal

    while current is not None:
        path.append(current)
        current = came_from[current]

    path.reverse()
    return path


def astar(grid: GridMap, start: GridPoint, goal: GridPoint) -> List[GridPoint]:
    """使用 A* 算法从 start 搜索到 goal。"""

    if not grid.in_bounds(start) or not grid.in_bounds(goal):
        raise ValueError("起点或终点不在地图范围内")

    if not grid.passable(start) or not grid.passable(goal):
        raise ValueError("起点或终点不能是障碍物")

    start_f_score = heuristic(start, goal)
    open_list: List[Tuple[int, int, GridPoint]] = []
    heapq.heappush(open_list, (start_f_score, 0, start))

    came_from: Dict[GridPoint, Optional[GridPoint]] = {start: None}
    g_score: Dict[GridPoint, int] = {start: 0}
    closed_set: set[GridPoint] = set()
    push_count = 0

    while open_list:
        _, _, current = heapq.heappop(open_list)

        if current in closed_set:
            continue

        if current == goal:
            return reconstruct_path(came_from, goal)

        closed_set.add(current)

        for neighbor in grid.neighbors(current):
            if neighbor in closed_set:
                continue

            new_g_score = g_score[current] + 1

            if new_g_score >= g_score.get(neighbor, 10**9):
                continue

            came_from[neighbor] = current
            g_score[neighbor] = new_g_score

            h_score = heuristic(neighbor, goal)
            f_score = new_g_score + h_score

            push_count += 1
            heapq.heappush(open_list, (f_score, push_count, neighbor))

    raise RuntimeError("没有找到可行路径")


def draw_grid(
    grid: GridMap,
    start: GridPoint,
    goal: GridPoint,
    path: List[GridPoint],
) -> str:
    """把地图、障碍物和路径画成终端字符图。"""

    path_set = set(path)
    lines: List[str] = []

    for y in reversed(range(grid.height)):
        row: List[str] = []

        for x in range(grid.width):
            point = (x, y)

            if point == start:
                row.append("S")
            elif point == goal:
                row.append("G")
            elif point in grid.obstacles:
                row.append("#")
            elif point in path_set:
                row.append("*")
            else:
                row.append(".")

        lines.append(" ".join(row))

    return "\n".join(lines)


def main() -> None:
    start = (0, 0)
    goal = (9, 7)

    obstacles = frozenset(
        {
            (3, 0),
            (3, 1),
            (3, 2),
            (3, 3),
            (3, 5),
            (3, 6),
            (6, 2),
            (6, 3),
            (6, 4),
            (6, 5),
            (7, 5),
            (8, 5),
        }
    )

    grid = GridMap(width=10, height=8, obstacles=obstacles)
    path = astar(grid, start, goal)

    print("A* path:")
    print(draw_grid(grid, start, goal, path))
    print()
    print(f"Path length: {len(path)}")
    print("Path:")
    print(" -> ".join(str(point) for point in path))


if __name__ == "__main__":
    main()

8. 示例运行结果

运行后会输出类似下面的地图：

. . . . . . . . . G
. . . # . * * * * *
. . . # . * # # # .
. . * * * * # . . .
. . * # . . # . . .
. . * # . . # . . .
. . * # . . . . . .
S * * # . . . . . .

对应路径为：

(0, 0) -> (1, 0) -> (2, 0) -> (2, 1) -> (2, 2) -> (2, 3) -> (2, 4) -> (3, 4) -> (4, 4) -> (5, 4) -> (5, 5) -> (5, 6) -> (6, 6) -> (7, 6) -> (8, 6) -> (9, 6) -> (9, 7)

9. 小结

A* 的关键不是盲目搜索整张地图，而是用 f(n)=g(n)+h(n) 选择更值得探索的节点。

在栅格地图中，可以把每个格子看作一个节点，把上下左右相邻格子看作可移动边。只要维护好 open_list、closed_set、g_score 和 came_from，就能从起点逐步搜索到终点，并最终回溯出完整路径。