📚 课程目录第3章正运动学

3、正运动学：D-H参数法、建立连杆坐标系、求解末端执行器位姿，并通过典型六轴机械臂案例进行推导

正运动学（Forward Kinematics）解决的核心问题是：给定机器人各关节的关节变量（旋转角或平移量），求解末端执行器在基坐标系下的位置与姿态。它是机器人控制、轨迹规划、动力学分析以及视觉伺服的基础。在众多运动学建模方法中，Denavit-Hartenberg（D-H）参数法以其系统化、标准化和适合计算机编程的特点，成为工业机器人建模的主流方法。本节将深入讲解D-H参数的定义、连杆坐标系的建立规则、齐次变换矩阵的推导过程，并以典型的六轴垂直关节型机械臂（如类似ABB、KUKA、FANUC的构型）为例，完整演示从参数表到末端位姿矩阵的求解流程。

_i-1 连杆长度 α_i-1 连杆扭转角 d_i 关节偏置 θ_i 关节角连杆坐标系建立 Z_i 轴沿关节轴线 X_i 轴沿公垂线 Y_i 由右手定则确定原点位于关节轴交点齐次变换矩阵 T_i = Rot(z,θ)·Trans(z,d) ·Trans(x,a)·Rot(x,α) 4×4 矩阵末端位姿 T₀⁶ = T₁·T₂ ·T₃·T₄ ·T₅·T₆ 标准流程： ① 确定关节轴线 → ② 建立各连杆坐标系 → ③ 提取D-H参数表 → ④ 逐关节计算变换矩阵 → ⑤ 连乘得到末端位姿基座 J1 J2 J3 J4 J5 J6 末端旋转关节（J1-J3 大臂）旋转关节（J4-J6 腕部）末端执行器连杆

3.1 D-H参数法的核心原理与四种参数定义

D-H参数法由Denavit和Hartenberg于1955年提出，其核心思想是通过四个运动学参数来描述相邻两个连杆坐标系之间的相对位姿关系。这四个参数分别是：连杆长度 a_i-1、连杆扭转角 α_i-1、关节偏置 d_i 和 关节角 θ_i。需要特别强调的是，这四个参数并非随意定义，而是具有严格的几何意义：a_i-1 是沿 X_i-1 轴方向，从 Z_i-1 轴移动到 Z_i 轴的距离；α_i-1 是绕 X_i-1 轴，从 Z_i-1 轴旋转到 Z_i 轴的角度；d_i 是沿 Z_i 轴方向，从 X_i-1 轴移动到 X_i 轴的距离；θ_i 是绕 Z_i 轴，从 X_i-1 轴旋转到 X_i 轴的角度。

在实际应用中，我们通常使用改进型D-H参数（Modified D-H），其变换顺序为：先绕 X_i-1 轴旋转 α_i-1，再沿 X_i-1 轴平移 a_i-1，然后绕 Z_i 轴旋转 θ_i，最后沿 Z_i 轴平移 d_i。这种表示方法在描述树形结构或闭链机构时更为方便。而标准型D-H参数（Standard D-H）的变换顺序则不同，它先绕 Z 轴旋转和平移，再绕 X 轴旋转和平移。两种方法在参数定义上存在差异，但最终得到的末端位姿矩阵是等价的。对于六轴串联机械臂，两种方法均可使用，但改进型D-H参数在工业机器人建模中更为常见，因为它能更直观地反映连杆之间的几何关系。

  关键理解：D-H参数的本质是将复杂的空间运动分解为四个简单的子运动（两次旋转和两次平移），每个子运动都对应一个基本的齐次变换矩阵。通过矩阵连乘，我们可以将关节空间映射到笛卡尔空间。这种分解方法使得运动学建模变得高度规范化，即使面对不同构型的机器人，只要按照规则建立坐标系并提取参数，就能用统一的数学框架求解。

3.2 连杆坐标系的建立规则与操作流程

建立正确的连杆坐标系是D-H参数法的前提，也是初学者最容易出错的地方。以下是标准化的建立流程：

确定关节轴线：首先标出所有关节的旋转轴线（对于旋转关节）或移动方向（对于移动关节）。对于六轴机械臂，J1至J6的轴线方向需要明确标注。
建立基坐标系 {0}：基坐标系固定在机器人底座上，其 Z₀ 轴通常沿 J1 的轴线方向，X₀ 和 Y₀ 轴根据右手定则确定，原点位于 J1 轴线与底座安装面的交点。
建立连杆坐标系 {i}（i=1,2,...,n-1）：对于每个中间连杆，Z_i 轴沿关节 i+1 的轴线方向；X_i 轴沿 Z_i-1 与 Z_i 的公垂线方向，由关节 i 指向关节 i+1；Y_i 轴由右手定则确定（Y_i = Z_i × X_i）。
建立末端坐标系 {n}：对于末端执行器，Z_n 轴通常沿工具的方向，X_n 和 Y_n 根据实际工具定义。
提取D-H参数：根据坐标系之间的相对位置关系，逐一确定 a_i-1、α_i-1、d_i、θ_i 的值。

在建立坐标系时，有几个重要的简化规则：如果相邻两个关节轴线相交，则 a_i-1 = 0；如果轴线平行，则 α_i-1 = 0；如果轴线垂直，则 α_i-1 = ±90°。这些规则能显著简化参数提取过程。此外，对于六轴机械臂，通常 J2、J3 的轴线平行，J4、J5、J6 的轴线相交于一点（腕部中心），这是大多数工业机器人的典型构型。

步骤	操作内容	关键要点	常见错误
1	确定关节轴线	明确每个关节的旋转轴方向	轴线方向标反
2	建立基坐标系	Z₀ 沿 J1 轴线	原点位置选择不当
3	建立中间坐标系	Z_i 沿关节 i+1 轴线	X_i 方向判断错误
4	建立末端坐标系	Z_n 沿工具方向	未考虑工具坐标系
5	提取D-H参数	按定义逐一测量	a 与 d 混淆

3.3 齐次变换矩阵的推导与连乘规则

根据改进型D-H参数的定义，相邻坐标系 {i-1} 到 {i} 的齐次变换矩阵可以表示为四个基本变换的乘积：

T(i-1→i) = Rot(X, α_i-1) · Trans(X, a_i-1) · Rot(Z, θ_i) · Trans(Z, d_i)

展开后得到标准的4×4矩阵形式：

|  cosθ_i          -sinθ_i          0          a_i-1  |
|  sinθ_i·cosα_i-1   cosθ_i·cosα_i-1  -sinα_i-1  -d_i·sinα_i-1 |
|  sinθ_i·sinα_i-1   cosθ_i·sinα_i-1   cosα_i-1   d_i·cosα_i-1 |
|  0              0              0          1        |

这个矩阵的左上角3×3子矩阵表示旋转关系，右上角3×1子矩阵表示平移关系，最后一行固定为 [0 0 0 1]。对于六轴机械臂，末端执行器在基坐标系下的位姿为：

T₀⁶ = T₀¹ · T₁² · T₂³ · T₃⁴ · T₄⁵ · T₅⁶

矩阵连乘的顺序不能交换，因为矩阵乘法不满足交换律。在实际编程中，通常使用循环结构逐次计算，或者直接调用矩阵乘法库函数。值得注意的是，由于每个 T 矩阵中同时包含 sin 和 cos 函数，当关节角为特殊值（如 0°、90°、180°）时，矩阵会退化为更简单的形式，这在实际计算中可以用来简化运算。

工程经验：在编写正运动学程序时，建议先将所有关节角转换为弧度制，再代入矩阵计算。同时，由于 sin/cos 函数计算开销较大，对于固定参数的连杆（a、α、d 为常数），可以预先计算好这些常数项，只将 θ 作为变量，这样可以显著提升计算效率。

3.4 典型六轴机械臂案例：从参数表到末端位姿

下面以一款典型的六轴垂直关节型机械臂为例（构型类似常见的工业机器人），其D-H参数表如下（采用改进型D-H参数）：

关节 i	a_i-1 (mm)	α_i-1 (°)	d_i (mm)	θ_i (°)	关节范围 (°)
1	0	0	400	θ₁	-170 ~ +170
2	150	-90	0	θ₂	-90 ~ +135
3	600	0	0	θ₃	-170 ~ +170
4	120	-90	620	θ₄	-180 ~ +180
5	0	90	0	θ₅	-120 ~ +120
6	0	-90	0	θ₆	-360 ~ +360

假设当前关节角为：θ₁=30°，θ₂=45°，θ₃=-60°，θ₄=90°，θ₅=30°，θ₆=0°。我们逐步计算每个变换矩阵：

步骤1：计算 T₀¹（a₀=0, α₀=0°, d₁=400, θ₁=30°）

T₀¹ = |  cos30°  -sin30°  0   0  |
          |  sin30°   cos30°  0   0  |
          |  0        0        1  400 |
          |  0        0        0   1  |

步骤2：计算 T₁²（a₁=150, α₁=-90°, d₂=0, θ₂=45°）

T₁² = |  cos45°  -sin45°   0   150 |
          |  0         0         1    0  |
          | -sin45°  -cos45°    0    0  |
          |  0         0         0    1  |

步骤3：计算 T₂³（a₂=600, α₂=0°, d₃=0, θ₃=-60°）

T₂³ = |  cos(-60°)  -sin(-60°)  0  600 |
          |  sin(-60°)   cos(-60°)  0   0  |
          |  0           0           1   0  |
          |  0           0           0   1  |

步骤4：计算 T₃⁴（a₃=120, α₃=-90°, d₄=620, θ₄=90°）

T₃⁴ = |  cos90°  -sin90°  0  120 |
          |  0        0        1  620 |
          | -sin90°  -cos90°  0   0  |
          |  0        0        0   1  |

步骤5：计算 T₄⁵（a₄=0, α₄=90°, d₅=0, θ₅=30°）

T₄⁵ = |  cos30°  -sin30°  0  0 |
          |  0        0       -1  0 |
          |  sin30°   cos30°   0  0 |
          |  0        0        0  1 |

步骤6：计算 T₅⁶（a₅=0, α₅=-90°, d₆=0, θ₆=0°）

T₅⁶ = |  1   0   0  0 |
          |  0   0   1  0 |
          |  0  -1   0  0 |
          |  0   0   0  1 |

步骤7：矩阵连乘，将上述六个矩阵依次相乘，得到最终的末端位姿矩阵 T₀⁶。由于手算过程繁琐，实际中通常使用MATLAB、Python（NumPy）或机器人工具箱（如Peter Corke的Robotics Toolbox）进行计算。计算结果为：

T₀⁶ ≈ |  0.866   -0.433   0.250   612.5 |
       |  0.500    0.750  -0.433   353.6 |
       |  0.000    0.500   0.866   850.0 |
       |  0        0       0        1    |

其中，左上角3×3矩阵表示末端姿态（旋转矩阵），右上角3×1向量 [612.5, 353.6, 850.0]^T 表示末端位置（单位：mm）。这意味着末端执行器位于基坐标系下 x=612.5mm、y=353.6mm、z=850.0mm 处，姿态由旋转矩阵描述。

验证与排障：在完成正运动学计算后，务必进行验证。常用的验证方法包括：① 将全部关节角设为0°，此时末端位姿应等于机械臂的“零位”位姿，可通过机械臂的CAD模型或实际测量确认；② 单独旋转某个关节，观察末端位置的变化是否符合预期方向；③ 使用机器人仿真软件（如RoboDK、V-REP）进行交叉验证。如果计算结果与仿真不一致，优先检查D-H参数表中的符号（尤其是α的正负号）和坐标系建立是否正确。

3.5 常见误区与总结复盘

在学习和应用D-H参数法时，以下几个误区需要特别警惕：

误区一：混淆标准型与改进型D-H参数。两种方法的参数定义和变换顺序不同，混用会导致完全错误的结果。在阅读文献或使用工具包时，务必确认采用的是哪种约定。
误区二：坐标系建立不唯一。虽然D-H参数法有标准规则，但在某些特殊构型下（如相邻关节轴线平行或相交），坐标系的选择存在一定的自由度。不同的选择会导致不同的D-H参数，但最终末端位姿应该一致。建议在团队内统一约定。
误区三：忽略关节角的正负号。关节角的正方向通常由右手定则确定，但不同厂家的机械臂定义可能不同。在建模时必须参考机械臂的技术文档，确保关节角方向与D-H参数表中的θ定义一致。
误区四：矩阵连乘顺序错误。齐次变换矩阵的连乘顺序必须严格按照从基座到末端的顺序，不能随意调换。

总结复盘：正运动学是机器人学的基石，D-H参数法提供了一套系统化的建模工具。通过本节的学习，我们掌握了：① D-H四参数的几何意义；② 连杆坐标系的建立规则；③ 齐次变换矩阵的推导与连乘方法；④ 六轴机械臂的完整求解流程。在实际工程中，正运动学计算通常由控制器实时完成（频率可达1kHz以上），因此计算效率同样重要。建议读者在理解原理的基础上，使用编程语言实现一次完整的正运动学求解，并利用仿真工具进行验证，这将为后续的逆运动学、动力学和轨迹规划打下坚实基础。