4、正运动学建模：从关节空间到笛卡尔空间的映射
正运动学，说白了就是回答一个问题：“我给了机器人每个关节一个角度，它的末端到底在哪儿？”
这个问题听起来简单，但实际做起来，你会发现里面全是矩阵乘法。我个人习惯把正运动学看作是“从关节空间到笛卡尔空间的一座桥”。关节空间就是电机转动的角度值，笛卡尔空间就是我们熟悉的XYZ坐标和姿态。
4.1 为什么需要正运动学？
你想想看，我们控制机器人，最终目的是让它的末端执行器（比如夹爪、焊枪）到达某个位置。但电机只能转角度，不能直接说“去X=100，Y=200”。所以，我们需要一个数学工具，把角度换算成坐标。
我在项目中遇到过这样一个坑：有一次调试六轴机器人，我直接给了末端一个笛卡尔坐标指令，结果机器人“咔”一声就撞到了限位。后来才发现，我忘了做正运动学验证——有些关节角度组合，末端虽然能到那个点，但中间会穿过奇异位形。嗯，这里要注意，正运动学不仅仅是算终点，它还能帮你预判路径上的风险。
核心概念：
- 关节空间：由所有关节角度组成的向量空间，比如 (θ₁, θ₂, θ₃, θ₄, θ₅, θ₆)
- 笛卡尔空间：由位置 (x, y, z) 和姿态 (roll, pitch, yaw 或 旋转矩阵) 组成的空间
- 正运动学映射：f(θ₁, θ₂, ..., θₙ) → (x, y, z, R)
4.2 DH参数：标准化你的机器人
DH参数法，全称Denavit-Hartenberg参数法。它就像给每个关节发了一张“身份证”，上面写着四个数字：a, α, d, θ。
为什么需要这四个参数？因为任何两个相邻的连杆，它们之间的相对位置和姿态，都可以用这四个参数唯一确定。我刚开始学的时候也觉得抽象，后来自己画了一个两连杆的机械臂，手动推导了一遍，一下子就通了。
| 参数 | 含义 | 物理意义 |
|---|---|---|
| aᵢ | 连杆长度 | 沿Xᵢ轴，从Zᵢ₋₁到Zᵢ的距离 |
| αᵢ | 连杆扭转角 | 绕Xᵢ轴，从Zᵢ₋₁到Zᵢ的转角 |
| dᵢ | 连杆偏距 | 沿Zᵢ₋₁轴，从Xᵢ₋₁到Xᵢ的距离 |
| θᵢ | 关节角 | 绕Zᵢ₋₁轴，从Xᵢ₋₁到Xᵢ的转角 |
我的小技巧：建立DH坐标系时，记住一个口诀——“Z轴是关节轴，X轴是公垂线”。每次建系前，先画Z轴，再画X轴，最后用右手定则确定Y轴。这样不容易乱。
4.3 正运动学方程推导
有了DH参数，正运动学就变成了一个机械化的过程。每个关节对应一个齐次变换矩阵 Tᵢ，形式如下：
Tᵢ = Rot(z, θᵢ) · Trans(z, dᵢ) · Trans(x, aᵢ) · Rot(x, αᵢ)展开成矩阵形式：
Tᵢ = | cosθᵢ -sinθᵢ·cosαᵢ sinθᵢ·sinαᵢ aᵢ·cosθᵢ |
| sinθᵢ cosθᵢ·cosαᵢ -cosθᵢ·sinαᵢ aᵢ·sinθᵢ |
| 0 sinαᵢ cosαᵢ dᵢ |
| 0 0 0 1 |然后，从基座到末端，把所有变换矩阵乘起来：
T₀ₙ = T₁ · T₂ · T₃ · ... · Tₙ这个 T₀ₙ 矩阵的左上角3x3就是末端姿态，右上角3x1就是末端位置。
我曾经踩过的坑：矩阵乘法顺序不能错！T₁·T₂ 和 T₂·T₁ 是完全不同的结果。我刚开始用MATLAB写的时候，把乘号左右搞反了，结果算出来的末端位置飞到了天上去。检查了整整两个小时才发现。
4.4 MATLAB/Python实现
下面我给出一个标准的六轴机器人正运动学实现。以经典的PUMA 560机器人为例，它的DH参数表如下：
| 关节 i | aᵢ (mm) | αᵢ (rad) | dᵢ (mm) | θᵢ (rad) |
|---|---|---|---|---|
| 1 | 0 | π/2 | 0 | θ₁ |
| 2 | 431.8 | 0 | 0 | θ₂ |
| 3 | 20.32 | -π/2 | 149.09 | θ₃ |
| 4 | 0 | π/2 | 433.07 | θ₄ |
| 5 | 0 | -π/2 | 0 | θ₅ |
| 6 | 0 | 0 | 56.25 | θ₆ |
Python实现（使用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
def forward_kinematics(theta_list, dh_params):
"""
正运动学计算
theta_list: [θ₁, θ₂, θ₃, θ₄, θ₅, θ₆]
dh_params: 每行 [a, alpha, d, theta_offset]
"""
T = np.eye(4)
for i in range(6):
a, alpha, d, theta_offset = dh_params[i]
theta = theta_list[i] + theta_offset
T_i = dh_transform(a, alpha, d, theta)
T = T @ T_i # 注意：左乘
return T
# PUMA 560 DH参数（theta_offset通常为0）
dh_params = [
[0, np.pi/2, 0, 0],
[431.8, 0, 0, 0],
[20.32, -np.pi/2, 149.09, 0],
[0, np.pi/2, 433.07, 0],
[0, -np.pi/2, 0, 0],
[0, 0, 56.25, 0]
]
# 测试：所有关节角度为0
theta_test = [0, 0, 0, 0, 0, 0]
T_end = forward_kinematics(theta_test, dh_params)
print("末端位置 (x, y, z):", T_end[:3, 3])
print("末端姿态矩阵:\n", T_end[:3, :3])
运行结果解读：当所有关节角度为0时，PUMA 560的末端位置大约在 (431.8, 0, 149.09+433.07+56.25) = (431.8, 0, 638.41) 附近。这个结果可以手动验证——关节2和关节3的连杆都伸直了，末端自然在最远端。
4.5 知识体系总览
下面这张图，是我自己总结的正运动学知识结构。你可以把它当作一张“地图”，学完这一章后，你应该能闭着眼睛画出这个流程。
这张图从左到右，从上到下，就是正运动学的完整流程。你写代码的时候，就照着这个流程一步步来，保证不会乱。
避坑指南：我曾经在写代码时，把DH参数表中的单位搞混了——a和d的单位是毫米，但角度单位是弧度。结果算出来的位置差了57倍（因为1弧度≈57.3度）。所以，一定要统一单位。我建议所有长度用毫米，所有角度用弧度，别混用。
好了，正运动学就讲到这里。你只要把DH参数表建好，矩阵乘法写对，剩下的就是调参和验证了。下一节我们会讲逆运动学——也就是反过来，已知末端位置，反推关节角度。那个更有意思，也更头疼。
可可精品资料库