3. RRT算法原理与实现：快速探索随机树、节点扩展策略、目标偏向与概率完备性

说到路径规划，我个人最偏爱的一类算法就是RRT。为什么？因为它简单、有效，而且特别适合高维空间。你想想看，机械臂的关节空间动辄六七个维度，传统的栅格法根本跑不动，但RRT却能轻松应对。

RRT的全称是Rapidly-exploring Random Tree，中文叫快速探索随机树。这名字起得很直白——它就是用随机采样的方式，快速在空间里长出一棵树，直到这棵树碰到目标点。说白了，就是让树自己去找路。

3.1 快速探索随机树的核心思想

RRT的基本思路其实很朴素。我们从起点开始，随机在空间里撒点，然后让树朝着这些随机点生长。每次只长一小步，慢慢延伸出去。这样反复迭代，树就会逐渐覆盖整个空间。

我记得第一次在六轴机械臂上跑RRT时，看着那棵树在关节空间里一点点蔓延开来，真的有种「它在探索未知世界」的感觉。嗯，这大概就是算法工程师的浪漫吧。

RRT的算法流程可以概括为以下几个步骤：

1. 初始化：以起点作为树的根节点
2. 随机采样：在状态空间中随机生成一个点
3. 寻找最近邻：在树上找到离随机点最近的节点
4. 扩展：从最近邻节点向随机点方向生长一段距离，得到新节点
5. 碰撞检测：检查新节点以及连接路径是否与障碍物碰撞
6. 添加节点：如果无碰撞，将新节点加入树中
7. 终止判断：如果新节点接近目标点，则返回路径；否则回到步骤2

这里面最关键的步骤就是「扩展」。怎么扩展？我画了一张图，你看一眼就明白了。

从图上你能看到，每次扩展就是沿着「最近邻节点 → 随机采样点」的方向走一个固定步长。这个步长很关键——太大会跳过狭窄通道，太小又会让树长得很慢。

3.2 节点扩展策略的细节

扩展策略看似简单，但实际工程里有很多门道。我把自己踩过的坑和总结的经验整理了一下。

3.2.1 步长选择

步长ε的选择直接影响算法性能。我一般这样设：

场景	步长建议	说明
开阔空间	较大（空间尺寸的5%-10%）	快速覆盖，减少迭代次数
狭窄通道	较小（空间尺寸的1%-3%）	提高通过狭窄区域的概率
高精度需求	动态步长	远处大步，近处小步

3.2.2 最近邻搜索

每次扩展都要在树上找一个离随机点最近的节点。如果树很大（几千上万个节点），暴力搜索就太慢了。我建议用KD-Tree或者R-Tree来加速。

这里给一个简单的Python实现框架：

import numpy as np
from scipy.spatial import KDTree

class RRT:
    def __init__(self, start, goal, obstacle_check, step_size=0.1, max_iter=10000):
        self.start = np.array(start)
        self.goal = np.array(goal)
        self.obstacle_check = obstacle_check  # 碰撞检测函数
        self.step_size = step_size
        self.max_iter = max_iter
        
        # 树结构
        self.nodes = [self.start]
        self.parents = [-1]  # 父节点索引
        self.kd_tree = KDTree([self.start])
    
    def extend(self, random_point):
        # 1. 找最近邻
        dist, idx = self.kd_tree.query(random_point)
        nearest = self.nodes[idx]
        
        # 2. 计算扩展方向
        direction = random_point - nearest
        direction_norm = np.linalg.norm(direction)
        if direction_norm < 1e-6:
            return False
        
        # 3. 生成新节点
        new_node = nearest + (direction / direction_norm) * self.step_size
        
        # 4. 碰撞检测
        if not self.obstacle_check(nearest, new_node):
            return False
        
        # 5. 加入树
        self.nodes.append(new_node)
        self.parents.append(idx)
        self.kd_tree = KDTree(self.nodes)  # 实际应用应增量更新
        
        return True

3.3 目标偏向策略

标准的RRT有个问题——它完全随机采样，导致树在空间里乱长，可能很久都碰不到目标点。你想想看，如果目标在右边，树却往左边长，那不是白费功夫吗？

目标偏向（Goal Bias）就是解决这个问题的。做法很简单：每次采样时，以一定概率直接选择目标点作为采样点，而不是随机生成。

我一般这样设：

• 偏向概率 p = 0.05 ~ 0.2：概率太小效果不明显，太大又会让树失去探索能力
• 动态调整：前期探索为主（p小），后期偏向为主（p大）

代码实现就一行改动：

def sample(self):
    if np.random.random() < self.goal_bias:
        return self.goal  # 以概率 p 直接选目标
    else:
        return np.random.uniform(self.low, self.high)  # 随机采样

3.4 概率完备性

这个词听起来很学术，但说白了就是：如果路径存在，RRT最终一定能找到它吗？

答案是：当迭代次数趋于无穷时，RRT找到路径的概率趋近于1。这就是概率完备性。

为什么是「概率」而不是「确定」？因为RRT依赖随机采样，你永远无法保证某一次运行一定能找到路径。但只要给够时间，它几乎肯定能找到。

我整理了一下影响概率完备性的因素：

因素	影响	怎么办
步长过大	可能跳过狭窄通道	减小步长或使用变步长
障碍物复杂	需要更多采样点	增加迭代次数上限
维度太高	空间体积爆炸式增长	结合启发式方法（如RRT*）
目标偏向过度	丧失探索能力	控制偏向概率在合理范围

3.5 完整算法流程总结

最后，我把整个RRT算法的流程用一张图串起来，方便你对照理解。

从流程图你能看到，RRT的核心就是一个循环：采样→扩展→检测→添加。每次迭代只做这几件事，简单但有效。