1. 导航基础与坐标系：地球形状与重力场模型、常用坐标系定义、坐标系转换方法

各位同学，欢迎来到《从零搭建INS/GPS组合导航系统》的第一章。

说实话，我刚开始接触组合导航那会儿，最头疼的就是坐标系。明明都是“位置”，换个坐标系就完全对不上了。后来踩了不少坑才明白——坐标系，是整个导航系统的“语言”。语言不通，系统就崩。

这一章，我们就来把“语言”捋顺。

1.1 地球形状与重力场模型

导航嘛，首先得知道我们在哪个星球上。地球不是完美的球体，这大家都知道。但具体怎么描述它？

地球形状，说白了就是个“扁球”。赤道半径约6378km，极半径约6357km，差了21公里。我当年第一次用WGS-84模型算位置时，心想这21公里能差多少？结果在长航时惯导里，误差会慢慢累积，最后定位偏出去几百米都不奇怪。

常用的地球模型有三个：

• WGS-84：GPS用的标准模型，全球通用。我建议你直接拿它当默认选项。
• CGCS2000：中国大地坐标系，国内项目必须用。
• PZ-90：GLONASS用的，俄罗斯标准。

重力场模型呢？简单说，就是地球不同地方的重力加速度不一样。为什么？因为地球密度不均匀，地形也不平。我做过一个项目，在高原地区跑惯导，直接用标准重力公式算，结果垂直通道发散得厉害。后来换了EGM2008模型，才稳住。

常用的重力公式（WGS-84）：

g(φ) = 9.7803253359 * (1 + 0.00193185265241 * sin²(φ)) / sqrt(1 - 0.00669437999014 * sin²(φ))

嗯，公式看着长，但代码里就一行。我习惯把它封装成一个函数，到处调用。

1.2 常用坐标系定义

搞导航，你绕不开这三个坐标系。我按使用频率排个序：

1.2.1 ECEF（地心地固坐标系）

ECEF，全称Earth-Centered Earth-Fixed。原点在地心，Z轴指向北极，X轴指向本初子午线与赤道交点，Y轴按右手定则补齐。

这个坐标系最大的特点：它跟着地球转。GPS卫星发出来的位置，就是ECEF坐标。你想想看，卫星在太空飞，地球在转，但ECEF坐标下卫星的位置是固定的——因为坐标系本身也在转。

我在做GPS数据解析时，第一步就是把卫星星历里的参数转成ECEF坐标。这一步错了，后面全白搭。

1.2.2 ENU（东北天坐标系）

ENU，东-北-天。原点在载体所在位置，X轴指向东，Y轴指向北，Z轴指向天顶。

这个坐标系最直观。你站在操场上，东在哪？北在哪？头顶在哪？就是ENU。我习惯在组合导航的误差状态方程里用ENU，因为它的物理意义清晰，调试起来方便。

1.2.3 NED（北东地坐标系）

NED，北-东-地。和ENU的区别？说白了就是Z轴方向反了。NED的Z轴指向地心，也就是“向下”。

为什么会有NED？因为飞机、导弹这些飞行器，高度往下掉是“负的”，用NED更符合直觉。我做过无人机项目，飞控里清一色用NED。你要是做车载导航，ENU更常见。

1.3 坐标系转换方法

坐标系转换，是组合导航的“基本功”。我见过不少新手，公式背得滚瓜烂熟，一写代码就出bug。为什么？因为符号搞反了，或者旋转顺序错了。

1.3.1 旋转矩阵

坐标系转换的核心，就是旋转矩阵。三维空间里的旋转，可以分解成绕三个轴的旋转：

• 绕Z轴旋转：航向角（yaw）
• 绕Y轴旋转：俯仰角（pitch）
• 绕X轴旋转：横滚角（roll）

旋转矩阵长这样（绕Z轴为例）：

R_z(ψ) = [[cosψ, -sinψ, 0],
          [sinψ,  cosψ, 0],
          [0,      0,    1]]

我曾经犯过一个低级错误：把旋转顺序搞反了。在组合导航里，旋转顺序通常是Z-Y-X（航向-俯仰-横滚）。你要是用了X-Y-Z，结果就是天差地别。嗯，那次我debug了整整两天。

1.3.2 四元数

旋转矩阵有个问题：万向锁。当俯仰角接近±90°时，航向和横滚会耦合，解算就炸了。

四元数就是来救场的。它用四个数表示旋转，没有奇点。我现在的项目，只要涉及姿态解算，一律用四元数。旋转矩阵只在需要可视化或者输出给其他模块时，才转换一下。

四元数转旋转矩阵的代码：

def quat_to_rotmat(q):
    # q = [qw, qx, qy, qz]
    qw, qx, qy, qz = q
    R = [[1-2*(qy**2+qz**2), 2*(qx*qy-qw*qz),   2*(qx*qz+qw*qy)],
         [2*(qx*qy+qw*qz),   1-2*(qx**2+qz**2), 2*(qy*qz-qw*qx)],
         [2*(qx*qz-qw*qy),   2*(qy*qz+qw*qx),   1-2*(qx**2+qy**2)]]
    return R

1.3.3 常用转换流程

实际项目中，最常见的转换路径是：

1. GPS输出（ECEF）→ 导航坐标系（ENU/NED）
2. IMU输出（载体坐标系）→ 导航坐标系
3. 导航坐标系 → 当地水平坐标系（用于显示）

我一般这样处理：先把GPS的ECEF坐标转成经纬高（LLA），再转成ENU。这样每一步都清晰，出了问题也好定位。

1.4 本章小结

这一章，我们聊了地球形状、重力场、三个常用坐标系，以及它们之间的转换方法。说白了，就是给导航系统搭好“地基”。

下一章，我们会进入惯性导航的核心——IMU测量模型与误差分析。到时候你会发现，坐标系转换只是热身，真正的挑战还在后面。

好，今天就到这里。有问题随时交流。

---

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