第3章正运动学

第三节：正运动学——D-H参数法、连杆坐标系建立与方程求解

各位同学，今天我们来聊聊正运动学。说白了，正运动学就是：给定机器人每个关节的角度（或位移），求末端执行器在空间中的位置和姿态。这个问题，我当年刚入行时觉得很简单，不就是几何计算嘛？结果第一次搭模型时，坐标系一乱，算出来的位置差了十万八千里。嗯，后来我才明白，关键就在于如何规范地描述相邻连杆之间的位姿关系。

这一节，我们重点讲三个东西：D-H参数法、连杆坐标系的建立、以及正运动学方程的求解。我会结合我实际项目中的经验，把那些容易踩的坑都指出来。

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

你想想看，一个六轴机器人，有六个连杆、六个关节。如果每个连杆都随便建坐标系，那相邻连杆之间的变换矩阵会非常混乱。D-H参数法，就是一套标准化规则，用四个参数就能唯一确定相邻连杆的位姿关系。

这四个参数分别是：

θ_i（关节角）：绕 Z_i-1 轴，从 X_i-1 转到 X_i 的角度。
d_i（连杆偏距）：沿 Z_i-1 轴，从 X_i-1 到 X_i 的距离。
a_i（连杆长度）：沿 X_i 轴，从 Z_i-1 到 Z_i 的距离。
α_i（连杆扭角）：绕 X_i 轴，从 Z_i-1 转到 Z_i 的角度。

核心思想：D-H参数法把相邻连杆的变换拆解为四个基本运动（旋转→平移→平移→旋转），每个运动都沿着当前坐标系的轴进行。这样，变换矩阵就是四个齐次变换矩阵的乘积。

我个人习惯，在建立D-H参数表之前，先画一个连杆-关节示意图，把每个关节的轴线画出来，标上序号。这样后面填参数时不容易搞混。

3.2 连杆坐标系的建立——手把手教你做

建立坐标系是正运动学的基础，也是最容易出错的地方。我给大家总结一个五步法，照着做基本不会错：

确定关节轴线：每个关节的旋转轴（或移动轴）就是 Z 轴。Z_i-1 对应关节 i。
确定原点 O_i：Z_i-1 和 Z_i 的公垂线与 Z_i 的交点。
确定 X_i 轴：沿公垂线方向，从 Z_i-1 指向 Z_i。
确定 Y_i 轴：由右手定则，Y_i = Z_i × X_i。
处理特殊情况：如果 Z_i-1 和 Z_i 平行，公垂线不唯一，通常取使 d_i = 0 的位置。

避坑指南：我曾经在做一个SCARA机器人时，因为两个相邻关节的Z轴平行，我随便选了一个公垂线位置，结果导致后面的运动学计算出现了奇异性。后来我养成了一个习惯：遇到平行轴，优先让公垂线通过前一个关节的原点，这样d_i = 0，参数表更简洁。

下面是一个典型的六轴工业机器人D-H参数表示例：

连杆 i	θ_i	d_i (mm)	a_i (mm)	α_i (°)
1	θ₁	400	150	-90
2	θ₂	0	600	0
3	θ₃	0	120	-90
4	θ₄	500	0	90
5	θ₅	0	0	-90
6	θ₆	100	0	0

这张表怎么来的？就是按照上面的五步法，一个关节一个关节地建坐标系，然后量出四个参数。注意，θ_i 是变量，其他参数在机器人设计完成后就是固定的。

3.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_0n = T_1 * T_2 * T_3 * ... * T_n

这里 n 是关节数量。对于六轴机器人，就是 T_06 = T_01 * T_12 * T_23 * T_34 * T_45 * T_56。

注意：矩阵乘法不满足交换律，顺序绝对不能错！我见过有同事把 T_12 和 T_23 乘反了，结果末端位置差了半米，找了半天bug才发现是顺序问题。

在实际编程中，我建议用符号计算先推导出解析表达式，然后再代入数值。比如用Python的SymPy库：

import sympy as sp

# 定义符号变量
θ1, θ2, θ3, θ4, θ5, θ6 = sp.symbols('θ1 θ2 θ3 θ4 θ5 θ6')

# 定义D-H参数（示例值）
d1, a1, α1 = 400, 150, -sp.pi/2
d2, a2, α2 = 0, 600, 0
# ... 依次定义所有参数

# 构建变换矩阵函数
def dh_matrix(θ, d, a, α):
    return sp.Matrix([
        [sp.cos(θ), -sp.sin(θ)*sp.cos(α),  sp.sin(θ)*sp.sin(α), a*sp.cos(θ)],
        [sp.sin(θ),  sp.cos(θ)*sp.cos(α), -sp.cos(θ)*sp.sin(α), a*sp.sin(θ)],
        [0,          sp.sin(α),             sp.cos(α),            d],
        [0,          0,                     0,                    1]
    ])

# 计算总变换矩阵
T01 = dh_matrix(θ1, d1, a1, α1)
T12 = dh_matrix(θ2, d2, a2, α2)
# ... 继续乘下去
T06 = T01 * T12 * T23 * T34 * T45 * T56

# 简化表达式
T06_simplified = sp.simplify(T06)
print(T06_simplified)

这样得到的 T_06 矩阵，左上角 3×3 是末端姿态（旋转矩阵），右上角 3×1 是末端位置（平移向量）。

3.4 知识体系总览

为了让大家更直观地理解这一节的知识结构，我画了一张流程图：

这张图把三个核心内容串起来了：D-H参数是输入，坐标系建立是过程，正运动学方程是输出。三者缺一不可。

3.5 实际项目中的一点体会

我记得有一次调试一个焊接机器人，末端位置总是偏了2毫米。查了半天，发现是D-H参数表中的 a₃ 写错了——图纸上标的是120mm，我写成了120cm。嗯，这种低级错误，只要用实际测量值验证一下就能发现。

所以我的建议是：拿到D-H参数表后，先选几个特殊关节角度（比如所有关节归零），手动计算末端位置，然后去现场用尺子量一下实际位置。如果对得上，说明参数表没问题；对不上，赶紧回去检查。

一个小技巧：在建立坐标系时，尽量让相邻关节的 Z 轴平行或垂直，这样很多参数会变成0或±90°，计算会简化很多。工业机器人厂商就是这么干的。

好了，正运动学就讲到这里。记住，D-H参数法是工具，坐标系建立是基础，矩阵乘法是手段。多练几次，你就能像我一样，看到机器人就能在脑子里自动生成D-H参数表了。

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

📚 课程目录

第三节：正运动学——D-H参数法、连杆坐标系建立与方程求解

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

3.2 连杆坐标系的建立——手把手教你做

3.3 正运动学方程求解——从参数到矩阵

3.4 知识体系总览

3.5 实际项目中的一点体会