简介这份PDF文档面向MATLAB机器人建模与控制的初学者及工程技术人员围绕MATLAB 2020a搭配Robotics Toolbox v10.4环境系统梳理工具箱常用命令与使用要点帮助读者解决版本差异导致命令不通用、函数调用记不住等实际问题可作为课程作业、科研仿真与机械臂算法验证的随查手册。资源包内仅1个PDF文件约101KB体积轻便便于手机、平板随时翻阅。内容从二维与三维位姿描述讲起用SE2、transl2、rotx、trplot、tranimate等函数演示坐标变换与动画效果接着进入运动学部分借助Link与SerialLink搭建机械臂模型说明DH参数、关节限位、fkine正解、ikine6s与ikine逆解以及雅可比矩阵的计算方式。轨迹规划章节给出关节空间jtraj与笛卡尔空间ctraj的调用格式动力学部分则覆盖rne逆动力学、gravload、inertia、coriolis、payload并以PD控制器为例演示fdyn正向动力学积分流程。目前已有717人学习适合按目录逐节对照实操、边查边练。1. 从一行 fkine 说起MATLAB 机器人工具箱解决的是什么问题很多人拿到一台串联机械臂的 DH 参数表之后卡住的地方其实不是数学而是把这张表变成一段能跑起来、能复现的代码。手写齐次变换矩阵行和列错一位、alpha 的正负号写反一次末端位姿就会整条链歪掉而且很难用肉眼看出是第几个关节出的问题。Robotics Toolbox 这套 MATLAB 机器人工具箱把连杆参数、正逆运动学、雅可比、动力学和三维显示封装成了几组类你只要把 DH 表填进 Link后面的 fkine、ikine、jtraj 全是几个字母的事。它适合做课程设计、论文里的运动学仿真以及给控制算法搭一个可信的被控对象。要提前记住两件事关节变量默认按弧度处理坐标系默认按标准 DH 搭这两点决定后面所有排错的方向。2. Robotics Toolbox 环境搭建与 DH 参数建模2.1 把 rvctools 挂进 MATLAB 搜索路径工具箱解压后是一个叫 rvctools 的目录树里面分了 robot、simulink、common、spatialmath 等子文件夹。常见做法是把它放在一个不带空格和中文的固定路径下比如 D:\matlab_ws\rvctools然后每次启动 MATLAB 时把整棵树挂进搜索路径。matlab安装完成后直接用默认工作目录解压往往会在后续 genpath 时因为中文路径出现找不到类的报错这一点在课程作业里踩得最多。% 工具箱根目录换成自己的实际路径 toolboxRoot D:\matlab_ws\rvctools; % genpath 会把所有子目录一次性加进来但会自动跳过 private 和以点开头的目录 addpath(genpath(toolboxRoot)); % 工具箱自带的启动脚本负责再注册一批示例和模型脚本 run(fullfile(toolboxRoot, startup_rvc.m)); % 验证能定位到类文件路径并且 exist 返回 8表示是一个类 which SerialLink exist(SerialLink, class)代码的逻辑是先加目录再加启动脚本顺序不能反过来因为 startup_rvc.m 自己也依赖 rvctools 里的相对路径。如果which SerialLink返回空先确认 genpath 的返回值里有没有 robot 子目录如果返回了路径但exist不是 8通常是路径里混进了同名脚本用which -all SerialLink能看清冲突来源。把这两行写进 MATLAB 的 startup.m可以免去每次手动加载。2.2 Link 与 SerialLink一张 DH 表变成可运行模型构造模型的核心是把 DH 表的四列按顺序喂给连杆构造函数。新版工具箱推荐用 Revolute 或 Prismatic 直接给命名参数可读性比早期的位置式写法好很多老代码里的Link([theta d a alpha sigma], standard)仍然能跑只是不建议在新项目里用。下面这张表是一台典型六轴工业臂的标准 DH 参数用作演示关节d (m)a (m)alpha (rad)关节类型100pi/2旋转200.43180旋转30.150050.0203-pi/2旋转40.43180pi/2旋转500-pi/2旋转6000旋转% 每行给一个连杆theta 列不写关节变量由后续的 q 提供 L(1) Revolute(d, 0, a, 0, alpha, pi/2); L(2) Revolute(d, 0, a, 0.4318, alpha, 0); L(3) Revolute(d, 0.15005, a, 0.0203, alpha, -pi/2); L(4) Revolute(d, 0.4318, a, 0, alpha, pi/2); L(5) Revolute(d, 0, a, 0, alpha, -pi/2); L(6) Revolute(d, 0, a, 0, alpha, 0); robot SerialLink(L, name, my6r); robot.display(); % 把 DH 表原样打印出来先核对一遍 % 给每个关节配上行程plot 时会按这个范围限制显示 for k 1:6 robot.qlim(k, :) [-pi pi]; end robot.plot([0 pi/4 pi 0 pi/4 0], scale, 0.6, workspace, [-1 1 -1 1 -0.5 1.5]);参数里要注意的是DH 表中的 theta 列在这里必须留空因为关节转角由调用时的 q 向量传入写死会变成固定关节如果某个关节存在机械零位偏置用 offset 参数补上而不是塞进 theta。SerialLink的 name 建议取个有意义的名字后面在同一个工作区里建多个模型时display 的输出能一眼分辨。robot.display()也可以省略括号直接敲变量名效果一样。2.3 标准 DH 与改进 DH同一台机器人为什么结果差一个符号两套约定的差别在于连杆坐标系固连在哪一端。标准 DH 把坐标系建在连杆远端也就是下一根关节轴上变换顺序是先绕 z 转 theta、再沿 z 移 d、再沿 x 移 a、最后绕 x 转 alpha改进 DH 把坐标系固连在近端顺序里平移和旋转的先后不同所以同一台机构用两套约定给出的 alpha 会差一个关节序号。工具箱里用 modified 标志或者 RevoluteMDH 类来切换。这是被问得最多的一类问题症状是正解算出来的位置对了但姿态整体差 90 度。% 标准 DH 版本 Lstd(1) Revolute(d, 0, a, 0, alpha, pi/2); Lstd(2) Revolute(d, 0, a, 0.4, alpha, 0); r_std SerialLink(Lstd, name, std); % 改进 DH 版本alpha 挂在下一个连杆上 Lmdh(1) RevoluteMDH(d, 0, a, 0, alpha, 0); Lmdh(2) RevoluteMDH(d, 0, a, 0.4, alpha, 0); r_mdh SerialLink(Lmdh, name, mdh); % 用同一个 q 对比末端位置参数若是按同一机构改写结果应接近 q [0.1 0.2]; norm(r_std.fkine(q).t - r_mdh.fkine(q).t)这段代码的价值不在算得多准而在于它是一个快速的交叉验证手段从文献里抄来一组 MDH 参数但混用了标准 DH 的构造函数用上面的方法跑一次就能立刻发现偏差。如果两个模型的末端位置差得离谱先别怀疑算法回头检查 alpha 是挂在第 i 根还是第 i-1 根连杆上。3. 正运动学、逆运动学与轨迹规划的落地命令3.1 fkine 和 tr2rpy把 4x4 矩阵读成能核对的数工具箱内部用齐次变换矩阵描述位姿但人眼核对时需要具体的坐标和角度。fkine 返回的是变换对象.t取平移列tr2rpy 把旋转部分拆成 roll-pitch-yaw。这一套组合是每次改完 DH 参数后第一件要做的事因为位置对不对是最好判断的。mdl_puma560; % 载入内置的 p560 以及 qz/qr/qs/qn 四个典型位形 T p560.fkine(qn); disp(T.t) % 末端位置 [x y z]观察是否落在可达空间内 disp(tr2rpy(T, deg)) % 姿态角加 deg 按角度输出不加则按弧度qn是工具箱自带的标称位形用它可以快速判断模型是否搭反。tr2rpy 的第二个参数决定输出单位这点在写报告时很关键混用会在图上产生量级错误。如果你更习惯四元数把 tr2rpy 换成 tr2quat 即可工具箱默认走的是 ZYX 顺序的欧拉角和多数机器人手册里 rpy 的定义一致。matlab画图时把这三行输出贴在图上比只放一张三维截图更有说服力。3.2 ikine 的 mask、初值和收敛判据怎么给逆运动学分成解析解和数值解两条路。解析解用 ikine6s要求机构是球腕结构速度快且能给出多解数值解用 ikine本质是把位姿误差做成最小二乘迭代所以必须给初值也必须说清楚哪些自由度参与误差计算这就是 mask 的作用。参数含义常见取值mask六个自由度是否参与误差顺序为 x y z r p y只求位置用 [1 1 1 0 0 0]q0迭代初值长度等于关节数取上一个轨迹点或典型位形tol收敛判据误差范数小于它即停1e-6 量级ilimit最大迭代次数几百到一千maxstep单步最大关节增量视关节行程调整T_target p560.fkine(qn); % 解析解只对球腕六轴有效解不唯一但速度极快 qi_ana p560.ikine6s(T_target); % 数值解给定初值和 mask只约束位置 q0 [0 0 0 0 0 0]; qi_num p560.ikine(T_target, q0, mask, [1 1 1 0 0 0], tol, 1e-6, ilimit, 500); % 反算一次看误差有多大 err norm(p560.fkine(qi_num).t - T_target.t); fprintf(位置残差 %.3e m\n, err);mask 是最容易被忽略的参数。默认六位全开一旦目标姿态在机构工作空间外迭代会在姿态分量上打架最后位置也解不准只做抓取点位规划时先把姿态屏蔽掉收敛会稳得多。初值建议取上一个轨迹点而不是一路用零位因为数值解会收敛到离初值最近的那个解支初值跳变会让关节角在相邻两点之间突然翻转。如果手头的 MATLAB 装了优化工具箱新版本里还可以试试 ikine_min它借助优化求解器处理带关节限位的逆解适合对行程有硬约束的场合。3.3 jtraj 与 ctraj关节空间和笛卡尔空间轨迹怎么选轨迹规划的两条基本路线一条在关节空间插值一条在笛卡尔空间插值。jtraj 生成的是五次多项式默认两端的速度和加速度都为零端点平滑不会冲击减速器ctraj 在笛卡尔空间走直线末端走的是真正的直线但需要逐点做逆解中途可能穿过奇异位形。t 0:0.02:2; % 关节空间一次算完三段分别是位置、速度、加速度 [q, qd, qdd] jtraj(qz, qr, t); p560.plot(q(1:5:end, :)); % 抽帧播放避免动画太慢 % 笛卡尔空间先取两端位姿再按直线插值 T0 p560.fkine(qz); T1 p560.fkine(qr); Tc ctraj(T0, T1, length(t)); % 旧版返回 4x4xN新版返回 SE3 数组 qc zeros(length(t), 6); for k 1:length(t) qc(k, :) p560.ikine6s(Tc(:, :, k)); % 新版改成 Tc(k) end选哪条路要看任务对末端的约束。码垛这类只关心起终点位姿的任务用 jtraj 就够关节运动更平顺焊接、涂胶这类要求末端沿直线走的任务必须用 ctraj但要提前检查 Tc 上的每一点都在工作空间内否则逆解会在某几帧返回 NaN。判断方法是先跑一遍循环统计sum(any(isnan(qc), 2))有 NaN 就把直线拆成几段或者调整中间路径点绕开奇异区。qd 和 qdd 不要算完就丢它们是后面做动力学前馈和电流限幅的输入。4. 雅可比、动力学与可视化验证4.1 jacob0 与可操作度奇异位形提前发现雅可比矩阵描述关节速度到末端速度的映射它同时是判断奇异位形的工具。工具箱提供 jacob0 和 jacobn 两个版本前者在基坐标系下表达后者在末端坐标系下表达做速度控制时一般用 jacob0做力控制时用 jacobn 更顺手。函数作用输出维度jacob0基坐标系下的雅可比6xNjacobn末端坐标系下的雅可比6xNdet(J*J)可操作度的平方标量J p560.jacob0(qn); w sqrt(det(J * J)); % Yoshikawa 可操作度越接近 0 越靠近奇异 % 扫一遍第 5 轴看可操作度在哪里塌下去 q5 linspace(-pi/2, pi/2, 61); w zeros(size(q5)); for k 1:numel(q5) q qn; q(5) q5(k); Jk p560.jacob0(q); w(k) sqrt(det(Jk * Jk)); end [minW, idx] min(w); fprintf(最小可操作度 %.4f出现在 q5 %.3f rad\n, minW, q5(idx)); plot(q5, w); xlabel(q5 (rad)); ylabel(可操作度);典型六轴的腕部奇异就出现在第 5 轴接近零的位置此时第 4 轴和第 6 轴共线雅可比降秩可操作度曲线会掉到接近零。规划轨迹时把这条曲线画出来能提前避开让关节速度爆掉的路段。注意可操作度是量纲相关的量位置部分和姿态部分混在一起算不同尺寸的机器人之间不能直接比数值只能在同一台机构上做相对比较。4.2 rne、gravload、inertia动力学参数怎么喂给控制器运动学只管几何真正决定电机选型和前馈补偿的是动力学项。rne 是逆动力学给一组 q、qd、qdd 返回关节力矩gravload 只算重力项是静态保持力矩inertia 返回关节空间惯性矩阵。这几个函数是搭 Simulink 模型、对接 matlab pid tuner 调参之前必须先跑一遍的。函数输入典型用途rneq, qd, qdd逆动力学算总力矩gravloadq重力补偿前馈inertiaq惯性矩阵用于动力学方程coriolisq, qd科氏力和离心力项% 静态保持力矩速度加速度都给零输出就是重力项 G p560.gravload(qn); % 完整逆动力学 tau p560.rne(qn, zeros(1,6), zeros(1,6)); % 关节空间惯性矩阵 M p560.inertia(qn); % 末端挂了 2.5 kg 负载质心偏法兰 0.1 m p560.payload(2.5, [0 0 0.1]); disp([G tau]); % 两者在静态下应当一致可用来互相验证rne和gravload在速度加速度为零时结果应当完全一致这是检查动力学模型有没有搭错的最快方法。加了负载之后力矩会明显变化尤其是第 2、3 轴做电机选型时别漏掉这一步。需要提醒的是工具箱默认的连杆惯量参数来自内置模型用在自己搭的机构上时要通过L(k).m、L(k).r、L(k).I把实际的质量、质心位置和惯性张量填进去否则算出来的力矩只有参考意义。4.3 plot、teach 与轨迹回放三维显示看起来是辅助功能实际上是最省时间的一环。plot 按关节角画出机构teach 打开滑动条交互plot3d 或者 plot 的 trail 选项可以留下末端轨迹。用这些功能把每一步的输出可视化比盯着矩阵数字找错快得多。p560.plot(qn, workspace, [-1 1 -1 1 -0.5 1.5], scale, 0.8, view, [135 30]); % 交互式滑动条拖动关节角实时看位姿变化 p560.teach(); % 轨迹回放并留下末端轨迹线 p560.plot(q, trail, b, delay, 0.02);workspace 参数同时控制坐标轴范围和画出来的地面网格写得太大会让机构显得很小scale 用于缩放显示尺寸不影响实际计算。轨迹回放时如果动画卡顿把 delay 调小或者像前面那样抽帧播放。teach 的滑动条数值实时反映关节角调到一个姿态之后可以直接把p560.getpos()取出来作为下一条轨迹的端点这个技巧在做示教复现的演示时特别顺手。5. 逆解不收敛与 DH 符号错的排查手法排查的第一步是分清问题出在哪一环。建议按下面的顺序逐个排除先用 fkine 验证正解再用 ikine6s 验证目标位姿是否可达最后才去调数值解的参数。逆解失败绝大多数时候不是求解器的问题而是目标位姿本身就在工作空间之外或者坐标系定义有偏差。症状大概率原因处理方式ikine6s 报非球腕错误结构不满足解析解条件改用 ikine 数值解ikine 返回 NaN目标点超出可达范围检查 fkine 出的位置量级姿态整体差固定角度标准 DH 与改进 DH 混用对照 alpha 的挂载位置位置总是偏一段距离未设置 base 或 tool补上底座和工具偏置轨迹中间某帧解飞掉经过奇异位形分段规划或抬高避让工具坐标系和底座偏置是最容易漏的一项。法兰到实际 TCP 之间往往还有一段夹具底座也可能垫高了这两项不补进去正解和实机永远对不上。% 工具偏置先绕末端 z 轴转 180 度再沿基坐标 z 方向平移 0.05 m p560.tool transl(0, 0, 0.05) * trotz(pi); % 底座抬高 0.8 m p560.base transl(0, 0, 0.8); % 重新验证这种情况下 fkine 会把两段偏置一起算进去 T p560.fkine(qn); disp(T.t); % 逆解残差检查正常应小于 1e-6 m q_sol p560.ikine(T, qn, mask, [1 1 1 1 1 1], tol, 1e-8, ilimit, 1000); fprintf(残差 %.3e m\n, norm(p560.fkine(q_sol).t - T.t));初值策略上有个实用经验把上一个轨迹点当初始值数值解会沿解支连续地走关节角不会在相邻两点之间跳变如果中途出现大幅跳变说明机器人穿过了奇异面此时把轨迹拆段在奇异位形附近插入一个过渡点重新起解。至于 tol设成 1e-8 之前先确认关节行程和末端的量级1e-8 米对多数应用已经远超过机械精度继续收小只会换来迭代次数的暴涨。最后把每次调整前后的残差都打印出来看数字从 1e-1 掉到 1e-6 的过程比任何解释都直观。本文还有配套的精品资源点击获取 MATLAB机器人工具箱：DH参数建模与正逆运动学轨迹规划实战
简介这份PDF文档面向MATLAB机器人建模与控制的初学者及工程技术人员围绕MATLAB 2020a搭配Robotics Toolbox v10.4环境系统梳理工具箱常用命令与使用要点帮助读者解决版本差异导致命令不通用、函数调用记不住等实际问题可作为课程作业、科研仿真与机械臂算法验证的随查手册。资源包内仅1个PDF文件约101KB体积轻便便于手机、平板随时翻阅。内容从二维与三维位姿描述讲起用SE2、transl2、rotx、trplot、tranimate等函数演示坐标变换与动画效果接着进入运动学部分借助Link与SerialLink搭建机械臂模型说明DH参数、关节限位、fkine正解、ikine6s与ikine逆解以及雅可比矩阵的计算方式。轨迹规划章节给出关节空间jtraj与笛卡尔空间ctraj的调用格式动力学部分则覆盖rne逆动力学、gravload、inertia、coriolis、payload并以PD控制器为例演示fdyn正向动力学积分流程。目前已有717人学习适合按目录逐节对照实操、边查边练。1. 从一行 fkine 说起MATLAB 机器人工具箱解决的是什么问题很多人拿到一台串联机械臂的 DH 参数表之后卡住的地方其实不是数学而是把这张表变成一段能跑起来、能复现的代码。手写齐次变换矩阵行和列错一位、alpha 的正负号写反一次末端位姿就会整条链歪掉而且很难用肉眼看出是第几个关节出的问题。Robotics Toolbox 这套 MATLAB 机器人工具箱把连杆参数、正逆运动学、雅可比、动力学和三维显示封装成了几组类你只要把 DH 表填进 Link后面的 fkine、ikine、jtraj 全是几个字母的事。它适合做课程设计、论文里的运动学仿真以及给控制算法搭一个可信的被控对象。要提前记住两件事关节变量默认按弧度处理坐标系默认按标准 DH 搭这两点决定后面所有排错的方向。2. Robotics Toolbox 环境搭建与 DH 参数建模2.1 把 rvctools 挂进 MATLAB 搜索路径工具箱解压后是一个叫 rvctools 的目录树里面分了 robot、simulink、common、spatialmath 等子文件夹。常见做法是把它放在一个不带空格和中文的固定路径下比如 D:\matlab_ws\rvctools然后每次启动 MATLAB 时把整棵树挂进搜索路径。matlab安装完成后直接用默认工作目录解压往往会在后续 genpath 时因为中文路径出现找不到类的报错这一点在课程作业里踩得最多。% 工具箱根目录换成自己的实际路径 toolboxRoot D:\matlab_ws\rvctools; % genpath 会把所有子目录一次性加进来但会自动跳过 private 和以点开头的目录 addpath(genpath(toolboxRoot)); % 工具箱自带的启动脚本负责再注册一批示例和模型脚本 run(fullfile(toolboxRoot, startup_rvc.m)); % 验证能定位到类文件路径并且 exist 返回 8表示是一个类 which SerialLink exist(SerialLink, class)代码的逻辑是先加目录再加启动脚本顺序不能反过来因为 startup_rvc.m 自己也依赖 rvctools 里的相对路径。如果which SerialLink返回空先确认 genpath 的返回值里有没有 robot 子目录如果返回了路径但exist不是 8通常是路径里混进了同名脚本用which -all SerialLink能看清冲突来源。把这两行写进 MATLAB 的 startup.m可以免去每次手动加载。2.2 Link 与 SerialLink一张 DH 表变成可运行模型构造模型的核心是把 DH 表的四列按顺序喂给连杆构造函数。新版工具箱推荐用 Revolute 或 Prismatic 直接给命名参数可读性比早期的位置式写法好很多老代码里的Link([theta d a alpha sigma], standard)仍然能跑只是不建议在新项目里用。下面这张表是一台典型六轴工业臂的标准 DH 参数用作演示关节d (m)a (m)alpha (rad)关节类型100pi/2旋转200.43180旋转30.150050.0203-pi/2旋转40.43180pi/2旋转500-pi/2旋转6000旋转% 每行给一个连杆theta 列不写关节变量由后续的 q 提供 L(1) Revolute(d, 0, a, 0, alpha, pi/2); L(2) Revolute(d, 0, a, 0.4318, alpha, 0); L(3) Revolute(d, 0.15005, a, 0.0203, alpha, -pi/2); L(4) Revolute(d, 0.4318, a, 0, alpha, pi/2); L(5) Revolute(d, 0, a, 0, alpha, -pi/2); L(6) Revolute(d, 0, a, 0, alpha, 0); robot SerialLink(L, name, my6r); robot.display(); % 把 DH 表原样打印出来先核对一遍 % 给每个关节配上行程plot 时会按这个范围限制显示 for k 1:6 robot.qlim(k, :) [-pi pi]; end robot.plot([0 pi/4 pi 0 pi/4 0], scale, 0.6, workspace, [-1 1 -1 1 -0.5 1.5]);参数里要注意的是DH 表中的 theta 列在这里必须留空因为关节转角由调用时的 q 向量传入写死会变成固定关节如果某个关节存在机械零位偏置用 offset 参数补上而不是塞进 theta。SerialLink的 name 建议取个有意义的名字后面在同一个工作区里建多个模型时display 的输出能一眼分辨。robot.display()也可以省略括号直接敲变量名效果一样。2.3 标准 DH 与改进 DH同一台机器人为什么结果差一个符号两套约定的差别在于连杆坐标系固连在哪一端。标准 DH 把坐标系建在连杆远端也就是下一根关节轴上变换顺序是先绕 z 转 theta、再沿 z 移 d、再沿 x 移 a、最后绕 x 转 alpha改进 DH 把坐标系固连在近端顺序里平移和旋转的先后不同所以同一台机构用两套约定给出的 alpha 会差一个关节序号。工具箱里用 modified 标志或者 RevoluteMDH 类来切换。这是被问得最多的一类问题症状是正解算出来的位置对了但姿态整体差 90 度。% 标准 DH 版本 Lstd(1) Revolute(d, 0, a, 0, alpha, pi/2); Lstd(2) Revolute(d, 0, a, 0.4, alpha, 0); r_std SerialLink(Lstd, name, std); % 改进 DH 版本alpha 挂在下一个连杆上 Lmdh(1) RevoluteMDH(d, 0, a, 0, alpha, 0); Lmdh(2) RevoluteMDH(d, 0, a, 0.4, alpha, 0); r_mdh SerialLink(Lmdh, name, mdh); % 用同一个 q 对比末端位置参数若是按同一机构改写结果应接近 q [0.1 0.2]; norm(r_std.fkine(q).t - r_mdh.fkine(q).t)这段代码的价值不在算得多准而在于它是一个快速的交叉验证手段从文献里抄来一组 MDH 参数但混用了标准 DH 的构造函数用上面的方法跑一次就能立刻发现偏差。如果两个模型的末端位置差得离谱先别怀疑算法回头检查 alpha 是挂在第 i 根还是第 i-1 根连杆上。3. 正运动学、逆运动学与轨迹规划的落地命令3.1 fkine 和 tr2rpy把 4x4 矩阵读成能核对的数工具箱内部用齐次变换矩阵描述位姿但人眼核对时需要具体的坐标和角度。fkine 返回的是变换对象.t取平移列tr2rpy 把旋转部分拆成 roll-pitch-yaw。这一套组合是每次改完 DH 参数后第一件要做的事因为位置对不对是最好判断的。mdl_puma560; % 载入内置的 p560 以及 qz/qr/qs/qn 四个典型位形 T p560.fkine(qn); disp(T.t) % 末端位置 [x y z]观察是否落在可达空间内 disp(tr2rpy(T, deg)) % 姿态角加 deg 按角度输出不加则按弧度qn是工具箱自带的标称位形用它可以快速判断模型是否搭反。tr2rpy 的第二个参数决定输出单位这点在写报告时很关键混用会在图上产生量级错误。如果你更习惯四元数把 tr2rpy 换成 tr2quat 即可工具箱默认走的是 ZYX 顺序的欧拉角和多数机器人手册里 rpy 的定义一致。matlab画图时把这三行输出贴在图上比只放一张三维截图更有说服力。3.2 ikine 的 mask、初值和收敛判据怎么给逆运动学分成解析解和数值解两条路。解析解用 ikine6s要求机构是球腕结构速度快且能给出多解数值解用 ikine本质是把位姿误差做成最小二乘迭代所以必须给初值也必须说清楚哪些自由度参与误差计算这就是 mask 的作用。参数含义常见取值mask六个自由度是否参与误差顺序为 x y z r p y只求位置用 [1 1 1 0 0 0]q0迭代初值长度等于关节数取上一个轨迹点或典型位形tol收敛判据误差范数小于它即停1e-6 量级ilimit最大迭代次数几百到一千maxstep单步最大关节增量视关节行程调整T_target p560.fkine(qn); % 解析解只对球腕六轴有效解不唯一但速度极快 qi_ana p560.ikine6s(T_target); % 数值解给定初值和 mask只约束位置 q0 [0 0 0 0 0 0]; qi_num p560.ikine(T_target, q0, mask, [1 1 1 0 0 0], tol, 1e-6, ilimit, 500); % 反算一次看误差有多大 err norm(p560.fkine(qi_num).t - T_target.t); fprintf(位置残差 %.3e m\n, err);mask 是最容易被忽略的参数。默认六位全开一旦目标姿态在机构工作空间外迭代会在姿态分量上打架最后位置也解不准只做抓取点位规划时先把姿态屏蔽掉收敛会稳得多。初值建议取上一个轨迹点而不是一路用零位因为数值解会收敛到离初值最近的那个解支初值跳变会让关节角在相邻两点之间突然翻转。如果手头的 MATLAB 装了优化工具箱新版本里还可以试试 ikine_min它借助优化求解器处理带关节限位的逆解适合对行程有硬约束的场合。3.3 jtraj 与 ctraj关节空间和笛卡尔空间轨迹怎么选轨迹规划的两条基本路线一条在关节空间插值一条在笛卡尔空间插值。jtraj 生成的是五次多项式默认两端的速度和加速度都为零端点平滑不会冲击减速器ctraj 在笛卡尔空间走直线末端走的是真正的直线但需要逐点做逆解中途可能穿过奇异位形。t 0:0.02:2; % 关节空间一次算完三段分别是位置、速度、加速度 [q, qd, qdd] jtraj(qz, qr, t); p560.plot(q(1:5:end, :)); % 抽帧播放避免动画太慢 % 笛卡尔空间先取两端位姿再按直线插值 T0 p560.fkine(qz); T1 p560.fkine(qr); Tc ctraj(T0, T1, length(t)); % 旧版返回 4x4xN新版返回 SE3 数组 qc zeros(length(t), 6); for k 1:length(t) qc(k, :) p560.ikine6s(Tc(:, :, k)); % 新版改成 Tc(k) end选哪条路要看任务对末端的约束。码垛这类只关心起终点位姿的任务用 jtraj 就够关节运动更平顺焊接、涂胶这类要求末端沿直线走的任务必须用 ctraj但要提前检查 Tc 上的每一点都在工作空间内否则逆解会在某几帧返回 NaN。判断方法是先跑一遍循环统计sum(any(isnan(qc), 2))有 NaN 就把直线拆成几段或者调整中间路径点绕开奇异区。qd 和 qdd 不要算完就丢它们是后面做动力学前馈和电流限幅的输入。4. 雅可比、动力学与可视化验证4.1 jacob0 与可操作度奇异位形提前发现雅可比矩阵描述关节速度到末端速度的映射它同时是判断奇异位形的工具。工具箱提供 jacob0 和 jacobn 两个版本前者在基坐标系下表达后者在末端坐标系下表达做速度控制时一般用 jacob0做力控制时用 jacobn 更顺手。函数作用输出维度jacob0基坐标系下的雅可比6xNjacobn末端坐标系下的雅可比6xNdet(J*J)可操作度的平方标量J p560.jacob0(qn); w sqrt(det(J * J)); % Yoshikawa 可操作度越接近 0 越靠近奇异 % 扫一遍第 5 轴看可操作度在哪里塌下去 q5 linspace(-pi/2, pi/2, 61); w zeros(size(q5)); for k 1:numel(q5) q qn; q(5) q5(k); Jk p560.jacob0(q); w(k) sqrt(det(Jk * Jk)); end [minW, idx] min(w); fprintf(最小可操作度 %.4f出现在 q5 %.3f rad\n, minW, q5(idx)); plot(q5, w); xlabel(q5 (rad)); ylabel(可操作度);典型六轴的腕部奇异就出现在第 5 轴接近零的位置此时第 4 轴和第 6 轴共线雅可比降秩可操作度曲线会掉到接近零。规划轨迹时把这条曲线画出来能提前避开让关节速度爆掉的路段。注意可操作度是量纲相关的量位置部分和姿态部分混在一起算不同尺寸的机器人之间不能直接比数值只能在同一台机构上做相对比较。4.2 rne、gravload、inertia动力学参数怎么喂给控制器运动学只管几何真正决定电机选型和前馈补偿的是动力学项。rne 是逆动力学给一组 q、qd、qdd 返回关节力矩gravload 只算重力项是静态保持力矩inertia 返回关节空间惯性矩阵。这几个函数是搭 Simulink 模型、对接 matlab pid tuner 调参之前必须先跑一遍的。函数输入典型用途rneq, qd, qdd逆动力学算总力矩gravloadq重力补偿前馈inertiaq惯性矩阵用于动力学方程coriolisq, qd科氏力和离心力项% 静态保持力矩速度加速度都给零输出就是重力项 G p560.gravload(qn); % 完整逆动力学 tau p560.rne(qn, zeros(1,6), zeros(1,6)); % 关节空间惯性矩阵 M p560.inertia(qn); % 末端挂了 2.5 kg 负载质心偏法兰 0.1 m p560.payload(2.5, [0 0 0.1]); disp([G tau]); % 两者在静态下应当一致可用来互相验证rne和gravload在速度加速度为零时结果应当完全一致这是检查动力学模型有没有搭错的最快方法。加了负载之后力矩会明显变化尤其是第 2、3 轴做电机选型时别漏掉这一步。需要提醒的是工具箱默认的连杆惯量参数来自内置模型用在自己搭的机构上时要通过L(k).m、L(k).r、L(k).I把实际的质量、质心位置和惯性张量填进去否则算出来的力矩只有参考意义。4.3 plot、teach 与轨迹回放三维显示看起来是辅助功能实际上是最省时间的一环。plot 按关节角画出机构teach 打开滑动条交互plot3d 或者 plot 的 trail 选项可以留下末端轨迹。用这些功能把每一步的输出可视化比盯着矩阵数字找错快得多。p560.plot(qn, workspace, [-1 1 -1 1 -0.5 1.5], scale, 0.8, view, [135 30]); % 交互式滑动条拖动关节角实时看位姿变化 p560.teach(); % 轨迹回放并留下末端轨迹线 p560.plot(q, trail, b, delay, 0.02);workspace 参数同时控制坐标轴范围和画出来的地面网格写得太大会让机构显得很小scale 用于缩放显示尺寸不影响实际计算。轨迹回放时如果动画卡顿把 delay 调小或者像前面那样抽帧播放。teach 的滑动条数值实时反映关节角调到一个姿态之后可以直接把p560.getpos()取出来作为下一条轨迹的端点这个技巧在做示教复现的演示时特别顺手。5. 逆解不收敛与 DH 符号错的排查手法排查的第一步是分清问题出在哪一环。建议按下面的顺序逐个排除先用 fkine 验证正解再用 ikine6s 验证目标位姿是否可达最后才去调数值解的参数。逆解失败绝大多数时候不是求解器的问题而是目标位姿本身就在工作空间之外或者坐标系定义有偏差。症状大概率原因处理方式ikine6s 报非球腕错误结构不满足解析解条件改用 ikine 数值解ikine 返回 NaN目标点超出可达范围检查 fkine 出的位置量级姿态整体差固定角度标准 DH 与改进 DH 混用对照 alpha 的挂载位置位置总是偏一段距离未设置 base 或 tool补上底座和工具偏置轨迹中间某帧解飞掉经过奇异位形分段规划或抬高避让工具坐标系和底座偏置是最容易漏的一项。法兰到实际 TCP 之间往往还有一段夹具底座也可能垫高了这两项不补进去正解和实机永远对不上。% 工具偏置先绕末端 z 轴转 180 度再沿基坐标 z 方向平移 0.05 m p560.tool transl(0, 0, 0.05) * trotz(pi); % 底座抬高 0.8 m p560.base transl(0, 0, 0.8); % 重新验证这种情况下 fkine 会把两段偏置一起算进去 T p560.fkine(qn); disp(T.t); % 逆解残差检查正常应小于 1e-6 m q_sol p560.ikine(T, qn, mask, [1 1 1 1 1 1], tol, 1e-8, ilimit, 1000); fprintf(残差 %.3e m\n, norm(p560.fkine(q_sol).t - T.t));初值策略上有个实用经验把上一个轨迹点当初始值数值解会沿解支连续地走关节角不会在相邻两点之间跳变如果中途出现大幅跳变说明机器人穿过了奇异面此时把轨迹拆段在奇异位形附近插入一个过渡点重新起解。至于 tol设成 1e-8 之前先确认关节行程和末端的量级1e-8 米对多数应用已经远超过机械精度继续收小只会换来迭代次数的暴涨。最后把每次调整前后的残差都打印出来看数字从 1e-1 掉到 1e-6 的过程比任何解释都直观。本文还有配套的精品资源点击获取