运动学基础（一）：刚体运动描述、旋转矩阵、欧拉角与四元数

各位同学，欢迎来到第二章。

人形机器人要动起来，第一步是什么？不是写代码，不是调PID。是描述它怎么动。你得先能用数学语言，把机器人的每一个关节、每一块肢体在空间中的位置和姿态说清楚。这就是运动学基础。

我刚开始做机器人那会儿，觉得这玩意儿就是纯理论，没啥用。直到有一次，我给一个六轴机械臂做正运动学解算，算出来的末端位置跟实际差了十几厘米。查了三天，最后发现是旋转矩阵乘反了。嗯，从那以后，我再也不敢小看这些基础了。

1. 刚体运动：位置 + 姿态

人形机器人的躯干、大腿、小腿、手臂，都可以看作刚体。刚体就是受力不变形的理想物体。

描述一个刚体在空间中的状态，需要6个自由度：

• 3个平移自由度：决定刚体在空间中的位置（x, y, z）。
• 3个旋转自由度：决定刚体的朝向（绕x轴、y轴、z轴的转动）。

说白了，你告诉机器人“你的手在坐标(1,2,3)处，并且手掌朝前”，这就是一个完整的位姿描述。

2. 旋转矩阵：最直观，但最啰嗦

旋转矩阵是描述姿态最基础的工具。它是一个3x3的矩阵，列向量分别代表刚体坐标系在参考坐标系下的三个轴方向。

// 绕Z轴旋转θ角的旋转矩阵
R_z(θ) = [
  [cosθ, -sinθ, 0],
  [sinθ,  cosθ, 0],
  [0,      0,    1]
]

旋转矩阵有三个重要性质：

• 正交性：R^T = R^-1，矩阵的逆等于它的转置。
• 行列式为+1：保证是纯旋转，不包含缩放或镜像。
• 可组合：连续旋转可以用矩阵乘法表示。R = R₁ · R₂ · R₃

旋转矩阵的缺点也很明显：9个元素描述3个自由度，冗余信息太多。而且人眼很难从一堆数字里看出姿态是什么。这就引出了更紧凑的表达方式。

3. 欧拉角：直观但容易翻车

欧拉角用三个角度来描述旋转。最常见的是ZYX欧拉角（也叫RPY角）：

• Roll（横滚）：绕X轴旋转
• Pitch（俯仰）：绕Y轴旋转
• Yaw（偏航）：绕Z轴旋转

你想想看，飞机飞行员用的就是这套。说“机头偏左30度”，比说一个3x3矩阵方便多了。

另外，欧拉角的旋转顺序也很重要。ZYX和XYZ得到的结果完全不同。所以用欧拉角时，一定要在注释里写清楚旋转顺序。

4. 四元数：工业界的标准答案

四元数是什么？说白了，它是一个没有奇点、计算稳定、插值平滑的姿态描述方式。

四元数由四个数组成：q = (w, x, y, z)。其中w是实部，(x,y,z)是虚部。它满足：w² + x² + y² + z² = 1（单位四元数）。

为什么工业界都用四元数？三个理由：

1. 无万向锁：任何姿态都能平滑表示。
2. 插值方便：用球面线性插值（SLERP）可以在两个姿态之间生成平滑轨迹。
3. 计算高效：组合旋转只需要一次乘法，比矩阵快。

// 四元数乘法（组合旋转）
q_result = q1 * q2

// 公式（简化版）：
// w = w1*w2 - x1*x2 - y1*y2 - z1*z2
// x = w1*x2 + x1*w2 + y1*z2 - z1*y2
// y = w1*y2 - x1*z2 + y1*w2 + z1*x2
// z = w1*z2 + x1*y2 - y1*x2 + z1*w2

5. 三种方法的对比

特性	旋转矩阵	欧拉角	四元数
参数数量	9个	3个	4个
奇点问题	无	有（万向锁）	无
直观性	差	好	中等
插值平滑性	差	差	好（SLERP）
计算效率	低	高	高
工业应用	底层计算	人机交互	核心算法

6. 知识体系总览

下面这张图，把本章的核心逻辑串起来了。你可以把它当作一个思维导图来看。

7. 实战建议

学完理论，怎么落地？我建议你按这个顺序来：

1. 先写一个旋转矩阵类：实现绕X/Y/Z轴的旋转，验证正交性。
2. 再实现欧拉角 ↔ 旋转矩阵的转换：注意ZYX和XYZ顺序的区别。
3. 最后实现四元数类：包括乘法、归一化、到旋转矩阵的转换。
4. 写一个SLERP插值函数：在两个四元数之间生成10个中间姿态，验证平滑性。