第3章 机械臂运动学建模：D-H参数法详解

各位同学，今天我们来聊聊机械臂运动学建模里最经典的方法——D-H参数法。说实话，我刚入行那会儿，看到一堆坐标系和矩阵就头大。后来做了几个项目才明白，这东西其实就是个套路，掌握了规律，6轴、7轴甚至协作臂都能一套方法搞定。

3.1 为什么要用D-H参数法？

我遇到过不少刚入行的工程师，上来就问：为什么不用欧拉角或者旋转矩阵直接算？嗯，理论上当然可以。但实际项目中，你面对的是一个6轴机械臂，每个关节都在动，如果用传统方法，每动一个关节就得重新算一遍全局坐标，那代码写出来你自己都不想看。

D-H参数法的好处，说白了就是标准化。它把每个关节-连杆对抽象成4个参数，然后通过4步变换矩阵，把局部坐标一步步传到末端。这样你改一个关节角度，只需要更新对应的变换矩阵，其他部分完全不用动。

方法	优点	缺点
D-H参数法	标准化、模块化、适合编程	坐标系建立有固定规则，需要适应
直接几何法	直观、适合简单结构	复杂臂型计算量爆炸
旋量法	数学优雅、无奇异点问题	入门门槛高，工程落地少

3.2 建立连杆坐标系——这一步最容易翻车

我记得第一次给一个6轴工业臂建D-H坐标系，折腾了一下午，结果算出来的末端位置差了10厘米。后来发现是Z轴方向搞反了。所以这一节，我带你手把手过一遍坐标系建立的规则。

3.2.1 基本规则

每个连杆i，我们定义两个坐标系：

• 坐标系{i-1}：固定在连杆i-1上，Z轴沿关节i-1的轴线方向
• 坐标系{i}：固定在连杆i上，Z轴沿关节i的轴线方向

然后，我们通过4步从{i-1}变换到{i}：

1. 绕Z_i-1轴旋转θ_i（关节角）
2. 沿Z_i-1轴平移d_i（连杆偏距）
3. 沿X_i轴平移a_i（连杆长度）
4. 绕X_i轴旋转α_i（连杆扭角）

3.2.2 6轴机械臂的坐标系分配

我们以一个典型的6轴工业臂为例（比如常见的KUKA、ABB那种构型）。它的关节配置是：

• 关节1：腰部旋转（绕Z₀）
• 关节2：肩部俯仰（绕Z₁）
• 关节3：肘部俯仰（绕Z₂）
• 关节4：前臂旋转（绕Z₃）
• 关节5：腕部俯仰（绕Z₄）
• 关节6：腕部旋转（绕Z₅）

下面这张图，是我用SVG画的坐标系分配示意，你感受一下：

3.3 正向运动学推导

坐标系建好了，接下来就是算。正向运动学说白了就是：给你6个关节角度，算出末端在基坐标系下的位置和姿态。

3.3.1 单个连杆的变换矩阵

每个连杆的变换矩阵长这样：

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

展开成4x4矩阵：
T_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         ]
]

3.3.2 从基座到末端的链式乘法

对于6轴机械臂，末端相对于基座的变换矩阵就是：

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

注意顺序！是从基座开始，依次乘到末端。我见过有人写成T_6 * T_5 * ...，那结果肯定不对。矩阵乘法不满足交换律，这个坑我当年也踩过。

最终T_0_6矩阵的左上角3x3是末端姿态（旋转矩阵），右上角3x1是末端位置（x, y, z坐标）。

3.4 正向运动学代码实现

好了，理论讲完了，咱们上代码。我用Python写了一个完整的正向运动学实现，你直接拿去用就行。

import numpy as np

def dh_transform(theta, d, a, alpha):
    """
    计算单个连杆的D-H变换矩阵
    参数:
        theta: 关节角 (弧度)
        d:     连杆偏距
        a:     连杆长度
        alpha: 连杆扭角 (弧度)
    返回:
        4x4 齐次变换矩阵
    """
    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(joint_angles, dh_params):
    """
    6轴机械臂正向运动学
    参数:
        joint_angles: 6个关节角 (弧度)
        dh_params:    D-H参数表, 每行 [d, a, alpha]
    返回:
        末端位姿矩阵 T_0_6
    """
    T = np.eye(4)
    for i in range(6):
        theta = joint_angles[i]
        d = dh_params[i][0]
        a = dh_params[i][1]
        alpha = dh_params[i][2]
        T_i = dh_transform(theta, d, a, alpha)
        T = T @ T_i  # 注意: 左乘
    return T

# 示例: 一个典型6轴工业臂的D-H参数 (单位: mm, rad)
# 这里用的是标准KUKA KR6构型的参数
dh_params = [
    [400,  150,  -np.pi/2],   # 关节1
    [0,    600,   0       ],   # 关节2
    [0,    200,   np.pi/2 ],   # 关节3
    [500,  0,    -np.pi/2 ],   # 关节4
    [0,    0,     np.pi/2 ],   # 关节5
    [100,  0,     0       ]    # 关节6
]

# 假设所有关节角度为0 (机械臂直立状态)
angles = np.zeros(6)
T_end = forward_kinematics(angles, dh_params)

print("末端位置 (x, y, z):")
print(T_end[:3, 3])
print("\n末端姿态 (旋转矩阵):")
print(T_end[:3, :3])

3.5 验证你的正向运动学

代码写完了，怎么知道对不对？我的做法是：

1. 零位测试：所有关节角为0时，末端应该在什么位置？对于大多数工业臂，此时机械臂应该是直立状态，末端在基座正上方某个高度。
2. 单关节运动：只动关节1，末端应该画一个圆（绕Z₀轴旋转）。只动关节2，末端应该在垂直平面内画弧。
3. 与实物对照：如果有真机，把计算出的关节角发给机器人，看末端是不是真的到了那个位置。