2. 坐标系与旋转表示：欧拉角、旋转矩阵、四元数、坐标系变换基础

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

做姿态估计，说白了就是搞清楚一个物体「怎么转」的。你想想看，一个无人机在空中，它到底是头朝下还是尾巴朝下？这背后就是坐标系和旋转表示的问题。

我刚开始做IMU融合的时候，被这玩意儿坑过好几次。有一次四轴飞行器试飞，明明算法算出来是水平的，结果飞机一上天就翻了个跟头。后来查了三天，发现是旋转矩阵的乘法顺序写反了。嗯，从那以后，我对坐标系变换就特别较真。

2.1 坐标系：你得先有个「家」

没有坐标系，你没法描述位置和姿态。我们常用的就两种：

• 世界坐标系（World Frame）：固定在地面上的参考系。比如东北天坐标系（ENU），X轴指东，Y轴指北，Z轴指天。这是绝对的。
• 载体坐标系（Body Frame）：固定在运动物体上的。比如飞机的机头方向是X轴，右翼是Y轴，垂直向下是Z轴。这是相对的。

我个人习惯，在做传感器融合时，先把所有传感器的数据都转到载体坐标系下，再统一转到世界坐标系。这样不容易乱。

2.2 欧拉角：最直观，但最危险

欧拉角就是三个角度：偏航（Yaw）、俯仰（Pitch）、横滚（Roll）。

听起来很简单对吧？但这里有个大坑——万向锁（Gimbal Lock）。

我曾经在一个平衡车项目里，直接用欧拉角做姿态控制。结果车在快速转弯时，俯仰和偏航突然耦合了，车直接原地打转。为什么？因为当俯仰角接近90度时，偏航和横滚的旋转轴重合了，你丢失了一个自由度。

欧拉角的旋转顺序也很关键。常见的顺序有：ZYX、ZXY等。不同的顺序，结果完全不同。你想想看，先绕Z轴转90度，再绕Y轴转90度，和先绕Y轴转90度再绕Z轴转90度，结果能一样吗？

2.3 旋转矩阵：数学上最严谨

旋转矩阵是一个3x3的正交矩阵，行列式为+1。它可以把一个向量从一个坐标系旋转到另一个坐标系。

比如，绕Z轴旋转θ角，旋转矩阵就是：

R_z(θ) = [cosθ  -sinθ  0]
         [sinθ   cosθ  0]
         [0      0     1]

多个旋转可以连乘：R = R_z * R_y * R_x。注意顺序！矩阵乘法不满足交换律。

我记得有一次调试视觉SLAM，发现重投影误差一直降不下去。后来发现是旋转矩阵的转置和逆搞混了。旋转矩阵的逆就是它的转置，这个性质一定要记住。

2.4 四元数：工程上的最优解

四元数是一个超复数，形式为 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部。它用四个数表示旋转，没有万向锁问题，而且插值平滑。

为什么用四元数？说白了，就是因为它「好算」。旋转矩阵有9个数，但只有3个自由度，冗余大。欧拉角有奇点。四元数只有4个数，没有奇点，而且可以方便地做球面线性插值（SLERP）。

我现在的所有IMU融合算法，核心都是用四元数。比如，用四元数表示姿态更新：

q_new = q_old * exp(0.5 * ω * dt)

其中 ω 是角速度，dt 是时间间隔。这个公式在工程上非常实用。

2.5 坐标系变换：把一切串起来

在实际系统中，你会有多个传感器：IMU、摄像头、GPS、激光雷达。每个传感器都有自己的坐标系。你需要把它们统一到一个坐标系下。

比如，IMU测的是加速度和角速度，是在IMU坐标系下的。摄像头看到的是像素坐标，是在相机坐标系下的。你需要一个外参矩阵（旋转+平移）把它们关联起来。

我曾经做过一个多传感器融合的项目，IMU和摄像头之间的外参标定没做好，导致融合后的轨迹漂移严重。后来用了一个标定板，手动采集了100组数据，才把外参标定准。

下面这张图，是我总结的坐标系变换的核心逻辑：

2.6 实战中的选择建议

说了这么多，到底该用哪个？我个人的经验是：

场景	推荐表示	原因
人机交互（遥控器）	欧拉角	直观，容易理解
姿态控制（飞控）	四元数	无奇点，适合全姿态
视觉SLAM	旋转矩阵 + 四元数	矩阵便于优化，四元数便于插值
IMU融合（卡尔曼滤波）	四元数	状态量少，更新方便

好了，这一章的内容就到这里。坐标系和旋转表示是姿态估计的基石，你花再多时间在上面都值得。下一章我们会深入IMU的测量模型，看看加速度计和陀螺仪到底是怎么工作的。

---