2、坐标系与时空基准：常用坐标系与时间同步基础
做多源融合定位，第一道坎就是坐标系。说白了，你得知道每个传感器在说哪国语言，然后才能把它们翻译到一起。我刚开始接触组合导航时，就吃过坐标系没对齐的亏——IMU数据明明很准，但融合结果却飘得离谱，查了半天才发现是ENU和ECEF搞混了。
这一章，咱们就把坐标系和时空基准这摊事理清楚。
2.1 常用坐标系
先说说最常用的几个坐标系。你想想看，一个无人机在天上飞，它关心的是相对于地面的位置（ENU），而卫星导航系统给的是地球上的绝对位置（WGS84），IMU测量的又是相对于自身姿态的加速度和角速度（载体坐标系）。这几个坐标系必须来回切换。
2.1.1 WGS84坐标系
WGS84是个地心地固坐标系，GPS用的就是它。坐标用经纬度和椭球高表示。嗯，这里要注意，WGS84的椭球参数是固定的——长半轴a=6378137米，扁率f=1/298.257223563。
核心公式：WGS84坐标转ECEF直角坐标
// 输入：纬度lat(rad)、经度lon(rad)、高度h(m)
// 输出：ECEF坐标(x, y, z)
double a = 6378137.0; // 长半轴
double f = 1.0 / 298.257223563; // 扁率
double e2 = 2*f - f*f; // 第一偏心率平方
double N = a / sqrt(1 - e2 * sin(lat) * sin(lat));
double x = (N + h) * cos(lat) * cos(lon);
double y = (N + h) * cos(lat) * sin(lon);
double z = (N * (1 - e2) + h) * sin(lat);我在项目中遇到过一个问题：用RTK得到的经纬度高程，直接当成平面坐标去算距离，结果误差大得离谱。原因就是没考虑椭球曲率。记住，WGS84是球面坐标，不是平面坐标。
2.1.2 ECEF坐标系
ECEF（地心地固坐标系）的原点在地球质心，Z轴指向北极，X轴指向本初子午线与赤道的交点。说白了，它就是个三维直角坐标系，方便做向量运算。
我个人习惯把ECEF当作中间坐标系——WGS84转ECEF，ECEF再转ENU，这样逻辑清晰，不容易出错。
2.1.3 ENU坐标系
ENU（东-北-天）是站心坐标系，原点在用户所在位置。东向指正东，北向指正北，天向指垂直于椭球面向上。这个坐标系最直观，因为人感知的就是前后左右上下。
从ECEF到ENU的转换，需要先知道原点的经纬度。公式如下：
// ECEF到ENU转换矩阵
// 原点经纬度：lat0, lon0
// 输入：ECEF坐标(dx, dy, dz) 相对于原点
// 输出：ENU坐标(e, n, u)
double sin_lat = sin(lat0);
double cos_lat = cos(lat0);
double sin_lon = sin(lon0);
double cos_lon = cos(lon0);
double e = -sin_lon * dx + cos_lon * dy;
double n = -sin_lat * cos_lon * dx - sin_lat * sin_lon * dy + cos_lat * dz;
double u = cos_lat * cos_lon * dx + cos_lat * sin_lon * dy + sin_lat * dz;避坑指南：我曾经在车载导航项目里，把ENU的Z轴方向搞反了。天向应该是向上为正，但IMU的Z轴通常是向下为正（重力方向）。如果你不做符号修正，融合出来的高度会一直往下掉。
2.1.4 载体坐标系
载体坐标系（Body Frame）固定在运动载体上。通常定义：X轴指向载体前方，Y轴指向右侧，Z轴指向下方（右-前-下或前-右-上，不同厂家定义不同）。IMU测量的加速度和角速度就是在这个坐标系下的。
从载体坐标系到导航坐标系（比如ENU）的转换，需要用到姿态角（横滚roll、俯仰pitch、航向yaw）。这个转换矩阵就是方向余弦矩阵（DCM），或者用四元数表示。
注意：不同IMU厂商对载体坐标系的定义可能不同。我踩过这个坑——换了一个IMU型号，没仔细看手册，结果姿态解算全反了。拿到新IMU，第一件事就是确认它的坐标系定义。
2.2 坐标转换方法
坐标转换说白了就是三步走：
- 统一到ECEF：把WGS84、ENU等所有坐标都转到ECEF
- 做运算：在ECEF下做向量加减、旋转等操作
- 转回去：把结果转回需要的坐标系
为什么选ECEF作为中间坐标系？因为它是全局统一的直角坐标系，没有奇点。ENU在极点附近会出问题（东向和北向定义模糊），而ECEF不会。
我建议你在代码里封装三个函数：wgs84_to_ecef()、ecef_to_enu()、enu_to_ecef()。这样不管传感器给什么坐标，都能统一处理。
2.3 时间同步基础
坐标系对齐了，时间没对齐，照样白搭。你想想看，如果GPS给的位置是10:00:00.000的，IMU给的数据是10:00:00.100的，融合出来的结果肯定不对。
2.3.1 GNSS时间
GPS时间从1980年1月6日0时开始计数，单位是周和周内秒。它不包含闰秒，所以和UTC会差一个整数秒（目前是18秒）。
北斗时间（BDT）从2006年1月1日0时开始，和UTC差4秒。GLONASS时间则包含闰秒，和UTC一致。
时间转换示例：GPS周内秒转UTC
// GPS周内秒转UTC时间（简化版）
// 注意：需要闰秒表，这里只做示意
double gps_week; // GPS周
double gps_tow; // 周内秒
int leap_seconds = 18; // 当前闰秒数
// GPS时间转UTC
double utc_time = gps_week * 7 * 86400 + gps_tow - leap_seconds;
// 再转成年月日时分秒
// ...2.3.2 UTC时间
UTC是我们日常用的时间，包含闰秒。但GNSS系统一般不用UTC做内部时间基准，因为闰秒会导致时间不连续。你想想看，如果时间突然跳了一秒，导航解算就全乱了。
所以实际项目中，我们通常用GNSS时间作为主时间基准，只在输出给用户时才转成UTC。
2.3.3 硬件时间戳
这是最容易被忽视的环节。传感器数据必须带有精确的时间戳，否则融合算法没法对齐。我见过有人直接用软件打时间戳——就是收到数据时调用gettimeofday()。这在低延迟系统里误差很大，因为数据从传感器到CPU有传输延迟、中断延迟、调度延迟。
正确的做法是用硬件时间戳：
- GPS接收机：PPS（秒脉冲）信号，精度纳秒级
- IMU：用FPGA或MCU的定时器打戳，精度微秒级
- 相机：用外部触发信号同步，精度微秒级
我曾经踩过的坑：在一个无人机项目里，IMU和GPS的时间戳差了50毫秒，但软件里没做任何补偿。结果融合出来的位置在转弯时严重震荡。后来加了硬件同步和软件插值，问题才解决。记住，时间不同步是融合定位的头号杀手。
2.4 知识体系总览
下面这张图总结了本章的核心逻辑：
从图上你能看到，坐标系和时间同步是融合定位的两大基石。坐标系没对齐，数据就是鸡同鸭讲；时间没同步，数据就是错位的历史。这两件事做好了，融合算法才能发挥威力。
我的建议：在实际项目中，先把坐标系转换和时间同步的代码封装成独立模块，做好单元测试。别等到融合出问题了再回头查——那时候你根本分不清是算法问题还是坐标/时间问题。
公众号:蓝海资料掘金营，微信deep3321