2、姿态解算中的坐标系：定义导航坐标系、载体坐标系，讲解坐标系转换与初始对准
各位同学，咱们今天聊点硬核的——坐标系。说实话，我见过太多新手在姿态解算上栽跟头，十有八九都是坐标系没搞明白。你想想看，连自己站在哪儿、往哪儿看都没搞清楚，算出来的姿态能准吗？
我个人习惯，做任何惯性导航项目之前，第一件事就是把坐标系钉死。这就像盖房子打地基，地基歪了，后面全白搭。
2.1 导航坐标系（n系）—— 你的绝对参考系
导航坐标系，也叫n系。说白了，就是你在三维空间里找方向的“绝对标尺”。
最常见的导航坐标系是 北-东-地（NED） 坐标系：
- X轴：指向地理北向
- Y轴：指向地理东向
- Z轴：指向地心（垂直向下）
为什么用NED？我在做无人机项目时深有体会——飞机的高度变化、水平移动，用NED描述最直观。你想想看，飞行员说“爬升”，对应的是Z轴负方向（因为Z轴向下），虽然反直觉，但这是行业标准。
重要提醒： 导航坐标系是惯性导航的“锚点”。所有载体姿态、速度、位置的最终输出，都要转换到n系下才有意义。
还有一种叫 东-北-天（ENU） 坐标系，常用于机器人领域。我建议你根据项目场景选，但一旦选定，就别来回切换。
2.2 载体坐标系（b系）—— 传感器自己的小世界
载体坐标系，也叫b系。它固定在载体（比如你的IMU模块、无人机、汽车）上，随载体一起运动。
标准定义是这样的：
- X轴：指向载体前进方向（右）
- Y轴：指向载体左侧（或右侧，看具体定义）
- Z轴：垂直于载体平面向上
嗯，这里要注意：不同厂家的IMU，b系定义可能不一样。我曾经踩过一个坑——某款工业级IMU的Z轴是向下的，而我默认用了向上的定义，结果积分出来的姿态直接反了。排查了整整两天才找到原因。
我的经验： 拿到新IMU的第一件事，用手册确认b系定义。然后写一段简单的测试代码，把传感器静止放平，看三轴输出是否与预期一致。这一步花10分钟，能省你后面10小时。
2.3 坐标系转换 —— 从b系到n系的“翻译官”
传感器测到的数据（加速度、角速度）是在b系下的。但我们要的是导航坐标系下的姿态。怎么办？做转换。
这个转换靠的是 旋转矩阵，也叫方向余弦矩阵（DCM）。
假设载体绕Z轴旋转了角度 ψ（偏航角），绕Y轴旋转了 θ（俯仰角），绕X轴旋转了 φ（横滚角）。那么从b系到n系的旋转矩阵 R 是这样的：
R = Rz(ψ) * Ry(θ) * Rx(φ)
其中：
Rz(ψ) = [cosψ -sinψ 0]
[sinψ cosψ 0]
[0 0 1]
Ry(θ) = [cosθ 0 sinθ]
[0 1 0]
[-sinθ 0 cosθ]
Rx(φ) = [1 0 0]
[0 cosφ -sinφ]
[0 sinφ cosφ]看着复杂？其实你只需要记住：旋转顺序很重要。我习惯用 Z-Y-X 顺序（先偏航，再俯仰，最后横滚）。顺序搞反了，姿态就全乱了。
避坑指南： 我曾经在项目中用了不同的旋转顺序（X-Y-Z），结果和同事的算法对不上。后来我们统一了标准，才把问题解决。记住：团队协作时，旋转顺序必须写在文档第一页。
2.4 初始对准 —— 让坐标系“对齐”的第一步
初始对准，说白了就是找到b系和n系之间的初始旋转关系。没有这一步，后面的积分全是空中楼阁。
常用的初始对准方法有两种：
- 静态对准：载体静止不动，利用重力加速度和地磁场确定初始姿态。
- 动态对准：载体运动时，利用GPS或其他外部信息辅助对准。
我重点讲讲静态对准，因为这是最常用的。
静态对准的原理很简单：
- 加速度计测到重力矢量（在b系下），而我们知道重力在n系下是 [0, 0, g]（NED系）。
- 磁力计测到地磁场矢量（在b系下），而地磁场在n系下是已知的（查表可得）。
- 利用这两个矢量，就能解出初始的横滚角 φ 和俯仰角 θ，以及偏航角 ψ。
公式如下：
// 从加速度计计算横滚和俯仰
roll = atan2(ay, az)
pitch = atan2(-ax, sqrt(ay*ay + az*az))
// 从磁力计计算偏航（需要先补偿横滚和俯仰）
// 这里假设磁力计数据已经校准过
mag_x_comp = mx * cos(pitch) + my * sin(pitch) * sin(roll) + mz * sin(pitch) * cos(roll)
mag_y_comp = my * cos(roll) - mz * sin(roll)
yaw = atan2(-mag_y_comp, mag_x_comp)关键点： 初始对准的精度直接影响后续积分效果。我建议你在对准时采集多帧数据取平均，别用单帧。另外，对准过程中载体必须保持静止，任何微小的振动都会引入误差。
2.5 知识体系总览
为了让你更直观地理解本章的知识结构，我画了一张图：
2.6 本章小结
好了，咱们把这一章的核心捋一捋：
- 导航坐标系（n系）：你的绝对参考系，常用NED或ENU。
- 载体坐标系（b系）：传感器自己的坐标系，定义要确认清楚。
- 坐标系转换：用旋转矩阵把b系数据转到n系，旋转顺序别搞错。
- 初始对准：利用重力和地磁场，确定初始姿态。静止、多帧平均是关键。
我个人觉得，坐标系这部分虽然基础，但怎么强调都不过分。你想想看，如果连坐标系都没对齐，后面那些卡尔曼滤波、积分补偿，全都是在错误的基础上做文章，结果可想而知。
下一章咱们会深入讲四元数——一种比旋转矩阵更优雅的姿态表示方法。到时候你会发现，坐标系转换可以变得更简单、更高效。