在网格上寻找最短路径

把自由空间变成一张图

在机器人规划路线之前，它需要一种方式来描述「自己可能去到的所有位置」。一个清晰的做法是给世界铺上一张网格，把地面切成一个个方格。每个方格要么是空的（机器人可以站在那里），要么被障碍物挡住——这种把自由空间与障碍空间区分开来的划分，是规划的基础。空的方格就成了我们被允许造访的地点。

现在，把每个空方格与它相邻的空方格连起来——也就是正北、正南、正东、正西的方格（通常还包括四个对角方格）。每一条连接都是一条你可以行走的「边」，每条边都有一个代价：通常就是跨过它所走的距离。你所搭建出来的，就是一张图——一堆由线（移动）连接起来的点（方格）。此时，寻找路线就和在地铁线路图上规划行程完全一样：从一站跳到下一站，直到抵达你的目的站。

这种重新表述之所以强大，是因为计算机科学家已经研究图搜索数十年了。一旦世界变成一张图，我们就能借用那些经过充分研究、可证明正确的算法，而不必发明某种脆弱的东西。网格还悄悄地照顾到了机器人的身体：只要把机器人放不进去的方格都标记为被占据，搜索就只会返回那些通过碰撞检测的路径。

Dijkstra：均匀蔓延的洪水

Dijkstra 算法是寻找最短路线时那种耐心而仔细的方法。想象把水倒在起点方格上。水向四周蔓延，总是优先填满最近的、还干着的方格。因为它总是从「到达代价最低的边界」开始扩张，所以当洪水触及终点的那一刻，你就知道自己是沿着可能的最短路线抵达的——没有任何更便宜的走法能更早到达那里。

给每个方格一个「从起点到此的最低代价」的累计值。起点为 0，其余所有方格初始为无穷大（尚未到达）。
反复挑出累计值最小的、尚未定下来的方格——也就是最便宜的边界——并将它「定下来」；它的代价从此确定。
对每个邻居，检查「经由当前方格到达它」是否比它现有的累计值更便宜。如果是，就调低它的累计值，并记住「你是从当前方格过去的」。
当终点被定下来时停止。然后从终点沿着「我是从哪过来的」链接一路倒着走回起点，就能读出整条路径。

A*：有方向感的 Dijkstra

Dijkstra 的「盲目」——哪怕是背离终点的方向也均匀蔓延——在你已经大致知道终点在哪里时就显得很浪费。A* 搜索通过给洪水一种方向感来解决这个问题。对每个方格，它额外加上一个启发值：对「还剩多少距离才到终点」的一个廉价猜测，通常就是直线（「乌鸦飞过去」）的距离。

Dijkstra 仅按「已花费的代价」给方格排序，而 A* 则按「已花费的代价」加上「对剩余代价的启发式猜测」来排序。用一条简短的公式来说，它按 f = g + h 给每个方格打分：g 是从起点已经真实走过的距离，h 是估计还剩下的距离。朝着终点方向的方格 f 值更小，会被优先探索，于是搜索就强烈地偏向正确的方向，把身后大半张地图都忽略掉。

priority of a cell  f(cell) = g(cell) + h(cell)
  g(cell) = real cost from start to this cell (known)
  h(cell) = guessed cost from this cell to goal (estimate)

Dijkstra is just A* with h = 0 (no sense of direction).

A* 按 f = g + h 给方格排序；把 h 设为零，它就退回成 Dijkstra。

从课本网格到机器人地图

真实的机器人不会被人递上一张整齐的网格——它要从自己的传感器里把网格建出来。常见的产物是一张占据栅格地图：一张方格网，每个格子的数值代表机器人对「这个格子被占据、空闲，还是未知」的信念。这张信念地图，正是 Dijkstra 与 A* 运行所需的那张「空闲对被占据」的图景。

在规划之前，机器人通常会把占据栅格转换成一张代价地图，其中每个格子携带的不只是「是否被占据」，而是「待在那里」的分级代价。紧贴障碍物的格子即便严格来说是空闲的，也会被「膨胀」成高代价，从而促使规划器留出舒适的余量，而不是擦着墙走。把这张代价地图交给 A*，你得到的路线既短，又明智地远离危险。

在实践中，运行于代价地图之上的 A*，就是机器人导航栈里那个经典的全局规划器。它在整张地图上勾勒出大方向的路线；随后由一个独立的、更快的局部规划器，把这条路线转化成平滑的、逐刻更新的运动指令，并避开那些在地图建好之后才冒出来的东西。这种全局规划器与局部规划器的分工，正是为什么一台配送机器人既能横穿一栋楼，又能绕开一辆经过的推车。

网格力不从心之处

网格很美妙，但它有两个不容回避的局限。第一，它会模糊掉精细的几何：一个格子要么开要么闭，所以一道恰好够机器人通过的门，如果正好跨在格子边界上就可能消失不见；而且除非事后做平滑，路径只能锁定在直角和 45 度的步子上。还要记住，网格规划的是空间中的一条路线——是路径，而非轨迹——把速度和时间留给后续阶段去填补。

第二个局限对机械臂来说是致命的。平坦的地面是一张二维网格；而一条有六个关节的机械臂，活在一个六维的位形空间里，那里的一个「格子」是六个关节角度的某一种组合。哪怕每个维度只切成 100 份，你也会得到 100^6——也就是一万亿——个格子。任何洪水，无论它的启发值多么聪明，都无法把一万亿个格子一一定下来。网格就这样直接爆炸了。

这种爆炸，正是高维规划器抛弃完整网格的原因。基于采样的规划不再造访每一个格子，而是把随机点撒进位形空间，只把那些被证明无碰撞的点连接起来——像快速扩展随机树（RRT）这样的方法，会朝着终点生长出一条路径，却从不把整个空间一一列举出来。一旦网格再也装不进内存，这就是顺理成章的下一步。