4. 运动学基础：正运动学与逆运动学概念
各位同学，欢迎来到第四章。今天咱们聊聊机器人学里最基础、也最绕不开的两个概念——正运动学和逆运动学。
说白了，正运动学就是：我知道每个关节转了多少角度，求末端在哪儿。逆运动学正好反过来：我知道末端要去哪儿，反推每个关节该转多少。
听起来好像正着推简单，反着推难？没错，实际情况就是这样。我在做第一个六轴机器人项目时，正运动学写了一个下午就调通了。逆运动学嘛……嗯，折腾了整整一周，还差点把电机烧了。后面我会细说这个坑。
4.1 为什么先学正运动学？
你可能会问：既然实际控制时更多用逆运动学，为什么教材总先讲正运动学？
原因有两点：
- 正运动学是逆运动学的基础——你连末端位置怎么算都不知道，怎么可能反推关节角？
- 正运动学有唯一解——给定一组关节角，末端位姿是确定的。这方便我们验证算法对不对。
我记得刚入行时，带我的老工程师说过一句话：「正运动学是你的右手，逆运动学是你的左手。右手不会的，左手也别想会。」当时觉得夸张，后来踩了坑才明白——确实如此。
4.2 二连杆机械臂：最简单的模型
为了讲清楚，咱们用一个最经典的模型——平面二连杆机械臂。
它长这样：
- 两个连杆：长度分别为 L₁ 和 L₂
- 两个关节：关节1（肩关节）和关节2（肘关节），都是旋转关节
- 末端：就是第二个连杆的端点
为什么选它？因为够简单，能用手算，又能把核心概念讲透。我在培训新人时，从来都是先让他们手推二连杆，再用代码验证。没人能跳过这一步。
核心思路：把每个连杆的坐标系变换写成矩阵，然后乘起来，就得到末端相对于基座的位姿。
4.3 正运动学方程推导
好，咱们开始手推。别怕，就几步。
4.3.1 建立坐标系
首先，在每个关节处建一个坐标系：
- 坐标系 {0}：固定在基座，x₀ 轴水平向右，y₀ 轴竖直向上
- 坐标系 {1}：固定在关节1，随连杆1转动
- 坐标系 {2}：固定在关节2，随连杆2转动
这里有个小技巧：坐标系 {i} 的 z 轴都指向纸面外（右手定则），这样旋转就发生在 xy 平面内，好算。
4.3.2 写出变换矩阵
从坐标系 {0} 到 {1} 的变换，包含两步：
- 绕 z₀ 轴旋转 θ₁ 角度
- 沿 x₁ 轴平移 L₁
写成矩阵形式：
T₀₁ = Rot(z, θ₁) · Trans(L₁, 0, 0)
= [cosθ₁ -sinθ₁ 0 L₁·cosθ₁]
[sinθ₁ cosθ₁ 0 L₁·sinθ₁]
[0 0 1 0 ]
[0 0 0 1 ]同理，从 {1} 到 {2}：
T₁₂ = Rot(z, θ₂) · Trans(L₂, 0, 0)
= [cosθ₂ -sinθ₂ 0 L₂·cosθ₂]
[sinθ₂ cosθ₂ 0 L₂·sinθ₂]
[0 0 1 0 ]
[0 0 0 1 ]4.3.3 末端位姿
末端在坐标系 {2} 中的位置是 (0, 0, 0, 1)ᵀ（就是原点）。要得到它在基座坐标系 {0} 中的位置，只需要左乘所有变换矩阵：
T₀₂ = T₀₁ · T₁₂算出来之后，取左上角 3×3 是旋转矩阵，最后一列前三个元素是位置：
x = L₁·cosθ₁ + L₂·cos(θ₁+θ₂)
y = L₁·sinθ₁ + L₂·sin(θ₁+θ₂)这就是二连杆机械臂的正运动学方程。
我的习惯：每次推完公式，我都会代入一组简单数值验算。比如 θ₁=0, θ₂=0，此时末端应该在 (L₁+L₂, 0)。代入公式：x = L₁·1 + L₂·1 = L₁+L₂，y = 0。对了。这个小习惯帮我抓出过不少笔误。
4.4 正运动学的几何意义
你仔细看这个公式：
- L₁·cosθ₁ 是连杆1在 x 方向的投影
- L₂·cos(θ₁+θ₂) 是连杆2在 x 方向的投影
- 两个投影加起来，就是末端 x 坐标
说白了，正运动学就是把每个连杆的贡献投影到基座坐标系上，然后累加。这个思路可以推广到任意自由度——只不过高自由度时，投影计算要用矩阵乘法自动完成。
下面这张图帮你直观理解整个流程：
4.5 正运动学 vs 逆运动学
讲完正运动学，咱们简单对比一下这对「孪生兄弟」：
| 对比项 | 正运动学 | 逆运动学 |
|---|---|---|
| 已知 | 关节角 θ₁, θ₂ | 末端位置 (x, y) |
| 求解 | 末端位姿 | 关节角 |
| 解的数量 | 唯一解 | 多解或无解 |
| 计算难度 | 简单，直接代入 | 复杂，需解方程组 |
| 典型用途 | 仿真、验证 | 轨迹规划、控制 |
我曾经踩过的坑：刚做第一个焊接机器人项目时，我直接用逆运动学算出来的角度发给电机，结果机器人「咔」一声撞到了工件上。后来才发现——逆运动学有多组解，我选的那组虽然数学上正确，但会让机器人穿过工作台。从那以后，我每次用逆运动学都会先做碰撞检测，再选最优解。
4.6 代码实现：验证正运动学
理论推完了，咱们写几行代码验证一下。我用 Python，你用什么语言都行，逻辑一样：
import numpy as np
def forward_kinematics(theta1, theta2, L1, L2):
"""
二连杆机械臂正运动学
:param theta1: 关节1角度 (rad)
:param theta2: 关节2角度 (rad)
:param L1: 连杆1长度
:param L2: 连杆2长度
:return: (x, y) 末端位置
"""
x = L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2)
y = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)
return x, y
# 测试：θ₁=30°, θ₂=45°, L₁=1.0, L₂=0.8
theta1 = np.deg2rad(30)
theta2 = np.deg2rad(45)
L1, L2 = 1.0, 0.8
x, y = forward_kinematics(theta1, theta2, L1, L2)
print(f"末端位置: x={x:.3f}, y={y:.3f}")
# 输出: 末端位置: x=1.432, y=1.366你可以改改角度试试。比如把 θ₂ 设成 0，看看末端是不是在 (L₁+L₂, 0) 附近。这个小实验我每次培训都让学员做——自己动手算一遍，比听十遍都管用。
4.7 小结
这一章咱们干了三件事：
- 搞清楚了正运动学和逆运动学的区别
- 用手推了二连杆的正运动学公式
- 用代码验证了推导结果
正运动学虽然简单，但它是整个运动学体系的基石。后面讲逆运动学、速度运动学、轨迹规划时，你会发现——所有复杂的东西，都是从这几行公式长出来的。
嗯，今天就到这儿。公式不多，但值得你多花点时间亲手推一遍。