4. 坐标系与时空对齐

在 ADAS 与域控系统中，传感器融合的前提不是“把点云和图像放在一起”，而是先回答两个问题：同一个物理点在不同传感器里被表达成什么坐标，以及不同传感器采集到它的时刻是否一致。坐标系定义解决“空间在哪里”，时间同步解决“什么时候在那里”，两者缺一不可。本节围绕车辆坐标系、传感器坐标系、世界坐标系的定义与转换关系，以及时间同步、空间标定和在线校准的工程实现展开。

一、三类坐标系的定义与层级关系

工程上通常采用右手坐标系，并明确各轴方向与旋转正方向。以常见的车辆坐标系为例：原点取在后轴中心地面投影点，x 轴指向车辆前进方向，y 轴指向车辆左侧，z 轴垂直向上。这一约定与 ISO 8855 一致，是后续所有外参的基准。传感器坐标系则各自独立：摄像头通常以光心为原点，z 轴沿光轴向前，x 轴向右，y 轴向下；毫米波雷达以天线阵面中心为原点，x 轴向前、y 轴向左；激光雷达以旋转中心为原点，x 轴向前、y 轴向左、z 轴向上。世界坐标系（也称全局坐标系）通常取车辆上电时刻的车辆坐标系，或取当地 ENU（东-北-天）坐标系，用于多帧累积和地图匹配。

三者之间通过刚体变换连接：传感器坐标系到车辆坐标系是外参 T_veh_sensor，包含旋转 R 和平移 t；车辆坐标系到世界坐标系是位姿 T_world_veh，由定位模块（GNSS/IMU/轮速）实时给出。一个传感器观测点 p_s 变换到世界坐标的完整链路为：

p_world = T_world_veh · T_veh_sensor · p_sensor
        = R_wv·(R_vs·p_s + t_vs) + t_wv

注意旋转顺序：若采用 Z-Y-X 欧拉角（yaw-pitch-roll），则 R = Rz(yaw)·Ry(pitch)·Rx(roll)。外参标定的本质就是求 R_vs 与 t_vs，而在线校准则是在车辆运行中持续修正这两个量。

二、时间同步机制

多传感器融合对时间对齐的要求通常在毫秒级，激光雷达与相机甚至要求到微秒级。工程上分三层实现：硬件同步、时钟同步和软件时间戳对齐。

硬件同步通过 PPS（秒脉冲）或同步触发线，让相机曝光、雷达帧起始、激光雷达扫描起始在同一物理时刻发生。例如域控输出 1 Hz PPS 与 gPTP 报文，相机在收到触发后开始曝光，雷达在帧起始沿打时间戳。时钟同步采用 IEEE 802.1AS（gPTP），域控作为 Grandmaster，各传感器作为 Slave，通过 Sync/Follow_Up 报文交换，将各节点时钟偏差收敛到 1 ms 以内，典型可到 100 μs。软件层则对每个数据包打上采集时刻的单调时间戳，融合节点按时间戳做插值或外推。

实际处理中，若相机帧率 30 fps、雷达 20 Hz、激光雷达 10 Hz，融合周期取 50 ms。对每个融合时刻 t_f，将各传感器数据按时间戳对齐到 t_f：位置量做线性插值，速度量做零阶保持，姿态量做球面插值（SLERP）。若某传感器最近一帧与 t_f 的时间差超过阈值（如雷达 60 ms、相机 40 ms），该帧不参与本次融合，避免引入运动畸变。

三、空间标定流程

空间标定分内参标定和外参标定。内参标定针对相机焦距、主点、畸变系数，用棋盘格或圆点靶标，通过张正友标定法求解；雷达和激光雷达的内参主要是通道间相位差、距离偏移和角度偏差，通常在产线用角反射器完成。外参标定是本节重点，目标是求传感器到车辆坐标系的 R 和 t。

以相机与激光雷达联合标定为例，常用流程如下：

1. 数据采集：车辆静止于标定场，前方布置棋盘格或已知尺寸靶标，同时采集相机图像和激光雷达点云，确保靶标在两者视野内且点云落在靶标平面上。
2. 特征提取：图像中提取棋盘格角点像素坐标 (u,v)；点云中分割出靶标平面，提取平面法向量和平面中心，或提取靶标边缘直线。
3. 初值求解：用 PnP 求相机到靶标的位姿，用平面拟合求激光雷达到靶标平面的位姿，再通过靶标坐标系中转，得到相机到激光雷达的初值 T_cl。
4. 非线性优化：以重投影误差和点到平面距离为代价函数，用 LM 算法优化 R、t。代价函数可写为：

   E = Σ ||π(K, R_cl·p_l + t_cl) - q_img||²
         + λ·Σ ||n_l·(R_cl·p_l + t_cl) - d_l||²

   其中 π 为投影函数，K 为相机内参，q_img 为图像角点，n_l、d_l 为激光雷达平面参数。
5. 结果验证：将激光雷达点云按标定结果投影到图像，观察点云是否贴合靶标边缘和地面线。若重投影误差均值小于 2 像素、点面距离均值小于 3 cm，认为标定合格。

毫米波雷达外参标定常用“直线行驶 + 静止目标”法：车辆沿直线行驶，前方放置角反射器，雷达观测到的目标方位角应随距离变化呈线性关系，通过拟合直线斜率修正安装偏航角。激光雷达与车辆坐标系标定则常用“地面平面 + 行驶方向”法：用地面点拟合平面得到俯仰和横滚，用直行时地面点云的主方向得到偏航。

四、在线校准方法

车辆行驶中的振动、温度变化、轻微碰撞都会使外参缓慢漂移，因此需要在线校准。在线校准不依赖标定场，而是利用自然场景中的约束持续估计外参修正量。常见方法有三类：

• 基于里程计的运动约束：车辆直行时，激光雷达点云的地面主方向应与车辆 x 轴一致；转弯时，各传感器估计的横摆角速度应一致。通过比较视觉里程计、雷达里程计和轮速里程计的轨迹，可估计偏航角偏差。
• 基于地标的重观测：将车道线、路沿、交通标志等作为地标，比较不同传感器对同一地标的观测位置，构建重投影误差，用扩展卡尔曼滤波在线更新外参。
• 基于互信息的配准：对相机与激光雷达，用互信息或归一化互相关衡量图像边缘与点云深度边缘的对齐程度，通过优化使互信息最大，从而修正外参。

在线校准的工程实现通常以“慢更新”方式运行：每 10 秒或每 1 km 触发一次估计，修正量限幅在 ±0.5° 和 ±5 cm 以内，避免单次误估计导致融合结果跳变。若连续多次估计的修正量同向且超过阈值，则判定为真实漂移，更新外参并写入非易失存储；若修正量在零附近波动，则保持原外参不变。

五、工程检查要点

在域控集成阶段，建议按以下顺序检查时空对齐链路：先确认所有传感器时间戳来自同一时钟源且单调递增；再验证各传感器数据在静止和直行工况下的空间一致性，例如同一目标在相机图像中的像素位置与激光雷达投影位置偏差是否小于 3 像素；最后在转弯和颠簸路段观察在线校准是否稳定，外参修正量是否收敛。只有空间和时间两条链路都稳定，后续的融合算法才有可靠输入。