简介面向机器人运动规划与路径规划研究者这份资源提供了RRTConnect算法的完整MATLAB实现。RRTConnect在经典RRT基础上采用双向搜索策略适合解决高维空间或复杂环境下两点间快速路径规划问题对自动化、机器人及相关专业学生开展课程设计或算法实验有直接帮助。压缩包内含7个文件主要由6个MATLAB脚本和1个用于坐标/环境数据的Excel文件组成包体仅13KB轻量易用。脚本模块覆盖随机采样、最近邻搜索、树扩展、碰撞检测及双向树连接等核心环节结构清晰便于阅读和二次开发。借助MATLAB的可视化能力运行代码可直观观察两棵搜索树逐步逼近并最终连通的过程有助于深化对采样型路径规划算法的理解。目前已有2600余人学习下载适合作为入门RRT系列算法的参考实现和实验基础。1. RRT-Connect 双树搜索这份 MATLAB 代码解决的是路径规划最后一公里做路径规划的人迟早会遇到这种事地图不大障碍不多普通 RRT 却要跑几千次迭代才找到一条路而换成 RRT-Connect 的 MATLAB 代码后同样的地图几百次迭代就出结果。RRT-Connect 的改进思路很直接——不再从起点单方向往终点长树而是从起点和终点各长一棵树两棵树互相朝对方扩展直到在中途接上。这份资源就是一套完整的 MATLAB 工程包含建图、双树扩展、碰撞检测、路径提取和可视化适合刚接触路径规划的人看原理也适合有地图和起终点、想快速把路径搜出来的实战开发者。它的代码量不大核心逻辑都拆成了独立函数跑通 Demo 之后改自己的场景并不难。2. 算法原理与代码结构从单树 RRT 到双向搜索的改进逻辑2.1 为什么选 RRT-Connect搜索效率与狭窄通道先讲清楚 RRT 和 RRT-Connect 的差别。经典 RRT 每次迭代只维护一棵树随机撒点在树上找最近节点沿采样点方向生长一个固定步长然后判断新节点是否接近目标。这个流程本身没问题但存在一个浪费树的大部分分支长在无关区域因为每次随机采样落在目标方向的概率太低尤其在走廊、门洞这类狭窄通道场景里单树要碰运气才能把分支探进通道。通道入口窄随机点基本不会落到通道内部树只能在通道口反复绕圈。RRT-Connect 把一棵树变成两棵一棵以起点为根一棵以终点为根。每一轮先随机扩展其中一棵树另一棵树则主动向对方刚产生的新节点方向生长。因为后扩展的那棵树不是随机撒点而是“瞄准”对方的当前节点所以两棵树会像两拨人相向挖隧道一样各自从两端往里推进。这个改动带来的提升不是常数级的在窄通道场景下往往能把迭代次数降一个数量级。从算法的理论基础看RRT-Connect 保留 RRT 的概率完备性也就是只要迭代次数足够多、路径存在找到路径的概率趋于 1。但它不保证路径最优这一点和 RRT* 不同。RRT* 每插入一个节点都要做邻居重连计算量大不少RRT-Connect 追求的是“先用尽量少的扩展找到一条可行路径”所以它更适合做前端的快速搜索层找到路径后再用剪枝和插值做后处理。这份 MATLAB 代码的定位就是这样它解决的是“尽快找到可用路径”不是“找到最短路径”。在实际工程里我一般会把 RRT-Connect 当作初始路径生成器先用它快速得到一条无碰撞路径再做路径剪枝和轨迹光滑而不是指望它直接输出能发给底盘执行的轨迹。理解了这一点再看代码就不会困惑——为什么没有代价函数、为什么节点只有坐标和父节点索引因为数据结构是为连接服务不是为了最优。2.2 代码文件划分与整体流程这类 RRT-Connect 的 MATLAB 工程文件职责通常分得比较清楚。常见的划分方式是下面这样文件作用main_rrt_connect.m入口脚本负责建图、设置起终点、循环调用扩展函数rrt_connect_grow.m单次扩展函数向目标点生长一个步长并做碰撞检测find_nearest.m在树中找到离目标点最近的节点check_collision.m检测两点连线是否与障碍物相交build_map.m生成地图矩阵0 表示空闲、1 表示障碍extract_path.m从两棵树的连接点回溯并拼接完整路径plot_result.m绘制地图、两棵树和最终路径不同版本的代码会有些出入但核心结构基本一致。读代码时不要一上来就啃 main 脚本的细节先搞清楚数据结构。这份资源里每个树节点都是一个结构体包含 x、y、parent 三个字段。parent 存的是父节点在节点数组里的下标这个设计很关键因为从任意节点顺着 parent 往前回溯都能回到根节点回溯出来的序列就是一条路径。主循环的骨架可以抽象成下面这段伪代码本质上就是代码包里循环部分的逻辑% RRT-Connect 主循环T_a 从起点生长T_b 从终点生长 for i 1:max_iterations % 1. 在地图空闲区域随机采样扩展 A 树 q_rand sample_free_point(map); q_new_a rrt_connect_grow(T_a, q_rand, step_size, map); % 2. 如果 A 树扩展成功让 B 树向新节点生长 if ~isempty(q_new_a) q_new_b rrt_connect_grow(T_b, q_new_a, step_size, map); % 3. 判断两树是否足够接近 if ~isempty(q_new_b) norm(q_new_a.pos - q_new_b.pos) goal_tolerance path extract_path(T_a, q_new_a, T_b, q_new_b); break; end end % 4. 交换两棵树下一轮从 B 树开始 [T_a, T_b] deal(T_b, T_a); end注意第 4 步的交换这是 RRT-Connect 最容易漏掉的细节。如果不交换每次都是 A 树随机扩展、B 树跟随B 树本身不产生新的探索方向双树的优势会退化成单树。交换之后两棵树轮流担任“随机扩展方”和“跟随方”探索范围更均衡。主循环里还有个隐式的终止条件当两棵树的最近距离小于 goal_tolerance 时认为连接成功。这个阈值不能太大也不能太小太大会把两段明明还有空隙的树强行拼起来太小则要迭代很久才能凑巧接上。后面避坑章会专门展开。2.3 核心函数说明rrt_connect_grow 是这份代码里最核心的扩展函数作用可以概括成一句话给定一棵树和一个目标点从树上找最近节点沿着目标方向走一个步长如果这段路没撞墙就把新节点挂到树上。代码通常长这样function q_new rrt_connect_grow(T, q_target, step_size, map, mode) % 找到树上离目标点最近的节点 q_near find_nearest(T, q_target); % 计算方向向量归一化后乘以步长 delta q_target.pos - q_near.pos; dist norm(delta); if dist step_size q_new.pos q_target.pos; % 目标点足够近直接落点 else q_new.pos q_near.pos step_size * delta / dist; end q_new.parent q_near.id; % 两点连线必须做碰撞检测碰墙就放弃 if check_collision(q_near.pos, q_new.pos, map) q_new []; end end这里最容易被新手跳过的是 dist step_size 这个分支。如果目标点距离当前节点不到一个步长还强行按 step_size 走新节点会越过目标点后续连接判断可能一直失败。正确做法是目标点足够近时直接落点这样两棵树才能精确靠近。另一个值得注意的点是碰撞检测发生在扩展前后而不是扩展之后独立检查。因为 RRT 的每一步都决定了树的结构任何一步碰墙都必须丢弃不能把撞墙节点挂在树上否则路径回溯时会串进障碍区。3. 运行与参数配置把 Demo 跑起来并改造成自己的地图3.1 环境配置与最小运行示例这份 MATLAB 代码不需要额外工具箱MATLAB R2016b 之后的版本都能直接跑用到的都是 rand、norm、plot、meshgrid 这类内置函数。拿到代码包后先把目录加入 MATLAB 路径然后在命令窗口运行cd(本地路径/rrt_connect_matlab); main_rrt_connect正常情况下会等一两秒然后弹出一张地图和两棵树生长的动画。如果动画刷得太快看不清扩展过程可以在 main 脚本里给循环加一行 pause(0.02)能看到两棵树从起终点同时往中间蔓延最后在某个位置接上。运行成功的标志是命令窗口打印一行连接信息。不同版本打印内容不一样但通常包含如下信息Connect! iterations 342, path length 48.7如果代码包没有打印可以在连接成功分支手动加一行 fprintf调试时很有用。没有看到连接信息基本可以判断是碰撞检测或扩展逻辑出了问题往下看避坑章节就能定位。跑 Dem o之前先花三十秒检查 MATLAB 当前路径下有没有同名脚本。以前遇到过文件夹里同时存在多个版本的 find_nearest.mMATLAB 优先调用路径顺序靠前的那个导致一直用的是旧版碰撞检测路径穿墙怎么调都修不好。最简单的办法是运行前输入 which find_nearest确认调用的确实是你这份代码包里的文件。3.2 参数表代码包在 main_rrt_connect.m 开头通常会集中放四个可调参数下面这份参数表是我整理出来的默认值和调参思路参数默认值作用调参建议max_iterations2000迭代上限防止找不到路径时无限循环地图越大上限越高step_size5每次扩展的步长单位是网格数太小收敛慢太大容易碰墙goal_tolerance3两棵树连接的距离阈值建议取 step_size 的 0.50.8 倍map_size[100, 100]地图网格尺寸缩放地图后需要同步修改step_size 和 goal_tolerance 的搭配是调参时最值得花时间的。如果容差设得跟步长一样大两棵树可能在相距一个步长时就“远程握手”拼出来的路径折角明显后续剪枝的工作量也大。容差太小两棵树必须精确顶到对方节点才肯停迭代次数会成倍上涨。我一般在陌生地图上先跑一次默认参数观察节点数增长速率再决定步长往哪个方向调。还有一个参数容易被忽略随机数种子。很多版本在 main 开头用 rng(shuffle)每次运行路径都不同。教学演示这样没问题但如果要做实验对比、验证改进效果一定要固定种子rng(42); % 固定随机种子保证结果可复现固定之后同一组参数每次跑出的路径完全一致调参才能形成对照。如果不固定你今天调好的参数明天可能又不行因为两次运行根本不在同一随机序列上。3.3 替换地图的步骤把 Demo 地图换成自己的环境是这份资源最常被问到的需求。常见做法是在 build_map.m 里用矩阵直接定义地图或者读一张图片转成栅格地图% 读入黑白图片白色为空闲、黑色为障碍 img imread(my_map.png); map img(:,:,1) 128; % 转成逻辑矩阵true 表示障碍 map double(map); % 转成 double和已有代码类型保持一致替换之后必须做三件事。第一确认起点和终点坐标在矩阵索引范围内且落在空闲区域。第二给地图外包一圈障碍防止机器人跑出边界map_padded ones(size(map) 2); map_padded(2:end-1, 2:end-1) map;第三检查采样函数是否把随机点限制在 2 到 size-1 的范围内。很多版本写的是 rand * map_size_x加边界后这个采样范围会包含最外圈的障碍行采样到墙上的概率变大扩展效率直线下降。正确写法是function q sample_free_point(map) [h, w] size(map); while true q [rand * (w - 2) 2, rand * (h - 2) 2]; if map(round(q(2)), round(q(1))) 0 break; end end end这套逻辑强制限制采样范围并在遇到障碍点时重新采样比单纯乘 map_size 可靠得多。换地图后先通过 plot 把地图和起终点画出来确认一遍再跑搜索能避免一大批低级错误。4. 关键实现细节与可视化输出拆解几个值得注意的函数4.1 最近节点搜索RRT 系列算法里find_nearest 决定每一步扩展的“方向锚点”也往往是性能瓶颈。节点少时无感节点上到几千、迭代次数上万后线性扫描的距离计算会占掉整个算法大半时间。最简单直观的写法是 for 循环逐节点算距离function q_near find_nearest(T, q_target) min_dist inf; q_near T(1); for i 1:numel(T) d norm([T(i).x - q_target(1), T(i).y - q_target(2)]); if d min_dist min_dist d; q_near T(i); end end end这份代码在教程版里最常见优点是逻辑一目了然缺点是大规模节点时慢。想提速可以用向量化写法把 x 和 y 抽成数组一次性算出所有距离平方function idx find_nearest_vec(T, q_target) x_arr [T.x]; y_arr [T.y]; dist2 (x_arr - q_target(1)).^2 (y_arr - q_target(2)).^2; [~, idx] min(dist2); end这里用平方距离代替欧氏距离开根号不影响最小值的位置但能省掉逐元素 norm 的开销。节点数超一万时向量化版本比 for 循环快三到五倍代价是代码可读性差一些适合对实时性有要求的场景。如果节点规模进一步膨胀比如超过十万可以考虑用 kd-tree 加速最近邻搜索但那是另一个工程量级的优化。对这份 MATLAB 代码的目标场景——教学 Demo 和中小地图路径规划——向量化版本已经够用没必要为了性能把代码复杂度抬上去。从工程角度说先跑通、再剖析热点、最后优化顺序不能反。4.2 碰撞检测碰撞检测的正确性决定路径能不能用但它的实现往往是最粗糙的。多数代码采用线段采样法把两点连线按小间隔取中间点逐个判断是否落在障碍上。采样间隔要和地图分辨率匹配太粗会漏掉细障碍太细计算量加倍。function is_collision check_collision(q1, q2, map) is_collision false; dist norm(q2 - q1); n_steps ceil(dist / 0.5); % 每 0.5 网格采样一次 for k 1:n_steps t k / n_steps; p round((1 - t) * q1 t * q2); if map(p(2), p(1)) 1 % 注意矩阵索引先 y 后 x is_collision true; return; end end end这里的两个坑值得单独提。第一round 之后的点可能恰好落在障碍边缘视觉上贴墙但程序认为碰撞导致树在通道口反复扩展失败。第二矩阵索引顺序。MATLAB 中 map(p(2), p(1)) 表示第 y 行第 x 列如果代码里写成 map(p(1), p(2))非对称的地图会莫名出现路径穿墙。排查方法很简单把地图换成对称形状看碰撞是否消失如果消失基本就是索引序反了。更稳妥的做法是用 Bresenham 直线算法替代均匀采样。Bresenham 沿像素栅格逐点遍历不会漏障碍计算量也稳定。不过这份代码包通常用均匀采样因为实现短、读起来直观。改自己的地图时只要保持采样间隔不大于地图最小障碍宽度的一半均匀采样完全够用。4.3 双树连接条件RRT-Connect 的收尾逻辑和单树 RRT 不同单树看新节点是否接近目标点双树看两棵树的节点是否足够接近。连接点的选取直接影响最终路径质量。常见实现是这样的B 树扩展完成后判断 q_new_b 和 q_new_a 的距离是否小于阈值如果小于就从这两个节点分别回溯到各自的根% 回溯 A 树路径从 q_new_a 到起点 path_a trace_back(T_a, q_new_a); % 回溯 B 树路径从 q_new_b 到终点 path_b trace_back(T_b, q_new_b); % 拼接时 A 树路径要翻转B 树路径去掉重复的连接点 path [flipud(path_a); path_b(2:end, :)];trace_back 的实现依赖 parent 索引一路往上找根节点。拼接方向是这个环节最经典的错误以起点为根的树从新节点回溯得到的是从新节点到起点的顺序直接拼接会把路径切成一段反向段机器人先走回起点再折向终点。所以 A 树路径必须 flipud。拼接完成之后还要再对整条路径做一次完整性检查。原因在于两棵树各自扩展时只验证了单步无碰撞但 q_new_a 到 q_new_b 的连线可能穿过障碍物特别是当 goal_tolerance 较大时这段空隙足够让一条细长障碍穿过去。不少穿墙路径就是这么产生的——连接时只判距离不判线段碰撞。另一个细节是回溯时每棵树的根节点类型。起点树的根是起点终点树的根是终点。如果代码里用同一个结构体数组存储两棵树回溯前必须确定当前节点属于哪棵树否则 parent 索引会串树。这也是为什么很多实现把 T_a 和 T_b 分开存储而不是合并成一个数组。5. 常见问题与避坑跑通 RRT-Connect 时的 5 个坑5.1 坑1程序长时间运行不返回现象主脚本卡住命令窗口光标一直转迭代很久也没有打印连接信息。原因最常见的是 max_iterations 设置过大而当前地图根本没有可达路径两棵树再长也碰不到其次是 step_size 太大每次扩展都碰撞失败树的节点数几乎没有增长却还在空转。解决先加一个每 200 次迭代打印一次节点数的临时代码确认两棵树的节点数是否在增长。如果完全不增长把 step_size 调小或先把地图换成最简单的空旷场景。如果节点数正常增长但始终不连接检查两步——一是 goal_tolerance 和 step_size 的比是否太小二是两棵树交换逻辑是否每轮都执行。这个方法能覆盖九成“跑不出来”的情况。5.2 坑2最终路径明显穿墙现象路径画出来后有一段斜线穿过障碍物肉眼可见机器人不可能这么走。原因碰撞检测的采样间隔太稀疏细长障碍在两点之间被跳过去了或者连接两棵树时只检查了距离没有检查 q_new_a 到 q_new_b 连线的碰撞。 这是最常见的两类原因占比大概各半。解决把 check_collision 里的采样间隔从 1 改成 0.5 或更小然后重新跑一次。如果还穿墙在拼接路径前对连接线段单独调用 check_collision发现碰撞就强制不连接、继续迭代。这一步改完穿墙问题基本消失。如果仍然存在大概率是矩阵索引顺序写反用对称地图验证即可。5.3 坑3可视化里树突然跳变现象播放生长过程时某一帧树的一部分突然出现在远处看起来像瞬移。原因主循环执行了 [T_a, T_b] deal(T_b, T_a)但绘图函数里还固定引用 T_a、T_b 这两个变量名导致画出来的树和实际扩展的树对不上。树本身没有瞬移是图上的标签张冠李戴了。解决交换后立即重新给绘图变量赋值不要在绘图函数里直接写死原始变量名。一个干净的写法是在交换后调用 plot_result(T_a, T_b)函数内部按传入参数画图这样无论怎么交换图都跟随当前树的状态。5.4 坑4步长太大两棵树始终接不上现象迭代次数耗尽也没输出 Connect但两棵树节点很多树之间总隔着一小段空隙。原因goal_tolerance 设置得比 step_size 小导致两棵树各自扩展时都跳过了对方永远差最后一步。另一种可能是在“B 树向 A 树新节点方向生长”这一步写成了随机扩展失去了双向搜索的意义两棵树可能平行生长很久都碰不到。解决把 goal_tolerance 先调到 step_size 的 0.8 倍确认能连接后再逐步收紧。同时检查 rrt_connect_grow 调用时传入的是 q_new_a 还是 q_rand。如果是 q_randB 树就和 A 树各长各的连接概率大幅下降。这个坑常常在代码重构后出现属于改了逻辑忘了改调用参数。5.5 坑5采样点落到边界外导致索引越界现象运行到中途报错“下标超出矩阵维度”错误信息指向 check_collision 或采样函数。原因随机采样直接用 rand * map_size_x而 MATLAB 矩阵索引从 1 开始采样点可能出现 0 或超过行列数的情况。其实不是随机数的问题是采样范围没有和地图尺寸对齐。解决在采样函数里强制夹取范围采样点限制在 2 到 size-1 之间q_rand [rand * (w - 3) 2, rand * (h - 3) 2];并加一层障碍点重采样逻辑。把这段封装成 sample_free_point 函数后主循环里只调用它后续无论换什么地图只要地图矩阵尺寸能读出来采样范围就不会出错。边界问题本质是“地图坐标”和“矩阵索引”两套体系的换算统一在一处处理才能避免到处修。6. 让路径更平滑路径后处理与时间参数化6.1 路径剪枝RRT-Connect 出来的路径由很多短线段组成能走但谈不上好走。最简单的后处理是剪枝从起点开始不断找路径上最远的、且与当前点连线无碰撞的点跳过中间冗余节点。function path_pruned prune_path(path, map) path_pruned path(1, :); cur 1; while cur size(path, 1) for j size(path, 1):-1:cur1 if ~check_collision(path(cur, :), path(j, :), map) path_pruned [path_pruned; path(j, :)]; cur j; break; end end end end剪枝的旋转方向很关键必须从远到近找最远可达点。如果从近到远只能找到第一个转弯点附近的局部短路径剪完路径仍然很碎。贪心剪枝不保证全局最优但对 RRT 系路径效果显著通常能缩短 30% 以上而且代码只有十几行性价比很高。6.2 时间参数化剪枝后的路径要实际执行还得加上时间信息。最简单的方式是匀速插值按路径总长把每个点映射到时间轴total_len sum(sqrt(sum(diff(path_pruned).^2, 2))); t cumsum([0; sqrt(sum(diff(path_pruned).^2, 2))]) / total_len * total_time;这段代码把路径点转换成从 0 到 total_time 的累计时间戳。但匀速轨迹在折角处会产生很大的加速度跳变移动底盘或机械臂执行时会抖。我一般会再加一层梯形速度规划按距离计算加速段、匀速段、减速段再反查路径点位置这样轨迹执行起来才平顺。这套 RRT-Connect 的 MATLAB 代码跑顺之后我习惯把每一步改动都做一次固定种子的复跑确保路径变化是参数或算法改进带来的而不是随机数波动的结果。从那以后我每次拿到新的路径规划代码都强制先跑一遍原始 Demo、固定随机种子、再动参数这个顺序帮我省掉了大量“昨天还能跑今天就穿墙”的排错时间。希望帮到你。本文还有配套的精品资源点击获取