3. 坐标系与标定
在泊车多传感器融合系统中，所有感知结果最终都要落到同一个空间基准上，才能判断“车位在哪里、车能不能停进去”。这个统一基准就是车辆坐标系。摄像头看到的是像素，毫米波雷达给出的是距离与角度，超声波给出的是回波时间，激光雷达给出的是点云坐标——它们各自处在自己的传感器坐标系中。标定的本质，就是建立这些坐标系之间的数学映射，并把映射误差控制在泊车决策可接受的范围内。
一、四类坐标系及其物理含义
工程上通常涉及四类坐标系，它们构成一条从“世界”到“图像”的完整链路：
- 车辆坐标系（Vehicle Frame）：以车辆自身为基准，原点一般取后轴中心或车辆几何中心，X 轴指向车头，Y 轴指向左侧，Z 轴向上。它是融合结果的最终表达空间，车位坐标、障碍物位置、可行驶区域都在此坐标系下描述。
- 传感器坐标系（Sensor Frame）：每个传感器独立定义，原点在传感器安装位置，轴向随安装姿态变化。例如前视摄像头常以光心为原点，Z 轴指向镜头前方；毫米波雷达以天线阵面中心为原点。
- 图像像素坐标系（Pixel Frame）：以图像左上角为原点，u 向右、v 向下，单位是像素。它描述的是目标在成像平面上的投影位置，不含深度信息。
- 相机坐标系（Camera Frame）：介于传感器与像素之间，原点在光心，Z 轴沿光轴，是内参作用的直接对象。
四者关系可以概括为：车辆坐标系 ←外参→ 传感器坐标系 ←外参→ 相机坐标系 ←内参→ 像素坐标系。外参描述“装在哪、朝哪”，内参描述“怎么成像”。
二、内参标定与外参标定的分工
内参标定解决“相机自身如何成像”的问题，输出焦距 fx、fy，主点 cx、cy，以及径向畸变 k1、k2、k3 和切向畸变 p1、p2。常用张正友棋盘格法，采集 15～25 张不同位姿的标定板图像，通过最小化重投影误差求解。内参一旦标定，在镜头未受撞击、未更换的情况下可长期复用。
外参标定解决“传感器装在车上哪个位置、朝向如何”的问题，输出旋转矩阵 R 和平移向量 t。泊车场景中，前视、侧视、后视摄像头与超声波雷达、毫米波雷达之间的外参必须精确到厘米级和零点几度级。外参会因碰撞、拆装、悬挂变化而漂移，因此需要定期校验。
核心公式：像素坐标 → 车辆坐标的完整链路为
P_vehicle = T_vs · T_sc · K⁻¹ · [u, v, 1]ᵀ · depth
其中 T_vs 为车辆到传感器外参，T_sc 为传感器到相机外参，K 为相机内参矩阵三、联合标定流程
联合标定是把多个传感器统一到车辆坐标系的关键工序，典型流程如下：
- 场地与靶标布置：在平整地面放置棋盘格、标定杆、角反射器等，确保各传感器均能观测到足够特征。
- 数据同步采集：车辆静止或低速直行，同步录制图像、点云、雷达目标与轮速信息，时间戳对齐误差应小于 10 ms。
- 单传感器内参标定：先完成各相机内参，保证图像去畸变正确。
- 外参初值估计：利用靶标在图像与点云中的对应点，通过 PnP 或 ICP 求初始外参。
- 联合优化：以重投影误差与点云配准误差之和为目标函数，用非线性最小二乘（如 Ceres）同时优化所有外参。
- 验证与固化：用独立测试集检查残差，写入车辆配置文件并锁定版本。
四、标定误差的影响与复盘要点
标定误差不会凭空消失，它会沿着感知链路逐级放大。下表列出泊车工程中最常见的误差来源与表现：
| 误差类型 | 典型来源 | 泊车场景表现 | 控制手段 |
|---|---|---|---|
| 外参角度偏差 | 安装支架变形、碰撞 | 车位线在融合图中整体旋转偏移 | 定期靶标校验，阈值 0.3° |
| 外参平移偏差 | 传感器拆装后未复标 | 近距障碍物距离系统性偏大或偏小 | 使用标准杆复测，阈值 2 cm |
| 内参焦距误差 | 标定板姿态单一 | 车位长度估计缩放，泊入后偏左或偏右 | 多姿态采集，重投影误差 < 0.3 px |
| 时间同步误差 | 触发信号延迟 | 动态车辆融合位置拖影，误判为障碍 | 硬触发同步，偏差 < 10 ms |
| 地面假设误差 | 俯仰角标定不准 | 地面点反投影高度漂移，车位平面倾斜 | 利用地面点云在线估计俯仰 |
常见误区：很多团队把内参标定一次后就不再检查，却忽略了温度变化、镜头松动和自动对焦带来的内参漂移；也有团队只做静态标定，未验证车辆满载、胎压变化后的外参一致性。泊车工况下车辆姿态变化频繁，建议在配置文件中保留外参在线微调接口，并在每次 OTA 后执行一次快速校验。
工程建议：把标定参数当作“带版本号的配置资产”管理。每次标定记录日期、场地、温度、操作人、残差指标，并与车辆 VIN 绑定。出现泊车偏移投诉时，先比对标定版本与残差趋势，往往比直接怀疑算法更高效。
总结来看，车辆坐标系是融合的“共同语言”，传感器坐标系是各传感器的“方言”，像素坐标系是图像的“母语”。内参保证相机看得准，外参保证大家站得齐，联合标定保证整条链路对得齐。只有把标定误差控制在厘米级和零点几度级，后续的车位检测、可行驶区域分析和泊入路径规划才有可靠的空间基础。