运动规划 RRT：快速探索随机树及其变体

运动规划中的快速探索随机树（RRT）

在机器人、自动驾驶和动画等领域，运动规划的核心任务是找到一条从起点到终点的无碰撞路径。当状态空间维度较高或障碍物复杂时，传统方法往往陷入维度灾难。快速探索随机树（Rapidly-exploring Random Tree，RRT） 以其概率完备性、无需显式建模障碍物、能高效探索高维空间的优势，成为最受欢迎的采样规划算法之一。本教程将从基本RRT出发，逐步深入到实战变体与优化技巧。

基本RRT算法原理

RRT 通过增量式地构建一棵树来探索状态空间。树上的每个节点代表一个状态，边代表可行的动作。算法不预先离散化空间，而是通过随机采样和局部连接，将搜索快速导向未探索区域，其心理论是Voronoi偏置：采样点落在大空白区的概率更高，导致树优先向空旷区域生长。

算法核心步骤

1. 初始化：将起点作为树的根节点。
2. 迭代（重复 N 次直到找到解）：
   • 随机采样：在状态空间中按均匀分布采样一个随机点 x_rand（有时会以小概率直接采样目标点）。
   • 最近邻查找：在已有树节点中找到离 x_rand 最近的节点 x_near。
   • 向前延伸 (Steer)：从 x_near 向 x_rand 方向移动一个步长 eps，得到新节点 x_new。
   • 碰撞检测：检查从 x_near 到 x_new 的局部路径是否与障碍物发生碰撞。
   • 若可行：将 x_new 加入树，并将 x_near 设为其父节点。
   • 结束条件：若 x_new 到目标点的距离小于阈值，且路径可行，则返回从起点到目标的路径。
3. 输出：从目标节点回溯父节点到起点，形成规划路径。

伪代码：

T.init(x_start)
for i=1 to K:
    x_rand = SampleFree()
    x_near = Nearest(T, x_rand)
    x_new = Steer(x_near, x_rand, step_size)
    if CollisionFree(x_near, x_new):
        T.add_node(x_new)
        T.add_edge(x_near, x_new)
        if distance(x_new, x_goal) < goal_tolerance:
            return PATH from x_start to x_new
return FAILURE

关键参数：步长 step_size 控制树的生长速度和路径平滑度；K 为最大迭代次数；goal_tolerance 决定何时视为到达。

RRT 的特性与适用场景

核心变体与改进

1. RRT-Connect：贪婪连接的双向生长

双向 RRT 分别从起点和目标点各生长一棵树，交替扩展。每次一棵树先朝着随机点生长，再尝试将另一棵树的最新节点作为“引导点”进行多次连续贪婪扩展，直到两棵树相遇或碰撞。这种策略极大加快了搜索速度，在工业应用中极为普遍。

优势：效率高，特别适用于具有狭窄通道的环境。

2. RRT*：渐进最优性

RRT* 在基本 RRT 的基础上增加了重连 (Rewiring) 步骤，当加入 x_new 时，不仅接入最近节点，还搜索其一定半径内的所有邻居节点，选择代价最低且无碰撞的邻居作为父节点，并检查是否可将邻居作为子节点以降低它们的代价。这使算法具备渐进最优性：随着样本数增加，路径成本收敛到理论最优解。

• 搜索半径常取 γ * (log(n)/n)^(1/d)，其中 n 为树节点数，d 为维度，γ 为适当常数。
• 代价通常为欧几里得路径长度，也可扩至能量、时间等指标。

伪代码改进部分：

x_new = Steer(x_near, x_rand)
X_near = Near(T, x_new, radius)
x_min = x_near; c_min = cost(x_near) + dist(x_near, x_new)
for each x_near in X_near:
    if CollisionFree(x_near, x_new) and cost(x_near)+dist(x_near,x_new) < c_min:
        x_min, c_min = x_near, cost(x_near)+dist(x_near,x_new)
T.add_node(x_new) with parent x_min
// Rewiring
for each x_near in X_near:
    if CollisionFree(x_new, x_near) and cost(x_new)+dist(x_new,x_near) < cost(x_near):
        T.change_parent(x_near, x_new)

RRT* 的规划时间通常更长，但路径质量显著提高，常用于离线精细规划。

3. Informed RRT*：启发式采样加速收敛

标准 RRT* 在整个状态空间均匀采样，寻优效率低。Informed RRT* 一旦找到初始解，便将采样域限制在一个以起点和目标为焦点、路径长度为长轴的超椭球内。所有可能改善当前解的样本都必定位于此椭球中，直接缩小了搜索范围，极大加速收敛。

• 椭球形状：由当前最优路径长度决定，样本越往后越集中。
• 对高维问题效果尤佳。

4. 其他实用变形

• RRT with Dubins/Reeds-Shepp：结合非完整约束，用于车辆、无人机等系统的运动规划。
• Multi-directional RRT：在狭窄通道处同时拉取多个方向节点，增强连通性。
• Kino-RRT：在前向延伸时直接使用正向动力学积分，确保边满足动力学约束，而非简单线性插值。
• Goal-Zoom / Goal Bias：以概率 p 直接采样目标点，引导树快速向目标生长，p 常取 0.05~0.1。

实现中的关键细节

• 最近邻搜索：维度较低时可用 KD-Tree，高维时使用近似最近邻库（如 FLANN、hnswlib）以加速。
• 碰撞检测：使用包围盒层次结构（BVH）或距离场加速，避免直接检查每对障碍物。
• 步长自适应：在稀疏区域可用较大步长，靠近障碍物时缩小步长，平衡探索速度与安全性。
• 路径后处理：可用剪枝、缩短、平滑等方法去除冗余节点，使路径更实用。

动手实践：从简单2D示例开始

若想快速上手，推荐在 Python 中实现：

1. 定义状态空间 [0,100]x[0,100]，放置若干圆形障碍物。
2. 实现 SampleFree(), Nearest(), Steer(), CollisionFree() 函数。
3. 编写主循环，画出树和最终路径。
4. 尝试将 RRT 改为 RRT-Connect，观察迭代次数变化。
5. 引入 RRT* 的重连接，观察路径长度的优化。

推荐库：numpy, matplotlib 用于可视化；scipy.spatial.KDTree 做最近邻。

总结与选择指南

RRT 系列算法的核心思想是“用随机性解决确定性计算的指数爆炸”，理解其 Voronoi 偏置和增量生长机制，就能灵活设计出适应不同机器人、不同约束的规划器。从简单的 2D 指引到复杂的机械臂操作，RRT 都是你算法工具箱中不可或缺的一员。