📚 课程目录第4章传感器与坐标系

4. 传感器与坐标系

在 Occupancy 网络训练与部署工程中，多传感器融合是提升环境感知鲁棒性的关键手段。相机提供稠密纹理与语义信息，激光雷达提供高精度三维几何，毫米波雷达则具备全天候测距测速能力。然而，这三类传感器各自拥有独立的坐标系，只有通过精确的外参标定与坐标变换，才能将它们的观测统一到同一参考系下，为 Occupancy 栅格或体素标签生成提供一致的空间基础。

一、核心原理：四类坐标系与刚体变换

多传感器系统通常涉及以下坐标系：

世界坐标系（World）：全局统一参考，常以车辆上电时刻的位姿或地图原点为基准。
自车坐标系（Ego / Body）：以车辆后轴中心或质心为原点，X 向前、Y 向左、Z 向上。
传感器坐标系（Sensor）：相机以光心为原点，Z 向前、X 向右、Y 向下；激光雷达以旋转中心为原点；毫米波雷达以天线阵面中心为原点。
像素/图像坐标系（Image）：通过内参将相机坐标系下的三维点投影到二维图像平面。

外参标定的本质是求解传感器坐标系到自车坐标系的刚体变换矩阵 T_ego_sensor，包含旋转 R（3×3）与平移 t（3×1）。对于任意传感器观测点 P_s，其在自车坐标系下的坐标可表示为：

P_ego = R * P_s + t
       = T_ego_sensor * [P_s; 1]

其中 T_ego_sensor 为 4×4 齐次变换矩阵：
T = [ R  t ]
    [ 0  1 ]

相机还额外需要内参矩阵 K 与畸变系数 D，完成从相机坐标系到像素坐标系的投影。激光雷达与毫米波雷达则主要依赖外参，将点云或目标列表变换到统一坐标系。

二、方法流程：从标定到坐标变换

外参标定通常分为离线标定与在线标定两个阶段。离线标定在车辆出厂或传感器安装后执行，在线标定则在运行过程中持续修正因振动、温度变化引起的漂移。

标定板/靶标布置：使用棋盘格、AprilTag 或专用标定靶，确保相机与激光雷达均能清晰观测。
数据采集：同步采集相机图像、激光雷达点云与毫米波雷达目标列表，记录时间戳。
特征提取与匹配：相机提取角点，激光雷达提取靶标平面或边缘，毫米波雷达提取角反射器位置。
外参求解：通过 PnP、ICP 或联合优化求解 R 与 t。
验证与评估：将激光雷达点云投影到图像上，检查边缘对齐程度；计算重投影误差。
在线修正：利用道路消失点、车道线、地面平面等自然特征，实时估计外参微调量。

关键提示：相机与激光雷达的外参标定精度通常要求旋转误差小于 0.1°，平移误差小于 1 cm；毫米波雷达因角分辨率较低，外参精度要求可适当放宽，但需保证与激光雷达的朝向一致性。

三、案例/场景：Occupancy 标签生成中的坐标变换

假设我们需要为 Occupancy 网络生成自车坐标系下的 3D 占用栅格标签。输入包括：

相机图像：用于语义分割，得到 2D 语义图。
激光雷达点云：用于几何重建，得到 3D 点云。
毫米波雷达目标：用于运动属性补充，得到速度与 RCS。

处理流程如下：

1. 激光雷达点云 P_lidar 经 T_ego_lidar 变换到自车坐标系：
   P_ego = T_ego_lidar * P_lidar

2. 相机图像经语义分割得到 2D 语义图 S_img，
   再通过相机内参 K 与外参 T_ego_cam 反投影到 3D 空间：
   P_cam = K^-1 * [u, v, 1]^T * depth
   P_ego = T_ego_cam * P_cam

3. 毫米波雷达目标 T_radar 经 T_ego_radar 变换到自车坐标系：
   T_ego = T_ego_radar * T_radar

4. 将上述三类观测融合到统一体素网格中，
   生成 Occupancy 标签：占据/空闲/未知。

在此过程中，任何外参误差都会导致语义与几何错位，例如将路侧车辆的错误语义投影到自车前方，造成 Occupancy 标签污染。因此，外参标定是 Occupancy 训练数据质量的生命线。

四、误区与复盘

常见误区：

忽略时间同步：仅做空间标定而不做时间对齐，高速运动场景下会产生显著的运动畸变。
混淆坐标系定义：相机坐标系与图像坐标系方向不同，直接套用旋转矩阵会导致投影翻转。
过度依赖单一标定方法：仅用棋盘格标定相机-激光雷达，在远距离处误差放大，需结合自然场景特征进行验证。
忽视在线漂移：车辆长期运行后，传感器支架可能发生微小形变，离线外参不再适用。

复盘时，建议建立外参标定质量监控指标，包括：重投影误差、点云-图像边缘对齐度、毫米波雷达与激光雷达目标关联一致性。将这些指标纳入数据闭环，定期触发重新标定或在线修正。

五、外参标定参数与检查清单

传感器	标定对象	典型精度要求	常用工具/方法
相机	内参 K、畸变 D、外参 T_ego_cam	重投影误差 < 0.5 px	张正友标定法、Kalibr
激光雷达	外参 T_ego_lidar	旋转 < 0.1°，平移 < 1 cm	ICP、平面拟合、靶标
毫米波雷达	外参 T_ego_radar	旋转 < 0.5°，平移 < 5 cm	角反射器、道路特征
联合标定	相机-激光雷达-毫米波雷达	跨模态对齐误差 < 2 px	联合优化、自动标定

在实际工程中，建议将外参以 YAML 或 JSON 格式固化，并在推理管线中统一加载。以下是一个典型的外参配置示例：

sensors:
  camera_front:
    T_ego_cam: [0.9998, -0.0175, 0.0087, 1.52,
                0.0175,  0.9998, 0.0000, 0.00,
                -0.0087, 0.0000, 0.9999, 1.20,
                0.0,     0.0,    0.0,    1.0]
  lidar_top:
    T_ego_lidar: [1.0, 0.0, 0.0, 0.0,
                  0.0, 1.0, 0.0, 0.0,
                  0.0, 0.0, 1.0, 1.80,
                  0.0, 0.0, 0.0, 1.0]
  radar_front:
    T_ego_radar: [0.9999, 0.0, 0.014, 2.10,
                  0.0,    1.0, 0.0,   0.00,
                  -0.014, 0.0, 0.9999, 0.60,
                  0.0,    0.0, 0.0,    1.0]

只有确保每个传感器的外参准确、时间同步可靠，后续的 Occupancy 网络才能学到正确的空间占用关系。外参标定不是一次性的任务，而是贯穿数据采集、训练与部署全流程的持续工程。