做机器人路径规划这些年，我接触最多的采样类算法就是RRT，Rapidly-exploring Random Tree。它和A*那种栅格搜索完全不是一个路子：不建网格、不求全局最优，靠随机采样在构型空间里长出一棵树，从起点一路探到目标点。机械臂抓取、无人车避障、无人机绕飞这类高自由度场景里，RRT几乎是默认的“起手式”。对我来说，它最大的价值是“写起来快、跑起来稳、高维环境也能出解”。但这玩意儿也是出了名的“玄学算法”，同样的代码、同样的参数，换个随机种子路径就能完全不一样。这篇文章把我这些年用RRT的完整心得梳理一遍，从算法原理到参数调节再到踩坑实录，适合刚入门想做机器人规划、或者已经跑通Demo但项目现场老出问题的读者。
1. RRT到底在解决什么问题
1.1 从构型空间说起
谈起运动规划，绕不开一个概念：构型空间，也就是C-space。机器人每个自由度取一个值，就组成一个构型；所有构型的集合就是构型空间。比如二维平面上平移的移动机器人，C-space是二维的，用(x, y)表示；六自由度的机械臂，C-space至少是六维的。障碍物在物理空间看着很规则，映射到C-space之后会长得奇奇怪怪，而且维度越高越难用解析式去描述。RRT这类采样算法的核心思路很朴素：既然障碍物边界算不出来，那我就不算边界，只在C-space里随机撒点，凡是落在安全区域的点就保留，凡是撞到障碍的点就丢弃。规划问题就这样被转化成“能不能用一系列无碰撞的采样点把起点和终点连起来”。
这个思路的好处是，它完全不依赖环境的具体建模方式。你只要提供一个“碰撞检测”接口，告诉算法“某个构型或某段构型轨迹是否安全”，RRT就能规划。实际工程里，这个接口可以是机器人几何模型对障碍物的相交测试，可以是Octomap上的占用查询，也可以是三维距离场判断。用什么底层结构都无所谓，算法本身不关心，这给了工程落地极大的自由度。
1.2 和A*对比，为什么高维空间必须换思路
很多入行不久的同学习惯拿A套一切，这其实把栅格地图给人用的惯性思维带到了机器人领域。A需要先把连续空间离散成栅格，再做图搜索；环境维度一上去，栅格数量就按指数暴涨。假设一个六自由度机械臂，每个关节方向只分100份，栅格总数就是100的6次方，也就是10的12次方个状态，别说搜索，光存储就没戏。RRT绕开了这个问题：它不预先离散空间，而是按需采样，样本数只跟规划的复杂度有关，不跟空间体积直接挂钩。
打个比方。A的做法是把整个城市的地图全部画成等间距方格，然后在这个网格上找路；RRT是拿出一张白纸，随机扔点，再把相邻点用线段连起来，只要线段不碰到障碍就算走得通。前者越精细越慢，后者无论城市多大，只要路存在，撒点密度够了就能找到。当然反过来，A在二维小范围地图上通常能找到比较短的路径，而RRT随机撒点找出来的路径往往七拐八绕，这就是后面RRT*和剪枝优化要解决的问题。
1.3 RRT的适用边界和它在规划流水线里的位置
明确了优势，也要讲清楚RRT不适合什么。第一，它不适合对全局最优有强要求的场景，经典RRT找到的是“一条可行解”，不是“最短解”；第二，它不适合环境已经可以小块离散化的简单场景，比如室内扫地机走栅格地图，A*或JPS更快更好；第三，纯RRT给出的是一条折线路径，和机器人的实际执行轨迹之间还有很大距离。
在我的工程习惯里，RRT从来不是“终点算法”，而是整个规划管线的第一段。它负责快速找到一条无碰撞的几何参考路径，后面再接剪枝、平滑、轨迹规划和局部跟踪。很多同学把RRT的结果直接扔给机器人执行，发现机器人走起来一顿一顿的，就说RRT不好用，其实冤枉它了，RRT只负责回答“这条路能不能走通”，不负责回答“怎么走得优雅”。
2. 经典RRT的核心机制拆解
2.1 算法主干：随机采样、最近邻、步长扩展
经典RRT的流程用一个伪代码来讲最直观。
tree = [start] for i in range(max_iter): if random() < goal_bias: sample = goal else: sample = random_sample() nearest = nearest_neighbor(tree, sample) new_node = steer(nearest, sample, step_size) if collision_free(nearest, new_node): tree.append(new_node) if distance(new_node, goal) < goal_threshold: return extract_path(tree, new_node) return None每一步的核心动作就三个：采样、找最近节点、朝样本方向走一步。为什么不是直接把采样点加入树？因为随机采样点可能离树很远，直接连过去容易跨越障碍，而且扩展步长过大会让路径不可控。steer函数做的事情是：计算从nearest指向sample的单位方向，然后沿这个方向走一个固定距离，得到new_node。这相当于控制树的“生长步幅”，让每次扩展都在安全裕度范围内。
采样时通常会引入goal_bias，即以一定概率直接把目标点作为采样样本。这个技巧作用很大，否则树只顾着随机探索，可能满空间乱长却迟迟连不到目标。但goal_bias也不能太高，因为树要扩展向目标，如果中间隔着障碍，这些偏向目标的采样会反复被碰撞检测拒绝，浪费计算。我自己常用的起点是0.05到0.15，后面的调参部分再细说。
2.2 概率完备和次优性从哪来
RRT最常被引用的理论性质是概率完备：只要有一条可行路径存在，并且采样代码没有刻意回避某个区域，当迭代次数趋于无穷时，找到解的概率收敛到1。这个性质听上去很强，但实际工程里，无穷迭代是不可能的。更常见的画面是：窄缝场景下，采样点落在可行通道内的概率本来就很低，树在窄缝外绕了几万次都进不去，规划超时失败。这是采样算法在复杂环境下的经典痛点。
为什么经典RRT不保证最优？因为它的目标只是连通，树的生长偏向“最新扩展节点附近的覆盖”，而最近节点未必在代价最小的方向上，因此找到的第一条路径大概率绕路。加上随机性，同一环境下两次规划的结果可能差上一大截。这在后面的RRT*、Informed RRT*里会改善。所以做工程要有一个心理预期：经典RRT是用来“快速找可行解”的，不是用来“寻路最短”的。
2.3 三个工程关键模块：碰撞检测、最近邻搜索、步长控制
跑通算法Demo很容易，但放到项目里，真正的瓶颈永远在三个地方。
第一是碰撞检测。RRT扩展出每个新节点，都要对“nearest到new_node的这段线段”做碰撞检查。最常用的做法是把线段离散成若干点，逐个判断点是否落在障碍内。离散分辨率很关键：太粗，可能从障碍物缝隙里“钻过去”，甚至直接跳过障碍；太细，每次扩展的耗时会骤增。我的经验是让检测点间距不大于机器人半径的一半，或者不大于场景里最窄通道宽度的一半，两者取小。碰撞检测是整个RRT里最慢的环节，它的实现质量直接决定规划的实时性。
第二是最近邻搜索。树长了之后，逐个比较所有节点找最近邻，时间复杂度是O(n)，n到几万之后就很吃力。工程上优先用KD-Tree，或者把构型空间离散成网格做空间哈希。Python可以直接用SciPy里的KDTree实现，C++可以用各开源库里的kdtree，或者用更轻量的一维化哈希都不难。我自己早期写原型图省事用了线性扫描，树长到2万节点后，单次最近邻查询明显拖慢整体流程，换KD-Tree后效率提升非常明显。
第三是步长。步长决定了树的扩展粒度，也是规划成败的敏感参数。步长太大，树容易跳到障碍另一侧，把一个窄通道“跳过去”，或者产生很粗的锯齿路径；步长太小，树生长缓慢，到目标可能要几十万次迭代。具体怎么定，见第4章的参数表，但核心原则始终是：步长应当小于环境中最窄的可通行通道宽度，否则理论上的连通性再好，实际也连不过去。
3. 从RRT到RRT与Informed RRT：最优化的演进
3.1 RRT*的rewire机制
RRT*只比RRT多了一个“重连”步骤，但效果变化巨大。在RRT扩展出一个新节点之后，它会在某个半径范围内搜索所有邻近节点，然后做两件事。第一件事叫做选择父节点：从这些邻近节点里，找哪一个作为new_node的父节点，能够让“起点到该节点再到new_node”的总代价最小，即使该节点不是几何上最近的。第二件事叫做重布线：再反过来检查，如果把new_node作为邻近节点的父节点，能否让每个邻近节点从起点来的代价变小，如果能且无碰撞，就重新连接。
正是这个rewire操作，让RRT*具备了渐进最优性：随着迭代次数增加，路径代价会不断被优化，并最终收敛到最优解。这里有一个理论参数，邻域搜索半径r一般取gamma乘以(log n / n)再开1/d次方，其中d是构型空间维度。理论上n越大半径越小，但在实际工程中，n比较小时按公式算出来的半径会非常大，导致每个新节点要检查一堆邻近节点，碰撞检测开销暴涨。所以我很少严格套公式，而是给它设一个上限，比如step_size的2到3倍，半径取理论值和上限值中较小的那个。这样代码简单不少，优化效果也够用。
3.2 Informed RRT*的椭圆形采样加速
RRT虽然渐进最优，但改进路径的过程很慢，因为树仍然在全空间均匀采样，大量采样点并不会改善当前路径。Informed RRT的思路是在找到第一条可行路径后，用这条路径的代价构造一个以起点和终点为焦点、路径代价为长轴的超椭圆，然后只在这个椭圆内采样。
这个想法很直白：任何能改进当前路径的节点，到起点和终点的距离之和一定小于当前路径总代价，否则它不可能优化出更短路径。所以椭圆之外的采样点对优化过程没有意义，可以完全忽略。椭圆长轴等于当前最佳路径代价c_best，焦距就是起点到终点的距离c_min，短半轴则是sqrt(c_best^2 - c_min^2) / 2。每次找到更好的路径，椭圆收缩一点，采样区域跟着变小，收敛速度显著变快。
实际使用中，Informed RRT很适合“先快速找到一个粗糙解，再用剩余时间不断优化”的规划场景。不过要注意，它要求已经找到过一条初始路径才会启动采样约束；如果第一次找解本身就失败，它不会比RRT更强，所以在窄通道极难连通的环境里，还是先用RRT-Connect把解找到再说。
3.3 RRT-Connect与PRM，不同场景下的选择
RRT-Connect算是工程里最常见的“实战改良版”。它同时从起点和终点各长一棵树，两棵树交替扩展，每次扩展之后还会尝试直接连接另一棵树最近节点。这种方式能大幅度减少窄通道里的无效探索，因为两棵树分别从两端向中间“夹击”，即使中间通道狭窄，只要有一侧的树伸进去，另一侧连接成功的概率也很高。机械臂规划场景里，我基本默认先把RRT-Connect跑通。
PRM，概率路线图，是另一条路线。它在规划前先大量采样建一张图，称为roadmap，之后每次查询只需要在图上做一次A*搜索。PRM适合环境基本不变、需要反复规划的静态场景；RRT则适合一次性规划或动态环境中的快速重规划。很多新人不理解为什么会有多种算法，其实没有谁替代谁，只有匹配不匹配。
4. 实操落地：参数怎么调、代码怎么写、路径怎么修
4.1 工程上我常用的参数表
下面这份参数表是我在不同项目里的经验值，可以直接当作起点，然后按环境尺寸微调。
- step_size：地图对角线长度的0.02到0.05，或者取机器人半径的0.5到1倍。原则是明显小于最窄通道宽度。
- goal_bias：0.05到0.15，初始探索期可以更低，接近目标时再调高。
- max_iter：1000到50000，视场景复杂度。如果到上限没解，优先查碰撞检测和步长，而不是无脑加迭代。
- goal_threshold：step_size的一半比较稳，给得太大可能在目标附近草率结束，路径明显弯曲。
- collision resolution：取机器人半径的一半或最小障碍尺寸的一半。
- RRT*的邻域半径：step_size的2到3倍。
我为什么要强调“先看环境尺寸”？因为RRT所有参数最终都服务于一个目标：让树在“可通行区域”内高效生长。比如一个100米乘100米的大场地，步长设为0.1米，那树从起点走到终点可能要几百万步，完全不可行；反过来在狭窄管道场景里，步长设为10米，树永远不可能进入管道。参数和场景的尺度绑定是第一原则。
4.2 一份可以直接改的代码骨架
我给一个完整但简化的Python实现，二维场景，圆形障碍物，非常适合用来理解算法主体。最近邻位置用了线性扫描，方便阅读；真正工程里换成KD-Tree即可。
import math import random class Obstacle: def __init__(self, x, y, r): self.x, self.y, self.r = x, y, r def distance(a, b): return math.hypot(a[0] - b[0], a[1] - b[1]) def steer(from_node, target, step): dist = distance(from_node, target) if dist < step: return target t = step / dist return (from_node[0] + t * (target[0] - from_node[0]), from_node[1] + t * (target[1] - from_node[1])) def is_collision_free(a, b, obstacles, resolution=0.1): dist = distance(a, b) steps = max(1, int(dist / resolution)) for i in range(steps + 1): t = i / steps p = (a[0] + t * (b[0] - a[0]), a[1] + t * (b[1] - a[1])) for ob in obstacles: if distance(p, (ob.x, ob.y)) < ob.r: return False return True def rrt_plan(start, goal, obstacles, config): tree = {start: None} for _ in range(config["max_iter"]): if random.random() < config["goal_bias"]: sample = goal else: sample = (random.uniform(0, config["world_x"]), random.uniform(0, config["world_y"])) nearest = min(tree.keys(), key=lambda p: distance(p, sample)) new_node = steer(nearest, sample, config["step_size"]) if not is_collision_free(nearest, new_node, obstacles, config["resolution"]): continue tree[new_node] = nearest if distance(new_node, goal) < config["goal_threshold"]: tree[goal] = new_node path = [] node = goal while node is not None: path.append(node) node = tree[node] return path[::-1] return None这个实现能跑，但只算“教学版”。真到工程里，我会把采样改成Informed采样、把最近邻换成KD-Tree、把碰撞检测换成距离场查询，并且给规划器加超时和迭代预算。理论算法要落地，每一步都是性能抠出来的。
4.3 后处理：剪枝、平滑、与轨迹规划的衔接
RRT直接输出的路径通常是锯齿状折线，节点密集、代价偏高。第一步我会做剪枝：从起点开始，尽量往后找能直连且无碰撞的节点，把中间点删掉。重复几轮之后，路径会明显“拉直”。剪枝不改变算法性质，只是减少冗余。
第二步是平滑。常见做法是用三次B样条或贝塞尔曲线对路径点拟合，把尖角变成圆滑曲线。但这里有个工程陷阱：平滑后的曲线可能穿过障碍物，必须逐段重新做碰撞检测，碰撞的地方局部调整控制点，或者保留剪枝后的折线作为安全参考，只对可通行区间平滑。
第三步是衔接轨迹生成。RRT给的是几何路径，机器人的实际执行还需要时间信息、速度限制和加速度限制。业界常见的做法是把几何路径交给轨迹优化器，生成带时间戳的轨迹，再用局部控制器跟踪。很多同学困惑“RRT规划出来的路径机器人走不了”，其实不是RRT的问题，是少了后半段轨迹规划。这步接不上，再好的全局路径也只是纸面结果。
5. 常见问题与现场排查经验
5.1 规划失败或超时的排查链路
遇到RRT规划失败，不要急着调参数，按顺序排查。先检查起点和终点本身是否在障碍物内；再看碰撞检测的离散分辨率是否合理，树能否穿过细障碍；接着观察树是否在持续生长，如果树始终停在某个区域，说明采样或步长有问题；最后再调max_iter。我见过不少“规划失败”最后都定位到碰撞检测bug，比如把“点是否在圆内”的半径判断写反了，或者漏了最后一个检测点。RRT的随机性会放大这类小错误，表现成“时灵时不灵”，特别迷惑人。
另一个常见坑是目标点被反复作为goal_bias采样，却因为终点附近有障碍而一直被拒绝，消耗大量迭代。这时可以把goal_bias调低，或者改成在目标附近小范围采样，不要死死咬住精确目标点。
5.2 路径抖动和毛刺的原因与对策
RRT的“抖动”在动态规划场景中特别致命。明明环境没大变，上一帧规划的路径走右边的门，这一帧突然走左边的门。原因很简单：每次规划都是重新跑随机树，只要有随机性，解就会漂移。如果下游的轨迹规划还加了时间维度，抖动会被进一步放大。
我习惯的解决办法有三个。第一，把上一帧的路径作为初始参考或起点放入新树的初始化，让新规划尽量沿着旧路径继续；第二，用RRT*这类渐进最优算法，即便每次只迭代少量次数，解的稳定性也会好很多；第三，规划完成后做剪枝和平滑，减少后期轨迹优化器的负担。路径抖动不是bug，而是随机采样方法的固有属性，工程上只能“控”不能“消”。
5.3 动态环境中实时规划的工程处理
动态环境一般不会指望RRT实时避让所有运动障碍，RRT的强项是静态环境下的快速全局规划。碰到移动障碍物多的场景，我通常搭建两级结构：前端用RRT做全局引导，后端用局部避障算法做实时反馈，全局路径负责告诉机器人“大致往哪走”，局部算法负责“别撞眼前的东西”。
如果坚持用RRT直接做重规划，就要严格控制单次规划的时间预算。比如规定最多30毫秒，超时没有结果就沿用上一帧全局路径，或者让机器人减速到安全状态。没有时间预算的RRT是危险的，因为迭代次数一高，响应延迟就不可控。做安全关键系统时，一定要把“规划超时”当成一个正常分支来处理，而不是异常。
6. 我在不同机器人平台上用RRT的体会
6.1 机械臂场景：关节空间与步长的坑
机械臂的RRT一般直接在关节空间采样，每个样本就是一组关节角。这里最容易踩的坑是度量不一致：有的关节运动范围是±180度，有的只有±30度，如果不做归一化直接算欧氏距离，距离值会被范围大的关节主导，树形同虚设。我第一次做7轴机械臂规划时就吃过这个亏，树一直朝某一侧长，后来把每个关节除以自己的运动范围再算距离，问题立刻消失。
另外机械臂的碰撞检测最耗时，因为要拿整个连杆模型去对场景做相交测试。工程上通常会在距离场中查询，或者先用包围盒粗筛，再对可能相交的部分做精细检测。机械臂场景里，RRT-Connect带KD-Tree和距离场，基本是起步配置。
6.2 移动机器人：从直线扩展到曲线扩展
移动机器人和机械臂有个重大区别：它有移动方向，不能原地横移。如果直接把RRT的steer函数用直线扩展，生成的路径对差速底盘来说是无法执行的。工程上常用Dubins曲线或Reeds-Shepp曲线替代直线段，也就是说，从near节点向采样点生长一段“满足转弯半径约束的曲线”，再把曲线离散成点做碰撞检测。RRT的框架完全不用改，只换扩展方式和碰撞检测对象即可。
这也解释了为什么RRT这种“采样基础”的框架生命力这么强：它不关心具体运动学，只要能定义“局部扩展方式”和“碰撞检测”，就能移植到各种平台。我后来做无人车的全局路径规划，用的还是同一套RRT框架，只是把steer函数换成了自行车模型的航向积分。
6.3 无人机场景：三维扩展与人为约束
无人机把RRT扩展到三维，逻辑上很简单，把坐标从(x,y)换成(x,y,z)即可。真正麻烦的是约束：无人机往往有最大爬升角、最小转弯半径、巡航高度限制等。我一般不会让RRT直接搜索原始状态空间，而是把这些约束拆进采样分布和扩展函数，比如限制采样高度范围、扩展时加入最大转弯角度检查。这样生成的路径天然满足平台限制，后续处理也省事。
如果任务要求时间最优或能量最优，几何RRT只是起点，后面通常接带时间约束的轨迹优化。方向对了，后面就好走了。
最后，我个人在实际项目里的体会是：不要迷信随机采样算法，随机只是手段，不是目的。碰到新问题，我最先做三件事，把碰撞检测写对，把步长定合理，把最近邻做高效。这三件基础工作到位，RRT基本不会太差；在这之上再去追求RRT*、Informed RRT*，才是真正的性能提升。如果只给一个技术建议，我会说先用RRT-Connect把流程跑通，再换RRT*求最优，工程上要比纯RRT稳定得多。
