坐标系与时空基准：WGS84、ENU、车身坐标系及时间同步机制

各位同学，咱们今天聊点实在的。

做自动驾驶定位，说白了就是回答三个问题：我在哪？我要去哪？我怎么去？但回答这些问题之前，你得先搞清楚一个更基础的问题——我用什么尺子量？

坐标系和时空基准，就是这把尺子。我见过太多项目，算法调得漂漂亮亮，最后定位飘了，查了半天发现是坐标系没对齐。嗯，这种坑，咱们今天一次性填平。

1. WGS84坐标系：地球的“身份证”

WGS84，全称World Geodetic System 1984。你打开手机GPS，拿到的经纬度，就是WGS84坐标。

它把地球近似成一个椭球体。为什么是椭球？因为地球自转，赤道会鼓起来一点。这个椭球的参数是固定的：长半轴6378137米，扁率1/298.257223563。

我当年第一次做车载定位时，直接拿GPS输出的经纬度去算距离，结果误差大得离谱。后来才明白——WGS84是球面坐标，不是平面坐标。两点之间不是直线，是大圆弧。

实际项目中，我们通常用经纬度做全局定位，但做局部路径规划时，必须转成平面坐标。怎么转？往下看。

2. ENU坐标系：局部导航的“好帮手”

ENU，东-北-天坐标系。说白了，就是以你当前位置为原点，东方向为X轴，北方向为Y轴，天方向为Z轴。

为什么需要它？你想想看，WGS84的经纬度算距离，要算球面三角，计算量大不说，还不直观。ENU就简单了——直接当平面直角坐标系用。

转换方法其实不复杂：

// WGS84 (lat, lon, alt) 转 ENU (x, y, z)
// 1. 先转成地心地固坐标 (ECEF)
// 2. 再通过旋转矩阵转到ENU

// 伪代码示例
function wgs84_to_enu(lat, lon, alt, ref_lat, ref_lon, ref_alt) {
    // 计算参考点的ECEF坐标
    ecef_ref = geodetic_to_ecef(ref_lat, ref_lon, ref_alt)
    // 计算当前点的ECEF坐标
    ecef_cur = geodetic_to_ecef(lat, lon, alt)
    // 计算差值
    dx = ecef_cur.x - ecef_ref.x
    dy = ecef_cur.y - ecef_ref.y
    dz = ecef_cur.z - ecef_ref.z
    // 旋转到ENU
    x = -sin(lon_ref)*dx + cos(lon_ref)*dy
    y = -sin(lat_ref)*cos(lon_ref)*dx - sin(lat_ref)*sin(lon_ref)*dy + cos(lat_ref)*dz
    z = cos(lat_ref)*cos(lon_ref)*dx + cos(lat_ref)*sin(lon_ref)*dy + sin(lat_ref)*dz
    return (x, y, z)
}

3. 车身坐标系：车辆自己的“小世界”

车身坐标系，也叫车辆坐标系。原点在车辆后轴中心（或者质心），X轴指向车头，Y轴指向左侧，Z轴指向上方。

为什么用后轴中心？因为车辆运动学模型里，后轴中心是非转向点，运动轨迹更干净。我刚开始做时用了前轴中心，结果阿克曼转向模型算出来全是偏差，后来才改过来。

坐标系	原点	X轴	Y轴	Z轴
WGS84	地球质心	本初子午线方向	东方向	北极方向
ENU	自定义参考点	东方向	北方向	天方向
车身坐标系	后轴中心	车头方向	左侧方向	上方方向

实际应用中，传感器数据都要统一到车身坐标系。比如激光雷达装在车顶，它的点云坐标要先转到车身坐标系，才能和IMU、GPS的数据融合。

4. 时间同步机制：所有数据的“对齐锁”

坐标系对齐了，时间没对齐，照样白搭。

你想想看，GPS数据是10Hz，IMU是100Hz，激光雷达是20Hz。如果时间戳没对齐，你拿到的“当前时刻”的位置，可能是100毫秒前的数据。车辆以60km/h行驶，100毫秒就是1.67米——车道都偏出去了。

常用的时间同步方案有两种：

1. 硬件同步（PPS + NMEA）：GPS模块输出PPS脉冲信号，所有传感器通过硬件触发采集。精度可达微秒级。
2. 软件同步（时间戳插值）：每个传感器记录自己的时间戳，主控通过线性插值对齐到统一时间基准。精度在毫秒级。

我个人更推荐硬件同步。虽然成本高一点，但省心。软件同步在高速场景下容易出问题——插值假设运动是线性的，但急转弯时这个假设不成立。

// 软件时间同步示例：线性插值对齐
// 假设IMU时间戳 t_imu，GPS时间戳 t_gps
// 需要将GPS位置插值到IMU的时间点

function interpolate_gps_to_imu(gps_data, imu_time) {
    // 找到imu_time前后的两个GPS数据点
    gps_before = find_last_gps_before(imu_time)
    gps_after = find_next_gps_after(imu_time)
    
    // 计算时间比例
    ratio = (imu_time - gps_before.t) / (gps_after.t - gps_before.t)
    
    // 线性插值位置
    x = gps_before.x + ratio * (gps_after.x - gps_before.x)
    y = gps_before.y + ratio * (gps_after.y - gps_before.y)
    
    return (x, y)
}

5. 知识体系总览

下面这张图，把今天讲的内容串起来了。你可以看到，从WGS84到ENU再到车身坐标系，是一个层层转换的过程。时间同步则是贯穿始终的“粘合剂”。

嗯，今天的内容就到这里。坐标系和时空基准，听起来基础，但确实是定位系统的地基。地基没打好，上面盖多高的楼都白搭。

下次咱们聊传感器标定——怎么把相机、激光雷达、IMU这些传感器的数据，真正对齐到同一个坐标系下。到时候你会发现，今天讲的这些坐标系知识，全都会用上。