A* 路径规划：四种算法的进化史-学习

Dijkstra 的谨慎，贪婪的果断，A* 全要了

一个只有五个符号的公式，撑起了今天几乎所有导航软件的寻路引擎。它叫 A*，而它背后，是一段算法"从笨到聪明"的进化史。

---

先记住一个数字：12%。

下面这张图，是一个叫 A* 的算法在一张布满凹形障碍的 34×34 地图上找路的过程。它只翻看了全图 12% 的格子，就拿到了和"地毯式搜索整张图"完全相同的最短路径。

是不是有点违反直觉？一张上千个格子的地图，它凭什么不用全都试一遍，还保证不走冤枉路？

其实 A* 不是某天一拍脑门"想通"的。在它之前，人们被同一个问题卡了几十年------它真正的聪明，是站在三个"踩过坑"的前辈肩膀上。

先说你天天都在用的它：

打开导航软件，输入一个 30 公里外的地址，不到三秒，一条路线就画出来了。

玩即时战略游戏，点一下地图角落，小兵自己绕开河流和石头走过去。

扫地机器人被沙发腿挡住，退半步，换个方向，继续。

这三件事，背后是同一类问题：在一张有障碍的图上，从 A 走到 B，怎么走最划算。

而要真正理解 A* 那个加号为什么管用，得先看看在它之前，人们是怎么被"卡住"的。

---

一、先把问题摆清楚

要让计算机找路，第一件事是把现实世界翻译成它能算的东西。最常用的办法叫栅格地图（Grid Map）：把空间切成一个个方格，能走的是空地，不能走的是障碍。

蓝色是起点，红色是终点，深灰是障碍。规则很简单：

• 每次只能从当前格子走到相邻的格子；

• 只允许上下左右，叫四邻域 ；再加上斜着走，就是八邻域；

• 走一步是有代价的：直着走算 1，斜着走算 √2 ≈ 1.414。

然后我们要的东西有两个，而且它们互相打架：

1. 路要够短 ------ 别绕远；

2. 算得够快 ------ 别把整张图都翻一遍。

接下来这四种算法，归根到底都是在这两件事之间找平衡。

---

二、广度优先 BFS：老实人的做法

最直觉的想法是：从起点开始，一圈一圈往外摸。

先看起点周围 8 个格子，再看这 8 个格子周围的格子，像往水里扔石头，涟漪一圈圈扩散出去，直到某一圈碰到了终点。

这就是广度优先搜索（Breadth First Search，BFS）。实现只需要一个队列：新发现的格子塞到队尾，每次从队头取一个出来处理。

这里有个所有寻路算法都要用的关键设计：父指针。

每个格子被发现时，都记下"我是从哪个格子走过来的"。等摸到终点，顺着这条父子链一路倒推回起点，路径就出来了。不用在搜索过程中保存任何完整路径，只存一个"来路"就够。

parent[起点] = None
while 队列不空:
    当前 = 队列.pop(0)
    if 当前 == 终点: break
    for 邻居 in 当前的八个方向:
        if 邻居可走 and 邻居没被访问过:
            parent[邻居] = 当前      # 记住来路
            队列.append(邻居)

BFS 的问题在哪？

它对所有方向一视同仁。终点在右上角，它照样往左下角认真地铺一大片。在我们这张 34×34 的地图上，它扩展了 925 个节点------全图 80% 的格子都被它翻了一遍。

更要命的是，BFS 默认每一步代价相同。可现实里，平地和沼泽、柏油路和碎石路，走起来完全不是一回事。

于是问题变成：如果每一步的代价不一样，怎么办？

---

三、Dijkstra：学会算账

1956 年，Edsger Dijkstra 给出了答案。核心改动只有一处：

别按"走了几步"排队，按**"一共花了多少代价"**排队。

给每个节点记一个值 g(n)：从起点走到 n，累计花掉的代价。然后把队列换成优先队列（小顶堆），每次弹出 g 值最小的那个节点来扩展。

g[起点] = 0
heapq.heappush(OPEN, (0, 起点))
while OPEN:
    _, s = heapq.heappop(OPEN)        # 弹出代价最小的
    if s == 终点: break
    for n in 邻居(s):
        新代价 = g[s] + cost(s, n)
        if 新代价 < g.get(n, 无穷大):   # 发现更便宜的走法
            g[n] = 新代价
            parent[n] = s
            heapq.heappush(OPEN, (新代价, n))

注意那句 if 新代价 < g[n] ------ 这是 Dijkstra 的灵魂。它允许反悔：一个节点之前算出来要花 10 块，后来发现绕另一条路只要 8 块，那就改过来。正是这种"随时修正"，保证了它找到的一定是最短路。

有个有意思的结论：当网格上每一步代价都相等时，Dijkstra 会退化成 BFS。 它俩其实是同一个东西的两个特例。

但 Dijkstra 还是慢，而且慢得冤枉。

因为它只关心"我从起点走了多远"，完全不知道终点在哪儿。终点明明在东北方向，它还是老老实实往西南方向铺开，把预算花在根本不可能用上的地方。

问题于是反过来了：能不能让它朝着终点走？

---

四、贪婪最佳优先：冲得太猛

那就把标准彻底翻转一下：不看走过的路，只看还剩多远。

给每个节点估一个值 **h(n)**：从 n 到终点大概还有多远（比如直线距离）。优先队列按 h 排序，谁离终点近就先扩展谁。

这叫贪婪最佳优先搜索（Greedy Best First Search）。

效果非常惊人。在同一张地图上，它只扩展了 179 个节点，是 BFS 的五分之一，几乎是直冲终点去的。

代价是什么？路不是最短的。

看图里那条绿线------它一头扎进了凹形障碍围出来的死胡同，撞了墙才退出来重找。它的路径长度是 69.9 ，而最优解只要 43.9 ------整整绕远了一倍半。

道理不难懂：贪婪算法只盯着"还剩多远"，从不回头看"已经绕了多少冤枉路"。就像开车只认导航箭头方向，不看路况，眼看着离目的地越来越近，结果拐进一条断头路。

至此，两个极端都摆在桌上了：

	看什么	结果
Dijkstra	只看过去：已经花了多少	一定最短，但慢
贪婪	只看未来：还要花多少	很快，但可能绕远

一个太谨慎，一个太莽撞。那......能不能都要？

---

五、A*：把两笔账加在一起

1968 年，斯坦福研究院的 Peter Hart、Nils Nilsson 和 Bertram Raphael 给出了那个公式：

翻译成大白话：

• g(n) = 已经花掉的钱。走过的路，确定值，一分不差。

• h(n) = 预估还要花的钱。没走的路，猜的 ，所以叫启发函数（Heuristic）。

• f(n) = 这条路总共大概要花多少。决策就看它。

每次从优先队列里弹出 f 值最小的节点。就这一处改动。

一句话记住：Dijkstra 管过去，贪婪管未来，A* 两头都管。

完整流程是这样的，需要两个集合：open_set（发现了但还没处理的）和 close_set（已经处理完的）：

1. 起点放进 open_set，f 值设为 0
2. 只要 open_set 不空，取出其中 f 最小的节点 n：
     若 n 是终点 → 顺着 parent 回溯，返回路径，结束
     否则：
       把 n 从 open_set 移到 close_set
       遍历 n 的每个邻居 m：
         m 在 close_set 里 → 跳过
         算出走到 m 的新 g 值：
           m 没见过，或者新 g 值更小 →
             更新 g[m]，设 parent[m] = n
             算 f[m] = g[m] + h(m)，放进 open_set
3. open_set 空了还没到终点 → 无解

为什么这么一加就管用了？

因为 g 和 h 互相牵制：

• h 项拉着它朝终点方向走，不像 Dijkstra 那样四面撒网；

• g 项拽着它记得已经绕了多远，一旦走进死胡同，g 值疯涨，f 值随之上去，这个方向自动降优先级，算法转头去试别的路。

看四张图放在一起的对比，这件事就非常直观了：

数据摆在这儿（同一张地图、同一起终点）：

算法	扩展节点数	路径长度	全图占比	结论
BFS	925	46.4	80%	步数最少（但代价不最优，因为有斜走 √2）
Dijkstra	934	43.9	81%	一定最短，但翻了大半张图
贪婪最佳优先	179	69.9	15%	极快，但绕远了一倍半
A*	144	43.9	12%	既最短，又省 85% 的活

A* 拿到了和 Dijkstra 一模一样的最优路径，工作量却只有它的 **15%**。BFS 因为只算步数、不认代价（八邻域里有 √2 斜走），反而找不到真正的最短。这就是那个加号的价值。

---

六、启发函数：A* 的灵魂旋钮

A* 的全部玄机，都在 h(n) 这一项上。它不是随便估的，估法直接决定算法的性格：

h(n) 的取值	算法变成什么	保证最短吗
h ≡ 0	退化成 Dijkstra	是，但慢
h < 真实剩余代价	标准 A*，偏保守	是，h 越小越慢
h = 真实剩余代价	理想状态，直奔终点	是，且最快
h > 真实剩余代价	冒进的 A*	否，但很快
h ≫ g	退化成贪婪算法	否

中间那条线叫可采纳性（Admissibility） ：只要 h(n) 从不高估真实代价，A* 就一定能找到最短路径。 这是 A* 最重要的一条理论保证，也是选启发函数时唯一必须守住的底线。

把这个规律画出来，比看表格更清楚。下面是同一个 A*，只把公式改成 f = g + w·h，调 w 这一个旋钮：

w=0 时它就是 Dijkstra，铺得到处都是；w=1 是标准 A*；w 调到 3，搜索区域瞬间瘦一圈，速度快了，但路径已经不是最优的了。

这个旋钮的实际意义：很多场景其实不需要绝对最短。游戏里的小兵、仓库里的 AGV，只要路看着合理、算得够快就行。这时候故意把 w 调大（这叫 **Weighted A***），是非常常见的工程取舍。

三种距离怎么选

具体到网格地图，h(n) 一般就用几何距离，关键是要和地图允许的移动方式对上：

# 只能上下左右（四邻域）→ 曼哈顿距离
h = D * (dx + dy)

# 允许斜着走（八邻域）→ 对角距离
h = D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)   # D2 = √2·D

# 任意角度（连续空间）→ 欧几里得距离
h = D * sqrt(dx*dx + dy*dy)

⚠️ 新手最常见的坑：在八邻域地图上用曼哈顿距离。

斜着走一步实际只花 √2 ≈ 1.41，曼哈顿距离却按 2 来算------高估了。高估就违反可采纳性，A* 会自信地给出一条不是最短的路，而且你还很难发现，因为它看起来挺像那么回事。

---

七、动手跑一遍

理论说完，来点能直接跑的。下面这版用 heapq 做优先队列，八邻域 + 对角距离，五十行以内：

import heapq, math

NEIGHBORS = [(-1,-1),(-1,0),(-1,1),(0,-1),(0,1),(1,-1),(1,0),(1,1)]

def heuristic(p, goal):
    """对角距离：匹配八邻域移动"""
    dx, dy = abs(p[0]-goal[0]), abs(p[1]-goal[1])
    return (dx + dy) + (math.sqrt(2) - 2) * min(dx, dy)

def step_cost(a, b):
    """直走 1，斜走 √2"""
    return math.sqrt(2) if (a[0]!=b[0] and a[1]!=b[1]) else 1.0

def astar(start, goal, is_free):
    g = {start: 0.0}
    parent = {start: None}
    closed = set()
    tie = 0                                   # 打破 f 值相同时的排序歧义
    open_q = [(heuristic(start, goal), tie, start)]

    while open_q:
        _, _, s = heapq.heappop(open_q)
        if s in closed:                       # 惰性删除：跳过过期条目
            continue
        closed.add(s)
        if s == goal:                         # 到了，回溯路径
            path = []
            while s is not None:
                path.append(s)
                s = parent[s]
            return path[::-1]

        for d in NEIGHBORS:
            n = (s[0]+d[0], s[1]+d[1])
            if not is_free(n) or n in closed:
                continue
            ng = g[s] + step_cost(s, n)
            if n not in g or ng < g[n]:       # ← 关键：允许反悔
                g[n] = ng
                parent[n] = s
                tie += 1
                heapq.heappush(open_q, (ng + heuristic(n, goal), tie, n))
    return None                               # 无解

跑出来的样子：

两个必须提醒的实现细节

网上流传最广的那版 A* 教程代码（包括几篇高赞博客），有两处会实实在在坑到人：

① 用列表线性扫描找最小 f 值

# ❌ 每次都遍历整个 open_set 找最小值，O(n)
for p in self.open_set:
    if self.TotalCost(p) < min_cost: ...

节点一多，这里就是性能瓶颈。**换成 heapq，取最小值从 O(n) 降到 O(log n)**，大地图上差距是数量级的。

② 节点已在 open_set 中时，不更新更优的 g 值

# ❌ 只在"没见过"的时候才处理
if not self.IsInOpenList(p):
    p.parent = parent
    p.cost = self.TotalCost(p)

这是个隐蔽的正确性 bug。一个节点先被一条较贵的路发现，后来又被一条更便宜的路碰到，这份代码会直接忽略后者。结果就是：算法照样跑完、照样给你一条路，但它不是最短的。

正确写法就是上面那句 if n not in g or ng < g[n] ------ A* 必须允许反悔。这一点它和 Dijkstra 完全一致。

---

八、A* 不是终点

现实里的问题比栅格地图麻烦得多，于是有了一大家子变种：

• Weighted A* ------ 就是上面那个 w 旋钮，用最优性换速度；

• ARA*（Anytime Repairing A*）------ 先用大 w 快速给一条能用的路，还有时间就继续优化，随时可以打断取当前最好结果。留给你的时间不确定时特别有用；

• D* / D* Lite ------ 地图会变怎么办？走到一半发现前面塌了，不用从头重算，只修补受影响的那部分。移动机器人的标配；

• Field D* ------ 用插值让路径脱离栅格的束缚，转弯更自然，不会走出锯齿；

• JPS （Jump Point Search）------ 在均匀网格上跳过大片对称的等价节点，同样最优，但快一个数量级；

• Hybrid A* ------ 加入车辆转弯半径约束，让规划出来的路"车真的能开"。自动驾驶泊车的常用方案。

最后一句实在话：A* 输出的是一串折线点，不是能直接执行的轨迹。

机器人不会瞬间 90 度转弯，车更不行。真实系统里，A* 之后一定还要接路径平滑 和轨迹优化（贝塞尔、B 样条、Minimum Snap 等等）。A* 干的是"从哪儿走"这一层，"怎么走得平顺"是下一层的事。

---

写在最后

回头看这条演化线，其实特别清晰：

BFS       一视同仁地铺开          →  不认路
Dijkstra  只看走过的路 g(n)       →  不看目标
贪婪      只看剩下的路 h(n)       →  容易撞墙
A*        f(n) = g(n) + h(n)     →  两头都管

每一步都是在上一步的缺陷上打补丁，最后那个加号，把两种偏执缝成了一个平衡。

A* 真正聪明的地方，从来不是它算得多快，而是它知道该往哪儿算。

你在什么项目里用过 A*？踩过哪个坑？欢迎在评论区聊聊。

---

参考素材

• 路径规划之 A* 算法（CSDN / 知乎，原文 paul.pub）

• 自动驾驶｜路径规划算法 Dijkstra 与 A*（CSDN，来源古月居）

• Introduction to the A* Algorithm，Red Blob Games

• 开源实现：github.com/zhm-real/PathPlanning