3、正运动学建模：D-H参数法、标准D-H与改进D-H的区别、正运动学求解流程与实例
正运动学（Forward Kinematics）解决的是“给定关节角度，求末端执行器位姿”的问题。它是机器人控制、轨迹规划、碰撞检测与离线编程的数学基石。在工业机器人、协作机械臂乃至仿人腿足系统中，D-H（Denavit-Hartenberg）参数法是最经典、最通用的建模工具。本章将系统讲解D-H参数的定义、标准D-H与改进D-H的核心差异、完整的求解流程，并通过一个六轴关节机器人的实例，演示从连杆坐标系建立到末端位姿矩阵计算的完整闭环。
3.1 D-H参数法的核心原理
D-H参数法的本质是用四个独立参数来描述相邻两个连杆坐标系之间的相对位姿关系。这四个参数分别是：关节角θᵢ（绕Z轴旋转角）、连杆偏距dᵢ（沿Z轴平移距离）、连杆长度aᵢ（沿X轴平移距离）、连杆扭角αᵢ（绕X轴旋转角）。其中θᵢ和dᵢ是关节变量（旋转关节θᵢ可变，移动关节dᵢ可变），aᵢ和αᵢ由机械结构决定，是固定常量。
理解D-H参数的关键在于坐标系建立规则。对于标准D-H法，坐标系{i}的原点位于关节i+1的轴线上，Zᵢ轴与关节i+1的轴线重合，Xᵢ轴沿aᵢ方向从关节i指向关节i+1。这种“前置”定义方式使得变换矩阵的推导顺序为：先绕Zᵢ₋₁旋转θᵢ，再沿Zᵢ₋₁平移dᵢ，然后沿Xᵢ平移aᵢ，最后绕Xᵢ旋转αᵢ。而改进D-H法将坐标系{i}固定在连杆i-1上，Zᵢ轴与关节i的轴线重合，变换顺序变为：先绕Xᵢ₋₁旋转αᵢ₋₁，再沿Xᵢ₋₁平移aᵢ₋₁，然后绕Zᵢ旋转θᵢ，最后沿Zᵢ平移dᵢ。
核心记忆点：标准D-H的变换是“先绕Z后绕X”，改进D-H的变换是“先绕X后绕Z”。这个顺序差异直接决定了参数表中a和α的下标不同——标准D-H中aᵢ和αᵢ描述的是连杆i自身的几何属性，而改进D-H中aᵢ₋₁和αᵢ₋₁描述的是连杆i-1的几何属性。
3.2 标准D-H与改进D-H的对比分析
两种方法在数学上完全等价，都能正确描述机器人的运动学关系，但在工程应用中存在显著差异。标准D-H法在串联机器人中应用更广泛，因为它的坐标系定义更符合“从基座到末端”的直觉思维；改进D-H法在并联机器人和闭环机构中更具优势，因为它能更自然地处理分支链路的坐标系分配。
| 对比维度 | 标准D-H（SDH） | 改进D-H（MDH） |
|---|---|---|
| 坐标系固定位置 | 坐标系{i}固定在连杆i上 | 坐标系{i}固定在连杆i-1上 |
| Z轴定义 | Zᵢ沿关节i+1轴线 | Zᵢ沿关节i轴线 |
| X轴定义 | Xᵢ沿aᵢ方向（从关节i到i+1） | Xᵢ沿aᵢ₋₁方向（从关节i-1到i） |
| 变换顺序 | RotZ(θᵢ)→TransZ(dᵢ)→TransX(aᵢ)→RotX(αᵢ) | RotX(αᵢ₋₁)→TransX(aᵢ₋₁)→RotZ(θᵢ)→TransZ(dᵢ) |
| 参数下标 | aᵢ、αᵢ描述连杆i | aᵢ₋₁、αᵢ₋₁描述连杆i-1 |
| 适用场景 | 串联开链、树形结构 | 并联机构、闭环链、移动机器人 |
| 建模直观性 | 从基座到末端逐级递推，直观 | 需要更多空间想象，但便于处理分支 |
在实际工程中，选择哪种方法主要取决于两个因素：一是机器人制造商提供的技术文档采用哪种约定；二是后续算法（如逆解、雅可比矩阵）的推导习惯。如果混用两种方法，会导致参数表混乱、坐标系错位，甚至出现无法求解的奇异问题。因此，一旦选定方法，整个项目必须保持一致。
3.3 正运动学求解流程与实例拆解
下面以一个典型的六轴串联关节机器人（类似常见的工业机械臂构型）为例，演示完整的正运动学求解流程。该机器人的关节配置为：J1绕Z₀轴旋转（腰部）、J2绕Z₁轴旋转（肩部）、J3绕Z₂轴旋转（肘部）、J4绕Z₃轴旋转（腕部俯仰）、J5绕Z₄轴旋转（腕部偏航）、J6绕Z₅轴旋转（末端法兰旋转）。
- 确定连杆参数：根据机械图纸，测量得到各连杆长度aᵢ、扭角αᵢ、偏距dᵢ。例如：a₁=150mm，a₂=600mm，a₃=120mm，d₁=350mm，d₄=620mm，d₆=100mm。
- 建立坐标系：按照标准D-H规则，在基座、各关节轴线和末端法兰处建立坐标系{Z₀}至{Z₆}。特别注意Zᵢ轴必须与关节i+1的旋转轴线重合。
- 填写D-H参数表：将测量值填入表格，关节角θᵢ为变量，其余为常量。
- 计算相邻变换矩阵：对每个关节i，代入公式计算ᵢ⁻¹Tᵢ矩阵。
- 连乘得到末端位姿：计算⁰T₆ = ⁰T₁ · ¹T₂ · ²T₃ · ³T₄ · ⁴T₅ · ⁵T₆。
- 验证：将零位角度代入，检查末端位置是否与机械零位一致。
| 关节i | θᵢ（变量） | dᵢ (mm) | aᵢ (mm) | αᵢ (°) |
|---|---|---|---|---|
| 1 | θ₁ | 350 | 150 | -90 |
| 2 | θ₂ | 0 | 600 | 0 |
| 3 | θ₃ | 0 | 120 | -90 |
| 4 | θ₄ | 620 | 0 | 90 |
| 5 | θ₅ | 0 | 0 | -90 |
| 6 | θ₆ | 100 | 0 | 0 |
以关节1的变换矩阵为例，代入标准D-H公式：
⁰T₁ = RotZ(θ₁) · TransZ(350) · TransX(150) · RotX(-90°)
= [cosθ₁ -sinθ₁cosα₁ sinθ₁sinα₁ a₁cosθ₁]
[sinθ₁ cosθ₁cosα₁ -cosθ₁sinα₁ a₁sinθ₁]
[0 sinα₁ cosα₁ d₁ ]
[0 0 0 1 ]
代入α₁=-90°后：
= [cosθ₁ 0 -sinθ₁ 150cosθ₁]
[sinθ₁ 0 cosθ₁ 150sinθ₁]
[0 -1 0 350 ]
[0 0 0 1 ]同理计算其余五个变换矩阵，最终连乘得到⁰T₆。当所有关节角为零时，末端位置应落在(150+600+120, 0, 350+620+100) = (870, 0, 1070)mm处，姿态矩阵为单位阵。这个结果可以作为后续编程调试的基准校验值。
3.4 常见误区与工程复盘
在正运动学建模中，最常见的错误集中在坐标系建立和参数符号上。第一个误区是Z轴方向定义错误——Z轴必须与关节旋转轴线重合，且方向遵循右手定则，但很多初学者会混淆相邻关节的Z轴方向。第二个误区是aᵢ和αᵢ的符号处理——aᵢ始终为正值（连杆长度），但αᵢ的正负取决于相邻Z轴的旋转方向，需要仔细判断。第三个误区是零位角度不一致——机械零位与数学零位可能不同，必须在参数表中明确标注。
风险提示：在将D-H参数表转换为代码时，务必注意角度单位（弧度/度）的统一。一个典型的工程事故是：某工程师在Python中使用math.cos()时直接传入角度值，导致所有关节角被当作弧度处理，末端位置偏差达到数百毫米。建议在代码入口处统一转换为弧度，并添加单位断言。
另一个容易被忽视的问题是坐标系之间的“隐藏偏移”。例如，当两个相邻关节轴线平行时，aᵢ和αᵢ的取值会退化为简单形式，但若机械结构存在加工误差，实际轴线并非完全平行，此时D-H参数法会产生建模误差。对于高精度应用，建议在建模后使用激光跟踪仪进行标定，将实际参数与理论参数对比修正。
复盘一个实际案例：某协作机器人项目在调试中发现，末端执行器在特定姿态下出现约2mm的位置偏差。排查过程如下：首先检查D-H参数表，发现J4的d₄值被误写为600mm（实际为620mm）；修正后偏差减小到0.5mm；进一步检查发现J2的α₂应为0°，但机械装配时存在0.3°的倾斜，导致末端在Z方向产生累积误差。最终通过运动学标定软件重新辨识参数，将误差控制在0.1mm以内。这个案例说明，D-H参数不仅是理论计算值，更是需要实测验证的工程参数。
经验总结：在完成正运动学建模后，务必进行三项验证：①零位验证——所有关节角为零时，末端位姿应与机械图纸一致；②单轴验证——只转动一个关节，观察末端轨迹是否为一个圆弧（旋转关节）或直线（移动关节）；③随机验证——在关节空间随机采样多组角度，用仿真软件（如RoboDK、MATLAB Robotics Toolbox）对比计算结果。这三步能有效发现90%以上的建模错误。
正运动学建模是机器人学中最基础也最关键的技能。掌握D-H参数法的核心在于理解坐标系变换的本质，而不是死记公式。建议学习者在纸上手动推导一个两连杆机构的完整变换过程，再逐步扩展到六轴机器人。只有亲手计算过每一个矩阵元素，才能真正理解正运动学的数学结构，为后续的逆运动学求解、速度雅可比分析和动力学建模打下坚实基础。