坐标系基础：世界坐标系、车辆坐标系、传感器坐标系
做多传感器融合，第一道坎就是坐标系。我见过太多项目，算法模型调得漂漂亮亮，一上路就崩——查到最后，往往是坐标系没对齐。说白了，坐标系就是给每个传感器定个「家」，然后告诉系统它们之间的亲戚关系。
今天咱们就把这几个坐标系掰扯清楚。我会结合自己踩过的坑，帮你建立直观理解。
一、世界坐标系：全局的「锚点」
世界坐标系是最大的那个坐标系。它固定在地球上，不随车辆移动而改变。你可以把它想象成一张大地图的经纬度网格。
通常我们用 WCS（World Coordinate System） 来表示。在自动驾驶里，常见的有两种定义方式：
- ENU坐标系：东-北-天方向。X轴指东，Y轴指北，Z轴指天。我习惯用这个，因为直观。
- UTM坐标系：通用横轴墨卡托投影。把地球表面投影到平面上，单位是米。
重要概念：世界坐标系是所有传感器数据的「最终归宿」。不管你用相机还是激光雷达，最后都要把检测结果转换到世界坐标系下，才能做全局路径规划和地图构建。
我在项目里遇到过一个问题：高精地图用的是UTM坐标，但IMU输出的是经纬度。如果不做投影转换，直接拿来用，偏差能到几十米。嗯，这个坑我替你们踩过了。
二、车辆坐标系：车身的「骨架」
车辆坐标系是固定在车身上的。它随着车一起动，一起转。你想想看，如果车在转弯，车辆坐标系也跟着转。
行业标准一般这样定义：
- X轴：指向车辆前进方向
- Y轴：指向车辆左侧（也有用右侧的，但ISO标准是左侧）
- Z轴：指向车顶方向
原点通常放在后轴中心，或者车辆质心。我个人习惯放在后轴中心，因为这样转弯时的运动学模型更简洁。
小技巧：调试时可以在车辆坐标系下画一个「前-左-上」的箭头，贴在车顶。这样看数据时，脑子能快速对应到物理方向。
为什么要单独搞一个车辆坐标系？因为传感器都装在车上，它们的测量结果天然就是相对于车身的。比如激光雷达说「前方5米有障碍物」，这个「前方」就是车辆坐标系的前方。
三、传感器坐标系：各说各话的「方言」
每个传感器都有自己的坐标系。它们就像不同国家的人，说着不同的「方言」。融合的任务，就是把这些方言翻译成同一种语言。
3.1 相机坐标系
相机坐标系比较特殊。它遵循的是计算机视觉的习惯：
- X轴：指向图像右侧
- Y轴：指向图像下方（对，你没看错，是向下）
- Z轴：指向相机前方（沿着光轴方向）
为什么Y轴向下？因为图像的行索引是从上往下增加的。这个设计让像素坐标和图像坐标的映射更直接。
注意：相机坐标系的Z轴是光轴方向，不是重力方向。所以相机装歪了，Z轴就不水平了。我曾经因为相机支架松动，导致Z轴偏了2度，融合出来的目标位置全偏了。嗯，从那以后我每次上车第一件事就是检查支架紧固螺丝。
3.2 激光雷达坐标系
激光雷达的坐标系相对直观：
- X轴：指向雷达前方（通常是扫描起始方向）
- Y轴：指向雷达左侧
- Z轴：指向雷达上方
但要注意，不同厂商的定义可能不一样。比如Velodyne的坐标系和Hesai的就有差异。我建议拿到新雷达后，第一件事就是看它的坐标系定义文档，别想当然。
激光雷达输出的是点云，每个点包含(x, y, z)坐标和反射强度。这些坐标就是相对于雷达自身的。
3.3 毫米波雷达坐标系
毫米波雷达的坐标系和激光雷达类似，但有一个关键区别：
- X轴：指向雷达前方
- Y轴：指向雷达左侧
- Z轴：指向雷达上方
但毫米波雷达通常只输出2D信息（距离、方位角），高度信息很弱。所以它的坐标系其实更接近一个极坐标系：距离r和方位角θ。
避坑指南：毫米波雷达的方位角分辨率通常只有1-2度。在远距离时，角度误差会导致横向位置偏差很大。比如100米外，1度误差就是1.7米的横向偏移。我曾经因为这个原因，把一辆停在路边的车误判到了车道上。
3.4 IMU坐标系
IMU（惯性测量单元）测量的是加速度和角速度。它的坐标系通常这样定义：
- X轴：指向车辆前进方向
- Y轴：指向车辆左侧
- Z轴：指向车辆上方（与重力方向相反）
IMU的坐标系和车辆坐标系高度重合，但安装时难免有偏差。哪怕偏了0.5度，积分出来的位置误差也会随时间累积。
我记得有一次做车辆定位，IMU数据一直有漂移。查了两天才发现，是IMU的安装底座有个0.3度的倾斜。后来我学乖了，每次装完IMU都会做一次「六位置标定」来校准。
四、坐标系之间的转换关系
搞清楚了各个坐标系，接下来就是它们之间的转换。说白了就是三步：
- 旋转：用旋转矩阵R把方向对齐
- 平移：用平移向量t把原点对齐
- 组合：用齐次变换矩阵T = [R | t] 一步搞定
举个例子，把激光雷达的点云转换到车辆坐标系：
// 伪代码示例
// 已知：激光雷达到车辆的变换矩阵 T_lidar_to_vehicle
// 输入：激光雷达坐标系下的点 P_lidar = (x, y, z, 1)
// 输出：车辆坐标系下的点 P_vehicle
P_vehicle = T_lidar_to_vehicle * P_lidar
// 其中 T_lidar_to_vehicle 是 4x4 齐次矩阵
// 包含旋转部分 R (3x3) 和平移部分 t (3x1)
// T = [R t]
// [0 1]经验之谈：我习惯把所有传感器的变换矩阵都存成一个配置文件，用YAML格式。这样换传感器或者调整安装位置时，改配置文件就行，不用动代码。
五、知识体系总览
下面这张图展示了本章的核心逻辑：从传感器到世界，坐标系逐级转换的链条。
从这张图可以看得很清楚：每个传感器先通过外参标定转换到车辆坐标系，再通过车辆定位（GPS+IMU）转换到世界坐标系。这就是多传感器对齐的完整链路。
六、总结与避坑
最后，我把自己这些年踩过的坑总结成几条铁律：
| 坐标系 | 常见坑点 | 我的建议 |
|---|---|---|
| 世界坐标系 | 不同地图源使用不同投影 | 统一用UTM，做好投影转换 |
| 车辆坐标系 | 原点位置不统一 | 固定在后轴中心，写进设计文档 |
| 相机坐标系 | Y轴方向容易搞反 | 画个坐标系示意图贴在调试台 |
| 激光雷达坐标系 | 不同厂商定义不同 | 拿到新雷达先看坐标系文档 |
| 毫米波雷达坐标系 | 角度分辨率导致远距离偏差 | 远距离目标要加置信度权重 |
| IMU坐标系 | 安装倾斜导致积分漂移 | 装完必须做六位置标定 |
最后一句掏心窝的话：坐标系对齐这件事，看起来简单，但80%的融合问题都出在这里。别问我怎么知道的——我当年花了两周时间，就为了追一个0.5度的旋转偏差。从那以后，我每做一个新项目，第一件事就是画坐标系转换图，然后逐级验证。
好了，坐标系的基础就聊到这儿。记住一句话：坐标系对齐了，融合就成功了一半。