3. 三维空间刚体运动：旋转矩阵、欧拉角、四元数、齐次变换矩阵

好，我们正式开始第三讲。如果你做过机器人，一定遇到过这个问题：怎么描述一个零件在空间里的姿态？说白了，就是它「转了多少」。

我刚开始做机械臂项目时，总觉得旋转不就是角度嘛，给三个数不就完了？结果第一次做逆解，发现角度算出来对不上，机械臂乱抖。嗯，从那以后我才老老实实把旋转矩阵、四元数这些东西啃了一遍。

这一章，我们就来把刚体运动的数学工具理清楚。你想想看，一个机械臂的末端执行器，既要平移又要旋转，怎么用一个统一的形式表达？答案就是——齐次变换矩阵。

核心观点：三维空间刚体运动 = 旋转 + 平移。旋转部分有3种主流表达方式：旋转矩阵、欧拉角、四元数。齐次变换矩阵把它们统一成一个4×4的矩阵，方便连续变换。

旋转矩阵，说白了就是一个3×3的正交矩阵，行列式为+1。它描述了一个坐标系相对于另一个坐标系的朝向。

假设你有一个点 p，在坐标系{A}中的坐标是 ^Ap，在坐标系{B}中的坐标是 ^Bp。那么：

^Ap = ^AR_B · ^Bp

这里的 ^AR_B 就是旋转矩阵。它的每一列，其实就是{B}坐标系的基向量在{A}中的投影。

我在项目中遇到过一个问题：两个工程师分别用不同的旋转矩阵表示同一个姿态，结果因为数值误差，矩阵不再正交，导致后续计算全错。所以，使用旋转矩阵时，一定要定期做正交化处理。

小技巧：旋转矩阵有9个元素，但自由度只有3。这意味着它有很多冗余。如果你在做优化或滤波，记得用流形上的约束，别直接优化9个数。

欧拉角用三个角度来描述旋转，比如绕X轴转α，绕Y轴转β，绕Z轴转γ。常见的顺序有ZYX、XYZ、ZYZ等。

你想想看，为什么会有这么多顺序？因为旋转不满足交换律。先绕X转再绕Y转，和先绕Y转再绕X转，结果完全不同。

⚠ 万向锁：当中间角达到±90°时，两个旋转轴会重合，丢失一个自由度。我曾经在调试六轴机械臂时，用欧拉角做插值，结果经过万向锁附近时，机械臂突然反转了180°。嗯，从那以后，插值我坚决用四元数。

四元数听起来高大上，其实就是一个四维向量：q = (w, x, y, z)，其中 w 是实部，x, y, z 是虚部。它满足 w² + x² + y² + z² = 1。

为什么用四元数？三个理由：

四元数旋转一个向量 v 的公式：

v' = q · v · q^-1

其中 q^-1 = (w, -x, -y, -z) / ||q||²。如果 q 是单位四元数，那 q^-1 就是它的共轭。

我的习惯：在代码里，我统一用四元数做内部计算，只在用户界面或调试时转成欧拉角给人看。这样既避免了万向锁，又保持了可读性。

好了，现在我们把旋转和平移合在一起。齐次变换矩阵是一个4×4的矩阵：

T = | R   t |
    | 0   1 |

其中 R 是3×3旋转矩阵，t 是3×1平移向量。最后一行是 (0, 0, 0, 1)。

用齐次坐标，一个点 p 的变换可以写成：

| ^Ap |   | ^AR_B   ^At_B |   | ^Bp |
|  1  | = |   0     1  | · |  1  |

连续变换就是矩阵乘法：^AT_C = ^AT_B · ^BT_C。这就是机器人运动学的基础。

避坑指南：我曾经在写URDF解析器时，发现两个相邻连杆的变换矩阵乘反了。记住：^AT_B 表示从{B}到{A}的变换。如果你要从基座到末端，就顺着链一路乘下去。

你想想看，在实际项目中怎么选？我个人建议：

总结一下：刚体运动的核心就是旋转+平移。旋转矩阵、欧拉角、四元数各有优劣，齐次变换矩阵把它们统一起来。掌握了这些工具，你就能描述任何机器人的位姿了。