做机器人路径规划这些年我接触最多的采样类算法就是RRTRapidly-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_bias0.05到0.15初始探索期可以更低接近目标时再调高。max_iter1000到50000视场景复杂度。如果到上限没解优先查碰撞检测和步长而不是无脑加迭代。goal_thresholdstep_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, resolution0.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(), keylambda 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稳定得多。 
长河君 · 2026/10/15 1:07:07 · 阅读 1.2k RRT路径规划算法详解：从原理到工程落地与参数调优
做机器人路径规划这些年我接触最多的采样类算法就是RRTRapidly-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_bias0.05到0.15初始探索期可以更低接近目标时再调高。max_iter1000到50000视场景复杂度。如果到上限没解优先查碰撞检测和步长而不是无脑加迭代。goal_thresholdstep_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, resolution0.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(), keylambda 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稳定得多。