2. 坐标系与时间系统：常用坐标系、时间基准与转换陷阱
各位同学，咱们今天聊点实在的。坐标系和时间系统，听起来像是教科书里翻两页就过去的东西。但我跟你说，导航融合里百分之八十的“灵异事件”，最后查下来都是坐标系搞错了，或者时间没对齐。我刚开始做组合导航那会儿，就因为这个栽过跟头，数据融合出来轨迹在天上飞，找了三天bug，结果是时间戳差了18秒。
好，咱们一个一个来拆。
2.1 常用坐标系：你手里拿的尺子不一样
做导航，你手里得有几把“尺子”。不同尺子量出来的数，不能直接加减。我习惯把坐标系分成三类：全局的、局部的、跟着载体跑的。
2.1.1 WGS84 坐标系
这是GPS用的坐标系。说白了，就是用一个椭球体来近似地球。你手机里拿到的经纬高（Lat, Lon, Alt），就是WGS84下的坐标。
这里有个坑：WGS84的椭球参数是固定的，但不同国家、不同时期用的椭球可能不一样。比如咱们国家以前用北京54、西安80，参数跟WGS84有偏差。你如果拿WGS84的GPS数据，直接往北京54的地图上怼，位置能偏出去几百米。
2.1.2 ENU 坐标系（东北天）
ENU是局部坐标系。原点定在某个点，东为X，北为Y，天为Z。这个坐标系的好处是直观，你开车时“向东10米”比“经度增加0.0001度”好理解得多。
ENU常用于短距离导航，比如无人机起飞点附近。但注意，ENU是平面近似，距离原点越远，误差越大。你想想看，地球是圆的，你拿平面坐标去算几百公里外的点，那肯定不准。
2.1.3 载体坐标系（Body Frame）
这个坐标系是跟着设备走的。一般定义：X轴朝前（车头方向），Y轴朝右，Z轴朝下（或者朝上，看具体定义）。IMU（惯性测量单元）输出的加速度和角速度，就是在载体坐标系下的。
嗯，这里要注意：不同厂家对载体坐标系的定义可能不一样。有的Z轴朝上，有的朝下。你融合数据前，必须确认清楚。我遇到过最坑的一次，是IMU的X轴定义成了车尾方向，结果航向角直接反了180度。
| 坐标系 | 特点 | 典型用途 |
|---|---|---|
| WGS84 | 全球统一，椭球模型 | GPS输出、卫星轨道 |
| ENU | 局部平面，直观 | 短距离导航、视觉SLAM |
| 载体坐标系 | 随载体运动 | IMU原始数据、控制指令 |
2.2 时间基准：对不上时间，一切都是白搭
坐标系是空间上的对齐，时间系统是时间上的对齐。两者缺一不可。
2.2.1 UTC 与 GPS 时
UTC（协调世界时）是我们日常用的时间，有闰秒调整。GPS时是连续的时间系统，没有闰秒。GPS时从1980年1月6日0点开始，到现在已经比UTC快了18秒（2024年数据）。
为什么会有这个差异？因为地球自转在变慢，UTC需要加闰秒来保持跟天文时间同步。GPS系统为了简化，干脆不加闰秒。
你融合GPS和IMU数据时，必须统一时间基准。我个人的习惯是：所有传感器数据都转成GPS时，或者都转成UTC，但一定要统一。我曾经见过一个项目，GPS给的时间是GPS时，IMU用的是UTC，融合结果一直在漂，查了整整两天。
2.2.2 时间戳的对齐
多传感器融合时，每个传感器都有自己的时钟。GPS的时钟精度高（纳秒级），IMU的时钟精度一般（毫秒级），相机的时钟可能更差。你需要做时间同步。
常用的方法有两种：
- 硬件同步： 用GPS的PPS（秒脉冲）信号给所有传感器授时。这是最准的，但需要硬件支持。
- 软件同步： 用插值或滤波的方式，把不同频率的数据对齐到同一个时间轴上。比如IMU是100Hz，GPS是10Hz，你可以用IMU的时间戳去插值GPS数据。
软件同步有个陷阱：如果你用线性插值，假设了运动是匀速的。但实际车辆加减速时，这个假设不成立。我建议用更高阶的插值，或者干脆用卡尔曼滤波的时间更新来处理。
2.3 坐标转换中的常见陷阱
好，重头戏来了。坐标转换看着简单，不就是乘个旋转矩阵吗？但实际工程里，坑多到你怀疑人生。
2.3.1 旋转顺序与方向
三维旋转不满足交换律。先绕X轴转30度再绕Y轴转60度，跟先Y后X，结果完全不同。你必须明确旋转顺序。常用的有：
- ZYX顺序（航向-俯仰-横滚）：用于航空领域
- ZXY顺序：用于一些机器人系统
另外，旋转方向也有正负之分。右手定则下，绕轴逆时针为正。但有些库（比如某些早期版本的Eigen）用的是左手系。你转换前，一定要看文档。
2.3.2 四元数的奇异性
四元数没有万向锁问题，但它有符号歧义。q和-q代表同一个旋转。你在做插值或滤波时，如果不处理这个歧义，会导致跳变。
我建议：在卡尔曼滤波的状态向量里，始终保证四元数的标量部分为正。或者用更鲁棒的姿态表示方法，比如旋转矢量。
2.3.3 地心地固系（ECEF）与经纬高的转换
这个转换有标准公式，但要注意：
- 高程是椭球高，不是海拔高。大地水准面跟椭球面有差异，最大能差100米。你如果拿GPS的椭球高去跟气压计的海拔高比较，那肯定对不上。
- 纬度有大地纬度和地心纬度之分。WGS84用的是大地纬度。有些算法库默认用地心纬度，你不转换就错了。
// 一个常见的 WGS84 转 ECEF 代码片段（C++）
// 注意：这里的 lat, lon 是弧度，h 是椭球高
double a = 6378137.0; // WGS84 长半轴
double f = 1.0 / 298.257223563; // 扁率
double e2 = 2*f - f*f; // 第一偏心率平方
double sinLat = sin(lat);
double cosLat = cos(lat);
double sinLon = sin(lon);
double cosLon = cos(lon);
double N = a / sqrt(1 - e2 * sinLat * sinLat);
double x = (N + h) * cosLat * cosLon;
double y = (N + h) * cosLat * sinLon;
double z = (N * (1 - e2) + h) * sinLat;
2.3.4 载体坐标系到ENU的转换
这个转换需要知道载体的姿态（航向、俯仰、横滚）。但有个常见错误：IMU输出的姿态是相对于导航坐标系（通常是ENU）的，而GPS输出的航向是相对于真北的。磁力计输出的是磁北。这三者之间有个磁偏角。
我曾经在项目里，直接用磁力计的航向角去做坐标转换，结果车辆在直道上跑出了S形。后来发现是没做磁偏角校正。不同地区的磁偏角能差十几度，你想想看，这能不出问题吗？
好了，坐标系和时间系统这块，咱们就聊到这儿。记住一句话：先对齐空间，再对齐时间，最后才谈融合。 顺序搞反了，后面全是白干。