简介这是一份基于MATLAB实现的RRTconnect双向路径规划算法源码适合机器人运动规划、自动驾驶避障等场景的学习与二次开发尤其适合刚接触RRT系列算法的学生和工程师。RRTconnect在标准RRT基础上引入双树扩展机制通过同时从起点和终点生长两棵快速搜索随机树来加速连接提高高维空间下的路径搜索效率。压缩包共7个文件其中6个为.m函数脚本覆盖主程序、随机采样、最近邻搜索、树节点扩展、碰撞检测以及两树连接等完整流程另附1个xlsx坐标数据文件用于存放地图或采样点数据整体仅13KB结构清晰轻量。目前已有2620人学习下载。使用者可在MATLAB中直接运行主文件查看动态建树过程并依据示例数据修改障碍物布局、采样数量或步长等参数深入理解算法各环节的作用代码模块化程度高也便于移植到其他项目或与RRT、RRT*等算法进行对比实验是学习路径规划的实用参考。1. RRTconnect 的 MATLAB 代码为什么双向生长反而比单向更难写对RRT-Connect 是路径规划里最常用的双向 RRT 变体从起点和终点各长一棵树两棵树相向扩展、互相靠近一旦接通就返回路径。很多刚接触的人第一反应是“这不就是把 RRT 的 extend 改成从两头各跑一遍吗”真动手写 MATLAB 代码时才发现两棵树的数据结构、最近邻搜索、连接判定和路径回溯各有各的脾气照着网上零散片段拼出来的程序十次里有八次要么两棵树永远接不上要么返回的路径带着莫名其妙的绕路。这篇笔记就按我自己从零写 RRTConnect.m 的完整过程来拆把核心函数、参数调法和常见翻车点一次讲清楚适合正在做机械臂、移动机器人或自动驾驶局部路径规划的工程师照着重现。先说结论RRT-Connect 的代码量并不大核心不到两百行但难点全在“两棵树怎么高效地互相靠近”和“连接成功之后怎么正确回溯路径”这两个点上。本文给出的程序不依赖任何工具箱函数纯粹的 MATLAB 原生语法复制到 2016 以后任意版本都能直接跑。读完你不仅能跑通二维平面地图还能自己动手改成三维空间版本或者接入真实机器人平台。2. RRT-Connect 到底改了什么从单向生长到贪心连接2.1 单向 RRT 的瓶颈和双向生长的动机普通 RRT 每轮迭代只从起点那棵树出发在自由空间里采样一个随机点然后找树上离它最近的节点朝这个方向扩展一小步。这种做法在空旷地图里没问题但一旦地图里有狭长走廊、U 形障碍物这类结构单向树要“绕”进走廊再“绕”出来需要大量无效迭代因为随机采样点大部分落在走廊外面的开阔区域树很难朝走廊深处推进。RRT-Connect 的核心思想是让树从两端一起长终点那棵树不是静止等待被连接而是主动朝起点树扩展两棵树互相“寻找”在走廊类场景里能把搜索效率提升一个量级。具体来说RRT-Connect 每一轮迭代做两件事第一从起点树随机采样一个点按 RRT 的方式扩展一步第二从终点树出发不再随机采样而是直接朝起点树刚生成的那个新节点做“贪心扩展”——也就是反复往这个方向走直到碰到障碍物、到达目标点或者两棵树成功接通。第二个步骤是 RRT-Connect 区别于普通 RRT 的关键它让终点树始终瞄准起点树的最新进展而不是漫无目的地随机游走。2.2 贪心扩展Connect和普通扩展Extend的区别普通 RRT 的 Extend 每次只走一个固定步长走完就结束本轮。RRT-Connect 的 Connect 则是在不碰障碍物的前提下沿着同一方向连续走多个步长直到以下三种情况之一发生到达了目标点也就是起点树的新节点、撞上障碍物、或者已经离目标点足够近小于连接阈值。这里有个很容易被忽略的点Connect 的返回结果不只是“扩展后的新节点”还要告诉调用者这次扩展是成功了、撞墙了还是到达了三个状态对应三种处理逻辑。从 MATLAB 实现角度看Extend 是一个步长一个步长地调用“朝目标点走一步”的函数而 Connect 是拿一个 while 循环反复调用同样的函数直到状态变化为止。所以代码组织上通常把“朝目标点走一步”的底层函数抽出来Extend 和 Connect 都复用它这样逻辑清晰且避免重复代码。我在工程里习惯把这个底层函数命名为 grow输入是当前点、目标点、步长和地图输出是新的点坐标以及是否发生碰撞。2.3 两棵树的连接判定距离阈值与真正的“握手”很多初版实现把连接条件写成“两棵树上出现坐标完全相同的节点”这在连续空间里几乎不可能发生程序会一直跑到最大迭代次数然后宣告失败。正确的做法是设定一个连接阈值当起点树上某个节点和终点树上某个节点的欧氏距离小于这个阈值时就认为两棵树接通了。阈值一般取步长的 0.5 到 1 倍太小会导致连接条件太苛刻、成功率下降太大则路径在连接点附近会出现明显的“折角”而且这个折角处的线段可能斜穿障碍物。连接成功之后还有一个关键步骤路径回溯。普通 RRT 只需要从终点节点沿着父亲指针一路走回起点但 RRT-Connect 有两条链——一条从连接点沿起点树的父指针回溯到起点另一条从连接点沿终点树的父指针回溯到终点。这里最常犯的错误是把两条链的节点直接拼在一起却忘了第二条链是“从终点倒着长出来的”它的父指针方向是朝向终点的所以必须先把第二条链反转再拼接。后面避坑章节会专门展开这个翻车点。3. 用 MATLAB 从零实现 RRTConnect核心函数与可运行代码3.1 地图建模和碰撞检测先让程序“看得见墙”路径规划的第一步不是写 RRT而是定义地图并实现可靠的碰撞检测。碰撞检测是 RRT 系列算法里最容易被低估的部分——很多跑不通的 RRT问题不是出在树生长逻辑而是碰撞检测漏检或误检。我用的是经典的栅格地图加线段碰撞检测地图用二维矩阵表示0 是自由空间1 是障碍物两点之间的连线是否碰到障碍物用 Bresenham 直线算法逐点查表。function collision checkCollision(p1, p2, map) % 检查线段 p1-p2 是否穿越障碍物栅格 % p1, p2: [x, y] 坐标map: 二维逻辑矩阵true 表示障碍 x1 round(p1(1)); y1 round(p1(2)); x2 round(p2(1)); y2 round(p2(2)); [rows, cols] size(map); % 越界直接视为碰撞 if x1 1 || x1 cols || y1 1 || y1 rows || ... x2 1 || x2 cols || y2 1 || y2 rows collision true; return; end % 使用Bresenham算法遍历线段经过的所有栅格 dx abs(x2 - x1); dy abs(y2 - y1); sx sign(x2 - x1); sy sign(y2 - y1); err dx - dy; x x1; y y1; while true if map(y, x) % 注意MATLAB矩阵索引是行y列x collision true; return; end if x x2 y y2 break; end e2 2 * err; if e2 -dy err err - dy; x x sx; end if e2 dx err err dx; y y sy; end end collision false; end这个函数有几个细节值得注意。第一MATLAB 矩阵索引是“先行后列”也就是 map(y, x)而不是 map(x, y)写反了会在非对称地图上出现诡异的碰撞误判。第二输入坐标要先 round 取整否则 Bresenham 算法在浮点坐标上会漏掉某些栅格。第三边界检查放在最前面因为扩展过程中新节点可能落在地图外这种情况一律按碰撞处理比单独处理越界要省事。3.2 树的数据结构用结构体数组还是用对象RRT 的 MATLAB 实现里每棵树要存两类信息节点的坐标列表以及每个节点在树里的父亲索引。常见做法是定义两个数组nodes 存坐标parents 存父节点下标两者长度一致起始节点坐标为起点父亲索引为 0。用结构体数组也行但 MATLAB 里动态扩展结构体数组的开销比普通数组大迭代两千次以后速度差异肉眼可见。我一般用两个普通数组配合一个 treeNodeCount 计数器来管理。从面向对象的角度可以写一个 RRTTree 类来封装这些数据但对这个规模的算法类的继承和封装反而增加理解成本。工程上的平衡做法是核心函数全部写成独立 function树的数据用结构体 tree 打包传递tree.nodes、tree.parents、tree.count 三个字段足够。这样既比裸数组清晰又不会因为对象继承体系影响调试。3.3 核心主循环两棵树交替生长的完整流程下面是 RRTConnect 的主函数每一步都配了注释说明这是在干什么。这个版本的参数放在结构体 config 里方便统一调整。function [path, tree1, tree2] RRTConnect(startPt, goalPt, map, config) % 主函数双向RRT-Connect路径规划 % startPt: [x,y] 起点坐标; goalPt: [x,y] 终点坐标 % config: 包含 stepSize, maxIter, connectDist, goalProb 等参数的结构体% 从起点初始化树1 tree1.nodes startPt; % 第一行是起点 tree1.parents 0; % 起点没有父亲 tree1.count 1; % 从终点初始化树2 tree2.nodes goalPt; tree2.parents 0; tree2.count 1; % 提前检查起点和终点本身是否在障碍物里 if checkCollision(startPt, startPt, map) || checkCollision(goalPt, goalPt, map) path []; return; end for iter 1:config.maxIter % 以一定概率采样目标点其余时间随机采样 if rand config.goalProb randPt goalPt; else randPt [rand * size(map, 2), rand * size(map, 1)]; end % 树1普通RRT扩展一步 [newNode1, success1] extend(tree1, randPt, map, config.stepSize); if success1 % 树2朝树1的新节点做贪心连接 connectResult connect(tree2, newNode1, map, config); % connectResult.status: reached表示两树接通 if strcmp(connectResult.status, reached) % 提取并返回路径 path extractPath(tree1, tree2, newNode1, connectResult.newNode, startPt, goalPt); return; end end % 交换两棵树下一轮从树2开始生长 temp tree1; tree1 tree2; tree2 temp; end % 达到最大迭代次数仍未找到路径 path [];end主循环里有三个设计决策需要说明。第一每轮迭代后交换两棵树这样起点树和终点树交替获得“主动扩展”的机会避免一棵树只顾自己长、另一棵树被动等待。第二extend 的随机采样只发生在一棵树上另一棵树永远朝对方的最新节点贪心前进这是 RRT-Connect 区别于“两棵都随机扩展”的朴素双向 RRT 的本质。第三提前检查起点终点是否在障碍物里是很便宜的防御性代码能避免后面很多莫名其妙的错误。3.4 extend 与 connect 的实现步进和贪心的正确姿势extend 和 connect 都依赖一个共同的底层操作从当前节点朝目标方向走一步。这一步要检查新节点是否合法合法则加入树并返回新节点下标。function [newNode, success] extend(tree, targetPt, map, stepSize) % 在树tree上朝targetPt扩展一步 % 找到离targetPt最近的现有节点 dists sqrt(sum((tree.nodes - targetPt).^2, 2)); [~, idx] min(dists); nearPt tree.nodes(idx, :);% 朝目标方向走stepSize距离 delta targetPt - nearPt; dist norm(delta); if dist stepSize newPt targetPt; % 目标点就在一步之内直接到达 else newPt nearPt stepSize * delta / dist; end % 检查这段路径是否碰撞 if checkCollision(nearPt, newPt, map) success false; newNode []; return; end % 加入树 tree.nodes(end1, :) newPt; tree.parents(end1, :) idx; tree.count tree.count 1; newNode newPt; success true;endconnect 函数则是在 extend 的基础上加一个 while 循环持续朝目标点延伸直到碰壁或接近到阈值以内。function result connect(tree, targetPt, map, config) % 贪心扩展持续朝targetPt生长直到撞墙或到达 while true distToTarget norm(tree.nodes(end, :) - targetPt); if distToTarget config.connectDist result.status reached; result.newNode tree.nodes(end, :); return; end [newNode, success] extend(tree, targetPt, map, config.stepSize); if ~success result.status blocked; result.newNode []; return; end % 如果这次扩展正好到达目标点距离小于一步长也算接通 if norm(newNode - targetPt) 1e-6 result.status reached; result.newNode newNode; return; end end end注意 connect 里判断是否到达用的条件是“当前树最后一个节点到目标点的距离小于连接阈值”而不是“新生成的节点坐标等于目标点坐标”。因为经过多轮扩展树2的末端可能已经越过目标点或落在目标点旁边此时再延伸已经没有意义直接判定接通更高效。3.5 路径回溯两条链的正确拼接方法路径回溯是 RRT-Connect 最容易写错的地方。两棵树在连接点握手后从连接点出发往起点方向和终点方向各有一条链这两条链的拼接顺序必须处理正确。function path extractPath(tree1, tree2, connectNode1, connectNode2, startPt, goalPt) % 从连接点回溯两条链并拼接 % tree1: 起点树, connectNode1: 树1侧连接点 % tree2: 终点树, connectNode2: 树2侧连接点% 回溯树1从连接点一路回到起点 path1 []; idx find(ismember(tree1.nodes, connectNode1, rows)); while idx 0 path1 [path1; tree1.nodes(idx, :)]; idx tree1.parents(idx); end path1 flipud(path1); % 从起点到连接点 % 回溯树2从连接点一路回到终点 path2 []; idx find(ismember(tree2.nodes, connectNode2, rows)); while idx 0 path2 [path2; tree2.nodes(idx, :)]; idx tree2.parents(idx); end % 注意path2 是从连接点到终点的顺序直接拼接到path1后面即可 % 拼接时避免连接点重复 path [path1(1:end-1, :); path2];end这段代码最关键的一行是最后的拼接path1 已经翻转成“起点→连接点”的顺序而 path2 本来就是“连接点→终点”的顺序因为树2是倒着长的所以直接拼接时要去掉 path1 的最后一个点即连接点本身否则连接点会出现两次。这个 bug 很隐蔽因为路径上多一个点不会报错只有当你统计路径长度或做平滑时才会发现数值不对。3.6 完整跑通的最小示例和可视化最后给一个完整的测试脚本能在一张带障碍物的地图上跑通并画图。把上面的函数按文件名保存checkCollision.m、extend.m、connect.m、extractPath.m再运行下面这段。% 测试脚本在简单地图上运行RRTConnect map zeros(50, 50); % 50x50栅格地图 map(20:30, 15:35) 1; % 中间放一块矩形障碍物 map(10:15, 30:35) 1; % 右上角放一块 startPt [5, 5]; goalPt [45, 45];config.stepSize 3; % 扩展步长单位栅格 config.maxIter 2000; % 最大迭代次数 config.connectDist 2; % 两树连接阈值 config.goalProb 0.1; % 采样目标点的概率[path, ~, ~] RRTConnect(startPt, goalPt, map, config);if ~isempty(path) figure; imagesc(map); colormap(gray); hold on; plot(path(:,1), path(:,2), r-, LineWidth, 2); plot(startPt(1), startPt(2), go, MarkerSize, 10, MarkerFaceColor, g); plot(goalPt(1), goalPt(2), bo, MarkerSize, 10, MarkerFaceColor, b); title(RRT-Connect Path); else disp(路径规划失败请调整参数); end跑通之后建议自己改几个参数看看效果把 connectDist 调大到 6路径会在中间出现明显的折线段把 goalProb 调大到 0.9树会更快偏向目标但可能跳过某些可行通道把 stepSize 调大到 8路径会变粗糙但规划速度明显提升。这些直观感受比任何理论分析都有助于理解参数的作用。4. 参数怎么调RRT-Connect 的 4 个必调参数和一组经验值4.1 步长 stepSize规划质量与成功率的直接平衡杆步长决定每次扩展在空间里走多远。步长太大路径粗糙且容易“跳过”狭窄通道表现为明明地图上有路规划却老是失败步长太小路径细腻但收敛极慢迭代几千次还在起点附近磨蹭。经验法则是步长取地图最短边长度的 1% 到 3%对 50x50 的栅格地图就是 1 到 3 格。更科学的调法是从最大迭代次数倒推。如果你希望规划器在 500 次迭代内出结果而地图对角线长度大约 70 格那步长至少要 70*2/500 ≈ 0.28 格——当然这是理论下限实际取 1 到 3 格更稳妥。另外步长还直接决定连接阈值的取值范围通常连接阈值取 0.5 到 1 倍的步长两个参数要一起调。4.2 连接阈值 connectDist路径折角和成功率的博弈连接阈值是 RRT-Connect 特有的参数标准 RRT 里没有对应项。它决定了“两棵树相距多近就算接通”。阈值设得越大两棵树越容易接通规划成功率越高但代价是连接点附近路径会出现明显的“V 形折角”——因为两个节点并没有真正重合中间隔着一小段空隙这段空隙直接连成直线。如果地图上障碍物较少、路径质量要求不高可以把阈值设到步长的 1.5 倍规划速度快如果路径要交给跟踪控制器执行阈值尽量控制在步长的 0.5 倍以内宁可多迭代几轮也要让连接点尽量贴近。我一般把默认值设为步长的 0.8 倍成功率与路径质量比较均衡。4.3 目标偏置概率 goalProb采样策略的甜点位RRT 系列算法的采样环节有个经典改进以一定概率直接采样目标点而不是完全随机。这样树会有一个明确的“引力”朝目标方向生长。goalProb 太大树容易被引导进死胡同然后反复碰壁太小则退化成纯随机采样狭窄通道场景效率低。RRT-Connect 里这个参数的作用比标准 RRT 弱一些因为树的另一半终点树本身就在主动朝起点树生长相当于天然的“双向偏置”。我通常设 0.05 到 0.15再大收益不大。在迷宫类地图里甚至可以设 0完全靠双向生长的贪心机制来探索。4.4 最大迭代次数 maxIter给规划器一个合理的预算maxIter 不是调出来的而是根据地图复杂度和实时性要求算出来的。二维平面地图空旷场景通常 200 到 500 次迭代就能找到路径有若干障碍物簇的复杂地图1000 到 3000 次比较常见如果 5000 次还找不到大概率不是迭代不够而是步长、连接阈值或地图本身有问题。工程上建议把 maxIter 设为运行时预算的一部分先测出单次迭代的平均耗时再乘以预算时间得到迭代次数上限。比如单次迭代 0.5ms、预算 2 秒那 maxIter 设 4000 就足够。还有一个细节循环里每轮都交换两棵树但 swap 操作本身也消耗时间迭代次数很大时可以考虑用奇偶判断来代替实际的变量交换。参数调好之后的验证方式我会在最后一章单独展开。这里先给一组在 50x50 栅格地图上比较稳的默认值stepSize3、connectDist2、goalProb0.1、maxIter2000。这组参数不是最优的但能保证大多数地图不失败适合作为调参起点。5. 避坑指南RRT-Connect MATLAB 实现的 5 个典型翻车现场5.1 路径回溯时连接点重复出现现象规划成功率正常但输出的路径里相邻两个点坐标完全相同导致路径长度计算偏大跟踪时机器人原地停顿一下。原因拼接 path1 和 path2 时没有去掉重复的连接点。path1 的最后一个点是连接点path2 的第一个点也是连接点因为 connect 函数返回的新节点就是树里已有的节点extractPath 里又把它加了一次。解决拼接前把 path1 的最后一个点删掉或者让 extractPath 在拼接时判断两个数组的端点是否相等相等则去重。我习惯用前一种方案因为更直接而且省一次判断。5.2 两棵树交叉越过但连接判定失败现象把树1和树2的节点画出来明显看到两条轨迹已经交错距离非常近但程序就是不报 reached一直跑到 maxIter 耗尽。原因connect 函数里的 reached 判断条件是“当前树最后一个节点到目标点的距离小于 connectDist”。如果两棵树的节点轨迹交错但树2的最后一个节点恰好不在目标点附近比如最后一次扩展走了一个大步长恰好越过了目标点那距离可能反而变大了。解决在 connect 循环里增加一个判断——本次扩展生成的新节点到目标点的距离如果小于连接阈值也算接通。也就是说每扩展一步都检查一次而不是只检查树末端节点。这个坑的血泪经验是树末端节点和“距离目标最近的节点”不是一回事尤其在步长较大的时候。5.3 碰撞检测把斜穿障碍物当成安全路径现象路径规划的树生长正常但最终路径画出来有明显的线段斜穿障碍物角落肉眼可见穿墙。原因碰撞检测用的 Bresenham 算法只检查线段经过的栅格中心点当障碍物是斜对角相邻的两个栅格时线段可能从两个栅格的角之间穿过栅格中心点检测不到。解决工程上最省事的办法是给障碍物做膨胀处理也就是在原始地图上把每个障碍物栅格周围一圈都标记为障碍。膨胀半径取 1 到 2 个栅格既避免斜穿又给路径留出安全余量。要注意膨胀会缩小可行空间狭窄通道可能会被堵死所以膨胀半径要小于通道宽度的一半。5.4 地图边界坐标和矩阵索引搞混现象程序在特定地图上运行时报错“索引超出矩阵维度”或者起点明明在地图内却提示路径规划失败。原因MATLAB 矩阵是先行后列map(row, col)而坐标习惯是 (x, y)。如果写成 map(x, y)当地图不是正方形时就会出问题——x 的范围可能超过行数。还有 round 的位置不对浮点坐标直接索引也会越界。解决约定所有输入输出都用 (x, y) 坐标在进入碰撞检测函数时统一转换成 (row, col)即 rowy、colx。函数内部全部用 (row, col)对外全部用 (x, y)接口处做一层转换。这个约定要在代码注释里写清楚否则过两周自己回来看也会忘。5.5 障碍物太密导致树“无处下脚”现象地图上障碍物很多但并非完全堵死理论上存在可行路径但规划器反复失败。原因stepSize 太大扩展一步的距离超过了可行通道的宽度每次扩展都踩到障碍物上。另一种情况是连接阈值太大连接点虽然“接通”了但中间那段直线路径穿过了障碍物——因为连接判定只看两点距离不检查连接线段是否与障碍物碰撞。解决先把 stepSize 缩小到通道宽度的三分之一以下再试。同时要在连接判定处补一个检查当两树节点距离小于 connectDist 时调用 checkCollision 确认这两个节点之间的连线没有碰撞。这个检查代价很小但能杜绝大量“假连接”。6. 进阶技巧路径剪枝与平滑让 RRT-Connect 的输出真正可用原始 RRT-Connect 输出的路径是一串密集的折线节点直接交给机器人执行会有两个问题路径长度明显大于最优解折角处速度规划困难因为曲率不连续。我一般会做两步后处理先剪枝再平滑。剪枝的思路是遍历路径点尝试把不相邻的点直接连线如果连线不碰障碍物就跳过中间所有点。这个操作能把路径点从几十个压缩到三五个同时显著缩短路径长度。剪枝代码的核心是一个双层循环。外层固定一个起点索引 i内层从路径末尾往前找第一个能与 i 直接相连的点 j找到后把 i 跳到 j继续下一轮。这样得到的路径是“尽可能少点”的近似最短路径虽然不保证全局最优但已经非常接近。注意内层循环要从后往前扫否则可能陷入局部贪心导致剪枝不彻底。平滑更适合用 B 样条或简单的三次样条插值。MATLAB 里用 spcrv 函数可以快速生成 B 样条曲线但要注意样条曲线会“切角”也就是可能把原本贴着障碍物走的路径拉向障碍物内部。所以平滑之后必须重新做一次碰撞检测把穿过障碍物的部分重新剪枝或调整。一个工程上可靠的流程是剪枝 → 平滑 → 碰撞检测 → 若有碰撞则对碰撞段局部重规划。这样既保留了 RRT-Connect 的全局探索能力又得到一条平滑可执行的路径。验证算法是否可靠不能只跑一次成功就算完。我会写一个蒙特卡洛脚本随机生成 50 张不同障碍物布局的地图每张地图跑 20 次统计成功率、平均路径长度和平均耗时。成功率低于 90% 就说明参数太激进或地图建模有问题路径长度方差过大则说明随机采样环节可能需要调整目标偏置概率。这套验证方法能帮你区分“这次运气好”和“这个算法配置真的稳定”也是我把 RRT-Connect 从课堂作业变成工程工具的关键一步。最后一个习惯所有参数和地图设置写死在脚本顶部用注释标明每个参数调整时的观察指标。这样下次遇到新地图十分钟内就能定位是哪个参数需要改而不是从头开始猜。这也是我在多个机器人项目里踩出来的教训——RRT-Connect 本身并不复杂复杂的是让它在各种地图条件下都稳定输出可用路径。希望这篇笔记能帮你少走这些弯路。本文还有配套的精品资源点击获取