3. 运动学模型入门：刚体运动描述、旋转矩阵与欧拉角、齐次变换矩阵
好，我们正式开始运动学模型。这一章是整个模型辨识的地基。你想想看，人形机器人要走路、要抓东西，本质上就是在描述一个个刚体——大腿、小腿、躯干——它们之间怎么相对运动。说白了，就是搞清楚「谁相对于谁，在哪儿，朝哪儿」。我当年刚入行时，觉得这玩意儿就是纯数学，结果第一次调实际机器人，发现坐标系没对齐，直接让机器人来了个劈叉……嗯，从那以后我再也不敢小看这章了。
3.1 刚体运动描述：位置与姿态
一个刚体在三维空间里有几个自由度？六个。三个位置，三个姿态。位置好说，就是 (x, y, z)。姿态呢？就是它「朝哪边转」。我们得用数学工具把这六个量统一描述清楚。
核心概念：刚体运动 = 位置 + 姿态。位置用向量，姿态用旋转矩阵或欧拉角。
我个人习惯把位置和姿态分开处理。位置就是平移，姿态就是旋转。两者合在一起，就是刚体在空间中的完整位姿。
3.2 旋转矩阵：姿态的数学语言
旋转矩阵是个 3x3 的方阵。它描述了一个坐标系相对于另一个坐标系的朝向。为什么用矩阵？因为矩阵可以连续乘，方便我们做坐标变换。
举个例子，一个点 p 在坐标系 {A} 中的坐标是 ^A p，在坐标系 {B} 中的坐标是 ^B p。如果我知道 {B} 相对于 {A} 的旋转矩阵 ^A_B R，那么：
^A p = ^A_B R · ^B p就这么简单。旋转矩阵的每一列，其实就是 {B} 坐标系的三个轴在 {A} 坐标系下的投影。我在项目中遇到过一个问题：有人把旋转矩阵写反了，结果坐标变换全错。记住，下标和上标要对应好，^A_B R 是把 {B} 中的向量转到 {A} 中。
小技巧：旋转矩阵是正交矩阵，行列式为 +1。你可以用这个性质快速检查你的矩阵对不对。
3.3 欧拉角：更直观的旋转表示
旋转矩阵虽然好用，但 9 个数字太冗余了。人脑很难直接看出「这个矩阵对应什么角度」。所以就有了欧拉角——用三个角度来描述旋转。
常见的欧拉角顺序有 ZYX、ZYZ、XYZ 等。人形机器人里最常用的是 ZYX 欧拉角，也叫 Roll-Pitch-Yaw（滚转-俯仰-偏航）。
| 名称 | 旋转轴 | 物理含义 |
|---|---|---|
| Yaw（偏航） | Z 轴 | 左右转头 |
| Pitch（俯仰） | Y 轴 | 点头/抬头 |
| Roll（滚转） | X 轴 | 歪头 |
欧拉角有个大坑——万向锁。当 Pitch 接近 ±90° 时，Yaw 和 Roll 会变得无法区分。我曾经在调试一个六轴机械臂时，遇到了万向锁，结果末端执行器突然乱转。后来我改用四元数才解决。不过在本课程中，我们先用欧拉角，因为它直观，适合模型辨识的初期阶段。
注意：欧拉角不唯一。同样的姿态可以用不同的角度组合表示。做辨识时，一定要约定好旋转顺序，否则数据会乱。
3.4 齐次变换矩阵：统一平移与旋转
旋转矩阵只处理旋转，平移还得单独加。能不能把两者统一成一个操作？能。齐次变换矩阵就是干这个的。
它是一个 4x4 的矩阵：
| R t |
| 0 1 |其中 R 是 3x3 旋转矩阵，t 是 3x1 平移向量。最后一行是 [0 0 0 1]。
用齐次变换矩阵，一个点的坐标变换就变成了矩阵乘法：
^A p̃ = ^A_B T · ^B p̃这里的 p̃ 是齐次坐标，就是在普通坐标后面加个 1。比如 (x, y, z, 1)。
为什么加个 1？因为这样平移就能用矩阵乘法表示了。你想想看，如果没有齐次坐标，平移就是加法，旋转就是乘法，两者混在一起很麻烦。齐次变换矩阵把两者统一成乘法，方便我们做链式变换。
关键公式：齐次变换矩阵的逆矩阵：
| R^T -R^T·t |
| 0 1 |这个公式在模型辨识中经常用到，建议背下来。
3.5 知识体系总览
下面这张图把本章的核心逻辑串起来了。你可以把它当作一个思维导图来用。
3.6 实战中的坐标系约定
做模型辨识，第一件事就是定坐标系。我建议你遵循以下规则：
- 世界坐标系：固定在地面，Z 轴向上。
- 基坐标系：机器人底座，与世界坐标系对齐或偏移。
- 关节坐标系：每个关节一个，Z 轴沿关节旋转轴。
- 工具坐标系：末端执行器，比如手爪中心。
我曾经在项目里吃过亏：两个工程师用了不同的坐标系约定，结果数据合在一起完全对不上。后来我们统一用右手系，Z 轴向上，才解决问题。
建议：在代码里用结构体或类把旋转矩阵、平移向量、齐次变换矩阵封装好。别裸写数组，容易出错。
3.7 本章小结
这一章我们讲了三个核心工具：
- 旋转矩阵：描述姿态，9 个数字，正交性可做校验。
- 欧拉角：直观，但有万向锁问题，适合初期辨识。
- 齐次变换矩阵：统一平移和旋转，4x4 矩阵，链式运算的利器。