机械臂MDH建模与正运动学：从坐标系到末端位姿的完整指南

先声明一下这篇文章不是教科书式的复读是我自己在做六轴机械臂和AR3这类开源机械臂时把MDH建模从头到尾啃透之后沉淀下来的一套“能落地”的思路。核心聚焦在坐标系怎么建、四个参数怎么填、矩阵怎么写、最后怎么乘出末端位姿争取让你看完就能在仿真或实机上跑通正解。我尽量把每一步“为什么这么做”讲清楚而不是只给公式让你背。1. 为什么绕不开MDH建模1.1 MDH是一种“给机械臂写身份证”的方式机械臂运动学说白了就一件事给你六个或者七个关节角度让你算出末端抓手在哪、朝向哪。要做到这件事你得先给每一个关节、每一个连杆定义一套统一的坐标系然后通过几次连续的坐标系变换把末端的位姿“翻译”到基座坐标下来。这整套规则就是D-H建模而MDH就是它的改良版本。D-H方法早在上世纪五十年代就被提出来了最初用在工业机器人上后来衍生出标准DHSDH和改进DHMDH两种写法。MDH的全称是Modified Denavit-Hartenberg属于Craig在《Introduction to Robotics》里推广的经典形式。它跟SDH的核心区别在于坐标系固定在连杆的前一个关节还是后一个关节。这个区别看起来小但直接影响参数含义和矩阵组装方式。我见过不少新手一上来就对着DH参数表套公式结果末端坐标完全不对原因基本都出在“坐标系怎么建”这一步上。MDH模型对轴线不平行、有偏置的关节尤其友好这也是现在ROS、PyBullet、MATLAB Robotics Toolbox里大多数机械臂模型都采用MDH的原因。你把这个坐标系建立规则吃透了后面看URDF里那些 和 配置、看别人写的运动学库源码都会顺畅很多。1.2 四个参数到底在描述什么东西MDH每个关节只需要四个参数括号里分别为连杆长度a、连杆扭角alpha、关节距离d、关节转角theta。这四个值看起来抽象其实各有几何意义a沿X轴测量的两轴前一个Z轴与当前Z轴之间的公垂线长度。你可以理解成连杆本身的“臂展”单位通常是米或毫米。alpha绕X轴旋转让前一个Z轴方向对齐当前Z轴方向所需的角度。它描述的是两个关节轴之间的空间夹角单位弧度。d沿Z轴测量的两个公垂线前一关节X轴与当前关节X轴之间的距离反映关节之间的“伸长量”。theta绕Z轴旋转让前一个X轴方向对齐当前X轴方向所需的角度。对转动关节来说它是变量对移动关节来说d才是变量。我需要特别强调一下MDH中每个坐标系i的原点定义在连杆i的远端也就是关节i1处。意味着第i个关节的theta旋转会直接改变第i个坐标系相对于第i-1个坐标系的姿态这个关系和通常直觉里“关节驱动连杆”的顺序是吻合的所以实际计算时更不容易绕晕。2. 坐标系建立从零开始给每个关节定“家”2.1 建系规则就五步按顺序来错不了我们要给n自由度机械臂建立从基座到末端一共n1个坐标系基座坐标系为0号末端工具坐标系为n号。MDH建系的具体操作我总结成五个步骤第一画出所有关节轴线并编号。转动关节的轴线是它旋转所围绕的那条直线移动关节的轴线是它移动方向所在的那条直线。对所有关节轴你需要标出方向一般用右手定则确定Z轴正方向。第二从基座开始确定每个坐标系的Z轴。按照MDH规则坐标系{i-1}的Z轴与关节i的轴线重合。比如3自由度机械臂坐标系0的Z0沿关节1轴线坐标系1的Z1沿关节2轴线以此类推。这一步最核心很多人栽跟头就是因为Z轴方向选反了或者没让Z轴严格与关节轴线共线。第三确定X轴。对于坐标系{i-1}它的X轴方向沿Z_{i-1}和Z_i的公垂线方向。如果Z_{i-1}和Z_i相交或平行那么公垂线可能退化为一个点或任意选一条垂直于两者的方向。处理平行情况时我习惯让X轴沿Z_{i-1}指向Z_i的方向处理相交情况时X轴方向取两者叉积方向右手定则。如果两者平行且方向相同叉积为零向量此时可以选择与前一个坐标系共面的方向保证一致性即可。第四确定原点。坐标系{i-1}的原点取公垂线与Z_i的交点或者当Z_{i-1}与Z_i相交时取交点。对于基座坐标系0通常让原点放在关节1轴线与关节2轴线的公垂线交点上或者干脆放在关节1的轴线上某个方便的位置。末端坐标系n的原点放在末端执行器的抓手中心或工具中心点这个会影响后续手眼标定时的工具坐标系转换建议一开始就规划好。第五确定Y轴。X轴和Z轴都定了Y轴直接通过叉积Y Z × X得到。这样保证坐标系是右手系后面计算旋转矩阵时符合所有标准库的约定。2.2 一个实例六轴机械臂的坐标系布局我用常见的六轴关节型机械臂来演示一下坐标系怎么建。假设它的关节1绕竖直轴旋转Z0竖直向上关节2和3绕水平平行轴旋转形成肩部和肘部关节4、5、6构成腕部其中关节4和6的轴线平行且都垂直于关节5轴线。按MDH规则Z0沿关节1竖直轴向上Z1沿关节2水平轴向里Z2沿关节3水平轴向里Z3沿关节4轴线方向与Z2垂直。然后腕部这里比较关键坐标系3的X3沿Z2指向Z3的公垂线方向坐标系4的X4沿Z3指向Z4的公垂线方向但因为Z3和Z4相交于腕部某点所以X4方向要根据叉积来确定。实际建模中关节4、5、6的轴线会相交于腕部中心附近这样设计的好处是腕部运动不会改变末端位置只会改变姿态轨迹规划会方便很多。建立完坐标系后我会挨个把每个关节的a、alpha、d、theta填进表里。这里有个检查技巧每填一个参数就在心里模拟一次从坐标系{i-1}变换到坐标系{i}的过程看看X轴方向对不对、旋转方向是否符合右手定则。如果某个坐标系绕X轴旋转角度为负但参数表填成正的后面矩阵算出来一定是错的。3. 正运动学矩阵从参数表到变换矩阵3.1 单个连杆变换矩阵的推导逻辑MDH建模中从坐标系{i-1}到坐标系{i}的齐次变换矩阵是一个4x4矩阵由绕X轴旋转、沿X轴平移、绕Z轴旋转、沿Z轴平移四个基本变换依次组合而成。具体的矩阵形式如下T_i RotX(alpha_{i-1}) * TransX(a_{i-1}) * RotZ(theta_i) * TransZ(d_i)展开后就是标准的4x4矩阵| cos(theta_i) -sin(theta_i) 0 a_{i-1} | | sin(theta_i)*cos(alpha_{i-1}) cos(theta_i)*cos(alpha_{i-1}) -sin(alpha_{i-1}) -sin(alpha_{i-1})*d_i | | sin(theta_i)*sin(alpha_{i-1}) cos(theta_i)*sin(alpha_{i-1}) cos(alpha_{i-1}) cos(alpha_{i-1})*d_i | | 0 0 0 1 |这里每个子矩阵的含义都对应一步坐标系变换。RotX(alpha)把上一个坐标系的Z轴方向调整到当前坐标系Z轴方向TransX(a)把原点沿X轴平移两个关节轴线公垂线的距离RotZ(theta)把X轴方向调整到当前坐标系X轴方向TransZ(d)把原点沿Z轴平移到当前坐标系原点。有人会问为什么顺序一定是“绕X、沿X、绕Z、沿Z”而不是别的顺序实际上这是从MDH的参数定义推导出来的alpha和a描述的几何关系发生在两个Z轴之间必须先用RotX和TransX去处理theta和d描述的是同一个坐标系内部绕当前Z轴的变换放在后面。调换顺序会破坏参数的真实几何含义。这个顺序对应了变换的“链式法则”每一步都是在当前坐标系下进行左乘所以矩阵书写的顺序和变换执行顺序一致不能乱。3.2 矩阵乘法顺序为什么是“从基座乘到末端”正运动学的最终目标是得到末端坐标系{n}相对于基座坐标系{0}的位姿即T_06 T_01 * T_12 * T_23 * T_34 * T_45 * T_56。这里的矩阵乘法从左到右依次进行前一个变换的结果不断被后一个变换作用最终把末端的坐标“层层翻译”回基座坐标系。这个乘法顺序一定要记牢。我见过有人把顺序写成T_01 * T_02 * T_03这种“跳级乘法”导致结果完全对不上。正确的思路是每个T矩阵描述的是相邻坐标系之间的关系要得到非相邻关系必须从基座开始逐个左乘直到目标坐标系。这就像你从家里出发去公司先走A路再走B路每条路的导航都是相对于当前所在位置给出的一个接一个叠加最后才能得到终点相对于起点的位置。还有一个容易混淆的点矩阵乘法不满足交换律所以T_01 * T_12和T_12 * T_01结果完全不同。前者表示先执行第一个关节的变换再执行第二个关节的变换后者正好反过来。机械臂的关节变换是有严格顺序的——先关节1后关节2所以必须保持“从左到右、从基座到末端”的顺序。实际用Python的NumPy计算时可以写循环连乘也可以用functools.reduce一行搞定。3.3 末端位姿的提取位置和姿态怎么读T_06是一个4x4矩阵左上3x3是旋转矩阵R前三行第四列是位置向量p。旋转矩阵的每一列分别代表末端坐标系的X、Y、Z轴在基座坐标系下的投影位置向量代表末端坐标系原点工具中心点在基座坐标系下的坐标。要从旋转矩阵中提取欧拉角比如roll-pitch-yaw需要根据旋转矩阵元素反解。不同机械臂库使用的欧拉角约定可能不同比如ROS的tf库和MATLAB的Robotics Toolbox在提取时使用的约定就有差异。最稳妥的方式是直接使用矩阵做三维向量变换不要轻易转成欧拉角因为欧拉角在奇异点pitch接近±90度时会发生退化导致角度跳变。做轨迹规划时如果一定要用欧拉角我建议在关节空间做插值然后通过正运动学逐点算出末端位姿避免欧拉角插值带来的方向抖动。4. 完整算一遍从关节角到末端矩阵4.1 一个2自由度平面机械臂的不完整试算为了演示计算流程我用一个简化的2自由度平面机械臂来跑一遍在实际六轴之前先建立直觉。假设该机械臂两个关节轴线平行连杆长度都为1米角度变量为theta1和theta2所有alpha都为0所有d都为0a00、a11。T_01矩阵在theta130度时| cos30° -sin30° 0 0 | | sin30° cos30° 0 0 | | 0 0 1 0 | | 0 0 0 1 |T_12矩阵在theta245度时| cos45° -sin45° 0 1 | | sin45° cos45° 0 0 | | 0 0 1 0 | | 0 0 0 1 |T_02 T_01 * T_12计算出来第三列前三个元素是位置向量。你可以验证一下当theta130度、theta245度时末端位置按几何法算应该是(1cos30° 1cos75°, 1sin30° 1sin75°, 0)这正好对应旋转矩阵和位置向量相乘的结果。这个小例子告诉你MDH矩阵计算与几何画图结果完全一致先建立这个验证习惯再上六轴就心里有底了。4.2 六轴机械臂的MDH参数表实测现在我给出一个典型六轴机械臂的MDH参数表数值仅为演示用不代表真实机型。这里我用的是标准MDH约定所有角度单位是弧度长度单位是毫米。注意alpha和a带下标i-1d和theta带下标i这个下标关系也是新手最容易混淆的地方。关节ialpha_{i-1} (rad)a_{i-1} (mm)d_i (mm)theta_i (rad)100330theta12-π/2350theta2 - π/2301300theta34-π/2-15305theta45π/200theta56-π/2055theta6这张表建立的关键点在于关节2的theta初始偏置是-π/2这是为了让机械臂在“零位”姿态时各连杆自然伸展而不是蜷缩在一起。你去读很多厂商的URDF文件会发现他们在joint的origin里也经常加一些固定的旋转偏置本质上就是同一个道理。实操中如果没有给出零位对应的theta偏置最简单的做法是先让机械臂摆出一个已知的、好测量的姿态比如竖直站立然后反解出各关节角再把这个角作为零位偏置填进参数表。4.3 用Python算正运动学有了参数表写代码就是几分钟的事。我习惯用NumPy手写矩阵不用现成的机器人库这样每一步都有掌控感。下面给出一段可运行的正运动学代码import numpy as np def dh_matrix(alpha, a, d, theta): ct np.cos(theta) st np.sin(theta) ca np.cos(alpha) sa np.sin(alpha) return np.array([ [ct, -st, 0, a], [st*ca, ct*ca, -sa, -sa*d], [st*sa, ct*sa, ca, ca*d], [0, 0, 0, 1] ]) # MDH参数表 alpha [0, -np.pi/2, 0, -np.pi/2, np.pi/2, -np.pi/2] a [0, 35, 130, -15, 0, 0] d [330, 0, 0, 305, 0, 55] theta [np.deg2rad(30), np.deg2rad(-60), np.deg2rad(30), np.deg2rad(90), np.deg2rad(90), np.deg2rad(0)] T np.eye(4) for i in range(6): T T dh_matrix(alpha[i], a[i], d[i], theta[i]) print(末端位置:, T[:3, 3]) print(末端姿态:, T[:3, :3])这一段代码就是把前面的公式直接翻译成程序。每次循环处理一个关节连乘顺序就是从基座到末端。我在实际项目中频繁调用这个函数配合机械臂SDK读取各关节角度就能实时计算末端坐标用来做可视化反馈或者碰撞检测的粗筛。4.4 验证结果的几个土办法算完矩阵后不急着仿真先在脑子里过三个自检第一把六个关节角度全部设为零看看末端位置是否和机械臂的“零位伸展”状态一致。比如很多六轴机械臂在零位时末端应该垂直向上或水平向前你可以对照实机的初始姿态确认。第二只改变第一个关节的角度看末端位置是否在一个水平圆上运动如果Z坐标也变了说明某个alpha或者d填错了。第三单个关节转30度看看末端位移的指向是否正确这能快速定位是哪个参数导致方向反了。这三个检查方法我从入门用到现在可以说救了无数次建模事故。矩阵乘法在计算机里是绝对精确的但参数表里任何一个符号错了计算结果就会差之千里。手工核对参数表是正运动学避坑成本最低的一步。5. 绕开MDH建模的经典大坑5.1 坐标系方向搞反MDH里Z轴方向的选择其实不是唯一的比如竖直关节既可以朝上也可以朝下水平关节既可以朝左也可以朝右。Z轴选反了会影响theta的初始角度但不会影响末端结果前提是你把theta的零位对应关系一并调整过来。麻烦的是X轴方向很多人会用叉积来确定X轴方向但叉积顺序错了会导致X轴朝反方向进而让alpha出现正负号翻转。我的建议是一旦选定所有Z轴方向X轴方向只遵循“从第i-1个坐标系指向第i个坐标系”的公垂线方向不要去套叉积公式。叉积公式只在两者相交没有公垂线时才使用。平行轴时公垂线方向是明确的直接按几何方向写垂直轴时取交叉乘积前一定要确认右手定则。这方面建议在纸上画一遍所有轴线把每个X轴箭头标好再填参数表。5.2 d和a的正负号陷阱MDH里a和d有明确的方向性a沿X轴方向d沿Z轴方向因此它们可以是负数。比如某些机型的关节4的a是负值表示坐标系3的原点在坐标系4原点的“负X方向”一侧。参数表里a是-15 mm这个负号非常重要忽略之后虽然矩阵看起来还是“能算出来”但末端会出现一个看起来很小但就是不对的误差而且这个误差会随着后续关节角度的变化被放大。处理办法从厂商给的URDF或者CAD模型里读取连杆尺寸时不要把“绝对长度”直接填入a和d而是根据坐标系方向判断是正还是负。判断方法无他就是让a的方向和坐标系的X轴正方向一致如果实际轴线偏移和目标X轴方向相反填负值。5.3 零位偏置和关节限位不一致很多新手在仿真里给机械臂设初始关节角为0但实际机械臂的电机关节位置传感器在零点位置并不一定对应MDH参数表里theta0的几何状态。这样就会导致仿真末端位姿和实机末端位姿对不上。解决方法是把实际机械臂摆到一个你已知末端位姿的参考姿态比如竖直向上记录此时各关节读数把该读数作为MDH参数表的“偏置项”在正运动学计算前先把它减掉再传入theta。我的经验是在软件架构里不要把偏置硬编码在theta计算中而是在读取关节角度后统一平移。比如motor_angle[i] - zero_offset[i]作为模型的theta输入这样以后校准时只改zero_offset数组不用改动运动学代码。AR3、Panda、UR这些开源机型都有类似的处理手法读源码时会发现它们存在home offset参数就是干这个用的。5.4 SDK或URDF中使用的DH约定和自己不一致你在ROS里加载一个机械臂模型它底层用的可能是标准DH而你手里拿到的参数表是MDH两者直接混用会导致灾难性错误。URDF本身描述的是关节和连杆的几何关系并不直接告诉你DH参数但在第三方运动学库中经常有DH参数和URDF互相转换的代码容易搞混。同时注意有些库的参数顺序是alpha、a、d、theta有些是d、theta、a、alpha复制代码时一定要看函数签名不要想当然。我的建议是在一个项目里统一使用MDH只在读取URDF时用它生成碰撞体和可视化网格运动学计算走自己的MDH代码。这样即使URDF更新你的正运动学核心逻辑也稳定可控。6. 从正运动学到后续扩展正运动学只是机械臂控制的地基但它管着后续几乎所有模块的输入输出逆运动学求解时需要在每个迭代步计算末端位置和雅可比矩阵轨迹规划时需要在笛卡尔空间插值然后用逆解得到关节轨迹每一点都要计算正运动学力控和重力补偿需要根据各关节角度和连杆质量分布计算每个关节的重力矩这也离不开正运动学的中间结果。所以我常说把正运动学算得又快又准后面所有东西都顺了。如果还想继续深挖可以从这几个方向入手雅可比矩阵的求解它本质上是对正运动学矩阵对关节角求偏导数用来做速度级逆解和奇异位形分析手眼标定本质上是求解AXXB形式的矩阵方程需要用到你对工具坐标系和相机坐标系的精确建模还有强化学习环境里的状态表示很多RL算法的观测空间就包含末端位姿的向量形式这时候正运动学代码的性能就尤为重要。我自己在实际操作中还有一个习惯每次拿到一台新机械臂第一件事就是建MDH模型然后写一个简单的Python脚本把末端在各类节角度下的坐标打印出来再和官方SDK返回的坐标做对比。如果误差小于1毫米说明模型可靠可以从容地进行下一步。如果是自己拼装的机械臂这个对比过程能帮你及时发现装配偏差——比如某个连杆的a其实不是图纸上的值而是装配偏移后的值。最后再分享一个小技巧在正运动学矩阵计算的基础上可以顺手加一个“手写求导版雅可比”用有限差分法验证正确性。这样在后续做轨迹规划或力控时你会有一个经过验证的高质量运动学核心省下来的调试时间远超当初建模花掉的时间。