4、RRT算法详解：快速随机搜索树原理、RRT Connect、RRT*算法对比
说到机械臂避障，RRT算法是个绕不开的话题。我最早接触它是在做移动机器人路径规划的时候，后来转到机械臂上，发现这东西真香——但也真坑。今天咱们就把RRT家族的三兄弟掰开揉碎了讲清楚。
4.1 快速随机搜索树（RRT）基本原理
RRT，全称Rapidly-exploring Random Tree。说白了，就是让一棵树在空间里乱长，直到碰到目标点。你想想看，机械臂的构型空间（C-Space）动不动就六维七维，传统方法根本没法建图。RRT的好处就是——它不需要显式建模整个空间。
核心思想其实很简单：
- 从起点开始，把这棵树种下去
- 随机采样，在空间里随便选个点
- 找最近邻，从树上找个离采样点最近的节点
- 朝采样点生长，走一小步（步长限制）
- 碰撞检测，如果这段路径没撞到障碍物，就把新节点加进去
- 重复，直到新节点离目标足够近
我在项目中遇到过一个问题：步长设得太小，树长得很慢；设得太大，又容易跳过狭窄通道。后来我总结了一个经验——步长一般取构型空间对角线长度的5%-10%。当然，这个要看具体场景。
关键点：RRT是概率完备的。什么意思？只要时间足够长，它一定能找到路径。但注意，它找到的路径通常不是最优的，甚至可以说挺烂的——弯弯绕绕，看着就难受。
这里我画了一张图，帮你理解RRT的生长过程：
4.2 RRT Connect：双向搜索的威力
RRT有个明显的毛病——如果目标点离得远，树要长很久才能过去。RRT Connect就是来解决这个问题的。它的思路很直接：从起点和目标同时长树，两棵树双向奔赴。
具体做法：
- 一棵树从起点开始长，另一棵从目标点开始长
- 每次迭代，一棵树先长一步，然后另一棵树尝试朝新节点生长
- 如果两棵树连上了，路径就找到了
我记得第一次在六轴机械臂上跑RRT Connect，效果惊艳。同样的场景，普通RRT要跑3秒，RRT Connect不到0.5秒就出结果了。为什么？因为两棵树同时长，搜索效率翻倍，而且目标点附近的那棵树天然就带着"目标导向"的特性。
我的小技巧：在实际项目中，我会让两棵树交替生长，而不是固定一棵先长。这样可以避免某棵树陷入局部区域。另外，当两棵树距离小于步长时，直接连接，不用再采样了——省时间。
4.3 RRT*：追求最优路径
RRT和RRT Connect找到的路径，说实话，挺丑的。弯弯绕绕不说，有时候还会出现不必要的回退。RRT*就是来解决这个问题的——它不仅要找到路径，还要找到渐近最优的路径。
RRT*在RRT的基础上加了两个关键操作：
- 重新选择父节点：新节点生成后，不是直接连到最近邻，而是在附近找一圈，看有没有更短的连接方式
- 重新布线：新节点加入后，检查附近的老节点，如果通过新节点能缩短路径，就重新连接
这两个操作听起来简单，但效果非常明显。随着迭代次数增加，路径会越来越平滑，越来越短。
注意：RRT*虽然路径质量好，但计算量也大。每次插入新节点都要做近邻搜索和路径重连。在机械臂的高维构型空间里，这个开销不容忽视。我曾经在一个7自由度臂上跑RRT*，每次迭代要检查上百个近邻点，速度比普通RRT慢了将近10倍。
4.4 三种算法对比
说了这么多，咱们用一张表来对比一下：
| 特性 | RRT | RRT Connect | RRT* |
|---|---|---|---|
| 搜索方式 | 单向搜索 | 双向搜索 | 单向搜索+优化 |
| 收敛速度 | 中等 | 快 | 慢（但路径质量好） |
| 路径质量 | 一般 | 一般 | 渐近最优 |
| 适用场景 | 简单环境 | 狭窄通道 | 需要高质量路径 |
| 计算开销 | 低 | 中 | 高 |
这里我给出一个简单的RRT核心代码片段，帮你理解实现逻辑：
def rrt_plan(start, goal, obstacles, max_iter=1000, step_size=0.1):
tree = {start: None} # 节点 -> 父节点
for i in range(max_iter):
# 随机采样
if random.random() < 0.1: # 10%概率直接采样目标点
sample = goal
else:
sample = random_sample()
# 找最近邻
nearest = find_nearest(tree, sample)
# 朝采样点生长
new_node = steer(nearest, sample, step_size)
# 碰撞检测
if not collision_check(nearest, new_node, obstacles):
tree[new_node] = nearest
# 检查是否到达目标
if distance(new_node, goal) < step_size:
return reconstruct_path(tree, new_node, start)
return None # 没找到路径这段代码虽然简单，但已经包含了RRT的核心逻辑。你注意看第6行——我习惯给目标点一个偏置采样概率，这样能加速收敛。这个技巧在很多里都有提到，但实际工程中很多人会忽略。
4.5 避坑指南
最后，分享几个我踩过的坑：
- 步长设置不当：我曾经在一个精密装配任务中，步长设得太小，导致树在狭窄通道里卡了上千次迭代。后来把步长从0.02调到0.08，问题立刻解决。
- 碰撞检测太粗糙：机械臂的碰撞检测不能只看末端，整个连杆都要检查。我见过有人只用球体包络，结果路径规划出来机械臂直接穿模。
- 忽略构型空间边界：关节是有限位的。采样点如果落在关节限位之外，直接丢弃，不要尝试生长。
嗯，RRT这块内容就讲到这里。记住一句话：没有最好的算法，只有最合适的算法。选哪个，看你的场景需求。
一句话总结：RRT快但路径差，RRT Connect更快适合狭窄通道，RRT*路径最优但计算量大。工程中我常用RRT Connect做初始规划，再用RRT*做路径优化——两全其美。