4. 正运动学建模：D-H参数法详解、建立连杆坐标系、正运动学方程推导

正运动学，说白了就是：给定关节角度，算出末端在哪儿。

我刚开始做人形机器人那会儿，总觉得这步是纯数学推导，跟实际工程关系不大。直到有一次，我调试一个双足平衡算法，发现脚底传感器数据跟模型预测的位置差了 5 厘米。查了两天，最后发现是 D-H 参数表里一个 a 值写反了。嗯，从那以后，我再也不敢小看这个建模步骤了。

这一章，我们就来把 D-H 参数法彻底讲透。你跟着我走一遍，保证能自己搭出任意构型机器人的正运动学模型。

4.1 D-H 参数法的核心思想

D-H 参数法，本质上是给每个关节建立一个坐标系，然后用 4 个参数来描述相邻坐标系之间的变换关系。这 4 个参数分别是：

• θ (关节角)：绕 Z 轴的旋转角度
• d (连杆偏距)：沿 Z 轴的平移距离
• a (连杆长度)：沿 X 轴的平移距离
• α (连杆扭角)：绕 X 轴的旋转角度

你想想看，一个刚体在空间中有 6 个自由度（3 个平移 + 3 个旋转）。但 D-H 法只用 4 个参数就描述了相邻连杆的位姿关系。为什么能省掉 2 个？因为 D-H 法对坐标系的建立方式做了约束——X 轴必须垂直于前一个 Z 轴。这个约束让两个自由度被隐含掉了。

4.2 建立连杆坐标系的步骤

建立坐标系是 D-H 法里最容易出错的一步。我总结了一个「三步法」，你照着做，基本不会跑偏。

1. 定 Z 轴：每个关节的 Z 轴沿着关节的旋转轴线方向。对于旋转关节，Z 轴就是旋转轴；对于移动关节，Z 轴就是平移方向。
2. 定 X 轴：X 轴沿着相邻两个 Z 轴的公垂线方向，从当前 Z 轴指向下一个 Z 轴。如果两个 Z 轴相交，X 轴垂直于两个 Z 轴所在的平面。
3. 定 Y 轴：Y 轴由右手定则确定，Y = Z × X。

这里有个坑，我踩过：基坐标系（第 0 号坐标系）的建立。基坐标系的 Z 轴通常与第一个关节的 Z 轴重合，但 X 轴可以任意选择，只要方便后续计算就行。我个人习惯把基坐标系的 X 轴指向机器人正前方。

4.3 D-H 参数表填写

坐标系建好了，接下来就是填参数表。我们以人形机器人的一条腿为例，假设它有 6 个自由度（髋关节 3 个 + 膝关节 1 个 + 踝关节 2 个）。

注意看，这里的 d_i 全部为 0。为什么？因为人形机器人的腿关节都是旋转关节，没有平移关节。如果你做的是工业机械臂，d_i 通常不为 0。

4.4 相邻连杆的变换矩阵

有了 D-H 参数，我们就可以写出相邻坐标系之间的齐次变换矩阵了。公式长这样：

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

展开成 4×4 矩阵形式：

| 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         |

这个矩阵，我建议你手推一遍。别偷懒。我当年在研究生阶段，把这个矩阵推了不下 20 遍，后来闭着眼睛都能写出来。为什么？因为后面所有的运动学计算，都基于这个矩阵。

4.5 正运动学方程推导

正运动学方程，就是把所有相邻变换矩阵乘起来：

T_0_n = T_1 * T_2 * T_3 * ... * T_n

对于人形机器人的一条腿，n=6，所以：

T_0_6 = T_1 * T_2 * T_3 * T_4 * T_5 * T_6

这个 T_0_6 矩阵的左上角 3×3 部分，就是末端（脚底）的姿态；右上角 3×1 部分，就是末端的位置。

实际工程中，我们很少手动去乘 6 个 4×4 矩阵。我一般用 MATLAB 的符号工具箱或者 Python 的 SymPy 来做符号推导。但有一点要注意：矩阵乘法不满足交换律，顺序一定不能错。

4.6 知识体系总览

下面这张图，把正运动学建模的整个流程串起来了。你对照着看，思路会更清晰。

4.7 实战：用 Python 实现正运动学

理论讲完了，我们上手写代码。下面是一个简单的 Python 实现，输入 D-H 参数和关节角度，输出末端位姿矩阵。

import numpy as np

def dh_transform(theta, d, a, alpha):
    """
    计算单个 D-H 变换矩阵
    """
    ct = np.cos(theta)
    st = np.sin(theta)
    ca = np.cos(alpha)
    sa = np.sin(alpha)
    
    T = np.array([
        [ct, -st*ca,  st*sa, a*ct],
        [st,  ct*ca, -ct*sa, a*st],
        [0,   sa,     ca,    d],
        [0,   0,      0,     1]
    ])
    return T

def forward_kinematics(dh_params, joint_angles):
    """
    正运动学计算
    dh_params: [[d, a, alpha], ...] 每个连杆的常量参数
    joint_angles: [theta1, theta2, ...] 关节角度
    """
    T = np.eye(4)
    for i in range(len(joint_angles)):
        Ti = dh_transform(joint_angles[i], dh_params[i][0], 
                          dh_params[i][1], dh_params[i][2])
        T = T @ Ti
    return T

# 示例：人形机器人右腿 D-H 参数（简化版）
# 参数格式：[d, a, alpha]
dh_params = [
    [0, 0, -np.pi/2],   # 髋关节偏摆
    [0, 0.4, 0],        # 髋关节俯仰 (大腿长度 0.4m)
    [0, 0, -np.pi/2],   # 髋关节滚动
    [0, 0.4, 0],        # 膝关节俯仰 (小腿长度 0.4m)
    [0, 0, np.pi/2],    # 踝关节俯仰
    [0, 0, 0]           # 踝关节滚动
]

# 假设所有关节角度为 0
joint_angles = [0, 0, 0, 0, 0, 0]
T_end = forward_kinematics(dh_params, joint_angles)
print("末端位姿矩阵：")
print(T_end)
print("\n末端位置：", T_end[:3, 3])

运行这段代码，你会得到末端位置是 [0.8, 0, 0]。为什么是 0.8？因为大腿和小腿长度都是 0.4 米，伸直状态下总长就是 0.8 米。这个结果跟机械设计图纸对得上，说明我们的 D-H 参数和坐标系建立是正确的。

4.8 常见错误与避坑指南

• Z 轴方向不一致：同一个机器人，所有关节的 Z 轴方向必须统一。我见过有人把左腿的 Z 轴朝上，右腿的 Z 轴朝下，结果正运动学算出来的两条腿长度不一样。
• α 的符号搞反：α 的正负取决于 X 轴的旋转方向。用右手定则判断，别想当然。
• 矩阵乘法顺序：T_0_n = T_1 * T_2 * ... * T_n，不是反过来。我刚开始写代码时，把乘法顺序写反了，结果末端位置飞到了天上去。
• 忽略基坐标系：基坐标系的位置和方向会影响所有后续计算。建议把基坐标系固定在机器人躯干的几何中心。