3. 数学基础（中）：旋转矩阵、欧拉角、四元数——三种旋转表示方法及其转换
好，咱们接着聊旋转的数学表示。上一节我们把刚体变换拆成了旋转+平移，平移很简单，就是个三维向量加减。但旋转这玩意儿，说实话，坑不少。
我在项目里见过不少同事，一上来就用欧拉角，结果遇到万向锁，整个系统直接炸了。也有人死磕旋转矩阵，16个浮点数存着，看着就心疼内存。所以这一节，我把三种主流旋转表示法——旋转矩阵、欧拉角、四元数——掰开了讲清楚。你以后做融合时，该用哪个、怎么切换，心里就有数了。
核心观点：没有完美的旋转表示法。旋转矩阵直观但冗余，欧拉角易理解但会锁死，四元数高效但反直觉。实际工程中，我习惯用四元数做运算，用欧拉角做调试，用旋转矩阵做中间转换。
3.1 旋转矩阵：最正统，但也最啰嗦
旋转矩阵，说白了就是一个3×3的正交矩阵，行列式为+1。它描述了一个坐标系相对于另一个坐标系的朝向。你想想看，一个点p在A系下的坐标，乘以R_AB，就得到了它在B系下的坐标。
公式很简单：p_B = R_AB * p_A
旋转矩阵的好处是——可以链式乘法。比如你有三个传感器，激光雷达→IMU→车身，外参一乘就完事了。我在做多传感器标定时，最喜欢用旋转矩阵做中间表示，因为乘法不丢精度。
但缺点也很明显：9个元素，只有3个自由度，剩下6个是冗余约束。你存一个旋转矩阵，要存9个float，而四元数只要4个。嵌入式平台上，这差距就出来了。
我的习惯：旋转矩阵只做中间转换用，不长期存储。每次标定完外参，我都会转成四元数存到配置文件中，省内存、也方便后续做插值。
3.2 欧拉角：直观，但小心「万向锁」
欧拉角用三个角度表示旋转：绕X轴转α（横滚roll）、绕Y轴转β（俯仰pitch）、绕Z轴转γ（偏航yaw）。顺序很重要！常见的顺序有ZYX、XYZ等，不同顺序得到的结果完全不同。
我在调试自动驾驶车辆时，经常用欧拉角看IMU的姿态。比如车在平路上，pitch接近0，roll接近0，yaw就是车头朝向。非常直观，一眼就能看出问题。
但欧拉角有个致命缺陷——万向锁。当pitch接近±90°时，roll和yaw的旋转轴会重合，丢失一个自由度。这时候你无论怎么调roll和yaw，效果都一样，系统就「锁死」了。
避坑指南：我曾经在一个无人机项目中，用欧拉角做姿态控制。飞机做大仰角机动时，pitch接近90°，控制器直接失稳，差点炸机。后来全部换成四元数，问题解决。所以——任何涉及大角度运动的场景，别用欧拉角做内部运算。
欧拉角转旋转矩阵的公式（以ZYX顺序为例）：
R = R_z(γ) * R_y(β) * R_x(α)
其中：
R_x(α) = [1, 0, 0;
0, cosα, -sinα;
0, sinα, cosα]
R_y(β) = [ cosβ, 0, sinβ;
0, 1, 0;
-sinβ, 0, cosβ]
R_z(γ) = [cosγ, -sinγ, 0;
sinγ, cosγ, 0;
0, 0, 1]3.3 四元数：SLAMer的瑞士军刀
四元数，很多人一听就头大。其实没那么玄乎。它就是一个超复数：q = w + xi + yj + zk，其中w是实部，(x,y,z)是虚部，且满足i² = j² = k² = ijk = -1。
单位四元数（模长为1）可以用来表示旋转。绕单位向量n旋转θ角，对应的四元数是：
q = [cos(θ/2), n_x*sin(θ/2), n_y*sin(θ/2), n_z*sin(θ/2)]四元数的好处太多了：
- 无奇点——不会像欧拉角那样锁死
- 紧凑——4个浮点数，比旋转矩阵少一半多
- 可插值——SLAM中做位姿平滑、关键帧插值，都用球面线性插值（SLERP）
- 组合高效——两个四元数相乘，就是旋转的复合
但缺点也有：不直观。你拿一个四元数给我，我看不出它代表什么姿态。所以我调试时，会先把四元数转成欧拉角，打印出来看。
工程经验：在LOAM系列算法中，位姿优化全部用四元数做参数化。为什么？因为优化过程中要不断迭代，如果用欧拉角，迭代到万向锁附近就崩了。四元数没有这个问题，而且雅可比矩阵也更好算。
3.4 三种表示法的相互转换
实际工程中，这三种表示法经常需要互相转换。我整理了一张表，你直接拿去用：
| 转换方向 | 方法简述 | 注意事项 |
|---|---|---|
| 旋转矩阵 → 四元数 | 从矩阵迹中提取w，再反算x,y,z | 注意数值稳定性，当迹接近-1时需特殊处理 |
| 四元数 → 旋转矩阵 | 直接用四元数元素构造矩阵 | 确保四元数已归一化，否则矩阵不正交 |
| 旋转矩阵 → 欧拉角 | 从矩阵元素中反解三个角度 | 注意万向锁判断（cosβ接近0时） |
| 欧拉角 → 旋转矩阵 | 按顺序乘三个基本旋转矩阵 | 顺序必须明确，ZYX和XYZ结果不同 |
| 四元数 → 欧拉角 | 先转矩阵，再转欧拉角；或直接公式 | 同样注意万向锁，建议用atan2避免象限问题 |
| 欧拉角 → 四元数 | 先转矩阵，再转四元数；或直接公式 | 顺序一致性是关键 |
下面给一段C++代码，展示四元数转旋转矩阵的典型实现。这段代码我在多个项目里用过，很稳：
// 四元数转旋转矩阵
// 输入: 单位四元数 q = [w, x, y, z]
// 输出: 3x3 旋转矩阵 R
Eigen::Matrix3d quatToRotMat(const Eigen::Quaterniond& q) {
double w = q.w(), x = q.x(), y = q.y(), z = q.z();
Eigen::Matrix3d R;
R(0,0) = 1 - 2*y*y - 2*z*z;
R(0,1) = 2*x*y - 2*w*z;
R(0,2) = 2*x*z + 2*w*y;
R(1,0) = 2*x*y + 2*w*z;
R(1,1) = 1 - 2*x*x - 2*z*z;
R(1,2) = 2*y*z - 2*w*x;
R(2,0) = 2*x*z - 2*w*y;
R(2,1) = 2*y*z + 2*w*x;
R(2,2) = 1 - 2*x*x - 2*y*y;
return R;
}小技巧：用Eigen库的话，直接 Eigen::Quaterniond q; Eigen::Matrix3d R = q.toRotationMatrix(); 一行搞定。但面试时，我建议你还是能手写出来，很多公司会考这个。
3.5 工程中怎么选？我的决策框架
说了这么多，到底什么时候用哪个？我个人的决策逻辑是这样的：
- 做标定、做外参链式乘法 → 旋转矩阵。因为乘法直观，不容易出错。
- 做SLAM位姿优化、做插值 → 四元数。无奇点、省内存、可SLERP。
- 做可视化、做调试打印 → 欧拉角。一眼就能看出车是不是翻了。
- 做IMU姿态解算 → 四元数。因为IMU更新频率高，四元数积分比欧拉角稳定得多。
嗯，这里要注意一点：转换过程中一定要做归一化。尤其是四元数，经过多次乘法后，浮点误差会累积，模长不再为1。我习惯每帧都对四元数做一次归一化，成本很低，但能避免很多诡异的问题。
曾经踩过的坑：有一次做多雷达融合，两个雷达的外参用四元数存着，但中间经过几次坐标变换后，四元数模长变成了1.002。结果融合出来的点云出现重影，查了两天才发现是四元数没归一化。从那以后，我所有四元数运算后都加一行 q.normalize()。
好了，三种旋转表示法就讲到这里。你只要记住：旋转矩阵是「通用语」，欧拉角是「人话」，四元数是「机器语」。在不同的场景下，选对工具，你的代码会干净很多。