简介基于 MATLAB 的 PUMA560 机械臂 RRT 路径规划算法仿真完整源码面向机器人、自动化、人工智能等方向的课程实训与毕业设计场景。项目包含路径规划核心算法、平滑处理、碰撞检测、路径可行性检查等完整模块并配有 RRT 生长过程、机械臂运动和工作空间可视化等演示动图可直观对比平滑前后的规划路径与运动效果适合从入门到进阶逐步复现、调试和理解算法细节。压缩包共 27 个文件以 .m 源文件为主辅以 8 个演示动图、项目说明文档和一个打包用 zip整体约 21.33MB目录结构清晰便于查阅。已有 604 人学习下载。读者既可将完整方案用于课程设计、初期项目立项演示也可在此基础上扩展功能进一步掌握采样规划与碰撞检测在机械臂控制中的实际应用。若基础较好还可修改障碍物布局与采样策略实现不同工况下的路径规划实验。1. 为什么课程实训里的 PUMA560 偏偏要和 RRT 绑在一起做课程实训最常踩的坑是把基于MATLAB的PUMA560机械臂RRT路径规划算法仿真当成两个作业先把机械臂画出来再把RRT跑起来。实际上画机械臂和跑RRT只用半天剩下的大把时间都耗在“为什么规划出来的路径让机械臂像抽搐一样乱抖”上。下面用一套完整的MATLAB源码拆开讲从采样空间、碰撞检测到参数调优和避坑让新手能按步骤跑通让已经跑通的人知道边界在哪。适合正在做机器人课程设计、毕业设计或者想快速验证RRT实际效果的从业者。2. RRT 规划前先搞清三件事采样空间、碰撞检测和 PUMA560 的逆解2.1 为什么在关节空间里跑 RRT自由度与逆解的账PUMA560 是一条六自由度串联机械臂工作空间里到达同一个末端位姿通常有 8 组以上的逆解。这个数字意味着如果你在笛卡尔空间里规划一个三维点规划完还要反算 6 个关节角那么每一次采样都可能面对“无解、多解、解不连续”三个问题。光处理多解就得写一大段分支逻辑而且很容易把一条连续路径拆成关节角突跳的碎片。RRT 的优势恰恰在关节空间里最容易发挥把每个关节角度当成一维状态整个规划空间就是一个 6 维度量空间采样时从关节限位里随机取值扩展时朝随机点走一小段全程只需要正向运动学来算机械臂位姿不需要逆解。这样一来起点和终点各做一次逆解中间几百个节点全部用 q1×6 的行向量直接表示路径天然是关节角连续的。另一个容易忽略的细节是关节空间里的“直线插值”对应到笛卡尔空间是一条弧线不是直线。所以就算你手工把起点和终点的末端坐标画成一条直线中间每个关节角线性变化时末端实际走的是弯曲轨迹。RRT 不做人工插值它让树自己通过采样和碰撞检测在高维空间里找一条可行通道这正是它适合高自由度机械臂的原因。课程实训里如果你看到路径的三维图是一堆乱线不必觉得算法错了先分清坐标轴是关节角还是笛卡尔坐标再判断合理性。常见的做法是直接用 Robotics Toolbox 里的 mdl_puma560 加载预设模型或者在脚本里临时构造 SerialLink。我一般会建议课程实训选手手动搭一遍 DH 参数别嫌麻烦因为后面写碰撞检测时你需要知道每一段连杆的起点和终点在哪而模型对连杆坐标系的定义直接影响你取点的方式。如果只用自带的模型你拿到手的可能是一堆封装好的坐标出问题时很难定位是模型问题还是碰撞检测问题。2.2 用 Robotics Toolbox 把 PUMA560 搭成可碰撞检测的模型下面这段代码用标准 DH 方式创建 PUMA560 的六个连杆。数值参考了教材里常见的参数如果你手里工具箱版本里已经有 mdl_puma560可以直接调用它来替换前几行不影响后面的 RRT 和碰撞检测函数接口。% 手写 DH 参数建立 PUMA560单位为米和弧度 L1 Link(d, 0, a, 0, alpha, pi/2, standard); L2 Link(d, 0.149, a, 0.4318, alpha, 0, standard); L3 Link(d, 0, a, 0.0203, alpha, -pi/2,standard); L4 Link(d, 0.4331, a, 0, alpha, pi/2, standard); L5 Link(d, 0, a, 0, alpha, -pi/2,standard); L6 Link(d, 0, a, 0, alpha, 0, standard); p560 SerialLink([L1 L2 L3 L4 L5 L6], name, PUMA560); % 关节限位可以从模型里直接读出 q_start [0 0 0 0 0 0]; q_goal [pi/4 -pi/6 pi/2 0 pi/3 0]; p560.plot(q_start);这里每个 Link 的 d 是沿 z 轴的偏移a 是沿 x 轴的连杆长度alpha 是连杆扭角。标准 DH 角度必须用弧度否则 SerialLink 算出来的正运动学会完全错位。p560.plot 在第一次调用时会弹出机械臂三维图它会把六个关节坐标系画出来这些坐标系就是后面碰撞检测取连杆端点的依据。要注意不同版本 Robotics Toolbox 对 Link 的参数顺序要求一致但返回值类型有差异新版本 fkine 返回 SE3 对象老版本返回 4×4 齐次矩阵后面写取坐标时要做对应适配。使用内置模型时还有一个坑mdl_puma560 会把全局变量 p560 直接放到工作区但如果你的脚本要在函数里调用它这个全局变量不会自动可见需要在函数内部用 evalin(base, p560) 或者作为参数传进来。手动创建 SerialLink 就没有这个问题这也是我推荐手动写一遍的原因之一。如果你用的是 MATLAB 2026b 或者别的较新版本工具箱的函数签名可能变化遇到报错先查一下对应文档里的 SE3 用法再回来改代码。2.3 采样与最近邻随机树是怎么长出来的RRT 的单步逻辑只有四行随机采样一个 q_rand在现有树里找离 q_rand 最近的 q_near从 q_near 朝 q_rand 方向走 step_size 得到 q_new检查 q_new 是否碰撞。如果未碰撞就把 q_new 挂到树里并记录它的父节点索引。这一段用 MATLAB 写非常顺手因为矩阵操作天然适合“树”这种结构每行是一个节点parents 向量记录父亲的第几行不需要像 C 语言那样手动管链表。% 在限位范围内随机采样一个关节角组合 q_rand p560.qlim(:,1) rand(1,6) .* (p560.qlim(:,2)-p560.qlim(:,1)); % 找最近邻计算当前树所有节点到 q_rand 的平方距离取最小值所在行 dist2 sum((tree.q - q_rand).^2, 2); [~, idx] min(dist2); q_near tree.q(idx, :); % 沿方向扩展 delta q_rand - q_near; step norm(delta); if step step_size q_new q_rand; else q_new q_near step_size * delta / step; end这段代码里的 tree.q 保存在主程序的循环里每扩展一个节点就 append 一行。用平方距离代替欧氏距离省掉 sqrt最近邻结果不变这是 MATLAB 里常见的加速技巧。关节空间的“距离”是六个关节角变化量的均方根它不等于末端走过的弧长但在关节空间做 RRT 时我们只需用它衡量“位形差”。如果想让路径更贴近末端轨迹可以改用加权距离权重越大的关节变化越被惩罚但课程实训一般不需要。树的数据结构如果要写干净推荐用两个变量tree_q 保存节点tree_parent 保存父节点索引。不需要额外堆一个 classdef那样反而把简单问题复杂化。每次扩展成功时tree_q(end1, :) q_new; tree_parent(end1) idx;当新节点刚好落在目标附近就从最后一个节点反向回溯 parent找到一条从起点到目标的路径。这个回溯过程是 RRT 里最简单的部分但很多同学在这里把方向搞反了后面避坑章会讲到相关的坑。3. 从零写一套可运行的 RRT 路径规划源码主程序、树扩展与碰撞检测3.1 主程序骨架参数定义、初始化和可视化环境把上一章提到的最小逻辑收拢到一个脚本里就得到课程实训里最常见的工程结构一个主脚本负责定义场景和参数一个 rrt_plan 函数负责扩展树一个 is_collision 函数负责碰撞检测。主脚本不需要写得花哨但要把起点、终点、障碍物、参数全部放在显眼的地方方便答辩时调数值。% RRT 主程序骨架 clear; clc; close all; % 如果工具箱自带 PUMA560 模型就用它否则用 2.2 的手写模型 mdl_puma560; % 起点和终点位形 q_start [0 0 0 0 0 0]; q_goal [pi/4 -pi/6 pi/2 0 pi/3 0]; % RRT 参数 step_size 0.05; % 关节空间步长单位弧度 max_iter 3000; % 最大迭代次数 goal_bias 0.1; % 目标偏置概率10% 的概率直接采样终点 % 调用规划器 [path, tree] rrt_plan(p560, q_start, q_goal, step_size, max_iter, goal_bias); % 绘图先画机械臂初始位形再画目标位形 p560.plot(q_start, workspace, [-0.8 0.8 -0.8 0.8 -0.1 1.2]); hold on; p560.plot(q_goal, workspace, [-0.8 0.8 -0.8 0.8 -0.1 1.2]); % 画树和最终路径这里简单画节点连线 plot3(tree.q(:,1), tree.q(:,2), tree.q(:,3), b.); plot3(path(:,1), path(:,2), path(:,3), r-, LineWidth, 2);这里 plot3 画的是前三个关节角在三维坐标里的轨迹只是一种可视化技巧不是机械臂末端轨迹。有些同学会把前三关节角当成 x/y/z 坐标画出来然后误以为路径绕开了障碍物实际上真正判断是否撞到障碍物的是 is_collision而不是这张图。plot 的 workspace 参数用来固定视角范围避免图形窗口缩放导致视觉误判。起点和终点最好设置在关节限位内部并且让两个位形差距不能太小否则 RRT 还没开始扩展就以为到达目标了。3.2 RRT 树扩展函数采样、最近邻与步进下面给出 rrt_plan 的完整实现。为了能让课程实训直接复用这里把树定义为结构体q 是 N×6 的节点矩阵parent 是 N×1 的父节点索引向量。根节点的父节点是 0。function [path, tree] rrt_plan(robot, q_start, q_goal, step_size, max_iter, goal_bias) tree.q(1,:) q_start; tree.parent(1) 0; for i 1:max_iter % 1. 采样 if rand goal_bias q_rand q_goal; else q_rand robot.qlim(:,1) rand(1,6) .* (robot.qlim(:,2)-robot.qlim(:,1)); end % 2. 找最近邻 d2 sum((tree.q - q_rand).^2, 2); [~, idx] min(d2); q_near tree.q(idx, :); % 3. 步进 delta q_rand - q_near; dist norm(delta); if dist step_size q_new q_rand; else q_new q_near step_size * delta / dist; end % 4. 碰撞检测 if ~is_collision(robot, q_new) tree.q(end1,:) q_new; tree.parent(end1) idx; % 5. 到达判定新节点离目标足够近 if norm(q_new - q_goal) step_size tree.q(end1,:) q_goal; tree.parent(end1) size(tree.q,1)-1; path extract_path(tree); return; end end end error(RRT: 在最大迭代次数内没有找到路径); end这里要注意几点采样是在整个关节限位内均匀采样目标偏置会让树以 10% 概率直接朝目标生长这是 RRT 能快速收敛的关键。步进是在关节空间做线性插值所以每两个相邻节点之间的关节角变化量不会超过 step_size。碰撞检测放在到达判定之前确保新节点安全。到达判定用的是关节空间距离所以 step_size 也顺带充当了“到达阈值”如果 step_size 设得太大比如 0.3 弧度程序会在离目标还很远时提前结束最后一段路径会“跳”过去。extract_path 是从父节点索引回溯路径的辅助函数逻辑很简单function path extract_path(tree) path tree.q(end,:); parent tree.parent(end); while parent 0 path [tree.q(parent,:); path]; parent tree.parent(parent); end end这里把当前节点不断接到 path 最前面直到根节点。注意 path 的行方向不能反否则后面画图时路径会从目标倒着走回起点导致动画方向错误。3.3 碰撞检测函数把连杆简化成线段再求距离碰撞检测是 RRT 里最影响“真实性”也最容易偷懒出错的地方。课程实训中常见做法是把机械臂每两个关节之间的连杆看成一条空间线段把障碍物看成球体或圆柱然后计算线段到球心的最短距离。这样做速度极快误差可控而且不需要画网格或者做三角剖分。function flag is_collision(robot, q) % 计算所有关节坐标六轴共 6 个关节坐标系原点 T robot.fkine(q); % 新版 Toolbox 返回 SE3 数组 pts zeros(6, 3); for i 1:6 if isa(T, SE3) pts(i,:) T(i).t; % 新版取平移向量 else pts(i,:) T(1:3,4,i); % 老版取齐次矩阵最后一列 end end % 障碍物定义一个球 obs_center [0.6, 0, 0.3]; obs_radius 0.1; % 检查相邻关节之间的连杆是否碰球 for i 1:5 p1 pts(i,:); p2 pts(i1,:); d point_segment_dist(obs_center, p1, p2); if d obs_radius flag true; return; end end flag false; end function d point_segment_dist(p, a, b) % 计算点 p 到线段 ab 的最小距离 ab b - a; t max(0, min(1, dot(p - a, ab) / dot(ab, ab))); closest a t * ab; d norm(p - closest); end代码里的 point_segment_dist 把点投影到线段上t 被截断在 [0,1] 之间当 t0 时最近点就是 at1 时最近点就是 b这样就避免了“无限延长线”误判。把 6 个关节坐标存成 pts 后相邻两点连线构成连杆的近似直线。这里有一个重要简化PUMA560 的第三根连杆不是笔直的有一个 0.0203 米的偏置如果实训要求比较严格应该把第三根连杆拆成两段或者用胶囊体包络否则在靠近末端时可能穿透障碍物。对于课程设计通常一段直线就够用但报告里要把这个近似说明白。还有一个性能问题rrt_plan 每次扩展都要对 q_new 做一次 fkine这个操作不算慢但如果在循环里频繁 plot 就会非常卡。想提速可以把碰撞检测里的障碍物参数改成全局变量或者把 is_collision 改成可传入 obs 参数避免每帧重复定义数据。更高效的做法是先把所有障碍物位置存成 N×3 矩阵再用向量化计算所有线段到所有球心的距离一次循环解决不过课程实训的数据量不大顺序检查也够用。4. 三个关键参数和一个双向改进把「找到路」变成「走好路」4.1 步长、最大迭代数和目标偏置概率怎么配RRT 对参数很敏感课程实训里的“玄学”绝大部分来自这三个参数。我一般会先用一组保守数值跑通step_size0.05max_iter3000goal_bias0.1。跑通后再逐步改看规划时间、路径质量和成功率的变化。下面的表格列出了我常用的经验范围不是官方标准但足够作为起点。参数常用范围调小的影响调大的影响step_size0.020.1 rad树扩展慢迭代更久可能跨越障碍路径粗糙max_iter100010000可能找不到路径耗时增加收敛更稳goal_bias0.050.2树更随机路径更曲折收敛快但易陷入局部震荡step_size 需要和机械臂尺寸挂钩。PUMA560 二连杆长度约 0.4318 米关节角变化 0.05 弧度时末端大约移动 0.05×0.43≈0.02 米。如果障碍物半径只有 0.05 米step_size 至少应小于 0.05否则一个步进可能直接从障碍物一侧“穿”到另一侧且碰撞检测只检查端点发现不了中间的穿透。另一个常见的配合是把 max_iter 设成动态上限循环里统计扩展成功次数连续 100 次没有扩展成功就提前退出这样的程序不至于挂着转半天。很多人把 goal_bias 设成 0.5以为收敛更快。实际测试下来目标偏置过大会让树一直朝目标方向生长一旦中间有障碍物树就会被“卡”在障碍物边缘反复扩展失败。比较好的做法是保持在 0.1 左右让随机采样去探索周围空间偶尔被目标吸引。如果你发现路径总是绕远可以把 goal_bias 提到 0.15再对比一次。4.2 后处理剪枝、插值与轨迹平滑原始 RRT 路径往往有大量冗余回折尤其随机采样多的场景。课程实训里最直接的改进是在找到路径后做贪心剪枝从起点开始尝试直接连到后面某个节点如果中间没碰撞就跳过中间的节点。这个操作能把路径长度压缩 30%50%并且让机械臂动作更干脆。下面是一个在关节空间做剪枝的参考实现function pruned prune_path(robot, path, step_size) pruned path(1,:); i 1; while i size(path, 1) % 从最后一个节点往前找看当前节点能直接连到多远 for j size(path, 1):-1:i1 if ~edge_collision(robot, path(i,:), path(j,:), step_size) pruned(end1,:) path(j,:); i j; break; end end end end function flag edge_collision(robot, q1, q2, step_size) % 把 q1 到 q2 的直线拆成若干小段逐段碰撞检测 n ceil(norm(q2 - q1) / (step_size * 0.5)); for k 0:n q q1 (q2 - q1) * k / n; if is_collision(robot, q) flag true; return; end end flag false; end这里断点距离取 step_size 的一半比原来更保守保证不会漏检长连杆。剪枝后路径的节点数可能从几百降到几十。要让轨迹可执行再用 interp1 做插值比如把每个关节角分别插值成 500 点的时间序列注意不要让相邻点角度差过大否则机械臂的实际速度会非常高。插值代码很短t_old linspace(0, 10, size(path,1)); t_new linspace(0, 10, 500); path_smooth interp1(t_old, path, t_new, pchip);pchip 是保形状的三次插值不会像 spline 那样产生过冲适合关节角轨迹。插值后的路径还要再做一次碰撞检查因为插值点有可能从障碍物边缘“抄近路”穿进去。4.3 双向 RRT 的思路与本项目里的最小改动如果单向 RRT 收敛太慢改进方向是双向 RRT起点和终点同时各长一棵树每次迭代两棵树交替向随机点扩展再尝试把两棵树连起来。这个思路在实际项目中尤其适合 PUMA560 这类六轴机械臂因为目标点附近的障碍物往往比起点多单向树很难“挤”进去而双向树从两头攻成功率和速度都会明显提升。实现上只需要把单棵树换成两个树结构核心循环里做一次交换。% 双向 RRT 核心循环节选 treeA.q(1,:) q_start; treeA.parent(1) 0; treeB.q(1,:) q_goal; treeB.parent(1) 0; for i 1:max_iter q_rand sample_q(robot, q_goal, goal_bias); [treeA, q_new] extend_tree(robot, treeA, q_rand, step_size); if ~isempty(q_new) [treeB, q_conn] extend_tree(robot, treeB, q_new, step_size); if norm(q_conn - q_new) step_size % 连接两棵树并回溯路径 path connect_paths(treeA, treeB); return; end end % 交换两棵树让本次朝目标生长的树成为下一次搜索树 [treeA, treeB] deal(treeB, treeA); end这段代码里的 extend_tree 就是把 3.2 里的“最近邻步进碰撞检测”抽成一个函数返回新的树和扩展出的 q_new。连接两棵树时需要把 treeB 的父节点方向反向链接回 treeA所以 treeB 在建立时也要记录父节点。要注意的是交换策略常规做法是每次扩展完后交换让“更靠近目标的树”交替生长也可以在树 A 连续多次无扩展时主动交换。双向 RRT 不是银弹当两棵树都困在同一片狭窄通道两侧时仍可能很长时间连不上这时需要结合目标偏置或者引导采样。课程实训做到这一步已经可以写在报告里作为“改进与对比”了不需要再上 RRT*。5. 避坑记PUMA560 与 RRT 仿真里最容易翻车的5个问题5.1 规划结果每次跑都不一样随机种子与初始树的影响现象同一个工程文件前后两次运行得到完全不同的路径一次 200 次迭代就找到另一次 2000 次还没找到甚至图形窗口里树的形状差异巨大。原因RRT 的采样用的是 rand 函数每次运行 MATLAB 时随机种子都不同。树的结构对采样序列异常敏感只要第一次采样点差一点后续整棵树就差很远。这属于算法本身的随机性不是代码写错。解决调试阶段在脚本开头加 rng(0) 固定随机种子让每次运行可复现汇报或对比算法时再用 rng(shuffle) 跑多次统计平均迭代次数和路径长度。固定随机种子后仍然出现差异才需要怀疑代码里有未初始化的变量。5.2 路径直线穿过障碍但碰撞检测没拦住只检测了末端点现象从画面上看机械臂的两个连杆明显从障碍球内部穿过去但程序一路返回“未碰撞”路径照样输出。原因is_collision 里只检查了 q_new 这一个点的关节坐标而 q_new 是独立位形不能代表从 q_near 到 q_new 之间整段运动是否安全。RRT 的扩展段虽然关节角变化量只有 step_size但如果 step_size 比障碍物半径还大两个端点都在障碍物外侧中间线段却可能穿过障碍物。解决在扩展函数里不要直接对整个 q_new 只做一次碰撞检测而是把从 q_near 到 q_new 的线段按 step_size 的一半拆成若干小段逐段调用 is_collision。前面 4.2 的 edge_collision 就是做这件事的。我一般会把这段检查放在“是否接受新节点”之前而 3.2 里的到达判定则放在其后保证路径上每个中间点都安全。5.3 机械臂到了目标点但关节角跳变逆解多解与最近解不连续现象RRT 找到了从 q_start 到 q_goal 的路径画动画时机械臂末端轨迹很顺畅但某个关节在某一帧突然从 100° 跳到 -100°像抽搐一样。原因RRT 在关节空间规划理论上关节角连续但如果你在起点或目标点用了逆解函数 ikine 获得 q_start 和 q_goal而逆解函数返回的是满足末端位姿的最近解不同迭代下可能落入不同解的流形。更常见的是你在可视化时对路径做了笛卡尔插值或者人为把关节角 wrap 到 [-π, π]导致原本连续的角度突变。解决在定义起点终点时直接用给定的关节角而不是用过逆解再反推如果非要用逆解就要把相邻节点的关节角做 unwrap 处理让每个关节的角度变化落在最小差值方向。检查方法很简单把 path 的每一列单独 plot看是不是平滑曲线如果有垂直跳线就是角度包裹问题。在报告里写清楚你用的是关节空间规划而不是笛卡尔空间一切就说得通了。5.4 程序运行几分钟没结果最大迭代次数和采样范围不匹配现象max_iter 设成 5000但程序跑了几分钟还在循环既不报错也不返回路径进度条被卡在机械臂初始化阶段。原因多数是采样范围太大而 step_size 太小树几乎一直在“原地打转”。PUMA560 每个关节限位加起来接近十几个弧度如果 step_size 设为 0.01树要扩展上千步才能走到目标而碰撞检测又检查了每一段连杆速度很慢。另一种原因是目标偏置概率为 0树完全靠随机采样撞运气。解决先用 4.1 里的保守参数跑通再调低碰撞检测频率。还可以在循环里加一个“连续 100 次扩展失败”的计数器超过阈值就提前 exit。如果确实需要快速出结果把 step_size 提高到 0.08或者改用双向 RRT。这里有个经验当 max_iter 超过 5000 还没找到路径先别急着把次数加到 20000而是回头检查障碍物是不是放在机械臂必经路线上或者起点终点是否不在同一个连通空间。5.5 用 plot 实时绘图卡死图形刷新频率太高现象程序运行后MATLAB 窗口一帧一帧地画机械臂每扩展一个节点就调一次 plot到后面画面越来越卡最后像是死机一样。原因plot 和 robot.plot 都是重量级图形操作在循环里每步调用会让渲染开销远超规划计算。尤其当路径有几千个节点时画线、更新视图、计算遮挡全部压在图形线程上。解决规划阶段不绘图用 hold on 和 plot3 只画节点与连线等规划结束后再用 robot.plot 播放一次动画。如果一定要实时看生长过程可以每扩展 50 个节点才刷新一次并且用 drawnow limitrate 控制帧率。另一个技巧是绘制时只画最新的线段不要让 MATLAB 重绘整棵树这能明显缓解卡顿。课程实训答辩时动画演示“先显示树生长再显示机械臂沿路径运动”就足够不需要每步都刷新。6. 最后一步不是仿真动画而是把路径交给 Simulink 做一次跟踪验证RRT 在关节空间规划出的路径是一串离散位形真正要证明它“可执行”还得让机械臂模型沿着路径走一遍看每个关节的角速度和角加速度是不是在合理范围内。课程实训里常见的做法是把 path 离散成时间序列放进 Simulink 模型里做一次轨迹跟踪。下面这段代码把路径转成 timeseries 对象% 把路径节点转成时间序列假设总时长 10 秒 t_old linspace(0, 10, size(path,1)); t_new linspace(0, 10, 500); path_smooth interp1(t_old, path, t_new, pchip); % 导出成 timeseries 给 Simulink 用 ts timeseries(path_smooth, t_new);这里用 pchip 插值而不是线性插值是因为线性插值会让关节角在节点处出现速度突变机械臂模型会像“咔哒”一下。pchip 保证了一阶导数连续Simulink 里的速度信号不会跳变。如果希望更平滑可以再做一轮低通滤波但注意滤波会把路径往障碍物方向拉偏滤波后再做一次碰撞检查不能省。我在做课程实训时跳过这一步直接在仿真动画里看到机械臂走完就算通过结果答辩时老师问“这条路能不能直接下发到真实机械臂”我答不上来。后来养成一个习惯每次规划完都先算一遍最大关节速度dt t_new(2) - t_new(1); vel diff(path_smooth) / dt; max_vel max(abs(vel(:)));如果 max_vel 超过 PUMA560 关节限位就退回去调小时间尺度或增加插值点。这个检查花两分钟但能让整个仿真从“看起来动了”变成“看起来能落地”。做完这一步再导到 Simulink 里才算是把 RRT 路径规划变成闭环。希望帮到你。本文还有配套的精品资源点击获取