第二章 运动学基础：正运动学与逆运动学，DH参数建模
各位同学，欢迎来到第二章。这一章是机器人控制的「地基」。地基没打好，后面做轨迹规划、力矩控制，都会出问题。我当年刚入行时，就吃过这个亏——模型参数写错一个符号，机械臂直接往桌面上砸。嗯，今天咱们就把这个地基夯实了。
2.1 正运动学：从关节角度到末端位姿
正运动学，说白了就是：我知道每个关节转了多少度，求出手臂末端在哪儿、朝哪个方向。
举个例子。你坐在工位上，抬起右臂。肩关节转了30°，肘关节弯了45°，手腕稍微偏了10°。你的指尖在空间中的位置和姿态，就是正运动学要算的东西。
数学上，我们用齐次变换矩阵来描述这个关系：
T_06 = T_01 * T_12 * T_23 * T_34 * T_45 * T_56其中 T_ij 表示从关节 i 到关节 j 的变换。六个矩阵乘在一起，就得到了末端相对于基座的位姿。
核心公式：
末端位姿 = f(θ₁, θ₂, θ₃, θ₄, θ₅, θ₆)
这是一个从关节空间到笛卡尔空间的映射，是唯一的、确定的。
我在项目中遇到过一件事：有次调试六轴机器人，正运动学算出来末端位置总是偏了2毫米。查了半天，发现是第三个关节的连杆长度标定错了。你看，理论再漂亮，实际参数不准，结果就是错的。
2.2 逆运动学：从末端位姿到关节角度
逆运动学正好反过来：我知道末端要去哪儿，求每个关节该转多少度。
这比正运动学难得多。为什么？因为解不唯一。同一个末端位置，可能有8组甚至16组关节角度解。你想想看，手臂够到同一个点，肘关节可以朝上也可以朝下，对吧？
逆运动学的求解方法主要有两种：
- 解析法：通过代数或几何推导，得到封闭解。速度快，适合实时控制。但只适用于满足Pieper准则的机器人（后三个关节轴线交于一点）。
- 数值法：用迭代逼近的方式求解。通用性强，但计算慢，可能不收敛。
我的建议： 对于六轴工业机器人，优先用解析法。我做过一个项目，用数值法做逆解，结果在奇异点附近迭代了200多次还没收敛，差点把电机烧了。后来换成解析法，一个周期内就搞定了。
逆运动学还有一个坑：多解选择。算出来8组解，选哪一组？
- 选关节运动最小的那组——省时间、省能量
- 选远离关节限位的那组——避免撞到机械限位
- 选避障的那组——别让手臂打到周围设备
我曾经在产线上调试，逆解选了最近的一组，结果机器人走到一半，肘关节撞到了旁边的夹具。嗯，从那以后，我每次都会把避障条件加进逆解的选择逻辑里。
2.3 DH参数建模：给机器人建个「身份证」
DH参数是Denavit和Hartenberg两位老先生提出的。说白了，就是用四个参数来描述相邻两个关节之间的关系：
| 参数 | 符号 | 含义 |
|---|---|---|
| 连杆长度 | aᵢ | 沿Xᵢ轴，从Zᵢ₋₁到Zᵢ的距离 |
| 连杆扭角 | αᵢ | 绕Xᵢ轴，从Zᵢ₋₁到Zᵢ的转角 |
| 连杆偏距 | dᵢ | 沿Zᵢ₋₁轴，从Xᵢ₋₁到Xᵢ的距离 |
| 关节转角 | θᵢ | 绕Zᵢ₋₁轴，从Xᵢ₋₁到Xᵢ的转角 |
对于六轴机器人，我们需要建立6组DH参数。每组参数对应一个关节。我习惯用改进DH（Modified DH），因为它在处理树形结构时更直观。
注意： DH参数有两种主流约定——标准DH和改进DH。它们的坐标系定义顺序不同。你选了一种，就全程用同一种。混着用？我见过有人把两种混在一起，结果算出来的末端位置差了10厘米。别犯这种低级错误。
下面是一个典型的六轴机器人DH参数表（以常见的工业机器人为例）：
| 关节 i | aᵢ (mm) | αᵢ (°) | dᵢ (mm) | θᵢ (°) |
|---|---|---|---|---|
| 1 | 0 | 0 | 0 | θ₁ |
| 2 | a₂ | -90 | 0 | θ₂ |
| 3 | a₃ | 0 | 0 | θ₃ |
| 4 | 0 | -90 | d₄ | θ₄ |
| 5 | 0 | 90 | 0 | θ₅ |
| 6 | 0 | -90 | d₆ | θ₆ |
有了这张表，你就可以写出每个关节的变换矩阵。然后一乘，正运动学就出来了。
2.4 知识体系总览
下面这张图，是我自己画的知识结构。你看一眼，就能明白这一章讲了什么：
2.5 代码实战：用Python实现正运动学
光说不练假把式。咱们写一段代码，把DH参数变成实际的变换矩阵。我用的是numpy，简单直接：
import numpy as np
def dh_transform(a, alpha, d, theta):
"""根据DH参数计算相邻关节的变换矩阵"""
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
# 以六轴机器人为例，假设DH参数已知
# 这里只展示前三个关节作为示例
a = [0, 0.3, 0.25] # 单位：米
alpha = [0, -np.pi/2, 0]
d = [0, 0, 0]
theta = [0.5, 0.8, -0.3] # 单位：弧度
T_01 = dh_transform(a[0], alpha[0], d[0], theta[0])
T_12 = dh_transform(a[1], alpha[1], d[1], theta[1])
T_23 = dh_transform(a[2], alpha[2], d[2], theta[2])
T_03 = T_01 @ T_12 @ T_23
print("末端位姿矩阵：\n", T_03)小技巧： 写代码时，先把DH参数表列成数组。然后循环乘起来。我习惯把六个关节的变换矩阵存成一个列表，方便后面调试。你想想看，如果某个关节参数错了，直接看列表里的矩阵就能定位问题。
好了，这一章的内容就到这儿。正运动学、逆运动学、DH参数，这三样东西是机器人控制的「三件套」。你把这章吃透了，后面做轨迹规划、力矩控制，心里就有底了。
可可精品资料库