2、刚体姿态描述：旋转矩阵、欧拉角、四元数、齐次变换矩阵

聊到机器人运动学，第一个绕不开的问题就是：怎么描述一个刚体在空间里“转”成了什么样子？

位置好办，三个坐标 (x, y, z) 就搞定了。但姿态——也就是朝向——就没那么直观了。我刚开始做机器人那会儿，总觉得姿态不就是三个角度嘛，后来真上手写代码才发现，这里面的坑一个接一个。

这一章，咱们就把四种最常用的姿态描述方法掰开揉碎讲清楚。它们分别是：旋转矩阵、欧拉角、四元数、齐次变换矩阵。

2.1 旋转矩阵——最笨但最可靠的方法

旋转矩阵，说白了就是用 3×3 的矩阵来描述一个坐标系相对于另一个坐标系的朝向。

假设我们有一个参考坐标系 {A}，和一个固连在刚体上的坐标系 {B}。那么旋转矩阵 ^A_BR 的每一列，就是 {B} 的基向量在 {A} 中的投影。

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

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

• 正交性：R^TR = I，也就是说逆矩阵等于转置。
• 行列式为 +1：保证了是纯旋转，没有缩放或镜像。

旋转矩阵的优点很明显：没有奇异性，任意姿态都能唯一表示。缺点也很要命：9 个元素，6 个冗余约束，做插值运算时效率低，还不直观。

2.2 欧拉角——直观但暗藏杀机

欧拉角用三个角度来描述旋转。最常见的用法是 ZYX 顺序：先绕 Z 轴转 yaw（偏航），再绕 Y 轴转 pitch（俯仰），最后绕 X 轴转 roll（横滚）。

你想想看，飞机、无人机、手机里的姿态传感器，基本都是用欧拉角输出的。为什么？因为直观啊！

// ZYX 欧拉角转旋转矩阵
R = R_z(yaw) * R_y(pitch) * R_x(roll)

欧拉角的三个主要问题：

• 万向锁：第二个角度为 ±90° 时，第一个和第三个角度无法区分。
• 多值性：同一姿态对应多组欧拉角。
• 顺序依赖：ZYX、XYZ、ZYZ……不同顺序结果完全不同。

2.3 四元数——插值神器

四元数，说白了就是一个实部加三个虚部：q = w + xi + yj + zk，或者写成 (w, x, y, z)。

它最牛的地方是：没有奇异性，而且做球面线性插值（SLERP）特别顺滑。

// 单位四元数表示绕单位向量 (x,y,z) 旋转 θ 角
q = (cos(θ/2), x*sin(θ/2), y*sin(θ/2), z*sin(θ/2))

// 四元数乘法（对应旋转复合）
q_result = q2 * q1  // 先应用 q1，再应用 q2

我记得第一次用四元数做相机姿态平滑时，效果惊艳。欧拉角插值会出现“抖动”，四元数 SLERP 出来的轨迹就像丝滑巧克力一样。

2.4 齐次变换矩阵——位姿一把抓

齐次变换矩阵，就是把旋转和位移塞到一个 4×4 的矩阵里。

T = [
  [R(3×3),  t(3×1)],
  [0 0 0,    1    ]
]

其中 R 是旋转矩阵，t 是平移向量。用齐次坐标的好处是：连续的坐标变换可以写成矩阵乘法链。

比如，从基座到末端执行器的变换：

T_0n = T_01 * T_12 * T_23 * ... * T_(n-1)n

嗯，这里要注意：齐次变换矩阵的逆矩阵也很好求，不用真的去算 4×4 的逆。

T_inv = [
  [R^T,  -R^T * t],
  [0 0 0,    1   ]
]

2.5 四种方法的对比总结

我个人在实际项目中的选择策略是这样的：

• 显示给用户看：用欧拉角，因为人看得懂。
• 内部姿态插值：用四元数，SLERP 真香。
• 运动学链计算：用齐次变换矩阵，链式乘法太方便了。
• 需要求逆或解算：用旋转矩阵，性质好，数学工具多。

好了，这一章的内容就到这里。姿态描述是机器人学的基石，把这四种方法吃透了，后面学运动学正解、逆解、轨迹规划都会轻松很多。