3. 坐标系与标定：车辆坐标系、相机坐标系、雷达坐标系及联合标定
各位同学，今天我们来聊聊感知融合里最基础、也最容易踩坑的一环——坐标系与标定。
说实话，我见过不少团队，算法模型跑得飞起，一上路就翻车。查到最后，十有八九是坐标系没对齐。你想想看，雷达说「前方5米有障碍」，相机说「左前方3米有车」，到底信谁？说白了，坐标系不统一，融合就是空谈。
3.1 车辆坐标系：一切感知的「锚点」
我个人习惯，先把车辆坐标系定死。这是所有传感器数据的最终归宿。
在自动驾驶领域，我们通常采用ISO 8855标准。车辆坐标系的原点一般放在后轴中心，或者车辆质心。具体怎么选？我建议放在后轴中心，因为车辆运动学模型用这个点最方便。
| 轴 | 方向 | 说明 |
|---|---|---|
| X轴 | 车辆前进方向 | 正前方为正 |
| Y轴 | 车辆左侧方向 | 符合右手定则 |
| Z轴 | 车辆正上方 | 垂直于地面向上 |
3.2 相机坐标系：从像素到空间的桥梁
相机坐标系，说白了就是解决「图像上的一个点，对应现实世界哪里」的问题。
相机坐标系的原点在光心，Z轴指向相机前方，X轴向右，Y轴向下。嗯，这里要注意，相机坐标系的Y轴是向下的，和车辆坐标系不一样。很多新手在这里搞混。
相机标定主要做两件事：
- 内参标定： 焦距、主点、畸变系数。我一般用张正友标定法，棋盘格拍20张左右就够了。
- 外参标定： 相机相对于车辆坐标系的旋转和平移。这个才是融合的关键。
核心公式：
// 像素坐标 -> 相机坐标
P_cam = K_inv * [u, v, 1]^T * depth
// 相机坐标 -> 车辆坐标
P_vehicle = R * P_cam + T其中R是旋转矩阵，T是平移向量。这两个参数，就是我们要标定的外参。
3.3 雷达坐标系：距离与速度的真相
雷达坐标系相对简单。原点在雷达中心，X轴指向雷达正前方，Y轴向左，Z轴向上。
雷达输出的数据是极坐标形式：距离、水平角、俯仰角。我们需要先转成直角坐标，再转到车辆坐标系。
3.4 联合标定：让所有传感器「说同一种语言」
联合标定，就是把相机、雷达、激光雷达等传感器的坐标系，统一到车辆坐标系下。
我常用的方法是：
- 找一个空旷场地，放置标定物（比如角反射器或棋盘格）
- 让车辆静止，采集各传感器数据
- 手动或自动匹配特征点
- 用最小二乘法求解外参
下面这张图，是我自己总结的标定流程框架：
3.5 实战中的标定技巧
说了这么多理论，来点实际的。我在项目里总结了几条铁律：
- 标定场地要够大： 至少20米×20米，太近了标定精度不够
- 多角度采集： 别只拍正面，左右各45度也要拍
- 温度影响： 夏天和冬天，传感器外壳热胀冷缩，外参会有微小变化。我建议每季度重新标定一次
避坑指南： 我曾经在高速项目上，因为相机和雷达的标定误差，导致车道线融合后出现「锯齿状」输出。排查了三天，最后发现是标定板的角点检测精度不够。后来改用高精度棋盘格，问题解决。
3.6 代码示例：外参标定计算
下面给一段我常用的外参求解代码，用SVD分解求旋转矩阵：
import numpy as np
def solve_extrinsics(points_cam, points_vehicle):
"""
求解相机到车辆坐标系的外参
points_cam: 相机坐标系下的点集 (N x 3)
points_vehicle: 车辆坐标系下的点集 (N x 3)
返回: R (3x3), T (3x1)
"""
# 计算质心
centroid_cam = np.mean(points_cam, axis=0)
centroid_vehicle = np.mean(points_vehicle, axis=0)
# 去中心化
points_cam_centered = points_cam - centroid_cam
points_vehicle_centered = points_vehicle - centroid_vehicle
# SVD分解
H = points_cam_centered.T @ points_vehicle_centered
U, S, Vt = np.linalg.svd(H)
R = Vt.T @ U.T
# 确保旋转矩阵行列式为1（右手系）
if np.linalg.det(R) < 0:
Vt[-1, :] *= -1
R = Vt.T @ U.T
# 计算平移向量
T = centroid_vehicle - R @ centroid_cam
return R, T好了，坐标系与标定这块，核心就是「统一」二字。所有传感器都对齐到车辆坐标系，融合才有意义。下一节我们会讲车道线检测的具体算法，到时候你会发现，标定做得好，后面事半功倍。