4. 坐标系与标定
在多传感器语义地图更新系统中，来自相机、激光雷达、毫米波雷达、IMU、轮速计与GNSS的数据只有被统一到同一时空基准下，才能融合成一致的地图。坐标系与标定正是这条数据链的“地基”：它决定了像素、点云、车辆本体与全局地图之间能否正确对应。若标定存在偏差，语义标签会漂移，地图更新会出现重影、错层甚至错误覆盖。
一、核心原理：从传感器坐标系到统一地图坐标系
每个传感器都有自己的坐标系。相机通常以光心为原点，Z轴沿光轴向前；激光雷达以旋转中心为原点，X轴向前、Y轴向左、Z轴向上；IMU以惯性测量单元中心为原点；车辆本体坐标系一般以后轴中心或质心为原点。标定的本质，是求取这些坐标系之间的刚体变换，包括旋转矩阵R与平移向量t，有时还包括尺度因子与畸变参数。
外参描述传感器相对于车辆本体或另一传感器的位姿，内参描述传感器自身成像或测距模型。以相机为例，内参矩阵K包含焦距与主点，畸变系数描述径向与切向畸变；激光雷达内参则包括每束激光的垂直角、水平角修正、距离偏移与时间偏移。只有内外参同时准确，才能把像素射线与激光点云对应到同一物理点。
二、时间同步：空间对齐之前必须先对齐时间
多传感器数据若时间戳不一致，运动中的物体会产生“时空错位”。时间同步分为硬同步与软同步。硬同步通过PPS、触发线或GPS秒脉冲让传感器在同一时刻曝光或采样；软同步则基于统一时钟源，对每帧数据打上时间戳，再通过插值或运动补偿对齐到同一时刻。
常见做法是使用ROS/ROS2的message_filters做近似时间同步，或使用PTP、gPTP实现微秒级时钟同步。对于激光雷达与相机，还需考虑曝光时间、滚动快门与点云逐点时间戳。若车辆在运动，必须用IMU或轮速计进行运动补偿，否则空间对齐只是在“错误时刻”上做正确变换。
# 近似时间同步示例（ROS2）
from message_filters import ApproximateTimeSynchronizer, Subscriber
sync = ApproximateTimeSynchronizer(
[Subscriber(self, Image, '/camera/image'),
Subscriber(self, PointCloud2, '/lidar/points')],
queue_size=10, slop=0.05)
sync.registerCallback(self.callback)三、空间对齐与手眼标定
空间对齐是把不同传感器的观测投影到同一坐标系。相机与激光雷达对齐时，常用标定板、棋盘格或特定靶标，先求相机内参，再求相机与激光雷达的外参。对于车载系统，通常把激光雷达、相机、雷达都标到车辆本体坐标系，再通过车辆位姿标到全局地图。
手眼标定解决的是“传感器与运动平台”之间的相对位姿。经典形式为AX=XB：A是机械臂或车辆的运动变换，B是传感器观测到的运动，X是待求的手眼矩阵。在自动驾驶中，手眼标定常用于相机与IMU、激光雷达与IMU之间的外参估计。求解方法包括Tsai-Lenz、Daniilidis对偶四元数法，以及基于优化的非线性最小二乘。
四、多传感器联合标定方法
联合标定把多个传感器放在同一优化框架中，同时估计所有内外参与时间偏移。常见策略包括：
- 分步标定：先分别标定相机内参、激光雷达内参，再两两标定外参，最后联合优化。实现简单，但误差会累积。
- 基于靶标的联合标定：使用棋盘格、圆形靶标、V形靶标或反光柱，让相机与激光雷达同时观测同一靶标，构建重投影误差与点面距离误差。
- 基于运动的手眼标定：利用车辆或机械臂的多组运动，构建AX=XB方程，同时估计传感器与IMU外参。
- 基于自然场景的在线标定：利用车道线、杆状物、地面平面等语义特征，在行驶中持续优化外参，适合长期运行系统。
| 标定对象 | 待求参数 | 常用方法 | 典型误差来源 |
|---|---|---|---|
| 相机内参 | K、畸变系数 | 棋盘格张正友标定 | 镜头畸变、标定板不平 |
| 相机-激光雷达 | R、t | 靶标法、互信息法 | 时间不同步、靶标提取偏差 |
| 激光雷达-IMU | R、t、时间偏移 | 手眼标定、连续时间优化 | 运动激励不足、IMU零偏 |
| 多传感器联合 | 全部内外参与时间偏移 | 因子图优化、非线性最小二乘 | 初值差、外点、观测退化 |
五、案例与场景：园区语义地图更新
某园区无人配送车搭载1个16线激光雷达、1个前视相机、1个IMU与轮速计。初始标定在车库完成：相机内参用棋盘格标定，相机与激光雷达用V形靶标求外参，激光雷达与IMU用手眼标定求外参和时间偏移。运行两周后，车辆经过减速带与颠簸路面，语义地图出现车道线错位。排查发现激光雷达支架轻微松动，外参俯仰角变化约0.8°。系统通过在线标定模块，利用地面平面与杆状物特征重新估计外参，地图重影消失。
六、常见误区与复盘
- 只做空间标定，忽略时间同步：运动场景下，时间误差会直接转化为空间误差。1 ms时间偏差在10 m/s车速下对应1 cm位移，在高速下更明显。
- 把外参当作永久不变：振动、温度与碰撞都会改变外参，必须设计在线监测与重标定机制。
- 用单一场景标定结果覆盖全工况：近距离靶标标定可能在远距离失效，应覆盖不同距离、角度与光照条件。
- 忽略标定残差与不确定性：只保存R、t而不保存协方差，后续融合无法正确加权。
总结来看，坐标系与标定是语义地图更新的“隐形基础设施”。内参决定传感器自身观测的准确性，外参决定多传感器之间的空间关系，时间同步决定运动场景下的一致性，手眼标定与联合标定则把这一切统一到可优化的框架中。只有把标定当作持续工程，而非一次性配置，才能让多传感器语义地图在长期运行中保持稳定、准确与可更新。