2. 坐标系与时空基准：WGS84、UTM、ENU、车身坐标系、时间同步机制
各位同学，欢迎来到第二讲。
说实话，做自动驾驶定位这几年，我踩过最大的坑，往往不是算法本身，而是坐标系搞混了。你想想看，一个点在地球上是WGS84坐标，到了局部地图变成了UTM，再转到车身坐标系，但凡有一个转换参数弄错，车就直接偏到沟里去了。嗯，这可不是开玩笑，我在项目里亲眼见过因为坐标系没对齐，高精地图和感知结果差了十几米，那场面……
所以这一章，咱们就把这些坐标系和时空基准彻底捋清楚。说白了，这就是定位系统的“语言”，语言不通，啥都白搭。
2.1 WGS84：全球通用的地理坐标系
WGS84，全称World Geodetic System 1984。它是目前GPS使用的默认坐标系。你手机里拿到的经纬度，就是WGS84坐标。
它的核心是一个地心地固坐标系（ECEF），原点在地球质心。Z轴指向北极，X轴指向本初子午线与赤道的交点，Y轴按右手定则确定。
在实际工程中，我们通常用纬度(lat)、经度(lon)、海拔(alt)来表示。但要注意，WGS84的纬度是大地纬度，不是地心纬度。这两者在地球椭球模型下是有差异的，虽然差异不大，但在高精度定位中不能忽略。
重要参数：
- 长半轴 a = 6378137.0 m
- 扁率 f = 1/298.257223563
- 地球自转角速度 ω = 7292115.0 × 10⁻¹¹ rad/s
我个人习惯在代码里把这些参数写成常量，避免每次手算。曾经有一次，我手抖把扁率写成了1/298.25，结果定位偏差在100公里外累积了将近2米。从那以后，我再也不敢偷懒了，全部从官方文档复制粘贴。
2.2 UTM：从球面到平面的投影
WGS84虽然全球通用，但它是球面坐标。做局部路径规划、地图匹配时，用经纬度算距离非常麻烦，还得考虑曲率。这时候就需要UTM投影。
UTM（Universal Transverse Mercator）把地球分成60个带，每个带宽度6度经度。每个带内，用横轴墨卡托投影把椭球面映射到平面上。投影后的坐标单位是米，这就方便多了。
我的经验：UTM投影在带内精度很高，但在带边界附近误差会急剧增大。如果你做跨带行驶的定位，比如从北京开到上海，一定要处理好带号切换。我建议在工程中预留一个“跨带检测”逻辑，当车辆接近带边界时，提前切换投影参数。
UTM坐标通常表示为(Easting, Northing, Zone, Hemisphere)。东向坐标Easting从500公里开始，北向坐标Northing在北半球从0开始，南半球从10000公里开始。为什么要加500公里？为了避免负值。嗯，这个设计挺巧妙的。
2.3 ENU：局部导航坐标系
ENU，即东-北-天坐标系。它是以某个参考点为原点的局部直角坐标系。X轴指向东，Y轴指向北，Z轴指向天顶。
为什么需要ENU？因为UTM虽然平面化了，但它的原点在赤道和中央经线交点，离车辆太远。做局部路径规划时，我们希望原点就在车辆附近，这样数值稳定，计算也方便。
从WGS84到ENU的转换，通常分两步：
- WGS84 (lat, lon, alt) → ECEF (X, Y, Z)
- ECEF (X, Y, Z) → ENU (E, N, U)，以某个参考点为中心
// 伪代码示例：WGS84转ENU
// 输入：纬度lat, 经度lon, 海拔alt, 参考点ref_lat, ref_lon, ref_alt
// 输出：东向e, 北向n, 天向u
// 第一步：将当前点和参考点都转为ECEF
ECEF current = wgs84_to_ecef(lat, lon, alt);
ECEF ref = wgs84_to_ecef(ref_lat, ref_lon, ref_alt);
// 第二步：计算差值
double dx = current.x - ref.x;
double dy = current.y - ref.y;
double dz = current.z - ref.z;
// 第三步：旋转到ENU
double sin_lat = sin(ref_lat);
double cos_lat = cos(ref_lat);
double sin_lon = sin(ref_lon);
double cos_lon = cos(ref_lon);
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;
这段代码我写过不下50遍。你想想看，每次做定位融合，都要把GPS的WGS84转到ENU，再和IMU、轮速计的数据对齐。如果旋转矩阵写错了，那定位结果就是天方夜谭。
2.4 车身坐标系：车辆自身的参考系
车身坐标系，也叫车辆坐标系。通常定义如下：
- X轴：指向车辆前进方向
- Y轴：指向车辆左侧（或右侧，各家定义不同）
- Z轴：指向车顶
注意：不同车厂对车身坐标系的定义可能不同。有的用“前-左-上”，有的用“前-右-上”。我在做多传感器标定时，曾经因为没注意这个差异，把激光雷达的点云投影到了车身坐标系的反方向。排查了整整两天才发现是Y轴方向反了。所以，拿到任何传感器数据，第一件事就是确认它的坐标系定义。
车身坐标系是定位融合的“锚点”。所有传感器的数据，最终都要统一到车身坐标系下。比如：
- GPS/IMU组合导航输出的是ENU下的位置和姿态
- 轮速计输出的是车身坐标系下的速度
- 激光雷达点云是传感器坐标系下的，需要外参转到车身
把这些数据融合在一起，就是卡尔曼滤波或者因子图优化的核心工作。
2.5 时间同步机制：所有数据的“对齐”
坐标系对齐了，时间没对齐，照样白搭。你想想看，如果GPS数据是100ms前的，IMU数据是50ms前的，轮速计是实时更新的，那融合出来的结果能准吗？
时间同步，说白了就是让所有传感器都使用同一个“时钟”。常见的做法有两种：
| 方法 | 原理 | 精度 | 适用场景 |
|---|---|---|---|
| 硬件同步 | 通过PPS脉冲或IEEE 1588协议，所有传感器共享同一个时钟源 | 微秒级 | 高精度定位、多传感器融合 |
| 软件同步 | 每个传感器打上时间戳，后处理时通过插值对齐 | 毫秒级 | 低成本方案、后处理 |
我个人强烈建议，如果条件允许，一定要上硬件同步。我在早期项目里图省事，用了纯软件同步，结果车辆高速行驶时，时间偏差导致的位置误差直接到了分米级。后来换了PPS同步，问题迎刃而解。
避坑指南：我曾经遇到过GPS的PPS信号和IMU的时钟不同步，原因是GPS接收机的PPS输出有固定的延迟（通常几十微秒）。这个延迟如果不补偿，在高动态场景下会引入明显的误差。解决办法是在标定阶段测量这个延迟，然后在软件中做补偿。
时间同步的另一个关键点是时间戳的统一格式。我建议所有传感器都使用UTC时间，并以纳秒为单位存储。这样在做插值和融合时，精度最高，也最方便。
2.6 总结与实战建议
好了，这一章的内容就这些。我们来捋一下重点：
- WGS84：全球经纬度基准，GPS直接输出
- UTM：平面投影，适合局部计算，注意带边界
- ENU：局部导航坐标系，以参考点为原点
- 车身坐标系：所有传感器数据的统一归宿
- 时间同步：硬件同步优先，软件同步做补偿
在下一章，我们会把这些坐标系和时间同步的知识，真正用到卡尔曼滤波的融合框架里。到时候你会发现，坐标系转换和时间对齐，就是整个定位系统的“地基”。地基打不牢，房子盖得再高也得塌。
嗯，今天就到这里。各位回去可以试试，把自己手里的GPS数据转成ENU，看看和实际轨迹对不对得上。有问题随时交流。