3、正运动学求解：从关节空间到笛卡尔空间的映射
正运动学，说白了就是「已知关节角度，求末端位姿」。
我刚开始接触机器人时，总觉得这玩意儿很玄乎。后来自己手算了一遍六轴机器人的正解，才发现——其实就是一连串坐标变换的叠加。你给每个关节一个角度，我就能告诉你机器人手爪在哪儿、朝哪个方向。
这一章，我们就拿六轴工业机器人开刀，把正运动学的底裤扒干净。
3.1 什么是正运动学？
先看一个简单问题：
你有一个两连杆机构，肩关节转 30°，肘关节转 45°，末端在哪儿？
这就是正运动学——从关节空间（每个关节的角度值）映射到笛卡尔空间（末端的位置和姿态）。
对于六轴机器人，输入是六个关节角度 θ₁~θ₆，输出是末端坐标系相对于基坐标系的位姿矩阵 T₀₆。
核心公式：
T₀₆ = T₀₁(θ₁) · T₁₂(θ₂) · T₂₃(θ₃) · T₃₄(θ₄) · T₄₅(θ₅) · T₅₆(θ₆)
其中 Tᵢ₋₁ᵢ 是相邻连杆间的齐次变换矩阵。
你想想看，这其实就是把六个小变换串起来，得到一个总变换。每个小变换只依赖一个关节角度，其他参数（连杆长度、偏距、扭角）都是出厂就定死的。
3.2 六轴机器人的连杆参数
要建立正运动学模型，第一步是定义连杆参数。业内标准是 Denavit-Hartenberg 参数法，简称 DH 参数。
每个连杆需要四个参数：
| 参数 | 符号 | 含义 | 单位 |
|---|---|---|---|
| 连杆长度 | aᵢ | 沿 xᵢ 轴，从 zᵢ₋₁ 到 zᵢ 的距离 | mm |
| 连杆扭角 | αᵢ | 绕 xᵢ 轴，从 zᵢ₋₁ 到 zᵢ 的转角 | ° |
| 关节偏距 | dᵢ | 沿 zᵢ₋₁ 轴，从 xᵢ₋₁ 到 xᵢ 的距离 | mm |
| 关节角度 | θᵢ | 绕 zᵢ₋₁ 轴，从 xᵢ₋₁ 到 xᵢ 的转角 | ° |
我习惯把 DH 参数表列出来，再写代码。这样不容易乱。下面是一个典型六轴机器人的 DH 参数表：
| 关节 i | aᵢ (mm) | αᵢ (°) | dᵢ (mm) | θᵢ (°) |
|---|---|---|---|---|
| 1 | 0 | 0 | d₁ | θ₁ |
| 2 | a₂ | -90 | 0 | θ₂ |
| 3 | a₃ | 0 | 0 | θ₃ |
| 4 | 0 | -90 | d₄ | θ₄ |
| 5 | 0 | 90 | 0 | θ₅ |
| 6 | 0 | -90 | d₆ | θ₆ |
我的经验：DH 参数表一定要和机器人实际结构一一对应。我在项目中遇到过两次「正解算出来末端位置飞了」，最后发现是 α₂ 的符号搞反了。建议你拿到机器人图纸后，先手动对一对每个坐标轴的方向。
3.3 齐次变换矩阵的构建
有了 DH 参数，每个相邻连杆的变换矩阵就可以直接套公式：
Tᵢ₋₁ᵢ = Rot(z, θᵢ) · Trans(z, dᵢ) · Trans(x, aᵢ) · Rot(x, αᵢ)展开成 4×4 矩阵：
Tᵢ₋₁ᵢ = [
[cosθᵢ, -sinθᵢ·cosαᵢ, sinθᵢ·sinαᵢ, aᵢ·cosθᵢ],
[sinθᵢ, cosθᵢ·cosαᵢ, -cosθᵢ·sinαᵢ, aᵢ·sinθᵢ],
[0, sinαᵢ, cosαᵢ, dᵢ ],
[0, 0, 0, 1 ]
]嗯，这里要注意：矩阵里的 sin 和 cos 都是针对 θᵢ 和 αᵢ 的。别把顺序搞混了。
3.4 正运动学计算流程
整个计算流程其实就三步：
- 查表：从 DH 参数表取出 aᵢ, αᵢ, dᵢ, θᵢ
- 算矩阵：对每个关节 i，计算 Tᵢ₋₁ᵢ
- 连乘：T₀₆ = T₀₁ · T₁₂ · T₂₃ · T₃₄ · T₄₅ · T₅₆
我一般用 Python 写个函数，输入 θ₁~θ₆，输出 T₀₆。下面是我常用的代码骨架：
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):
# dh_params: list of [a, alpha, d] for each joint
T = np.eye(4)
for i in range(6):
a, alpha, d = dh_params[i]
Ti = dh_transform(a, alpha, d, theta[i])
T = T @ Ti
return T我曾经踩过的坑：用 Python 的 @ 运算符做矩阵乘法时，要确保两个矩阵都是 np.ndarray 类型。有一次我用了 list 嵌套，结果 @ 变成了逐元素乘法，末端位置直接偏了 200mm。排查了整整一个下午。
3.5 正运动学的几何意义
你可能会问：算出来一个 4×4 矩阵，然后呢？
T₀₆ 矩阵其实包含了两个信息：
- 位置：矩阵右上角的 3×1 列向量，就是末端在基坐标系下的 x, y, z 坐标
- 姿态：矩阵左上角的 3×3 旋转矩阵，表示末端坐标系相对于基坐标系的朝向
举个例子，如果 T₀₆ 是：
T₀₆ = [
[0, -1, 0, 500],
[1, 0, 0, 200],
[0, 0, 1, 800],
[0, 0, 0, 1]
]那就意味着：末端在 (500, 200, 800) 这个位置，而且末端坐标系的 x 轴指向基坐标系的 y 轴方向（因为旋转矩阵第一列是 [0,1,0]ᵀ）。
说白了，正运动学就是帮你把「关节转了多少度」翻译成「手在哪儿、朝哪儿」。这是所有运动学控制的基础。
3.6 正运动学验证方法
代码写完了，怎么知道算得对不对？
我一般用两个方法：
- 零位验证：把所有关节角度设为零，看末端位置是否和机械图纸一致
- 单关节运动：只转动一个关节，看末端轨迹是否合理（比如转关节 1，末端应该在水平面画圆）
举个例子，零位时（所有 θᵢ = 0），我的机器人末端位置应该是 (a₂ + a₃, 0, d₁ - d₄ - d₆)。如果算出来不对，那就是 DH 参数或者矩阵顺序有问题。
我的习惯：每次改完 DH 参数，先用零位验证跑一遍。这步花不了 30 秒，但能省下后面调试的半天时间。
3.7 正运动学的知识体系
下面这张图是我自己总结的正运动学知识结构，帮你理清思路：
3.8 常见问题与避坑
最后，分享几个我实际项目中遇到的坑：
- DH 参数符号搞反：α 的正负号决定了坐标轴旋转方向，错了末端姿态会完全不对
- 矩阵乘法顺序：一定是 T₀₁ · T₁₂ · T₂₃ · ...，不能反过来
- 角度单位：Python 的 math.sin 默认用弧度，DH 参数表里如果是角度，记得转弧度
- 浮点误差：连乘六个矩阵后，旋转矩阵可能不再正交。我一般会做一次 Gram-Schmidt 正交化
我曾经犯过的错：有一次在产线上，机器人抓取位置总是偏 2mm。查了两天，最后发现是 DH 参数里的 d₆ 写成了 0，而实际末端执行器有 50mm 的偏距。从那以后，我每次拿到新机器人，第一件事就是拿尺子量一遍所有连杆尺寸。
正运动学是机器人学的基石。你把它吃透了，后面学逆运动学、轨迹规划、动力学都会轻松很多。这一章的内容，我建议你亲手算一遍——拿笔算一个关节的变换矩阵，再用代码验证。嗯，动手才是最快的理解方式。