干过几年机器人路径规划的人大概率绕不开RRT这个名字。RRT全称是Rapidly-exploring Random Tree中文常译作随机快速搜索树也有叫快速探索随机树的。它做的事情一句话就能说清在一片有障碍物的空间里从起点随机长出一棵树不断扩展直到树梢碰到目标点再从目标点沿父子关系回溯得到一条无碰撞路径。移动机器人、机械臂、无人机、自动驾驶里的局部规划、游戏AI寻路都能看到它的身影。这篇文章不是纯理论推导更像是我在实际项目和仿真里反复折腾出来的总结。我会先讲为什么会有这种“随机”规划算法再把核心步骤拆开给一份能直接跑的Python实现最后聊调试中的高频问题和真正该选的RRT变体。如果你刚开始接触运动规划或者正在被某个“看起来简单但跑不通”的规划问题折磨这篇应该能让你少走几步弯路。1. 为什么路径规划里会冒出RRT这种“随机”方法1.1 高维空间让老办法力不从心说到路径规划很多人第一反应是A*把地图栅格化每个格子有代价值然后用优先队列搜最短路径。二维小地图里这套方案确实好用代码简单结果也直观。但一旦维度拉高A*就迅速变得力不从心。举个例子一个6自由度机械臂每个关节如果能离散成100个角度那勾型空间就有100的6次方个状态任何栅格搜索算法都撑不住这种指数爆炸。所以大家慢慢意识到与其把空间切成一个个小格子不如直接在连续的构型空间里采样。RRT的思路很简单每次在空间里随机挑一个点然后从已经生长的树上找离它最近的节点朝这个随机点方向走一小步如果中间没有碰撞就把新点挂到树上。这样不断循环树就像藤蔓一样在空白区域里蔓延。它不用提前知道空间的结构只看局部不搞全局建模因此特别适合高维、复杂、未知环境。可以用一个生活化的类比帮助理解。A*像你在陌生城市找路先下载一张精确街区图划分网格然后逐个街区搜索。RRT更像在夜里拿着手电筒随机往远处扔飞镖插中了就朝那个方向走一段再继续扔。它并不追求一次找到最短路只求先探出一条能走的路。这种“先保证可行再谈最优”的思路是采样规划算法和传统搜索方法最本质的区别。1.2 RRT跟A*、PRM到底差在哪做规划的人经常把RRT和PRM放到一起说因为二者都是基于采样的。PRM会先在空间里撒大量采样点构建一张概率路线图再在图上做查询。它适合“建一次图、查很多次路径”的场景比如多机器人长时间在同一个环境里工作。RRT则是一边采样一边生长单次规划就能出结果更适合在线规划和实时重规划场景。几种方法放一起对比会更直观。A*能得到栅格离散意义下的最优路径但高维下不可行PRM要预建图适合多查询RRT不要求最优胜在灵活、收敛快。我用得最多的对比表长这样维度A*PRMRRT空间处理网格离散化随机采样建图随机采样建树查询方式单次多查询复用单次在线生长最优性离散最优依赖图质量可做到渐近最优只保证可行解高维表现差中等好实现复杂度低中低RRT有一个性质叫“概率完备性”。意思是当随机采样点数趋于无穷时只要存在一条可行路径找到这条路径的概率趋近于1。注意是趋于1不是一定等于1。这意味着你在有限迭代次数内可能跑不出解但只要时间给够算法几乎总能找到一条路。另一方面RRT天生不保证最优因为找到一条可行路径后它通常直接返回或者即便不返回它的生长策略也从不考虑边的代价。这也是为什么后来冒出RRT*、Informed RRT*这些变体专门去优化代价。2. 算法原理拆解一棵树是怎么“随机长出来”的2.1 核心循环五步很多资料喜欢直接甩伪代码看下去一头雾水。我习惯把RRT的核心循环理解成五步采样、找最近、迈步、检测、挂树。伪代码如下1. T 空树把起点作为根节点加入 2. 重复直到达到最大迭代次数 a. x_rand Sample() // 随机采样一个状态 b. x_near Nearest(T, x_rand) // 在树中找到离 x_rand 最近的节点 c. x_new Steer(x_near, x_rand, step) // 朝 x_rand 方向走 step 距离 d. 如果 x_near 到 x_new 之间无碰撞 - 把 x_new 加入树 T - 记录 x_new 的父节点为 x_near e. 如果 x_new 距离目标点小于容差 返回从目标回溯得到的路径 3. 失败返回空这里每个环节都值得细看。Sample()是随机采样的核心。最简单的实现就是在整个地图范围内均匀随机取一个点。它对发现未知区域很有用但完全随机也会让树长得很“散”后面会说怎么加目标偏置。Nearest(T, x_rand)是在现有树节点里选一个离随机点最近的节点这一步决定树往哪个方向长选错了路径就会歪。Steer()则是朝随机点方向走固定步长不直接走到随机点否则两个节点之间可能穿过障碍物而且步长不受控。CollisionFree()检查从x_near到x_new这条短线段是否碰到障碍物这是整个算法里最消耗计算量的环节。整个过程用一句话概括树并不在乎空间里最短路是什么它只关心怎么从已经到过的地方再往前试探一小步。每成功加入一个新节点树就多覆盖一块空间。循环跑得越多树逐渐“探”满整个自由区域最终碰到目标点。2.2 三个小动作目标偏置、步长与容差原始RRT是纯均匀随机采样问题在于如果目标点在一大片空白区域的另一头纯随机可能要长几千个节点才能探过去。工程上最常用的改进就叫目标偏置goal bias。做法很简单每次采样时以一定概率比如5%到10%直接把目标点当作x_rand其他时候才用均匀随机采样。我自己的经验是目标偏置不要给太高。曾经为了加快收敛把它调到0.4结果树频繁朝目标点扩展一旦目标方向被障碍物挡住新节点总是撞墙树反而在原地绕来绕去。二维地图里用0.05到0.15比较合适机械臂等高维场景也要控制在0.1以内。固定的随机种子更容易复现和调试建议代码里把seed暴露成参数。步长step的选择很有讲究。它代表树每次往前迈多远。如果步长大于狭窄通道的宽度树很容易“一步跨过”可通行区域或者把边画进障碍物里导致漏检。步长太小树每轮只前进一点点迭代几千次还在原地附近徘徊。通用的经验是在二维地图里步长取地图尺寸的1%到2%左右。比如10x10的地图step取0.1到0.2如果地图有很多窄缝step要再小一点。目标到达容差goal_tol通常取step的一半或者更小。如果你的机器人本身有半径那step至少要小于机器人可通行的净空宽度。2.3 碰撞检测和最近邻是真正的瓶颈很多人写RRT第一版跑通了就以为大功告成等节点数一多立刻卡成PPT。瓶颈基本都出在两个地方碰撞检测和最近邻搜索。碰撞检测要回答的问题非常具体从点A到点B这段直线会不会跟障碍物撞上2D仿真里最简单的方法是沿着线段密集采样几个点逐个判断是否落在障碍物内部。这个“密集采样”的密度要足够高否则细长障碍物可能被漏掉。我在Demo里每0.2个单位采样一次如果地图很大或者精度要求高这个间隔还要更小。真实工程里则不建议自己写碰撞检测机械臂场景直接接FCL这类成熟的碰撞检测库底盘场景则常把周围障碍物膨胀成距离场碰撞检测退化成一次查表速度快很多。最近邻搜索也很容易成为性能瓶颈。朴素实现是遍历树里所有节点算一遍欧氏距离取最小值。节点数1000以内还好到5000以上每轮都做一遍O(N)遍历总复杂度就成了O(N^2)非常酸爽。更实用的做法是基于网格分桶把地图按固定分辨率划分成格子节点按所在格子索引存好查询时只搜附近几个格子的节点。这样虽然不像KD树那样理论完美但实现简单也更适合树不断动态增长的场景。如果项目里愿意花精力每几十轮用cKDTree重建一次也可以注意不要每轮都重建。3. 手把手实现一个二维RRT3.1 最简Python实现我直接把一个能跑的二维Demo贴出来。这个版本包含节点类、RRT类、圆障碍物碰撞检测代码量不大适合拿来改。跑通之后再往自己的工程里移植也容易。import math import random class Node: def __init__(self, x, y): self.x x self.y y self.parent None class RRT: def __init__(self, start, goal, obstacles, bounds, step0.8, max_iter3000, goal_bias0.1, goal_tol0.4): self.start Node(*start) self.goal Node(*goal) self.nodes [self.start] self.obstacles obstacles # [(ox, oy, radius), ...] self.bounds bounds # (min_x, max_x, min_y, max_y) self.step step self.max_iter max_iter self.goal_bias goal_bias self.goal_tol goal_tol def random_state(self): if random.random() self.goal_bias: return self.goal.x, self.goal.y x random.uniform(self.bounds[0], self.bounds[1]) y random.uniform(self.bounds[2], self.bounds[3]) return x, y def nearest(self, x, y): best None best_dist float(inf) for node in self.nodes: d math.hypot(node.x - x, node.y - y) if d best_dist: best_dist d best node return best def steer(self, near, x, y): d math.hypot(near.x - x, near.y - y) if d 1e-8: return None ratio self.step / d new_x near.x (x - near.x) * ratio new_y near.y (y - near.y) * ratio return Node(new_x, new_y) def collision_free(self, n1, n2): dist math.hypot(n2.x - n1.x, n2.y - n1.y) n max(int(dist / 0.2), 1) for i in range(n 1): t i / n px n1.x (n2.x - n1.x) * t py n1.y (n2.y - n1.y) * t for ox, oy, r in self.obstacles: if math.hypot(px - ox, py - oy) r: return False return True def plan(self): for _ in range(self.max_iter): rx, ry self.random_state() near self.nearest(rx, ry) new self.steer(near, rx, ry) if new is None: continue if not self.collision_free(near, new): continue new.parent near self.nodes.append(new) if math.hypot(new.x - self.goal.x, new.y - self.goal.y) self.goal_tol: return self.extract_path(new) return None def extract_path(self, node): path [] while node is not None: path.append((node.x, node.y)) node node.parent return path[::-1]调用方式也很直接。下面这个例子是10x10地图起点在左下角终点在右上角中间放三个圆障碍物。random.seed(42) obstacles [(5, 5, 1.0), (3, 8, 0.8), (8, 3, 0.8)] rrt RRT(start(0, 0), goal(10, 10), obstaclesobstacles, bounds(0, 10, 0, 10), step0.8, max_iter3000, goal_bias0.1) path rrt.plan() if path: print(找到路径节点数:, len(rrt.nodes)) print(path) else: print(在迭代次数内没有找到路径)第一次跑起来你会发现路径很“直”但又很“怪”节点之间是一段段折线整体可能绕了远路。这是RRT的正常表现能走就行别急着在原始版本上要求路径优美。3.2 参数调整与代码优化同样的地图换不同参数表现会差很多。我把自己常用的初始参数整理成一张表方便你从这些值开始试参数建议范围说明step地图尺寸的1%~2%步长过大会漏过窄通道过小会收敛慢goal_bias0.05~0.15目标偏置过高会让树在障碍物前绕圈goal_tolstep/2 左右目标判定容差max_iter2000~10000迭代上限越复杂给越多种子固定否则每次结果不同不利于排查碰撞检测采样间隔0.1~0.5视障碍物最小尺寸而定代码优化方面最容易见效的是把最近邻搜索从“遍历所有节点”改成“空间分桶”。简单做法是维护一个字典键是格子索引值是格子里的节点列表。每次查询只检查周边格子中的节点。碰撞检测也可以用距离场代替逐段采样。2D地图里开一个分辨率相同的网格提前计算每个格子到最近障碍物的距离大于机器人半径即可通行。这样collision_free变成两个网格查询性能提升非常明显。3.3 从Demo到真实工程对接在真正的机器人和机械臂上很少有人从头手写RRT更多是用开源运动规划库。常用的方案是配OMPL它内置了RRT、RRT*、RRT-Connect等一堆算法。你用OMPL时核心要干三件事定义状态空间比如2D的SE2、机械臂的RealVectorStateSpace设置碰撞检测接FCL或者自定义然后创建规划问题并选择planner。算法细节由库处理你只管地图、状态和终端约束。但要注意RRT输出的是几何路径不是机器人能直接执行的轨迹。几何路径只有位置信息缺少速度、加速度和时间分配。这就好比给你一张导航地图上面有路线但没告诉你每个路口该开多快。真实项目里还需要把路径平滑成连续曲线再做时间插值生成带速度约束的轨迹。平滑常用Bezier曲线、B样条或者裁剪“shortcut”法。另外在动态环境里地图是不断更新的RRT的好处是单次规划的代价低适合放进“感知—规划—控制”的实时循环里每次拿到新障碍物信息就重新规划一次。4. 调试中那些让人抓狂的问题4.1 目标点明明不远树就是不长过去这是我被问到最多的问题。目标点就在地图右上角树也已经在附近了但RRT始终不把最后一步迈过去。排查顺序通常是这样的先看目标偏置是不是设得太小如果只有1%那树几乎没有动力朝目标走纯粹靠随机采样碰运气效率自然低。可以把goal_bias逐步提到0.1到0.15再跑一次。还要确认目标点本身是否可到达。如果目标点放在了障碍物内部或者离障碍物边界太近机器人半径膨胀后根本站不进去那不管你跑多少次目标容差都无法满足。解决办法是给目标点也做一轮碰撞检测并且在生成目标位置后做一次“可到达性”检查。最后再看goal_tol是不是设得太小。步长0.5容差0.05最后一步恰好跨过去了但不在容差范围内就会出现“擦肩而过”的假象。把容差调到步长的一半很多问题立刻消失。4.2 狭窄通道场景怎么破狭窄通道是RRT的经典噩梦。想象一个地图中间只有一条宽度等于机器人直径的缝除此之外全是障碍物。均匀随机采样想命中这条缝概率可能只有百分之几。树在外围长了几千个节点就是钻不进去。应对策略有很多。最实用的是双向RRT也叫RRT-Connect同时从起点和终点各长一棵树两棵树交替扩展然后尝试把两棵树最近的节点连接起来。因为目标方向参与引导狭缝两端的树更容易“接上头”。还有一种思路是降低步长让树有更多机会在缝隙附近“微调”。更高级一点的做法是桥采样刻意在障碍物两侧采样并检测连接专门发现狭窄通道。如果你的问题场景结构特殊用网格预处理生成一个“走廊骨架”再沿骨架引导RRT采样也能明显改善。调试时一定记得把随机种子固定否则同样的配置每次结果都不一样很难判断到底是改对了还是运气好。4.3 越跑越慢最近邻和碰撞检测拖后腿RRT慢通常不是采样慢而是每轮都要在所有节点里找最近邻同时做多次碰撞检测。仿真里几百个节点不觉得到几千个节点时程序会越来越卡最后几乎走不动。我见过有人把节点存在Python list里每轮从头到尾遍历5000个节点时每轮循环要算5000次距离总共迭代一万轮就是5000万次距离计算。不慢才怪。排查顺序建议是先profile确认瓶颈是在Nearest还是CollisionFree再动手。Nearest可以换成空间分桶或者每50轮重建一次cKDTree。CollisionFree在2D里优先做距离场在3D和机械臂场景接FCL。如果你在主循环里反复分配新的Node对象也可以改成复用对象或者用数组存坐标。这些改完同样地图上性能翻几倍不夸张。4.4 原始路径太难看RRT找出来的路经常是一堆折线转角很生硬甚至贴着障碍物走。这不算bug是算法特性。最近邻策略导致树尽量连到已有节点路径完全没有代价概念锯齿和大绕路都很正常。处理办法有两层。第一层是shortcut拿到路径后反复尝试把相隔较远的两个节点直接相连如果中间无碰撞就把中间节点全部删掉路径立刻变短变顺。第二层是平滑用Bezier或B样条把折线变成连续曲线让机器人转起来不突兀。注意平滑后要做碰撞检测防止曲线贴着障碍物太近。真实项目中这两步几乎是标配因为很多执行器不能接受突然转向。5. RRT的“变种家族”与选型建议5.1 RRT*从可行解走向近似最优RRT最大的槽点是不找最优路径。RRT*就是在原始RRT上加了两个关键操作选择父节点时不只选最近邻而是在一个邻域范围内挑一个能让起点到新节点累计代价最小的节点新节点加入后再尝试用它重新连接邻域内的其他节点降低它们的累计代价这招叫rewire。rewire让RRT拥有了“渐进最优”的性质随着迭代次数增长路径代价会不断下降收敛到最优解附近。代价下降速度和质量与rewire半径有关常用理论值是这样的r gamma * (log(n) / n)^(1/d)其中n是节点数d是空间维度gamma跟环境有关。实际工程里不用太纠结公式直接给一个半径比如step的1.5到2倍或者地图尺寸的2%到5%再观察路径代价是否在下降。RRT的代价是计算量上去了每次插入新节点要多查邻域、多算几轮重新连接节点多的时候性能也会下降。5.2 还有哪些值得记住的变体除了RRT*RRT-Connect是工程里出现频率非常高的变体。它从起点和终点同时长树两棵树交替扩展每一次迭代都会尝试把两棵树的节点连接起来。优势是在狭窄通道和复杂环境中找路更快所以机械臂静态避障场景大家经常先用它跑通。它同样不保证最优。Informed RRT是RRT的一个漂亮改进它先运行RRT*找到一条可行路径然后后续采样不再全图撒点而是限制在一个以起终点为焦点的椭圆区域里。这个椭圆正好包含所有可能比当前路径更短的位置所以后续迭代都集中在“有意义”的区域收敛到最优的速度明显加快。还有Anytime RRT系列可以在有限时间内不断给出“当前最好”的路径适合有强实时性要求但算力还能保证的场景。最后如果机器人带动力学约束比如车不能原地转向、机械臂有速度和加速度限制就要用Kinodynamic RRT它在Steer环节不直接走直线而是满足动力学方程的运动原语。5.3 实际项目里怎么选我在不同项目里挑算法时会先问自己三个问题维度高不高环境是不是静态对路径代价敏不敏感如果只是验证规划链路无脑先用RRT参数随便给都能跑链路通了再说。机械臂在某固定工位干活环境基本不变优先考虑RRT-Connect或RRT*前者快后者路径更省。移动底盘在楼层里跑如果地图是2D栅格其实A或Hybrid A往往比RRT更合适因为地面环境偏结构化而且底盘运动有非完整约束。这时候硬套RRT反而得不偿失。有人喜欢把所有问题都用RRT解决我不建议。RRT的优势是通用和简单但通用意味着它没有利用具体场景的结构特性。在结构化、低维、静态的地图里A*的效率和路径质量都更好。正确的姿势是把它放进工具箱面对新问题时先用RRT类算法快速出可行解再根据需求换更优的算法。我自己的习惯是任何新环境第一次跑规划都会先用原始RRT做baseline。项目里第一次实车验证时同事非要先把RRT参数调好再动我建议先用RRT把整个闭环跑通结果半小时就定位到了测量地图和机器人半径不匹配的bug。后来换成RRT只改了规划层代码其他部分几乎没动。所以说别一上来就追求完美RRT最大的价值不是它自己有多强而是它能让你最快看到一条可以走的路然后你再慢慢优化。这就是我写这篇规划算法专栏第一篇的原因。 
长河君 · 2026/10/15 14:15:30 · 阅读 1.2k RRT路径规划算法：原理、Python实现与工程调试实战
干过几年机器人路径规划的人大概率绕不开RRT这个名字。RRT全称是Rapidly-exploring Random Tree中文常译作随机快速搜索树也有叫快速探索随机树的。它做的事情一句话就能说清在一片有障碍物的空间里从起点随机长出一棵树不断扩展直到树梢碰到目标点再从目标点沿父子关系回溯得到一条无碰撞路径。移动机器人、机械臂、无人机、自动驾驶里的局部规划、游戏AI寻路都能看到它的身影。这篇文章不是纯理论推导更像是我在实际项目和仿真里反复折腾出来的总结。我会先讲为什么会有这种“随机”规划算法再把核心步骤拆开给一份能直接跑的Python实现最后聊调试中的高频问题和真正该选的RRT变体。如果你刚开始接触运动规划或者正在被某个“看起来简单但跑不通”的规划问题折磨这篇应该能让你少走几步弯路。1. 为什么路径规划里会冒出RRT这种“随机”方法1.1 高维空间让老办法力不从心说到路径规划很多人第一反应是A*把地图栅格化每个格子有代价值然后用优先队列搜最短路径。二维小地图里这套方案确实好用代码简单结果也直观。但一旦维度拉高A*就迅速变得力不从心。举个例子一个6自由度机械臂每个关节如果能离散成100个角度那勾型空间就有100的6次方个状态任何栅格搜索算法都撑不住这种指数爆炸。所以大家慢慢意识到与其把空间切成一个个小格子不如直接在连续的构型空间里采样。RRT的思路很简单每次在空间里随机挑一个点然后从已经生长的树上找离它最近的节点朝这个随机点方向走一小步如果中间没有碰撞就把新点挂到树上。这样不断循环树就像藤蔓一样在空白区域里蔓延。它不用提前知道空间的结构只看局部不搞全局建模因此特别适合高维、复杂、未知环境。可以用一个生活化的类比帮助理解。A*像你在陌生城市找路先下载一张精确街区图划分网格然后逐个街区搜索。RRT更像在夜里拿着手电筒随机往远处扔飞镖插中了就朝那个方向走一段再继续扔。它并不追求一次找到最短路只求先探出一条能走的路。这种“先保证可行再谈最优”的思路是采样规划算法和传统搜索方法最本质的区别。1.2 RRT跟A*、PRM到底差在哪做规划的人经常把RRT和PRM放到一起说因为二者都是基于采样的。PRM会先在空间里撒大量采样点构建一张概率路线图再在图上做查询。它适合“建一次图、查很多次路径”的场景比如多机器人长时间在同一个环境里工作。RRT则是一边采样一边生长单次规划就能出结果更适合在线规划和实时重规划场景。几种方法放一起对比会更直观。A*能得到栅格离散意义下的最优路径但高维下不可行PRM要预建图适合多查询RRT不要求最优胜在灵活、收敛快。我用得最多的对比表长这样维度A*PRMRRT空间处理网格离散化随机采样建图随机采样建树查询方式单次多查询复用单次在线生长最优性离散最优依赖图质量可做到渐近最优只保证可行解高维表现差中等好实现复杂度低中低RRT有一个性质叫“概率完备性”。意思是当随机采样点数趋于无穷时只要存在一条可行路径找到这条路径的概率趋近于1。注意是趋于1不是一定等于1。这意味着你在有限迭代次数内可能跑不出解但只要时间给够算法几乎总能找到一条路。另一方面RRT天生不保证最优因为找到一条可行路径后它通常直接返回或者即便不返回它的生长策略也从不考虑边的代价。这也是为什么后来冒出RRT*、Informed RRT*这些变体专门去优化代价。2. 算法原理拆解一棵树是怎么“随机长出来”的2.1 核心循环五步很多资料喜欢直接甩伪代码看下去一头雾水。我习惯把RRT的核心循环理解成五步采样、找最近、迈步、检测、挂树。伪代码如下1. T 空树把起点作为根节点加入 2. 重复直到达到最大迭代次数 a. x_rand Sample() // 随机采样一个状态 b. x_near Nearest(T, x_rand) // 在树中找到离 x_rand 最近的节点 c. x_new Steer(x_near, x_rand, step) // 朝 x_rand 方向走 step 距离 d. 如果 x_near 到 x_new 之间无碰撞 - 把 x_new 加入树 T - 记录 x_new 的父节点为 x_near e. 如果 x_new 距离目标点小于容差 返回从目标回溯得到的路径 3. 失败返回空这里每个环节都值得细看。Sample()是随机采样的核心。最简单的实现就是在整个地图范围内均匀随机取一个点。它对发现未知区域很有用但完全随机也会让树长得很“散”后面会说怎么加目标偏置。Nearest(T, x_rand)是在现有树节点里选一个离随机点最近的节点这一步决定树往哪个方向长选错了路径就会歪。Steer()则是朝随机点方向走固定步长不直接走到随机点否则两个节点之间可能穿过障碍物而且步长不受控。CollisionFree()检查从x_near到x_new这条短线段是否碰到障碍物这是整个算法里最消耗计算量的环节。整个过程用一句话概括树并不在乎空间里最短路是什么它只关心怎么从已经到过的地方再往前试探一小步。每成功加入一个新节点树就多覆盖一块空间。循环跑得越多树逐渐“探”满整个自由区域最终碰到目标点。2.2 三个小动作目标偏置、步长与容差原始RRT是纯均匀随机采样问题在于如果目标点在一大片空白区域的另一头纯随机可能要长几千个节点才能探过去。工程上最常用的改进就叫目标偏置goal bias。做法很简单每次采样时以一定概率比如5%到10%直接把目标点当作x_rand其他时候才用均匀随机采样。我自己的经验是目标偏置不要给太高。曾经为了加快收敛把它调到0.4结果树频繁朝目标点扩展一旦目标方向被障碍物挡住新节点总是撞墙树反而在原地绕来绕去。二维地图里用0.05到0.15比较合适机械臂等高维场景也要控制在0.1以内。固定的随机种子更容易复现和调试建议代码里把seed暴露成参数。步长step的选择很有讲究。它代表树每次往前迈多远。如果步长大于狭窄通道的宽度树很容易“一步跨过”可通行区域或者把边画进障碍物里导致漏检。步长太小树每轮只前进一点点迭代几千次还在原地附近徘徊。通用的经验是在二维地图里步长取地图尺寸的1%到2%左右。比如10x10的地图step取0.1到0.2如果地图有很多窄缝step要再小一点。目标到达容差goal_tol通常取step的一半或者更小。如果你的机器人本身有半径那step至少要小于机器人可通行的净空宽度。2.3 碰撞检测和最近邻是真正的瓶颈很多人写RRT第一版跑通了就以为大功告成等节点数一多立刻卡成PPT。瓶颈基本都出在两个地方碰撞检测和最近邻搜索。碰撞检测要回答的问题非常具体从点A到点B这段直线会不会跟障碍物撞上2D仿真里最简单的方法是沿着线段密集采样几个点逐个判断是否落在障碍物内部。这个“密集采样”的密度要足够高否则细长障碍物可能被漏掉。我在Demo里每0.2个单位采样一次如果地图很大或者精度要求高这个间隔还要更小。真实工程里则不建议自己写碰撞检测机械臂场景直接接FCL这类成熟的碰撞检测库底盘场景则常把周围障碍物膨胀成距离场碰撞检测退化成一次查表速度快很多。最近邻搜索也很容易成为性能瓶颈。朴素实现是遍历树里所有节点算一遍欧氏距离取最小值。节点数1000以内还好到5000以上每轮都做一遍O(N)遍历总复杂度就成了O(N^2)非常酸爽。更实用的做法是基于网格分桶把地图按固定分辨率划分成格子节点按所在格子索引存好查询时只搜附近几个格子的节点。这样虽然不像KD树那样理论完美但实现简单也更适合树不断动态增长的场景。如果项目里愿意花精力每几十轮用cKDTree重建一次也可以注意不要每轮都重建。3. 手把手实现一个二维RRT3.1 最简Python实现我直接把一个能跑的二维Demo贴出来。这个版本包含节点类、RRT类、圆障碍物碰撞检测代码量不大适合拿来改。跑通之后再往自己的工程里移植也容易。import math import random class Node: def __init__(self, x, y): self.x x self.y y self.parent None class RRT: def __init__(self, start, goal, obstacles, bounds, step0.8, max_iter3000, goal_bias0.1, goal_tol0.4): self.start Node(*start) self.goal Node(*goal) self.nodes [self.start] self.obstacles obstacles # [(ox, oy, radius), ...] self.bounds bounds # (min_x, max_x, min_y, max_y) self.step step self.max_iter max_iter self.goal_bias goal_bias self.goal_tol goal_tol def random_state(self): if random.random() self.goal_bias: return self.goal.x, self.goal.y x random.uniform(self.bounds[0], self.bounds[1]) y random.uniform(self.bounds[2], self.bounds[3]) return x, y def nearest(self, x, y): best None best_dist float(inf) for node in self.nodes: d math.hypot(node.x - x, node.y - y) if d best_dist: best_dist d best node return best def steer(self, near, x, y): d math.hypot(near.x - x, near.y - y) if d 1e-8: return None ratio self.step / d new_x near.x (x - near.x) * ratio new_y near.y (y - near.y) * ratio return Node(new_x, new_y) def collision_free(self, n1, n2): dist math.hypot(n2.x - n1.x, n2.y - n1.y) n max(int(dist / 0.2), 1) for i in range(n 1): t i / n px n1.x (n2.x - n1.x) * t py n1.y (n2.y - n1.y) * t for ox, oy, r in self.obstacles: if math.hypot(px - ox, py - oy) r: return False return True def plan(self): for _ in range(self.max_iter): rx, ry self.random_state() near self.nearest(rx, ry) new self.steer(near, rx, ry) if new is None: continue if not self.collision_free(near, new): continue new.parent near self.nodes.append(new) if math.hypot(new.x - self.goal.x, new.y - self.goal.y) self.goal_tol: return self.extract_path(new) return None def extract_path(self, node): path [] while node is not None: path.append((node.x, node.y)) node node.parent return path[::-1]调用方式也很直接。下面这个例子是10x10地图起点在左下角终点在右上角中间放三个圆障碍物。random.seed(42) obstacles [(5, 5, 1.0), (3, 8, 0.8), (8, 3, 0.8)] rrt RRT(start(0, 0), goal(10, 10), obstaclesobstacles, bounds(0, 10, 0, 10), step0.8, max_iter3000, goal_bias0.1) path rrt.plan() if path: print(找到路径节点数:, len(rrt.nodes)) print(path) else: print(在迭代次数内没有找到路径)第一次跑起来你会发现路径很“直”但又很“怪”节点之间是一段段折线整体可能绕了远路。这是RRT的正常表现能走就行别急着在原始版本上要求路径优美。3.2 参数调整与代码优化同样的地图换不同参数表现会差很多。我把自己常用的初始参数整理成一张表方便你从这些值开始试参数建议范围说明step地图尺寸的1%~2%步长过大会漏过窄通道过小会收敛慢goal_bias0.05~0.15目标偏置过高会让树在障碍物前绕圈goal_tolstep/2 左右目标判定容差max_iter2000~10000迭代上限越复杂给越多种子固定否则每次结果不同不利于排查碰撞检测采样间隔0.1~0.5视障碍物最小尺寸而定代码优化方面最容易见效的是把最近邻搜索从“遍历所有节点”改成“空间分桶”。简单做法是维护一个字典键是格子索引值是格子里的节点列表。每次查询只检查周边格子中的节点。碰撞检测也可以用距离场代替逐段采样。2D地图里开一个分辨率相同的网格提前计算每个格子到最近障碍物的距离大于机器人半径即可通行。这样collision_free变成两个网格查询性能提升非常明显。3.3 从Demo到真实工程对接在真正的机器人和机械臂上很少有人从头手写RRT更多是用开源运动规划库。常用的方案是配OMPL它内置了RRT、RRT*、RRT-Connect等一堆算法。你用OMPL时核心要干三件事定义状态空间比如2D的SE2、机械臂的RealVectorStateSpace设置碰撞检测接FCL或者自定义然后创建规划问题并选择planner。算法细节由库处理你只管地图、状态和终端约束。但要注意RRT输出的是几何路径不是机器人能直接执行的轨迹。几何路径只有位置信息缺少速度、加速度和时间分配。这就好比给你一张导航地图上面有路线但没告诉你每个路口该开多快。真实项目里还需要把路径平滑成连续曲线再做时间插值生成带速度约束的轨迹。平滑常用Bezier曲线、B样条或者裁剪“shortcut”法。另外在动态环境里地图是不断更新的RRT的好处是单次规划的代价低适合放进“感知—规划—控制”的实时循环里每次拿到新障碍物信息就重新规划一次。4. 调试中那些让人抓狂的问题4.1 目标点明明不远树就是不长过去这是我被问到最多的问题。目标点就在地图右上角树也已经在附近了但RRT始终不把最后一步迈过去。排查顺序通常是这样的先看目标偏置是不是设得太小如果只有1%那树几乎没有动力朝目标走纯粹靠随机采样碰运气效率自然低。可以把goal_bias逐步提到0.1到0.15再跑一次。还要确认目标点本身是否可到达。如果目标点放在了障碍物内部或者离障碍物边界太近机器人半径膨胀后根本站不进去那不管你跑多少次目标容差都无法满足。解决办法是给目标点也做一轮碰撞检测并且在生成目标位置后做一次“可到达性”检查。最后再看goal_tol是不是设得太小。步长0.5容差0.05最后一步恰好跨过去了但不在容差范围内就会出现“擦肩而过”的假象。把容差调到步长的一半很多问题立刻消失。4.2 狭窄通道场景怎么破狭窄通道是RRT的经典噩梦。想象一个地图中间只有一条宽度等于机器人直径的缝除此之外全是障碍物。均匀随机采样想命中这条缝概率可能只有百分之几。树在外围长了几千个节点就是钻不进去。应对策略有很多。最实用的是双向RRT也叫RRT-Connect同时从起点和终点各长一棵树两棵树交替扩展然后尝试把两棵树最近的节点连接起来。因为目标方向参与引导狭缝两端的树更容易“接上头”。还有一种思路是降低步长让树有更多机会在缝隙附近“微调”。更高级一点的做法是桥采样刻意在障碍物两侧采样并检测连接专门发现狭窄通道。如果你的问题场景结构特殊用网格预处理生成一个“走廊骨架”再沿骨架引导RRT采样也能明显改善。调试时一定记得把随机种子固定否则同样的配置每次结果都不一样很难判断到底是改对了还是运气好。4.3 越跑越慢最近邻和碰撞检测拖后腿RRT慢通常不是采样慢而是每轮都要在所有节点里找最近邻同时做多次碰撞检测。仿真里几百个节点不觉得到几千个节点时程序会越来越卡最后几乎走不动。我见过有人把节点存在Python list里每轮从头到尾遍历5000个节点时每轮循环要算5000次距离总共迭代一万轮就是5000万次距离计算。不慢才怪。排查顺序建议是先profile确认瓶颈是在Nearest还是CollisionFree再动手。Nearest可以换成空间分桶或者每50轮重建一次cKDTree。CollisionFree在2D里优先做距离场在3D和机械臂场景接FCL。如果你在主循环里反复分配新的Node对象也可以改成复用对象或者用数组存坐标。这些改完同样地图上性能翻几倍不夸张。4.4 原始路径太难看RRT找出来的路经常是一堆折线转角很生硬甚至贴着障碍物走。这不算bug是算法特性。最近邻策略导致树尽量连到已有节点路径完全没有代价概念锯齿和大绕路都很正常。处理办法有两层。第一层是shortcut拿到路径后反复尝试把相隔较远的两个节点直接相连如果中间无碰撞就把中间节点全部删掉路径立刻变短变顺。第二层是平滑用Bezier或B样条把折线变成连续曲线让机器人转起来不突兀。注意平滑后要做碰撞检测防止曲线贴着障碍物太近。真实项目中这两步几乎是标配因为很多执行器不能接受突然转向。5. RRT的“变种家族”与选型建议5.1 RRT*从可行解走向近似最优RRT最大的槽点是不找最优路径。RRT*就是在原始RRT上加了两个关键操作选择父节点时不只选最近邻而是在一个邻域范围内挑一个能让起点到新节点累计代价最小的节点新节点加入后再尝试用它重新连接邻域内的其他节点降低它们的累计代价这招叫rewire。rewire让RRT拥有了“渐进最优”的性质随着迭代次数增长路径代价会不断下降收敛到最优解附近。代价下降速度和质量与rewire半径有关常用理论值是这样的r gamma * (log(n) / n)^(1/d)其中n是节点数d是空间维度gamma跟环境有关。实际工程里不用太纠结公式直接给一个半径比如step的1.5到2倍或者地图尺寸的2%到5%再观察路径代价是否在下降。RRT的代价是计算量上去了每次插入新节点要多查邻域、多算几轮重新连接节点多的时候性能也会下降。5.2 还有哪些值得记住的变体除了RRT*RRT-Connect是工程里出现频率非常高的变体。它从起点和终点同时长树两棵树交替扩展每一次迭代都会尝试把两棵树的节点连接起来。优势是在狭窄通道和复杂环境中找路更快所以机械臂静态避障场景大家经常先用它跑通。它同样不保证最优。Informed RRT是RRT的一个漂亮改进它先运行RRT*找到一条可行路径然后后续采样不再全图撒点而是限制在一个以起终点为焦点的椭圆区域里。这个椭圆正好包含所有可能比当前路径更短的位置所以后续迭代都集中在“有意义”的区域收敛到最优的速度明显加快。还有Anytime RRT系列可以在有限时间内不断给出“当前最好”的路径适合有强实时性要求但算力还能保证的场景。最后如果机器人带动力学约束比如车不能原地转向、机械臂有速度和加速度限制就要用Kinodynamic RRT它在Steer环节不直接走直线而是满足动力学方程的运动原语。5.3 实际项目里怎么选我在不同项目里挑算法时会先问自己三个问题维度高不高环境是不是静态对路径代价敏不敏感如果只是验证规划链路无脑先用RRT参数随便给都能跑链路通了再说。机械臂在某固定工位干活环境基本不变优先考虑RRT-Connect或RRT*前者快后者路径更省。移动底盘在楼层里跑如果地图是2D栅格其实A或Hybrid A往往比RRT更合适因为地面环境偏结构化而且底盘运动有非完整约束。这时候硬套RRT反而得不偿失。有人喜欢把所有问题都用RRT解决我不建议。RRT的优势是通用和简单但通用意味着它没有利用具体场景的结构特性。在结构化、低维、静态的地图里A*的效率和路径质量都更好。正确的姿势是把它放进工具箱面对新问题时先用RRT类算法快速出可行解再根据需求换更优的算法。我自己的习惯是任何新环境第一次跑规划都会先用原始RRT做baseline。项目里第一次实车验证时同事非要先把RRT参数调好再动我建议先用RRT把整个闭环跑通结果半小时就定位到了测量地图和机器人半径不匹配的bug。后来换成RRT只改了规划层代码其他部分几乎没动。所以说别一上来就追求完美RRT最大的价值不是它自己有多强而是它能让你最快看到一条可以走的路然后你再慢慢优化。这就是我写这篇规划算法专栏第一篇的原因。