UR5机械臂MATLAB轨迹规划：DH建模、逆解校验与上机前仿真

简介：围绕UR5机械臂在MATLAB环境中的轨迹规划与实现，面向机器人工程、自动化控制方向的学习者和研究者，帮助其打通运动学、动力学建模到路径生成的完整链路。压缩包共二十九个文件，整体约八百五十九KB，以二十一个m脚本为主体，配合四个mat数据文件、一个ttt仿真场景、一份pdf报告与md说明，并附远程接口动态库，可支撑正逆运动学求解、雅可比计算、位姿与欧拉角变换、关节设置及仿真联调等练习。内容围绕平滑连续且避碰的轨迹规划展开，涉及样条插值、速度加速度约束、优化求解与碰撞检测思路，读者可据此复现实验、理解算法结构并排查接口调用问题。已有5605人学习，适合具备MATLAB基础、希望深入UR5建模与控制实践的中高级读者参考，目录结构也便于按模块检索与对照。

1. 让 UR5 少走弯路：MATLAB 里先把轨迹规划跑通，再上机

有个做毕设的同学拿着一台 UR5 的仿真模型来找我，说点到点运动跑出来末端抖得厉害，动画里还会突然穿进底座。问题既不在 UR5 本身，也不在 MATLAB，而在于轨迹规划这一层被跳过了——直接把逆解算出来的离散点当轨迹用，中间的速度、加速度根本没有任何约束。

这套 UR5 机械臂在 MATLAB 下的轨迹规划脚本，做的就是把这层补上：DH 建模、正逆运动学、关节空间与笛卡尔空间插值、速度加速度校验、三维动画回放，最后把关节序列导成控制器能直接执行的脚本。适合两类人，一类是要做机械臂仿真课程设计或毕设的学生，一类是现场要离线验证一条抓放轨迹、不想在示教器上反复试的工程师。它最终的价值不在于画出一条好看的曲线，而在于让你在上机前就知道这条轨迹会不会超速、会不会在奇异位形附近打滑。

2. 工程骨架与 DH 建模：从参数表到一棵能动的刚体树

拿到一堆 .m 文件，第一件该干的事是看入口脚本调了谁，别急着逐行读。这个项目的组织方式很朴素，圈子里常见的分法就三类：模型、算法、工具函数，各占一个文件夹。下面这个结构是后面所有代码讨论的基础，你的目录名可以不一样，但职责划分最好照抄。

2.1 目录职责与入口脚本

ur5_traj/ ├── config_ur5.m # 全局配置：采样周期、路径、单位约定 ├── model/ │ └── ur5_dh.m # DH 参数 -> SerialLink，唯一的模型来源 ├── traj/ │ ├── traj_joint.m # 关节空间：三次/五次多项式、梯形速度 │ ├── traj_line.m # 笛卡尔空间：直线位移 + 姿态插值 │ └── traj_quintic.m # 手写五次多项式内核，可指定首末速度与加速度 ├── utils/ │ ├── ik_select.m # 逆解多解筛选：就近、限位、避奇异 │ ├── check_limit.m # 关节限位 / 速度 / 加速度越界检查 │ └── plot_robot.m # 动画与末端轨迹描线 ├── export/ │ └── to_urscript.m # 关节序列 -> 控制器脚本 └── demo_pick_place.m # 一个完整抓放例子，只做流程编排

demo_pick_place.m是唯一需要按顺序读的文件：先addpath(genpath(pwd))，再robot = ur5_dh()，然后算两个位姿，调traj_joint或traj_line出关节序列，最后check_limit过一遍再plot_robot回放。这种「入口只编排、算法全在 traj 里」的写法看着啰嗦，好处是你换任务时只改入口，算法一行不用动。

% demo_pick_place.m 的开头部分 addpath(genpath(fileparts(mfilename('fullpath')))); cfg = config_ur5(); % 采样周期 Ts = 0.008，与 125Hz 控制器对齐 robot = ur5_dh(); robot.model3d.addtext(); % 关掉默认的坐标文字，动画更干净 q_home = [0 -pi/2 pi/2 -pi/2 -pi/2 0]; q_pick = [0.3 -1.2 1.4 -1.8 -1.5 0.2]; q_place= [-0.4 -1.0 1.2 -1.7 -1.4 -0.3];

config_ur5.m里我只放三样东西：Ts（采样周期）、ws（动画工作空间范围）、lim（关节限位与最大速度、加速度）。把这些散在脚本里的魔法数字收拢到一个文件，是这类小项目最省事的工程习惯，后面调参数不用满仓库搜索。

2.2 DH 参数表：六个关节怎么落进变换矩阵

UR5 是标准的六轴串联结构，用标准 DH 描述时，六组参数基本是固定的。长度单位统一用米，角度统一用弧度——这一条必须写死在注释里，因为后面 90% 的「模型比例不对」「动画尺寸变小」都是 mm 和 m 混用导致的问题。

把这张表翻译成代码，就是ur5_dh.m的全部内容：

function robot = ur5_dh() % UR5_DH 标准 DH 建模，长度 m，角度 rad d = [0.089159, 0, 0, 0.10915, 0.09465, 0.0823]; a = [0, -0.425, -0.39225, 0, 0, 0 ]; alpha = [pi/2, 0, 0, pi/2, -pi/2, 0 ]; qlim = repmat([-2*pi, 2*pi], 6, 1); % UR5 六个关节都接近 ±360° L = cell(1,6); for i = 1:6 L{i} = Link('d', d(i), 'a', a(i), 'alpha', alpha(i), ... 'qlim', qlim(i,:), 'standard'); end robot = SerialLink([L{:}], 'name', 'UR5'); end

Link的standard关键字决定变换顺序是Rz(θ)·Tz(d)·Tx(a)·Rx(α)，如果你手头的参数来自说明书上的「改进 DH」，就要换成'modified'，此时顺序变为Rx(α)·Tx(a)·Rz(θ)·Tz(d)。这两者混用会得到一个能算但完全不对的模型，末端位置差几十厘米，属于典型的翻车点。

a2和a3取负值，是因为 UR5 的肩肘结构在标准 DH 下的 x 轴方向朝内，这不是笔误。d4的 0.10915 是腕部的侧向偏置，正是它让 UR5 的第 4、5、6 轴不相交于一点，所以它没有封闭形式的解析逆解，只能用数值迭代或者分段解析。这一点直接决定了后面笛卡尔空间插值的写法。

2.3 工具链选型：三套 API 的差别

同一个模型，MATLAB 生态里至少有三种建法，选错了后面全是返工。

开源那套的优势是 API 贴着手册，jtraj、ctraj、fkine、plot一行一个动作，写 demo 极快；缺点是内部实现是个黑匣子，ikine收敛不了只给个 warning，不抛异常。官方那套的inverseKinematics支持给每个关节设权重、支持指定多组初始猜测，遇到无解时会返回最优的次优解并在info里给状态，接工程更稳，但要额外配置求解器参数。

我的建议是：先用开源那套把模型跑通、把动画对出来，确认 DH 没错；真要交付给现场，把逆解换成官方那套或者自己写带收敛判据的迭代。两套模型可以并存，验证方法很简单——同一组关节角，两套fkine出来的末端位姿误差应该在 1e-9 量级，差得多就是 DH 或者单位的问题。

3. 关节空间轨迹：三次、五次多项式与梯形速度曲线

关节空间规划是所有轨迹的地基。它的输入只是起点关节角和终点关节角，输出是一串随时间变化的q，中间不关心中间点末端走的是什么形状。好处是永远不会跑出关节限位，坏处是末端轨迹是条弧线，抓取时可能蹭到工件。

3.1 jtraj 在内部到底算了什么

最省事的写法是一行：

t = linspace(0, 2, 250); % 2 秒，250 个采样点 [q, qd, qdd] = jtraj(q_home, q_pick, t);

jtraj默认用的是五次多项式（工具箱里的tpoly），它同时约束了起点和终点的位置、速度、加速度六个边界条件。这意味着起停都是零速零加速度，机械臂不会在启动瞬间「弹」一下，这是三次多项式做不到的——三次只能约束位置和速度，加速度在端点处是跳变的，实际运行会听到明显的「咔」一声。

用五次多项式时，整条轨迹的时长直接决定速度峰值。250 个点在 2 秒内跑完，单关节最大速度大概是角度差的 1.875 倍每秒。如果这一段角度差是 2 弧度，峰值速度就接近 3.75 rad/s，而 UR5 关节的额定上限大约在 3.14 rad/s 左右，这一条轨迹实际上超速了。所以拿到qd之后必须做一次max(abs(qd))的检查，这一步我在任何脚本里都不会省。

vmax = max(abs(qd), [], 1); % 每个关节的峰值速度，1x6 if any(vmax > cfg.lim.vmax) warning('关节 %s 超速，峰值 %.2f rad/s', ... mat2str(find(vmax > cfg.lim.vmax)), max(vmax)); end

3.2 手写五次多项式：把首末加速度也按死

工具箱的jtraj只接受首末速度为零的情况（严格说是靠内部参数），一旦你要做「从运动中的 A 点平滑过渡到 B 点」，起点速度不是零，就得自己解方程。做法是把每个关节的角度写成五次多项式q(t)=c0+c1t+c2t²+c3t³+c4t⁴+c5t⁵，然后凑六个边界条件解 6×6 线性方程组。

function [q, qd, qdd] = traj_quintic(q0, qf, v0, vf, a0, af, T, Ts) % 六关节同时求解，q0/qf/v0/vf/a0/af 都是 1x6 行向量 t = (0:Ts:T)'; A = [1 0 0 0 0 0; 1 T T^2 T^3 T^4 T^5; 0 1 0 0 0 0; 0 1 2*T 3*T^2 4*T^3 5*T^4; 0 0 2 0 0 0; 0 0 2 6*T 12*T^2 20*T^3]; B = [q0(:)'; qf(:)'; v0(:)'; vf(:)'; a0(:)'; af(:)']; % 6 x 6 c = A \ B; % 每列是一组系数 q = c(1,:) + t.*c(2,:) + t.^2.*c(3,:) + t.^3.*c(4,:) ... + t.^4.*c(5,:) + t.^5.*c(6,:); qd = c(2,:) + 2*t.*c(3,:) + 3*t.^2.*c(4,:) ... + 4*t.^3.*c(5,:) + 5*t.^4.*c(6,:); qdd = 2*c(3,:) + 6*t.*c(4,:) + 12*t.^2.*c(5,:) + 20*t.^3.*c(6,:); end

A矩阵的六行分别对应 t=0 的位置、t=T 的位置、t=0 的速度、t=T 的速度、t=0 的加速度、t=T 的加速度；B的第 i 列就是第 i 个关节的六个边界值。用A \ B一次性解完六个关节，比 for 循环解六次快，也更好读。

这里有个容易忽略的点：T必须大于零，且不能让v0、vf大到方程解出剧烈振荡的系数。判定方法是把q画出来看有没有过冲——如果中间段角度冲出了[q0, qf]区间很多，说明速度约束给得太激进，实际机械臂会在中途「回抽」一下。我一般的做法是把v0、vf限制在相邻两段平均速度的 1.2 倍以内。

3.3 梯形速度：节拍优先时该选哪个

五次多项式好看，但它全程都在变速，加减速段偏长，节拍上吃亏。产线上更常见的是梯形速度：加速段匀加速、中间匀速、末段匀减速，全程贴着最大速度跑。

function [s, sd, sdd] = trap_profile(T, Ta, Ts) % 归一化路径参数 s: 0 -> 1，Ta 为加速段时长 t = (0:Ts:T)'; vmax = 1 / (T - Ta); % 中间匀速段的速度 acc = vmax / Ta; % 加速段加速度 s = zeros(size(t)); sd = s; sdd = s; i1 = t <= Ta; s(i1) = 0.5*acc*t(i1).^2; sd(i1) = acc*t(i1); sdd(i1) = acc; i2 = t > Ta & t <= T - Ta; s(i2) = 0.5*acc*Ta^2 + vmax*(t(i2) - Ta); sd(i2) = vmax; sdd(i2) = 0; i3 = t > T - Ta; dt = T - t(i3); s(i3) = 1 - 0.5*acc*dt.^2; sd(i3) = acc*dt; sdd(i3) = -acc; end

输出的s是 0 到 1 的归一化进度，套到关节上就是q = q0 + s.*(qf - q0)。这种写法的好处是任务层只需要关心「这段路走多久、加速段占多少」，不用管具体角度。Ta一般取T的 0.2 到 0.3，太小加速度会飙上去，太大会失去匀速段的意义。

提示：梯形速度的加速度在三个切换点上是阶跃的，实测会有轻微冲击。对平滑度要求高的场合，把它换成 S 型（七段式）速度曲线，代价是同样节拍下峰值速度要再降一档。

工具箱里对应的函数是lspb，用法[q,qd,qdd] = lspb(q0, qf, t, V)，第四个参数直接给匀速段速度。它和上面手写版本的区别是lspb会自己算加速段时长，你给的V太大时会自动退化成三角形速度曲线，不会报错，这一点得心里有数。

4. 笛卡尔空间轨迹：直线、姿态插值与逆解跟随

抓取任务里，接近工件的那一段必须是直线，否则末端会沿着弧线切进去，轻则蹭花工件，重则撞夹具。这就得在笛卡尔空间插值，再把每个插值点逆解成关节角。这一段是整个项目里最容易出问题的地方，因为逆解不是一一对应的。

4.1 ctraj 的位置和姿态是分两条线插的

T0 = robot.fkine(q_home); % 4x4 齐次变换 T1 = robot.fkine(q_pick); N = 80; Tc = ctraj(T0, T1, N); % 返回 4x4xN 的位姿序列

ctraj内部对位置做线性插值，对姿态做四元数球面插值（trinterp里的实现）。位置线性意味着末端在直线上是匀速的，但姿态的角速度不是常数——四元数球面插值保证的是旋转「最短路径」，角速度会因为起末姿态差异而在中间段有起伏。

如果节拍要求严格，需要自己按弧长重新参数化：

% 先算出每段的位移与转角，合成一条归一化弧长 s p = squeeze(transl(Tc))'; % N x 3 d = [0; cumsum(vecnorm(diff(p), 2, 2))]; s = d / d(end); % 按实际路径长度归一化 Tc = trinterp(T0, T1, s); % 再用 s 重新插值

注意trinterp的第三个参数接受 0 到 1 之间的向量，返回对应的位姿序列，这比在循环里手写插值干净得多。

4.2 逆解多解与就近选解

UR5 因为腕部没有三轴交点，ikine6s那套解析解不适用，只能用数值迭代ikine。数值解的问题是有多解、可能不收敛、可能跳到另一个位形分支。所以逐点求解时必须把上一点作为初值。

q = zeros(N, 6); q(1,:) = q_home; for k = 2:N q(k,:) = robot.ikine(Tc(:,:,k), 'q0', q(k-1,:), ... 'mask', [1 1 1 1 1 1], 'tol', 1e-8, 'maxiter', 500); % 关节跳变保护：相邻两点任一关节变化超过阈值就认为解跳分支了 if max(abs(q(k,:) - q(k-1,:))) > 0.3 warning('第 %d 点逆解跳变，可能存在多解或接近奇异', k); end end

mask的六个 1 表示六个自由度全部约束，做五轴任务时可以关掉某个旋转自由度。q0是迭代初值，给上一点的解能让求解器落在同一个分支上，这是最有效的一招。tol设到 1e-8 已经够用，再小只是浪费时间；maxiter给 500 是防止在奇异位形附近死循环。

ik_select.m干的事情更彻底一些：对每个位姿用多个初值分别求解，把不满足关节限位的丢掉，剩下的按与上一点的关节空间距离排序取最近。多花几倍时间，换来的是整条轨迹的连续性。

4.3 离散点够不够密：用雅可比和速度曲线反查

N = 80是拍脑袋定的，真正的判据是相邻两点的关节角变化量对应的速度有没有超标。

Ts = 0.008; qd_num = diff(q) / Ts; % 数值微分得到关节速度 vmax = max(abs(qd_num), [], 1); fprintf('峰值关节速度: %s rad/s\n', mat2str(round(vmax, 2))); % 奇异位形检查：最小奇异值太小说明雅可比接近降秩 for k = 1:N J = robot.jacob0(q(k,:)); smin(k) = min(svd(J)); end

smin掉到 0.01 以下就要警惕了，那里关节速度会被放大，动画里表现为某一帧突然甩一下。解决办法要么把路径抬高绕开，要么在接近奇异时降速通过。这里没有后悔药可吃——轨迹一旦导成控制器的定速指令，现场就只能靠降倍率硬扛。

还有一个隐蔽的坑：笛卡尔直线的两个端点如果都在工作空间边缘，中间点未必可达。检查方法是把Tc每个点都逆解一次，然后正解回去比对残差：

err = zeros(N,1); for k = 1:N err(k) = norm(transl(robot.fkine(q(k,:))) - transl(Tc(:,:,k))); end max(err) % 超过 1e-3 就说明有解不收敛，轨迹不可用

5. 踩坑排查：五个把轨迹跑歪的真实原因

这部分是血泪经验，每一条都在实际脚本里留下过痕迹。现象、原因、解决三段式记下来，下次遇到同类问题能省半天。

5.1 逆解与位形相关的坑

现象：动画里前面几十帧都正常，突然某一帧整个小臂翻到另一边，后面又恢复正常，但末端位置还是对的。原因：ikine每个点独立求解，走到了另一个位形分支，位置残差依然满足容差，所以不会报错。解决：把上一点解作为q0传入，并且加一条相邻点关节变化阈值判断，超过 0.3 弧度就停下来看，而不是让脚本一路跑完。

现象：直线轨迹末端明显偏离目标，误差几厘米，但ikine没有任何警告。原因：迭代到了maxiter就退出了，工具箱默认不把不收敛当异常处理。解决：算完之后统一正解回代，比对norm(transl(fkine(q)) - transl(T))，超过 1e-3 的点标红。这一步我改成必跑，不通过就不许导出。

5.2 单位、角度与参数表的坑

现象：模型建出来末端只有 10 厘米高，动画像个玩具。原因：DH 表里抄的是毫米值，代码里当米用了。解决：ur5_dh.m的注释里写死「单位：m / rad」，config_ur5.m里加一句断言assert(max(d) < 1, 'DH 参数疑似用了毫米')，这种离谱错误一次就够。

现象：同样的关节角，两套工具箱算出来的末端位姿对不上。原因：标准 DH 和改进 DH 混用，或者alpha的正负号抄反了。解决：把六个关节都置零，手算一遍末端位置，和fkine(zeros(1,6))对比。UR5 全零位时末端大概在基座正上方 0.6 米出头，这个值可以当校验基准。

5.3 动画、限位与真机对接的坑

现象：动画看起来丝滑，导出到控制器第一帧机械臂猛冲一下。原因：控制器当前位姿和脚本里的q(1,:)不一致，servoj直接把第一帧当成目标点。解决：脚本开头先发一条moveJ到起始位并等到位，再开始流式发送，别指望控制器自己平滑过去。

现象：robot.plot画出来的机械臂比例怪异，杆件特别粗。原因：plot的scale参数和workspace范围不匹配。解决：固定写成robot.plot(q, 'workspace', [-1 1 -1 1 -0.2 1.2], 'scale', 0.5, 'delay', 0.02)，delay是每帧停顿秒数，设成Ts就能和真实节拍对上。

注意：plot的delay只是视觉停留，不改变轨迹数据本身的时间参数。想让动画和实际速度一致，得用Ts作为轨迹采样周期，别用默认的时间向量。

6. 二次开发：把一次性脚本改成任务配置驱动

这类项目最常见的退化方式是：每换一个抓放任务，就把demo_pick_place.m复制一份，改几个关节角，几个月后文件夹里躺着二十个版本，谁也不知道哪个能用。我在第二个项目上就吃过这个亏，后来改成了一个很土但很有效的做法——把任务描述抽成结构体数组，主脚本只负责遍历。

% tasks.m —— 一个任务就是一段路 tasks(1) = struct('type','joint','q',q_home, 'T',1.5,'vmax',0.8); tasks(2) = struct('type','line', 'pose',T_pick,'T',2.0,'N',80); tasks(3) = struct('type','joint','q',q_place, 'T',2.5,'vmax',1.0);

主循环里根据type分发到traj_joint或traj_line，每段结束后把末态关节角作为下一段的起点，顺手做一次限位检查。T是这段路的时间预算，vmax是允许的峰值速度上限，N是笛卡尔段的离散点数。

验证顺序我固定成四步，一步不跳。第一步查模型，fkine(zeros(1,6))和手算值对比；第二步查逆解残差，全段正解回代误差小于 1e-4；第三步查速度曲线，diff(q)/Ts的峰值不超过关节额定值，加速度不超过额定值的两倍；第四步才是动画回放，看有没有穿模和跳变。前两步是数值，能自动判；后两步靠眼睛，别偷懒。

导出环节留个后门很有必要。to_urscript.m里除了生成servoj流，再额外输出一份 CSV，每行是时间戳加六个关节角。现场控制器如果只吃点位不接受流式指令，就拿 CSV 走离线程序；如果预算够，直接上流式。两种格式同时产出，成本几乎为零。

function to_urscript(q, Ts, filename) fid = fopen(filename, 'w'); fprintf(fid, 'def traj():\n'); for k = 1:size(q,1) fprintf(fid, ' servoj([%.6f,%.6f,%.6f,%.6f,%.6f,%.6f], a=1.2, v=0.5, t=%.3f)\n', ... q(k,:), Ts); end fprintf(fid, 'end\n'); fclose(fid); writematrix([ (0:size(q,1)-1)'*Ts, q ], strrep(filename,'.script','.csv')); end

参数名在不同固件版本上会有出入，输出前对着现场控制器的脚本手册核一遍servoj的字段顺序，这一步千万别想当然。

从那以后我每次改轨迹参数，都强制走一遍「模型校验 → 残差 → 速度 → 动画」这四关，不管改动多小。看着慢，实际上比在机器上试错快得多，也安全得多。希望帮到你。

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