我最早接触RRT的时候心里就一个想法这不就是随机撒点碰运气吗后来在一个给仓储AGV做全局路径规划的项目里传统图搜索算法在一张布满货架的栅格地图上动不动就“无解”反而是这个看似瞎跑的算法用几千次随机采样就把路径给掏出来了。从那时候起我开始认真啃RRT及其变种一发不可收拾。RRT的全称是Rapidly-exploring Random Tree快速探索随机树属于基于采样的运动规划算法。它跟传统A*、Dijkstra最大的区别在于A需要显式构建栅格或拓扑图而RRT只需要一个“碰撞检测”接口就能在高维、复杂约束空间里搜索可行路径。名字里带“随机”看起来不靠谱但它恰恰是解决机械臂高维规划、自动驾驶绕障、无人机室内导航这类问题的利器。这篇文章我会按自己的学习路径来写先讲清楚RRT为什么能工作然后手写一遍基础版代码再逐个拆解RRT、RRT-Connect、Informed RRT*等经典变种最后分享工程部署中真正会踩的坑。无论你是刚入行做机器人导航的学生还是已经接了移动底盘项目的工程师这套内容都能直接用。1. RRT为什么能“随机瞎跑”核心原理拆解1.1 从“拿针找洞”到“用绳子包住洞”理解RRT先忘掉所有“智能算法”的刻板印象。想象一个蒙着眼睛的人站在房间一角要走到对角但房间里摆满了家具。他每走一步就随机朝某个方向丢一颗豆子豆子落点就是他下一段路的目标点。豆子落在家具上碰撞就换个方向重丢没撞上就朝那条路迈一步。重复几千次他慢慢在房间里织出一张“豆子路线网”最终总有一颗豆子丢到了对角门口一条从起点到终点的路径就这样连出来了。这是RRT最核心的直观理解树上每次扩展从状态空间里随机采样一个点丢豆子在已生成的树上找到离它最近的节点向它方向生长固定步长如果这一段路无障碍就把它加进树里。之所以叫“快速探索随机树”是因为这套逻辑让树以极快速度向未知区域扩张而不是像栅格搜索那样老老实实扫描每一格。这种从“全局建模”退化为“局部探索”的思路带来一个巨大优势你不需要提前知道环境的完整地图不需要把空间离散成网格或路点图。只要你能写一个函数判断“线段A到B是否穿过障碍”RRT就能跑。对于六自由度机械臂这种动辄十几维的规划问题来说栅格法的内存开销直接爆炸而RRT的复杂度主要取决于采样次数和碰撞检测次数跟维数增长基本不挂钩。1.2 算法骨架只有四件事把RRT拆开看每一轮迭代就四个环节采样、找最近邻、碰撞检测、生长。我用Python写过一个极简版本核心不超过40行。def rrt_plan(start, goal, obstacles, map_size, step_size10, max_iter5000, goal_tolerance5.0, goal_sample_rate0.1): tree {start: None} for _ in range(max_iter): # 1. 采样小概率直接采目标点大概率均匀随机采样 if random.random() goal_sample_rate: sample goal else: sample (random.uniform(0, map_size[0]), random.uniform(0, map_size[1])) # 2. 找最近邻在已有树节点中找欧氏距离最近的点 nearest_node min(tree.keys(), keylambda n: dist(n, sample)) # 3. 生长从最近邻朝采样点方向走一个步长 new_node steer(nearest_node, sample, step_size) # 4. 碰撞检测连线没碰到障碍就加入树 if not check_collision(nearest_node, new_node, obstacles): tree[new_node] nearest_node # 到达判定 if dist(new_node, goal) goal_tolerance: return extract_path(tree, new_node) return None这里最关键的是第2步“找最近邻”。如果每次用遍历找迭代5000次就是5000次全树扫描复杂度是O(N^2)树一大就卡。所以工程实现中要么用KD-Tree维护节点索引要么在节点数量几千以内时用空间分桶加速。第3步的生长距离步长也特别讲究它能直接决定整个规划的成败我后面单独说。1.3 “概率完备”和“渐进最优”到底是什么意思RRT刚出来被人嘴最狠的一点是它不是确定的每次跑结果可能不一样。但深入了解后你会发现随机恰恰是它的护城河。RRT具备概率完备性只要可行路径存在迭代次数趋向无穷时找到路径的概率趋近于1。它不保证一次找到也不保证最快找到但保证不会彻底漏掉解。代价是路径质量一言难尽。基础RRT生成的路径往往拐来拐去一点不像人走的路线因为它压根不优化任何代价指标只负责“找到一条路”。这就像你随机串门居然到达了目的地但走的尽是偏门小路。于是后面才有RRT*、Informed RRT*这一大堆变种目标就是在这套采样框架里把路径质量拉上去。2. 手写一份基础RRT环境、代码、参数一次说清2.1 写一个能跑的最小仿真环境既然要手把手玩我先说下我用的最小环境。Python 3.8以上只需要numpy和matplotlib两个库不需要引入ROS也不依赖MoveIt。仿真地图我用100 x 100的单位平面随机生成若干个圆形障碍圆的半径各不相同。之所以用圆形障碍是因为圆和线段的碰撞检测有解析解写起来最省事也最不容易出bug。import numpy as np import matplotlib.pyplot as plt import random from math import sqrt, atan2, cos, sin def generate_obstacles(num8, map_size(100, 100)): obstacles [] for _ in range(num): x random.uniform(5, map_size[0] - 5) y random.uniform(5, map_size[1] - 5) r random.uniform(4, 10) obstacles.append((x, y, r)) return obstacles def dist(p1, p2): return sqrt((p1[0] - p2[0])**2 (p1[1] - p2[1])**2) def steer(from_node, to_sample, step_size): d dist(from_node, to_sample) if d step_size: return to_sample ratio step_size / d return (from_node[0] (to_sample[0] - from_node[0]) * ratio, from_node[1] (to_sample[1] - from_node[1]) * ratio)这里steer函数的写法很多初学者容易搞错直接算单位向量乘步长是错的因为如果采样点距离比步长还近就会漏掉那些真正接近目标的节点。所以我先判断距离再按比例缩放这样既保证步长不超过设定值又保证采样点近在咫尺时能直接落点。地图随机生成后配合matplotlib把圆形障碍画出来肉眼就能看到规划结果调起参来特别直观。我在自己机器上跑这个demo从写第一行代码到第一张路径图出来半小时以内就能搞定。2.2 碰撞检测线段与圆的相交判定碰撞检测是整个RRT的“物理引擎”它错了后面全是白搭。对圆形障碍来说问题本质是判断线段AB和圆C是否相交。最稳妥的方法是参数方程求解def check_collision(p1, p2, obstacles): for (cx, cy, r) in obstacles: # 线段参数方程: P P1 t * (P2 - P1), t in [0, 1] dx p2[0] - p1[0] dy p2[1] - p1[1] fx p1[0] - cx fy p1[1] - cy a dx * dx dy * dy b 2 * (fx * dx fy * dy) c fx * fx fy * fy - r * r if a 1e-10: # p1和p2几乎重合 if fx * fx fy * fy r * r: return True continue disc b * b - 4 * a * c if disc 0: continue t1 (-b - sqrt(disc)) / (2 * a) t2 (-b sqrt(disc)) / (2 * a) if (0 t1 1) or (0 t2 1): return True # 如果线段完全在圆内部两个交点t都超过1也要算碰撞 if (t1 0 and t2 0) or (t2 0 and t1 0): return True return False很多人会偷懒把线段离散成几十个点逐个判断点是否在圆内看起来简单实际会出两个问题一是离散密度不够时快速飞过的线段可能“穿透”圆形障碍而不被检出二是判断量大幅上升渲染慢规划更慢。解析法算一次只有常数时间强烈建议直接用。提示真实机器人导航中障碍物膨胀半径一定要大于机器人底盘半径。如果你用RRT规划出来的是机器人中心点轨迹却只按障碍原始轮廓做碰撞检测那实际跑起来经常擦墙。我习惯在障碍半径上额外加一个安全余量这个余量等于机器人半径加5到10厘米的兜底。2.3 参数选型经验步长、迭代次数、目标偏置怎么定玩RRT大概率的调试时间都花在参数上。我踩过不少坑后整理出一套经验区间纯个人实测仅供参考参数经验取值说明步长 step_size地图短边尺寸的1%~5%100的地图我常用8到15太小树长得慢太大容易穿障碍最大迭代次数 max_iter5000~20000越低实时性越好但成功率下降复杂场景建议1万起步目标容差 goal_tolerancestep_size的0.5~1倍小于步长容易导致终点永远够不着目标偏置率 goal_sample_rate0.05~0.15太高会过度贪婪树容易卡在障碍附近反复碰壁采样上限地图边界采样点超出地图直接丢弃避免生成无效节点重点说下步长。步长设成2那树平均要长四五千个节点才能穿过同一片空地慢得让人怀疑人生步长设成30障碍一密集起来新节点直接“一步跨过”障碍物碰撞检测明明没报错路径却直接从墙这头穿到墙那头。我见过有人为了追求成功率把步长调得极小结果规划时间从0.5秒膨胀到30秒。实际项目里步长应该控制在机器人本体宽度的0.5到1倍左右既保证几何可行性又不会让搜索太磨叽。目标偏置也是新手最爱乱动的一个参数。把goal_sample_rate调到0.5你会看到树确实在疯狂往目标点长但一旦目标方向被障碍挡住所有偏置采样都撞墙树直接原地打转探索能力几乎归零。我建议0.05起步最多不超过0.15留出足够多的随机采样去开辟绕行路。3. RRT全家桶从“找到路”到“找到好路”3.1 RRT*加入代价迭代后路径不再歪歪扭扭基础RRT只管“接入树”完全不在乎新节点是不是走了弯路。RRT*做的改进非常直接新节点加入后不只连到最近邻而是把它周围一定半径内的所有节点都翻出来挑一个从起点到这个新节点总代价最小的当父节点接着再尝试把周围节点重接到新节点上看能不能降低它们原有的代价。这个过程叫重连rewire是RRT*实现渐进最优的关键。它借鉴了图论里最短路径树的思路每次新增节点都像在电网里修一条新路如果新路能替代旧路就把旧路打断重接。代价函数通常就是路径长度但也可以换成能耗、时间窗这类自定义指标。需要说明的是RRT保证的是“随着迭代次数增长路径代价概率收敛到最优”而不是某一次运行一定最优。所以它比基础RRT更吃算力每次加入节点都要做一次邻域搜索和多次碰撞检测。如果你只想要一条能跑的路径用RRT纯属浪费但如果路径质量直接决定下游控制品质那这点算力花得值。3.2 RRT-Connect两棵树对撞速度翻倍RRT-Connect的思路堪称暴力美学从起点和终点各自长一棵树两棵树交替向外扩展每一次不再只是新树朝对侧树的随机采样点走一步而是不断朝对侧树的最近节点做贪心延伸直到撞上障碍或与对方接通。双向生长让树的扩展速度几乎翻倍而贪心延伸又让两棵树在无障碍区域快速“互相奔赴”收拢速度极快。这个变种在狭窄通道场景里表现尤其突出。想象一条仅比机器人宽一点的长走廊单向RRT要先歪歪扭扭穿进去可能反复碰撞才找到通道双向RRT两棵树从两头往中间一怼碰面概率大幅提升。我在实际项目里也注意到RRT-Connect用于机械臂无碰撞路径规划时解算时间经常只有基础RRT的1/3到1/5。代价是它保留“找到就好”的思路不做任何优化。所以工程上常见的组合拳是先用RRT-Connect快速找一条可行路径再用短时间内的RRT*对它做局部优化或者直接套二次平滑。3.3 Informed RRT*路径找到了绕着这条“走廊”精修Informed RRT的出发点是另一个痛点标准RRT找到一条可行路径后后续迭代还在全地图乱采样大量计算浪费在完全无关区域。于是它把寻优过程限制在一个以起点和终点为焦点的椭圆区域内椭圆的短轴由当前最优路径长度决定路径越优椭圆越小采样越集中。这个策略非常像“先粗跑一遍再沿着粗跑轨迹周围精修”。算法流程上Informed RRT*在找到初始可行路径后切换全局均匀采样为椭圆内采样后续每优化一次路径椭圆就跟着缩短。理论保证它最终能收敛到最优解但实际调参时要注意椭圆长轴必须大于起点到终点的直线距离否则采样区域为空。它为“先发现后优化”的规划范式开了个好头。你做多层规划时完全可以用它把耗时很长的RRT*寻优阶段控制在一个局部空间里拿实时性换路径最优性这笔买卖对于割草机器人、扫地机器人这类对电池续航敏感的移动平台非常划算。3.4 其他不能忽视的变种RRT家族远不止上面三个。做移动底盘项目我建议关注Kinodynamic RRT它把机器人的运动学约束最小转弯半径、最大加速度、Dubins路径或Reeds-Shepp曲线直接塞进扩展步里生成出来的路径不是折线而是可执行的曲线省掉下游一堆平滑处理。Anytime RRT则是给每条路径打上“当前最优”标签规划时间越充分方案越优适合救援侦察这种有时间截止要求的场景。RRT-Connect是把双向生长和重连结合既快又优就是实现复杂度翻倍代码Debug时间直线上升。3.5 全家桶选型速查算法核心改进优势代价适用场景基础RRT随机采样最近邻扩展实现简单成功率高路径歪非最优快速验证、教学、目标可达性判断RRT*邻域搜索父节点重选重连渐进最优路径平滑度好计算量大初期收敛慢离线全局规划、路径质量优先RRT-Connect双向树贪心延伸极快适合狭窄通道不优化路径实时规划、机械臂高维避障Informed RRT*椭圆区域内外采样切换优化阶段效率高依赖初始路径质量长时间寻优、二次优化Kinodynamic RRT运动学约束嵌入扩展输出可直接跟踪状态空间设计繁琐AGV、无人机、带执行器约束的机器人这么多变种怎么选我给自己定了个简单原则现阶段只需要“能到”就选RRT-Connect需要“到得好”就选Informed RRT*如果还要考虑底盘能不能跟得上那老老实实上Kinodynamic RRT。4. 工程部署中的常见问题与排查技巧4.1 碰撞检测的边界陷阱第一坑是浮点精度。圆形障碍和线段的关系判定里判别式disc非常接近0但又是负数会被当成无交点直接放行。第二坑是线段两端都落在圆外但线段中段穿过圆这种情况t1和t2可能都不落在[0,1]区间里。我代码里补了“t10且t20”的判断就是为了兜住线段横穿圆但两端都在外部的情况。第三坑是节点恰好落在障碍边缘距离等于半径数学上不算碰撞物理上机器人已经蹭到障碍了。工程上一律给障碍统一膨胀宁严勿松。4.2 路径不平滑RRT类算法直接输出折线路径节点越多转弯越碎底盘控制模块一跑就“抖”。我常用的后处理三板斧是路径剪枝去掉冗余节点凡是直线段不碰障碍的中间节点全删、贝塞尔曲线平滑保留几何形状同时让曲率连续、上下位规划器配合全局给粗路径局部用DWA或TEB做精细跟踪。分享一个简单的两点间贝塞尔插值思路def bezier_smooth(points, n10): if len(points) 3: return points # 对相邻三点组做二次贝塞尔插值n控制插值密度 smooth [] for i in range(len(points) - 2): p0, p1, p2 points[i], points[i1], points[i2] for t in [j/n for j in range(n)]: x (1-t)**2 * p0[0] 2*(1-t)*t * p1[0] t**2 * p2[0] y (1-t)**2 * p0[1] 2*(1-t)*t * p1[1] t**2 * p2[1] smooth.append((x, y)) return smooth [points[-1]]注意平滑后的曲线必须重新过一遍碰撞检测因为贝塞尔插值可能把路径往障碍方向拉偏。我遇到过平滑完路径直接从墙里穿过去的惨案加一道校验就老实了。4.3 实时性不够先看采样和最近邻再想优化算法很多同学觉得RRT慢上来就换更复杂的变种结果更慢。其实基础RRT的瓶颈九成在最近邻查找用线性扫描找最近邻N个节点就要算N次欧氏距离5000次迭代就是几千万次计算。换成KD-Tree之后这里能提速一到两个数量级。Python可以直接用scipy.spatial.cKDTree注意它是静态结构树长一部分就要重建一次所以我一般设置成每50个新节点重建一次兼顾精度与开销。还有一个容易忽略的加速手段降低碰撞检测的调用频率。每次扩展成功前可以先检测“目标点是否在障碍内部”这种粗筛条件把明显不行的采样点直接丢掉省掉一次线段相交判定。4.4 无解场景与超时机制RRT是概率完备不是绝对完备在某些极端环境狭窄迷宫、孤立封闭区域里迭代几万次都可能找不到路。工程上绝不能让它无限跑。我处理这类问题的标准做法给规划器设一个超时上限超时后返回“当前最接近目标的部分路径”或直接报“无解”由上层调度决定是原地旋转重试还是走备份预案。另外一个实用技巧是降低目标容差重新尝试。很多“无解”其实是目标点周围被障碍包围容差设太大导致终点附近所有扩展都被判定失败但容差缩到半个步长反而能探到缝隙。我调试时会连续跑三组不同随机种子如果三组都无解才判断是真的没有可行路径。4.5 调参速查遇到问题先查这张表症状优先排查尝试手段规划很慢但能找到路径最近邻太慢、迭代太多换KD-Tree、加大步长、调低max_iter老是找不到路径步长过大穿模、容差过窄缩小步长、扩大goal_tolerance、提升障碍膨胀精度路径特别绕目标偏置太低、没有优化机制目标偏置调到0.1、切换到RRT*/Informed RRT*树长不到目标附近采样空间被地形分割调高目标偏置、用双向树、手动注入引导点路径贴着障碍物走膨胀半径不足给障碍加半径余量、碰撞检测统一用膨胀后尺寸平滑后撞墙后处理没重新校验平滑曲线重跑碰撞检测、降低插值密度5. 仿真实验对比与个人体会5.1 用同一张地图给全家桶排队为了让大家直观感受差异我用同一张随机地图做了一组对照实验。地图尺寸100 x 100随机生成8个圆形障碍起点(5, 5)终点(95, 95)每个算法独立跑50次统计平均结果算法平均规划时间秒平均路径长度成功率基础RRT0.21148.786%RRT*2000次迭代1.42126.580%RRT-Connect0.08151.399%Informed RRT*2000次迭代1.05118.284%我的实验机配置很普通数据只能代表这张地图上的行为但规律非常明显RRT-Connect速度优势压倒性成功率也最高适合实时场景RRT和Informed RRT虽然更慢但路径长度明显变短符合“渐进最优”的设定基础RRT介于两者之间简单但不精致。5.2 数据背后的真实规律实验做多了我发现几个非常容易误导人的点。第一成功率跟场景强相关换个障碍布局基础RRT可能冲到98%RRT也可能掉到60%所以对比算法一定要固定地图或者混合多种地图别拿单张图下结论。第二Informed RRT的初始路径质量决定后续优化上限先跑出来的路径越差椭圆越大优化阶段效率越低所以它往往要搭配RRT-Connect做初始路径发现。第三迭代次数不是越多越好超过一定数量后路径长度改善微乎其微但耗时线性增长我一般用“2000次迭代后连续500次无改善”作为提前终止条件能省一多半时间。5.3 我的实践心得最后说几句个人体会。第一新手不要一上来就上全家桶先手工实现一个能可视化跑通的基础RRT把采样、最近邻、碰撞检测、生长这四个模块烂熟于心变种无非是在这几个模块上做文章。第二把碰撞检测和最近邻封装成独立接口后面换变种、换地图、换真实机器人模型都能直接复用这个设计比算法本身更值钱。第三调参永远一次只动一个变量同时改步长和偏置率出了问题你根本不知道是谁的锅。第四工程里RRT永远只是全局规划的一环后面还要接膨胀层、平滑模块、局部规划器、速度控制别指望一个规划器包打天下。最后分享一个小技巧无论你用哪个变种务必保留随机种子可控性。我在项目里给规划器加了一个seed参数调试时用固定种子能复现路径回归测试时再用随机种子压测。这个习惯救了我无数次否则规划结果一随机bug复现全靠缘分。