简介：围绕UR5六轴工业机械臂在MATLAB环境下的轨迹规划，这份资料面向机器人工程、自动化控制方向的学生与入门研究者，帮助其打通从正逆运动学求解、动力学建模到平滑路径生成的完整链路。压缩包共29个文件、约859KB，以21个MATLAB脚本（.m）为主体，配合4个数据文件（.mat）、仿真场景文件、远程接口动态库、说明文档与一份PDF报告，内容涉及逆运动学求解、雅可比矩阵计算、欧拉角与旋转矩阵变换、关节角设置及轨迹插值等模块，可在MATLAB中直接运行调试。资源已有5605人学习下载，在同类教学案例中具备较好的参考基础。读者可据此复现UR5的建模与规划流程，理解速度、加速度约束与碰撞规避的处理思路，并借助报告文档梳理算法脉络，适合课程设计、毕业设计及机器人入门实验使用。

1. 从一条"看起来很顺"的轨迹说起

一台 UR5 摆在台面上，末端沿直线走得挺顺，导进控制器跑到第三个路点却抖了一下——这种在 MATLAB 里画得漂亮、一上机就翻车的场面，凡是真做过机械臂轨迹规划的人都遇到过。这里要拆的是一套 UR5 在 MATLAB 下的轨迹规划工程链路：从 DH 参数建模、正逆运动学自证，到关节空间五次多项式、笛卡尔直线插补，再到雅可比与奇异位形体检，最后整理成能往控制器上对的关节序列。它适合刚拿到 UR5 或它的仿真模型、需要先把运动学链路跑通的人，也适合已经在跑但说不清"为什么抖、为什么逆解偶尔不收敛"的人。整套东西不依赖额外硬件，一台装了 MATLAB 的机器就能从零跑到可视化动画。

2. 把 UR5 立起来：DH 参数表与正运动学的自证

建模这一步看着最枯燥，但它决定了后面所有轨迹的天花板。模型错了，后面调采样周期、调速度上限全是白费。

2.1 标准 DH 与改进 DH，先选定一套再动手

串联机械臂的 DH 参数有两套主流约定：标准 DH（Distal）和改进 DH（Modified，也叫 Craig 约定）。两者的差别不在参数本身，而在连杆变换的乘法顺序。标准 DH 的单连杆变换是 Rot(z,θ)·Trans(z,d)·Trans(x,a)·Rot(x,α)，改进 DH 是 Rot(x,α)·Trans(x,a)·Rot(z,θ)·Trans(z,d)。顺序一换，每个参数的几何含义就跟着换，a 和 d 到底描述的是哪一段距离、α 拧的是哪一个坐标轴，全都不一样。

我一般直接选标准 DH，理由有三条。第一，Robotics Toolbox for MATLAB 里Link默认就是标准 DH，除非显式加'modified'，不写就是标准的，少一处出错点。第二，网上能搜到的 UR5 参数表绝大多数是标准 DH 形式，对照成本低。第三，标准 DH 的正解展开形式在教科书里最统一，手工验算的时候不容易自我怀疑。

真正致命的坑是混用：从一份资料抄了 a 和 α，又从另一份资料抄了 d，两套约定缝在一起，正解出来的位置能差几十厘米，但姿态看着还挺像回事，很多人就这么带着错误模型往下跑了。判断方法在 2.3 节。

2.2 六行连杆表：UR5 的 a、d、alpha 怎么填

UR5 是六自由度、全旋转关节、球形腕的结构，标准 DH 下六个连杆的固定参数如下。表里只列固定量，θ 由关节角实时决定，不写进表里。

把这六行写进 MATLAB：

% UR5 标准 DH 连杆表，theta 由关节角输入，这里只给固定项 L(1) = Link('d', 0.089159, 'a', 0, 'alpha', pi/2, 'qlim', [-pi pi]); L(2) = Link('d', 0, 'a', -0.425, 'alpha', 0, 'qlim', [-pi pi]); L(3) = Link('d', 0, 'a', -0.39225, 'alpha', 0, 'qlim', [-pi pi]); L(4) = Link('d', 0.10915, 'a', 0, 'alpha', pi/2, 'qlim', [-pi pi]); L(5) = Link('d', 0.09465, 'a', 0, 'alpha', -pi/2, 'qlim', [-pi pi]); L(6) = Link('d', 0.0823, 'a', 0, 'alpha', 0, 'qlim', [-pi pi]); ur5 = SerialLink(L, 'name', 'UR5'); ur5.tool = transl(0, 0, 0); % 法兰中心；带夹爪时在此叠加偏移

参数说明几件事。qlim这里统一填了 ±π，只是为了让plot和ikine有个名义边界，真实限位一定要按你手上那台的实际值改，UR5 各关节的行程并不都到 ±180°，尤其腕部几个关节。d的单位是米，不是毫米，这一条每年都能放倒一批人。a取负值不是笔误，是标准 DH 约定下大臂和小臂沿 x 轴的方向问题，如果你抄到的表里 a 是正的，那多半是改进 DH 表，别硬套。

ur5.tool这一行现在设成零，意味着末端坐标系就是法兰中心。实际用夹爪或焊枪的时候，工具中心点（TCP）离法兰还有一段距离，这段距离不补，后面所有笛卡尔轨迹都会整体偏一个工具长度，而且偏得很"合理"，很难一眼看出来。

2.3 零位自检：三个数字判断模型有没有建错

模型建完别急着做轨迹，先跑正解自检。三个检查点，任何一个不对就回去查表。

% 检查点 1：全零位的末端位置 T0 = ur5.fkine(zeros(1, 6)); disp(transl(T0)); % 检查点 2：全零位的几何形态 ur5.plot(zeros(1, 6)); % 手臂应呈一伸直姿态，关节 2/3 不折 % 检查点 3：一组非零关节角，与控制器读数对照 q_test = deg2rad([0 -90 90 -90 -90 0]); T1 = ur5.fkine(q_test); disp(T1); disp(ur5.ikine(T1, 'q0', q_test, 'mask', [1 1 1 1 1 1])); % 应回到 q_test

第一个数字，全零位时末端到基座的距离应该在 0.8 m 量级。UR5 的臂展大概在 850 mm 这个级别，零位末端位置体现的就是这个尺度。如果算出来是 80 或者 0.08，先看单位；如果算出 0.4 左右，八成是某个a的符号反了，两段臂长互相抵消了一部分。这个检查不需要记住精确坐标，只需要确认"量级对"。

第二个检查点看图形。plot出来的姿态要和你手上那台机器人的零位形态一致。UR5 零位是大臂小臂基本伸直、腕部朝一个固定方向的形态。如果画出来肘部折成 90°，那不是参数抄错就是把 θ 的零位定义搞反了。

第三个检查点是闭环验证：拿一组关节角正解出一个位姿，再用这个位姿反解回去，看能不能回到原关节角。能回去说明正逆解是自洽的，后面做轨迹才有意义。注意ikine是数值解，q0种子给的是q_test本身，所以这条测试偏宽松，它验证的是"函数能算"，不是"逆解唯一"。

2.4 工具坐标系与基座坐标系的一次性对齐

如果 UR5 不是装在理想位置——比如倒装、侧装，或者装在一块斜的台面上——base这一项必须补，否则轨迹规划出来的所有位姿都是相对机器人基座坐标系的，和你在工作空间里量的坐标对不上。

% 机器人倒装在天花板上，绕 x 轴转 180 度，再平移 ur5.base = transl(1.2, 0, 2.0) * trotx(pi); % 末端装了一个 150 mm 长的夹爪，沿工具 z 轴伸出 ur5.tool = transl(0, 0, 0.15) * trotx(pi/2);

base描述的是机器人基座在世界坐标系里的位姿，tool描述的是 TCP 在末端法兰坐标系里的位姿。这两项都是左乘/右乘一次就固定的量，写一次，后面所有fkine、ikine、jacob0都会自动带上。常见错误是每次做轨迹都手动在结果上补偏移，补着补着就补重复了。这两项设完之后，用 2.3 的检查点 1 再跑一遍，末端位置应该正好平移了一个基座偏移量，对不上就说明乘法顺序写反了。

3. 轨迹怎么生成：五次多项式、笛卡尔直线与采样周期

模型自证通过之后，才是轨迹本身。轨迹规划这件事的核心分歧只有一个：在关节空间插值，还是在笛卡尔空间插值。选错了，后面所有的速度、加速度约束都是无效功。

3.1 jtraj 的三个输出，哪个才是你要下发的

关节空间插值最常用的是五次多项式，jtraj就是干这个的。它保证关节位置、速度、加速度在起点和终点都连续，起点终点速度为零、加速度为零，中间过程平滑。这条性质对真实机械臂很重要：加速度不连续意味着加加速度（jerk）是冲激，机械结构上就是"咔"的一声，减速器跟着受罪。

q0 = deg2rad([0 -90 90 -90 -90 0]); qf = deg2rad([60 -60 45 -60 -90 30]); t = 0:0.005:3; % 3 秒，200 Hz [q, qd, qdd] = jtraj(q0, qf, t); fprintf('最大关节速度 %.3f rad/s\n', max(abs(qd), [], 'all')); fprintf('最大关节加速度 %.3f rad/s^2\n', max(abs(qdd), [], 'all'));

三个输出的含义要说清：q是关节角序列，单位 rad，行数等于时间点数，列数等于关节数；qd是关节角速度，rad/s；qdd是关节角加速度，rad/s²。下发给控制器的是q，qd和qdd是用来做约束检查的，不是拿来下发的——绝大多数控制器自己会做插补，你只要给路点和总时长就行。

t既可以给时间向量（像上面这样），也可以直接给点数，比如jtraj(q0, qf, 100)就是插 100 个点。给时间向量更可控，因为你能明确知道采样周期是多少。这里 0.005 秒对应 200 Hz，是个保守选择，下一节细说。

还有一个容易忽略的点：jtraj是逐关节独立插值的，六个关节各自走各自的五次多项式，同时开始同时结束。这意味着末端的实际轨迹是一条曲线，不是直线。如果你的任务是"末端沿直线走"，jtraj从原理上就给不了你要的东西，必须换笛卡尔空间插值。

3.2 笛卡尔直线为什么要靠数值逆解兜底

笛卡尔空间插值用ctraj，它在两个位姿之间做位姿插值（位置线性插值，姿态用四元数或旋转矩阵插值），生成一串末端位姿，再逐帧反解成关节角。

T0 = ur5.fkine(q0); T1 = transl([0.4, -0.2, 0.5]) * trotx(pi); % 目标位姿，位置 + 姿态 tc = linspace(0, 1, 601); % 归一化时间，601 帧 Tc = ctraj(T0, T1, tc); q = zeros(numel(tc), 6); q(1, :) = q0; for k = 2:numel(tc) q(k, :) = ur5.ikine(Tc(:, :, k), 'q0', q(k-1, :), ... 'mask', [1 1 1 1 1 1], 'tol', 1e-6, 'ilimit', 200); end % 数值微分求关节速度（rad/s），采样周期 0.005 s qd_num = diff(q) / 0.005;

这段代码有两个关键设计，都是踩过坑之后才加上的。

第一，必须用循环，每帧把上一帧的解q(k-1,:)作为下一帧的种子q0。如果图省事写ur5.ikine(Tc, 'q0', q0, ...)一次性把所有位姿丢进去，ikine会对每一帧独立求解，而六自由度机械臂在大多数位姿下有 8 组解，独立求解很可能第 1 帧选一组、第 2 帧选另一组，关节角直接跳 180°。这就是后面 4.1 节要说的"跳解"。

第二，用ikine而不是ikine6s。ikine6s是解析解，速度快、结果稳定，但它对机械臂构型有严格要求，需要满足特定的腕部几何条件，而且得指定'ru'、'lu'、'rn'、'ln'之类的构型字符串。UR5 的腕部有偏置，直接套ikine6s经常报奇异或返回明显不合理的解。稳妥做法是：先试ikine6s，能跑通且结果连续就用它（快得多），跑不通或者出现跳变就退回ikine。我自己的习惯是直接用ikine加逐帧种子，慢一点但省心。

'mask'参数告诉求解器要对哪几个自由度做约束，[1 1 1 1 1 1]就是六自由度全约束。如果只关心位置不关心姿态（比如做抓取前的接近动作），可以设成[1 1 1 0 0 0]，求解会快很多，但要注意这时候返回的姿态是随机的，不能直接拿去下发。

'tol'是迭代收敛容差，'ilimit'是最大迭代次数。这两个值调小了可能不收敛，调大了耗时。tol=1e-6、ilimit=200在 UR5 这个尺度上是比较合适的起点。

3.3 用速度上限反推总时长

轨迹生成完之后，第一件事不是画图，是查速度。五次多项式虽然平滑，但它不保证速度在你给定的 3 秒内不超限。

% 各关节额定速度上限，按实际值填，这里取常见量级 qd_lim = deg2rad([150, 150, 180, 180, 180, 180]); scale = max(abs(qd) ./ qd_lim, [], 'all'); if scale > 1 t = t * scale; % 整体拉长时间 [q, qd, qdd] = jtraj(q0, qf, t); % 重新生成 fprintf('时长放大 %.2f 倍以适配速度上限\n', scale); end

逻辑很简单：算出每个关节实际最大速度占额定速度的比例，取所有关节里的最大值，如果超过 1，就把总时长按这个比例放大，然后重新生成轨迹。因为是同一条五次多项式曲线在时间上做缩放，速度整体降到原来的 1/scale，加速度降到 1/scale²，都落在安全范围内。

这里有个细节值得说：为什么要整体缩放而不是单独给每个关节分配时间？因为jtraj生成的是一条同步轨迹，六个关节共享同一个时间轴。如果单独缩放某个关节的时间，六个关节就对不齐了，末端轨迹会变形。整体缩放是唯一安全的方式，代价是动作变慢——但对 UR5 这种协作臂来说，慢一点总比超速报错强。

scale算出来之后，别忘了同时对笛卡尔轨迹做同样的处理。笛卡尔轨迹的时间缩放不能用这个方法，得反过来：先按目标线速度v算出总时长L/v（L是路径长度），生成轨迹，再查关节速度，超了就把v降下来重算。这是个迭代过程，一般两三轮就能收敛。

3.4 采样周期定多少：5 ms 还是 10 ms

采样周期这个参数看起来只是"数据密一点疏一点"，实际上直接影响三件事：动画流畅度、下发时的插补质量、以及逆解能不能收敛。

5 ms（200 Hz）是我最常用的值。原因是大多数工业控制器的轨迹插补周期在 4～8 ms 这个量级，5 ms 的轨迹点密度差不多正好对齐，控制器拿到之后做线性插补，误差可以忽略。10 ms（100 Hz）也够用，轨迹点少一半，MATLAB 跑得快，但在曲率大的地方（比如圆弧过渡段）线性插补的弦高误差会明显一些。

% 两种周期的对比：同一段直线轨迹，看最大弦高误差 for dt = [0.005, 0.010, 0.020] tc = 0:dt:3; Tc = ctraj(T0, T1, tc / 3); p = squeeze(transl(Tc))'; % 每行一个位置 % 相邻点连线与真实直线的最远偏离（弦高） chord = max(vecnorm(p - (p(1,:) + (p(end,:)-p(1,:)) .* ... ((p-p(1,:))*(p(end,:)-p(1,:))') ./ norm(p(end,:)-p(1,:))^2, 2))); fprintf('dt=%.3f s, 最大弦高 %.4f mm\n', dt, chord * 1000); end

20 ms 的时候弦高误差已经到毫米级了，做精密装配肯定不行，做搬运上下料勉强能接受。判断标准其实就一条：你的工艺允许多大的轨迹偏差。搬运类任务几毫米无所谓，涂胶、焊接这类就跟不上了。

还有一个反向的约束：采样周期不能太密到 MATLAB 算不过来。ikine单次求解在普通笔记本上大概几毫秒，600 帧的循环要跑好几秒。如果你想做实时可视化，帧率跟不上，动画看起来一顿一顿的，那不是算法问题，是animate里每帧的pause在拖后腿。

4. 避坑排查：五类让仿真结果不能上机的现场

这一节全是实际跑出来的问题，每条按"现象—原因—解决"写。

4.1 逆解跳解：相邻帧关节角突变 2π

现象：画关节角曲线，某一帧某个关节从 -170° 直接跳到 +170°，或者反过来。末端轨迹看起来是连续的（因为 ±180° 在物理上是同一个位置），但下发到控制器会触发"关节超程"或者让机械臂猛地转一圈。

原因：数值逆解每次独立求解时，会从多组解里挑离种子最近的那组。如果种子是固定的初始q0，随着末端走远，最近的解会切换到另一支。ikine内部用四元数处理姿态，返回的角度默认落在 (-π, π]，跨过边界时就会跳。

解决：三件事一起做。第一，逐帧用上一帧解做种子（3.2 节的循环写法），这是最根本的。第二，算完之后做一次解缠（unwrap），把跳变补回来：

q_unwrap = unwrap(q, [], 1); % 沿时间维解缠，消除 ±2π 跳变 jump = max(abs(diff(q_unwrap)), [], 'all'); fprintf('解缠后最大帧间跳变 %.4f rad\n', jump); if jump > 0.5 warning('仍有大跳变，检查是否发生构型切换'); end

第三，解缠之后要重新检查限位。解缠可能把某个关节推到 ±2π 之外，而真实机械臂走不到那儿。这种情况说明起点和终点的关节角配置本身就不适合走直线，得换路点或者换构型。

unwrap只处理 2π 的整数倍跳变，处理不了构型切换（比如肘部从"上肘"翻到"下肘"），那种跳变幅度不规律，只能靠人工看曲线。

4.2 奇异位形附近关节速度爆表

现象：笛卡尔直线轨迹跑到中段，某个关节速度突然冲到几十甚至上百 rad/s，但位置曲线看着完全正常。

原因：末端接近奇异位形，雅可比矩阵降秩，逆运动学求解需要无穷大的关节速度才能维持给定的末端速度。这在腕部奇异（关节 5 接近 0，关节 4 和 6 轴线共线）和肩部奇异（腕心落在关节 1 轴线上）附近特别常见。

解决：在生成轨迹的过程中实时监控可操作度，超阈值就报警或改道：

w = zeros(size(q, 1), 1); for k = 1:size(q, 1) J = ur5.jacob0(q(k, :)); w(k) = sqrt(det(J * J')); % Yoshikawa 可操作度 end bad = find(w < 1e-3); if ~isempty(bad) fprintf('第 %d 帧附近接近奇异，可操作度 %.2e\n', bad(1), w(bad(1))); end

可操作度的量纲不直观，实践里更好用的是条件数cond(J)：小于 10 很健康，10～100 要留意，超过 100 基本就不能指望直线精度了。检测到接近奇异时，工程上的处理方式无非三种——绕开（改路点，让轨迹从奇异点旁边过去）、降速（速度降下来，虽然速度比还是差，但绝对误差可控）、换空间（这一段改用关节空间插值，牺牲直线性换可行性）。选哪种看工艺要求，没有通用答案。

4.3 单位混用与 DH 约定混用

现象：正解位置差了 57 倍，或者差了将近一倍。

原因：57 倍是度与弧度的换算系数（180/π ≈ 57.3），说明某处该用弧度的地方填了度。差近一倍则是 DH 约定混用，或者a的符号抄反了。

解决：在代码最开头统一单位，所有入口都用deg2rad转换，中间过程一律用弧度，只在显示时转回度。DH 约定方面，判断方法见 2.3 节的检查点 1——看全零位末端到基座的距离量级对不对。另外Link的qlim参数也吃弧度，见过有人在qlim里填[-180 180]，结果plot的时候关节臂画得乱七八糟。

养成一个习惯：所有和角度有关的变量名带上_deg或_rad后缀。听起来很啰嗦，但真出问题时能省半小时。

4.4 工具坐标系漏设与轨迹整体偏移

现象：仿真里末端走得完美，实机上末端总是偏一个固定距离，方向沿着工具轴。

原因：ur5.tool没设，仿真算的是法兰中心，实机上控制器算的是 TCP。两者差了整个工具的长度。

解决：按 2.4 节把tool补上，补完之后重新跑 2.3 的零位自检。补tool之后还有一个连带影响：ikine求解的目标变成了 TCP 位姿，原来能收敛的位姿可能收敛不了，因为 TCP 的可达空间比法兰小一圈。所以补tool之后要把整条轨迹重新跑一遍，检查所有路点是否还在可达空间内。

验证方法很直接：把ur5.tool设成零和设成实际值，分别对同一组关节角做fkine，两个结果的位置差应该正好等于工具长度。

4.5 结果不可复现：随机种子与动画阻塞

现象：同样的代码跑两遍，关节角序列不一样；或者加了animate之后，轨迹数据本身没变但耗时长了十倍。

原因：两件事。一是ikine在某些版本的实现里会用到随机初值（尤其是q0没给或者给了但解空间多解的时候），不固定种子结果就有波动。二是animate默认每帧有pause，600 帧下来光等待就得好几秒，如果放在循环里做求解，整体耗时会被拖垮。

解决：求解阶段关掉动画，数据全部算完存成矩阵，再单独拿出来做可视化。

% 求解阶段：纯数值，无绘图 q = solveCartesianTrack(ur5, Tc, q0); % 可视化阶段：数据已固定，动画只是回放 figure; ur5.plot(q(1, :)); for k = 2:size(q, 1) ur5.animate(q(k, :)); drawnow limitrate; % 比 pause 更可控 end

drawnow limitrate比裸pause好在它会自动跳帧，不会因为绘图慢而拖累循环。如果还是卡，就隔几十帧画一次。

另外一个可复现性的细节：ikine的q0种子每次都要显式传，别依赖默认值。默认值在不同版本的工具箱里可能不一样，代码换个环境跑结果就变了。

5. 上机前的体检：雅可比、可操作度与限位表

轨迹生成完、坑也排完了，还不能直接下发。这一节是最后一道闸，四件事做完，数据才敢往控制器上送。

5.1 雅可比不是给你看的，是给奇异判据用的

雅可比矩阵J描述的是关节速度到末端速度的线性映射：v = J·qd。对六自由度机械臂，jacob0返回的是 6×6 矩阵，前三行对应线速度，后三行对应角速度，都在基座坐标系下表达。

q_mid = q(round(size(q,1)/2), :); J = ur5.jacob0(q_mid); disp(J); fprintf('条件数 %.2f\n', cond(J)); fprintf('可操作度 %.4e\n', sqrt(det(J * J')));

这个矩阵本身没什么可"看"的，一屏数字看不出门道。它的价值全在派生指标上：cond(J)反映速度放大的倍数，sqrt(det(J*J'))反映离奇异的远近。前者更直观，因为它直接告诉你"末端速度 1 m/s 时，某个关节可能要跑 cond 倍"。

还有一个用法是检查方向性：对J做奇异值分解，最小奇异值对应的方向就是最"费劲"的方向。如果某段轨迹恰好在那个方向上运动，速度会异常大。这个检查比事后看速度曲线更早发现问题。

5.2 可操作度与条件数的阈值怎么定

阈值没有绝对标准，取决于机械臂尺度和任务精度。对 UR5 这种中小型臂，我用的经验值如下表。

w_all = zeros(size(q, 1), 1); c_all = zeros(size(q, 1), 1); for k = 1:size(q, 1) J = ur5.jacob0(q(k, :)); w_all(k) = sqrt(det(J * J')); c_all(k) = cond(J); end fprintf('可操作度最小 %.2e @ 第 %d 帧\n', min(w_all), find(w_all == min(w_all), 1)); fprintf('条件数最大 %.1f @ 第 %d 帧\n', max(c_all), find(c_all == max(c_all), 1));

这两个数要一起看。可操作度是绝对值，跟机械臂尺度有关，换一台臂就不能直接比；条件数是相对值，跨构型可比性更好。我的做法是用条件数做主判据，可操作度做交叉验证，两个都报警才认为是真奇异，只有一个报警可能只是某一段的局部退化。

5.3 关节限位与速度限位的两道闸

位置限位和速度限位必须分开查，不能只查一个。

% 位置限位：逐关节对比 qlim qlim = vertcat(ur5.qlim); % 6x2，每行 [min max] viol_pos = (q < qlim(:,1)') | (q > qlim(:,2)'); if any(viol_pos, 'all') [k, j] = find(viol_pos, 1); fprintf('第 %d 帧关节 %d 位置超限：%.3f rad\n', k, j, q(k, j)); end % 速度限位：数值微分的关节速度 qd_num = [zeros(1,6); diff(q) / 0.005]; viol_vel = abs(qd_num) > qd_lim; if any(viol_vel, 'all') [k, j] = find(viol_vel, 1); fprintf('第 %d 帧关节 %d 速度超限：%.3f rad/s\n', k, j, qd_num(k, j)); end % 加速度限位：二次微分，检查有没有冲击 qdd_num = [zeros(1,6); diff(qd_num) / 0.005]; fprintf('最大关节加速度 %.2f rad/s^2\n', max(abs(qdd_num), [], 'all'));

位置超限通常说明路点选得不对，得换个目标位姿。速度超限按 3.3 节缩放时长。加速度超限是最容易被忽略的——位置和速度都合规，但加速度尖峰很大，实机上表现为明显的抖动和异响，长期跑会伤减速器。看到加速度超限，说明轨迹在某个点有突变，回去查是不是ctraj和ikine之间出现了不连续。

数值微分这一步要小心：diff会损失一帧，上面用补零的方式对齐。如果轨迹本身有噪声，二次微分会把噪声放大得很厉害，这时候更适合用jtraj直接给出的qdd，而不是数值微分。

5.4 导出成控制器能吃的关节序列

最后一步是导出。别小看这一步，列顺序、单位、时间戳这三样任何一样错了，控制器那边就是一堆乱动作。

% 导出格式：时间(s) + 六个关节角(rad) + 六个关节速度(rad/s) M = [t(:), q, qd_num]; fid = fopen('ur5_traj.csv', 'w'); fprintf(fid, '# 单位: 时间 s, 关节角 rad, 关节速度 rad/s\n'); fprintf(fid, '# 列序: t, q1..q6, qd1..qd6\n'); fprintf(fid, '# 采样周期: %.4f s, 总帧数: %d\n', 0.005, size(M,1)); fclose(fid); writematrix(M, 'ur5_traj.csv', 'WriteMode', 'append');

文件头写清单位和列序，这不是形式主义。过两个月你自己回头看这个文件，没注释的话还得靠数一数有几列来猜。团队协作时更是如此，别人拿到文件第一件事就是找注释。

如果控制器吃的是角度而不是弧度，导出前转换，别指望控制器帮你转。有些控制器接受度但内部按弧度算，转换环节藏在某个配置项里，出问题时极难定位。

时间戳这一列也有讲究：有些控制器不需要时间列，自己按固定周期插补；有些需要。给的时候用相对时间（从 0 开始），别用绝对时间戳，时区问题能让人抓狂。

6. 收在一套可复用的封装与自检习惯上

前面五节的东西散在各处，真用起来我一般会封成一个函数，输入起点关节角、终点位姿、采样周期、速度上限，输出关节序列和一份自检报告。函数骨架大概是这样：

function [q, report] = planCartesianTrack(ur5, q0, T1, dt, qd_lim) % 1. 生成笛卡尔轨迹 T0 = ur5.fkine(q0); tc = 0:dt:estimateDuration(T0, T1, 0.1); Tc = ctraj(T0, T1, tc / tc(end)); % 2. 逐帧数值逆解，上一帧做种子 q = zeros(numel(tc), 6); q(1, :) = q0; for k = 2:numel(tc) q(k, :) = ur5.ikine(Tc(:,:,k), 'q0', q(k-1,:), ... 'mask', [1 1 1 1 1 1], 'tol', 1e-6, 'ilimit', 200); end q = unwrap(q, [], 1); % 3. 速度检查与时长缩放 qd = [zeros(1,6); diff(q) / dt]; scale = max(abs(qd) ./ qd_lim, [], 'all'); if scale > 1 tc = tc * scale; Tc = ctraj(T0, T1, tc / tc(end)); q = zeros(numel(tc), 6); q(1, :) = q0; for k = 2:numel(tc) q(k, :) = ur5.ikine(Tc(:,:,k), 'q0', q(k-1,:), ... 'mask', [1 1 1 1 1 1]); end q = unwrap(q, [], 1); end % 4. 体检报告 report = struct(); report.duration = tc(end); report.jump_max = max(abs(diff(q)), [], 'all'); report.cond_max = max(arrayfun(@(k) cond(ur5.jacob0(q(k,:))), 1:size(q,1))); report.vel_max = max(abs([zeros(1,6); diff(q)/dt]), [], 'all'); report.limit_viol = any(q < vertcat(ur5.qlim)(:,1)', 'all') || ... any(q > vertcat(ur5.qlim)(:,2)', 'all'); end

这个封装的价值不在于省几行代码，而在于把"体检"这一步强制化。以前我经常是先把轨迹跑出来、看着动画顺眼就导出了，速度超限、接近奇异这些问题要等到实机上才暴露。吃过两次亏之后，现在所有轨迹规划都强制走一遍report，四项指标——帧间跳变、最大条件数、最大关节速度、限位越界——任何一项不合格就不往下走。

这套习惯里最值钱的是jump_max这一项。它抓的是 4.1 节的跳解问题，而且是从结果反推的：不管中间哪个环节出了问题，只要最终的关节序列有超过 0.5 rad 的帧间跳变，就一定有问题。这比逐段检查逆解收敛情况省事得多，算是整个流程里的"后悔药"。

另外补一句关于可达性的判断。ikine不收敛的时候不一定报错，有时候返回一个明显跑偏的解，位置误差几百毫米。所以在循环里加一句误差回检：

T_actual = ur5.fkine(q(k, :)); err = norm(transl(T_actual) - transl(Tc(:,:,k))); if err > 1e-3 warning('第 %d 帧逆解误差 %.2f mm，检查可达性', k, err * 1000); end

三毫米以上的误差基本就可以判定不可达或者求解失败了。这个检查每帧多花不到一毫秒，但能省掉大量"轨迹看着对、上机不走"的排查时间。

最后说个容易忽略的事：这些代码跑出来的结果，是在你建的模型上成立的。模型和真机之间有装配误差、连杆实际长度误差、零点偏移，这些在 MATLAB 里都体现不出来。所以第一版轨迹下发之前，一定要用很低的速度（比如额定的 10%）空跑一遍，看着实际末端走的位置和仿真差多少。差得多的话，问题不在轨迹算法，在标定。这一步没有捷径，只能实机验。

希望帮到你。

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