简介这份资源面向机器人学、自动化控制方向的学生与工程师聚焦三维机械臂在固定障碍物约束下的路径规划问题提供基于RRT算法的工程实现思路与配套Matlab代码。内容围绕构型空间建模、随机采样、树状扩展与碰撞检测等关键环节展开帮助读者理解如何在高维空间中从起点构型搜索到目标构型的无碰撞路径并兼顾路径连续性与可行性。资源包共1个文件为html格式压缩后约11KB可直接在浏览器中查阅算法说明与代码要点。目前已有18人学习下载。对于希望快速上手RRT算法、理解机械臂避障规划流程的读者这份材料可作为入门参考配合Matlab环境复现采样、扩展与碰撞检测步骤并为进一步优化路径质量、尝试RRT*或RRT-connect等改进版本提供基础。1. 从构型空间说起为什么3D机械臂的路径规划不能直接套用二维A*做过二维移动机器人路径规划的人第一次把RRT搬到3D机械臂上大概率会翻车。原因不复杂移动机器人的位姿是平面上的(x, y, θ)而一台六自由度机械臂的“位置”是六个关节角拼成的向量它活在一个六维甚至更高维的构型空间Configuration Space里。你在笛卡尔空间里看到末端执行器从A点走到B点很直但映射回关节空间可能是某个关节要转两百多度、另外两个关节要同时反向补偿的一条诡异曲线。更麻烦的是笛卡尔空间里的一个“固定障碍物”映射到构型空间后不再是那个规规矩矩的方块而是一团形状未知、边界扭曲的禁区——这就是路径规划里常说的C-space障碍。RRTRapidly-exploring Random Tree快速扩展随机树之所以在3D机械臂场景里被反复拿出来用核心原因是它不需要显式构造C-space障碍的解析表达。它只在采样点上去问一句“这个关节构型碰不碰障碍”碰了就丢不碰就留。这个“黑匣子式”的碰撞检测接口让RRT天然适配机械臂这种正运动学容易算、逆运动学多解且C-space障碍难解析的对象。这篇笔记要讲清楚的就是在一个含固定障碍物的约束环境里怎么用RRT从起点构型找到一条到目标构型的无碰撞路径以及MATLAB代码里那些真正决定成败的参数和坑。适合已经懂基本运动学、想把这套东西跑通并调稳的从业者。2. 把机械臂和障碍物都建模成可计算的对象2.1 用改进DH参数建立机械臂运动学模型RRT本身不关心你是几自由度但碰撞检测必须知道每一根连杆在空间中的实际位置。常见做法是用改进DHModified Denavit-Hartenberg参数描述连杆然后逐帧算正运动学。下面这段代码建立一个六自由度机械臂的连杆模型并给出任意关节角向量对应的各连杆末端位姿。% 六自由度机械臂改进DH参数表 % 每行: [a(i-1), alpha(i-1), d(i), theta_offset(i)] dh [ 0, 0, 0.35, 0; 0.05, -pi/2, 0, 0; 0.42, 0, 0, 0; 0.04, -pi/2, 0.40, 0; 0, pi/2, 0, 0; 0, -pi/2, 0.08, 0]; function T_all forwardKinematics(dh, q) % 输入: dh参数表, 关节角向量q(1x6) % 输出: 每个连杆坐标系相对基座的4x4变换矩阵 n size(dh, 1); T_all cell(1, n); T eye(4); for i 1:n a dh(i, 1); alpha dh(i, 2); d dh(i, 3); theta q(i) dh(i, 4); % 改进DH单连杆变换 Ti [cos(theta), -sin(theta), 0, a; sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -d*sin(alpha); sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), d*cos(alpha); 0, 0, 0, 1]; T T * Ti; T_all{i} T; end end逻辑说明forwardKinematics逐连杆累乘变换矩阵T_all{i}就是第i个连杆坐标系在基座系下的位姿。参数说明a是连杆长度alpha是连杆扭角d是连杆偏距theta_offset是关节零位偏置。这几个值必须和你的实际机械臂一致否则碰撞检测会整体偏移后面RRT找到的路径看着无碰撞实际执行时直接撞上去。我一般会先用一组已知关节角把末端位姿和示教器上读到的值对一遍确认运动学没错再往下走。2.2 固定障碍物的几何表达与碰撞检测接口障碍物建模的原则是宁可保守一点也不要漏检。常见做法是把障碍物简化成一组球体或轴对齐包围盒AABB的并集。球体做碰撞检测最快AABB次之两者都比精确网格模型省算力。RRT要调用碰撞检测成千上万次这里省下的每一毫秒都会放大。% 障碍物用球体集合表示: 每行 [cx, cy, cz, r] obstacles [0.30, 0.10, 0.50, 0.12; 0.15, -0.25, 0.35, 0.10; 0.45, 0.20, 0.25, 0.08]; function isCollision checkCollision(dh, q, obstacles, linkRadius) % 输入: DH表, 关节角, 障碍物球集合, 连杆等效半径 % 输出: true表示碰撞 T_all forwardKinematics(dh, q); n size(dh, 1); isCollision false; % 对每根连杆取若干离散点做球-球检测 for i 1:n if i 1 p_start [0; 0; 0]; else p_start T_all{i-1}(1:3, 4); end p_end T_all{i}(1:3, 4); % 沿连杆均匀采样5个点 for t linspace(0, 1, 5) p p_start t * (p_end - p_start); for k 1:size(obstacles, 1) c obstacles(k, 1:3); r obstacles(k, 4); if norm(p - c) (r linkRadius) isCollision true; return; end end end end end逻辑说明把每根连杆近似成一条线段沿线采样若干点每个点当成一个半径为linkRadius的球去和障碍物球做距离判断。参数说明linkRadius是连杆的等效半径取值要略大于实际连杆半径留出安全裕度采样点数5是精度和速度的折中连杆越长、障碍物越小这个数要往上加。注意这里用的是“离散采样”采样太稀会漏检细长障碍物这是后面避坑章节要展开的一个典型问题。3. 用RRT在构型空间里长出第一棵树3.1 RRT主循环采样、最近邻、扩展、碰撞检查RRT的骨架非常朴素在关节限位范围内随机撒一个构型找到树上离它最近的节点朝它迈固定步长如果这段新边不碰撞就加进树里。重复到某个新节点离目标构型足够近为止。function [path, tree] rrtPlan(dh, q_start, q_goal, q_min, q_max, ... obstacles, linkRadius, stepSize, maxIter, goalBias) % 输入: 运动学与碰撞参数, 起止构型, 关节限位, 步长, 最大迭代, 目标偏置 % 输出: 路径(从起点到目标的关节角序列), 树结构 n length(q_start); tree.nodes q_start(:); % 每行一个构型 tree.parent 0; % 根节点无父节点 path []; for iter 1:maxIter % 目标偏置: 一定概率直接采样目标点加速收敛 if rand() goalBias q_rand q_goal(:); else q_rand q_min(:) rand(1, n) .* (q_max(:) - q_min(:)); end % 找最近邻节点 dists vecnorm(tree.nodes - q_rand, 2, 2); [~, idx_near] min(dists); q_near tree.nodes(idx_near, :); % 朝随机点方向扩展固定步长 dir q_rand - q_near; d norm(dir); if d 1e-6 continue; end q_new q_near stepSize * dir / d; % 关节限位检查 if any(q_new q_min) || any(q_new q_max) continue; end % 碰撞检查 if checkCollision(dh, q_new, obstacles, linkRadius) continue; end % 加入树 tree.nodes [tree.nodes; q_new]; tree.parent [tree.parent; idx_near]; % 判断是否到达目标 if norm(q_new - q_goal) stepSize path reconstructPath(tree, size(tree.nodes, 1), q_goal(:)); return; end end end function path reconstructPath(tree, idx, q_goal) % 从目标节点回溯到根再反转 path q_goal; while idx ~ 0 path [tree.nodes(idx, :); path]; idx tree.parent(idx); end end逻辑说明goalBias是目标偏置概率让树有一定概率直接朝目标长避免纯随机采样在狭窄通道里磨蹭太久。stepSize是每次扩展的步长太大容易跳过窄通道太小则迭代次数暴涨。reconstructPath从命中目标的节点沿父指针回溯得到完整路径。参数说明maxIter一般设5000到20000取决于构型空间维度和障碍物密度goalBias常用0.05到0.2环境越复杂越要小否则树会过早收敛到局部。3.2 步长、目标偏置、迭代上限三个参数的联动这三个参数不是独立的调一个必须看另外两个。步长设0.1弧度、目标偏置0.1、迭代上限5000是我在六自由度、三四个球形障碍物场景下的常用起点。如果发现规划经常失败先别急着加迭代上限先看步长是不是太大导致窄通道过不去。反过来如果规划成功但路径抖得厉害说明步长偏小、采样点太密可以适当放大步长再做平滑。参数常用范围调大后果调小后果stepSize0.05~0.2 rad窄通道漏检、路径粗糙迭代暴涨、路径抖动goalBias0.05~0.2陷入局部、探索不足收敛慢、随机性强maxIter5000~20000耗时增加规划失败率上升提示调参时固定另外两个只动一个记录成功率和平均迭代次数别凭感觉一次改三个。4. 让路径从“能走”变成“能执行”4.1 路径剪枝去掉冗余的中间节点原始RRT路径是一堆步长拼接的折线节点密集且方向反复。直接丢给控制器关节会抖。剪枝的思路是从起点开始尝试直接连到尽可能远的后续节点如果这条直连边不碰撞中间节点全删。function path_smooth prunePath(dh, path, obstacles, linkRadius) % 贪心剪枝: 能直连就跳过中间点 n size(path, 1); path_smooth path(1, :); i 1; while i n j n; % 从最远的点往回找第一个可直连的 while j i 1 if ~edgeCollision(dh, path(i,:), path(j,:), obstacles, linkRadius) break; end j j - 1; end path_smooth [path_smooth; path(j, :)]; i j; end end function isCol edgeCollision(dh, q1, q2, obstacles, linkRadius) % 沿两构型之间的直线插值检查碰撞 isCol false; steps 20; for t linspace(0, 1, steps) q q1 t * (q2 - q1); if checkCollision(dh, q, obstacles, linkRadius) isCol true; return; end end end逻辑说明prunePath用贪心策略每次尽量跳到最远的可直连节点。edgeCollision在两点之间插值20步做碰撞检查步数要足够密否则会误判直连可行。参数说明插值步数20是经验值构型空间维度越高、步长越大这个数要相应增加。剪枝后路径节点数通常能降到原来的三分之一到五分之一。4.2 用三次B样条做时间连续平滑剪枝后的路径还是折线关节速度和加速度不连续。常见做法是对每个关节角序列单独做三次B样条拟合得到连续可导的轨迹。function [q_smooth, qd_smooth, qdd_smooth] bsplineSmooth(path, numSamples) % 对每个关节做三次B样条拟合 n size(path, 2); t linspace(0, 1, size(path, 1)); tt linspace(0, 1, numSamples); q_smooth zeros(numSamples, n); qd_smooth zeros(numSamples, n); qdd_smooth zeros(numSamples, n); for j 1:n % 三次样条插值边界自然 pp spline(t, path(:, j)); q_smooth(:, j) ppval(pp, tt); % 数值求导得速度和加速度 qd_smooth(:, j) gradient(q_smooth(:, j), tt); qdd_smooth(:, j) gradient(qd_smooth(:, j), tt); end end逻辑说明spline做三次样条插值ppval在密集时间点上求值gradient数值求导得到速度和加速度。参数说明numSamples一般取200到500太少轨迹不平滑太多计算量上升但精度提升有限。注意平滑后必须重新做一次碰撞检查样条曲线可能偏离原折线钻进障碍物。5. 避坑与排查RRT在3D机械臂上最容易翻车的五个地方5.1 现象规划成功率极低几乎每次都跑满迭代上限原因最常见的是碰撞检测的连杆等效半径linkRadius设得过大把本来可通行的构型全判成碰撞树根本长不开。其次是关节限位范围设错比如把某个关节的限位写成了对称的±π实际机械臂只能转±2.5弧度导致大量采样点无效。解决先把linkRadius调到接近零跑一次看能不能规划成功。如果能说明是半径问题逐步加大到实际连杆半径加安全裕度。关节限位一定从机械臂手册或示教器上抄别凭印象写。5.2 现象路径在MATLAB里显示无碰撞实际执行撞了原因碰撞检测的离散采样太稀。连杆采样5个点、边插值20步在长连杆或小障碍物场景下会漏检。另一个原因是障碍物模型和实际环境有偏差比如实际障碍物比建模的球体大了一圈。解决把连杆采样点加到10个以上边插值步数加到50。障碍物球半径在实测基础上加5%到10%的裕度。宁可保守不可漏检。5.3 现象路径能规划出来但关节抖动严重控制器报跟随误差原因原始RRT路径是折线关节角速度在节点处突变。如果直接拿折线路径做位置控制每个节点都是一次阶跃关节自然抖。解决必须做剪枝加B样条平滑。剪枝去掉冗余节点B样条保证速度和加速度连续。平滑后重新做碰撞检查确认没有因为平滑而偏离到障碍物里。5.4 现象换一个目标构型就规划失败之前明明能跑原因RRT是概率算法单次成功不代表稳定。目标构型附近如果有狭窄通道纯随机采样很难撞进去。另外目标构型本身可能就在障碍物附近goalBias采样到目标后扩展的边直接碰撞树永远够不到。解决检查目标构型本身是否无碰撞。如果是窄通道问题加goalBias或者改用RRT-Connect双向生长。我一般会跑100次统计成功率低于90%就说明参数或环境建模有问题。5.5 现象规划耗时忽长忽短偶尔几秒偶尔几十秒原因MATLAB里forwardKinematics每次碰撞检测都重新算一遍所有连杆变换而RRT要调用几万次。如果DH表或障碍物在循环里被重复读取开销会放大。另外最近邻搜索用的是全树遍历树大了之后vecnorm那行会越来越慢。解决把DH表和障碍物预加载到函数外避免循环内重复构造。最近邻搜索在树节点超过几千后改用KD-treeMATLAB的knnsearch配合KDTreeSearcher。这两个改动通常能把规划时间压到原来的三分之一以下。6. 进阶用双向RRT-Connect和碰撞缓存把成功率拉满单棵RRT在六自由度、多障碍物场景下成功率能到七八成就算不错。想把成功率稳定在95%以上我一般会做两件事换成双向RRT-Connect以及给碰撞检测加缓存。RRT-Connect的思路是同时从起点和终点各长一棵树每轮交替扩展一棵树扩展一步后另一棵树尝试朝新节点贪婪直连。两棵树一旦连通路径就找到了。它对窄通道的穿透能力比单棵树强很多因为两端同时探索相遇概率大幅提升。function [path, treeA, treeB] rrtConnect(dh, q_start, q_goal, q_min, q_max, ... obstacles, linkRadius, stepSize, maxIter) % 双向RRT-Connect treeA.nodes q_start(:); treeA.parent 0; treeB.nodes q_goal(:); treeB.parent 0; for iter 1:maxIter % A树朝随机点扩展一步 q_rand q_min(:) rand(1, length(q_start)) .* (q_max(:) - q_min(:)); [treeA, q_newA] extendTree(dh, treeA, q_rand, q_min, q_max, ... obstacles, linkRadius, stepSize); if isempty(q_newA), continue; end % B树朝A的新节点贪婪直连 [treeB, q_newB, connected] connectTree(dh, treeB, q_newA, q_min, q_max, ... obstacles, linkRadius, stepSize); if connected pathA reconstructPath(treeA, size(treeA.nodes,1), q_newA); pathB reconstructPath(treeB, size(treeB.nodes,1), q_newB); path [pathA; flipud(pathB(1:end-1, :))]; return; end % 交换两棵树下一轮从B树开始 [treeA, treeB] deal(treeB, treeA); end path []; end逻辑说明extendTree朝随机点走一步connectTree朝目标点反复走步直到碰撞或到达。两棵树交替生长连通即返回。参数说明stepSize在RRT-Connect里可以比单棵树稍大因为双向探索弥补了单步的盲目性。maxIter可以比单棵树小通常2000到5000就够。碰撞缓存是另一个提速手段。RRT在扩展时会反复检查相近的构型如果每次都用完整正运动学算一遍浪费很大。做法是用一个哈希表把构型量化到一定精度后作为键碰撞结果作为值。命中缓存直接返回省掉正运动学计算。% 碰撞缓存: 构型量化到0.01弧度精度 cache containers.Map(KeyType, char, ValueType, logical); function isCol checkCollisionCached(dh, q, obstacles, linkRadius, cache) key sprintf(%.2f_, round(q, 2)); if isKey(cache, key) isCol cache(key); return; end isCol checkCollision(dh, q, obstacles, linkRadius); cache(key) isCol; end逻辑说明把关节角四舍五入到两位小数拼成字符串做键命中就返回缓存结果。参数说明量化精度0.01弧度是精度和命中率的折中太细命中率低太粗会误判。注意缓存只对固定障碍物有效动态障碍物场景不能用。验证方法上我习惯跑三组指标100次规划的成功率、平均规划时间、平滑后路径的最大关节速度和加速度。成功率低于90%就回去查碰撞检测和参数最大关节速度超过控制器限制就回去调B样条的时间缩放。这套流程跑下来六自由度机械臂在固定障碍物环境里的规划基本能稳定落地。血泪经验是别信单次成功的路径一定要统计成功率别跳过平滑后的碰撞复检后悔药没地方买。希望帮到你。本文还有配套的精品资源点击获取