坐标系基础：地心地固系、地理坐标系、导航坐标系、载体坐标系及其转换

各位同学好，我是你们的组合导航讲师。今天咱们来聊聊坐标系——这玩意儿是IMU和GPS融合的根基。说句实话，我见过不少工程师，算法写得飞起，结果坐标系搞反了，整个系统直接崩掉。嗯，咱们今天就把这几个坐标系彻底捋清楚。

1. 地心地固坐标系（ECEF）

地心地固系，英文叫Earth-Centered Earth-Fixed，简称ECEF。说白了，就是一个原点在地球质心，跟着地球一起转的坐标系。

它的定义很简单：

  • 原点：地球质心
  • X轴：指向本初子午线与赤道的交点
  • Y轴：指向东经90度方向
  • Z轴：指向北极（地球自转轴方向）

为什么叫“地固”？因为它固定在地球上。你站在北京不动，你的ECEF坐标会随着地球自转不断变化——嗯，这听起来有点反直觉，但确实如此。

重要概念：ECEF是右手坐标系，单位是米。GPS卫星的广播星历解算出来的位置，默认就是ECEF坐标。我刚开始做GPS接收机时，拿到原始数据直接当经纬度用，结果画出来的轨迹在天上飞……后来才明白，得先转成LLA。

2. 地理坐标系（LLA）

地理坐标系就是我们常说的经纬度+海拔高度。它用三个参数描述一个点：

  • 纬度（Latitude）：-90°到+90°，北正南负
  • 经度（Longitude）：-180°到+180°，东正西负
  • 高度（Altitude）：相对于参考椭球面的高度，单位米

这里有个坑——高度是相对于椭球面的，不是相对于海平面的。我遇到过有人把气压计测的海拔高度直接当椭球高度用，结果融合出来的位置差了十几米。你想想看，这误差在无人机降落时可是要命的。

避坑指南：我曾经在一个车载导航项目里，发现GPS输出的高度和地图上的海拔对不上。查了半天，原来是GPS用的是WGS84椭球高度，而地图用的是EGM96大地水准面高度。两者差了几十米！解决办法是查表做修正，或者用模型近似。

3. 导航坐标系（NED）

导航坐标系，也叫NED系（North-East-Down）。它是个局部坐标系，原点通常选在载体当前位置。

  • N轴：指向地理北
  • E轴：指向地理东
  • D轴：指向地心（向下）

为什么叫“导航坐标系”？因为在这个坐标系里描述运动最直观。你想啊，说“向北走10米，向东走5米，下降2米”，比说“在ECEF里移动了某个向量”要舒服得多。

我个人习惯在卡尔曼滤波的状态量里用NED坐标。为什么呢？因为NED下的位置和速度是解耦的，协方差矩阵更容易初始化。当然，也有人用ECEF做滤波，各有优劣。

4. 载体坐标系（Body Frame）

载体坐标系是固定在运动物体上的坐标系。对于常见的车辆或无人机，定义如下：

  • X轴（前）：指向载体前进方向
  • Y轴（右）：指向载体右侧
  • Z轴（下）：指向载体下方（满足右手定则）

IMU测量的加速度和角速度，默认就是在载体坐标系下的。所以，你要把IMU数据用到导航里，第一步就是做坐标系转换。

经验之谈：IMU的安装方向一定要标定清楚。我见过一个项目，IMU装反了90度，结果航向角一直偏。后来加了安装角标定，问题才解决。建议大家在硬件设计时就留好安装基准面，省得后面折腾。

5. 坐标系之间的转换

好了，四个坐标系都介绍完了。接下来是重头戏——它们之间怎么转？

5.1 ECEF ↔ LLA

这个转换有标准公式，我直接给代码：

// WGS84椭球参数
const double a = 6378137.0;          // 长半轴
const double f = 1.0 / 298.257223563; // 扁率
const double e2 = 2*f - f*f;         // 第一偏心率平方

// LLA → ECEF
void lla2ecef(double lat, double lon, double alt, 
              double &x, double &y, double &z) {
    double sinLat = sin(lat * M_PI / 180.0);
    double cosLat = cos(lat * M_PI / 180.0);
    double sinLon = sin(lon * M_PI / 180.0);
    double cosLon = cos(lon * M_PI / 180.0);
    
    double N = a / sqrt(1 - e2 * sinLat * sinLat);
    
    x = (N + alt) * cosLat * cosLon;
    y = (N + alt) * cosLat * sinLon;
    z = (N * (1 - e2) + alt) * sinLat;
}

// ECEF → LLA（迭代法）
void ecef2lla(double x, double y, double z,
              double &lat, double &lon, double &alt) {
    lon = atan2(y, x) * 180.0 / M_PI;
    
    double p = sqrt(x*x + y*y);
    double lat_old = atan2(z, p * (1 - e2));
    double N;
    
    for (int i = 0; i < 10; i++) {
        double sinLat = sin(lat_old);
        N = a / sqrt(1 - e2 * sinLat * sinLat);
        double lat_new = atan2(z + e2 * N * sinLat, p);
        if (fabs(lat_new - lat_old) < 1e-12) break;
        lat_old = lat_new;
    }
    
    lat = lat_old * 180.0 / M_PI;
    alt = p / cos(lat_old) - N;
}

注意：ECEF转LLA时，纬度计算需要迭代。虽然也有直接公式，但迭代法更稳定。我实测过，一般3-5次迭代就收敛了，不用担心性能问题。

5.2 ECEF ↔ NED

这个转换需要知道参考点的经纬度。假设参考点坐标为(lat0, lon0, alt0)：

// 计算旋转矩阵
void computeRotationMatrix(double lat0, double lon0, double R[3][3]) {
    double sinLat = sin(lat0 * M_PI / 180.0);
    double cosLat = cos(lat0 * M_PI / 180.0);
    double sinLon = sin(lon0 * M_PI / 180.0);
    double cosLon = cos(lon0 * M_PI / 180.0);
    
    // NED到ECEF的旋转矩阵
    R[0][0] = -sinLat * cosLon;  R[0][1] = -sinLon;  R[0][2] = -cosLat * cosLon;
    R[1][0] = -sinLat * sinLon;  R[1][1] =  cosLon;  R[1][2] = -cosLat * sinLon;
    R[2][0] =  cosLat;           R[2][1] =  0;        R[2][2] = -sinLat;
}

// ECEF → NED
void ecef2ned(double x, double y, double z,
              double x0, double y0, double z0,
              double R[3][3],
              double &n, double &e, double &d) {
    double dx = x - x0;
    double dy = y - y0;
    double dz = z - z0;
    
    n = R[0][0]*dx + R[0][1]*dy + R[0][2]*dz;
    e = R[1][0]*dx + R[1][1]*dy + R[1][2]*dz;
    d = R[2][0]*dx + R[2][1]*dy + R[2][2]*dz;
}

5.3 NED ↔ Body Frame

这个转换靠的是姿态角（横滚roll、俯仰pitch、偏航yaw）。旋转顺序一般是Z-Y-X（偏航→俯仰→横滚）：

// 欧拉角 → 旋转矩阵（NED到Body）
void euler2matrix(double roll, double pitch, double yaw, double C[3][3]) {
    double cr = cos(roll);   double sr = sin(roll);
    double cp = cos(pitch);  double sp = sin(pitch);
    double cy = cos(yaw);    double sy = sin(yaw);
    
    C[0][0] = cp*cy;          C[0][1] = cp*sy;          C[0][2] = -sp;
    C[1][0] = sr*sp*cy-cr*sy; C[1][1] = sr*sp*sy+cr*cy; C[1][2] = sr*cp;
    C[2][0] = cr*sp*cy+sr*sy; C[2][1] = cr*sp*sy-sr*cy; C[2][2] = cr*cp;
}

// 使用：body_vector = C * ned_vector

小技巧：实际工程中，我建议用四元数代替欧拉角做姿态表示。欧拉角有万向锁问题，而且插值不线性。四元数虽然理解起来费劲点，但用起来真香。后面章节我会专门讲四元数。

6. 总结与建议

好了，四个坐标系和它们之间的转换都讲完了。最后给几点实用建议：

  1. 统一基准：整个系统里，所有坐标都用同一个椭球模型。我推荐WGS84，GPS和大部分地图都支持。
  2. 注意精度：单精度float在经纬度转换时可能引入米级误差。用double，别省这点内存。
  3. 做好标定：IMU安装角、杆臂效应（IMU中心与GPS天线中心的偏移）都要标定。我见过最离谱的案例，杆臂没补偿，融合结果差了2米。
  4. 测试验证：写个简单的闭环测试——从LLA转到ECEF再转回LLA，看误差是否在1e-6度以内。这是检验代码正确性的最快方法。

下一章咱们讲IMU的测量模型和误差特性。到时候会用到今天学的坐标系知识，建议大家先把代码跑通，有问题随时交流。

← 上一章 📖 返回目录 下一章 →