简介：这是一份基于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(end+1, :) = newPt; tree.parents(end+1, :) = 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 栅格地图上比较稳的默认值：stepSize=3、connectDist=2、goalProb=0.1、maxIter=2000。这组参数不是最优的，但能保证大多数地图不失败，适合作为调参起点。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)，即 row=y、col=x。函数内部全部用 (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 本身并不复杂，复杂的是让它在各种地图条件下都稳定输出可用路径。希望这篇笔记能帮你少走这些弯路。本文还有配套的精品资源，点击获取
简介：这是一份基于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(end+1, :) = newPt; tree.parents(end+1, :) = 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 栅格地图上比较稳的默认值：stepSize=3、connectDist=2、goalProb=0.1、maxIter=2000。这组参数不是最优的，但能保证大多数地图不失败，适合作为调参起点。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)，即 row=y、col=x。函数内部全部用 (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 本身并不复杂，复杂的是让它在各种地图条件下都稳定输出可用路径。希望这篇笔记能帮你少走这些弯路。本文还有配套的精品资源，点击获取