RRT-Connect双树搜索MATLAB代码详解：从原理到实现

简介：面向机器人运动规划与路径规划研究者，这份资源提供了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 脚本的细节，先搞清楚数据结构。这份资源里每个树节点都是一个结构体，包含 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.m，MATLAB 优先调用路径顺序靠前的那个，导致一直用的是旧版碰撞检测，路径穿墙怎么调都修不好。最简单的办法是运行前输入 which find_nearest，确认调用的确实是你这份代码包里的文件。

3.2 参数表

代码包在 main_rrt_connect.m 开头通常会集中放四个可调参数，下面这份参数表是我整理出来的默认值和调参思路：

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_rand，B 树就和 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:cur+1 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、固定随机种子、再动参数，这个顺序帮我省掉了大量“昨天还能跑今天就穿墙”的排错时间。希望帮到你。

本文还有配套的精品资源，点击获取