第二章：常用坐标系定义与转换
做惯导这么多年，我最大的体会就是：坐标系搞不清楚，后面全白搭。你想想看，飞机在天上飞，你得知道它相对于地球在哪儿、朝哪个方向、姿态怎么样。这些信息，全靠坐标系来定义。
今天咱们就聊聊三个最常用的坐标系：地心地固系、导航系、机体系。我个人习惯把它们叫做“地球的坐标系”、“走路用的坐标系”、“飞机自己的坐标系”。
2.1 地心地固坐标系（ECEF）
地心地固系，英文叫 Earth-Centered Earth-Fixed，简称 ECEF。说白了，就是原点在地心，跟着地球一起转的坐标系。
定义要点：
- 原点 O：地球质心
- X 轴：指向本初子午线与赤道的交点
- Y 轴：指向东经 90° 方向
- Z 轴：指向北极（地球自转轴方向）
这个坐标系最大的特点就是“固连在地球上”。你站在地面上不动，你在 ECEF 系里的坐标是不变的。我在项目中遇到过一个问题：有人用 GPS 输出的经纬高直接当 ECEF 坐标用，结果导航解算全乱了。嗯，这里要注意——GPS 给的是经纬高，不是 ECEF。
核心公式：经纬高 → ECEF
// 已知：纬度 φ，经度 λ，高度 h
// 地球长半轴 a = 6378137.0 m
// 地球扁率 f = 1/298.257223563
double e2 = 2*f - f*f; // 第一偏心率平方
double N = a / sqrt(1 - e2 * sin(φ)*sin(φ));
double x = (N + h) * cos(φ) * cos(λ);
double y = (N + h) * cos(φ) * sin(λ);
double z = (N * (1 - e2) + h) * sin(φ);我的经验：做高精度导航时，千万别用球近似。地球是椭球，用 WGS-84 椭球参数，精度能差出几十米。
2.2 导航坐标系（NED）
导航坐标系，也叫 NED 系——North-East-Down。这个坐标系是“跟着你走的”。
定义：
- 原点：载体所在位置（通常是飞机质心）
- N 轴：指向地理北
- E 轴：指向地理东
- D 轴：指向地心（向下）
为什么叫“导航系”？因为飞行员和导航员习惯用“北-东-地”来思考。你想想看，航向角就是相对于北的，高度就是相对于海平面的。这个坐标系最直观。
我曾经犯过一个低级错误：把 NED 系的 D 轴方向搞反了。结果飞机明明在爬升，高度显示却在下降。排查了三天，最后发现是坐标系符号定义的问题。从那以后，我每次建坐标系都会在代码里写清楚“D 轴向下为正”。
避坑指南：有些文献用 ENU（东-北-天）坐标系，D 轴向上为正。如果你看到别人的代码里高度是负的，别慌，先看看他用的是 NED 还是 ENU。
2.3 机体坐标系（Body Frame）
机体系，就是“绑在飞机身上的坐标系”。
定义：
- 原点：飞机质心
- X 轴：指向机头（前进方向）
- Y 轴：指向右翼
- Z 轴：指向机腹（向下）
这个坐标系对飞控来说太重要了。IMU 测量的角速度和加速度，都是在机体系下的。你想想看，陀螺仪装在飞机上，它测的是飞机绕自身轴的转动，不是绕地球的转动。
我习惯把机体系叫做“传感器坐标系”。因为所有机载传感器——IMU、磁力计、空速管——输出的原始数据都是在这个系下的。
2.4 坐标系转换
三个坐标系之间怎么转？这是惯导的核心问题之一。
2.4.1 ECEF → NED 转换
这个转换需要知道当前位置的经纬度。转换矩阵长这样：
// 从 ECEF 到 NED 的旋转矩阵
// φ: 纬度, λ: 经度
R_ecef_to_ned = [
[-sin(φ)*cos(λ), -sin(φ)*sin(λ), cos(φ)],
[ -sin(λ), cos(λ), 0 ],
[-cos(φ)*cos(λ), -cos(φ)*sin(λ), -sin(φ)]
];2.4.2 NED → 机体 转换（姿态角）
这个转换用三个欧拉角：航向角 ψ、俯仰角 θ、横滚角 φ。转换顺序是：先航向，再俯仰，最后横滚。
旋转矩阵（NED → 机体）：
// 航向 ψ，俯仰 θ，横滚 φ
// 注意：这里用的是 3-2-1 旋转顺序
R_ned_to_body = R_x(φ) * R_y(θ) * R_z(ψ);
// 其中：
R_z(ψ) = [cos(ψ), sin(ψ), 0;
-sin(ψ), cos(ψ), 0;
0, 0, 1];
R_y(θ) = [cos(θ), 0, -sin(θ);
0, 1, 0;
sin(θ), 0, cos(θ)];
R_x(φ) = [1, 0, 0;
0, cos(φ), sin(φ);
0, -sin(φ), cos(φ)];我的建议：实际工程中，别用欧拉角做插值和微分。欧拉角有万向锁问题。我一般用四元数做姿态解算，只在输出给用户时转成欧拉角。
2.5 知识体系总览
下面这张图，是我自己总结的坐标系转换关系。你看一眼就能明白整个逻辑：
2.6 实际工程中的坐标系选择
做惯导系统时，坐标系选对了，能省一半的调试时间。我个人的经验是：
| 应用场景 | 推荐坐标系 | 原因 |
|---|---|---|
| 全球导航 | ECEF | 不需要处理极区奇点 |
| 局部导航（100km以内） | NED | 直观，便于飞行员理解 |
| 姿态控制 | 机体 | 传感器原始数据就在这个系 |
| 组合导航（GPS+IMU） | ECEF + NED 混合 | GPS用ECEF，IMU用NED，中间做转换 |
我曾经踩过的坑：做某型无人机时，用了 NED 系做全球导航。飞到高纬度地区（接近北极），NED 系的北向定义开始发散，导航误差直接爆表。后来老老实实换回 ECEF 系，问题解决。记住：NED 系在极区不好使。
好了，坐标系这块就聊到这儿。说白了就是三件事：知道每个坐标系怎么定义的，知道它们之间怎么转的，知道什么时候该用哪个。搞清楚了这些，后面的惯导解算才能走得顺。