4. RRT算法实现：从伪代码到工程落地

RRT（Rapidly-exploring Random Tree）这个名字，说白了就是「快速探索随机树」。我最早接触它是在2015年做七轴协作臂避障项目时，当时被它的「随机」二字迷惑过——总觉得随机出来的路径不靠谱。后来才发现，正是这种随机性让它在高维空间里比传统方法快了一个数量级。

这一章，咱们手把手把RRT的核心模块拆开。我会给出Python风格的伪代码，然后逐个讲清楚：节点采样、最近邻搜索、Steer函数、碰撞检测。嗯，每个模块我都踩过坑，咱们一起避过去。

4.1 整体伪代码框架

先看骨架。RRT的伪代码其实很简洁，核心就是一个while循环：

def rrt_plan(q_start, q_goal, max_iter=1000, step_size=0.1):
    # 1. 初始化树
    tree = Tree()
    tree.add_node(q_start)
    
    for i in range(max_iter):
        # 2. 随机采样
        q_rand = sample_random()
        
        # 3. 最近邻搜索
        q_near = nearest_neighbor(tree, q_rand)
        
        # 4. Steer扩展
        q_new = steer(q_near, q_rand, step_size)
        
        # 5. 碰撞检测
        if not collision_free(q_near, q_new):
            continue  # 有碰撞，跳过
        
        # 6. 添加节点和边
        tree.add_node(q_new)
        tree.add_edge(q_near, q_new)
        
        # 7. 检查是否到达目标
        if distance(q_new, q_goal) < goal_threshold:
            return extract_path(tree, q_new)
    
    return None  # 规划失败

这个框架我用了好多年，基本没大改过。你想想看，每次迭代就做五件事：采样、找最近、扩展、检测、添加。简单粗暴，但有效。

4.2 节点采样：随机与引导的平衡

采样是RRT的「眼睛」。最简单的做法就是均匀随机采样——在关节空间里每个关节角度范围内均匀抽一个值。但纯随机有个问题：树可能到处乱长，就是不长向目标。

我常用的改进是目标偏置采样：

def sample_random(goal_bias=0.1):
    if random.random() < goal_bias:
        return q_goal  # 以一定概率直接采样目标点
    else:
        # 均匀随机采样
        q = []
        for joint_range in joint_limits:
            q.append(random.uniform(joint_range[0], joint_range[1]))
        return q

goal_bias我一般取0.05~0.2。取大了树容易「急功近利」——总想一步跨到目标，结果被障碍物挡住，反复失败。取小了又太散漫。我在一个六轴码垛项目里试过，0.1是个不错的起点。

4.3 最近邻搜索：KD-Tree是标配

最近邻搜索就是找树里离q_rand最近的节点。最笨的办法是遍历所有节点算欧氏距离——树小的时候还行，树长到几千个节点就慢得让人抓狂。

我个人的建议：直接用KD-Tree。Python里scipy.spatial.cKDTree就很好用，查询复杂度O(log N)。

from scipy.spatial import cKDTree

class RRTTree:
    def __init__(self):
        self.nodes = []      # 节点列表
        self.parent = {}     # 父子关系
        self.kd_tree = None  # KD-Tree
    
    def add_node(self, q):
        idx = len(self.nodes)
        self.nodes.append(q)
        # 重建KD-Tree（增量式更新更高效，但这里简化）
        self.kd_tree = cKDTree(self.nodes)
        return idx
    
    def nearest_neighbor(self, q_rand):
        # 返回最近节点索引和距离
        dist, idx = self.kd_tree.query(q_rand, k=1)
        return self.nodes[idx], idx

注意：每次添加节点都重建KD-Tree其实有开销。如果节点数超过1万，我建议用增量式KD-Tree或者每加N个节点才重建一次。我在一个需要实时重规划的项目里，就是每加50个节点重建一次，性能提升很明显。

4.4 Steer函数：一步走多远

Steer函数决定了树「长」的方向和步长。说白了就是：从q_near出发，朝着q_rand方向走一步，但最多走step_size那么远。

def steer(q_near, q_rand, step_size):
    # 计算方向向量
    delta = [q_rand[i] - q_near[i] for i in range(len(q_near))]
    dist = math.sqrt(sum(d**2 for d in delta))
    
    if dist < step_size:
        # 目标就在一步之内，直接返回
        return q_rand[:]
    else:
        # 归一化后走一步
        step = [d / dist * step_size for d in delta]
        return [q_near[i] + step[i] for i in range(len(q_near))]

step_size怎么选？我有个经验公式：step_size ≈ 关节范围最大值的5%~10%。比如关节范围是[-180°, 180°]，那step_size取18°左右。太小了树长太慢，太大了容易跳过狭窄通道。

4.5 碰撞检测：最耗时的环节

碰撞检测是RRT的性能瓶颈。每次扩展都要检查q_near到q_new这一段路径是否与障碍物碰撞。你不能只检查q_new这一个点——万一中间穿模了呢？

标准做法是离散化检测：把路径分成若干小段，逐段检测。

def collision_free(q_near, q_new, resolution=0.01):
    # 计算路径长度
    dist = distance(q_near, q_new)
    # 按分辨率采样
    num_steps = max(2, int(dist / resolution))
    
    for i in range(num_steps + 1):
        t = i / num_steps
        # 线性插值
        q = [q_near[j] + t * (q_new[j] - q_near[j]) for j in range(len(q_near))]
        
        # 正运动学：关节角 → 末端位姿 + 连杆位姿
        poses = forward_kinematics(q)
        
        # 对每个连杆做碰撞检测
        for pose in poses:
            if check_collision(pose):
                return False  # 有碰撞
    
    return True

这里有个关键点：正运动学要算所有连杆的位姿，不只是末端。我见过有人只检查末端和基座，结果机械臂中间连杆穿过了障碍物——那画面太美我不敢看。

4.6 完整伪代码整合

把上面所有模块拼起来，就是一个可运行的RRT规划器了。我贴一个完整版的伪代码，注意看各个模块之间的衔接：

class RRTPlanner:
    def __init__(self, joint_limits, obstacles, step_size=0.1, goal_bias=0.1):
        self.joint_limits = joint_limits
        self.obstacles = obstacles
        self.step_size = step_size
        self.goal_bias = goal_bias
        self.tree = RRTTree()
    
    def plan(self, q_start, q_goal, max_iter=1000):
        self.tree.add_node(q_start)
        
        for i in range(max_iter):
            # 采样
            q_rand = self.sample(self.goal_bias, q_goal)
            
            # 最近邻
            q_near, _ = self.tree.nearest_neighbor(q_rand)
            
            # Steer
            q_new = self.steer(q_near, q_rand, self.step_size)
            
            # 碰撞检测
            if not self.is_collision_free(q_near, q_new):
                continue
            
            # 添加节点
            idx_new = self.tree.add_node(q_new)
            self.tree.add_edge(q_near, q_new)
            
            # 检查目标
            if self.distance(q_new, q_goal) < 0.05:
                return self.extract_path(idx_new)
        
        return None  # 规划失败

这个框架我一直在用，从六轴工业臂到七轴协作臂，从静态环境到动态避障，核心逻辑基本没变过。你把这个吃透了，后面学RRT*、Informed RRT*、RRT-Connect都会轻松很多。