第一章：导航基础与坐标系

各位同学，欢迎来到《组合导航系统从零搭建实战》的第一课。

做导航，说白了就是回答三个问题：我在哪？我要去哪？怎么去？而回答第一个问题，就必须先搞定坐标系。坐标系选错了，后面算出来的位置全是错的。我当年刚入行时，就因为搞混了导航系和地心地固系，让整个团队多花了三天排查问题。嗯，这种坑，咱们今天一次性填平。

1.1 地球形状与重力场模型

地球不是完美的球体。你想想看，它自转嘛，赤道部分被甩得鼓起来，两极就扁一些。这个形状，我们叫它「旋转椭球体」。

在实际工程中，我们常用 WGS-84 椭球模型。它的参数如下：

参数	数值	说明
长半轴 a	6378137.0 m	赤道半径
短半轴 b	6356752.3142 m	极半径
扁率 f	1/298.257223563	描述椭球扁平程度
第一偏心率 e²	0.00669437999014	用于大地坐标计算

重力场模型呢？它描述的是地球引力随位置的变化。我建议初学者先记住一个结论：重力加速度 g 大约为 9.78 ~ 9.83 m/s²，随纬度和高度变化。在组合导航中，重力模型误差会直接影响加速度计积分的结果。我曾经在极地测试时，因为用了赤道的重力值，导致垂直通道发散——那教训，够深刻。

1.2 常用坐标系

坐标系是导航的「语言」。不同坐标系有不同的用途，咱们一个一个说。

1.2.1 地心地固系（ECEF）

原点在地球质心，Z 轴指向北极，X 轴指向本初子午线与赤道的交点，Y 轴按右手定则确定。这个坐标系跟着地球一起转。说白了，它适合描述地面固定点的位置。GPS 输出的原始坐标就是 ECEF 下的。

1.2.2 导航坐标系（n系）

也叫当地水平坐标系。原点在载体所在位置，X 轴指向东，Y 轴指向北，Z 轴指向天顶（即 ENU 坐标系）。为什么用这个？因为人在走路、车在跑的时候，我们更关心「向东走了多远、向北走了多远」，而不是「离地心多远」。我个人习惯在导航解算时全部用 n 系，最后再转回 ECEF 输出。

1.2.3 载体坐标系（b系）

原点在载体质心，X 轴指向载体前方，Y 轴指向右侧，Z 轴指向下方（即右前下，RFD）。IMU 传感器输出的加速度和角速度，默认就是在 b 系下的。所以，你拿到 IMU 数据后，第一件事就是把它转到 n 系去。

1.3 坐标系转换方法

坐标系转换，是组合导航的核心基本功。你想想看，IMU 在 b 系下测量，GPS 在 ECEF 下输出，最后要在 n 系下做融合。不会转换，寸步难行。

1.3.1 欧拉角

欧拉角用三个角度描述旋转：横滚角（roll）、俯仰角（pitch）、航向角（yaw）。直观，但有个大坑——万向锁。当俯仰角接近 ±90° 时，横滚和航向会耦合，导致自由度丢失。我在做无人机飞控时，就因为这个原因，飞机在垂直爬升时姿态解算直接炸了。

欧拉角的旋转顺序通常为 Z-Y-X（即先航向、再俯仰、最后横滚）。转换矩阵如下：

// 从 b 系到 n 系的旋转矩阵（欧拉角形式）
// roll = φ, pitch = θ, yaw = ψ
R_b2n = [
  [cosθ*cosψ,  sinφ*sinθ*cosψ - cosφ*sinψ,  cosφ*sinθ*cosψ + sinφ*sinψ],
  [cosθ*sinψ,  sinφ*sinθ*sinψ + cosφ*cosψ,  cosφ*sinθ*sinψ - sinφ*cosψ],
  [-sinθ,      sinφ*cosθ,                    cosφ*cosθ]
];

1.3.2 四元数

四元数用四个数表示旋转：q = [q0, q1, q2, q3]，其中 q0 是标量部分。它没有万向锁问题，计算效率高，适合做姿态递推。我建议所有组合导航算法都用四元数做核心姿态表示。

四元数更新公式（离散形式）：

// 角速度增量 Δθ = ω * dt
// 四元数更新
dq = [cos(|Δθ|/2), (Δθ/|Δθ|) * sin(|Δθ|/2)]
q_new = q_old ⊗ dq  // ⊗ 表示四元数乘法

四元数转方向余弦矩阵也很方便：

R_q = [
  [1-2(q2²+q3²),  2(q1q2 - q0q3),  2(q1q3 + q0q2)],
  [2(q1q2 + q0q3), 1-2(q1²+q3²),   2(q2q3 - q0q1)],
  [2(q1q3 - q0q2), 2(q2q3 + q0q1), 1-2(q1²+q2²)]
];

1.3.3 方向余弦矩阵（DCM）

DCM 是一个 3×3 的正交矩阵，每一列代表一个坐标轴在另一个坐标系中的投影。它是最直观的旋转表示方式，但需要 9 个参数，且必须保持正交性。在长时间积分中，DCM 会因数值误差失去正交性，需要定期重新正交化。

我个人习惯：用四元数做核心运算，需要显示或输出时再转成欧拉角。DCM 只在需要做坐标变换时临时使用。

1.4 知识体系结构图

下面这张图，是我自己梳理的本章知识脉络。你把它存下来，后面学完整个课程再回来看，会更有感觉。

1.5 本章小结

这一章，我们搞定了三件事：地球长什么样、坐标系怎么定义、坐标系之间怎么转。嗯，听起来简单，但这些都是后面所有算法的地基。我见过太多人一上来就调卡尔曼滤波，结果坐标系搞反了，滤波器收敛到错误的位置还浑然不知。

所以，我的建议是：把这一章的代码和公式亲手推一遍，别只看不练。等你把四元数更新写熟了，后面学惯导解算会轻松很多。

---