坐标系与时空基准：WGS84坐标系、ENU坐标系、载体坐标系、时间同步基础
做融合定位这些年，我踩过最大的坑，往往不是算法本身，而是坐标系搞混了。
你想想看，GPS给的是经纬度，IMU算的是加速度积分，激光雷达扫出来的是相对于车身的点云。这三个数据要是没对齐坐标系，融合出来的结果就是一团浆糊。我刚开始带项目时，就因为在ENU和WGS84之间少转了一步，定位结果直接漂了十几米，排查了整整两天。
所以这一章，咱们把坐标系和时空基准彻底聊透。
1. WGS84坐标系：全球通用的“大地尺子”
WGS84，全称World Geodetic System 1984。说白了，就是一套定义地球形状和位置的全球标准。GPS卫星发下来的原始数据，用的就是WGS84坐标系。
它的核心参数有三个：
- 椭球体模型：地球不是完美的球体，WGS84用一个旋转椭球来近似。长半轴a = 6378137.0米，扁率f = 1/298.257223563。
- 坐标表达：经度(longitude)、纬度(latitude)、高度(height)。注意，高度是相对于椭球面的，不是海拔。
- 地心地固坐标系(ECEF)：WGS84还有一种直角坐标形式，原点在地心，X轴指向本初子午线与赤道交点，Z轴指向北极。GPS解算出来的位置，其实先算的是ECEF，再转成经纬高。
重要概念：WGS84的高度是椭球高，而地图上标注的海拔是大地水准面高。两者差一个“大地水准面差距”，不同地区能差几十米。做高精度定位时，这个修正必须做。
我在项目中遇到过一件事：某次测试，RTK给出的高程和气压计算出来的高度差了15米。查了半天才发现，RTK输出的是WGS84椭球高，而气压计校准用的是当地海拔。嗯，从那以后，我每次做融合前都会先确认：你用的高度是椭球高还是海拔高？
2. ENU坐标系：局部导航的“东北天”
WGS84虽然全球通用，但做局部导航时用经纬度算距离，计算量太大，而且不直观。这时候就需要ENU坐标系。
ENU，就是东(East)、北(North)、天(Up)。它是个局部直角坐标系，原点通常选在载体起始位置或某个固定参考点。
- X轴：指向东
- Y轴：指向北
- Z轴：指向天（垂直于椭球面向上）
从WGS84转到ENU，一般分两步：
- 先把经纬高转成ECEF直角坐标
- 再通过旋转矩阵，把ECEF转到以参考点为原点的ENU
// 伪代码示例：WGS84(经纬高) -> ENU
// 输入：lat, lon, alt (WGS84)
// 参考点：ref_lat, ref_lon, ref_alt
// 1. 经纬高 -> ECEF
ecef = geodetic_to_ecef(lat, lon, alt)
// 2. 参考点经纬高 -> ECEF
ref_ecef = geodetic_to_ecef(ref_lat, ref_lon, ref_alt)
// 3. 计算差值
dx = ecef.x - ref_ecef.x
dy = ecef.y - ref_ecef.y
dz = ecef.z - ref_ecef.z
// 4. 旋转矩阵 (根据参考点经纬度构建)
R = build_rotation_matrix(ref_lat, ref_lon)
// 5. 得到ENU坐标
enu = R * [dx, dy, dz]^T
// enu.x = 东向位移, enu.y = 北向位移, enu.z = 天向位移
我的习惯：做融合定位时，我通常把滤波器的状态量定义在ENU坐标系下。这样位置、速度的物理意义清晰，而且状态更新时不用频繁做经纬度计算，效率高很多。
3. 载体坐标系：传感器自己的“小世界”
载体坐标系(Body Frame)，是固定在运动物体上的坐标系。比如车、无人机、机器人，都有自己的载体坐标系。
常见的定义方式：
- X轴：指向载体前进方向（车头、机头）
- Y轴：指向载体右侧
- Z轴：指向载体下方（符合右手定则，向下为正）
为什么需要载体坐标系？因为IMU、轮速计、激光雷达这些传感器，都是安装在载体上的。它们测量的数据，天然就是载体坐标系下的值。
举个例子：IMU输出的加速度，是相对于IMU自身坐标系的。如果IMU安装时歪了5度，那测出来的加速度就带着5度的误差。这时候就需要做“标定”，把IMU坐标系对齐到载体坐标系。
避坑指南：我曾经因为IMU安装角度标定差了0.5度，导致融合后的航向角在10分钟内漂了30度。后来加了一个“在线安装角估计”模块，才彻底解决。所以，传感器安装偏差，能标定就标定，别指望算法自动补偿。
4. 坐标系之间的转换关系
融合定位的核心，就是不停地在这些坐标系之间来回切换。我画了一张图，帮你理清它们的关系：
5. 时间同步基础：所有数据对齐的“命门”
坐标系对齐了，时间没对齐，照样白搭。
你想想看，GPS数据是1秒更新一次，IMU是100Hz，激光雷达是10Hz。如果这些数据的时间戳没对齐，融合出来的位置就是“过去的位置”和“现在的姿态”拼在一起，结果可想而知。
时间同步分两个层面：
5.1 硬件时间同步
- PPS秒脉冲：GPS模块会输出一个PPS(Pulse Per Second)信号，精度在纳秒级。所有传感器通过PPS信号对齐自己的时钟。
- IEEE 1588 (PTP)：精确时间协议，通过以太网同步多个设备的时间，精度可达微秒级。
- NTP：网络时间协议，精度在毫秒级，适合对时间要求不高的场景。
5.2 软件时间同步
- 时间戳插值：当传感器数据到达时，记录系统时间戳。如果两个传感器的时间基准不同，通过线性插值对齐到同一个时间点。
- 时间偏移估计：在滤波器里把时间偏移作为一个状态量来估计。我做过一个项目，IMU和GPS之间有固定的50ms延迟，通过在线估计把这个延迟补偿掉，定位精度提升了30%。
我的建议：做融合定位系统，先把时间同步搞定，再谈算法。我见过太多团队，算法调得天花乱坠，结果发现是时间戳没对齐，白费功夫。先确保所有数据的时间戳误差在1ms以内，再开始写融合代码。
6. 实战中的坐标系与时间同步检查清单
| 检查项 | 常见问题 | 我的解决经验 |
|---|---|---|
| WGS84高度类型 | 椭球高 vs 海拔高混用 | 统一使用椭球高，最后输出时再转换 |
| ENU参考点选择 | 参考点离载体太远，导致数值误差 | 参考点选在起始位置，或每10分钟重置一次 |
| IMU安装角度 | 安装偏差未标定 | 做一次六面标定，或加在线估计 |
| 时间戳基准 | 不同传感器使用不同时钟源 | 统一使用GPS时间作为主时钟 |
| 数据延迟 | 传感器处理延迟导致时间戳滞后 | 记录数据到达时间，减去已知延迟 |
嗯，这一章的内容就到这里。坐标系和时空基准，听起来基础，但确实是融合定位的根基。我见过太多人在这上面栽跟头，包括我自己。希望你能把这些基础打牢，后面的算法调优才能事半功倍。
一句话总结：坐标系不对，定位全废；时间不同步，融合白做。