第二章：坐标系与姿态表示
各位同学，咱们今天聊聊坐标系和姿态表示。说实话，这章是传感器融合的“地基”。地基没打好，后面盖什么楼都得塌。我当年刚入行时，就在这上面栽过跟头——明明算法算得挺漂亮，结果飞控一跑起来，姿态直接翻了个个儿。后来才发现，是坐标系定义搞反了。
2.1 世界坐标系与机体坐标系
先说说世界坐标系。你可以把它想象成一个“绝对参考系”。通常我们取东北天（ENU）或者北东地（NED）作为标准。我个人习惯用东北天，因为直观——X轴指向东，Y轴指向北，Z轴指向天。嗯，这里要注意，不同行业习惯不一样，航空航天喜欢NED，机器人领域更爱ENU。
机体坐标系呢？它绑在设备上。比如你手里拿个PDA，屏幕朝上，那X轴一般指向设备右侧，Y轴指向前方，Z轴垂直屏幕向上。说白了，设备怎么动，这个坐标系就怎么动。
为什么要区分这两个坐标系？因为传感器数据都是在机体坐标系下测的，但导航、控制需要世界坐标系下的信息。你得做转换。我在项目中遇到过，有人直接把加速度计读数当世界坐标系下的重力方向用，结果可想而知——姿态解算全乱套了。
2.2 欧拉角
欧拉角，说白了就是用三个角度来描述姿态。分别是：
- 横滚角（Roll）：绕X轴旋转，像飞机侧身
- 俯仰角（Pitch）：绕Y轴旋转，像飞机抬头低头
- 偏航角（Yaw）：绕Z轴旋转，像飞机转向
你想想看，这三个角度是不是很直观？我刚开始学的时候也觉得挺好理解。但实际用起来，坑不少。
欧拉角的旋转顺序也很关键。常见的顺序有ZYX、ZXY等。不同顺序，同一个角度组合代表不同姿态。我建议你在项目一开始就定好顺序，并在代码注释里写清楚。不然过两个月你自己都忘了。
2.3 旋转矩阵
旋转矩阵，就是用3x3的矩阵来表示旋转。它没有万向锁问题，运算也方便。比如，把机体坐标系下的向量v转换到世界坐标系下：
v_world = R * v_body其中R就是旋转矩阵。它由三个基本旋转矩阵相乘得到：
R = Rz(yaw) * Ry(pitch) * Rx(roll)每个基本旋转矩阵长这样（绕X轴旋转角度θ）：
Rx(θ) = [1, 0, 0 ]
[0, cosθ, -sinθ ]
[0, sinθ, cosθ ]不过旋转矩阵也有缺点——它用了9个参数，但实际只有3个自由度。冗余参数多了，运算量就大，还容易引入数值误差。我在做嵌入式优化时，就因为这个原因放弃了旋转矩阵，改用四元数。
2.4 四元数基础
四元数，听起来挺玄乎。其实你可以把它理解成“带约束的复数”。一个四元数q由四个分量组成：
q = [w, x, y, z]其中w是实部，x、y、z是虚部。它满足一个约束：w² + x² + y² + z² = 1。这叫单位四元数。
为什么用四元数？三个理由：
- 无万向锁——随便你怎么转，不会丢自由度
- 运算快——只有4个参数，乘法比矩阵快
- 易插值——做平滑旋转时，球面线性插值（SLERP）很自然
四元数表示旋转的公式也很简洁。比如，用四元数q旋转一个向量v：
v' = q * v * q_conj其中q_conj是q的共轭。注意，这里的乘法是四元数乘法，不是普通乘法。我刚开始写代码时，经常搞混乘法顺序，结果旋转方向反了。嗯，这个坑我踩过。
2.5 三种表示方式的转换
实际项目中，这三种表示方式经常需要互相转换。我整理了一个对照表：
| 转换方向 | 公式/方法 | 注意事项 |
|---|---|---|
| 欧拉角 → 旋转矩阵 | 三个基本矩阵相乘 | 注意旋转顺序 |
| 旋转矩阵 → 四元数 | 从矩阵元素提取 | 注意数值稳定性 |
| 四元数 → 欧拉角 | 用atan2和asin | 注意奇点处理 |
举个例子，从四元数转欧拉角的代码（C语言风格）：
// 四元数 q = [w, x, y, z]
roll = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y));
pitch = asin(2*(w*y - z*x));
yaw = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z));注意，当pitch接近±90度时，asin的参数可能超出[-1,1]范围。我建议加个钳位处理，防止NaN。
好了，这一章的内容就这些。坐标系、欧拉角、旋转矩阵、四元数，它们是传感器融合的“语言”。你掌握了它们，后面学卡尔曼滤波、姿态解算就会轻松很多。下一章，咱们聊聊加速度计和陀螺仪的数据怎么处理——那又是另一番天地了。