MATLAB机器人工具箱：机械臂建模、轨迹与动力学仿真

简介这份文档面向使用 MATLAB 开展机器人建模、运动学与动力学学习的本科生、研究生及工程技术人员以 Robotics Toolbox v10.4配合 MATLAB 2020a为环境系统整理工具箱常用命令、参数含义与调用格式并提示不同版本间的命令差异可作随查随用的速查资料。内容按位姿描述、运动学、轨迹规划、动力学四个模块展开位姿部分涵盖 SE2/SE3 与 transl、trotx 等变换运动学部分包括 Link/SerialLink 建模、DH 参数与关节限制、fkine 正运动学、ikine/ikine6s 逆解、jacob0 与 jacobn 雅克比矩阵轨迹规划给出 jtraj、ctraj 两种空间方案动力学部分整理 rne、gravload、inertia、coriolis、payload、fdyn 等函数并附 PD 控制器与正向动力学调用示例。资源包为单个 PDF 文档约 101KB命令多以代码注释形式呈现便于检索对照已有 717 人学习。1. 手推 DH 矩阵三小时不如先用 Robotics Toolbox 把模型跑通做机械臂算法的人大多经历过这个阶段照着教材把 DH 参数抄下来一行行推导相邻连杆变换再乘出末端位姿结果和实物差一个符号。问题往往不在推导本身而在坐标系定义、旋转顺序、关节零点这几处互相打架。Robotics ToolboxPeter Corke 那套社区常叫 RoboticsToolbox 或 MATLAB 机器人工具箱把这些易错环节标准化了SE2/SE3 描述位姿Link 描述连杆SerialLink 串成机械臂fkine、ikine、jacob0 直接给结果trplot 和 teach 还能把模型画出来、拖出来。它在 MATLAB 2020a 上对应 v10.4命令与早期 v9 有差异照旧版教程抄会碰到函数找不到或者返回值类型对不上的情况。下面按位姿描述、运动学建模、轨迹规划、动力学仿真、版本排错五块展开配套的文档资料也照这个顺序整理每一段都给出可直接运行的命令和参数含义。2. 位姿描述SE2/SE3 构造、trplot 可视化与取角第一次用工具箱的人常卡在同一个点同样是「绕 x 轴转 30 度」rotx 返回 3×3 矩阵trotx 返回 4×4 齐次变换两者混着乘要么维度报错要么算出一个看着正常其实坐标系错位的结果。位姿描述是后面所有运动学、动力学的地基这一层弄混后面全是连锁错误。2.1 二维位姿SE2、transl2 与 trot2 的组合关系二维场景在移动机器人、平面机械臂里很常见工具箱给了专门的 SE2 类和 trplot2 绘图函数。% SE2(x, y, theta)x、y 为平移量theta 为旋转角度弧度 T1 SE2(1, 2, 30*pi/180); % 等价写法纯平移左乘纯旋转顺序不能颠倒 T2 transl2(1, 2) * trot2(30*pi/180); disp(T1.T - T2.T) % 全零矩阵说明两种写法结果一致 trplot2(T1, frame, A, color, b); hold on trplot2(transl2(0, 0), frame, O, color, k); axis equalT1.T取回底层的 3×3 齐次矩阵方便和手推结果比对。transl2只做平移trot2只做旋转两者相乘才是完整位姿顺序反了意味着先在原坐标系旋转再平移和实际装配关系不符。trplot2的frame给坐标系起名图上直接显示字母color控制颜色坐标系太小看不清时用axis手动放大视野。2.2 三维旋转矩阵与齐次变换rotx 和 trotx 不能混用三维部分函数数量翻倍用一张表把返回维度记牢比反复查文档快得多。函数返回值作用典型搭配rotx/roty/rotz3×3 矩阵纯旋转可作用于 3×1 向量与 rt2tr 组合成齐次变换trotx/troty/trotz4×4v10 为 SE3 对象纯旋转的齐次形式与 transl 直接相乘transl4×4SE3 对象纯平移左乘表示在基坐标系下平移rt2tr4×4把 3×3 旋转和 3×1 位移拼起来手算旋转矩阵后转入齐次域SE34×4直接构造齐次变换对象做轨迹插值、复合变换R rotx(pi/6) * roty(pi/4); % 3x3先绕 x 再绕 y T transl(0.4, 0, 0.3) * rt2tr(R, [0 0 0]); % 4x4 齐次变换 tranimate(T, frame, B, axis, [-1 1 -1 1 -1 1]); % 从单位位姿动画过渡到 Ttransl(p) * trotx(a)表示先绕当前坐标系的 x 轴旋转再整体平移到基坐标系下的 p写成trotx(a) * transl(p)则是沿旋转后的轴平移工程上九成场景用的是前一种。tranimate的axis一定要设否则自动缩放会让动画看起来在抖。2.3 trplot、tranimate 的常用参数与坐标系对齐检查trplot参数不用背常用的就四个trplot(T, frame, E, color, r, length, 0.2, axis, [-1 1 -1 1 -1 1]);length控制坐标轴箭头长度默认值在机械臂尺度下常常只有几个像素调到 0.1~0.3 才看得清axis固定视野范围多坐标系叠画时不设会出现「动画在跳」的错觉。检查两个坐标系是否对齐有个笨办法但很有效把其中一个的变换乘上它的逆结果应该是单位矩阵norm(T * inv(T) - eye(4))小于 1e-12 才算数值上干净。注意trotx这类函数在 v10 返回的是 SE3 对象而不是 double 矩阵直接T(1:3,4)索引会报错要先.T取数值。用class(T)看一眼返回类型能省掉大量排查时间。2.4 从矩阵里取角tr2rpy、tr2eul、tr2angvec 的差别与陷阱正解算出齐次变换只是中间结果很多时候需要把它还原成人类看得懂的角度。T transl(0.5, 0.2, 0.8) * trotz(pi/3) * troty(pi/6); rpy tr2rpy(T); % 1x3绕 Z-Y-X 顺序的滚转/俯仰/偏航 eul tr2eul(T); % 1x3ZYZ 欧拉角 [theta, v] tr2angvec(T); % 等效轴角转角 theta 单位轴向量 vtr2rpy和tr2eul的旋转顺序完全不同同一个矩阵取出来的三个数不通用写进控制器前一定确认对方用的是哪套。tr2rpy在俯仰角接近 ±90° 时会出现万向节死锁两个角度值会突变做插值时最好绕开这段区间或者改用四元数。tr2angvec的返回顺序在 v9 和 v10 之间有调整用之前先help tr2angvec看一眼当前版本的签名这类小差异最容易在移植旧代码时踩到。3. Link 与 SerialLink 建模用 fkine/ikine 做闭环验证模型建错后面轨迹规划和动力学全是错的而且错得不明显——图能画出来曲线也平滑就是和实物对不上。所以串完机械臂第一件事不是跑轨迹而是用正解和逆解互相验证一遍确认 DH 参数、关节零点、单位这三处都没问题。3.1 Link 的三组参数表运动学、动力学、电机Link一个对象上挂了三类属性很多人只填前四个就往下走到动力学仿真阶段才发现inertia返回全零矩阵。% 标准 DH先绕 z 转 theta再沿 z 移动 d再沿 x 移动 a最后绕 x 转 alpha L(1) Link(d, 0.400, a, 0.025, alpha, pi/2, qlim, deg2rad([-180 180])); L(2) Link(d, 0, a, 0.560, alpha, 0, qlim, deg2rad([-90 90]), offset, pi/2); L(3) Link(d, 0, a, 0.035, alpha, pi/2, qlim, deg2rad([-180 180])); % 移动副用 jointtype P位置参数写法里的 sigma1 是同一件事 L(4) Link(theta, 0, d, 0.3, alpha, 0, jointtype, P);分组参数含义备注运动学theta / d / a / alpha关节角、连杆偏距、连杆长度、连杆转角单位统一用弧度运动学jointtypeR 转动副P 移动副决定 theta 还是 d 是变量运动学mdh0 标准 DH1 改进 DH选错会导致整条链偏移运动学offset关节变量零点偏移实物装配角不为零时必须填运动学qlim关节变量上下限不设会显示 0~0很多求解器直接报错动力学m / r / I连杆质量、3×1 质心坐标、3×3 惯性矩阵rne、inertia、fdyn 依赖这三个动力学B / Tc粘性摩擦力、库仑摩擦力1×1 或 2×1含电机侧时可给两元素电机G / Jm齿轮传动比、电机惯性矩做关节力矩换算时需要Link([theta, d, a, alpha, sigma])这种位置参数写法在老教程里很常见但mdh位置一改就容易整体错位我一般直接用名值对可读性也更好。3.2 SerialLink 组装、display 与 teach 的校验动作robot SerialLink(L, name, SixLink); robot.display(); % 打印 DH 表和关节限制先看单位是 rad 还是 deg q0 [0, pi/6, -pi/4, 0, pi/3, 0]; robot.plot(q0, workspace, [-1.5 1.5 -1.5 1.5 -0.5 2], scale, 0.6); robot.teach(); % 滑条交互用来快速判断各关节正负方向display的输出是第一个检查点qlim那一列如果全是 0说明前面没设qlimoffset那一行如果和实物装配角度差 90°逆解出来的姿态会整体转一个角度。teach界面里拖动滑条末端位姿实时显示把每个关节单独推到正方向看机械臂往哪边转比对着 DH 表推符号快得多。plot的workspace要按实际臂长给给太小机械臂会被裁掉一半给太大又看不清关节细节。3.3 fkine 正解ikine6s 与 ikine 的适用边界q_true [0.2, -0.3, 0.5, 0, 0.4, 0]; T robot.fkine(q_true); % 正解返回 SE3 q_ana robot.ikine6s(T); % 封闭解仅限 6 轴且满足三轴交于一点 q_num robot.ikine(T, q0, q_ana, mask, [1 1 1 1 1 1], ... tol, 1e-6, ilimit, 500); % 数值解 fprintf(位置误差 %.3e m\n, norm(transl(T) - transl(robot.fkine(q_num))));ikine6s走解析解快且稳定但前提是构型满足 Pieper 条件后三轴轴线交于一点腕部偏置的机械臂直接用它结果会是错的。ikine走数值迭代q0给初值mask指定需要满足的自由度只要求位置对齐时把后三位设 0 就行这在抓取平面物体时很实用。tol收敛容差和ilimit迭代上限是两个常调的参数容差给到 1e-6 以上基本够用迭代上限不够会出现「返回了结果但误差很大」的假成功所以要养成算完回代fkine检查的习惯。提示v10 里还提供了ikcon、ikunc这类基于数值优化的逆解代价函数里能加关节限位和避障项但依赖 Optimization Toolbox环境没装会直接报函数未定义。3.4 jacob0/jacobn 与可操作度、奇异位形检查q [0.3, -0.5, 0.7, 0, 0.4, 0.2]; J0 robot.jacob0(q); % 相对基坐标系6x6 Jn robot.jacobn(q); % 相对末端坐标系6x6 sv svd(J0); fprintf(最小奇异值 %.4f可操作度 %.4f\n, min(sv), sqrt(det(J0*J0)));jacob0前 3 行是线速度映射、后 3 行是角速度映射所以上半部分和下半部分的量纲不同不要直接拿整列做归一化比较。jacobn是同一个雅可比在末端坐标系下的表达做阻抗控制、力控时用末端表达的版本更顺手。最小奇异值接近 0 就是奇异位形此时逆解速度会飙到无穷大轨迹规划阶段可以在路径上采样几个点查一遍svd把危险区段提前标出来。4. 轨迹规划与动力学仿真jtraj、ctraj、rne 到 fdyn运动学通了以后下一步是让机械臂「动起来」。关节空间和笛卡尔空间两条路各有各的适用场景选错会出现关节速度突变或者末端走不出直线。动力学部分则决定了仿真结果能不能和真实控制器对上。4.1 关节空间 jtraj步数与时间向量两种调用q0 zeros(1, 6); qf [pi/3, -pi/4, pi/2, 0, pi/6, 0]; [q, qd, qdd] jtraj(q0, qf, 100); % 第三种参数是步数 t linspace(0, 2, 101); [q2, qd2, qdd2] jtraj(q0, qf, t); % 也可以直接传时间向量 robot.plot(q, fps, 30, trail, r-); plot(t, qd2); grid on; xlabel(t/s); ylabel(关节速度/(rad/s));jtraj默认生成五次多项式起止速度和加速度都为零所以曲线两端是平滑的。返回的q、qd、qdd都是 n 步 × n 关节的矩阵q(k,:)是第 k 步的关节角。传时间向量时返回的行数跟着t走适合和实测数据对齐采样率。想指定端点速度用jtraj(q0, qf, t, qd0, qd1)做连续轨迹拼接时把上一段的末速度传进来能避免停顿。4.2 笛卡尔空间 ctraj 与逆解串联的注意点T0 robot.fkine(q0); T1 transl(0.4, 0.1, 0.6) * troty(pi/2); Tc ctraj(T0, T1, 50); % 返回 1x50 的 SE3 数组 q_traj zeros(50, 6); for k 1:50 q_traj(k,:) robot.ikine(Tc(k), q0, q_traj(max(k-1,1),:), ... mask, [1 1 1 1 1 1]); endctraj对位置做线性插值、对姿态做插值描述的是末端在笛卡尔空间的过渡路径。这里有两个坑拼逆解时每一步的初值要取上一步的结果否则解会在不同分支之间跳画出来机械臂会突然翻个身路径经过或靠近奇异位形时关节速度会瞬间放大size(Tc)之后最好先扫一遍每步的svd(jacob0(q))再决定要不要改路径。4.3 逆动力学 rne 与 gravload/inertia/coriolis 的分量关系函数输入输出物理含义rneq, qd, qdd可选 grav、fext1×n 关节力矩逆动力学各项之和gravloadq1×n重力项 G(q)inertiaqn×n关节空间惯性矩阵 M(q)coriolisq, qdn×n科氏力与向心力耦合矩阵 C(q,qd)payloadM, P无在指定位置挂载质量 M 的载荷fdynT, torqfunt, q, qd正向动力学积分q [0.3, -0.5, 0.7, 0, 0.4, 0.2]; qd [0.1, 0.2, -0.1, 0, 0.3, 0.1]; qdd [0.5, -0.3, 0.2, 0, 0.1, -0.2]; tau robot.rne(q, qd, qdd); % 全部力矩 G robot.gravload(q); % 重力项 M robot.inertia(q); % 惯性矩阵 C robot.coriolis(q, qd); % 科氏/向心项 res tau - (M*qdd C*qd G); % 应为接近 0 的向量 fprintf(动力学方程残差范数 %.3e\n, norm(res));残差范数这句话就是动力学方程tau M(q)qdd C(q,qd)qd G(q)的自检。rne的第四个参数grav默认是[0 0 9.81]方向不对重力项会整体反号第五个参数fext是末端受到的外力/力矩取[Fx Fy Fz Mx My Mz]六维形式做打磨、装配这类接触仿真时填写。payload(M, P)是把载荷折算到连杆上的便捷入口比手动改link.m和link.r更不容易出错。4.4 fdyn 正向动力学PD 力矩函数怎么写才不发散正向动力学是「给力矩算运动」是搭关节控制器仿真的常用入口。function tau mytorqfun(t, q, qd, qstar, P, D) % qstar 为目标关节角P、D 为增益矩阵 tau P*(qstar - q) - D*qd; % 位置反馈 速度阻尼 endP 100*eye(6); D 20*eye(6); qstar [pi/3, -pi/4, pi/2, 0, pi/6, 0]; [t, q, qd] robot.fdyn(5, mytorqfun, qstar, P, D); plot(t, q); grid on; xlabel(t/s); ylabel(关节角/rad); legend(q1,q2,q3,q4,q5,q6);速度项前面必须是负号写成正号就成了正反馈几秒内关节角就会发散这是很多人第一次跑fdyn遇到的现象。增益矩阵用对角阵就够P 太大出现过冲和振荡D 太小则收敛慢一般先把 D 调到临界阻尼附近再加重 P。另外fdyn依赖Link里的m、r、I这些没填全时惯性矩阵接近奇异积分会直接失败跑之前先robot.dyn看一遍动力学参数是否完整。不同版本fdyn的返回值和参数顺序有调整如果返回列数不对用help fdyn确认当前签名。5. v10.4 命令差异与逆解不收敛的排查顺序版本差异是这套工具箱最耗时间的部分同一段代码在 v9 上跑得好好的换到 v10 就报错而报错信息往往指向一个看起来毫不相干的函数。5.1 返回类型与命名差异先用 class 和 help 确认最常见的一类差异是返回类型v9 里trotx、transl、fkine返回 double 矩阵v10 里统一返回 SE3 对象。旧代码里的T(1:3,4)、T*T、inv(T)都会因此失效前者报索引越界后面的可以用.T、T2 T1 * T2、inv(T)这类对象运算替代。写跨版本代码时加一层判断最稳if isa(T, SE3) Tnum T.T; % 取出底层 4x4 else Tnum T; % v9 直接就是矩阵 end命名差异集中在取角函数和逆解函数上tr2angvec的返回顺序、ikine的mask默认值、jacob0与jacobn的行排列方式都出现过调整。排查顺序建议是先class()看类型再help 函数名看签名最后doc 函数名看带交互示例的说明三级下来基本能定位。5.2 逆解不收敛的四步排查数值逆解失败时按下面顺序查比漫无目的地调参数效率高得多。第一步查单位DH 参数和qlim必须统一成弧度混用角度会让求解器在一个完全错误的构型附近找解表现为迭代到上限也不收敛。第二步查 DH 类型mdh从 0 改成 1 对应的是连杆坐标系定义方式的变化整条链会整体偏移症状是位置误差恒定且很大。第三步查初值与mask初值离真值太远时迭代会跑到另一个解分支mask全 1 表示六个自由度都要满足对不到位的自由度可以放宽。T_ref robot.fkine(q_true); q_sol robot.ikine(T_ref, q0, q_true 0.05*randn(1,6), ... tol, 1e-8, ilimit, 200, mask, [1 1 1 1 1 1]); % 角度差要绕到 [-pi, pi] 再比较避免 ±2pi 造成的假误差 dq mod(q_sol - q_true pi, 2*pi) - pi; fprintf(最大关节误差 %.3e rad\n, max(abs(dq)));第四步查奇异在奇异位形附近雅可比接近奇异求解器会在步长和容差之间反复震荡此时回到 3.4 节的方法扫一遍最小奇异值确认目标位姿本身是否可达。快速判断可达性还有个技巧先用ikine6s试一次能出解说明构型满足解析条件再拿它的结果当ikine的初值收敛率会明显提高。注意命令行里敲rtbdemo会打开一个交互式演示入口位姿、运动学、轨迹、动力学各模块都有可运行示例对照着改参数比翻 PDF 快尤其是确认某个函数的返回顺序时。本文还有配套的精品资源点击获取