3. RRT算法原理：快速随机搜索树的基本思想、树扩展流程、概率完备性分析
各位好，我是老张。今天咱们聊RRT——快速随机搜索树。这玩意儿在机械臂运动规划里，可以说是“万金油”级别的存在。我最早接触它是在一个七轴协作臂的避障项目里，当时客户要求机械臂在堆满料箱的狭小空间里取放零件，传统方法根本跑不动。后来换了RRT，问题迎刃而解。
说白了，RRT的核心思想就一句话：用随机采样在C空间里“长”出一棵树，直到它碰到目标点。听起来简单，但背后有不少门道。
3.1 基本思想：为什么是“树”？
你想想看，机械臂的运动规划，本质上是在高维C空间里找一条从起点到终点的连续路径。传统方法比如PRM，是先建图再查路径。而RRT不一样——它边探索边建树，而且这棵树天生就是一条路径。
为什么用树而不是图？我个人习惯从工程角度理解：
- 树结构天然无环：每次扩展只加一个节点、一条边，不会出现冗余回路。内存开销小，回溯路径也简单。
- 生长方向可控：你可以通过采样策略引导树往目标方向长，也可以让它均匀探索整个空间。
- 增量式构建：不需要事先知道整个空间长什么样，边采样边扩展，适合在线规划。
核心洞察：RRT的“随机”不是瞎蒙，而是用概率手段在高维空间里做“有偏采样”。你每次采样，其实都在问一个问题——“下一步往哪走，才能更快碰到目标？”
3.2 树扩展流程：一步一步来
RRT的扩展流程，我习惯把它拆成5步。每一步都有坑，咱们一个一个说。
- 初始化：把起点作为树的根节点。
- 随机采样：在C空间里随机选一个点
q_rand。 - 找最近邻：在现有树里找到离
q_rand最近的节点q_near。 - 扩展：从
q_near朝q_rand方向走一步（步长δ），得到新节点q_new。 - 碰撞检测：检查
q_near到q_new的边是否与障碍物碰撞。如果安全，就把q_new加入树中。
重复2-5步，直到 q_new 到达目标区域，或者达到最大迭代次数。
我的经验：步长 δ 的选择很关键。设太大，容易跳过狭窄通道；设太小，树长得太慢，收敛慢。我一般取C空间对角线长度的1%~5%，然后根据实际效果微调。
下面这张图展示了RRT的扩展过程：
3.3 概率完备性：为什么RRT一定能找到路径？
这个问题我当年也纠结过。RRT是随机算法，凭什么保证能找到路径？
答案是：概率完备性。意思是，如果路径存在，随着采样点数量趋于无穷，RRT找到路径的概率趋近于1。
为什么会这样？我换个角度解释：
- RRT的采样是均匀的（或者有偏的），只要空间里存在一条可行路径，它周围就存在一个“管状区域”。
- 随着采样点增多，树会越来越密，最终一定会“钻”进这个管状区域，沿着路径长到目标。
- 数学上可以证明，RRT的覆盖密度随迭代次数指数增长。
注意：概率完备性≠一定能找到。它只保证“如果时间足够长，几乎肯定能找到”。实际应用中，你不可能无限迭代。所以RRT的变种（如RRT*、Informed RRT）都在想办法加速收敛。
我曾经在一个六轴机器人项目里遇到过极端情况：狭窄通道只有关节角度的1°宽。RRT跑了10万次都没找到路径。后来我改用RRT*，配合双向搜索，5000次就搞定了。嗯，这就是理论和实践的差距。
3.4 RRT的优缺点对比
| 维度 | 优点 | 缺点 |
|---|---|---|
| 探索能力 | 能快速覆盖大范围C空间 | 在狭窄通道中效率低 |
| 路径质量 | 总能找到一条路径（概率完备） | 路径通常不平滑，有锯齿 |
| 实现难度 | 代码简单，核心逻辑不到50行 | 参数（步长、采样策略）调优靠经验 |
| 适用场景 | 高维空间、动态环境 | 对路径质量要求高的场景需后处理 |
3.5 代码实现：核心逻辑
下面是一个简化版的RRT核心代码。我剔除了碰撞检测和可视化，只保留树扩展逻辑。你可以在自己的项目里直接套用这个框架。
class RRT:
def __init__(self, start, goal, obstacle_list, step_size=0.1, max_iter=1000):
self.start = Node(start)
self.goal = Node(goal)
self.obstacle_list = obstacle_list
self.step_size = step_size
self.max_iter = max_iter
self.node_list = [self.start]
def plan(self):
for i in range(self.max_iter):
# 1. 随机采样
q_rand = self.sample()
# 2. 找最近邻
q_near = self.nearest(q_rand)
# 3. 扩展
q_new = self.steer(q_near, q_rand)
# 4. 碰撞检测
if self.collision_free(q_near, q_new):
self.node_list.append(q_new)
# 5. 检查是否到达目标
if self.distance(q_new, self.goal) < self.step_size:
return self.extract_path(q_new)
return None # 未找到路径
def sample(self):
# 以一定概率直接采样目标点（引导搜索）
if random.random() < 0.05:
return self.goal.position
else:
return [random.uniform(-5, 5), random.uniform(-5, 5)]
def nearest(self, q_rand):
# 遍历所有节点，找欧氏距离最近的
return min(self.node_list, key=lambda node: self.distance(node, q_rand))
def steer(self, q_near, q_rand):
# 从q_near朝q_rand走一步
dx = q_rand[0] - q_near.position[0]
dy = q_rand[1] - q_near.position[1]
dist = math.hypot(dx, dy)
if dist < self.step_size:
return Node(q_rand)
else:
ratio = self.step_size / dist
new_x = q_near.position[0] + dx * ratio
new_y = q_near.position[1] + dy * ratio
return Node([new_x, new_y])避坑指南：我曾经在 sample() 函数里忘了加目标偏置，结果树在空旷区域乱长，死活不往目标方向走。后来我加了5%的概率直接采样目标点，收敛速度提升了3倍。这个技巧叫“目标偏置采样”，几乎所有工程实现都会用。
3.6 工程中的常见问题
最后聊几个我在项目中踩过的坑：
- 步长与障碍物尺寸不匹配：如果步长大于狭窄通道宽度，树永远穿不过去。解决办法是动态调整步长，或者用RRT*的“rewire”机制。
- 高维空间采样效率低：七轴机械臂的C空间是7维，均匀采样效率极低。我一般用“任务空间引导”——先在笛卡尔空间采样，再逆解到关节空间。
- 路径后处理：RRT出来的路径通常很“丑”，有大量锯齿。我习惯用B样条或五次多项式做平滑，再用时间最优轨迹插值。