采样式路径规划：RRT 家族为什么统治了机械臂规划

TOPP-RA 那篇的开头说过：“找到一条无碰路径”和“让机器人沿它跑得最快”是两件事。那篇讲了后一半，这篇补上前一半——无碰路径从哪来。对 6、7 自由度机械臂，这个问题的工业标准答案是采样式规划（RRT 家族），本文讲清楚它为什么赢、怎么实现、以及规划器输出的锯齿路径如何变成能执行的轨迹。

1. 问题住在构型空间里

规划的舞台不是三维工作空间，而是构型空间（C-space）：每个点是一组关节角 $\mathbf q \in \mathbb R^n$，障碍物变成 $C_{obs}$——所有会导致碰撞（含自碰撞）的构型集合。路径规划即在 $C_{free} = C \setminus C_{obs}$ 中找一条连接起点和目标的连续曲线。

换舞台的代价是看不见障碍：工作空间里一个箱子，映射到 7 维关节空间是一个没有解析表达的怪异形状。我们对 $C_{obs}$ 的全部访问手段只有一个谓词：

$$\texttt{collision_free}(\mathbf q) \to {\text{true}, \text{false}}$$

——给定构型做一次碰撞查询。这个“只有点查询”的接口直接淘汰了一大类方法：

• 网格 + A*：分辨率 $r$ 的网格有 $r^n$ 个格子，7 自由度、每轴 100 格就是 $10^{14}$——维度灾难，2、3 维之后免谈；
• 精确胞分解 / 可视图：需要 $C_{obs}$ 的几何表达，而我们没有；
• 人工势场：会卡在局部极小，且没有完备性保证。

采样式规划的立足点正是这个接口：不解析描述 $C_{free}$，用随机采样 + 碰撞查询把它“探”出来。代价是完备性降级为概率完备——解存在时找到它的概率随采样数趋于 1，但永远无法证明“无解”（只能超时放弃）。工程上这个交换极其划算。

2. RRT：为什么随机树能快速铺满空间

RRT（快速探索随机树）的循环只有四步：

tree ← {q_start}
loop:
    q_rand ← 均匀采样（以小概率 p 直接取 q_goal——目标偏置）
    q_near ← 树上离 q_rand 最近的节点
    q_new  ← 从 q_near 朝 q_rand 走一步 Δ
    if 线段 [q_near, q_new] 无碰:
        tree.add(q_new, parent=q_near)
    if q_new 够接近 q_goal: 回溯出路径，结束

看似朴素，藏着一个关键机制——Voronoi 偏置：均匀采样的 $\mathbf q_{rand}$ 落进哪个节点的 Voronoi 胞，哪个节点就被选为 $\mathbf q_{near}$ 去扩展。而 Voronoi 胞最大的恰恰是树的边疆节点（周围空旷），所以树自动优先向未探索区域生长，不需要任何显式的探索启发式。这是 RRT 比“随机游走”快几个量级的原因。

实现层面三个参数各管一件事：步长 $\Delta$ 太大容易穿墙失败、太小则树长得慢（经验值取关节空间对角线的 15%）；目标偏置 $p$（典型 510%）让树时不时朝目标伸一把，没有它树会均匀铺满全空间才碰到目标；距离度量决定“最近”的含义——关节空间加权欧氏距离要给大臂关节更高权重（同样 0.1 rad，肩关节末端扫过的距离远大于腕关节）。

单向不如双向：RRT-Connect

实践里几乎没人用单树 RRT。RRT-Connect 从起点和目标各长一棵树，交替扩展并贪心互连（connect 步不止走一步 $\Delta$，而是朝对方一直走到碰撞为止）。机械臂的起点和目标往往都在“钻进夹具/货架”的狭窄区域，双向树恰好让两端的困难区域各自被自己那棵树就地探索。MoveIt 的默认规划器就是它，典型 6 轴场景 10~100 ms 出解。

3. RRT*：把“可行”升级成“最优”

RRT 概率完备但与最优无缘——第一条摸到目标的路径是什么样就是什么样，且不随采样增加而改善。RRT* 加两个机制修复它：

1. choose-parent：新节点 $\mathbf q_{new}$ 不再直接认 $\mathbf q_{near}$ 当父亲，而是在半径 $r_n$ 邻域内选使起点到 $\mathbf q_{new}$ 代价最小的节点；
2. rewire：反过来检查邻域内每个老节点，若“经过 $\mathbf q_{new}$ 绕一下”更便宜，就把老节点的父边改接到 $\mathbf q_{new}$ 上。

树因此不断自我修正，解的代价单调下降，以概率 1 收敛到最优——渐近最优。邻域半径按 $r_n \propto \left(\log n / n\right)^{1/d}$ 收缩（$d$ 是空间维数），这是 Karaman & Frazzoli 证明里的关键：半径缩得比这个快就失去最优性，慢则邻域查询拖垮性能。

同样 2500 次采样（本图为真实运行结果）：RRT 的树是一次性的探索痕迹，路径 25.4 m；RRT* 经过持续 rewire，树呈现出“扇形代价场”的形态，路径 17.9 m，逼近最优

再往上还有一层加速——Informed RRT*：一旦有了代价为 $c$ 的解，任何能改进它的路径必然落在以起点、终点为焦点、长轴为 $c$ 的椭球内（椭圆的定义），于是采样直接限制在这个椭球里，收敛速度大幅提升，解越好椭球越瘦。

工程上的取舍很清楚：只要可行解（抓取、换姿态）用 RRT-Connect 加后处理；路径质量重要（周期性作业、能耗敏感）用 Informed RRT* 或 BIT* 给足时间预算。中间态是常见做法：RRT-Connect 先 50 ms 拿到保底解，剩余预算交给优化型规划器继续改进，超时取当前最好。

4. 碰撞检测：规划器 90% 的时间花在哪

采样式规划的性能画像出人意料：典型场景下 90% 以上的 CPU 时间在做碰撞查询，规划算法本身只是薄薄一层。所以工程优化的杠杆都在这里：

• 两阶段查询：宽阶段用 AABB 树等包围盒层次快速剔除不可能碰撞的几何对，窄阶段（GJK/EPA 算法）只对幸存对做精确检测。FCL、Bullet 都是这个结构；
• 碰撞几何 ≠ 显示几何：拿装饰性的 10 万面片网格做碰撞是常见的性能自杀。碰撞模型用凸包、胶囊体简化，复杂形状做凸分解，通常快一个数量级以上；
• 自碰撞矩阵：$n$ 个连杆有 $O(n^2)$ 个碰撞对，其中大半要么永远碰不到、要么天生相邻必然“接触”。离线（MoveIt 的 SRDF 生成工具做的就是这件事）把这些对剔除，运行时只查剩下的；
• 连续性的坑：collision_free 只查点，边 $[\mathbf q_{near}, \mathbf q_{new}]$ 靠离散插值逐点查——分辨率太粗会隧穿（薄障碍物从两个采样点之间穿过去）。分辨率的选择要联动最大杆长：相邻检查点间任何部位扫过的距离不得超过障碍最薄尺寸。要严格保证用连续碰撞检测（CCD），代价更高，安全关键场景才上。

另一个容易忽略的实现细节：随机种子要可控。规划器输出不确定，调试时“复现不了上次那条怪路径”是真实的痛苦；固定种子、记录采样序列，把不确定性关进笼子。

5. 从锯齿路径到可执行轨迹：完整管线

RRT 家族输出的路径是分段直线、带随机抖动的——直接发给控制器，机械臂会跳一段机械舞。到执行还差两步，恰好接上本博客已有的两篇：

捷径平滑（shortcutting）：反复随机取路径上两点，若直连无碰就删掉中间段。简单粗暴，但对随机规划器的输出极其有效——通常几十毫秒内路径长度再降 20~40%。比“更聪明的规划器”性价比高得多，这也是实践中容忍 RRT-Connect 粗糙输出的底气。

时间参数化：平滑后的几何路径交给 TOPP-RA 求速度/加速度约束下的时间最优时间律；在线跟踪与目标切换则由 Ruckig 这类 OTG 兜底。

管线视角还能纠正一个常见的错位：抱怨“RRT 路径太丑”的团队，缺的往往不是更好的规划器，而是后处理和时间参数化这两级；反过来，抓取失败率高的根源常在逆解选支（见运动学的工程对照表）而不是规划本身。

6. 小结

采样式规划是“用计算换建模”的典型：它对环境几乎零假设，代价是随机性和只有概率意义的保证。把它放进带后处理、带时间预算、带保底解的管线里，这些短板都能被工程手段接住——这正是它从论文走到每台工业臂上的原因。

参考

• S. M. LaValle. Planning Algorithms. Cambridge University Press.（免费在线，C-space 与 RRT 的标准教材）
• S. Karaman, E. Frazzoli. Sampling-based Algorithms for Optimal Motion Planning. IJRR 2011.（RRT* 与渐近最优性证明）
• J. D. Gammell, S. S. Srinivasa, T. D. Barfoot. Informed RRT*. IROS 2014.
• J. J. Kuffner, S. M. LaValle. RRT-Connect: An Efficient Approach to Single-Query Path Planning. ICRA 2000.
• I. A. Șucan, M. Moll, L. E. Kavraki. The Open Motion Planning Library (OMPL). IEEE RAM 2012.