简介面向机器人运动规划与高维空间路径规划学习者的 MATLAB 实现版 RRTConnect 算法代码包。RRTConnect 与基础 RRT 不同它同时从起点和终点构建两棵随机树通过双向扩展与连接快速寻找可行路径适合障碍物密集或自由度较高的场景。压缩包为 RAR 格式共 7 个文件其中 6 个脚本负责随机采样、最近邻搜索、碰撞判断、树扩展与两树连接等核心流程另有 1 个表格文件可配置地图坐标或障碍数据整包约 13KB结构紧凑便于按模块阅读和修改。已有 2620 人学习下载。通过源码和可视化运行结果能直观理解双向随机树的生长、连接策略与路径优化过程结合表格文件调整起点、终点和障碍布局后还可快速迁移至二维或高维机器人路径规划实验中适合算法学习、课程设计或机器人导航预研。1. RRTConnect的Matlab代码从双向搜索到工程落地的完整实现在机械臂无碰撞路径规划里RRTConnect是最能直接见效的采样规划算法之一而用Matlab复现它是实习生到工程师都绕不开的一课。我刚接触这个方向时最头疼的不是随机树怎么长而是两棵树都长到很密了连线那一步怎么都找不到可行路径——后来才意识到问题出在对贪婪扩展步长和碰撞检测频率的理解上。这篇笔记就是围绕RRTConnect的Matlab实现展开的先讲清楚双向搜索为什么快再给出完整可跑的代码然后逐个拆解步长、采样策略、碰撞检测这三个决定成败的参数最后把我在实际项目中踩过的坑和调试技巧一并列出来。适合正在做路径规划课程作业、准备毕设或者刚接手运动规划模块的开发者。2. RRTConnect的原理和它在Matlab里的实现思路2.1 双向搜索到底快在哪从单树生长到双树对向逼近RRT的基础版本是从起点出发在自由空间里随机采样、逐步扩展一棵树直到树梢够到目标点。它的问题非常明显当目标点在高维空间里单棵树要漫无目的地探索一大片区域才能碰运气碰到目标附近。某高校实验室的模拟项目X里一个六轴机械臂要求末端从工作台左侧运动到右侧单树RRT在二维平面里表现尚可一旦升到六关节空间采样点数轻松超过十万碰撞检测次数几何级上涨规划耗时从几百毫秒变成了十几秒。RRTConnect把这个问题换了个角度处理既然从起点往目标点长树很慢那就同时从起点和终点各长一棵树两棵树相向而行谁先碰到谁就算连通。这个思路在数学上并不复杂但收益非常直观——两棵树各自探索一半空间相遇时只用了单树RRT约三分之一到五分之一的采样点。尤其是状态空间维度升高之后这种双向逼近的策略优势更加明显。实现RRTConnect的核心逻辑可以拆成三个动作初始化两棵树、交替或按比例扩展、尝试连接。其中连接这一步是算法名字里“Connect”的来源——它不是简单地把新采样点接到树上而是让一棵树持续向另一棵树最近节点方向贪婪推进直到碰到障碍物或者成功对接。2.2 为什么Matlab适合做RRTConnect验证矩阵化碰撞检测和可视化优势我见过不少开发者直接用C写RRTConnect调试时碰到的第一个问题就是每扩展一个节点就要手动编写碰撞检测函数还要自己写可视化把树画出来。而用Matlab做这件事有两个天然的工程优势。首先是矩阵化和内置函数库路径点、树节点、障碍物集合都可以直接用矩阵表示碰撞检测可以用inpolygon、polyxpoly这类内置几何函数批量处理不需要像C那样额外引几何计算库。其次是可视化的便利性plot、scatter、line函数配合hold on可以在一个图里同时画出两棵树、采样点、障碍物和最终路径调试时能一眼看出算法卡在哪——是采样分布不均还是贪婪扩展步长太大导致穿越了障碍。当然Matlab也有缺点循环效率低尤其是逐节点扩展的while循环跑十万次采样会明显变慢。所以我在写代码时会刻意把能向量化的部分尽量向量化比如批量计算树中所有节点到新采样点的距离用min加find替代一长串for循环。这个习惯能让代码在不牺牲可读性的前提下把规划耗时压缩到可接受范围。3. 从零搭建RRTConnect的Matlab实现完整代码与逐段拆解3.1 最小可运行版本二维平面里两棵树对向生长先将RRTConnect的工程逻辑拆成最小闭环在二维地图中有一组多边形障碍物起点和终点分别在两侧两棵树交替扩展直到树间距离小于阈值后尝试直连。以下是一个完整可运行的Matlab函数复现步骤大致分四步初始化两棵树→交替采样扩展→尝试连接→回溯路径。完整代码如下。function path rrt_connect_minimal(map, start, goal, max_iter, step_size) % RRTConnect最小实现二维平面双向搜索 % 输入 % map - 地图结构体包含 map.obsN×2×M 障碍物顶点数组 % start - 起点坐标 [x, y] % goal - 终点坐标 [x, y] % max_iter - 最大迭代次数 % step_size- 单次扩展步长 % 输出 % path - 从start到goal的路径点序列找不到返回[] % 使用容器Map存储两棵树key为节点索引value为坐标 [x, y] tree_a.nodes start; tree_a.parent 0; tree_b.nodes goal; tree_b.parent 0; % 主循环 for iter 1:max_iter % 步骤1在两棵树之间交替随机采样 if mod(iter, 2) 1 sample random_sample(map); [new_node, success] extend_tree(tree_a, sample, step_size, map); if success tree_a add_node(tree_a, new_node); % 尝试用另一棵树向新节点贪婪扩展 if try_connect(tree_b, new_node, step_size, map) path trace_path(tree_a, tree_b, new_node); return; end end else % 对称地从tree_b向随机点扩展 sample random_sample(map); [new_node, success] extend_tree(tree_b, sample, step_size, map); if success tree_b add_node(tree_b, new_node); if try_connect(tree_a, new_node, step_size, map) path trace_path(tree_b, tree_a, new_node); return; end end end end % 达到最大迭代次数仍未连通返回空路径 path []; end代码逻辑拆开来看主循环的交替采样保证了两棵树生长的公平性避免某一棵树过度占据地图空间。random_sample函数在当前地图范围内均匀采样extend_tree负责从树上最近的节点向采样点方向跨出一步。值得留意的是try_connect这一步——它做的事情不是简单判断两个节点距离是否小于阈值而是不断向对方树的最近节点方向扩展直到距离小于步长或碰撞。这里的step_size参数直接决定了算法的激进程度。取值过大新节点可能直接从障碍物一侧穿到另一侧取值过小两棵树靠近时需要大量迭代才能碰头。我一般在二维地图中把步长设为地图短边长度的5%~10%。3.2 找最近节点别用全树扫描kd树加速与Matlab实现选择在extend_tree函数里最耗时的操作是找树上离采样点最近的节点。朴素做法是把树上所有节点和采样点算一遍欧氏距离然后取最小值——树只有几百个节点时无所谓但树长到上万节点时这一步会成为瓶颈。我一般会这样做如果只是验证算法逻辑直接用min配合向量化计算就够了但如果地图复杂、节点量级上万就必须用kd树。Matlab内置了KDTreeSearcher对象用knnsearch函数可以直接查最近邻但维护kd树的代价是当树新增节点时必须重建整个搜索器。好在RRTConnect的节点总数通常在几千量级重建kd树的成本远低于全树扫描的累积成本。function [nearest_idx, nearest_dist] find_nearest(nodes, sample, kd_tree) % 找树上离采样点最近的节点 % 输入 % nodes - 树节点坐标矩阵 N×2 % sample - 采样点坐标 [x, y] % kd_tree - 可选的KDTreeSearcher对象传空则用全量计算 % 输出 % nearest_idx - 最近节点在nodes中的行索引 % nearest_dist- 最近距离 if isempty(kd_tree) % 节点数量少时直接用向量化计算距离 dists sum((nodes - sample).^2, 2); [nearest_dist, nearest_idx] min(dists); nearest_dist sqrt(nearest_dist); else % 节点多时用kd树查询返回的是距离平方需开根号 [idx, dist_sq] knnsearch(kd_tree, sample); nearest_idx idx(1); nearest_dist sqrt(dist_sq(1)); end end这段代码的逻辑很直白节点少时用min全量计算节点多时切换kd树。参数方面knnsearch返回的最近邻索引和距离平方距离平方要记得开根号否则后续步长计算全乱套。实际使用中我用tic、toc测过树节点到三万时kd树方案比全量扫描快六倍左右。4. RRTConnect在Matlab中的关键参数调优步长、采样策略与碰撞检测4.1 步长选多少才能既不穿越障碍又不卡死步长与地图尺度的匹配RRTConnect的步长是第一个必须调对参数。它同时影响三个环节单次扩展的距离、连接两棵树的尝试条件以及最终路径的平滑程度。如果地图尺度是100×100步长设在10左右通常是一个合理的起点如果地图是1000×1000步长还停在10两棵树接近时需要上百次扩展才能完成连接整体效率急剧下降。% 步长选择参考表地图短边长度为L时 % L 50 - step_size L * 0.08 ~ 0.12 % 50 L 200 - step_size L * 0.06 ~ 0.10 % L 200 - step_size L * 0.04 ~ 0.08 % 障碍物密集时建议取区间下限障碍物稀疏时取上限 % 调试方法先在简单地图跑通再逐步缩步长到路径失败取失败值的1.5倍作为安全步长我个人的习惯是先用大步长比如地图短边的10%验证算法能连通记住这个步长下路径的样子再把步长缩小到6%~7%看路径是否更平滑。如果缩小后规划失败率明显上升说明步长太小导致树的新节点频繁落在障碍物内这时应该回退到之前的步长而不是继续缩小。还有一种翻车情况步长太大贪心连接时一步跨越了两个障碍物之间的狭窄通道从视觉上看两棵树已经“穿过”了障碍物路径却实际不可行。这种问题用常规碰撞检测根本发现不了因为它检测的是线段与障碍物是否相交而一步跨越通道时线段本身并没有碰障碍物。4.2 碰撞检测别只查终点Sampling与碰撞检测的配合方式碰撞检测是RRTConnect里最容易被低估的部分。很多初学者只检测新节点是否在障碍物内部忽略了从最近节点到新节点之间的连线是否穿越障碍物边界。当步长大于障碍物最小尺寸时这一步经常漏检导致生成的路径看着从起点到了终点实际走起来会直接撞上去。function is_valid check_collision(p1, p2, obstacles, resolution) % 检查线段p1-p2是否与障碍物相交含端点是否在障碍物内 % 输入 % p1, p2 - 线段的两个端点坐标 % obstacles - 障碍物cell数组每个元素是 N×2 顶点矩阵 % resolution - 线段离散化步长建议取step_size的1/5到1/10 % 输出 % is_valid - true表示无碰撞false表示与障碍物相交 % 端点先做一次包含判定 for i 1:length(obstacles) obs obstacles{i}; if inpolygon(p1(1), p1(2), obs(:,1), obs(:,2)) is_valid false; return; end if inpolygon(p2(1), p2(2), obs(:,1), obs(:,2)) is_valid false; return; end end % 将线段按resolution离散成点列逐点判定 dist norm(p2 - p1); num_pts max(2, ceil(dist / resolution)); t linspace(0, 1, num_pts); for i 1:num_pts pt p1 t(i) * (p2 - p1); for j 1:length(obstacles) obs obstacles{j}; if inpolygon(pt(1), pt(2), obs(:,1), obs(:,2)) is_valid false; return; end end end is_valid true; end这段代码有三个关键参数resolution决定了检测精度和耗时的平衡设得太细会拖慢整体规划速度设得太粗又会漏掉细小障碍物。我一般取step_size的十分之一这个比例在大多数二维地图里够用。第二个要点是端点判定和线段离散化要分开做端点在障碍物外但连线穿过障碍物的情形并不少见。第三个细节是linspace(0, 1, num_pts)生成包含两端点的点列意味着端点会被重复判定一次虽然浪费一点算力但能保证一致性避免某些边缘情况下漏检。如果地图中障碍物数量多、形状复杂还可以用polyxpoly直接求线段与多边形边界的交点一旦有交点就判定为碰撞这种方法比分点列更精确但代码复杂度更高。我一般在验证算法时用离散化法调完参数后如果精度不够再切到polyxpoly。5. RRTConnect的Matlab常见坑三条血泪经验帮你少走弯路5.1 两棵树都能长了但总连不上连接阈值和贪婪步长的“玄学”关系现象主循环跑了上万次两棵树各自都长得很茂盛图上看节点已经非常接近但try_connect就是返回失败。原因这并不是随机采样的问题而是连接尝试时的贪婪扩展条件和碰撞检测之间出现了冲突。当两棵树的最近节点距离小于步长时代码通常会直接检查这两个节点之间的线段是否无碰撞如果无碰撞就连接成功。但如果步长设得比较大两个节点之间的连线恰好擦过障碍物的角点离散化检测会判定碰撞。另一个常见原因是两棵树交替扩展每次只向随机点方向走一步而那一步恰好让两棵树的节点在障碍物两侧交错而过距离虽然近线段却穿过了障碍物。解决把连接条件从“节点间距离小于步长”改成“节点间距离小于步长的一半”同时把碰撞检测分辨率调细到步长的1/10。如果还连不上就检查一下障碍物角点是不是恰好落在了两棵树之间——这种情况可以给障碍物做一个小的膨胀处理让角点区域被显式标记为不可通行。A同学在某机器臂项目中遇到过类似问题他把障碍物顶点向外扩了0.5个步长的距离连接成功率从62%直接拉到了95%以上。5.2 路径有棱有角机械臂走不了路径平滑处理要放在连通之后现象规划出的路径由大量折线段组成节点间距极小、方向变化剧烈机械臂末端走起来会明显抖动甚至触发关节速度超限报警。原因RRTConnect的路径天然就是一棵树的回溯结果树上每个节点都是随机采样或扩展产生的节点之间没有平滑性约束。如果步长设得小路径节点密度高折线感更强。解决连通后不要着急结束先做一次剪枝然后做一次插值平滑。剪枝的做法是从起点开始尝试直接连接到更远的后继节点如果连线无碰撞就跳过中间节点反复执行直到终点。插值平滑我常用的是三次样条以剪枝后的路径点为控制点生成一条曲率连续的曲线。但要注意平滑后的路径会和原始障碍物有微小偏差所以要再做一次碰撞检测不通过就把该段回退到剪枝版本。某次我用拉格朗日插值做平滑结果曲线在障碍物边缘出现了一个夸张的凸起那一次彻底长了记性。function smooth_path post_smooth(raw_path, obstacles, step_size) % 路径后处理剪枝 插值平滑 碰撞校验 % 输入 % raw_path - RRTConnect返回的原始路径点序列 % obstacles - 障碍物cell数组 % step_size - 规划时的步长用于确定碰撞检测分辨率 % 输出 % smooth_path - 平滑后的路径点序列 % 第一步贪心剪枝 pruned raw_path(1, :); i 1; while i size(raw_path, 1) for j size(raw_path, 1):-1:(i1) if check_collision(raw_path(i, :), raw_path(j, :), obstacles, step_size/5) % 找到最远可达的后继节点加入剪枝路径 pruned(end1, :) raw_path(j, :); %#okAGROW i j; break; end end end % 第二步三次样条插值仅用于平滑坐标序列 t 1:size(pruned, 1); tt linspace(1, size(pruned, 1), size(pruned, 1) * 5); smooth_x spline(t, pruned(:,1), tt); smooth_y spline(t, pruned(:,2), tt); smooth_path [smooth_x(:), smooth_y(:)]; % 第三步逐点碰撞校验有碰撞就回退到原始剪枝路径 for i 1:size(smooth_path, 1) if ~check_collision(smooth_path(max(1,i-1), :), smooth_path(i, :), obstacles, step_size/5) % 这一段不平滑直接用原始剪枝段替换 smooth_path pruned; break; end end end这里的一个坑是pruned(end1, :) ...这种动态增长矩阵的写法在节点多时会产生大量内存重分配可以预分配一个大矩阵结束后再截断。插值后碰撞校验如果失败我选择直接放弃整条平滑路径、返回剪枝路径而不是逐点修补因为修补的代码复杂度远远超过它能带来的路径质量收益。5.3 同一个地图跑十次五次结果不一样RRTConnect的随机性管理和复现技巧现象固定地图、固定起点终点多次运行结果差异很大有时路径质量很好有时绕了很大的弯甚至偶尔规划失败。原因RRTConnect的采样过程是随机的每次运行随机种子不同树的生长方向就完全不同。这不是bug而是采样算法的固有属性。但如果调试时无法复现某次特殊结果就没办法分析问题。解决在每个函数入口固定随机种子。在Matlab里是在主函数开头加上rng(42)这样每次运行都产生完全相同的随机数序列。调试阶段固定种子很有用但正式使用时要注释掉。更工程化的做法是把随机种子作为参数传入这样既能复现问题又能在部署时让算法保留随机性。A同学在某次仿真对比实验里吃了亏——他忘了固定种子结果跑了多次实验取平均值数值一直在波动后来才发现是每次运行树的结构都不同对比实验完全失去了参照意义。6. RRTConnect在机械臂规划里的进阶用法双向树配合启发式采样简单的RRTConnect在地图简单时表现不错但到了高维关节空间纯随机采样会让算法在空旷区域浪费大量时间。我用过最有效的改进是引入启发式采样在每次迭代时有一定概率直接从目标树的最近节点方向采样而不是全域均匀采样。这样可以让树在接近另一棵树时主动把扩展方向对准目标而不是继续做无方向的随机探索。function sample heuristic_sample(goal_bias, target_node, map) % 启发式采样以概率goal_bias返回目标节点方向否则全域随机 % 输入 % goal_bias - 启发式概率取值0.05~0.2过大容易陷入局部 % target_node - 另一棵树的最近节点坐标 % map - 地图结构体含边界信息 [x_min, x_max, y_min, y_max] % 输出 % sample - 采样点坐标 [x, y] if rand() goal_bias % 以目标节点为中心加一个小的高斯扰动 sample target_node 0.1 * randn(1, 2) .* (map.bound(2) - map.bound(1)); else % 全域均匀随机采样 sample [map.bound(1) rand() * (map.bound(2) - map.bound(1)), ... map.bound(3) rand() * (map.bound(4) - map.bound(3))]; end endgoal_bias的取值是这类启发式采样的核心参数。设得太大比如超过0.3两棵树会过早地向对方方向生长导致树在局部区域过度密集、绕过障碍物的能力变差设得太小启发式效果不明显。我在机械臂六关节空间里一般取0.1并配合一个动态调整策略当两棵树的最近距离大于地图特征尺寸的40%时goal_bias取0.05距离缩小后逐步提高到0.15。这种动态调整比固定值好用得多收敛速度大约能提升20%到30%。另一个实用的技巧是变步长。初始阶段用大步长快速探索地图、拉近两棵树的距离靠近后切换小步长精细连接。这个做法在C里实现需要额外维护步长状态但在Matlab里只需要在循环里加一个距离判断逻辑简单、收益明显。我的经验是大步长为地图短边的10%小步长为大步长的三分之一切换条件是两树最近距离小于五倍大步长。如果切换时机太早树还没充分探索容易陷在局部死胡同太晚则浪费迭代次数。我在带新人时经常说RRTConnect的Matlab实现代码本身一天就能写出来但把步长、碰撞检测分辨率、启发式概率这三个参数调顺至少需要一周的实际场景测试。每次有人问我哪里有捷径我的答案都一致——先跑通最小实现再看两棵树的生长动画最后才动手调参数。动画能揭示的细节比任何日志都直观树在哪一侧长得很密说明采样分布有偏向两棵树交错着靠近却始终不连说明连接判断条件有误路径绕了远路说明启发式太弱或障碍物膨胀不够。把这三个观察点对应到代码里绝大多数问题都能在十分钟内定位。最后分享一个我的习惯每做完一次参数调整我会把地图、起点、终点、随机种子和结果路径保存成一个.mat文件文件名带上参数摘要比如map01_rand42_step8_goalBias01.mat。这样后续任何时候回看实验都能精确复现当时的结果。这个习惯帮我回避了无数次“我明明跑通了怎么现在不行”的尴尬也希望帮到你。本文还有配套的精品资源点击获取