3. 正运动学建模：D-H参数法、标准D-H与改进D-H的区别、正运动学求解流程
正运动学（Forward Kinematics）解决的核心问题是：已知机器人各关节的关节变量（旋转角或平移量），求解末端执行器在基坐标系下的位姿（位置与姿态）。它是机器人控制、轨迹规划、动力学分析以及逆解算法验证的基础。在工程实践中，最通用的建模方法是Denavit-Hartenberg参数法（简称D-H法），它通过在每个连杆上固连坐标系，用4个参数（a, α, d, θ）描述相邻连杆之间的相对位姿关系，从而将复杂的空间几何问题转化为齐次变换矩阵的连乘问题。本章将深入剖析D-H参数法的数学本质、标准D-H（SDH）与改进D-H（MDH）的差异，并给出完整的正运动学求解流程与工程注意事项。
3.1 D-H参数法的核心原理与数学基础
D-H参数法的本质是用4个独立参数描述两个相邻关节轴之间的空间关系。设机器人由n个连杆和n个关节组成，基座为连杆0，末端执行器为连杆n。对于任意相邻连杆i-1和连杆i，我们定义如下四个参数：
- 连杆长度 aᵢ₋₁：沿Xᵢ₋₁轴方向，从Zᵢ₋₁轴移动到Zᵢ轴的距离。它描述了两个关节轴之间的公垂线长度，恒为非负值。
- 连杆扭转角 αᵢ₋₁：绕Xᵢ₋₁轴，从Zᵢ₋₁轴旋转到Zᵢ轴的角度。它描述了两个关节轴之间的夹角，取值范围为[-π, π]。
- 连杆偏距 dᵢ：沿Zᵢ轴方向，从Xᵢ₋₁轴移动到Xᵢ轴的距离。对于旋转关节，dᵢ为常数；对于移动关节，dᵢ为变量。
- 关节角 θᵢ：绕Zᵢ轴，从Xᵢ₋₁轴旋转到Xᵢ轴的角度。对于旋转关节，θᵢ为变量；对于移动关节，θᵢ为常数。
基于上述定义，相邻坐标系{i-1}到{i}的齐次变换矩阵为：
ᵢ₋₁Tᵢ = Rot(z, θᵢ) · Trans(z, dᵢ) · Trans(x, aᵢ₋₁) · Rot(x, αᵢ₋₁)
[ cosθᵢ -sinθᵢ·cosαᵢ₋₁ sinθᵢ·sinαᵢ₋₁ aᵢ₋₁·cosθᵢ ]
[ sinθᵢ cosθᵢ·cosαᵢ₋₁ -cosθᵢ·sinαᵢ₋₁ aᵢ₋₁·sinθᵢ ]
= [ 0 sinαᵢ₋₁ cosαᵢ₋₁ dᵢ ]
[ 0 0 0 1 ]该矩阵的左上3×3子矩阵表示姿态旋转，右上3×1子矩阵表示位置平移。通过将n个相邻变换矩阵依次相乘，即可得到末端执行器相对于基座的总变换矩阵：⁰Tₙ = ⁰T₁ · ¹T₂ · ... · ⁿ⁻¹Tₙ。
工程经验：在建立D-H坐标系时，务必遵循"Z轴沿关节轴线，X轴沿公垂线"的原则。若相邻两关节轴线平行，则公垂线不唯一，此时可任意选择一条公垂线作为X轴，但需保证aᵢ₋₁和αᵢ₋₁的一致性。若相邻两关节轴线相交，则aᵢ₋₁=0，X轴方向由Zᵢ₋₁×Zᵢ确定。
3.2 标准D-H与改进D-H的深度对比
标准D-H（SDH）由Denavit和Hartenberg于1955年提出，而改进D-H（MDH）由Craig在1986年系统化。两者的根本区别在于坐标系固连位置和变换顺序不同。
| 对比维度 | 标准D-H (SDH) | 改进D-H (MDH) |
|---|---|---|
| 坐标系固连位置 | 坐标系{i}固连于连杆i的远端（关节i+1处） | 坐标系{i}固连于连杆i的近端（关节i处） |
| 变换顺序 | 绕Zᵢ₋₁转θᵢ → 沿Zᵢ₋₁移dᵢ → 沿Xᵢ移aᵢ → 绕Xᵢ转αᵢ | 绕Xᵢ₋₁转αᵢ₋₁ → 沿Xᵢ₋₁移aᵢ₋₁ → 绕Zᵢ转θᵢ → 沿Zᵢ移dᵢ |
| 参数下标 | aᵢ, αᵢ 属于连杆i，dᵢ, θᵢ 属于关节i | aᵢ₋₁, αᵢ₋₁ 属于连杆i-1，dᵢ, θᵢ 属于关节i |
| 适用场景 | 串联开链机器人（如6轴工业机械臂） | 树形结构、闭链结构（如并联机器人、人形机器人腿部） |
| 优点 | 参数直观，与关节变量对应清晰 | 对分支结构建模更自然，便于模块化 |
| 缺点 | 对树形结构需额外处理分支 | 参数下标容易混淆，需仔细核对 |
在实际工程中，串联6轴机器人通常采用SDH，因为其参数表与机械结构对应关系直观，且已有大量成熟库（如MATLAB Robotics Toolbox）支持。而移动机器人、双足机器人或包含闭链的机构，推荐使用MDH，因为MDH的坐标系定义更符合"每个连杆一个坐标系"的直觉，且便于处理分支和闭环约束。
风险提示：在混合使用SDH和MDH时，极易出现参数混淆。例如，同一台机器人用SDH和MDH建模，得到的a和α参数下标不同（SDH用aᵢ, αᵢ，MDH用aᵢ₋₁, αᵢ₋₁）。若直接套用标准公式，会导致变换矩阵错误。建议在项目文档中明确标注采用哪种约定，并在代码中统一封装。
3.3 正运动学求解完整流程与案例拆解
正运动学求解的工程流程可分为五个步骤：建立坐标系 → 填写参数表 → 计算相邻变换 → 矩阵连乘 → 提取位姿。下面以典型的6轴串联工业机器人（如类似KUKA KR系列）为例进行拆解。
- 建立连杆坐标系：基座坐标系{0}固定于机器人基座，Z₀轴沿关节1轴线方向。坐标系{1}固连于连杆1远端（SDH约定），Z₁轴沿关节2轴线方向，X₁轴沿Z₀与Z₁的公垂线方向。依此类推，直到末端坐标系{6}。
- 填写D-H参数表：根据机械设计图纸，测量或计算各连杆的a、α、d值，θ为关节变量（初始零位时通常为0）。
- 计算相邻变换矩阵：将每个关节的4个参数代入ᵢ₋₁Tᵢ公式，得到6个4×4矩阵。
- 矩阵连乘：计算⁰T₆ = ⁰T₁ · ¹T₂ · ²T₃ · ³T₄ · ⁴T₅ · ⁵T₆。
- 提取位姿：从⁰T₆中提取位置向量P = [px, py, pz]ᵀ，以及旋转矩阵R，并可进一步转换为欧拉角或四元数。
以下是一个简化的2连杆平面机械臂的D-H参数表与求解示例（单位：mm, rad）：
| 关节i | aᵢ₋₁ | αᵢ₋₁ | dᵢ | θᵢ |
|---|---|---|---|---|
| 1 | 0 | 0 | 0 | θ₁ |
| 2 | L₁=300 | 0 | 0 | θ₂ |
代入公式后，末端位置为：
px = L₁·cosθ₁ + L₂·cos(θ₁+θ₂)
py = L₁·sinθ₁ + L₂·sin(θ₁+θ₂)
pz = 0该结果与几何法推导完全一致，验证了D-H法的正确性。
实战场景：在某焊接机器人项目中，现场调试时发现机器人末端实际位置与控制器显示位置偏差约5mm。经排查，原因是D-H参数表中的a₂值误用了设计值（未考虑焊枪延长杆的偏移量）。修正参数后，偏差降至0.2mm以内。这提醒我们：D-H参数必须基于实际装配后的测量值，而非纯设计值，尤其是末端执行器（焊枪、夹爪等）的附加偏移。
3.4 常见误区与总结复盘
在正运动学建模与求解过程中，工程师常犯以下错误：
- 坐标系方向错误：Z轴未沿关节轴线，或X轴方向选择不一致，导致α符号错误。建议在建立坐标系时，用右手定则逐轴验证。
- 参数单位混淆：长度单位混用mm和m，角度单位混用rad和deg。建议在代码中统一使用SI单位（m, rad），并在输入输出接口处做转换。
- SDH与MDH混用：在同一个机器人模型中，部分连杆用SDH，部分用MDH，导致变换矩阵不连续。必须全程采用同一种约定。
- 忽略关节零位偏移：实际机器人的关节编码器零位与D-H参数表中的θ=0位置可能不一致，需在控制软件中设置零位偏移量。
- 未验证奇异位形：当两个相邻关节轴线平行时，D-H参数中的α=0，此时若a=0，则可能出现奇异，需在算法中特殊处理。
复盘建议：完成正运动学建模后，务必进行两项验证：① 零位验证：将所有关节变量设为0，计算末端位姿，与机械设计图纸上的零位姿态对比；② 单轴运动验证：仅驱动一个关节转动，观察末端轨迹是否为一个圆弧（旋转关节）或直线（移动关节），且半径/方向与理论一致。这两项验证能快速发现参数错误。
正运动学是机器人学的基石，D-H参数法提供了一套标准化的建模语言。掌握SDH与MDH的区别，熟练运用求解流程，并具备参数校验的工程意识，是后续学习逆运动学、动力学与控制的基础。在实际项目中，建议将D-H参数表作为配置数据独立管理，便于调试和维护。