坐标系基础：ECEF、LLA、导航系、载体系与坐标转换
各位同学，今天我们来聊聊组合导航里最绕不开的一个话题——坐标系。说实话，我刚开始做组合导航那会儿，被坐标系绕得晕头转向。有一次在跑紧耦合算法，结果定位结果飞到了大西洋中间，查了半天才发现是坐标系转换搞反了。嗯，从那以后，我对坐标系就格外上心。
你想想看，GNSS卫星在天上飞，给的是地心地固坐标；IMU装在车上，测的是载体坐标；而我们导航时想要的是经纬高或者东北天坐标。这几个坐标系如果不理清楚，算法写得再漂亮也是白搭。
核心观点：坐标系转换是组合导航的"语言翻译器"。搞不定坐标系，就别谈紧耦合。
1. 地心地固坐标系（ECEF）
ECEF，全称Earth-Centered Earth-Fixed。说白了，就是一个原点在地球质心，跟着地球一起转的坐标系。它的Z轴指向北极，X轴指向本初子午线与赤道的交点，Y轴按右手定则确定。
我个人习惯把ECEF想象成一个"地球的骨架"。卫星的位置、速度，通常都是用ECEF坐标给出的。为什么？因为卫星绕地球转，用ECEF描述最直观。
| 轴 | 指向 | 说明 |
|---|---|---|
| X轴 | 本初子午线与赤道交点 | 随地球自转 |
| Y轴 | 东经90°方向 | 右手定则确定 |
| Z轴 | 指向北极 | 与地球自转轴重合 |
实战经验：我在做车载导航时，发现很多同学把ECEF和地心惯性系搞混。记住：ECEF是跟着地球转的，惯性系是不转的。这个区别在长时间导航时特别重要。
2. 地理坐标系（LLA）
LLA就是纬度、经度、高度。这是人类最熟悉的坐标系，也是GNSS输出的标准格式。但要注意，这里的纬度是大地纬度，不是地心纬度。
为什么会有两种纬度？因为地球不是完美的球体，而是一个椭球。我常用的参考椭球是WGS-84，也就是GPS用的那个。大地纬度是椭球面上某点的法线与赤道面的夹角，而地心纬度是该点与地心连线与赤道面的夹角。两者差个零点几度，但在高精度导航里不能忽略。
注意：高度是椭球高，不是海拔高。椭球高和海拔高之间有个大地水准面差距，一般在几米到几十米。如果你用RTK做厘米级定位，这个差距必须补偿。
3. 导航坐标系（n系）
导航坐标系，通常指东北天（ENU）或者北东地（NED）。我习惯用东北天，因为直观。n系的原点就在载体所在位置，X轴指向东，Y轴指向北，Z轴指向天。
你可能会问：为什么不用ECEF直接导航？因为ECEF的数值太大，而且变化不直观。东北天坐标系下，东向速度、北向速度、天向速度，一看就知道车往哪开。
我在做紧耦合时，惯导的力学编排就是在n系下完成的。IMU输出的加速度和角速度，先转到n系，然后积分得到位置、速度、姿态。这个过程中，坐标系转换的精度直接影响导航结果。
4. 载体坐标系（b系）
载体坐标系是固定在载体上的。对于车辆来说，通常X轴指向车头，Y轴指向右侧，Z轴指向车顶。IMU就安装在载体上，它输出的原始数据就是b系下的。
这里有个坑：IMU的安装方向不一定和载体坐标系对齐。我遇到过一台设备，IMU装歪了5度，结果导航结果偏得离谱。后来加了标定参数才搞定。
关键点：b系到n系的转换，就是姿态矩阵。这个矩阵由三个欧拉角（横滚、俯仰、航向）决定。紧耦合的核心，就是不断修正这个姿态矩阵。
5. 坐标系转换
坐标系转换是本章的重头戏。我把它分成三块来说：
5.1 ECEF ↔ LLA
这个转换有标准公式。从LLA到ECEF：
// LLA to ECEF
double a = 6378137.0; // WGS-84长半轴
double e2 = 0.00669437999014; // 第一偏心率平方
double sinLat = sin(lat);
double cosLat = cos(lat);
double N = a / sqrt(1 - e2 * sinLat * sinLat);
double x = (N + h) * cosLat * cos(lon);
double y = (N + h) * cosLat * sin(lon);
double z = (N * (1 - e2) + h) * sinLat;
从ECEF到LLA稍微复杂点，需要迭代计算纬度。不过现在有直接公式，精度也够用。
5.2 ECEF ↔ n系
这个转换需要知道当前位置的经纬度。转换矩阵是：
// ECEF to n系 (ENU) 转换矩阵
// lat, lon 是当前位置的纬度和经度
double sinLat = sin(lat);
double cosLat = cos(lat);
double sinLon = sin(lon);
double cosLon = cos(lon);
double R[3][3] = {
{-sinLon, cosLon, 0},
{-sinLat*cosLon, -sinLat*sinLon, cosLat},
{ cosLat*cosLon, cosLat*sinLon, sinLat}
};
这个矩阵我背了很久才记住。后来发现规律：第一行是东向，第二行是北向，第三行是天向。这样就好记多了。
5.3 b系 ↔ n系
这就是姿态矩阵，由三个欧拉角决定。我习惯用ZYX顺序：先航向，再俯仰，最后横滚。
// 欧拉角到姿态矩阵 (ZYX顺序)
// yaw, pitch, roll 单位是弧度
double cy = cos(yaw);
double sy = sin(yaw);
double cp = cos(pitch);
double sp = sin(pitch);
double cr = cos(roll);
double sr = sin(roll);
double C[3][3] = {
{cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr},
{sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr},
{-sp, cp*sr, cp*cr}
};
我的习惯：在代码里，我通常用四元数代替欧拉角，避免万向锁问题。但调试时还是看欧拉角直观。所以我会在代码里同时维护四元数和欧拉角，一个用于计算，一个用于显示。
6. 知识体系总览
下面这张图，是我自己总结的坐标系转换关系。每次做项目前，我都会看一眼，确保没搞混。
曾经踩过的坑：有一次我在做GPS+IMU紧耦合，GPS输出的是ECEF坐标，IMU输出的是b系数据。我忘了把GPS转到n系，直接和IMU的n系数据融合。结果定位结果一直在漂。查了两天才发现是坐标系没统一。从那以后，我每次做融合前都会检查：所有数据在哪个坐标系下？
7. 实战建议
最后，给各位几个实战建议：
- 统一坐标系：所有传感器数据，在进入滤波器之前，先转到同一个坐标系。我习惯用n系，因为直观。
- 注意精度：坐标系转换涉及三角函数，浮点精度很重要。我一般用double，不用float。
- 写测试用例：坐标系转换最容易出错。我会写一个测试：已知一个点的LLA，转到ECEF，再转回LLA，看误差是否在1e-6以内。
- 画图辅助：调试时，把各个坐标系下的轨迹画出来。一眼就能看出转换有没有问题。
我的小技巧：在代码里，我会给每个变量加后缀，比如pos_ecef、pos_lla、pos_n。这样一眼就知道数据在哪个坐标系下，避免搞混。
好了，坐标系基础就讲到这里。这些内容看起来简单，但实际项目中出问题最多的就是坐标系。希望大家多动手写代码，多画图，把坐标系转换练成肌肉记忆。