第四讲：正运动学——D-H参数法、连杆坐标系建立与六轴机械臂正解计算

正运动学解决的核心问题是：已知各关节的转角（或位移），求解末端执行器在笛卡尔空间中的位姿（位置与姿态）。它是机器人学中最基础、最核心的数学工具，也是后续进行轨迹规划、动力学分析、标定与补偿的前提。本讲将系统讲解D-H参数法的两种建模流派、连杆坐标系的建立规则、正运动学方程的推导流程，并以典型六轴垂直串联机械臂为例，完成从参数表到末端位姿矩阵的完整手算与验证。

4.1 D-H参数法的核心原理与两种建模流派

D-H（Denavit-Hartenberg）参数法由Jacques Denavit和Richard Hartenberg于1955年提出，其核心思想是：用四个最小独立参数来描述相邻两个连杆坐标系之间的相对位姿关系。这四个参数分别是：关节角θ、连杆偏距d、连杆长度a、连杆扭角α。通过将每个关节的齐次变换矩阵依次连乘，即可得到末端坐标系相对于基坐标系的位姿矩阵。

在实际工程应用中，存在两种主流的D-H建模方法，它们的坐标系建立位置和参数定义存在差异，初学者极易混淆：

本课程以标准型D-H（SDH）为主线展开，因为它在工业六轴机械臂的教学与工程调试中应用最广，且与大多数商用机器人控制器（如KUKA、ABB、FANUC）的说明书参数表兼容性更好。

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

建立连杆坐标系是D-H参数法的第一步，也是最容易出错的一步。坐标系建立是否正确，直接决定了后续参数表是否正确。以下是标准型D-H坐标系的建立步骤：

1. 确定关节轴：画出所有关节的旋转轴或移动轴方向，用z轴表示。对于旋转关节，z轴沿旋转轴方向；对于移动关节，z轴沿移动方向。
2. 确定公垂线：对于相邻的z轴z_{i-1}和z_i，找到它们之间的公垂线（或交点）。公垂线的方向即为x_i轴的方向。
3. 确定坐标系原点：坐标系{i}的原点位于公垂线与z_i轴的交点处。若z_{i-1}与z_i相交，则原点取交点；若z_{i-1}与z_i平行，则原点可沿z_i轴任意选择（通常取使d_i为零或便于计算的位置）。
4. 确定y轴：根据右手定则，y_i = z_i × x_i。
5. 基坐标系{0}：通常固定不动，z_0沿关节1的轴线方向，x_0可任意选择（一般与x_1在初始位形时对齐）。
6. 末端坐标系{n}：对于六轴机械臂，末端坐标系{6}的原点通常取在法兰盘中心，z_6沿末端执行器的接近方向。

建立坐标系后，即可按以下规则读取四个D-H参数：

• θ_i：绕z_{i-1}轴，从x_{i-1}旋转到x_i的角度（右手定则）。
• d_i：沿z_{i-1}轴，从坐标系{i-1}原点移动到x_i轴与z_{i-1}轴交点的距离。
• a_i：沿x_i轴，从z_{i-1}轴与x_i轴的交点移动到坐标系{i}原点的距离（即公垂线长度）。
• α_i：绕x_i轴，从z_{i-1}轴旋转到z_i轴的角度（右手定则）。

4.3 正运动学方程的推导与齐次变换矩阵

在完成坐标系建立和参数表填写后，正运动学方程的推导就变成了一个机械化的矩阵连乘过程。标准型D-H的相邻坐标系变换矩阵为：

T_i = Rot(z, θ_i) · Trans(z, d_i) · Trans(x, a_i) · Rot(x, α_i)

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

对于六轴机械臂，末端位姿矩阵为：

T_06 = T_01 · T_12 · T_23 · T_34 · T_45 · T_56

其中T_06的左上3×3子矩阵表示末端姿态（旋转矩阵R），右上3×1子矩阵表示末端位置（平移向量P）。在实际工程计算中，通常不直接展开全部矩阵元素，而是采用符号化简+数值代入的两步策略：先用符号推导出各元素的表达式，再代入具体关节角计算数值。

4.4 典型六轴垂直串联机械臂正解计算案例

以常见的六轴垂直串联工业机械臂（类似ABB IRB 120或国产教学平台）为例，其结构特点为：关节1垂直基座，关节2和3水平转动，关节4、5、6构成腕部（三个旋转轴交于一点）。该机械臂的D-H参数表如下：

现在，我们计算当关节角为θ = (0°, 90°, 0°, 0°, 0°, 0°)时的末端位姿。这是一个典型的“机械臂竖直向上”的位形，便于验证。

第一步：代入θ_1 = 0°，计算T_01：

T_01 = | 1   0    0    0 |
       | 0   0    1    0 |
       | 0  -1    0  290 |
       | 0   0    0    1 |

第二步：代入θ_2 = 90°，计算T_12：

T_12 = | 0  -1    0    0 |
       | 1   0    0  270 |
       | 0   0    1    0 |
       | 0   0    0    1 |

第三步：θ_3 = 0°，计算T_23：

T_23 = | 1   0    0   70 |
       | 0   0    1    0 |
       | 0  -1    0    0 |
       | 0   0    0    1 |

第四步：θ_4 = 0°，计算T_34：

T_34 = | 1   0    0    0 |
       | 0   0   -1    0 |
       | 0   1    0  302 |
       | 0   0    0    1 |

第五步：θ_5 = 0°，计算T_45：

T_45 = | 1   0    0    0 |
       | 0   0    1    0 |
       | 0  -1    0    0 |
       | 0   0    0    1 |

第六步：θ_6 = 0°，计算T_56：

T_56 = | 1   0    0    0 |
       | 0   1    0    0 |
       | 0   0    1   72 |
       | 0   0    0    1 |

第七步：将六个矩阵依次连乘，得到T_06。为减少计算量，可先计算T_04 = T_01·T_12·T_23·T_34，再计算T_46 = T_45·T_56，最后相乘。经过矩阵乘法运算，得到：

T_06 = | 0   0    1    0   |
       | 0   1    0    0   |
       | -1  0    0  632  |
       | 0   0    0    1   |

结果解读：末端位置为(0, 0, 632)mm，即机械臂末端法兰中心位于基座正上方632mm处。末端姿态矩阵的第三列[1, 0, 0]^T表示z_6轴指向x_0方向，说明末端法兰面朝上（z轴水平指向x方向），这与“机械臂竖直向上”的物理直觉一致。

4.5 常见误区与工程复盘

在正运动学的学习与工程应用中，以下误区值得特别注意：

• 误区一：混淆SDH与MDH的参数定义。同一台机器人，用两种方法建立的参数表完全不同。解决方法是：在项目开始时明确约定采用哪种标准，并在所有文档中统一标注。
• 误区二：忽略关节零位偏移。实际机械臂的关节编码器零位与D-H参数表中的θ=0位置往往存在偏差。在将理论正解用于实际控制时，必须进行零位标定，否则末端位置误差会随关节角增大而累积。
• 误区三：认为正解只能用于仿真。实际上，正解是实时控制系统的基础。现代机器人控制器以1kHz以上的频率实时计算正解，用于伺服闭环、力控和视觉伺服。正解的计算效率直接影响控制周期。
• 误区四：忽视数值稳定性。当机械臂处于奇异位形（如腕部中心与肩部中心重合）时，正解矩阵本身仍然正确，但基于正解的雅可比矩阵会退化，导致逆解不稳定。此时需要单独处理奇异位形。

案例复盘：在某次六轴机械臂的精度调试中，工程师发现末端定位误差达到15mm，远超设计指标±0.5mm。排查过程如下：首先检查D-H参数表，发现a_2被误写为270mm（实际应为270.5mm），修正后误差降至3mm；进一步检查发现关节2的零位偏移为0.8°，补偿后误差降至0.3mm。这个案例说明，正解模型的精度不仅取决于算法，更取决于参数标定的准确性。建议在出厂前使用激光跟踪仪对D-H参数进行辨识，并在控制器中保存补偿后的参数表。

正运动学是机器人学的基石，掌握D-H参数法的本质——用四个参数描述相邻坐标系的变换关系——比死记硬背公式更重要。下一阶段的学习中，我们将基于正解结果，进一步推导雅可比矩阵和逆运动学解算，逐步构建完整的运动学知识体系。