Part IV · 第 14 章运动规划 · 14.2

采样式规划：PRM、RRT、RRT*

一台七轴机械臂的构型空间是七维的，把它画成网格再跑 A* 根本存不下。采样式规划器换了一个思路：不去描绘自由空间，只在里面随机撒点，问碰撞检测器“这里撞不撞、这一段撞不撞”，再把自由的点连成树（RRT）或路网（PRM）。本节在浏览器里真跑这些算法：看 RRT 为什么天生偏爱未探索的区域（Voronoi 偏置），看双向 RRT-Connect 和多查询的 PRM 各自擅长什么，看 RRT* 怎样靠“重连”把路径越磨越短，看窄通道怎样拖垮均匀采样、“概率完备”到底保证了什么，最后在 7.2 节那台两连杆臂的构型空间里规划，并体会碰撞检测分辨率的代价。

约 40 分钟前置：7.2 构型空间 · 4.9 采样方法 · 14.1 图搜索与 A* 6 个交互

1为什么机器人需要它

让一台机械臂把杯子从桌上搬进柜子，而不碰到柜门、桌子和它自己。7.2 节说过，这就是在构型空间 $\mathcal C$ 里找一条从 $\mathbf q_{\text{start}}$ 到 $\mathbf q_{\text{goal}}$ 的曲线，并且整条曲线都待在自由空间 $\mathcal C_{\text{free}}$ 里。14.1 节的 A* 能在网格上找到最短路，但网格要先把 $\mathcal C$ 切成格子：两个关节、每个关节 180 格只有 $3.2\times10^4$ 个格子；六个关节就是 $180^6\approx3.4\times10^{13}$ 个（7.2 节算过），而且每个格子都要做一次碰撞检测才知道它是不是自由的。

更麻烦的是，$\mathcal C_{\text{free}}$ 没有显式的公式。我们手里只有一个碰撞检测器：给它一个构型，它把机器人的三维模型摆到那个姿态，和场景的网格做几何求交，回答“撞”或“不撞”。它对单个构型很快（微秒级），但只能逐点地问。

采样式规划器正是为这种处境设计的：只在随机采到的构型上调用碰撞检测器，用这些点和连接它们的短线段搭一张图，再在图上找路。它们放弃了“完备性”（无解时也能确定地报告无解），换来与维数关系不大的工程可行性。今天 MoveIt 给机械臂规划时默认用的就是这一族里的 RRT-Connect（通过 OMPL 库）。本节讲清楚这一族的三个代表：RRT、PRM 与 RRT*。

2直觉：朝随机点迈一步

快速扩展随机树（Rapidly-exploring Random Tree，RRT，LaValle 1998）的一次迭代只有三步：在整个空间里均匀地采一个随机点 $\mathbf q_{\text{rand}}$；找到树上离它最近的节点 $\mathbf q_{\text{near}}$；从 $\mathbf q_{\text{near}}$ 朝 $\mathbf q_{\text{rand}}$ 走一小步（最长 $\eta$），如果这段路不撞，就把终点 $\mathbf q_{\text{new}}$ 加到树上。就这些。

神奇之处在第二步。哪个节点会被选中去扩展？恰好是“离随机点最近”的那个，所以一个节点被选中的概率，等于空间里离它比离其他节点都近的那片区域（它的 Voronoi 单元）的面积占比。树内部的节点被邻居团团围住，单元很小；树边缘的节点面对整片未探索的空地，单元很大。于是 RRT 不需要任何“往外长”的指令，就会自动、偏执地冲向空白处。这叫 Voronoi 偏置。

为了看出这有多重要，下图把 RRT 和一棵“天真的随机树”并排生长：后者每次随便挑一个已有节点，朝随便一个方向走同样长的一步。两者都是“随机的树”，差别只在挑节点的方式。

FIG 1Voronoi 偏置：RRT 与天真随机树，同样的节点数

RRT

天真随机树

两棵树都从中心出发（页面加载和重置时先各长到 25 个节点），步长 $\eta=0.4$ m，空间是 12 m × 8 m 的空盒子。青色是树的边与节点，琥珀色的线段是最近一次扩展。打开“Voronoi 单元”后，每个节点的单元用琥珀色填充：越琥珀 = 单元越大；对 RRT 来说，这正是该节点下一次被扩展的概率（对天真树则不是：它对每个节点一视同仁，概率都是 $1/n$）。覆盖率是盒子里离最近节点不超过 0.5 m 的面积占比。

试一试

把两棵树都长到 300 个节点（按“运行”或“+50”）。比较覆盖率：RRT 约 77%，天真树只有约 13%，最远节点离根约 6.4 m 对 2.2 m（20 个随机种子的中位数）。

核心洞见

RRT 的探索偏好不是写进去的规则，而是“找最近邻”这个操作的副产品：大的 Voronoi 单元收到更多随机点。打开单元可以看到，在 20 到 100 个节点时，单元最大的那 10% 节点占了大约一半的面积（20 个种子的中位数在 47% 到 53% 之间），也就拿走了大约一半的扩展机会。随着树填满空间，单元越来越均匀，RRT 自然从“探索”过渡到“加密”。

3动手：RRT、目标偏置与 RRT-Connect

现在给空间放上障碍物，并给定起点 S 和终点 G。点机器人的构型就是它的位置，所以这里的工作空间就是构型空间，红色障碍物就是 C-障碍物（7.2 节）。每次新节点落到离 G 不到一步的地方，RRT 就试着直接连 G；连上就找到了路径：沿父指针从 G 回溯到 S。

两个常见的改进可以切换。目标偏置：以概率 $p_g$ 把随机点直接换成 G，让树时不时“朝目标冲一下”。RRT-Connect（Kuffner & LaValle 2000）：从 S 和 G 各长一棵树；每次迭代先让一棵树像 RRT 那样朝随机点迈一步，然后让另一棵树朝这个新节点一步接一步地贪心冲过去，直到连上或撞墙，然后两棵树交换角色。

FIG 2RRT 实验台：拖动 S 和 G，换场景、换算法

红色方块是障碍物；蓝色圆点 S 是起点，绿色圆点 G 是终点（都可拖动）。青色是从 S 长出的树，紫色是 RRT-Connect 里从 G 长出的树，琥珀色粗线是找到的路径。速度慢（每秒 20 次迭代以下）或单步时，图上会画出这一次迭代：琥珀色空心圆是随机点 $\mathbf q_{\text{rand}}$，青色大圆圈标出 $\mathbf q_{\text{near}}$，琥珀色实心点是 $\mathbf q_{\text{new}}$，红叉表示这一步撞了、被丢弃。“碰撞检测”数的是线段检测的次数（这里用精确的线段与矩形求交）。“最优”是用可见图（障碍物角点之间的最短路）精确算出的最短路径长度。右侧表格记录最近几次成功的运行。

“树林”场景、普通 RRT：把步长 $\eta$ 调到 0.2 或更小再求解。迭代次数大约是 $\eta=0.5$ 时的 2.5 倍（60 个种子的中位数：715 对 281）：步子小，树要多走很多步才能铺开。
“口袋”场景（S 在一个开口背对 G 的 U 形口袋里）：分别用目标偏置 $p_g\le0.05$ 和 $p_g\ge0.8$ 各解一次（S 留在口袋里）。偏置大反而慢得多：中位数约 690 对 2900 次迭代，因为朝 G 冲的那些步全撞在口袋底上。
还是“口袋”，换成 RRT-Connect 求解：中位数约 250 次迭代，不到普通 RRT 的一半。从 G 长出的紫色树在口袋外面等着，青色树一钻出口袋就被它“接住”。

值得注意的几点

(1) 路径是歪的：RRT 只管“找到一条”，不管好不好。树林场景里 RRT 路径的中位长度约为最优长度的 1.3 倍，这需要后处理（第 6 节的捷径化）或者 RRT*（第 4 节）。(2) 目标偏置是一把双刃剑：在开阔场景里，5% 到 20% 的偏置能少走一些迭代；当 G 被挡在墙后，偏置就是在浪费碰撞检测。(3) 迭代次数是随机变量，同样的设置换个种子可能差一倍以上；比较算法要看很多次运行的统计，而不是一次。

4另外三个视角：路网、最优性与窄通道

PRM：先建路网，再回答很多次查询

RRT 是单查询的：每换一对起点终点就重新长一棵树。如果环境不变（一个固定的工作单元、一张不变的地图），而查询要回答成百上千次，就值得先花力气建一张概率路网（Probabilistic Roadmap，PRM，Kavraki、Švestka、Latombe、Overmars 1996）：在 $\mathcal C_{\text{free}}$ 里撒 $N$ 个点（采到撞的点就扔掉，这就是 4.9 节的拒绝采样），把每两个距离不超过 $r$ 且连线不撞的点连起来。查询时只需把 S 和 G 接到附近的路网节点上，再在图上跑 14.1 节的 A*。

FIG 3PRM：路网的连通性，与几乎免费的查询

节点按路网的连通分量着色：与 S 相连的是绿色，与 G 相连（但不与 S 相连）的是紫色，其余分量用坐标轴的中性色。青色细线是路网的边，琥珀色是查询得到的路径。S 周围的虚线圆是连接半径 $r$。拖动 S 或 G 只会重新做查询（把它们接入路网并跑 A*），路网本身不变；改 $N$、$r$ 或场景会重建路网。

默认设置（树林，$N=60$，$r=1.5$）下路网碎成好几块，查询失败。反复按“+30 个节点”直到查询成功：通常到 150 个左右就够了（40 个种子里 98% 在 $N=150$ 时连通），这时建图大约做了 700 次线段检测。
路网连通后，把 S 或 G 拖到 3 个不同的地方（每次移动 2 m 以上），每次查询都成功：读数显示每次查询只需二三十次线段检测，路网照常复用。

RRT*：重连，让路径越来越短

RRT 找到第一条路以后，就算继续长下去，那条路也几乎不会变好：每个节点的父节点在它出生时就定了，永远不改。Karaman 与 Frazzoli（2011）证明了普通 RRT 以概率 1 收敛到一条非最优的路径，并提出了 RRT*：在新节点 $\mathbf q_{\text{new}}$ 周围半径 $r_n$ 的球里，(1) 挑一个“从起点经它到 $\mathbf q_{\text{new}}$ 最便宜”的节点当父节点；(2) 再反过来检查球里的每个邻居：如果“从起点经 $\mathbf q_{\text{new}}$ 到它”比它现在的路线更便宜，就把它的父节点改成 $\mathbf q_{\text{new}}$。这一步叫重连（rewire）。半径 $r_n$ 随节点数 $n$ 缓慢收缩（第 5 节），既保证邻居数大致按 $\log n$ 增长，又不让代价爆炸。

FIG 4RRT* 与 RRT 用同一串随机数赛跑：路径代价随迭代的变化

两个规划器用同一个种子、同一个步长 $\eta=1$ 同步运行，直到 5000 次迭代。左：所选的那棵树（青色）、琥珀色的 RRT* 当前最好路径、紫色虚线的 RRT 当前最好路径，以及绿色虚线的真正最短路（可见图）。右：两者的最好路径长度除以最短路长度，随迭代次数的变化（1 = 最优）。$\gamma$ 是 RRT* 半径公式里的常数，以 $\gamma^*$（第 5 节的阈值，树林场景约 8.6 m）为单位。

默认设置（树林，$\gamma=1.5\gamma^*$）跑到 2000 次迭代以上：RRT* 离最优不到 3%（50 个种子里最差的是 2.3%），而 RRT 的中位数停在约 34% 之上，最好的种子也高出 12%。
把 $\gamma$ 调到 $0.3\gamma^*$ 或更小，跑满 3000 次迭代：球太小，重连太少，代价仍然高出 5% 以上（50 个种子全部如此，中位数高出约 30%）。

窄通道与“概率完备”

采样式规划器的保证是概率完备：只要有解，采的点越多，找不到解的概率就越小，并趋于 0。但趋于 0 的速度取决于自由空间里最难的那段路有多“窄”：一个随机点落进窄通道的概率和通道的体积成正比（7.2 节的门洞）。下图是一堵 4 m 厚的墙，中间有一条宽 $w$ 的通道；PRM 不断加点，直到 S 和 G 连通为止，重复 60 次（固定的 60 个种子）。三种采样器都是一半节点来自均匀采样，另一半来自所选的采样器：高斯采样（Boor、Overmars、van der Stappen 1999）在一个随机点旁边按高斯分布再采一个点，两个点一个撞一个不撞时，保留不撞的那个，所以点会集中在障碍物表面附近；桥测试（Hsu 等 2003）采两个都撞的点，如果它们的中点不撞，就保留中点：这正是窄缝的特征。

FIG 5窄通道：失败概率随节点数指数下降，速度取决于通道宽度和采样器

左：最后一次试验的全部节点，青色来自均匀采样，琥珀色来自所选的采样器（连接半径 1.2 m，高斯与桥测试的偏移标准差 0.6 m）。右：“仍未连通的试验比例”对节点数，纵轴是对数刻度：一条直线就意味着指数衰减。曲线颜色：均匀为青色，高斯为紫色，桥测试为琥珀色。每次试验最多加 3000 个节点。读数里“点检测”是所有采样时问过碰撞检测器的点数，“边检测”把每条候选边按 0.1 m 的分辨率折算成点检测的次数。

把通道宽度调到 0.2 m 或更窄，用“均匀”跑 60 次试验：中位数要 800 个以上的节点才连通（$w=0.2$ 时为 820）。
同样的宽度，再用“高斯”跑 60 次：所需节点数的中位数不到均匀采样的一半（$w=0.2$ 时为 273），曲线也陡得多。

代价只是被挪了地方

再试试桥测试：在 $w=0.2$ 时它只要约 209 个节点，比高斯还少，但为了找到这些节点问了碰撞检测器约 3.1 万个点，是均匀采样（约 1200 个）的 25 倍：大多数“两个都撞”的点对的中点也在墙里。哪个采样器划算，取决于点检测和边检测哪个更贵、以及维数：节点越少，边检测越少、图越小、查询越快；在高维构型空间里，窄通道的体积占比小得多，桥测试这类“专门找窄缝”的采样器才真正值得（Hsu 等人的实验就是在高维的机械臂问题上做的）。

5再看数学

提示：把鼠标悬停在公式中带底色的符号上，图中对应的元素会被高亮（FIG 1 的 Voronoi 单元，FIG 2 的随机点、最近节点、步长圆和新节点，FIG 3 的连接半径，FIG 4 的近邻球与重连的边，FIG 5 的失败曲线，FIG 6 的检测点与 C-障碍物）。

问题与唯一的“预言机”

给定 $\mathcal C_{\text{free}}=\mathcal C\setminus\hl{cobs}{\mathcal C_{\text{obs}}}$（7.2 节）、起点 $\mathbf q_s$ 与终点 $\mathbf q_g$，求连续路径 $\sigma:[0,1]\to\mathcal C_{\text{free}}$，$\sigma(0)=\mathbf q_s$，$\sigma(1)=\mathbf q_g$。若还给定代价（本节都用路径长度 $c(\sigma)=\int\|\dot\sigma\|\,dt$），就是最优规划。采样式规划器对 $\mathcal C_{\text{obs}}$ 一无所知，只能调用两个函数：

$$ \operatorname{Free}(\mathbf q)\in\{0,1\},\qquad \operatorname{FreeEdge}(\mathbf q,\mathbf q')=\prod_{k=1}^{m}\operatorname{Free}\!\Big(\mathbf q+\tfrac km(\mathbf q'-\mathbf q)\Big)\qquad $$ $$ m=\Big\lceil\frac{\|\mathbf q'-\mathbf q\|}{\hl{delta}{\delta}}\Big\rceil $$

第二个函数就是“沿线段每隔 $\delta$ 检测一次”的离散化边检测（OMPL 的默认做法；FIG 2 到 FIG 5 的点机器人可以对矩形做精确求交，FIG 6 的手臂就用这个）。对角度坐标，“线段”和“距离”都要在环面上取最短的那条（7.2 节的 $\operatorname{wrap}$）。

RRT 与它的 Voronoi 偏置

$\mathcal T\leftarrow\{\mathbf q_s\}$
循环： $\hl{rand}{\mathbf q_{\text{rand}}}\sim\operatorname{Uniform}(\mathcal C)$ （以概率 $p_g$ 取 $\mathbf q_g$）
$\hl{near}{\mathbf q_{\text{near}}}=\arg\min_{\mathbf q\in\mathcal T}\|\mathbf q-\mathbf q_{\text{rand}}\|$
$\hl{new}{\mathbf q_{\text{new}}}=\mathbf q_{\text{near}}+\min\!\big(\hl{eta}{\eta},\|\mathbf q_{\text{rand}}-\mathbf q_{\text{near}}\|\big)\,\dfrac{\mathbf q_{\text{rand}}-\mathbf q_{\text{near}}}{\|\mathbf q_{\text{rand}}-\mathbf q_{\text{near}}\|}$
若 $\operatorname{FreeEdge}(\mathbf q_{\text{near}},\mathbf q_{\text{new}})$：把 $\mathbf q_{\text{new}}$ 加入 $\mathcal T$，父节点为 $\mathbf q_{\text{near}}$；若它离 $\mathbf q_g$ 不到 $\eta$ 且能直连，返回路径

第 3 行决定了 Voronoi 偏置。节点 $\mathbf v$ 的 Voronoi 单元 $\operatorname{Vor}(\mathbf v)=\{\mathbf x:\|\mathbf x-\mathbf v\|\le\|\mathbf x-\mathbf u\|\ \forall\mathbf u\in\mathcal T\}$ 恰好是“使 $\mathbf v$ 成为最近节点”的随机点的集合，所以在均匀采样下，$\mathbf v$ 被扩展的概率 $P_{\text{ext}}(\mathbf v)$ 是（$\mu$ 表示面积或体积）

$$ P_{\text{ext}}(\mathbf v)=\frac{\mu\big(\hl{vor}{\operatorname{Vor}(\mathbf v)}\big)}{\mu(\mathcal C)} $$

天真随机树的同一个概率是 $1/n$，与几何无关，所以它的新节点大多长在已经很密的根部附近（FIG 1）。RRT-Connect 再加一层贪心：另一棵树对着新节点反复调用第 4、5 行，直到撞墙或到达。它仍然概率完备，在没有窄通道的场景里通常快很多。最近邻查询是 RRT 的另一大开销：线性扫描是 $O(n)$，大规模时要用 k-d 树或 GNAT 这类结构（OMPL 在度量空间里默认用 GNAT），而且距离函数必须和构型空间的拓扑一致（7.2 节的度量）。

PRM 与概率完备性

PRM 的路网 $G=(V,E)$ 由 $n$ 个自由样本和所有满足 $\|\mathbf q-\mathbf q'\|\le\hl{rad}{r}$ 且 $\operatorname{FreeEdge}(\mathbf q,\mathbf q')$ 的边组成。为什么撒的点越多越可靠？设存在一条长度为 $L$ 的解路径，它离障碍物的距离处处至少为 $\rho$（间隙），连接半径 $r\ge3\rho/2$。沿路径放 $M=\lceil 2L/\rho\rceil$ 个半径 $\rho/2$ 的小球，相邻球心相距不超过 $\rho/2$。如果每个小球里都至少有一个样本，那么相邻小球里的两个样本相距不超过 $3\rho/2$，它们之间线段上的每一点离路径都不超过 $3\rho/4\lt\rho$，所以这条线段不撞；只要 $r\ge3\rho/2$，它就会被连上，于是路网里有一条从 S 到 G 的路。这是 Kavraki、Kolountzakis、Latombe（1998）分析的思路：沿路径放半径 $\rho/2$ 的小球并取并集界；他们的路网尝试连接任意两点，这里加上半径条件 $r\ge3\rho/2$，是为了套用到只连近邻的 PRM 上。记 $P_{\text{fail}}(n)$ 为撒了 $n$ 个样本后路网仍找不到路的概率。单个小球是空的概率是 $\big(1-\tfrac{\zeta_d(\rho/2)^d}{\mu(\mathcal C_{\text{free}})}\big)^n\le e^{-\sigma\rho^dn}$，对 $M$ 个小球取并集界：

$$ \hl{pfail}{P_{\text{fail}}(n)}\ \le\ \Big\lceil\frac{2L}{\rho}\Big\rceil\,e^{-\sigma\rho^d n},\qquad \sigma=\frac{\zeta_d}{2^d\,\mu(\mathcal C_{\text{free}})} $$

其中 $\zeta_d$ 是 $d$ 维单位球的体积（$\zeta_2=\pi$）。三点解读：(1) 失败概率随 $n$ 指数下降，这就是 FIG 5 里对数坐标下的直线；(2) 衰减速率正比于 $\rho^d$：通道宽度减半，在 $d$ 维里速率要除以 $2^d$，维数越高窄通道越致命；(3) 这是“有解时”的保证。如果无解，PRM 和 RRT 都会永远跑下去，它们不能证明无解。所以说它们是概率完备的，而不是完备的。RRT 同样概率完备（LaValle 与 Kuffner 2001）；几何情形下带指数速率的严格证明由 Kleinbort 等人（2019）给出。

FIG 5 的数字可以对上这个直觉。通道体积 $4w$，自由空间面积 $96-32+4w$，$w=0.2$ 时一个均匀样本落进通道的概率只有 $0.8/64.8\approx1.2\%$；而墙厚 4 m、连接半径 1.2 m，路网至少要在通道里串起好几个节点，这就是均匀采样要 800 多个节点的原因。

RRT*：选父节点、重连与收缩的半径

设 $c(\mathbf q)$ 是树上从起点到 $\mathbf q$ 的路径长度，$X_{\text{near}}=\{\mathbf q\in\mathcal T:\|\mathbf q-\mathbf q_{\text{new}}\|\le r_n\}$。RRT* 在 RRT 的第 5 行做两件事：

$$ \operatorname{parent}(\mathbf q_{\text{new}})=\arg\min_{\mathbf q\in X_{\text{near}},\ \operatorname{FreeEdge}(\mathbf q,\mathbf q_{\text{new}})}\Big(c(\mathbf q)+\|\mathbf q-\mathbf q_{\text{new}}\|\Big) $$ $$ \hl{rewire}{\forall\,\mathbf q\in X_{\text{near}}:\ \ c(\mathbf q_{\text{new}})+\|\mathbf q_{\text{new}}-\mathbf q\|\lt c(\mathbf q)\ \wedge\ \operatorname{FreeEdge}(\mathbf q_{\text{new}},\mathbf q)\ \Rightarrow\ \operatorname{parent}(\mathbf q)\leftarrow\mathbf q_{\text{new}}} $$ $$ \hl{ball}{r_n}=\min\Big\{\gamma\Big(\frac{\log n}{n}\Big)^{1/d},\ \eta\Big\},\qquad \gamma>\gamma^*=\Big(2\Big(1+\frac1d\Big)\Big)^{1/d}\Big(\frac{\mu(\mathcal C_{\text{free}})}{\zeta_d}\Big)^{1/d} $$

重连之后，被改了父节点的那个节点的整棵子树的代价都下降了，实现上要把新代价沿子树往下传。半径的形状有清楚的道理：$n$ 个均匀点在 $d$ 维里平均间距 $\sim n^{-1/d}$；乘上 $(\log n)^{1/d}$ 让球里的期望邻居数按 $\log n$ 增长，正好够把“相邻样本”连成越来越好的近似最优路径（这一点和随机几何图的连通性阈值有关），而每次迭代的工作量只多一个对数因子。Karaman 与 Frazzoli（2011）证明，$\gamma>\gamma^*$ 时 RRT* 渐近最优：最好路径的代价以概率 1 收敛到最优值。Solovey 等人（2020）后来发现原证明有一处漏洞，他们补上的严格证明要求稍大的半径，按 $(\log n/n)^{1/(d+1)}$ 收缩；所以实践里把 $\gamma$ 取得宽松一些。FIG 4 的树林场景 $\mu(\mathcal C_{\text{free}})\approx 77.2\ \text{m}^2$，$\gamma^*=\sqrt3\,\sqrt{77.2/\pi}\approx8.6$ m；FIG 4 默认取 $\gamma=1.5\gamma^*$，树上有 $n=1000$ 个节点时 $1.5\gamma^*\sqrt{\ln n/n}\approx1.07$ m，略大于 $\eta=1$ 而被截到 1，$n=3000$ 时降到约 0.67 m。

碰撞检测：真正的开销

在真实的机械臂规划里，绝大部分时间花在碰撞检测上（网格对网格的求交、距离查询），而不是采样或最近邻。离散化边检测的次数是 $m=\lceil\|\Delta\mathbf q\|/\delta\rceil$，所以 $\delta$ 减半，边检测的代价就翻倍。但 $\delta$ 太大会漏检：两个被检测的构型之间，机器人可能扫过一根细杆。对 FIG 6 的平面两连杆臂，关节角变化 $\Delta\boldsymbol\theta$ 时，手臂上任何一点的位移不超过

$$ \|\Delta\mathbf p\|\ \le\ (L_1+L_2)\,|\Delta\theta_1|+L_2\,|\Delta\theta_2|\ \le\ \sqrt{(L_1+L_2)^2+L_2^2}\ \|\Delta\boldsymbol\theta\|\approx1.97\,\|\Delta\boldsymbol\theta\|\ \text{m} $$

（第一项：绕基座转动时，离基座最远的点到基座不超过 $L_1+L_2$；第二项：绕肘部转动只牵动第二根连杆；第二个不等号是柯西-施瓦茨。）于是只要每个检测点的间隙（到障碍物的距离）都大于 $1.97\,\delta$，两点之间的整段运动就一定不撞：这是“保守的”边检测，FCL 这类库提供距离查询和连续碰撞检测，就是为了做这件事。FIG 6 的细杆加上连杆的半径只有 0.21 m 粗；$\delta=0.3$ rad 时两次检测之间手爪最多移动约 0.59 m，足够整个跨过去。

6机器人中的它：给一台两连杆臂规划

回到 7.2 节那台平面两连杆臂（$L_1=1$ m，$L_2=0.8$ m，连杆半径 0.045 m）。它的构型空间是环面 $S^1\times S^1$：右边的方块左右边、上下边是粘在一起的。规划器只会调用碰撞检测器：右图里的红色 C-障碍物是为了让你看见而另外算出来的（每个格子测一次），规划器从来没看过这张图。这里用的是 RRT-Connect，步长 $\eta=0.3$ rad，距离和插值都按环面取最短的方向（所以树枝和路径可以穿过方块的边界，从对面出来）。

规划完的原始路径还要“捷径化”（shortcutting）：反复随机挑路径上的两个点，如果它们之间的直线可以通过检测，就把中间的弯路剪掉。这是 OMPL 和 MoveIt 在交出路径前都会做的简化步骤。之后要么直接做时间参数化，要么先把它按弧长重采样成均匀间隔的航点，作为轨迹优化的初值，让 CHOMP 或 TrajOpt 修成光滑、留有余量的曲线，再做时间参数化（14.3 节；6.13 节讲过几何路径为什么还不能直接执行）。

FIG 6两连杆臂：在环面上规划，检测分辨率决定会不会“穿模”

工作空间：拖动圆盘

构型空间 (θ₁, θ₂)：拖动 S、G

左：手臂与四个圆盘障碍物（红色，可拖动，其中一根是细杆）。手臂在 S（蓝色虚影）与 G（绿色虚影）两个构型之间，琥珀色实体是沿路径播放时的当前姿态。右：环面构型空间，红色是 C-障碍物；青色和紫色是从 S 和 G 长出的两棵树，琥珀色是路径（捷径化之后变短）。每次按“规划”都换一个随机种子。规划完成后，页面会用 0.005 rad 的分辨率把路径重新检查一遍：如果发现规划器漏检的碰撞，就在两张图上用红叉标出。

把检测分辨率 $\delta$ 调到 0.3 rad 或更大，反复按“规划”，直到复查发现漏检（每次约一半的机会；100 个种子里有 46 到 53 个）。按“播放”看手臂怎样直接穿过细杆。再把 $\delta$ 调回 0.05：100 个种子里一次漏检也没有，但碰撞检测次数从约 150 涨到约 490。
在没有漏检的情况下规划一次（$\delta\le0.1$），再按“捷径化”：构型空间里的路径长度缩短 10% 以上（100 个种子的中位数缩短 18%）。

真实系统里

OMPL 与 MoveIt：开放运动规划库 OMPL（Şucan、Moll、Kavraki 2012）实现了几十种采样式规划器（PRM、PRM*、RRT、RRT-Connect、RRT*、Informed RRT*、BIT*、FMT*、KPIECE、EST 等），它刻意不包含碰撞检测和机器人模型，只要求使用者提供“状态是否有效”的函数，正是第 5 节的预言机模式。MoveIt 把 OMPL 作为默认规划插件，默认规划器是 RRT-Connect；碰撞检测默认用 FCL（Pan、Chitta、Manocha 2012）。边检测的分辨率在 MoveIt 里是 longest_valid_segment_fraction 参数（状态空间尺度的一个比例），也就是 FIG 6 里的 $\delta$。规划出的路径先经 OMPL 的路径简化（捷径化等），再由时间参数化（如 Kunz 与 Stilman 的时间最优轨迹生成）配上速度和加速度。速度：Thomason、Kingston 与 Kavraki（2024）的 VAMP 用 SIMD 向量化一次检测许多构型，据作者报告，7 自由度 Franka Panda 机械臂上的 RRT-Connect 在 MotionBenchMaker 基准问题上，单个 CPU 核的中位规划时间只有几十微秒。 移动机器人与汽车：车有非完整约束（9.1 节），“朝随机点走一步”要换成“用一段可行的控制量前推”，这就是动力学 RRT（LaValle 与 Kuffner 2001）；MIT 在 2007 年 DARPA 城市挑战赛中的无人车用的就是闭环 RRT（Kuwata 等 2009）。地面导航里更常见的是 14.1 节的 Hybrid A* 和状态格规划。 与优化的分工：采样式规划负责“全局地找到一条”，轨迹优化（14.3 节的 CHOMP、TrajOpt）负责“局部地把它变好”，它们常常串联使用：捷径化后的路径按弧长重采样成航点，作为优化器的初值；TAMP（14.4 节）则把采样式规划当作底层的可行性检查。

7常见误区

误区 1：把“超时没找到”当成“无解”

概率完备只保证“有解时迟早找到”。超时可能是真的无解（起点和终点在 $\mathcal C_{\text{free}}$ 的不同连通分量里，7.2 节），也可能只是一条窄通道还没被采到（FIG 5 里均匀采样在 $w=0.2$ 时 90% 分位要上千个节点）。上层逻辑（比如 TAMP 的回溯，14.4 节）应该把“规划失败”当成带不确定性的信号：换种子重试、加长时限、换采样器，而不是立刻断定几何上不可行。

误区 2：边检测分辨率太粗

只检测节点、或者沿边检测得太稀，规划出的路径会“穿模”（FIG 6）。危险在于它在仿真的可视化里常常看不出来：路点本身都不撞。要么按第 5 节的界选 $\delta$（最大位移 $\le$ 最小间隙），要么用连续碰撞检测或带安全裕度的距离查询；把细小物体膨胀一圈也是常见的保险。反过来，$\delta$ 太细会让碰撞检测的次数成倍增长，规划变慢。

误区 3：距离函数和构型空间对不上

最近邻、插值和 RRT* 的代价都依赖距离。对转动关节直接相减，会让 $\theta=3.1$ 和 $\theta=-3.1$ 看起来相距 6.2 rad（7.2 节），树会绕远路，甚至让关节整整转一圈；把米和弧度直接放进同一个欧氏距离里（移动底座加机械臂），会让一个方向在最近邻里占绝对主导。正确做法是按拓扑处理角度，并为不同单位的分量设权重。

误区 4：直接执行 RRT 的原始路径

原始路径是一串折线，弯弯绕绕（FIG 2 里约为最优长度的 1.3 倍），在拐点处速度不连续。机器人照着它走会抖、慢，甚至超出加速度限制。至少要做捷径化（FIG 6），再做平滑或轨迹优化（14.3 节）与时间参数化。需要高质量路径时，用 RRT*、Informed RRT* 或 BIT* 这类渐近最优的规划器，并给足时间。

误区 5：参数照抄，不看场景

目标偏置在开阔场景里有帮助，在“口袋”里有害（FIG 2）；RRT* 的半径常数取得太小，就不再渐近最优，表现和 RRT 差不多（FIG 4，$\gamma=0.3\gamma^*$）；步长 $\eta$ 太小会让树长得很慢，太大则每次扩展的边检测更贵、也更容易被挡。这些参数都应该按场景的尺度和维数设定，并用多个种子的统计来调，而不是看一次运行。

8小练习

1. 在没有障碍物的空间里，一棵 RRT 有 100 个节点。下一次迭代最可能扩展哪个节点？

节点被扩展当且仅当随机点落在它的 Voronoi 单元里，所以概率等于单元面积除以总面积（第 5 节）。边缘节点面对大片空地，单元最大。选项 B 描述的是天真随机树（FIG 1 右图），它因此在根部附近堆成一团。

2. 一个 3 维构型空间里有一条窄通道，它在其中 2 个方向上很窄。把通道在这两个方向上的宽度都减半，一个均匀样本落进通道的概率大约变为原来的多少？

通道体积 = 长度 × 宽 × 宽，两个窄方向各减半，体积变为 $1/2\times1/2=1/4$，落入概率也变为 1/4，需要的样本数大约变成 4 倍。窄的方向越多，这个因子越大：这就是窄通道在高维里格外难的原因（第 5 节的 $\rho^d$）。

3. RRT 找到第一条路径后继续运行 10 倍的时间（不做任何后处理）。最可能发生什么？

RRT 的父节点在节点出生时就定了，新节点不会改变已有的路线；Karaman 与 Frazzoli 证明 RRT 以概率 1 收敛到非最优解。FIG 4 里 RRT 在第 2000 到 5000 次迭代之间几乎是一条水平线（中位数高出最优约 34%）。要渐近最优需要 RRT* 的重连。

4. 一个固定的装配工作站里，场景从不改变，机械臂每天要在几千对不同的姿态之间规划。更合适的是？

这正是 PRM 的设计用途：多查询、静态环境。建图的成本（FIG 3 里约 700 次线段检测）摊到成千上万次查询上，每次查询只要几十次检测。两者都概率完备（B 错）；6 维网格在每维分辨率稍高时就大得不可行（C 错）。如果场景每次都变，单查询的 RRT-Connect 更合适。

5. FIG 6 的手臂（$L_1=1$ m，$L_2=0.8$ m）用 $\delta=0.3$ rad 做边检测。场景里有一根加上连杆半径后直径约 0.21 m 的细杆。下面哪个说法对？

由第 5 节的界，$\|\Delta\mathbf p\|\le1.97\times0.3\approx0.59$ m，远大于 0.21 m。FIG 6 里这样的设置约一半的规划会漏检。要么把 $\delta$ 降到使最大位移小于间隙，要么用连续碰撞检测。增大 $\eta$ 只会让边更长，于情无补。

9延伸阅读

S. M. LaValle, Planning Algorithms, Cambridge University Press, 2006, Ch. 5 — 采样式规划的系统讲解：采样、碰撞检测、增量式与路网方法
H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L. E. Kavraki & S. Thrun, Principles of Robot Motion, MIT Press, 2005, Ch. 7 — PRM、RRT 及其分析
L. E. Kavraki, P. Švestka, J.-C. Latombe & M. H. Overmars, Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces, IEEE Transactions on Robotics and Automation, 1996 — PRM 的原始论文
L. E. Kavraki, M. N. Kolountzakis & J.-C. Latombe, Analysis of Probabilistic Roadmaps for Path Planning, IEEE Transactions on Robotics and Automation, 1998 — 第 5 节失败概率界的来源
S. M. LaValle, Rapidly-Exploring Random Trees: A New Tool for Path Planning, Technical Report TR 98-11, Iowa State University, 1998 — RRT 的提出
J. J. Kuffner & S. M. LaValle, RRT-Connect: An Efficient Approach to Single-Query Path Planning, ICRA 2000 — 双向树与贪心连接
S. M. LaValle & J. J. Kuffner, Randomized Kinodynamic Planning, International Journal of Robotics Research, 2001 — 带动力学约束的 RRT
S. Karaman & E. Frazzoli, Sampling-Based Algorithms for Optimal Motion Planning, International Journal of Robotics Research, 2011 — RRT*、PRM* 与渐近最优性
D. Hsu, T. Jiang, J. Reif & Z. Sun, The Bridge Test for Sampling Narrow Passages with Probabilistic Roadmap Planners, ICRA 2003 — 桥测试
J. D. Gammell, S. S. Srinivasa & T. D. Barfoot, Informed RRT*, IROS 2014 — 找到第一条解之后只在可能改进的椭球里采样
I. A. Şucan, M. Moll & L. E. Kavraki, The Open Motion Planning Library, IEEE Robotics & Automation Magazine, 2012 — OMPL 的设计