2. 坐标系与姿态表示：地球坐标系、导航坐标系、载体坐标系、欧拉角、四元数、旋转矩阵及其转换关系

做组合导航这些年，我最大的体会就是：坐标系搞不清楚，后面全是坑。你想想看，IMU 测的是载体自身的加速度和角速度，GNSS 给的是地球上的经纬高，这两者要融合，首先得让它们「说同一种语言」。这个语言，就是坐标系和姿态表示。

这一章，咱们就把这些基础概念掰开揉碎。我会结合自己踩过的坑，帮你把坐标系和姿态这块彻底打通。

2.1 三大坐标系：你总得知道自己在哪

组合导航里常用的坐标系有三个。我个人习惯把它们分成「地球的」、「导航用的」和「载体自己的」。

2.1.1 地球坐标系（ECEF）

这个坐标系的原点在地球质心。Z 轴指向北极，X 轴指向本初子午线与赤道的交点，Y 轴按右手定则确定。说白了，它是一个随地球自转的坐标系。

GNSS 解算出来的位置，原始输出就是 ECEF 下的 XYZ 坐标。但咱们平时用经纬高更习惯，所以需要转换。嗯，这里要注意：ECEF 到经纬高的转换有迭代算法，我见过有人直接用近似公式，结果在高纬度地区误差很大。

2.1.2 导航坐标系（NED / ENU）

导航坐标系是当地水平的。最常用的是北东地（NED）和东北天（ENU）。

我个人更习惯用 NED，因为无人机和机器人的控制通常基于 NED。原点就在载体当前位置，X 轴指向北，Y 轴指向东，Z 轴指向地心（向下）。

为什么需要这个系？因为 IMU 测的是加速度，但加速度包含重力。在导航系下，我们可以方便地把重力分量扣除掉。我在项目中遇到过，有人直接在载体系下减重力，结果姿态一偏，重力就扣不干净，速度发散得飞快。

2.1.3 载体坐标系（Body Frame）

载体坐标系是固定在载体上的。通常定义：X 轴指向载体前进方向（前），Y 轴指向右侧（右），Z 轴指向下方（下）。IMU 的三个轴就沿着这个坐标系安装。

这里有个坑：IMU 的安装方向不一定和载体坐标系完全对齐。我做过一个项目，IMU 在电路板上转了 45 度安装，结果没做标定，导航解算出来的航向直接偏了 45 度。后来花了半天才排查出来。

2.2 姿态表示：三种方式，各有千秋

姿态就是载体坐标系相对于导航坐标系的旋转关系。常用的表示方法有三种：欧拉角、旋转矩阵、四元数。

2.2.1 欧拉角

欧拉角最直观：横滚（Roll）、俯仰（Pitch）、航向（Yaw）。分别绕 X、Y、Z 轴旋转。

但欧拉角有个致命问题——万向锁。当俯仰角接近 ±90° 时，横滚和航向会耦合，丢失一个自由度。我在做飞行器控制时遇到过，飞机垂直爬升时，航向突然跳变，差点失控。

所以，欧拉角适合人机交互显示，不适合内部解算。

2.2.2 旋转矩阵

旋转矩阵是一个 3x3 的正交矩阵。它没有万向锁问题，但参数多（9 个），而且必须保持正交性。在滤波更新时，如果直接加噪声，矩阵很容易不再正交，需要不断重新正交化。

我记得早期做捷联惯导时，用的就是旋转矩阵。每次更新完都要做 Gram-Schmidt 正交化，代码写起来挺麻烦的。

2.2.3 四元数

四元数是我现在最常用的姿态表示方式。它只有 4 个参数，没有万向锁，更新只需要简单的四元数乘法，而且很容易归一化。

四元数 q = [w, x, y, z]^T，其中 w 是标量部分，[x, y, z] 是矢量部分。它表示绕某个轴旋转某个角度。

2.3 转换关系：怎么从 A 到 B

这三种表示方式可以互相转换。下面给出最常用的转换公式。

2.3.1 四元数 → 旋转矩阵

// 给定四元数 q = [w, x, y, z]
// 对应的旋转矩阵 R 为：
R = [
  [1 - 2*y*y - 2*z*z,   2*x*y - 2*w*z,       2*x*z + 2*w*y],
  [2*x*y + 2*w*z,       1 - 2*x*x - 2*z*z,   2*y*z - 2*w*x],
  [2*x*z - 2*w*y,       2*y*z + 2*w*x,       1 - 2*x*x - 2*y*y]
]

2.3.2 旋转矩阵 → 欧拉角（NED 系，ZYX 顺序）

// 从旋转矩阵 R 提取欧拉角
pitch = asin(-R[2][0])          // 俯仰
roll  = atan2(R[2][1], R[2][2]) // 横滚
yaw   = atan2(R[1][0], R[0][0]) // 航向

注意：当 pitch 接近 ±90° 时，roll 和 yaw 的计算会不稳定。这就是万向锁的数学表现。

2.3.3 四元数 → 欧拉角

// 四元数 q = [w, x, y, z] 转欧拉角（ZYX 顺序）
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))

2.4 知识体系总览

下面这张图，是我自己梳理的坐标系与姿态表示的知识结构。你可以把它当作本章的「地图」。

2.5 实战中的选择建议

说了这么多理论，到底怎么选？我给出几条实际建议：

• IMU 数据预处理：在载体系下做，因为 IMU 原始输出就在这个系
• 姿态更新：用四元数，用陀螺仪的角速度做四元数微分方程更新
• 重力补偿和速度积分：在导航系下做，先把加速度从载体系转到导航系，再减去重力
• GNSS 位置融合：在导航系下做，把 ECEF 转成 NED 或 ENU

嗯，这一章的内容就到这里。坐标系和姿态表示是组合导航的基石，看似简单，但每个细节都值得深究。我当年刚入行时，就是在这上面栽了不少跟头。希望你能少走弯路。

---

公众号:蓝海资料掘金营，微信deep3321