第3章正运动学

第3章：正运动学：D-H参数法建模、连杆坐标系建立规则、正运动学方程推导（以六轴为例）

正运动学，说白了就是：给定每个关节的角度，算出末端执行器在空间中的位置和姿态。

你想想看，我们控制机械臂，最终目的是让末端（比如夹爪、焊枪）到达目标点。但电机只能控制关节转多少度。这中间怎么换算？就是正运动学要解决的问题。

我个人习惯把正运动学比作「搭积木」。每个关节是一块积木，D-H参数就是积木的尺寸和连接方式。我们只需要按顺序把积木一块块拼起来，末端的位置自然就出来了。

核心思想：从基座开始，通过齐次变换矩阵，一步步传递到末端。

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

六轴机械臂有6个旋转关节。如果直接写几何关系，你会疯掉的——每个关节的旋转轴方向不同，连杆长度不同，还有偏距……

1955年，Denavit和Hartenberg两位老前辈提出了一套标准化方法。说白了就是：给每个连杆建立一个坐标系，然后用4个参数来描述相邻坐标系的关系。

这4个参数分别是：

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的转角

我的经验：刚开始学的时候，我总搞混a和d。后来我记住一个口诀——「a是横着的，d是竖着的」。a描述的是连杆本身的长度（水平方向），d描述的是相邻关节之间的偏移（垂直方向）。

3.2 连杆坐标系建立规则

这一步很关键。坐标系建错了，后面全白搭。

规则其实就三条：

Z轴：沿关节轴线方向。旋转关节就是旋转轴方向。
X轴：沿相邻Z轴的公垂线方向，从Z_i-1指向Z_i。
Y轴：由右手定则确定，Y = Z × X。

嗯，这里要注意：基座坐标系（0号）可以任意放置，但通常让Z₀沿关节1的轴线，且与基座底面重合。末端坐标系（6号）的Z₆沿关节6的轴线，X₆方向任意。

我曾经踩过的坑：有一次给一个协作机器人建D-H模型，我把关节2和关节3的Z轴方向搞反了。结果算出来的末端位置差了十几厘米。排查了整整一个下午……后来我养成了一个习惯：建完坐标系后，先手动验证几个特殊位姿（比如所有关节归零），看看末端位置是否合理。

3.3 六轴机械臂D-H参数表

以典型的六轴工业机器人为例（比如常见的6R构型），D-H参数表如下：

连杆 i	a_i-1	α_i-1	d_i	θ_i
1	0	0°	d₁	θ₁
2	a₁	-90°	0	θ₂
3	a₂	0°	0	θ₃
4	a₃	-90°	d₄	θ₄
5	0	90°	0	θ₅
6	0	-90°	d₆	θ₆

你看，每个关节只有一个变量θ_i（旋转关节），其他三个参数都是固定的。这就是D-H参数法的魅力——把复杂的空间几何问题，简化为4个参数的查表问题。

3.4 正运动学方程推导

有了D-H参数表，下一步就是推导变换矩阵。

相邻连杆的变换矩阵公式是：

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

展开成4×4矩阵就是：

i-1T_i = 
[ cosθ_i    -sinθ_i    0      a_i-1 ]
[ sinθ_icosα_i-1  cosθ_icosα_i-1  -sinα_i-1  -d_isinα_i-1 ]
[ sinθ_isinα_i-1  cosθ_isinα_i-1   cosα_i-1   d_icosα_i-1 ]
[ 0        0        0      1 ]

然后，从基座到末端的总变换矩阵就是：

0T_6 = 0T_1 · 1T_2 · 2T_3 · 3T_4 · 4T_5 · 5T_6

这个矩阵的左上角3×3是姿态（旋转矩阵），右上角3×1是位置。

实际项目中的做法：我一般不会手算这个矩阵。太容易出错了。我会用MATLAB或者Python的符号计算库（比如SymPy）来推导。把D-H参数定义成符号变量，然后让计算机帮我乘。最后得到的表达式，直接复制到C代码里。

3.5 知识体系结构图

下面这张图，帮你理清本章的知识脉络：

3.6 代码实现示例

下面是用Python实现的正运动学计算函数。我习惯用numpy来处理矩阵运算：

import numpy as np

def dh_transform(a, alpha, d, theta):
    """计算相邻连杆的变换矩阵"""
    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(theta, dh_params):
    """
    正运动学计算
    theta: 6个关节角 (弧度)
    dh_params: D-H参数表 [a, alpha, d, theta_offset]
    """
    T = np.eye(4)
    for i in range(6):
        a = dh_params[i][0]
        alpha = dh_params[i][1]
        d = dh_params[i][2]
        theta_i = theta[i] + dh_params[i][3]  # 加上偏置
        T = T @ dh_transform(a, alpha, d, theta_i)
    return T

一个小技巧：实际项目中，D-H参数表里通常还有一个θ偏置（theta_offset）。因为机械臂的零位不一定在D-H定义的零位。比如，有些机器人关节2的零位是手臂水平，但D-H模型里零位是手臂竖直。这个偏置值，可以从机器人的技术手册里找到，或者通过标定得到。

3.7 验证方法

代码写完了，怎么知道对不对？

我的做法是：找几个特殊位姿，手动算一下。

所有关节归零：此时末端应该在什么位置？根据机械臂的几何尺寸，应该能估算出来。
只转动关节1：末端应该画一个圆，圆心在基座中心。
关节2和3联动：末端应该在垂直平面内运动。

如果这些特殊位姿都对，那基本可以放心了。

我曾经犯过的错：有一次我忘了把角度从度转成弧度，结果算出来的位置完全离谱。检查了半天才发现……所以我现在写代码时，都会在函数入口处加一个断言：assert np.max(np.abs(theta)) < 2*np.pi，防止传入度数。

正运动学是整个运动控制的基石。虽然公式看起来有点复杂，但只要你理解了D-H参数法的本质——用4个参数描述相邻连杆的关系——剩下的就是矩阵乘法了。多练几次，你也会觉得「不过如此」。

公众号：蓝海资料掘金营，微信deep3321

📚 课程目录