做移动机器人路径规划的朋友对RRTRapidly-exploring Random Tree快速扩展随机树应该都不陌生。它是很典型的采样类规划算法实现简单、通用性强我最早接触路径规划就是拿它练手的。不过实际跑起来就会发现标准RRT有个让人又爱又恨的特点随机采样太“佛系”树在自由空间里四处探索明明终点就在不远处却要等很久才能连上。要是在模拟环境里玩倒还好放到真实机器人导航场景里规划慢半拍整个运动控制链路都得跟着卡顿。这篇文章主要讲的是我对RRT做的一版工程化改进把目标偏置和高斯分布采样两种思路结合起来在一个二维规划场景里实现“目标偏置高斯分布RRT”并附上可以直接改着玩的Matlab代码。所谓目标偏置就是采样时以一定概率直接把目标点拿来当采样点让树有方向感而高斯分布则是在目标点附近按正态分布撒点把搜索重心集中到目标周围。两者结合之后既能保持RRT原有的探索能力又能明显加快收敛速度、缩短路径长度。如果你正在做机器人导航、机械臂避障这类任务或者刚学路径规划、想看看教科书之外的工程化实现长什么样这篇文章应该能给你一些参考。1. 为什么标准RRT不够用先看它的问题在哪1.1 标准RRT到底是怎么跑的先把标准RRT的流程说透。整个算法只有几步在地图范围内随机采样一个点 q_rand。在已经构建好的树上找到离 q_rand 最近的节点 q_near。从 q_near 朝 q_rand 方向扩展一个固定步长得到新节点 q_new。检查 q_near 到 q_new 这条线段是否与环境中的障碍物碰撞。如果没碰撞把 q_new 加入树并记录它的父节点是 q_near。重复以上过程直到树上某个节点落到终点 q_goal 的容差邻域内。这个过程很像把一棵树的根须不断往未知空间里伸每次只长一小截生长方向完全由随机数决定。因为采样点是全图均匀随机生成的所以树最终会填满整个自由空间——只要迭代次数够多找到路径从概率上讲是必然的。这也是RRT相比栅格搜索类算法最大的优势不用提前建图也不用对环境做太多简化假设甚至在障碍物分布很不规则的环境里也能跑。1.2 纯随机采样带来的两个真实痛点第一个痛点是收敛太慢。假设地图是 100×100 的单位平面起点在左下角终点在右上角。均匀随机采样下树有相当大比例的生长方向是贡献很小的——比如朝左下角或者中路偏左的方向长。虽然从长期看树能覆盖全图但现实中的机器人导航是有迭代预算的。我们肯定不希望机器人站在那儿等上几千次迭代才规划出一条路线。第二个痛点是路径质量差。随机扩展缺少方向引导树会先充满大量无关区域最后找到的路径往往又绕又长而且每次运行结果都不一样。同一张地图这次规划出的路径和下次规划出的路径可能差很远。这在某些需要稳定重复执行的工业场景里是比较头疼的。1.3 目标偏置和高斯分布是两条互补的改进路线改进方向很自然既然慢是因为随机采样不带方向那就让采样点更偏向目标。最直接的做法是目标偏置——每次迭代以概率 P_goal 直接令 q_rand q_goal这样树会有一定比例的节点强行朝终点生长。这个做法见效特别快但用多了也有副作用如果终点被障碍物半包围采样点全是目标点时扩展请求会被碰撞检测反复拒绝白白消耗迭代次数。另一条路线是改变采样分布。既然全图均匀采样太浪费那就在目标点附近以高斯分布采样比如 q_rand q_goal sigma * randn(1,2)。这样采样点以一定概率密集落在目标邻域内又能保持小范围的探索弹性比单纯把采样点钉死在终点上更有自由度。两条路线并不冲突甚至可以叠加。标准做法是在采样环节设置三分支一部分概率直接取目标点一部分概率在目标邻域做高斯采样剩下小概率继续全局随机采样。这样树既知道“终点在哪”又不会因为目标方向被堵死而原地空转还能保持探索未知空间的能力。这就是目标偏置高斯分布RRT的核心思想。2. 算法设计拆解采样策略才是灵魂所在2.1 三分支采样策略怎么定采样函数是我在设计时最看重的地方它直接决定整个算法的性格。具体分支如下目标偏置分支概率 P_goal。此时 q_rand 直接取终点。这个分支是“冲刺信号”每隔几次迭代就让树朝目标方向强行生长一次。高斯采样分支概率 P_gauss。此时以终点为中心按标准差 sigma 生成二维正态分布采样点。这个分支负责在目标邻域内“布网”专门应对目标点被局部障碍物遮挡的情况。全局随机分支概率 1 - P_goal - P_gauss。此时在全图均匀随机采样。这个分支保证树不会因为过度聚焦目标而失去探索地图远端的可能性在某些需要绕大圈的场景里非常关键。我在某个模拟项目里测试过P_goal 取 0.2、P_gauss 取 0.5、全局随机概率 0.3 的效果就比较均衡。之后继续跑测试时发现这个配比还要配合 sigma 一起看不能机械照搬。2.2 高斯采样的数学原理和直观理解二维高斯分布采样在Matlab里实现很简单randn(1,2) 生成两个相互独立的标准正态分布随机数均值是0方差是1。然后做线性变换q_rand q_goal sigma * randn(1,2)这样生成的点会以 q_goal 为中心向四周扩散扩散程度由 sigma 控制。大约有 95% 的采样点会落在以 q_goal 为圆心、半径约 2*sigma 的圆内。为什么这种分布在路径规划里好用因为路径规划最后一段往往也是最难的一段终点附近可能贴着障碍物或者需要从某条狭窄通道进入终点区域。如果采样点只是均匀撒在地图上落在终点附近自由空间里的概率并不高。而高斯采样把计算资源集中投放到终点邻域能让树在这个关键区域更快积累节点、找到可行的进入通道。可以把它理解为缩小版的“搜索热力图”——离终点越近搜索越密。2.3 完整伪代码与运行流程整个改进算法的伪代码如下。其实主体结构和标准RRT完全一致变化只在采样这一步输入地图 map起点 start终点 goal参数集合 params 初始化vertices ← [start]parents ← [0] 重复直到达到最大迭代次数 maxIter r ← rand() 如果 r P_goal q_rand ← goal 否则如果 r P_goal P_gauss q_rand ← goal sigma * randn(1,2) 将 q_rand 限制在地图范围内 否则 q_rand ← 地图内均匀随机点 在 vertices 中找离 q_rand 最近的节点 q_near q_new ← 从 q_near 向 q_rand 扩展 stepSize 步长 如果线段(q_near, q_new) 无碰撞 将 q_new 加入 verticesparents 记录父节点索引 如果 norm(q_new - goal) goalTol 从 q_new 回溯生成路径返回成功 返回失败从伪代码可以看出来改进版RRT的工程改动量其实非常小。但这恰恰是它的优点核心理念变了代码变动却控制在很小的范围迁移成本低、调试方便非常适合作为路径规划功能的基础框架。3. Matlab实现核心代码逐段讲解3.1 主函数与数据结构规划Matlab是我做算法原型验证比较喜欢用的工具主要原因有三点矩阵运算方便、内置绘图交互顺手、脚本调试时变量窗口能直接看到树节点的分布情况。写路径规划算法时建议把地图、障碍物、参数都封装成结构体避免散落一堆全局变量。下面的主函数是完整框架function [path, vertices, parents] rrtGoalBiasGaussian(map, start, goal, params) vertices start; % 节点列表N行2列每行一个坐标 parents 0; % 父节点索引根节点为0 path []; for iter 1 : params.maxIter qRand sampleGoalBiasGaussian(goal, params); [idx, ~] nearestNeighbor(vertices, qRand); qNear vertices(idx, :); qNew extend(qNear, qRand, params.stepSize); if isCollisionFree(map, qNear, qNew, params) vertices(end1, :) qNew; parents(end1) idx; if norm(qNew - goal) params.goalTol path findPath(vertices, parents, length(parents), start); break; end end end end这里把树结构简化为两个数组vertices 存节点坐标parents 存每个节点的父节点索引。每次加入新节点只需要动态追加一行Matlab处理这种操作没有问题。等节点数上万之后才需要考虑预分配内存一般二维规划场景用不到那个量级。3.2 目标偏置高斯采样函数采样函数是整个改进的核心我单独拉出来讲。注意高斯采样得到的点可能超出地图边界必须做限制不然后续最近邻搜索和碰撞检测都会出现索引越界的问题。function qRand sampleGoalBiasGaussian(goal, params) r rand(); if r params.pGoal % 目标偏置分支直接返回终点 qRand goal; elseif r params.pGoal params.pGauss % 高斯采样分支在终点周围按正态分布撒点 qRand goal params.sigma * randn(1, 2); % 限制在地图边界范围内 qRand(1) min(max(qRand(1), params.mapXmin), params.mapXmax); qRand(2) min(max(qRand(2), params.mapYmin), params.mapYmax); else % 全局随机分支保留探索能力 qRand [params.mapXmin rand() * (params.mapXmax - params.mapXmin), ... params.mapYmin rand() * (params.mapYmax - params.mapYmin)]; end end这里有一个容易被忽略的坑randn 生成的是标准正态分布可能产生负值或者较大的离群值。虽然高斯分布本身就允许这种“长尾”出现但在边界限制时要意识到超出边界的点会被粗暴地压到边界线上大量点叠在边界上会引导树朝边界生长。如果地图的终点本来就贴边这个问题影响不大如果终点在地图中央sigma 就不要取太大避免高斯采样的点过多被压到地图边缘。3.3 最近邻搜索与扩展逻辑最近邻搜索最直接的做法是线性扫描计算所有节点到 qRand 的距离取最小值。节点数量在几千量级时Matlab在几毫秒内就能算完。如果之后想扩展到复杂地图或高维空间可以把线性扫描换成K-D树但初期不需要这么做先跑通流程更重要。function [idx, minDist] nearestNeighbor(vertices, qRand) diff vertices - qRand; distSq sum(diff.^2, 2); [minDistSq, idx] min(distSq); minDist sqrt(minDistSq); end扩展函数的逻辑是固定步长截断。如果 qNear 和 qRand 的距离本身就小于 stepSize直接让 qNew 等于 qRand 就行这样可以更精确地“够到”采样点。function qNew extend(qNear, qRand, stepSize) dirVec qRand - qNear; dist norm(dirVec); if dist stepSize qNew qRand; else qNew qNear (dirVec / dist) * stepSize; end end扩展步长直接决定了树的疏密程度。步长太大会“跨过”狭窄通道导致明明有路却找不到步长太小迭代次数需求猛增规划时间线性上涨。一般取地图最长边尺寸的 5% 到 10% 做初始值比较合适。3.4 碰撞检测为什么不能只检查端点我最初写RRT时犯过一个经典错误碰撞检测只检查 qNew 这个点是否在障碍物内结果路径直接斜穿矩形障碍物的角。原因很简单qNear 和 qNew 之间的线状路径穿越了障碍物但点状检查恰好避开。正确做法是把这条线段离散成若干采样点逐一判断是否落在障碍物区域function ok isCollisionFree(map, qNear, qNew, params) segLen norm(qNew - qNear); nSteps ceil(segLen / params.checkResolution) 1; ok true; for s 0 : nSteps t s / nSteps; p qNear (qNew - qNear) * t; if ~isPointFree(map, p) ok false; return; end end endisPointFree 的实现取决于障碍物的表达方式。如果地图用栅格矩阵表示直接看对应像素是否为0如果障碍物用圆形列表表示就计算点到各圆心距离是否小于半径加安全裕量。我习惯用圆形障碍物列表做测试因为场景变化快不用重新画地图。3.5 路径回溯与可视化找到终点邻域内的节点后沿 parent 索引一路回溯到起点就可以得到完整路径。路径顺序需要反转一下让结果从起点到终点排列方便后续直接给运动控制模块用。function path findPath(vertices, parents, nodeIdx, start) path []; while nodeIdx 0 path [vertices(nodeIdx, :); path]; nodeIdx parents(nodeIdx); end end可视化部分的代码不多但很有价值。在Matlab里把树节点、扩展边、障碍物、最终路径用不同颜色画在一张图上参数调优时可以非常直观地发现问题。树的生长方向是否集中、哪些区域节点冗余、路径有没有异常折线一眼就能看出来。figure; hold on; % 画树边 for i 2 : length(parents) pIdx parents(i); plot([vertices(i,1), vertices(pIdx,1)], ... [vertices(i,2), vertices(pIdx,2)], b-, LineWidth, 0.5); end % 画路径 plot(path(:,1), path(:,2), r-, LineWidth, 2);4. 关键参数怎么调从慢吞吞到干净利落4.1 采样概率怎么配比目标偏置概率 P_goal 和高斯采样概率 P_gauss 是影响算法性格的一组关键参数。我实测下来P_goal 设在 0.2 到 0.3 比较合适。低于 0.1方向引导效果微弱高于 0.4扩展请求被障碍物拒绝的概率会明显提升。P_gauss 通常在 0.4 到 0.5 之间和 P_goal 加起来控制在 0.7 以内给全局随机分支留出至少 0.3 的探索空间。有个很直观的现象可以说明配比的重要性如果把 P_goal 拉到 0.9从起点看向终点的连线方向一旦被障碍物挡住树会不断尝试从同一个方向穿越障碍物被碰撞检测拒绝后单次迭代的有效扩展率极低。从可视化窗口看树在障碍物前面堆了一大团节点就是过不去。这时候把一部分概率还给高斯采样分支采样点会分散到终点周边树就有机会从侧面绕进去。4.2 高斯方差 sigma 和扩展步长的配合sigma 的选择没有绝对标准但要结合地图尺寸和目标区域的环境复杂度。假设地图 100×100sigma 取 15 到 25 是合理的起步范围。sigma 太小高斯采样点都堆在终点极小的邻域里很难形成有效扩展sigma 太大高斯采样的效果就退化成全局随机采样失去意义。扩展步长 stepSize 和高斯 sigma 之间存在配合关系。如果 sigma 只有 10而 stepSize 取 30那么高斯采样点即使落在终点附近单次扩展也会直接跨越较大的距离最后路径粗糙、无法贴近终点。我一般建议 stepSize 不大于 sigma这样树在目标区域生长时能有足够的节点密度把最后一段路径“切”得细致一点。我整理了一份参数参考表适合 100×100 的二维地图参数推荐范围影响P_goal0.2 - 0.3越大方向性越强但容易被障碍物卡住P_gauss0.4 - 0.5控制终点邻域的搜索密度sigma15 - 25高斯采样的扩散半径stepSize5 - 10树的生长粒度goalTol5 - 8判定到达终点的阈值checkResolution2 - 5碰撞检测离散采样间隔4.3 最大迭代次数与容差最大迭代次数 maxIter 不宜设成固定值建议设大上限并在找到路径后提前退出。比如 maxIter 设 5000实际可能几百次迭代就成功了。这样既保证成功概率又不会在测试阶段浪费时间。goalTol 取 stepSize 的 0.8 到 1.2 倍比较合理太大了路径终点离真实目标点明显偏移太小了最后几段扩展很难精确落进容差范围内。4.4 标准RRT与改进版RRT的效果对比我在同一个地图场景里对比过标准RRT、纯目标偏置RRT和本文的改进版本。地图上布置了五个圆形障碍物和两个矩形障碍物起点在左下角终点在右上角一侧中间有一段不算宽的通道。结果非常能说明问题标准RRT大概需要 1800 次迭代才能找到路径而且路径明显绕路纯目标偏置RRT把迭代次数压到了 800 次左右但成功率受障碍物分布影响大目标偏置高斯分布RRT稳定在 400 次上下路径长度也缩短了约 20%。当然这个数据只代表我设置的场景换一张地图结果会有波动。但方向上可以确认采样策略的改进确实能把规划效率拉升一个明显台阶。5. 调试过程中踩过的坑与排查技巧5.1 树在原地打转长不出去症状是树在起点附近堆积大量节点迭代几百次后树的覆盖范围还是很有限。我排查时发现原因主要是 stepSize 太小或者 sigma 取值不合理。stepSize 太小导致每次扩展距离不足树每轮只挪动一点点sigma 太小则高斯采样点全部挤在终点周围远离终点的区域几乎得不到采样树自然长不远。先把 stepSize 调到地图尺寸的 10% 附近再把 sigma 调大问题基本就解决了。5.2 路径锯齿严重机器人没法直接执行RRT系算法的通病找到的路径是一串折线段转折角往往很尖锐。严格来说这不算 BUG但给到机器人运动控制时会造成频繁加减速。我的处理办法是在路径回溯完成后加一个后处理贪心平滑或三次样条插值。贪心平滑的思路是尝试把路径上的第 i 个点和第 i2 个点直接相连如果线段无碰撞就跳过一个中间点重复遍历几轮之后路径会明显变顺。5.3 碰撞检测误判导致路径穿墙这个问题我在前面提过属于最容易踩的坑。只检查 qNew 单点是否在障碍物内线段穿越障碍物中段时是完全检查不出来的。另外 checkResolution 取值太大会漏掉薄的障碍物取值太小计算量上升但结果也更可靠。建议先按地图尺寸的 2% 做离散间隔再根据不同障碍物尺寸手动微调。5.4 规划结果不稳定同一地图每次跑都不一样这是RRT本身的随机性造成的。如果业务场景需要稳定输出有两个办法一是固定随机数种子至少保证调试时可复现二是做“规划重试”连续跑三轮取路径最短的那次。第二个办法我在实际测试中用的更多因为算法本身很快多跑几次并不浪费多少时间。现象可能原因处理建议树堆积在起点stepSize太小 / sigma太小增大扩展步长调整高斯方差路径穿障碍碰撞检测只查端点对线段离散化逐点检查规划成功率低P_goal过高降目标偏置概率增加高斯分支路径太绕缺少后处理贪心平滑或样条插值5.5 最后一公里的节点密度问题终点公开满足 norm(qNew - goal) goalTol 条件就停止但路径最后一段往往是从较远方向大步长扩展过来的终点附近的路径点稀疏形成一条长折线。改进办法是在找到路径后不直接返回而是在终点邻域内继续做几次小步长扩展让树有更多靠近终点的节点再把最佳接入点作为路径末端。这个小技巧能让路径的末端贴合度明显提升视觉上和实际控制效果都更好。我在实际使用中还有一点体会调试路径规划算法一定要把可视化打开。Matlab的绘图交互对排查节点堆积、障碍物边缘碰撞这类问题帮助极大。你可以很直观地看到树在每个区域分配了多少节点、扩展被拒绝的位置集中在哪里这些信息比看日志和数据文件有用得多。这篇内容后续还可以往几个方向扩展。一是把碰撞检测从圆和矩形障碍物推广到任意多边形二是把采样策略和 RRT* 的重新布线机制结合在路径长度和曲率上继续优化三是把算法移植到更高维度的机械臂规划场景。如果你对其中某个方向有实践成果欢迎一起交流。
