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

先声明一下，这篇文章不是教科书式的复读，是我自己在做六轴机械臂和AR3这类开源机械臂时，把MDH建模从头到尾啃透之后，沉淀下来的一套“能落地”的思路。核心聚焦在坐标系怎么建、四个参数怎么填、矩阵怎么写、最后怎么乘出末端位姿，争取让你看完就能在仿真或实机上跑通正解。我尽量把每一步“为什么这么做”讲清楚，而不是只给公式让你背。

1. 为什么绕不开MDH建模

1.1 MDH是一种“给机械臂写身份证”的方式

机械臂运动学说白了就一件事：给你六个或者七个关节角度，让你算出末端抓手在哪、朝向哪。要做到这件事，你得先给每一个关节、每一个连杆定义一套统一的坐标系，然后通过几次连续的坐标系变换，把末端的位姿“翻译”到基座坐标下来。这整套规则就是D-H建模，而MDH就是它的改良版本。

D-H方法早在上世纪五十年代就被提出来了，最初用在工业机器人上，后来衍生出标准DH（SDH）和改进DH（MDH）两种写法。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的远端（也就是关节i+1处）。意味着第i个关节的theta旋转会直接改变第i个坐标系相对于第i-1个坐标系的姿态，这个关系和通常直觉里“关节驱动连杆”的顺序是吻合的，所以实际计算时更不容易绕晕。

2. 坐标系建立：从零开始给每个关节定“家”

2.1 建系规则就五步，按顺序来错不了

我们要给n自由度机械臂建立从基座到末端一共n+1个坐标系（基座坐标系为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都为0，a0=0、a1=1。

T_01矩阵在theta1=30度时：

| cos30° -sin30° 0 0 | | sin30° cos30° 0 0 | | 0 0 1 0 | | 0 0 0 1 |

T_12矩阵在theta2=45度时：

| cos45° -sin45° 0 1 | | sin45° cos45° 0 0 | | 0 0 1 0 | | 0 0 0 1 |

T_02 = T_01 * T_12，计算出来第三列前三个元素是位置向量。你可以验证一下：当theta1=30度、theta2=45度时，末端位置按几何法算应该是(1cos30° + 1cos75°, 1sin30° + 1sin75°, 0)，这正好对应旋转矩阵和位置向量相乘的结果。这个小例子告诉你：MDH矩阵计算与几何画图结果完全一致，先建立这个验证习惯，再上六轴就心里有底了。

4.2 六轴机械臂的MDH参数表实测

现在我给出一个典型六轴机械臂的MDH参数表（数值仅为演示用，不代表真实机型）。这里我用的是标准MDH约定，所有角度单位是弧度，长度单位是毫米。注意alpha和a带下标i-1，d和theta带下标i，这个下标关系也是新手最容易混淆的地方。

这张表建立的关键点在于：关节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参数表里theta=0的几何状态。这样就会导致仿真末端位姿和实机末端位姿对不上。解决方法是把实际机械臂摆到一个你已知末端位姿的参考姿态（比如竖直向上），记录此时各关节读数，把该读数作为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. 从正运动学到后续扩展

正运动学只是机械臂控制的地基，但它管着后续几乎所有模块的输入输出：逆运动学求解时需要在每个迭代步计算末端位置和雅可比矩阵；轨迹规划时需要在笛卡尔空间插值然后用逆解得到关节轨迹，每一点都要计算正运动学；力控和重力补偿需要根据各关节角度和连杆质量分布计算每个关节的重力矩，这也离不开正运动学的中间结果。所以我常说，把正运动学算得又快又准，后面所有东西都顺了。

如果还想继续深挖，可以从这几个方向入手：雅可比矩阵的求解，它本质上是对正运动学矩阵对关节角求偏导数，用来做速度级逆解和奇异位形分析；手眼标定，本质上是求解AX=XB形式的矩阵方程，需要用到你对工具坐标系和相机坐标系的精确建模；还有强化学习环境里的状态表示，很多RL算法的观测空间就包含末端位姿的向量形式，这时候正运动学代码的性能就尤为重要。

我自己在实际操作中还有一个习惯：每次拿到一台新机械臂，第一件事就是建MDH模型，然后写一个简单的Python脚本，把末端在各类节角度下的坐标打印出来，再和官方SDK返回的坐标做对比。如果误差小于1毫米，说明模型可靠，可以从容地进行下一步。如果是自己拼装的机械臂，这个对比过程能帮你及时发现装配偏差——比如某个连杆的a其实不是图纸上的值，而是装配偏移后的值。

最后再分享一个小技巧：在正运动学矩阵计算的基础上，可以顺手加一个“手写求导版雅可比”，用有限差分法验证正确性。这样在后续做轨迹规划或力控时，你会有一个经过验证的高质量运动学核心，省下来的调试时间远超当初建模花掉的时间。