📚 课程目录第4章 RRT与RRT*算法

4. RRT与RRT*算法

各位同学，今天我们来聊聊路径规划里一个非常经典的方法——RRT算法。说实话，我刚入行那会儿，第一次看到RRT的，心里想的是：「这玩意儿真能用在机器人上？」后来在AGV项目里真刀真枪干了一回，才发现它比我想象的实用得多。

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

RRT，全称Rapidly-exploring Random Tree。说白了，就是让一棵树在空间里「乱长」，但长得有策略。

它的核心逻辑很简单：

从起点开始，随机撒点
找到树上离随机点最近的节点
朝随机点方向生长一小段
重复，直到碰到终点

你想想看，这就像你在黑暗里走路，每走一步就随便扔个石子，然后朝石子方向迈一步。虽然看起来笨，但最终总能走到想去的地方。

关键点：RRT不保证找到最短路径，但它保证——只要路径存在，且采样足够多，就一定能找到一条路径。这在复杂环境里特别有用。

1.2 RRT*的渐进最优性

RRT有个明显的毛病：它找到的路径往往弯弯绕绕，不是最优的。我在项目里遇到过，RRT规划出来的路径，机器人走起来像喝醉了酒——来回晃悠。

RRT*就是来解决这个问题的。它做了两件事：

重新选择父节点——新节点生成后，检查附近有没有更近的节点可以当爹
重新布线——如果新节点能让附近节点的路径更短，就「抢」过来当爹

嗯，这里要注意：RRT*的「渐进最优性」是有前提的——采样次数趋近无穷时，路径才趋近最优。实际项目中，我们不可能采样无限次，所以它只是「越来越优」，不是「最优」。

我的经验：在大多数实际场景里，RRT*跑个几千次采样，路径质量就已经比RRT好很多了。没必要追求理论上的最优，够用就行。

1.3 采样策略与步长调参

调参是RRT系列算法的灵魂。我见过太多人把参数调得乱七八糟，然后说算法不好用。其实，问题往往出在参数上。

参数	作用	调参建议
步长	每次生长的距离	步长太大容易撞墙，太小效率低。我一般设成机器人半径的1.5倍
采样概率	是否偏向终点采样	设个0.1~0.3的概率直接采样终点，能加速收敛
搜索半径	RRT*中重新布线的范围	跟步长挂钩，一般设成步长的2~3倍
最大迭代次数	算法终止条件	别设太大，500~2000次一般够用

避坑指南：我曾经在狭窄通道场景里，步长设得太大，结果树永远穿不过门缝。后来把步长缩小到通道宽度的1/3，问题就解决了。记住：步长要小于环境中最窄通道的宽度。

1.4 采样策略的进阶技巧

基本的均匀采样，在空旷环境里还行。但到了复杂环境，比如有大量障碍物、狭窄通道，均匀采样就效率很低了。

我常用的几种采样策略：

偏向采样——以一定概率直接采样终点方向，加速收敛
障碍物边界采样——在障碍物边缘附近多采样，更容易找到绕行路径
自适应采样——根据已探索区域密度，动态调整采样分布

举个例子，在仓库AGV项目里，货架之间只有1米宽的通道。如果用均匀采样，大部分点都落在货架区域，浪费计算资源。我改成偏向通道区域的采样，规划时间从3秒降到了0.8秒。

1.5 实战应用：复杂环境中的RRT*

来，我们看一个实际代码片段。这是我在一个巡检机器人项目里用过的RRT*核心逻辑：

def rrt_star(start, goal, obstacles, max_iter=1000, step_size=0.5):
    tree = {start: None}
    costs = {start: 0}
    
    for i in range(max_iter):
        # 采样：20%概率直接采终点，80%随机
        if random.random() < 0.2:
            sample = goal
        else:
            sample = random_sample(obstacles)
        
        # 找最近节点
        nearest = find_nearest(tree, sample)
        
        # 生长新节点
        new_node = steer(nearest, sample, step_size)
        
        if not collision(nearest, new_node, obstacles):
            # 找附近节点
            near_nodes = find_near(tree, new_node, step_size*2)
            
            # 选最优父节点
            best_parent = nearest
            best_cost = costs[nearest] + distance(nearest, new_node)
            
            for node in near_nodes:
                new_cost = costs[node] + distance(node, new_node)
                if new_cost < best_cost:
                    if not collision(node, new_node, obstacles):
                        best_parent = node
                        best_cost = new_cost
            
            # 加入树
            tree[new_node] = best_parent
            costs[new_node] = best_cost
            
            # 重新布线
            for node in near_nodes:
                new_cost = costs[new_node] + distance(new_node, node)
                if new_cost < costs[node]:
                    if not collision(new_node, node, obstacles):
                        tree[node] = new_node
                        costs[node] = new_cost
    
    return extract_path(tree, start, goal)

这段代码看起来简单，但有几个坑要注意：

find_near的搜索半径不能太大，否则计算量爆炸
碰撞检测要高效，我一般用AABB包围盒加速
终点判断别太严格，设个容忍距离，比如0.1米

我的习惯：在实际部署时，我会先跑一次RRT快速找条可行路径，然后用RRT*在路径附近精细优化。这样既快又好，比直接跑RRT*省一半时间。

1.6 知识体系总览

下面这张图，是我自己总结的RRT系列算法知识体系。你看一眼，心里就有谱了：

这张图把RRT系列的核心脉络理清楚了。你从基础版RRT入手，理解它的随机探索机制，然后升级到RRT*，学会如何优化路径质量。最后根据实际场景，选择合适的变种。

好了，这一章的内容就到这儿。记住：RRT算法的精髓不在于它多聪明，而在于它用随机性解决了确定性方法搞不定的复杂问题。下一章我们会深入聊RRT*的数学原理和代码实现细节。

可可精品资料库