图搜索与 A*

仓库里的搬运机器人要从货架 A 开到打包台，扫地机器人要绕过沙发回到充电座，自动驾驶汽车要从车道开进一个车位，机械臂要从一个姿势摆到另一个姿势而不碰到桌子。它们共同的问题是运动规划：在构型空间的自由部分 \(\mathcal C_{\text{free}}\) 里，找一条从起点到终点的路径，最好还是最短、最省时或最安全的那条。

最直接的办法是把连续的空间离散化：把地图切成格子（每个空闲格是一个节点，相邻格之间连一条边，边上写着走过去的代价），问题就变成了在一张图上找最短路。6.12 节已经用值迭代在格子世界里算出过“每个格子到终点的最优代价” \(V\)，那是对所有起点一次性算完。可是机器人通常只关心一个起点：此刻它在哪。这时没必要把整张表算出来，只要从起点出发，按某种聪明的顺序往外探，探到终点就停。这种顺序的选择，就是 BFS、Dijkstra 与 A* 的区别。

A* 是 1968 年为 SRI 的移动机器人 Shakey 发明的，五十多年后仍然是 ROS 导航栈、游戏寻路和自动驾驶泊车规划的核心。本节不仅讲它为什么对、为什么快，也讲在真实机器人上用它时要面对的几件事：启发式怎么选、能不能为了速度牺牲一点最优性、栅格路径为什么歪歪扭扭、汽车不能原地转向怎么办、地图边走边变时怎么不必从头再来。

看下面这张小路网：10 个路口，路的长度就是两点间的直线距离（虚线那条是土路，同样长度要付 1.5 倍代价）。从 S 到 G 怎么走最短？

Dijkstra 算法维护两个集合。开放集（open list，也叫“前沿”）放的是“已经找到一条路能到、但还不确定是不是最短”的节点，每个节点记着目前已知的从起点出发的最小代价 \(\hl{g}{g(n)}\)；关闭集（closed list）放的是“最短代价已经确定”的节点。每一步：从开放集里取出 \(g\) 最小的节点 \(u\)，把它移进关闭集；然后看它的每个邻居 \(v\)，如果“经过 \(u\) 到 \(v\)”比 \(v\) 现有的记录更便宜，就更新 \(g(v)\) 并记下 \(v\) 的父节点是 \(u\)（这一步叫松弛）。终点被取出时，沿父节点倒推回去就是最短路。

为什么取出的那一刻 \(g(u)\) 就是最短的？因为边的代价都不为负，开放集里别的节点的 \(g\) 都不比它小，任何经过它们再绕到 \(u\) 的路只会更长。所以 Dijkstra 是按“离起点的距离”从近到远、一圈圈地把节点定下来：这正是 6.12 节值迭代的“水波”，只是从起点出发、并且每个节点只处理一次。

A* 只改了一处：取节点时不看 \(g(n)\)，而看 \(f(n)=g(n)+\hl{h}{h(n)}\)，其中 \(h(n)\) 是对“从 \(n\) 到终点还要多少”的估计，这里取到 G 的直线距离。远离目标的节点 \(h\) 大，就被往后排。下面一步步跑一遍，对比两者取出了多少个节点。

现在换成机器人最常用的图：占据栅格。每个空闲格是一个节点；“4 连通”只能上下左右走（每步代价 1），“8 连通”还能走斜线（代价 \(\sqrt2\)），但斜走时不能切过墙格的角（斜向两侧的两个直邻格都必须空闲）。走进一个格子要付该格的价格：普通格 1，绿色泥地 4（和 6.12 节一样）。墙不能走。

栅格上 \(h\) 有几种常见选择，都按“最便宜的格子价格 1”来估计剩余距离，设 \(\Delta x,\Delta y\) 是到终点相差的格数：曼哈顿距离 \(\Delta x+\Delta y\)（4 连通空地图上恰好等于真实代价）；八方向距离（octile）\(\max(\Delta x,\Delta y)+(\sqrt2-1)\min(\Delta x,\Delta y)\)（8 连通空地图上恰好等于真实代价）；欧氏距离 \(\sqrt{\Delta x^2+\Delta y^2}\)（两种连通都不会高估，但比前两者更“松”）。另有两个变体：加权 A* 用 \(g+w\,h\)，\(w>1\) 时更“急于”冲向终点；贪心最佳优先只看 \(h\)，完全不管已经走了多远。

每个格子都有一个真实的“到终点的最小代价” \(\hl{hstar}{h^*(n)}\)：它就是 6.12 节的价值函数 \(V(n)\)，可以从终点反向跑一遍 Dijkstra 算出来。启发式 \(h\) 是它的廉价替身，好坏可以用比值 \(h/h^*\) 衡量：处处等于 1 时 A* 只展开最短路上的格子；处处等于 0（即 \(h\equiv0\)）时 A* 退化成 Dijkstra。只要 \(h\le h^*\)（称为可容许，admissible），A* 就保证最优；一旦某处 \(h>h^*\)，就可能错过最短路。

加权 A* 故意把 \(h\) 乘上 \(\hl{w}{w}>1\)，让它在很多格子上高估。好消息是代价有上界：只要原来的 \(h\) 满足一致性（第 5 节），找到的路径代价不超过 \(w\) 倍最优。下图左边按 \(w\,h/h^*\) 给每个格子着色，右边扫一遍 \(w\) 从 1 到 5，画出展开数和代价比。

几点观察。(1) 在 U 形陷阱里，\(h=0\)、欧氏、八方向距离分别展开 295、132、114 个格子：\(h\) 越接近 \(h^*\)，展开越少。但 U 形开口里所有格子的 \(h\) 都远小于 \(h^*\)（直线距离穿墙而过），A* 也只好把陷阱“填满”才绕出来，这是所有只看几何距离的启发式的通病。(2) 加权 A* 的代价比往往远低于上界 \(w\)：上界是最坏情况的保证，不是典型值。(3) 曼哈顿距离在 8 连通栅格上不可容许，但大多数起点下它仍然碰巧给出最短路；“不可容许”说的是失去保证，不是每次都错。

栅格上的“最短路”只是在允许的移动方向里最短。4 连通只能走 0°、90°……，一条 45° 方向的直线要走成阶梯，长度是 \(\hl{c4}{\Delta x+\Delta y}\)，比直线长 \(\sqrt2-1\approx41\%\)。8 连通加上了斜线，长度变成 \(\hl{c8}{\max+(\sqrt2-1)\min}\)，最坏出现在 22.5° 方向，长 8.2%。16 连通再加上“马步” \((2,1)\) 一类的移动，最坏只长 2.7%。更麻烦的是形状：同样代价的阶梯路径有很多条，哪条被选中取决于平局怎么打破，结果往往是不自然的折线，贴着墙角拐弯。

任意角度（any-angle）规划不再把路径限制在栅格边上。Theta*（Nash 等，2007）在展开节点 \(u\)、更新它的邻居 \(v\) 时，检查 \(v\) 和 \(u\) 的父节点之间有没有直线视线，有就让 \(v\) 直接连到那个父节点上（跳过 \(u\)），于是路径的拐点只出现在障碍的角上。它不保证是真正的欧氏最短路，但通常非常接近。

图 \(\mathcal G=(\mathcal V,\mathcal E)\)，每条边有非负代价 \(c(u,v)\ge0\)。一条路径的代价是它各边代价之和，\(C^*\) 是从起点 \(s\) 到终点 \(t\) 所有路径中的最小代价。记 \(g^*(n)\) 为从 \(s\) 到 \(n\) 的最小代价，\(\hl{hstar}{h^*(n)}\) 为从 \(n\) 到 \(t\) 的最小代价，它们各满足一个贝尔曼方程：

第二个方程正是 6.12 节的 \(V\)（\(\gamma=1\) 的最短路问题）。最后一个等式说：经过最短路上任意一点 \(n\) 的 \(g^*+h^*\) 都等于 \(C^*\)，不在任何最短路上的点则严格大于 \(C^*\)。A* 的全部思想，就是用一个可计算的 \(h\) 去近似这里的 \(h^*\)。

把起点放进开放集 \(\hl{open}{\mathcal O}\)，\(g(s)=0\)，其余 \(g=\infty\)。重复：从 \(\mathcal O\) 取出排序键最小的 \(u\)，放进关闭集 \(\hl{closed}{\mathcal K}\)；若 \(u=t\) 则沿父指针返回路径；否则对每个邻居 \(v\notin\mathcal K\)，若 \(g(u)+c(u,v)<g(v)\)，令 \(g(v)\leftarrow g(u)+c(u,v)\)、父节点 \(\pi(v)\leftarrow u\)，并把 \(v\) 以新的键放进 \(\mathcal O\)。排序键：

开放集用二叉堆实现，复杂度为 \(O\big((|\mathcal V|+|\mathcal E|)\log|\mathcal V|\big)\)；栅格上每个节点至多 8 条边。BFS 用普通队列，\(O(|\mathcal V|+|\mathcal E|)\)，但只在所有边代价相同时最优（FIG 2 的泥地）。实现上通常不做“降低堆中键值”，而是把新键再压一次堆，取出时跳过过期的条目（惰性删除）。

(1) 是可容许，(2) 是一致。一致（也叫单调）是边上的三角不等式：估计值走一步最多下降这一步的代价。沿着从 \(n\) 到 \(t\) 的最短路把它加起来，就得到 \(h(n)\le h^*(n)\)：一致蕴含可容许。直线距离、八方向距离（8 连通）、曼哈顿距离（4 连通）都是一致的。

理解一致性最干净的方式是约化代价（reduced cost）。定义

任何一条从 \(s\) 出发的路径 \(P_{s\to n}\)，其约化代价只是原代价减去常数 \(h(s)\) 再加上终点的 \(h(n)\)，所以到 \(t\) 的路径谁短谁长完全不变（\(h(t)=0\)），而 Dijkstra 在约化代价上的键 \(\tilde g(n)=g(n)+h(n)-h(s)=f(n)-h(s)\)，与 A* 的 \(f\) 只差一个常数。结论：一致启发式下的 A* 就是边代价为 \(\tilde c\ge0\) 的 Dijkstra。Dijkstra 的一切性质随之而来：节点被取出时 \(g\) 已是最优，关闭的节点不必重开，取出的 \(f\) 值单调不减，取出 \(t\) 时路径最优。只可容许、不一致的 \(h\) 也能保证最优，但需要允许重开关闭集里的节点。

A* 展开哪些节点。由于 \(f\) 单调不减，所有 \(g^*(n)+h(n)<C^*\) 的节点在 \(t\) 之前必然被展开，\(f>C^*\) 的节点永远不会。所以 \(h\) 越大（越接近 \(h^*\)），这个集合越小：若 \(h_1\ge h_2\) 且都一致，用 \(h_1\) 展开的节点（除平局外）是用 \(h_2\) 的子集。两个极端：\(h\equiv0\) 时是 Dijkstra；\(h=h^*\) 时 \(f\) 在最短路上处处等于 \(C^*\)，配合“平局取 \(g\) 大”的规则，A* 只展开最短路上的节点。这也是“学习一个启发式”有意义的原因：价值函数本身就是最好的启发式。

加权 A* 的上界。令 \(f_w=g+w\,h\)。取出 \(t\) 的那一刻，最短路上总有某个节点 \(n\) 在开放集里且 \(g(n)=g^*(n)\)，于是

这一推导在允许重开节点时直接成立；Likhachev 等（2003）证明了 \(h\) 一致时即使不重开也成立（FIG 2、FIG 3 都不重开）。所以 \(w\) 是一个旋钮：用至多 \(w\) 倍的代价，换取通常少得多的展开。ARA*（随时修复 A*）从大 \(w\) 开始，先快速给出一条路，再逐步降低 \(w\) 并复用之前的搜索，时间允许就一直改进。

9.1 节的运动学自行车模型：后轴中心 \((x,y)\)、航向 \(\theta\)，

在 \((x,y)\) 栅格上，8 连通的“边”要求原地转向，车做不到。办法是把图的节点换成 \((x,y,\theta)\)，边换成运动基元：在曲率约束下实际能开出来的短轨迹（直行、左转弧、右转弧，前进或倒车）。

• 状态格（state lattice，Pivtoraiko 与 Kelly）：航向只取有限几个值（FIG 5 用 8 个，每 45° 一个），每个基元都精心设计成从一个格点出发、恰好落在另一个格点上（FIG 5 的转弯基元是半径 \(R=2+2\sqrt2\approx4.83\) m 的 45° 圆弧加 0.83 m 直线，正好从 \((0,0,0°)\) 到 \((4,2,45°)\)）。它仍是一张普通的图，A* 的最优性在这张图上成立；代价是表达能力受基元集合限制。
• Hybrid A*（Dolgov 等）：节点里存连续的位姿，基元直接从这个连续位姿积分出去，只用 \((x,y,\theta)\) 的栅格单元来去重：同一个单元里只保留代价最小的那个连续状态。这样每条边都严格可行，但同一单元里被丢掉的状态可能正是通往终点所需要的，所以它既不完备也不最优，只是“分辨率够细时很好用”。真实实现在接近终点时还会尝试一条 Reeds–Shepp 曲线直接解析地连到目标位姿（解析扩展），FIG 5 为简洁省略了这一步，改为允许到达目标附近 0.8 m、航向差 0.25 rad（约 14°）以内。

启发式取两个估计的较大者（Dolgov 等称两者都可容许）：考虑运动学、忽略障碍（无障碍时的 Reeds–Shepp 最短长度，可以离线制表），以及考虑障碍、忽略运动学（在二维栅格上从终点跑一次 Dijkstra）。前者在开阔处管用（车需要掉头时直线距离严重低估，9.1 节已经指出），后者把搜索从死胡同里拉出来。FIG 5 只实现了后者与直线距离的较大者。注意二维栅格距离可能比连续空间的真实距离略长（FIG 4：最多 8%），严格说并不可容许；FIG 5 因此把它减去 1.5 m 的余量再用，实践中这点误差通常被接受。

机器人边走边用传感器发现新障碍，每发现一次，图上就有几条边的代价变成 \(\infty\)。从头跑一遍 A* 当然可以，但大部分旧结果其实仍然有效。D* Lite（Koenig 与 Likhachev，2002）的做法：从终点向机器人方向搜索，于是 \(g(s)\) 是“从 \(s\) 到终点”的代价，机器人移动时终点不变，这些值不会作废；再给每个节点加一个一步前瞻值

\(g=\mathrm{rhs}\) 的节点叫局部一致；边代价一变，只有它两端的节点变得不一致，被放进队列。队列按两段键 \(k\) 处理不一致的节点，直到机器人所在节点一致且队列中再没有键更小的节点：效果上等价于“在新地图上重新算出了需要的那部分 \(g\)”，但只碰受影响的区域。\(h(s_{\text{start}},\cdot)\) 让修补朝机器人聚焦（像 A*），机器人每走一段，\(k_m\) 加上 \(h(s_{\text{last}},s_{\text{start}})\)，使队列里旧的键仍然是下界，不必重排整个队列。D* Lite 的路径与在当前已知地图上重新跑 A* 的代价完全相同（我们在 FIG 6 两张地图、半径 2 到 6 的每一次重规划上都核对过），省的只是计算。

下图是一辆 4.0 m × 1.8 m 的车，最小转弯半径 4.83 m，状态是后轴中心的 \((x,y,\theta)\)。车身用沿中心线的 3 个圆盘近似做碰撞检测（这是常见做法：圆盘到障碍的距离很便宜）。Hybrid A* 每次展开生成 5 种曲率 \(\times\) 前进/倒车的 1.6 m 弧，用 1 m × 1 m × 5°（72 个航向）的单元去重；代价是路程，倒车的路程乘 2，每次换挡加 3 m，打方向的变化也有小惩罚。

几个细节。(1) 换成“状态格”：停车场仍能停进去，但要倒车调整两次，代价约为 Hybrid A* 的两倍；死胡同里要换向 5 次，代价 66.6，是 Hybrid A* 的 33.9 的近两倍；侧方停车干脆无从下手：目标位姿吸附到最近的整数格点 \((10,1)\) 后，车身压到了场地边界，读数显示“起点或目标与障碍相撞”。原因是 8 个航向、整数格点、4 m 长的转弯基元太粗了，换成更密的航向和更短的基元才行。基元集合的设计就是状态格规划器的全部功夫。(2) Hybrid A* 的路径还要后处理：实际系统会再做平滑（Dolgov 等用了带障碍项的共轭梯度优化，与 6.13、14.3 节的轨迹优化同类），再交给 9.1 节的路径跟踪控制器。(3) 打开“显示栅格 A*”比一比：点机器人的路径更短，却是车开不出的折线。

下图的机器人一开始不知道墙在哪，只假设未知的地方都能走（乐观假设）。它每走一格，用半径为 \(r\) 的传感器看一圈，发现新墙就重规划。同一时刻用同样的已知地图做两件事：D* Lite 增量修补，以及从机器人当前位置从头跑一次 A*，比较两者展开的节点数。

在迷宫里（传感器半径 3），D* Lite 的累计展开只有从头 A* 的约三分之一（943 对 2767）；半径越大，一次看到的墙越多，优势越小（半径 6 时约一半）。原因是：迷宫里直线距离启发式很差，从头 A* 每次都要重新填满大片走廊，而墙总是在机器人身边被发现，D* Lite 只需修补靠近机器人的一小块。在“房间”地图上差别就小得多，甚至可能反过来（半径 3 时 326 对 300）：开阔地上 A* 本来就只展开一条窄带，D* Lite 每次修补还要付出维护 \(\mathrm{rhs}\) 和队列的开销。增量方法省多少，取决于变化发生在哪里、启发式有多好。还要注意第一次规划：迷宫里（半径 3）D* Lite 展开 350 个节点，A* 只展开 33 个。在还几乎全是未知（当作空闲）的地图上，许多格子的 \(f\) 相同；D* Lite 的第二段键取 \(g\) 小的先处理，会把这些平局的格子都处理一遍，而这里的 A* 平局时取 \(g\) 大的。D* Lite 的优势要在之后多次重规划里才体现出来。

接下来：图搜索在二维、三维里无可替代，但到了机械臂的 6、7 维关节空间就撑不住了。14.2 节改为在 \(\mathcal C\) 里随机撒点、用碰撞检测连线（PRM、RRT），14.3 节把图搜索或采样得到的折线优化成平滑、可执行的轨迹，15.3 节讲规划器所用的代价地图从哪里来。