Part IV · 第 14 章运动规划 · 14.2Part IV · Ch. 14 Motion Planning · 14.2

采样式规划：PRM、RRT、RRTSampling-Based Planning: PRM, RRT, RRT

一台七轴机械臂的构型空间是七维的，把它画成网格再跑 A* 根本存不下。采样式规划器换了一个思路：不去描绘自由空间，只在里面随机撒点，问碰撞检测器“这里撞不撞、这一段撞不撞”，再把自由的点连成树（RRT）或路网（PRM）。本节在浏览器里真跑这些算法：看 RRT 为什么天生偏爱未探索的区域（Voronoi 偏置），看双向 RRT-Connect 和多查询的 PRM 各自擅长什么，看 RRT* 怎样靠“重连”把路径越磨越短，看窄通道怎样拖垮均匀采样、“概率完备”到底保证了什么，最后在 7.2 节那台两连杆臂的构型空间里规划，并体会碰撞检测分辨率的代价。 The configuration space of a seven-axis arm is seven-dimensional; drawing it as a grid and running A* on it does not even fit in memory. Sampling-based planners take another route: they never describe free space, they just throw random points into it, ask a collision checker "is this point free, is this segment free?", and link the free points into a tree (RRT) or a roadmap (PRM). This lesson runs those algorithms for real in your browser: why RRT is naturally drawn to unexplored space (the Voronoi bias), what bidirectional RRT-Connect and the multi-query PRM are each good at, how RRT* "rewires" its tree to grind the path shorter and shorter, how narrow passages defeat uniform sampling and what "probabilistically complete" actually promises, and finally planning in the configuration space of the two-link arm of Lesson 7.2, where the resolution of collision checking has a price.

约 40 分钟~40 min 前置：7.2 构型空间 · 4.9 采样方法 · 14.1 图搜索与 A*Prereqs: 7.2 Configuration space · 4.9 Sampling methods · 14.1 Graph search & A* 6 个交互6 interactives

1为什么机器人需要它Why robots need this

让一台机械臂把杯子从桌上搬进柜子，而不碰到柜门、桌子和它自己。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*。

Have an arm move a cup from the table into a cabinet without hitting the door, the table or itself. As Lesson 7.2 said, this means finding a curve in configuration space $\mathcal C$ from $\mathbf q_{\text{start}}$ to $\mathbf q_{\text{goal}}$ that stays in the free space $\mathcal C_{\text{free}}$ all the way. The A* of Lesson 14.1 finds shortest paths on a grid, but a grid must first chop $\mathcal C$ into cells: two joints at 180 cells each give only $3.2\times10^4$ cells; six joints give $180^6\approx3.4\times10^{13}$ (computed in 7.2), and every cell needs a collision check before we even know whether it is free.

Worse, $\mathcal C_{\text{free}}$ has no explicit formula. All we have is a collision checker: give it a configuration, it poses the robot's 3D model there, intersects it with the scene's geometry and answers "collides" or "free". It is fast for one configuration (microseconds), but it can only be asked point by point.

Sampling-based planners are designed for exactly this situation: call the collision checker only at randomly sampled configurations, build a graph from those points and short segments between them, and search the graph. They give up "completeness" (certainly reporting "no solution" when there is none) and in exchange stay practical in high dimensions. When MoveIt plans for an arm today, its default is RRT-Connect from this family (through the OMPL library). This lesson covers the three representatives: RRT, PRM and RRT*.

2直觉：朝随机点迈一步Intuition: take one step toward a random point

快速扩展随机树（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 和一棵“天真的随机树”并排生长：后者每次随便挑一个已有节点，朝随便一个方向走同样长的一步。两者都是“随机的树”，差别只在挑节点的方式。

One iteration of a Rapidly-exploring Random Tree (RRT, LaValle 1998) has three steps: sample a random point $\mathbf q_{\text{rand}}$ uniformly over the whole space; find the tree node $\mathbf q_{\text{near}}$ nearest to it; take a short step (at most $\eta$) from $\mathbf q_{\text{near}}$ toward $\mathbf q_{\text{rand}}$, and if that segment is collision-free, add its end $\mathbf q_{\text{new}}$ to the tree. That is all.

The magic is in step two. Which node gets extended? Exactly the one "nearest to the random point", so a node's chance of being picked equals the share of space that is closer to it than to any other node: the area of its Voronoi cell. Nodes inside the tree are hemmed in by neighbours and own small cells; nodes on the rim face the whole unexplored space and own large ones. So with no instruction to "grow outward", RRT rushes into empty space automatically and relentlessly. This is the Voronoi bias.

To see how much it matters, the figure grows RRT next to a "naive random tree", which picks any existing node at random and steps the same length in a random direction. Both are "random trees"; they differ only in how the node to extend is chosen.

FIG 1Voronoi 偏置：RRT 与天真随机树，同样的节点数The Voronoi bias: RRT versus a naive random tree, same number of nodes

RRT

天真随机树NAIVE RANDOM TREE

两棵树都从中心出发（页面加载和重置时先各长到 25 个节点），步长 $\eta=0.4$ m，空间是 12 m × 8 m 的空盒子。青色是树的边与节点，琥珀色的线段是最近一次扩展。打开“Voronoi 单元”后，每个节点的单元用琥珀色填充：越琥珀 = 单元越大；对 RRT 来说，这正是该节点下一次被扩展的概率（对天真树则不是：它对每个节点一视同仁，概率都是 $1/n$）。覆盖率是盒子里离最近节点不超过 0.5 m 的面积占比。 Both trees start at the centre (and are pre-grown to 25 nodes on load and reset) with step $\eta=0.4$ m in an empty 12 m × 8 m box. Cyan shows tree edges and nodes; the amber segment is the latest extension. With "Voronoi cells" on, each node's cell is filled with amber: more amber = larger cell, which for RRT is exactly the chance that this node is extended next (not for the naive tree, which treats every node alike: each has chance $1/n$). Coverage is the share of the box lying within 0.5 m of some node.

试一试Try this

把两棵树都长到 300 个节点（按“运行”或“+50”）。比较覆盖率：RRT 约 77%，天真树只有约 13%，最远节点离根约 6.4 m 对 2.2 m（20 个随机种子的中位数）。Grow both trees to 300 nodes (press "Run" or "+50"). Compare coverage: about 77% for RRT and only about 13% for the naive tree, with the farthest node about 6.4 m versus 2.2 m from the root (medians over 20 random seeds).

核心洞见Key insight

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

RRT's appetite for exploration is not a written rule but a by-product of the nearest-neighbour step: big Voronoi cells receive more random points. Turn the cells on and you can see that between 20 and 100 nodes, the 10% of nodes with the largest cells own about half the area (medians over 20 seeds between 47% and 53%), and therefore get about half of all extensions. As the tree fills the space the cells even out, and RRT shifts naturally from "exploring" to "refining".

3动手：RRT、目标偏置与 RRT-ConnectPlay: RRT, goal bias and RRT-Connect

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

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

Now put obstacles in the space and fix a start S and a goal G. A point robot's configuration is its position, so here the workspace is the configuration space and the red obstacles are the C-obstacles (Lesson 7.2). Whenever a new node lands within one step of G, RRT tries to connect to G directly; once it succeeds, the path is found by following parent pointers from G back to S.

Two common improvements can be switched on. Goal bias: with probability $p_g$ the random point is replaced by G itself, so the tree "lunges at the goal" now and then. RRT-Connect (Kuffner & LaValle 2000): grow one tree from S and one from G; in each iteration one tree takes an ordinary RRT step toward a random point, and then the other tree greedily steps toward that new node, again and again, until it connects or hits something; then the two trees swap roles.

FIG 2RRT 实验台：拖动 S 和 G，换场景、换算法The RRT bench: drag S and G, change the scene and the algorithm

红色方块是障碍物；蓝色圆点 S 是起点，绿色圆点 G 是终点（都可拖动）。青色是从 S 长出的树，紫色是 RRT-Connect 里从 G 长出的树，琥珀色粗线是找到的路径。速度慢（每秒 20 次迭代以下）或单步时，图上会画出这一次迭代：琥珀色空心圆是随机点 $\mathbf q_{\text{rand}}$，青色大圆圈标出 $\mathbf q_{\text{near}}$，琥珀色实心点是 $\mathbf q_{\text{new}}$，红叉表示这一步撞了、被丢弃。“碰撞检测”数的是线段检测的次数（这里用精确的线段与矩形求交）。“最优”是用可见图（障碍物角点之间的最短路）精确算出的最短路径长度。右侧表格记录最近几次成功的运行。 Red blocks are obstacles; the blue dot S is the start and the green dot G the goal (both draggable). Cyan is the tree grown from S, purple the tree grown from G in RRT-Connect, and the thick amber line is the path found. When running slowly (below 20 iterations per second) or stepping, the current iteration is drawn: the hollow amber circle is the random point $\mathbf q_{\text{rand}}$, the large cyan ring marks $\mathbf q_{\text{near}}$, the solid amber dot is $\mathbf q_{\text{new}}$, and a red cross means the step collided and was discarded. "Collision checks" counts segment checks (here an exact segment-rectangle intersection). "Optimal" is the exact shortest path length from the visibility graph (shortest paths through obstacle corners). The table on the right logs the latest successful runs.

“树林”场景、普通 RRT：把步长 $\eta$ 调到 0.2 或更小再求解。迭代次数大约是 $\eta=0.5$ 时的 2.5 倍（60 个种子的中位数：715 对 281）：步子小，树要多走很多步才能铺开。"Forest" scene, plain RRT: set the step $\eta$ to 0.2 or less and solve. It takes about 2.5 times the iterations of $\eta=0.5$ (medians over 60 seeds: 715 versus 281): with small steps the tree needs many more of them to spread out.
“口袋”场景（S 在一个开口背对 G 的 U 形口袋里）：分别用目标偏置 $p_g\le0.05$ 和 $p_g\ge0.8$ 各解一次（S 留在口袋里）。偏置大反而慢得多：中位数约 690 对 2900 次迭代，因为朝 G 冲的那些步全撞在口袋底上。"Pocket" scene (S sits in a U-shaped pocket whose opening faces away from G): solve once with goal bias $p_g\le0.05$ and once with $p_g\ge0.8$ (keep S inside the pocket). The strong bias is much slower: medians of about 690 versus 2900 iterations, because every lunge at G hits the back of the pocket.
还是“口袋”，换成 RRT-Connect 求解：中位数约 250 次迭代，不到普通 RRT 的一半。从 G 长出的紫色树在口袋外面等着，青色树一钻出口袋就被它“接住”。Still in the "pocket", switch to RRT-Connect and solve: a median of about 250 iterations, less than half of plain RRT. The purple tree from G waits outside, and the moment the cyan tree crawls out of the pocket it gets "caught".

值得注意的几点Things worth noticing

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

(1) The paths are crooked: RRT only cares about finding a path, not a good one. In the forest the median RRT path is about 1.3 times the optimal length, which calls for post-processing (shortcutting, Section 6) or RRT* (Section 4). (2) Goal bias cuts both ways: in open scenes a 5% to 20% bias saves some iterations; when G is behind a wall, the bias wastes collision checks. (3) Iteration counts are random variables; the same settings with another seed can differ by a factor of two or more, so compare algorithms by statistics over many runs, never by one.

4另外三个视角：路网、最优性与窄通道Three more views: roadmaps, optimality and narrow passages

PRM：先建路网，再回答很多次查询PRM: build a roadmap once, answer many queries

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

RRT is single-query: every new start-goal pair grows a new tree. If the environment does not change (a fixed work cell, a static map) and queries come by the hundreds or thousands, it pays to build a probabilistic roadmap first (PRM; Kavraki, Švestka, Latombe & Overmars 1996): scatter $N$ points in $\mathcal C_{\text{free}}$ (discarding samples that collide, which is the rejection sampling of Lesson 4.9), and link every two points that are at most $r$ apart and see each other without collision. A query then just hooks S and G onto nearby roadmap nodes and runs the A* of Lesson 14.1 on the graph.

FIG 3PRM：路网的连通性，与几乎免费的查询PRM: roadmap connectivity, and queries that are almost free

节点按路网的连通分量着色：与 S 相连的是绿色，与 G 相连（但不与 S 相连）的是紫色，其余分量用坐标轴的中性色。青色细线是路网的边，琥珀色是查询得到的路径。S 周围的虚线圆是连接半径 $r$。拖动 S 或 G 只会重新做查询（把它们接入路网并跑 A*），路网本身不变；改 $N$、$r$ 或场景会重建路网。 Nodes are coloured by connected component of the roadmap: green for the one containing S, purple for the one containing G (when it is not S's), and the neutral axis colour for the rest. Thin cyan lines are roadmap edges, amber is the query's path. The dashed circle around S is the connection radius $r$. Dragging S or G only re-runs the query (hook them in, run A*); the roadmap stays as it is. Changing $N$, $r$ or the scene rebuilds it.

默认设置（树林，$N=60$，$r=1.5$）下路网碎成好几块，查询失败。反复按“+30 个节点”直到查询成功：通常到 150 个左右就够了（40 个种子里 98% 在 $N=150$ 时连通），这时建图大约做了 700 次线段检测。With the defaults (forest, $N=60$, $r=1.5$) the roadmap falls into several pieces and the query fails. Press "+30 nodes" until the query succeeds: around 150 nodes usually suffice (98% of 40 seeds connect at $N=150$), by which point building took about 700 segment checks.
路网连通后，把 S 或 G 拖到 3 个不同的地方（每次移动 2 m 以上），每次查询都成功：读数显示每次查询只需二三十次线段检测，路网照常复用。Once the roadmap is connected, drag S or G to 3 different places (more than 2 m each time) with every query succeeding: the readout shows that each query needs only twenty or thirty segment checks, while the roadmap is reused as is.

RRT：重连，让路径越来越短RRT: rewire, and the path keeps getting shorter

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$ 增长，又不让代价爆炸。

Once RRT finds its first path, growing further hardly ever improves it: each node's parent is fixed at birth and never revised. Karaman & Frazzoli (2011) proved that plain RRT converges with probability one to a non-optimal path, and proposed RRT*: in the ball of radius $r_n$ around a new node $\mathbf q_{\text{new}}$, (1) choose as parent the node that gives the cheapest route from the start through it to $\mathbf q_{\text{new}}$; (2) then check every neighbour in the ball the other way round: if going from the start via $\mathbf q_{\text{new}}$ to it is cheaper than its current route, change its parent to $\mathbf q_{\text{new}}$. That step is the rewiring. The radius $r_n$ shrinks slowly with the node count $n$ (Section 5), so the number of neighbours grows roughly like $\log n$ and the cost does not explode.

FIG 4RRT* 与 RRT 用同一串随机数赛跑：路径代价随迭代的变化RRT* and RRT race on the same random numbers: path cost versus iterations

两个规划器用同一个种子、同一个步长 $\eta=1$ 同步运行，直到 5000 次迭代。左：所选的那棵树（青色）、琥珀色的 RRT* 当前最好路径、紫色虚线的 RRT 当前最好路径，以及绿色虚线的真正最短路（可见图）。右：两者的最好路径长度除以最短路长度，随迭代次数的变化（1 = 最优）。$\gamma$ 是 RRT* 半径公式里的常数，以 $\gamma^*$（第 5 节的阈值，树林场景约 8.6 m）为单位。 Both planners run in lockstep with the same seed and the same step $\eta=1$, up to 5000 iterations. Left: the selected tree (cyan), the amber best RRT* path so far, the dashed purple best RRT path so far, and the dashed green true shortest path (visibility graph). Right: each planner's best path length divided by the shortest length, against iterations (1 = optimal). $\gamma$ is the constant in the RRT* radius formula, in units of $\gamma^*$ (the threshold of Section 5, about 8.6 m for the forest).

默认设置（树林，$\gamma=1.5\gamma^*$）跑到 2000 次迭代以上：RRT* 离最优不到 3%（50 个种子里最差的是 2.3%），而 RRT 的中位数停在约 34% 之上，最好的种子也高出 12%。With the defaults (forest, $\gamma=1.5\gamma^*$) run past 2000 iterations: RRT* comes within 3% of optimal (the worst of 50 seeds is at 2.3%), while RRT's median sits about 34% above and even its best seed stays 12% above.
把 $\gamma$ 调到 $0.3\gamma^*$ 或更小，跑满 3000 次迭代：球太小，重连太少，代价仍然高出 5% 以上（50 个种子全部如此，中位数高出约 30%）。Set $\gamma$ to $0.3\gamma^*$ or less and run 3000 iterations: the ball is too small, there is too little rewiring, and the cost stays more than 5% above optimal (for all 50 seeds; the median is about 30% above).

窄通道与“概率完备”Narrow passages and "probabilistic completeness"

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

What sampling-based planners guarantee is probabilistic completeness: if a solution exists, the probability of not finding one shrinks to zero as the number of samples grows. How fast it shrinks depends on how "narrow" the hardest stretch of free space is: a random point lands in a narrow passage with probability proportional to the passage's volume (the doorway of Lesson 7.2). The figure shows a 4 m thick wall with a passage of width $w$; PRM keeps adding points until S and G are connected, 60 times over (a fixed set of 60 seeds). All three samplers draw half of the nodes uniformly and half from the chosen sampler: Gaussian sampling (Boor, Overmars & van der Stappen 1999) draws a second point at a Gaussian offset from a random one and keeps the free one when exactly one of the two collides, so points gather near obstacle surfaces; the bridge test (Hsu et al. 2003) draws two points that both collide and keeps their midpoint if it is free, which is exactly the signature of a narrow gap.

FIG 5窄通道：失败概率随节点数指数下降，速度取决于通道宽度和采样器Narrow passages: the failure probability decays exponentially with the node count, at a rate set by the width and the sampler

左：最后一次试验的全部节点，青色来自均匀采样，琥珀色来自所选的采样器（连接半径 1.2 m，高斯与桥测试的偏移标准差 0.6 m）。右：“仍未连通的试验比例”对节点数，纵轴是对数刻度：一条直线就意味着指数衰减。曲线颜色：均匀为青色，高斯为紫色，桥测试为琥珀色。每次试验最多加 3000 个节点。读数里“点检测”是所有采样时问过碰撞检测器的点数，“边检测”把每条候选边按 0.1 m 的分辨率折算成点检测的次数。 Left: all nodes of the latest trial, cyan from uniform sampling and amber from the chosen sampler (connection radius 1.2 m; Gaussian and bridge offsets with standard deviation 0.6 m). Right: "share of trials still unconnected" against the number of nodes, on a logarithmic vertical axis: a straight line means exponential decay. Curve colours: uniform cyan, Gaussian purple, bridge amber. Each trial adds at most 3000 nodes. In the readout, "point checks" counts every point the samplers asked the collision checker about, and "edge checks" converts each candidate edge into point checks at a 0.1 m resolution.

把通道宽度调到 0.2 m 或更窄，用“均匀”跑 60 次试验：中位数要 800 个以上的节点才连通（$w=0.2$ 时为 820）。Narrow the passage to 0.2 m or less and run 60 trials with "uniform": the median needs more than 800 nodes to connect (820 at $w=0.2$).
同样的宽度，再用“高斯”跑 60 次：所需节点数的中位数不到均匀采样的一半（$w=0.2$ 时为 273），曲线也陡得多。At the same width, run 60 trials with "Gaussian": the median node count is less than half the uniform one (273 at $w=0.2$), and the curve is much steeper.

代价只是被挪了地方The cost is only moved around

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

Now try the bridge test: at $w=0.2$ it needs only about 209 nodes, fewer than Gaussian, but to find them it asked the collision checker about some 31 thousand points, 25 times as many as uniform sampling (about 1200): the midpoints of most "both colliding" pairs are inside the wall as well. Which sampler pays off depends on whether point checks or edge checks are more expensive, and on the dimension: fewer nodes mean fewer edge checks, a smaller graph and faster queries; in a high-dimensional configuration space a passage's share of the volume is far smaller, and that is where a sampler that "hunts for narrow gaps" like the bridge test really earns its keep (Hsu et al. ran their experiments on high-dimensional arm problems).

5再看数学The math

提示：把鼠标悬停在公式中带底色的符号上，图中对应的元素会被高亮（FIG 1 的 Voronoi 单元，FIG 2 的随机点、最近节点、步长圆和新节点，FIG 3 的连接半径，FIG 4 的近邻球与重连的边，FIG 5 的失败曲线，FIG 6 的检测点与 C-障碍物）。Tip: hover the highlighted symbols in the formulas; the matching elements light up in the figures (the Voronoi cells of FIG 1; the random point, nearest node, step circle and new node of FIG 2; the connection radius of FIG 3; the neighbour ball and rewired edges of FIG 4; the failure curves of FIG 5; the checked configurations and C-obstacles of FIG 6).

问题与唯一的“预言机”The problem and the only "oracle"

给定 $\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}}$ 一无所知，只能调用两个函数：

Given $\mathcal C_{\text{free}}=\mathcal C\setminus\hl{cobs}{\mathcal C_{\text{obs}}}$ (Lesson 7.2), a start $\mathbf q_s$ and a goal $\mathbf q_g$, find a continuous path $\sigma:[0,1]\to\mathcal C_{\text{free}}$ with $\sigma(0)=\mathbf q_s$ and $\sigma(1)=\mathbf q_g$. If a cost is also given (in this lesson always the length $c(\sigma)=\int\|\dot\sigma\|\,dt$), the problem is optimal planning. A sampling-based planner knows nothing about $\mathcal C_{\text{obs}}$ and may only call two functions:

$$ \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}$）。

The second is the discretized edge check "test every $\delta$ along the segment" (OMPL's default; the point robot of FIG 2 to FIG 5 can intersect rectangles exactly, while the arm in FIG 6 uses this). For angle coordinates both "segment" and "distance" must take the shortest way around the torus (the $\operatorname{wrap}$ of Lesson 7.2).

RRT 与它的 Voronoi 偏置RRT and its Voronoi bias

$\mathcal T\leftarrow\{\mathbf q_s\}$
循环：repeat: $\hl{rand}{\mathbf q_{\text{rand}}}\sim\operatorname{Uniform}(\mathcal C)$ （以概率 $p_g$ 取 $\mathbf q_g$）(with probability $p_g$ take $\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}}\|}$
若if $\operatorname{FreeEdge}(\mathbf q_{\text{near}},\mathbf q_{\text{new}})$：把 $\mathbf q_{\text{new}}$ 加入 $\mathcal T$，父节点为 $\mathbf q_{\text{near}}$；若它离 $\mathbf q_g$ 不到 $\eta$ 且能直连，返回路径: add $\mathbf q_{\text{new}}$ to $\mathcal T$ with parent $\mathbf q_{\text{near}}$; if it is within $\eta$ of $\mathbf q_g$ and can connect to it, return the path

第 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$ 表示面积或体积）

Line 3 is where the Voronoi bias comes from. The Voronoi cell of node $\mathbf v$, $\operatorname{Vor}(\mathbf v)=\{\mathbf x:\|\mathbf x-\mathbf v\|\le\|\mathbf x-\mathbf u\|\ \forall\mathbf u\in\mathcal T\}$, is exactly the set of random points that make $\mathbf v$ the nearest node, so under uniform sampling the probability $P_{\text{ext}}(\mathbf v)$ that $\mathbf v$ is extended is ($\mu$ denotes area or volume)

$$ 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 节的度量）。

For the naive random tree the same probability is $1/n$, regardless of geometry, so most of its new nodes grow near the already crowded root (FIG 1). RRT-Connect adds a layer of greed: the other tree calls lines 4 and 5 again and again toward the new node until it is blocked or arrives. It remains probabilistically complete and is usually much faster when there are no narrow passages. Nearest-neighbour queries are RRT's other big cost: a linear scan is $O(n)$, so large problems use structures such as k-d trees or GNAT (OMPL's default for metric state spaces is GNAT), and the distance must respect the topology of the configuration space (the metrics of Lesson 7.2).

PRM 与概率完备性PRM and probabilistic completeness

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$ 个小球取并集界：

The PRM roadmap $G=(V,E)$ consists of $n$ free samples and every edge with $\|\mathbf q-\mathbf q'\|\le\hl{rad}{r}$ and $\operatorname{FreeEdge}(\mathbf q,\mathbf q')$. Why do more points make it more reliable? Suppose a solution path of length $L$ exists that stays at least $\rho$ away from obstacles everywhere (its clearance), and the connection radius is $r\ge3\rho/2$. Place $M=\lceil 2L/\rho\rceil$ small balls of radius $\rho/2$ along the path, consecutive centres at most $\rho/2$ apart. If every ball contains at least one sample, samples in consecutive balls are at most $3\rho/2$ apart and every point of the segment between them is within $3\rho/4\lt\rho$ of the path, so the segment is free; with $r\ge3\rho/2$ it gets connected, and the roadmap contains a route from S to G. This follows the analysis of Kavraki, Kolountzakis & Latombe (1998): balls of radius $\rho/2$ along the path and a union bound. Their roadmap tries to connect any two samples; the condition $r\ge3\rho/2$ is added here to fit a PRM that links only near neighbours. Let $P_{\text{fail}}(n)$ be the probability that the roadmap still has no route after $n$ samples. One ball is empty with probability $\big(1-\tfrac{\zeta_d(\rho/2)^d}{\mu(\mathcal C_{\text{free}})}\big)^n\le e^{-\sigma\rho^dn}$; a union bound over the $M$ balls gives

$$ \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 多个节点的原因。

where $\zeta_d$ is the volume of the $d$-dimensional unit ball ($\zeta_2=\pi$). Three readings: (1) the failure probability falls exponentially in $n$, the straight lines on the log axis of FIG 5; (2) the rate is proportional to $\rho^d$: halving the passage width divides the rate by $2^d$ in $d$ dimensions, which is why narrow passages are deadlier in higher dimensions; (3) this is a guarantee when a solution exists. When there is none, both PRM and RRT run forever: they cannot prove that no path exists. Hence "probabilistically complete", not "complete". RRT is probabilistically complete as well (LaValle & Kuffner 2001); a fully rigorous proof for the geometric case, with an exponential rate, was given by Kleinbort et al. (2019).

The numbers of FIG 5 match this intuition. The passage has area $4w$ and the free space $96-32+4w$, so at $w=0.2$ a uniform sample lands in the passage with probability only $0.8/64.8\approx1.2\%$; with a 4 m thick wall and a 1.2 m connection radius, the roadmap must string several nodes through the passage, which is why uniform sampling needs over 800 nodes.

RRT：选父节点、重连与收缩的半径RRT: choosing the parent, rewiring, and the shrinking radius

设 $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 行做两件事：

Let $c(\mathbf q)$ be the length of the tree path from the start to $\mathbf q$, and $X_{\text{near}}=\{\mathbf q\in\mathcal T:\|\mathbf q-\mathbf q_{\text{new}}\|\le r_n\}$. RRT* does two things at line 5 of RRT:

$$ \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。

After a rewire, the cost of the rewired node's whole subtree drops, so an implementation must push the new costs down the subtree. The shape of the radius has a clear reason: $n$ uniform points in $d$ dimensions are spaced about $n^{-1/d}$ apart; the extra factor $(\log n)^{1/d}$ makes the expected number of neighbours in the ball grow like $\log n$, just enough to link "neighbouring samples" into ever better approximations of the optimal path (this is tied to the connectivity threshold of random geometric graphs), while each iteration does only a logarithmic factor more work. Karaman & Frazzoli (2011) proved that for $\gamma>\gamma^*$ RRT* is asymptotically optimal: the cost of the best path converges to the optimum with probability one. Solovey et al. (2020) later found a gap in that proof; their rigorous replacement needs a somewhat larger radius, shrinking like $(\log n/n)^{1/(d+1)}$, which is one more reason to choose $\gamma$ generously in practice. In the forest of FIG 4, $\mu(\mathcal C_{\text{free}})\approx 77.2\ \text{m}^2$ and $\gamma^*=\sqrt3\,\sqrt{77.2/\pi}\approx8.6$ m; FIG 4 uses $\gamma=1.5\gamma^*$ by default, so with $n=1000$ tree nodes $1.5\gamma^*\sqrt{\ln n/n}\approx1.07$ m, a bit above $\eta=1$ and therefore capped at 1, and at $n=3000$ it has shrunk to about 0.67 m.

碰撞检测：真正的开销Collision checking: the real cost

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

In real arm planning most of the time goes into collision checking (mesh-mesh intersection, distance queries), not into sampling or nearest neighbours. A discretized edge needs $m=\lceil\|\Delta\mathbf q\|/\delta\rceil$ checks, so halving $\delta$ doubles the edge cost. But too large a $\delta$ misses collisions: between two checked configurations the robot may sweep straight through a thin pole. For the planar two-link arm of FIG 6, when the joint angles change by $\Delta\boldsymbol\theta$, no point of the arm moves more than

$$ \|\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，足够整个跨过去。

(First term: rotating about the base, no point is farther than $L_1+L_2$ from it; second term: rotating about the elbow moves only the second link; the second inequality is Cauchy-Schwarz.) So if every checked configuration has a clearance (distance to obstacles) larger than $1.97\,\delta$, the whole motion between checks is certainly free: this is "conservative" edge checking, and it is why libraries such as FCL offer distance queries and continuous collision checking. The thin pole in FIG 6, plus the link radius, is only 0.21 m across; at $\delta=0.3$ rad the hand can move up to about 0.59 m between checks, enough to jump right over it.

6机器人中的它：给一台两连杆臂规划In robotics: planning for a two-link arm

回到 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 节讲过几何路径为什么还不能直接执行）。

Back to the planar two-link arm of Lesson 7.2 ($L_1=1$ m, $L_2=0.8$ m, link radius 0.045 m). Its configuration space is the torus $S^1\times S^1$: the left and right edges, and the top and bottom edges, of the square on the right are glued together. The planner only calls the collision checker: the red C-obstacles on the right are computed separately (one check per cell) so that you can see them; the planner never looks at that picture. The planner is RRT-Connect with step $\eta=0.3$ rad, with distances and interpolation taken the short way around the torus (so branches and paths may leave through an edge of the square and come back on the opposite side).

The raw path still has to be "shortcut": repeatedly pick two random points on the path and, if the straight segment between them passes the check, cut out the detour in between. OMPL and MoveIt both do such simplification before returning a path. Then either the path is time-parameterized directly, or it is first resampled by arc length into evenly spaced waypoints that seed a trajectory optimizer such as CHOMP or TrajOpt, which makes it smooth and adds clearance, before time parameterization (Lesson 14.3; Lesson 6.13 explained why a geometric path is not yet executable).

FIG 6两连杆臂：在环面上规划，检测分辨率决定会不会“穿模”A two-link arm: planning on the torus, where the check resolution decides whether it "phases through"

工作空间：拖动圆盘WORKSPACE · drag the discs

构型空间 (θ₁, θ₂)：拖动 S、GC-SPACE (θ₁, θ₂) · drag S, G

左：手臂与四个圆盘障碍物（红色，可拖动，其中一根是细杆）。手臂在 S（蓝色虚影）与 G（绿色虚影）两个构型之间，琥珀色实体是沿路径播放时的当前姿态。右：环面构型空间，红色是 C-障碍物；青色和紫色是从 S 和 G 长出的两棵树，琥珀色是路径（捷径化之后变短）。每次按“规划”都换一个随机种子。规划完成后，页面会用 0.005 rad 的分辨率把路径重新检查一遍：如果发现规划器漏检的碰撞，就在两张图上用红叉标出。 Left: the arm and four disc obstacles (red, draggable; one is a thin pole). The arm's ghosts mark S (blue) and G (green); the solid amber arm is the current pose while the path plays. Right: the torus C-space with C-obstacles in red; cyan and purple are the trees from S and G, amber the path (shorter after shortcutting). Each press of "Plan" uses a new random seed. After planning, the page re-checks the path at a 0.005 rad resolution and marks any collision the planner missed with red crosses in both panels.

把检测分辨率 $\delta$ 调到 0.3 rad 或更大，反复按“规划”，直到复查发现漏检（每次约一半的机会；100 个种子里有 46 到 53 个）。按“播放”看手臂怎样直接穿过细杆。再把 $\delta$ 调回 0.05：100 个种子里一次漏检也没有，但碰撞检测次数从约 150 涨到约 490。Set the check resolution $\delta$ to 0.3 rad or more and press "Plan" until the re-check finds a missed collision (about half the time; 46 to 53 of 100 seeds). Press "Play" and watch the arm pass straight through the pole. Then set $\delta$ back to 0.05: not a single miss in 100 seeds, but the collision checks rise from about 150 to about 490.
在没有漏检的情况下规划一次（$\delta\le0.1$），再按“捷径化”：构型空间里的路径长度缩短 10% 以上（100 个种子的中位数缩短 18%）。Plan once without a miss ($\delta\le0.1$), then press "Shortcut": the C-space path length drops by more than 10% (a median of 18% over 100 seeds).

真实系统里In real systems

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 节）则把采样式规划当作底层的可行性检查。

OMPL and MoveIt: the Open Motion Planning Library OMPL (Şucan, Moll & Kavraki 2012) implements dozens of sampling-based planners (PRM, PRM*, RRT, RRT-Connect, RRT*, Informed RRT*, BIT*, FMT*, KPIECE, EST and more), and deliberately contains no collision checking and no robot model: the user supplies an "is this state valid?" function, the oracle pattern of Section 5. MoveIt uses OMPL as its default planning plugin with RRT-Connect as the default planner, and FCL (Pan, Chitta & Manocha 2012) for collision checking by default. The edge resolution is MoveIt's longest_valid_segment_fraction parameter (a fraction of the state space's extent), i.e. the $\delta$ of FIG 6. The planned path then goes through OMPL's path simplification (shortcutting and more) and a time parameterization (such as the time-optimal trajectory generation of Kunz & Stilman) that assigns velocities and accelerations. Speed: VAMP by Thomason, Kingston & Kavraki (2024) checks many configurations at once with SIMD vectorization and its authors report median RRT-Connect planning times of tens of microseconds on one CPU core for the 7-DoF Franka Panda on the MotionBenchMaker benchmark problems. Mobile robots and cars: a car has nonholonomic constraints (Lesson 9.1), so "step toward the random point" must become "propagate a feasible control for a short time": kinodynamic RRT (LaValle & Kuffner 2001). MIT's vehicle in the 2007 DARPA Urban Challenge used closed-loop RRT (Kuwata et al. 2009). Ground navigation more often uses the Hybrid A* and state lattices of Lesson 14.1. Division of labour with optimization: sampling-based planning "finds one, globally"; trajectory optimization (CHOMP and TrajOpt, Lesson 14.3) "makes it good, locally", and the two are often chained: the shortcut path, resampled by arc length into waypoints, becomes the optimizer's initial guess; TAMP (Lesson 14.4) uses sampling-based planning as its low-level feasibility check.

7常见误区Common pitfalls

误区 1：把“超时没找到”当成“无解”Pitfall 1: reading "timed out" as "no solution"

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

Probabilistic completeness only promises "if there is a solution, it will be found eventually". A timeout may mean there truly is none (start and goal in different components of $\mathcal C_{\text{free}}$, Lesson 7.2), or merely that a narrow passage has not been sampled yet (in FIG 5, uniform sampling at $w=0.2$ needs over a thousand nodes at the 90th percentile). Higher-level logic (such as backtracking in TAMP, Lesson 14.4) should treat "planning failed" as an uncertain signal: retry with another seed, allow more time, change the sampler, rather than concluding at once that the geometry is infeasible.

误区 2：边检测分辨率太粗Pitfall 2: too coarse an edge resolution

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

Checking only the nodes, or checking too sparsely along edges, gives paths that "phase through" obstacles (FIG 6). The danger is that the visualization often hides it: the waypoints themselves are all free. Either choose $\delta$ from the bound of Section 5 (largest displacement $\le$ smallest clearance), or use continuous collision checking or distance queries with a safety margin; inflating thin objects is also common insurance. Conversely, too fine a $\delta$ multiplies the number of checks and slows planning down.

误区 3：距离函数和构型空间对不上Pitfall 3: a distance that does not fit the configuration space

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

Nearest neighbours, interpolation and the cost of RRT* all depend on the distance. Subtracting revolute angles naively makes $\theta=3.1$ and $\theta=-3.1$ look 6.2 rad apart (Lesson 7.2), so the tree takes the long way round and may even spin a joint a full turn; putting metres and radians into one Euclidean distance (a mobile base with an arm) lets one direction dominate the nearest-neighbour search. Handle angles according to their topology, and weight components with different units.

误区 4：直接执行 RRT 的原始路径Pitfall 4: executing the raw RRT path

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

The raw path is a crooked polyline (about 1.3 times the optimal length in FIG 2) with velocity discontinuities at every corner. A robot that follows it shakes, is slow, and may exceed its acceleration limits. At least shortcut it (FIG 6), then smooth or optimize it (Lesson 14.3) and time-parameterize it. When path quality matters, use an asymptotically optimal planner such as RRT*, Informed RRT* or BIT*, and give it time.

误区 5：参数照抄，不看场景Pitfall 5: copying parameters without looking at the scene

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

Goal bias helps in open scenes and hurts in a "pocket" (FIG 2); an RRT* radius constant that is too small is no longer asymptotically optimal and behaves much like RRT (FIG 4, $\gamma=0.3\gamma^*$); too small a step $\eta$ makes the tree grow slowly, while too large a step makes each extension's edge check more expensive and more likely to be blocked. Set these by the scale and dimension of the scene, and tune them on statistics over many seeds, not on one run.

8小练习Check yourself

1. 在没有障碍物的空间里，一棵 RRT 有 100 个节点。下一次迭代最可能扩展哪个节点？1. In a space without obstacles, an RRT has 100 nodes. Which node is most likely to be extended in the next iteration?

节点被扩展当且仅当随机点落在它的 Voronoi 单元里，所以概率等于单元面积除以总面积（第 5 节）。边缘节点面对大片空地，单元最大。选项 B 描述的是天真随机树（FIG 1 右图），它因此在根部附近堆成一团。A node is extended exactly when the random point falls into its Voronoi cell, so the probability is the cell's area over the total area (Section 5). Rim nodes face the open space and own the largest cells. Option B describes the naive random tree (FIG 1, right), which is why that tree clumps around the root.

2. 一个 3 维构型空间里有一条窄通道，它在其中 2 个方向上很窄。把通道在这两个方向上的宽度都减半，一个均匀样本落进通道的概率大约变为原来的多少？2. A 3-dimensional configuration space has a narrow passage that is narrow in 2 of its directions. If the passage width is halved in both of those directions, roughly what fraction of the original probability remains for a uniform sample to land in the passage?

通道体积 = 长度 × 宽 × 宽，两个窄方向各减半，体积变为 $1/2\times1/2=1/4$，落入概率也变为 1/4，需要的样本数大约变成 4 倍。窄的方向越多，这个因子越大：这就是窄通道在高维里格外难的原因（第 5 节的 $\rho^d$）。Passage volume = length × width × width; halving both narrow directions gives $1/2\times1/2=1/4$ of the volume, so a quarter of the hit probability, and roughly four times as many samples are needed. The more narrow directions, the bigger the factor: that is why narrow passages are especially hard in high dimensions (the $\rho^d$ of Section 5).

3. RRT 找到第一条路径后继续运行 10 倍的时间（不做任何后处理）。最可能发生什么？3. After finding its first path, RRT keeps running for ten times as long (with no post-processing). What is most likely to happen?

RRT 的父节点在节点出生时就定了，新节点不会改变已有的路线；Karaman 与 Frazzoli 证明 RRT 以概率 1 收敛到非最优解。FIG 4 里 RRT 在第 2000 到 5000 次迭代之间几乎是一条水平线（中位数高出最优约 34%）。要渐近最优需要 RRT* 的重连。RRT fixes each parent when the node is born, and new nodes never change existing routes; Karaman and Frazzoli proved that RRT converges to a non-optimal solution with probability one. In FIG 4 RRT is almost flat between iterations 2000 and 5000 (median about 34% above optimal). Asymptotic optimality needs the rewiring of RRT*.

4. 一个固定的装配工作站里，场景从不改变，机械臂每天要在几千对不同的姿态之间规划。更合适的是？4. In a fixed assembly cell the scene never changes, and the arm must plan between thousands of different pairs of poses every day. Which fits better?

这正是 PRM 的设计用途：多查询、静态环境。建图的成本（FIG 3 里约 700 次线段检测）摊到成千上万次查询上，每次查询只要几十次检测。两者都概率完备（B 错）；6 维网格在每维分辨率稍高时就大得不可行（C 错）。如果场景每次都变，单查询的 RRT-Connect 更合适。That is exactly what PRM is designed for: many queries in a static environment. The cost of building (about 700 segment checks in FIG 3) is spread over thousands of queries that need only a few dozen checks each. Both are probabilistically complete (B is wrong); a 6-dimensional grid becomes infeasibly large at any decent resolution (C is wrong). If the scene changed every time, single-query RRT-Connect would fit better.

5. FIG 6 的手臂（$L_1=1$ m，$L_2=0.8$ m）用 $\delta=0.3$ rad 做边检测。场景里有一根加上连杆半径后直径约 0.21 m 的细杆。下面哪个说法对？5. The arm of FIG 6 ($L_1=1$ m, $L_2=0.8$ m) checks edges at $\delta=0.3$ rad. The scene contains a pole about 0.21 m across once the link radius is added. Which statement is right?

由第 5 节的界，$\|\Delta\mathbf p\|\le1.97\times0.3\approx0.59$ m，远大于 0.21 m。FIG 6 里这样的设置约一半的规划会漏检。要么把 $\delta$ 降到使最大位移小于间隙，要么用连续碰撞检测。增大 $\eta$ 只会让边更长，于情无补。By the bound of Section 5, $\|\Delta\mathbf p\|\le1.97\times0.3\approx0.59$ m, far more than 0.21 m. In FIG 6 about half of the plans at this setting miss a collision. Either lower $\delta$ until the largest displacement is below the clearance, or use continuous collision checking. A larger $\eta$ only makes the edges longer, which does not help.

9延伸阅读Further reading

S. M. LaValle, Planning Algorithms, Cambridge University Press, 2006, Ch. 5 — 采样式规划的系统讲解：采样、碰撞检测、增量式与路网方法a systematic treatment of sampling-based planning: sampling, collision checking, incremental and roadmap methods
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 及其分析PRM, RRT and their analysis
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 的原始论文the original PRM paper
L. E. Kavraki, M. N. Kolountzakis & J.-C. Latombe, Analysis of Probabilistic Roadmaps for Path Planning, IEEE Transactions on Robotics and Automation, 1998 — 第 5 节失败概率界的来源the source of the failure bound of Section 5
S. M. LaValle, Rapidly-Exploring Random Trees: A New Tool for Path Planning, Technical Report TR 98-11, Iowa State University, 1998 — RRT 的提出where RRT was introduced
J. J. Kuffner & S. M. LaValle, RRT-Connect: An Efficient Approach to Single-Query Path Planning, ICRA 2000 — 双向树与贪心连接bidirectional trees and greedy connection
S. M. LaValle & J. J. Kuffner, Randomized Kinodynamic Planning, International Journal of Robotics Research, 2001 — 带动力学约束的 RRTRRT with dynamic constraints
S. Karaman & E. Frazzoli, Sampling-Based Algorithms for Optimal Motion Planning, International Journal of Robotics Research, 2011 — RRT*、PRM* 与渐近最优性RRT*, PRM* and asymptotic optimality
D. Hsu, T. Jiang, J. Reif & Z. Sun, The Bridge Test for Sampling Narrow Passages with Probabilistic Roadmap Planners, ICRA 2003 — 桥测试the bridge test
J. D. Gammell, S. S. Srinivasa & T. D. Barfoot, Informed RRT*, IROS 2014 — 找到第一条解之后只在可能改进的椭球里采样after the first solution, sample only inside the ellipsoid that could still improve it
I. A. Şucan, M. Moll & L. E. Kavraki, The Open Motion Planning Library, IEEE Robotics & Automation Magazine, 2012 — OMPL 的设计the design of OMPL