坐标系与时空基准：WGS84、ENU、载体坐标系、时间同步基础
大家好，欢迎来到多传感器融合定位的第一章。
说实话，做融合定位这么多年，我见过太多人一上来就调算法、跑滤波，结果定位结果飘得离谱。最后排查半天，发现是坐标系搞错了，或者时间戳没对齐。嗯，这种坑我踩过不止一次。
所以这一章，咱们先把地基打牢。坐标系和时空基准，说白了就是让所有传感器说同一种语言、对同一个表。这件事做不好，后面再牛的算法也是白搭。
核心观点：多传感器融合的本质，是把不同坐标系下的观测数据，统一到同一个时空参考系下进行估计。坐标系错了，融合就是垃圾进垃圾出。
1. WGS84坐标系：全球定位的通用语言
WGS84，全称World Geodetic System 1984。你手机里的GPS，用的就是它。
它是一个地心地固坐标系（ECEF）。什么意思呢？原点在地球质心，Z轴指向北极，X轴指向本初子午线与赤道的交点，Y轴按右手定则确定。
我们平时说的经纬度、海拔高度，就是WGS84下的地理坐标表示。但注意，GPS直接输出的是大地坐标（纬度B、经度L、高度H），不是ECEF直角坐标。
我的习惯：在融合算法内部，我一般不用经纬高做运算。因为经纬度不是笛卡尔坐标系，算距离、算角度都很麻烦。我会先把GPS的经纬高转成ECEF直角坐标，或者转成局部ENU坐标，再做后续处理。
这里给一个常用的转换公式，把经纬高转成ECEF：
// 经纬高 → ECEF (简化版)
double a = 6378137.0; // 地球长半轴
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;
这段代码我用了很多年。你可能会问，为什么不用现成的库？我的回答是：理解原理比调库更重要。万一哪天库出bug了，你得能自己顶上。
2. ENU坐标系：局部导航的实用选择
ENU，东-北-天坐标系。原点通常设在某个参考点（比如车辆启动位置），X轴指向东，Y轴指向北，Z轴指向天顶。
为什么需要ENU？因为ECEF虽然全球统一，但数值太大，动辄几百万米，计算起来浮点精度容易丢。ENU是局部坐标系，数值小，直观，适合做短距离的导航和融合。
从ECEF转到ENU，需要两步：
- 选定一个参考点（经纬高），算出该点的ECEF坐标作为原点。
- 把其他点的ECEF坐标减去原点坐标，再旋转到ENU方向。
// ECEF → ENU (参考点 lat0, lon0, h0)
// 先算参考点的ECEF坐标 (x0, y0, z0)
// 然后对每个点:
double dx = x - x0;
double dy = y - y0;
double dz = z - z0;
double sinLat0 = sin(lat0);
double cosLat0 = cos(lat0);
double sinLon0 = sin(lon0);
double cosLon0 = cos(lon0);
double east = -sinLon0 * dx + cosLon0 * dy;
double north = -sinLat0 * cosLon0 * dx - sinLat0 * sinLon0 * dy + cosLat0 * dz;
double up = cosLat0 * cosLon0 * dx + cosLat0 * sinLon0 * dy + sinLat0 * dz;
我曾经踩过的坑：参考点选得太远。有一次我把参考点设在起点，结果车子开了50公里后，ENU坐标的数值变得很大，浮点误差开始显现。后来我改成每10公里重置一次参考点，问题就解决了。说白了，ENU只适合局部区域，别指望它覆盖全球。
3. 载体坐标系：传感器安装的本地视角
载体坐标系（Body Frame），通常记作b系。原点在载体质心，X轴指向载体前方，Y轴指向右侧，Z轴指向下方（符合右手定则）。
IMU、轮速计、激光雷达这些传感器，输出的数据都是在各自的安装坐标系下。但融合算法需要把它们统一到载体坐标系下，才能描述载体的整体运动。
这里有个关键问题：传感器安装位置和朝向，需要精确标定。我见过一个项目，IMU装歪了2度，结果航向角误差越跑越大，半小时后偏了十几米。
我的建议：在代码里，一定要把外参（传感器相对于载体的旋转和平移）单独拎出来，写成可配置的参数。别写死在代码里。因为实车安装位置经常调整，写死了改起来很痛苦。
举个例子，把IMU的加速度从IMU坐标系转到载体坐标系：
// 外参：从IMU系到载体系的旋转矩阵 R_imu_to_body
// 假设已经标定好了 R_imu_to_body 和 t_imu_to_body
Vector3d acc_body = R_imu_to_body * acc_imu;
// 如果IMU不在载体质心，还要考虑杆臂效应补偿
Vector3d angular_vel = gyro_imu; // 角速度
Vector3d lever_arm = t_imu_to_body; // 杆臂
Vector3d acc_centrifugal = angular_vel.cross(angular_vel.cross(lever_arm));
Vector3d acc_body_corrected = acc_body + acc_centrifugal;
杆臂效应补偿很多人会忽略。你想想看，IMU装在车顶，离质心半米远，转弯时离心加速度会额外叠加到IMU读数上。不补偿的话，加速度计数据就是错的。
4. 时间同步基础：所有传感器的共同节拍
坐标系统一了，时间没对齐，照样白搭。
多传感器融合里，时间同步是最容易被低估的环节。GPS是整秒触发，IMU是固定频率中断，激光雷达是旋转扫描，相机是曝光时刻。这些时间戳如果不统一，融合出来的轨迹就是扭曲的。
时间同步分两个层面：
- 硬同步：通过硬件信号（如PPS、触发线）让多个传感器在同一时刻采集数据。精度可达微秒级。
- 软同步：通过插值或滤波，把不同时间戳的数据对齐到同一个时间基准上。精度取决于传感器频率和运动速度。
实战经验：在量产项目中，硬同步成本高，很多方案只用软同步。但软同步有个前提：所有传感器的时钟必须统一到同一个系统时钟上。我习惯用GPS的PPS信号来同步系统时钟，然后所有传感器的时间戳都以这个系统时钟为准。
软同步的常见做法是线性插值。比如IMU是100Hz，激光雷达是10Hz，激光雷达来一帧数据时，需要找到前后两帧IMU数据，按时间比例插出当前时刻的IMU状态：
// 时间戳对齐：线性插值IMU数据
// t_target: 激光雷达的时间戳
// imu_prev: 前一帧IMU (时间戳 t_prev)
// imu_next: 后一帧IMU (时间戳 t_next)
double alpha = (t_target - t_prev) / (t_next - t_prev);
IMUData imu_interpolated;
imu_interpolated.acc = imu_prev.acc * (1 - alpha) + imu_next.acc * alpha;
imu_interpolated.gyro = imu_prev.gyro * (1 - alpha) + imu_next.gyro * alpha;
注意：如果IMU频率太低（比如50Hz以下），或者载体运动太快（比如急转弯），线性插值的误差会很大。这时候可以考虑用更高阶的插值，或者干脆上硬同步。我曾经在高速测试车上吃过这个亏，50Hz的IMU插出来角度差了0.5度，融合结果直接发散。
本章知识体系总览
下面这张图，是我自己梳理的坐标系与时空基准的核心逻辑。你可以把它当作本章的思维导图：
这张图把本章的核心内容串起来了。你从上往下看：多传感器融合定位，需要同时解决空间基准和时间基准两个问题。空间基准里，WGS84、ENU、载体坐标系各有各的用途，需要互相转换。时间基准里，硬同步和软同步各有优劣，得根据项目选型。
最后落到关键操作上：坐标转换、外参标定、时间戳对齐。这三件事做好了，融合定位就成功了一半。
一个小建议：刚开始做融合的同学，我建议你先在仿真环境里把坐标系和时间同步的逻辑跑通。别急着上实车。仿真里可以随意控制传感器频率、时间延迟、安装误差，把这些情况都测一遍，实车时心里就有底了。