4、正运动学求解：从基座到末端的齐次变换
正运动学，说白了就是：给定六个关节角度，算出末端在哪儿、朝哪儿。
我刚入行时觉得这玩意儿就是矩阵乘法，没啥技术含量。直到有一次在产线上调试，机器人末端总是差那么几毫米对不准工件——查了两天才发现，是某个连杆的DH参数符号搞反了。嗯，从那以后我再也不敢小看这一步了。
这一章，我们就来手撕6轴工业机器人的正运动学。核心工具就是上一章建立的DH参数，加上齐次变换矩阵的连乘。
4.1 齐次变换矩阵回顾
每个关节i到关节i+1的变换，可以用一个4x4矩阵表示：
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 |我个人习惯把这个矩阵记作 A_i。每个A_i描述了从关节i坐标系到关节i+1坐标系的变换。
4.2 6轴机器人DH参数表
以典型的6轴工业机器人为例（比如常见的六关节串联臂），DH参数表长这样：
| 关节i | θ_i (关节变量) | d_i (mm) | a_i (mm) | α_i (rad) |
|---|---|---|---|---|
| 1 | θ₁ | d₁ | a₁ | -π/2 |
| 2 | θ₂ | 0 | a₂ | 0 |
| 3 | θ₃ | 0 | a₃ | -π/2 |
| 4 | θ₄ | d₄ | 0 | π/2 |
| 5 | θ₅ | 0 | 0 | -π/2 |
| 6 | θ₆ | d₆ | 0 | 0 |
注意：θ₁到θ₆是变量，其他参数（d₁、a₁、a₂、a₃、d₄、d₆）是机器人结构决定的常量。
4.3 正运动学推导：矩阵连乘
从基座到末端，总变换矩阵就是6个A_i的乘积：
T_06 = A_1 · A_2 · A_3 · A_4 · A_5 · A_6这个T_06是一个4x4矩阵：
| R_06(3x3) p_06(3x1) |
| 0 0 0 1 |其中：
- R_06：末端坐标系相对于基座的旋转矩阵（表示姿态）
- p_06：末端坐标系原点在基座坐标系中的位置（表示位置）
我建议你推导时不要一次性把6个矩阵全乘开——那会得到一个巨长的表达式，容易出错。我的做法是：
- 先算前两个：T_02 = A_1 · A_2
- 再乘第三个：T_03 = T_02 · A_3
- 依次类推，直到T_06
这样每一步的中间结果都可以打印出来检查。
4.4 位置与姿态的解析计算
最终T_06矩阵中，位置向量p_06就是末端在基座坐标系下的三维坐标(x, y, z)。
姿态则可以从旋转矩阵R_06中提取。常用的表示方式有两种：
| 表示方式 | 提取方法 | 适用场景 |
|---|---|---|
| 欧拉角 (RPY) | 从R_06计算绕固定轴的旋转角 | 人机交互、示教 |
| 四元数 | 从R_06的迹和元素计算 | 插值、控制 |
我个人更推荐在内部计算时用四元数，因为避免了万向锁问题。但给用户看的时候，欧拉角更直观。
4.5 代码实现
下面是一个Python实现的核心片段。我习惯用numpy来操作矩阵：
import numpy as np
def dh_transform(theta, d, a, alpha):
"""根据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
def forward_kinematics(joint_angles, dh_params):
"""
正运动学计算
joint_angles: [θ1, θ2, θ3, θ4, θ5, θ6]
dh_params: 每行 [d, a, alpha] 的列表（不含theta）
"""
T = np.eye(4)
for i in range(6):
theta = joint_angles[i]
d, a, alpha = dh_params[i]
Ai = dh_transform(theta, d, a, alpha)
T = T @ Ai
return T # 返回4x4齐次变换矩阵T = T @ Ai 是左乘——每次将新的关节变换乘到当前累积变换的右侧。这对应了从基座到末端的链式传递。
4.6 验证：一个简单例子
假设所有关节角度都为零：
joint_angles = [0, 0, 0, 0, 0, 0]
T_06 = forward_kinematics(joint_angles, dh_params)
print(T_06)你会得到一个矩阵，其位置向量p_06应该等于所有连杆长度在基座坐标系下的投影之和。如果结果不对，先检查DH参数表中的d和a值是否与机器人实际结构一致。
我记得有一次，学生拿到的机器人手册里d₁和a₁的定义方式和标准DH不一样，结果算出来末端位置差了半个手臂的长度。所以拿到任何机器人的参数，第一件事就是确认DH约定。
4.7 本章知识体系
下面这张图总结了正运动学的核心流程：
正运动学是整个运动控制的基础。你想想看，如果连末端在哪儿都算不准，后面的轨迹规划、逆运动学、力控制全都是空中楼阁。所以这一章的内容，值得你花时间亲手推导一遍、写一遍代码、再验证一遍。