简介：这份资料围绕UR5六轴机械臂在MATLAB环境下的轨迹规划展开，面向机器人工程、自动化控制方向的学习者与实验开发者，侧重解决从运动学、动力学建模到平滑路径生成的完整实践问题。内容涉及正逆运动学求解、雅可比计算、欧拉角变换、关节空间与任务空间轨迹设计，并触及碰撞检测与速度加速度约束等环节，适合具备一定MATLAB基础、希望深入理解机械臂控制流程的中高级读者。压缩包共29个文件，约859KB，以21个m脚本和4个mat数据文件为主体，辅以ttt项目说明、md文档、pdf报告及dll动态库，便于按模块查阅与复现。已有5605人学习下载。读者可从中获得运动学与动力学计算的代码实现思路、轨迹插值及路径规划算法的组织方式，并借助报告与脚本理解UR5建模、仿真验证和参数调试的完整链路。

1. 从一条抖动的直线说起：UR5 在 MATLAB 里到底要规划什么

有人在 MATLAB 里给 UR5 规划一条 200 mm 的直线，仿真里末端走得笔直，把关节角序列丢给控制器，机械臂走到中段突然一顿，末端偏移肉眼可见。这类问题的根子通常不在控制器，而在轨迹规划那一段：采样太稀、逆解跳了分支，或者路径擦着奇异位形走过去，关节速度被放大好几倍。MATLAB 下的 UR5 轨迹规划，本质上就三件事——把运动学模型建对、把插补算法选对、把约束校核做全。这套流程适合刚接手六轴臂、手上只有一台 UR5 和一份 MATLAB 授权的人，也适合仿真跑通了但实机总出岔子的熟手。下面按建模、关节插补、笛卡尔插补、排错、落地验证的顺序拆，代码可以直接抄。

2. 运动学底座：UR5 的 DH 参数表与刚体树搭建

轨迹规划的精度上限由运动学模型决定。UR5 是六个旋转关节串联、球腕结构的工业臂，标称负载 5 kg、工作半径 850 mm，关节行程 ±360°，标称最大关节速度在 π rad/s 量级。这些规格决定了后面所有插补参数的天花板：单关节从 0 转到 180° 最快也要 1 s，任何规划出来的关节速度只要超过这个数，实机就会限幅、滞后，末端轨迹跟着变形。所以先别急着上 jtraj，把模型和正逆解的误差压到 1e-9 量级，后面的坑能少一半。

我一般把这个阶段拆成三步：写死 DH 参数、在刚体树里搭出来、正逆解互验。任何一步没闭环，后面插补出来的曲线都不可信。

2.1 把 DH 参数钉死：六组 a、d、alpha

UR5 的标准 DH 参数是公开的，但抄的时候最容易错在符号和单位上。a 是连杆长度，用米；d 是连杆偏距，也用米；alpha 是连杆扭角，用弧度。下面这张表建议直接存成脚本里的常量，不要在多个文件里各写一份。

注意第二、三关节的 a 是负值。这个负号决定了肘部朝向，写错了正解出来的末端位置会整体镜像，逆解收敛到一个完全不同的构型，而仿真里看不出问题，因为末端位姿可能碰巧对得上——这是最阴的一类错。

先写一个最小可用的 DH 变换函数，后面所有验证都靠它：

function T = dhT(a, alpha, d, theta) % 标准DH单连杆变换：Rot_z(theta)*Trans_z(d)*Trans_x(a)*Rot_x(alpha) ct = cos(theta); st = sin(theta); ca = cos(alpha); sa = sin(alpha); T = [ct, -st*ca, st*sa, a*ct; st, ct*ca, -ct*sa, a*st; 0, sa, ca, d; 0, 0, 0, 1]; end

这个函数把四个基本变换乘在一起，theta 传 0 的时候得到的就是“与关节变量无关的那部分固定变换”，也就是刚体树里需要写进 joint 的固定位姿。参数说明：a、d 单位必须是米，alpha 必须是弧度；如果手上拿到的是角度制数据，先乘 pi/180，别指望工具箱帮你猜。

2.2 用刚体树把 UR5 搭起来

MATLAB 自带 Robotics System Toolbox 的rigidBodyTree，社区里那套开源机器人工具箱也能做同样的事，两者思路一致：把每个连杆挂到父连杆上，关节类型设为 revolute，转动轴默认沿 z，关节变量由求解器填。关键点在于setFixedTransform只吃“关节到父连杆”的固定部分，theta 要留给关节变量。

% UR5 标准DH参数，单位：米 / 弧度 a = [0, -0.425, -0.39225, 0, 0, 0]; d = [0.089159, 0, 0, 0.10915, 0.09465, 0.0823]; alpha = [pi/2, 0, 0, pi/2, -pi/2, 0]; robot = rigidBodyTree('DataFormat', 'column', 'MaxNumBodies', 7); parentName = 'base'; for i = 1:6 body = rigidBody(sprintf('link%d', i)); jnt = rigidBodyJoint(sprintf('joint%d', i), 'revolute'); % 把DH里与theta无关的固定部分写进关节 setFixedTransform(jnt, dhT(a(i), alpha(i), d(i), 0)); % 关节限位，UR5 标称 ±360° jnt.PositionLimits = [-2*pi, 2*pi]; body.Joint = jnt; addBody(robot, body, parentName); parentName = sprintf('link%d', i); end

逻辑说明：循环里每一轮新建一个 body 和一个 revolute 关节，把 DH 固定变换挂上去，再把 body 挂到上一级。父连杆名从base开始，逐级替换成link1…link5，最后形成 base→link1→…→link6 的链。参数上唯一要改的是PositionLimits，如果项目里用的是限位收窄的机型，把 ±2pi 换成实际值，否则后面优化出来的轨迹可能跑到实机动不了的角度上。

建完先看一眼结构是否正常：

show(robot, homeConfiguration(robot), 'Frames', 'off', 'PreservePlot', false);

如果画出来的臂形是“立起来、肘部朝前”的常态位，说明 DH 抄对了；如果整条臂横着躺或者折向反方向，回去查第二、三关节 a 的符号。

2.3 正逆解互验：别信第一次跑通的 ikine

刚体树建好之后，正解直接调getTransform或fkine就行，但我不建议只信工具箱。用一个手写的矩阵连乘做交叉验证，两边误差应该在小数点后十几位，差出 1e-3 以上就说明 DH 或父子关系有问题。

q = [0.3; -1.2; 1.0; -1.4; -1.57; 0.2]; % 任意一组测试关节角 % 路径一：工具箱正解 T_tool = getTransform(robot, q, 'link6'); % 路径二：手写连乘 T_manual = eye(4); for i = 1:6 T_manual = T_manual * dhT(a(i), alpha(i), d(i), q(i)); end err = norm(T_tool(1:3,4) - T_manual(1:3,4)); fprintf('末端位置差 %.3e m\n', err); % 期望 < 1e-12

正解对上了，再验逆解。用逆解对象时，权重向量决定求解器更在意位置还是姿态，六维全给 1 是最朴素的设法；如果做抓取对准，位置权重可以给到 2、姿态给 1，反过来做姿态对齐就调个头。

ik = inverseKinematics('RigidBodyTree', robot, 'SolverParameters', ... struct('MaxIterations', 2000, 'GradientTolerance', 1e-8)); weights = [1 1 1 1 1 1]; q0 = homeConfiguration(robot); [qSol, info] = ik('link6', T_manual, weights, q0); fprintf('迭代状态: %s, 用时 %.1f ms\n', info.Status, info.Iterations);

这里有个必须做的动作：把qSol代回正解，和T_manual比一次。数值逆解在奇异位形附近会给出一个“看起来收敛了、其实差一截”的结果，info.Status显示成功也不代表位姿对得上。我的习惯是加一条硬断言，位置误差超过 1e-6 m 或姿态误差超过 1e-6 rad 就报错停下，别让它带着脏数据往下一阶段走。

2.4 雅可比与奇异位形预判

轨迹规划的很多“玄学抖动”，追到根上都是雅可比的条件数爆了。UR5 的腕部奇异出现在第五关节接近 0 的时候，肩部和肘部奇异出现在腕心落到第一关节轴线附近或臂完全伸直的时候。工程上不会去推导解析条件，直接在采样点上算雅可比、看最小奇异值。

J = geometricJacobian(robot, q, 'link6'); % 6x6 s = svd(J); fprintf('最小奇异值 %.4f, 条件数 %.1f\n', min(s), max(s)/min(s)); % 常见阈值：最小奇异值低于 0.05 认为进入危险区 if min(s) < 0.05 warning('当前构型接近奇异，末端速度会被放大 %.1f 倍', 1/min(s)); end

阈值 0.05 不是标准答案，是经验值。负载重、减速比小的场合要更保守，我给到 0.08 也常见。真正要记住的是：这个检查必须对整条轨迹逐点做，而不是只看起点终点。一条直线从工作空间中间穿过去，中间某一帧的雅可比可能已经退化，端点却毫无征兆。

3. 关节空间插补：五次多项式与梯形速度的取舍

关节空间插补是 UR5 最常用的模式，控制器接的也是关节角序列。它不保证末端走直线，但保证每个关节的速度、加速度连续，起停柔顺。选插补曲线的核心就一条：加速度不能突变，否则减速机承受的冲击力矩会有尖峰，长期跑下来异响和背隙都会来。

3.1 五次多项式为什么是默认解

三次多项式能保证位置和速度连续，加速度是线性变化的，在起点和终点处会有阶跃。对 UR5 这种谐波减速器加伺服的结构，加速度阶跃意味着力矩阶跃，实机上表现为起停那一下“咯”的一声。五次多项式多给两个约束，把起点和终点的加速度也钉成零，代价是曲线更“绕”一点，中间段速度峰值略高。

边界条件写成：t=0 时 q=q0、qd=0、qdd=0；t=T 时 q=qf、qd=0、qdd=0。解出来只有 a3、a4、a5 三个非零系数，形式很干净：

q(t) = q0 + 10·(qf-q0)·(t/T)³ - 15·(qf-q0)·(t/T)⁴ + 6·(qf-q0)·(t/T)⁵

由此得到速度峰值出现在 t=T/2，大小是 1.875·(qf-q0)/T；加速度峰值出现在 t≈0.211T 和 0.789T，大小是 5.773·(qf-q0)/T²。这两个系数是做时间估算的关键——想反推 T，直接拿这两条式子解就行，不用跑仿真。

3.2 手写系数求解与工具箱函数对照

工具箱里的jtraj做的是同样的五次多项式，还带初末速度参数。但项目里经常需要把多项式系数导出去给别的模块，所以我一般手写一遍，再和jtraj对一次，两边一致才放心用。

function [q, qd, qdd] = quinticTraj(q0, qf, T, dt) % 五次多项式插补，起末速度、加速度均为零 t = (0:dt:T)'; s = t / T; % 归一化多项式基 h = 10*s.^3 - 15*s.^4 + 6*s.^5; hd = (30*s.^2 - 60*s.^3 + 30*s.^4) / T; hdd= (60*s - 180*s.^2 + 120*s.^3) / T^2; dq = qf - q0; % 6x1 q = q0' + h * dq'; % N x 6 qd = hd * dq'; qdd = hdd * dq'; end

逻辑说明：把时间归一化成 s∈[0,1]，位置、速度、加速度各写一个基函数，最后乘上关节增量。这么做的好处是系数物理意义清楚——h 从 0 走到 1，hd 和 hdd 在两端都是零。参数方面，T是总时长，dt是采样周期，q0、qf是 6×1 列向量。返回的 q 是 N×6，直接喂给show做动画或者存成 CSV 都行。

对照验证：

dt = 0.008; T = 2.0; q0 = homeConfiguration(robot); qf = q0 + [0.5; -0.4; 0.3; -0.2; 0.4; 0.3]; [q1, qd1, qdd1] = quinticTraj(q0, qf, T, dt); [q2, qd2, qdd2] = jtraj(q0, qf, 0:dt:T); fprintf('位置最大偏差 %.3e\n', max(abs(q1 - q2), [], 'all')); fprintf('速度最大偏差 %.3e\n', max(abs(qd1 - qd2), [], 'all'));

两者应该完全重合。如果差出量级，多半是jtraj的输入时间向量你少给了末点，或者 q0、qf 的转置搞混了。

3.3 梯形速度：什么时候该换 LSPB

五次多项式的问题是速度曲线是“驼峰形”，全程都在变，中段速度高、两端速度低。做长距离搬运、希望尽可能快地走过去的时候，梯形速度（LSPB）更合适：加速段匀加速、中间匀速、末段匀减速，速度峰值低，走同样的距离耗时更短或者同样耗时力矩更小。

代价是加速度在三个切换点上有阶跃，实机上会有轻微冲击。折中做法是用带抛物线过渡的梯形（lspb就是这个），把加速度阶跃改成线性过渡，冲击小很多。

% 带抛物线过渡的梯形速度，指定采样时间与总时长 t = 0:dt:T; [qTrap, qdTrap, qddTrap] = lspb(q0, qf, t); % 或直接指定最大速度，让函数自己算时长 vmax = 0.5; % rad/s t2 = 0:dt:1.5*T; [qTrap2, qdTrap2] = lspb(q0, qf, t2, vmax);

参数上要盯的是vmax：传给lspb的如果是标量，所有关节共用一个速度上限；如果是 6×1 向量，就按关节分别限速。UR5 的腕部三个关节转动惯量小、允许速度高，但第一、二关节带整条臂，实际能跑的角速度比标称值低不少，我一般按标称的 60%~70% 给。

选型上有个简单判据：起停频繁、负载接近额定、对轨迹形状没要求，用五次多项式；长距离、节拍要求紧、末端不要求走直线，用 LSPB。两者不要混着用在同一条轨迹的不同段上，除非你能保证切换点的速度和加速度真的连续——这一点后面排错章还会提。

3.4 采样周期与速度加速度校核

UR5 控制器的插补周期通常在毫秒级（常见 0.008 s 或 0.002 s），MATLAB 侧的规划采样最好取成控制器周期的整数倍，否则下发给控制器时还得二次插值，白白引入误差。我一般直接用 0.008 s，和控制器对齐。

规划完必须做一次数值校核，别只信解析式：

dt = 0.008; qd_num = [zeros(1,6); diff(q1)] / dt; % 前向差分 qdd_num = [zeros(1,6); diff(qd_num)] / dt; qdotMax = [pi; pi; pi; pi; pi; pi] * 0.7; % 留30%余量 qddMax = [8; 8; 8; 8; 12; 12]; % rad/s^2，经验上限 fprintf('关节速度峰值: '); disp(max(abs(qd_num), [], 1)); fprintf('关节加速度峰值: '); disp(max(abs(qdd_num), [], 1)); if any(max(abs(qd_num), [], 1) > qdotMax') warning('存在关节超速，需要放大总时长 T'); end

这里用前向差分而不是中心差分，是因为控制器实际执行时也是拿相邻两帧做差分，用中心差分算出来的峰值会偏低，容易漏判。加速度上限qddMax没有统一定值，和负载、安装姿态、减速机型号都有关，8 rad/s² 是空载下的保守值，带载后建议压到 5 以下再测一轮。

4. 笛卡尔空间轨迹：位姿插补与逆解连续性

需要末端走直线、走圆弧，或者做恒定姿态的贴合动作时，关节空间插补就不够用了，得在笛卡尔空间插补位姿，再逐点逆解成关节角。麻烦的地方全在逆解上：UR5 一个末端位姿对应八组解，相邻两帧如果选到不同分支，关节角会突变上百度和，实机上就是一次急停。

4.1 位姿插补：四元数 slerp 而不是欧拉角线性插值

姿态插值最省事的写法是把欧拉角拿出来做线性插值，RPY 三个角各插各的。这种做法在俯仰角接近 ±90° 时会出问题，中间姿态可能绕远路，或者干脆翻转。正确做法是用齐次矩阵的测地线插值，内部走四元数的球面线性插值，MATLAB 自带transformtraj做的就是这件事。

T0 = getTransform(robot, q0, 'link6'); Tf = T0; Tf(1:3,4) = T0(1:3,4) + [0.15; 0.10; -0.05]; % 末端平移 15/10/-5 cm % Tf(1:3,1:3) 保持不变，做纯平移；要改姿态就乘一个旋转矩阵 tSamples = 0:dt:T; trajCart = transformtraj(T0, Tf, [0 T], tSamples, ... 'TimeScaling', [0 0 1 1]); % 起末速度为零

逻辑说明：transformtraj的前两个参数是起末位姿，第三个是时间区间，第四个是采样时间向量，TimeScaling用 [0 0 1 1] 表示起末速度为零，插值曲线平滑起停。返回的是一个 4×4×N 的三维数组，逐页取出来就是每一帧的目标位姿。

这里默认走的是空间直线——末端位置沿直线匀加速-匀减速过去。要画圆弧，就换成绕固定轴旋转的插值，用罗德里格斯公式逐点生成位姿，再丢给同一个逆解流程。

4.2 逆解分支连续性：上一点作初值加关节距离选优

逐帧逆解时，最忌讳每次都用homeConfiguration当初值。正确做法是把上一帧的解当成这一帧的初值，这样求解器会收敛到同一分支上的最近解，绝大多数情况下关节角的变化是连续的。

但光靠初值还不够。某些位形下，即使初值给得对，数值解也可能跳到另一支，尤其是路径经过腕部奇异附近。我的做法是每次求解时多跑几组初值，再按“与上一帧关节角距离最小”挑一个：

function qBest = solveIKNearest(ikObj, tform, qRef, weights) % 多初值逆解 + 关节空间最近解选优 n = numel(qRef); qCand = zeros(n, 0); % 初值集合：上一帧解 + 各关节 ±0.3 rad 扰动 q0List = [qRef, qRef + 0.3*eye(n), qRef - 0.3*eye(n)]; for k = 1:size(q0List, 2) [qk, info] = ikObj('link6', tform, weights, q0List(:, k)); if strcmp(info.Status, 'success') || strcmp(info.Status, 'joint-limit') qCand = [qCand, qk]; %#ok<AGROW> end end if isempty(qCand) error('所有初值均未收敛，检查目标位姿是否可达'); end % 角度差归一化到 [-pi, pi)，避免 ±pi 处的假跳变 dif = mod(qCand - qRef + pi, 2*pi) - pi; [~, idx] = min(sum(dif.^2, 1)); qBest = qRef + dif(:, idx); end

逻辑说明：q0List的第一列是上一帧解，后面 12 列是每个关节单独扰动 ±0.3 rad 的初值，覆盖邻近的几个解支。求解结果按关节空间的欧氏距离排序，取最近的那个。角度差用mod归一化是关键一步，不然 -pi 和 +pi 会被判成相差 2pi，选出一个明明更远的解。

参数上，扰动幅度 0.3 rad 是折中值：太小覆盖不到相邻分支，太大会收敛到很远的构型。如果某条轨迹的关节角变化本来就大，可以放到 0.5。另外注意info.Status除了success，joint-limit也要接受——有时候最优解正好卡在限位上，直接丢掉会导致整条轨迹断掉。

4.3 直线轨迹的速度重规划与导出

笛卡尔空间等距采样，逆解出来的关节角在时间上不一定均匀。末端匀速走直线时，靠近工作空间边缘的那一段关节角速度会明显偏高。所以逆解完之后必须做一次时间重规划：算出每个关节的实际速度，找出超限比例，把总时长整体放大，再重新采样。

Q = zeros(numel(tSamples), 6); qRef = q0; for k = 1:numel(tSamples) qRef = solveIKNearest(ik, trajCart(:,:,k), qRef, weights); Q(k,:) = qRef'; end % 时间缩放：迭代两到三次即可收敛 for iter = 1:3 qd = [zeros(1,6); diff(Q)] / dt; ratio = max(max(abs(qd)) ./ qdotMax'); if ratio <= 1, break; end T = T * ratio * 1.05; % 留5%余量 tSamples = 0:dt:T; trajCart = transformtraj(T0, Tf, [0 T], tSamples, 'TimeScaling', [0 0 1 1]); Q = zeros(numel(tSamples), 6); qRef = q0; for k = 1:numel(tSamples) qRef = solveIKNearest(ik, trajCart(:,:,k), qRef, weights); Q(k,:) = qRef'; end end

逻辑说明：外层循环最多三轮，每轮先算当前轨迹的关节速度峰值，除以各关节的速度上限得到缩放比例，把总时长乘上去再重新采样、重新逆解。乘 1.05 是为了避免边界值反复触发。三轮还收敛不了，说明路径本身穿过了奇异区，靠拉长时间救不回来，得改路径。

导出给控制器时，直接写 CSV，第一列时间戳，后面六列关节角（弧度），并且一定要把采样周期写在文件名里：

writematrix([tSamples(:), Q], sprintf('ur5_joint_traj_dt%.3f.csv', dt));

5. 避坑排查：UR5 轨迹规划里最容易翻车的五件事

下面这五条都是我在真机上踩过的，仿真里一个都看不出来。每条按“现象、原因、解决”记录，看的时候对照自己的代码逐条排查。

5.1 逆解跳分支：相邻两帧关节角突变 180°

现象：仿真动画里机械臂走到某一段突然“翻腕”，关节角序列里某一帧相对前一帧跳了接近 π。实机上表现为急停或者控制器报跟随误差超限。

原因：逐帧逆解时每帧都用同一个初值（比如 home 位），求解器在多个等价位姿解之间来回跳。UR5 有球腕，同一末端位姿最多八组解，跳支在数值求解里非常常见。

解决：改用上一帧解作初值，并按关节距离在多组候选里选优，就是 4.2 节那段代码。另外加一条断言，max(abs(diff(Q)))超过 0.5 rad 就打印出那一帧的索引，人工确认是否合理——正常的连续轨迹不该有这种跳变。

5.2 奇异位形：末端匀速、关节速度爆表

现象：末端走得好好的，某个关节速度曲线出现尖峰，峰值是其他段的五到十倍，控制器直接限幅，末端轨迹跟着偏离直线。

原因：路径擦着腕部奇异走过去，雅可比最小奇异值接近零，逆解要把末端速度映射回关节速度，放大倍数就是奇异值的倒数。

解决：先定位——遍历整条轨迹算min(svd(J))，把低于阈值的帧索引打出来。然后分两种情况处理：能改路径就改路径，让直线绕开奇异点；改不了就在奇异段附近降速，或者上阻尼最小二乘（DLS），在雅可比上加一个阻尼项 λ，代价是末端会有微小位置误差，但关节速度可控。

5.3 单位与角度制：不收敛的第一个元凶

现象：ik一直返回迭代失败，或者正解出来的末端位置在米和毫米之间差 1000 倍，机械臂在画面里跑到很远的地方。

原因：DH 参数里的 a、d 混用了毫米，或者关节角传了角度值给只吃弧度的函数。这类错误不会报语法错，只会让结果莫名其妙。

解决：所有内部计算统一用 SI 单位加弧度，只在读写文件、和上位机通讯的边界上做转换。在建模脚本开头加断言：assert(max(abs(d)) < 2, 'd 疑似用了毫米');，UR5 的连杆偏距最大不到 0.2 m，超过 2 就肯定是单位错了。同理，关节角断言all(abs(q) <= 2*pi)，多一个数量级就是角度制没转。

5.4 姿态插值：欧拉角直插导致中间翻转

现象：末端从位姿 A 到 B 的直线看着没问题，但中间某几帧姿态突然绕某个轴转了很大一圈，实机上腕部剧烈转动。

原因：把 RPY 三个角独立做线性插值。俯仰角经过 ±90° 时欧拉角表示本身有奇异性，插出来的中间姿态不是最短路径。

解决：不要碰欧拉角做插值。位姿统一用 4×4 齐次矩阵，插值走transformtraj或者自己用四元数 slerp。如果下游接口必须给 RPY，那就先插值生成矩阵序列，最后一步再转成 RPY 输出，转换本身不做插值。

5.5 采样太稀：仿真平滑、实机抖

现象：MATLAB 里曲线光滑，速度加速度都连续，实机跑起来中段有明显抖动和异响。

原因：规划采样周期做大了，比如用了 0.05 s。控制器在 0.008 s 插补时，两个稀疏点之间只能线性过渡，加速度在两帧之间是阶跃的，减速机感受到的是持续的冲击。另外稀疏采样下，数值差分算出来的速度峰值也偏低，超速判不出来。

解决：规划采样周期取控制器周期的整数倍且不超过 0.01 s，重采样之后再算一次加速度和加加速度（jerk）。jerk 是很多人漏掉的一项，它的峰值比加速度峰值更能预测实机抖动，控制在 500 rad/s³ 以内基本不会有明显异响。

6. 落地前的最后一道关：时间缩放与回代验证

把轨迹交给实机之前，我固定做两件事：统一时间缩放和全程回代。前者保证所有关节同时到达约束边界，后者保证规划出来的每一帧真的能被执行。

6.1 统一时间缩放：一次算清速度与加速度余量

前面 4.3 节的缩放只看了速度，加速度没管。正确的做法是速度和加速度各算一个缩放系数，取大的那个，整条轨迹一起放慢。这样做的好处是所有关节保持在同一条时间轴上，不会出现某个关节被单独拉伸导致末端轨迹变形。

function scale = timeScaleFactor(Q, dt, qdotMax, qddMax) % 返回满足速度与加速度约束的统一时间缩放因子 qd = [zeros(1,6); diff(Q)] / dt; qdd = [zeros(1,6); diff(qd)] / dt; sV = max(max(abs(qd)) ./ qdotMax'); sA = sqrt(max(max(abs(qdd)) ./ qddMax')); % 加速度随时长平方反比 scale = max([sV, sA, 1]) * 1.05; end

逻辑说明：速度与时间成反比，所以速度超限比例直接就是时间缩放因子；加速度与时间平方成反比，所以加速度的缩放因子要开平方。乘 1.05 是余量。拿到 scale 之后，把总时长 T 乘上去，重新生成轨迹，再跑一遍 5.5 节的 jerk 检查，两项都过才算完。

这套流程跑熟之后，一组轨迹从建模到导出大概十分钟，比在实机上一遍遍试快得多。

6.2 回代验证清单

导出的关节角序列，我会按下面这张表逐项过一遍，任何一项不过就回到对应章节改：

回代这一步最容易被跳过，因为仿真里看着都对。但逆解是数值迭代出来的，收敛标志为成功不代表位姿精确——我在一个贴合项目里就吃过这个亏，某几帧的迭代停在了梯度容差边缘，位置差 3 mm，肉眼完全看不出来，实机贴上去的时候才发现末端翘了一个角。从那以后我每次导出轨迹前都强制走一遍回代比对，误差超限就直接报错，不留“应该没事”的侥幸。这套流程看着啰嗦，但能省掉大量在实机前面反复试的时间，希望帮到你。

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