4、坐标系与变换：世界坐标系、车辆坐标系、传感器坐标系、坐标变换矩阵
做自动驾驶，说白了就是在跟坐标系打交道。
我记得刚入行那会儿，带我的老工程师跟我说过一句话：「搞懂坐标系，你的端到端就入门了一半。」当时我不信，觉得不就是几个坐标轴嘛。直到我自己在实车测试中栽了个大跟头——传感器数据死活对不上，折腾了两天才发现是坐标系定义搞反了。嗯，从那以后我再也不敢小看这个章节了。
4.1 为什么需要多个坐标系？
你想想看，一辆车上装了摄像头、激光雷达、毫米波雷达，还有GPS和IMU。每个传感器都有自己的「视角」。摄像头看到的是像素，激光雷达看到的是点云，GPS告诉你在地球上的经纬度。
这些数据格式完全不同，怎么融合？
答案就是：统一到一个坐标系下。
我个人习惯把坐标系分成三个层级：
- 世界坐标系：全局的、固定的，比如经纬度+海拔
- 车辆坐标系：跟着车走的，车头方向是X轴
- 传感器坐标系：每个传感器自己的小天地
核心思想：所有传感器数据最终都要变换到车辆坐标系下，才能被下游的规划和控制模块使用。
4.2 世界坐标系（World Coordinate System）
世界坐标系是「上帝视角」。它不随车辆移动而改变，是一个绝对参考系。
在自动驾驶中，常用的世界坐标系有：
- WGS-84：GPS用的经纬度坐标系，地球是个椭球体
- UTM：把地球投影到平面上，用米做单位
- 局部笛卡尔坐标系：以某个点为原点，X轴指向东，Y轴指向北
我在项目中遇到过一个问题：GPS信号在高架桥下会丢失，这时候就得靠IMU和轮速计做航位推算。说白了，世界坐标系虽然稳定，但有时候靠不住。
实战建议：做高精地图匹配时，尽量用UTM坐标系。经纬度做距离计算太麻烦，还得考虑地球曲率。
4.3 车辆坐标系（Vehicle Coordinate System）
车辆坐标系是「车的视角」。它遵循ISO 8855标准：
- X轴：指向车辆前进方向
- Y轴：指向车辆左侧（注意！有些标准指向右侧）
- Z轴：指向车辆上方
- 原点：通常在后轴中心，或者车辆质心
为什么原点放在后轴中心？
因为车辆的运动学模型（比如阿克曼模型）以后轴为参考点最方便。前轮转向时，后轴中心的轨迹更容易计算。
避坑指南：我曾经因为搞混了「左为正」和「右为正」，导致车道保持功能在弯道里疯狂画龙。检查了三天代码，最后发现是坐标系Y轴方向定义反了。所以，拿到传感器数据第一件事：确认坐标系定义！
4.4 传感器坐标系（Sensor Coordinate System）
每个传感器都有自己的坐标系，这是最容易出问题的地方。
| 传感器 | 坐标系定义 | 常见坑点 |
|---|---|---|
| 摄像头 | 图像坐标系(u,v)，相机坐标系(Xc,Yc,Zc) | Z轴指向相机前方，但不同厂家可能不同 |
| 激光雷达 | X轴指向车头，Y轴指向左侧，Z轴向上 | 有些LiDAR的Y轴指向右侧，需要手动翻转 |
| 毫米波雷达 | 极坐标系(距离,角度,速度) | 角度定义是顺时针还是逆时针？ |
| IMU | 载体坐标系，与车辆坐标系对齐 | 安装角度偏差需要标定 |
我个人习惯的做法是：拿到一个新传感器，先打印它的原始数据，然后手动旋转车辆，观察坐标值的变化。这样能最快发现坐标系定义是否和预期一致。
4.5 坐标变换矩阵（Coordinate Transformation Matrix）
坐标变换，说白了就是「把一个点从一个坐标系搬到另一个坐标系」。数学上用一个4x4的齐次变换矩阵搞定：
// 齐次变换矩阵 T = [R t; 0 1]
// R: 3x3 旋转矩阵
// t: 3x1 平移向量
// 示例：将激光雷达点变换到车辆坐标系
// 假设已知 LiDAR 到 Vehicle 的变换矩阵 T_lidar_to_vehicle
Eigen::Matrix4f T_lidar_to_vehicle;
// ... 从标定文件读取或计算得到
// 激光雷达坐标系下的点 (x_l, y_l, z_l)
Eigen::Vector4f point_lidar(x_l, y_l, z_l, 1.0);
// 变换到车辆坐标系
Eigen::Vector4f point_vehicle = T_lidar_to_vehicle * point_lidar;
// 取出变换后的坐标
float x_v = point_vehicle(0);
float y_v = point_vehicle(1);
float z_v = point_vehicle(2);这里有个关键点：变换矩阵是链式的。
比如要把世界坐标系的点变换到摄像头像素坐标系，需要经过：
- 世界 → 车辆（GPS+IMU给出车辆位姿）
- 车辆 → 相机（标定得到的外参）
- 相机 → 像素（内参矩阵）
每一步都是一个4x4矩阵，最终乘在一起得到一个总的变换矩阵。
数学本质：坐标变换 = 旋转 + 平移。旋转用欧拉角或四元数表示，平移就是三个方向的偏移量。
4.6 坐标系变换的工程实现
在实际项目中，我不会手写变换矩阵。太容易出错了。
我推荐用现成的库：
- ROS的tf库：最常用，支持动态变换树
- Eigen：C++矩阵运算库，轻量高效
- PyTorch3D：如果做深度学习，可以用这个做可微分的坐标变换
我曾经在项目里用ROS的tf树，结果发现变换关系写成了循环依赖——A依赖B，B依赖C，C又依赖A。tf直接报错，整个系统崩溃。所以，画变换关系图很重要。
调试技巧：写一个简单的可视化工具，把各个坐标系的原点和轴画出来。用不同颜色区分：红色X轴，绿色Y轴，蓝色Z轴。一眼就能看出变换对不对。
4.7 本章知识体系
下面这张图总结了坐标系与变换的核心逻辑：
4.8 避坑总结
最后，分享几个我踩过的坑：
- 坐标系定义不一致：不同传感器厂商、不同算法库可能用不同标准。拿到数据先确认。
- 变换顺序搞反：先旋转再平移，还是先平移再旋转？顺序错了结果完全不对。
- 忘记齐次坐标：3D点(x,y,z)要补成(x,y,z,1)才能乘4x4矩阵。我见过有人直接用3x3矩阵去乘3D点，结果平移分量丢了。
- 标定参数过期：传感器安装位置变了（比如碰撞后），标定参数要重新做。
血的教训：我曾经在量产项目里，因为一个传感器的标定文件版本没更新，导致自动泊车功能把车停到了草坪上。从那以后，我要求团队每次上车前都要做一次「坐标系一致性检查」——打印几个关键点的坐标，人工确认是否合理。
坐标系与变换，听起来枯燥，但它是端到端自动驾驶的「地基」。地基不稳，上面盖的楼再漂亮也得塌。嗯，这一章就到这里。