3. 坐标系与空间变换：世界坐标系、车辆坐标系、传感器坐标系、坐标变换矩阵、外参标定原理
坐标系这东西，刚入行时觉得就是数学概念，没什么大不了的。直到我第一次做多传感器融合项目，激光雷达和摄像头对同一个目标的位置差了半米多，我才意识到——坐标系搞不清楚，后面全是白干。
说白了，标定匹配的核心就一句话：把不同传感器看到的东西，统一到同一个坐标系下。你想想看，摄像头看到的是像素，激光雷达看到的是三维点云，毫米波雷达看到的是带速度的目标列表。它们各自活在自己的世界里，怎么对齐？
嗯，这就是我们今天要聊的内容。
3.1 四大坐标系，你得心里有数
自动驾驶里常用的坐标系有四个。我习惯把它们分成两类：全局的和局部的。
| 坐标系 | 原点位置 | 典型用途 | 我踩过的坑 |
|---|---|---|---|
| 世界坐标系 | 地球上的固定点（如UTM原点） | 高精地图、全局路径规划 | 不同地图供应商的UTM带号不一致，导致拼接错位 |
| 车辆坐标系 | 车辆后轴中心或质心 | 车辆动力学控制、底盘信号 | ISO和SAE的Z轴方向相反，搞混过一次 |
| 传感器坐标系 | 传感器光学中心或安装点 | 原始数据采集、感知算法 | IMU的安装角度偏差，导致姿态估计漂移 |
| 像素/图像坐标系 | 图像左上角或主点 | 视觉感知、目标检测 | 畸变校正没做干净，投影误差累积 |
这里有个关键点：车辆坐标系是中间桥梁。所有传感器数据最终都要转到车体坐标系下，再通过定位模块转到世界坐标系。我曾经见过一个团队，直接把激光雷达数据转到世界坐标系，结果车辆一动，地图就飘了——因为没经过车体系做时间对齐。
3.2 坐标变换矩阵，别被公式吓到
坐标变换说白了就是旋转+平移。旋转用矩阵，平移用向量，组合起来就是4x4的齐次变换矩阵。
我个人习惯用这种形式：
# 齐次变换矩阵 T = [R | t]
# [0 | 1]
# 其中 R 是 3x3 旋转矩阵
# t 是 3x1 平移向量
T_sensor_to_vehicle = np.array([
[0.9998, -0.0175, 0.0000, 0.5000],
[0.0175, 0.9998, -0.0001, -0.2000],
[0.0000, 0.0001, 1.0000, 0.1000],
[0.0000, 0.0000, 0.0000, 1.0000]
])
你可能会问：为什么非要用4x4矩阵？直接用3x3旋转矩阵加3x1平移向量不行吗？
当然可以。但当你需要连续做多次变换时——比如从传感器到车辆，再到世界坐标系——4x4矩阵可以直接相乘，一次搞定。用分离的形式，你得先旋转再平移，代码写起来啰嗦，还容易出错。
3.3 外参标定原理，核心就两件事
外参标定，就是求传感器坐标系到车辆坐标系的变换矩阵。说白了，就是找到传感器装在车上的什么位置、什么角度。
标定原理其实不复杂：
- 找对应点：在传感器数据和车辆坐标系下找到同一组特征点
- 解方程：通过最小二乘法或SVD分解，求解旋转矩阵和平移向量
我记得第一次做激光雷达和摄像头的联合标定时，用了棋盘格。激光雷达扫到棋盘格的角点，摄像头也拍到棋盘格的角点。理论上它们对应的是同一个物理点，但实际测量总有误差。这时候就需要采集多组数据，用优化方法求最优解。
min ∑ || R·p_i + t - q_i ||²
解出R和t，就是外参。
3.4 外参标定的三种主流方法
实际项目中，外参标定方法大致分三类。我按推荐程度排个序：
| 方法 | 原理 | 精度 | 适用场景 | 我的评价 |
|---|---|---|---|---|
| 靶标法 | 用棋盘格、ArUco码等已知尺寸的标定物 | 高（亚毫米级） | 实验室、产线 | 最可靠，但需要专用场地 |
| 自然特征法 | 利用道路上的车道线、杆状物等自然特征 | 中等（厘米级） | 现场标定、OTA更新 | 方便，但受环境影响大 |
| 手眼标定法 | 利用传感器自身的运动估计，解AX=XB方程 | 中等（厘米级） | IMU+相机、多激光雷达 | 不需要标定物，但收敛慢 |
3.5 坐标系变换的完整流程
一个典型的感知数据流转过程是这样的：
# 伪代码：激光雷达点云从传感器到世界坐标系
# 1. 传感器坐标系 -> 车辆坐标系
points_vehicle = T_sensor_to_vehicle @ points_sensor
# 2. 车辆坐标系 -> 世界坐标系（通过定位模块）
points_world = T_vehicle_to_world @ points_vehicle
# 3. 世界坐标系 -> 局部规划坐标系（可选）
points_local = T_world_to_local @ points_world
这里每一步都涉及时间同步。我记得有一次调试，激光雷达和IMU的时间戳差了50毫秒，车辆在转弯时，点云和姿态对不上，地图直接扭曲了。后来加了硬件同步信号，问题才解决。
嗯，时间同步和坐标系变换，其实是同一个问题的两个面。后面我们会专门讲。
3.6 知识体系总览
下面这张图，把本章的核心逻辑串起来了。我建议你保存下来，做项目时对照着看。
3.7 量产部署中的坐标系管理
量产和研发不一样。研发时你可以慢慢调，量产时每辆车的外参都有细微差异。我建议你注意以下几点：
- 统一约定：团队内部必须统一坐标系定义，包括X/Y/Z轴方向、旋转顺序（RPY还是欧拉角）、单位（米还是毫米）。我曾经吃过亏，算法团队用ISO标准，底盘团队用SAE标准，联调时发现Z轴方向反了。
- 标定数据管理：每辆车下线时，外参数据要写入车辆的配置文件，并备份到云端。换传感器时，直接OTA下发新外参，不用重新标定。
- 在线监测：量产车在路上跑，外参会因为振动、温度变化而漂移。我建议加一个在线监测模块，实时评估外参置信度。如果发现异常，及时报警或触发在线标定。
好了，坐标系和空间变换就聊到这里。记住一句话：标定的本质，就是找到不同坐标系之间的那个变换矩阵。后面我们会深入讲具体的标定工具和流程，到时候你会回来感谢今天打下的基础。