简介：基于 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 维度量空间，采样时从关节限位里随机取值，扩展时朝随机点走一小段，全程只需要正向运动学来算机械臂位姿，不需要逆解。这样一来，起点和终点各做一次逆解，中间几百个节点全部用 q（1×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(end+1, :) = q_new; tree_parent(end+1) = 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(end+1,:) = q_new; tree.parent(end+1) = idx; % 5. 到达判定：新节点离目标足够近 if norm(q_new - q_goal) < step_size tree.q(end+1,:) = q_goal; tree.parent(end+1) = 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(i+1,:); 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] 之间；当 t=0 时最近点就是 a，t=1 时最近点就是 b，这样就避免了“无限延长线”误判。把 6 个关节坐标存成 pts 后，相邻两点连线构成连杆的近似直线。这里有一个重要简化：PUMA560 的第三根连杆不是笔直的，有一个 0.0203 米的偏置，如果实训要求比较严格，应该把第三根连杆拆成两段，或者用胶囊体包络，否则在靠近末端时可能穿透障碍物。对于课程设计，通常一段直线就够用，但报告里要把这个近似说明白。

还有一个性能问题：rrt_plan 每次扩展都要对 q_new 做一次 fkine，这个操作不算慢，但如果在循环里频繁 plot 就会非常卡。想提速可以把碰撞检测里的障碍物参数改成全局变量，或者把 is_collision 改成可传入 obs 参数，避免每帧重复定义数据。更高效的做法是先把所有障碍物位置存成 N×3 矩阵，再用向量化计算所有线段到所有球心的距离，一次循环解决；不过课程实训的数据量不大，顺序检查也够用。

4. 三个关键参数和一个双向改进：把「找到路」变成「走好路」

4.1 步长、最大迭代数和目标偏置概率怎么配

RRT 对参数很敏感，课程实训里的“玄学”绝大部分来自这三个参数。我一般会先用一组保守数值跑通：step_size=0.05，max_iter=3000，goal_bias=0.1。跑通后再逐步改，看规划时间、路径质量和成功率的变化。下面的表格列出了我常用的经验范围，不是官方标准，但足够作为起点。

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:i+1 if ~edge_collision(robot, path(i,:), path(j,:), step_size) pruned(end+1,:) = 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 路径规划变成闭环。希望帮到你。

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