2、传感器基础与坐标系：常见传感器（相机、激光雷达、IMU、GPS/RTK）的工作原理、输出特性，以及世界坐标系、车体/机体坐标系、传感器坐标系的定义与转换关系
多传感器融合系统的第一步，不是算法，而是深刻理解每个传感器“看到世界的方式”。相机输出的是像素矩阵，激光雷达输出的是三维点云，IMU输出的是加速度与角速度，GPS/RTK输出的是经纬度与高程。这些数据在物理含义、采样频率、噪声特性、参考基准上截然不同。要将它们统一到同一个时空框架下进行融合，就必须先建立清晰的坐标系体系，并掌握坐标系之间的刚性变换关系。本章从传感器工作原理与输出特性出发，系统梳理世界坐标系、车体/机体坐标系、传感器坐标系的定义，并给出完整的转换链路与标定逻辑。
上图展示了多传感器融合中的核心坐标系层级关系。世界坐标系（W系）是全局参考基准，车体坐标系（B系）是车辆运动状态的载体，传感器坐标系（S系）则是各传感器原始数据的直接参考。三者之间通过刚体变换矩阵 T_WB 和 T_BS 串联，构成完整的空间变换链路。下面逐一展开。
2.1 常见传感器的工作原理与输出特性
2.1.1 相机（Camera）
核心原理：相机通过镜头将三维世界投影到二维图像平面，本质是透视投影模型。针孔模型下，空间点 P(X,Y,Z) 投影到像素坐标 (u,v) 的关系为：
Z_c · [u, v, 1]ᵀ = K · [R|t] · [X, Y, Z, 1]ᵀ其中 K 为内参矩阵（焦距 fx, fy，主点 cx, cy），[R|t] 为外参（相机在世界坐标系中的位姿）。实际镜头存在径向畸变（k1, k2, k3）和切向畸变（p1, p2），需在标定中求解。
输出特性：
- 数据形式：RGB/BGR 图像，分辨率从 720p 到 8MP 不等，像素值 8bit 或 12bit。
- 采样频率：典型 10-60 Hz，高帧率相机可达 120 Hz。
- 噪声特性：光照敏感，暗光下噪声增大；运动模糊与曝光时间相关；卷帘快门存在果冻效应。
- 时间基准：图像曝光中点时间戳，而非帧读出完成时刻。
2.1.2 激光雷达（LiDAR）
核心原理：激光雷达通过发射激光脉冲并测量反射时间差（TOF）或调频连续波（FMCW）来测距。机械式雷达通过旋转电机带动激光器扫描，固态雷达则通过光学相控阵或 MEMS 振镜实现扫描。每个激光点包含三维坐标 (x,y,z) 和反射强度 i。
输出特性：
- 数据形式：点云（Point Cloud），每帧包含数万到数百万个点，每个点有 x,y,z,i 四个属性。
- 采样频率：5-20 Hz，典型 10 Hz。
- 噪声特性：距离越远噪声越大；雨雾天气存在散射噪声；低反射率物体（黑色车辆）点云稀疏。
- 时间基准：机械式雷达各点云有不同时间戳（旋转角度不同），需按点或按扫描行插值。
2.1.3 IMU（惯性测量单元）
核心原理：IMU 包含三轴加速度计和三轴陀螺仪。加速度计测量比力（Specific Force），陀螺仪测量角速度。通过积分可获得速度、位置和姿态，但积分漂移随时间累积。
输出特性：
- 数据形式：三轴加速度 (ax, ay, az) 单位 m/s²，三轴角速度 (wx, wy, wz) 单位 rad/s。
- 采样频率：100-1000 Hz，典型 200 Hz。
- 噪声特性：包含零偏（Bias）、随机游走（Random Walk）、尺度因子误差、交轴耦合误差。
- 时间基准：IMU 时间戳通常由硬件时钟直接产生，延迟极小（微秒级）。
2.1.4 GPS/RTK（全球定位系统/实时动态差分）
核心原理：GPS 通过接收至少 4 颗卫星信号，利用伪距测量解算接收机位置。RTK 通过地面基准站发送差分改正数，消除电离层、对流层延迟和卫星钟差，实现厘米级定位。
输出特性：
- 数据形式：经纬度（WGS84）、高程、速度、航向角（双天线 RTK）。
- 采样频率：1-20 Hz，典型 10 Hz。
- 精度：单点 GPS 3-5 m，RTK 1-3 cm + 1ppm（基线距离）。
- 时间基准：GPS 时间戳基于接收机内部时钟，需与系统主时钟同步。
2.2 坐标系定义与转换关系
2.2.1 世界坐标系（W系）
世界坐标系是全局参考基准，用于描述车辆在环境中的位置。常用形式：
| 类型 | 定义 | 适用场景 |
|---|---|---|
| ENU（东-北-天） | X向东，Y向北，Z向上 | 自动驾驶局部导航 |
| UTM（通用横轴墨卡托） | 平面投影坐标，带号+北向 | 地图构建、路径规划 |
| ECEF（地心地固） | 原点在地心，Z指向北极 | 卫星定位解算 |
| 局部参考系 | 自定义原点（如起点） | 园区、矿区等封闭场景 |
实际工程中，通常将 GPS 输出的 WGS84 经纬度转换为 UTM 或 ENU 局部坐标，以米为单位进行后续计算。
2.2.2 车体/机体坐标系（B系）
车体坐标系固连于车辆，描述车辆自身的运动状态。常见定义：
- 前-左-上（FLU）：X向前，Y向左，Z向上。常用于乘用车。
- 右-前-上（RFU）：X向右，Y向前，Z向上。常用于机器人平台。
- 原点位置：后轴中心（车辆动力学模型）、IMU 安装位置（组合导航）、质心（动力学控制）。
2.2.3 传感器坐标系（S系）
每个传感器都有独立坐标系：
- 相机：Z轴沿光轴向前，X向右，Y向下（图像坐标系）。
- 激光雷达：Z轴向上，X向前（或按厂商定义），Y向左。
- IMU：X向前，Y向左，Z向上（通常与车体一致）。
- GPS天线：相位中心为原点，无固定朝向。
2.2.4 坐标转换关系
刚体变换矩阵 T ∈ SE(3)：
T = [R t]
[0 1]其中 R 为 3×3 旋转矩阵，t 为 3×1 平移向量。转换关系：
- 传感器→车体：P_B = R_BS · P_S + t_BS（外参标定获得）
- 车体→世界：P_W = R_WB · P_B + t_WB（组合导航提供）
- 传感器→世界：P_W = R_WB · (R_BS · P_S + t_BS) + t_WB
逆变换：T⁻¹ = [Rᵀ | -Rᵀ·t]
2.3 外参标定方法
2.3.1 相机-激光雷达标定
核心原理：利用标定板（棋盘格或反光板）同时被相机和激光雷达观测，建立 3D-2D 对应点，通过 PnP 求解外参。
操作流程：
- 放置标定板于不同位置和角度，采集 10-20 组数据。
- 相机提取棋盘格角点，激光雷达提取标定板平面点云。
- 建立 3D 点（激光雷达）到 2D 点（图像）的对应关系。
- 使用 PnP + RANSAC 求解 R, t。
- 重投影误差评估，残差小于 1 像素为合格。
2.3.2 激光雷达-IMU 标定
利用连续帧点云配准（ICP/NDT）估计运动，与 IMU 积分姿态对齐，构建优化问题求解外参。常用工具：LiDAR-IMU Calibration（如 lidar_align）。
2.3.3 相机-IMU 标定
使用 Kalibr 工具，通过相机观测标定板 + IMU 积分，联合优化求解外参和时延。关键步骤：
# Kalibr 标定命令示例
kalibr_calibrate_imu_camera \
--target aprilgrid.yaml \
--cam camchain.yaml \
--imu imu.yaml \
--bag data.bag \
--time-calibration2.4 实战案例：多传感器外参标定与坐标转换闭环
场景：某园区无人配送车，搭载前视相机（分辨率为 1920×1080）、32 线激光雷达（10 Hz）、消费级 IMU（200 Hz）和 RTK-GPS（10 Hz）。车辆在园区内以最高 20 km/h 行驶，需实现厘米级定位与障碍物检测。
初始现象：融合算法输出的障碍物位置与真实位置偏差约 30-50 cm，且车辆转弯时偏差增大。
分析过程：
- 检查时间同步：发现相机时间戳为帧读出完成时刻，未补偿曝光中点，导致 20 km/h 下产生约 5 cm 位置误差。
- 检查外参标定：使用重投影误差评估，发现相机-激光雷达外参误差约 2 像素，对应 10 m 处约 10 cm 偏差。
- 检查杆臂补偿：GPS 天线位于车顶，与 IMU 中心存在 0.8 m 水平距离和 0.5 m 垂直距离，未补偿导致转弯时定位偏差。
解决步骤：
- 修正相机时间戳为曝光中点，误差降至 2 cm。
- 重新进行相机-激光雷达外参标定，采集 15 组数据，重投影误差降至 0.8 像素。
- 测量 GPS 天线与 IMU 的杆臂，在组合导航中配置补偿参数。
- 使用标定后的外参将激光雷达点云投影到图像，验证对齐效果。
结果对比：融合定位精度从 30-50 cm 提升至 5-8 cm，满足园区配送需求。
复盘经验：外参标定不是一次性工作，车辆碰撞、传感器松动、温度变化都可能导致外参漂移。建议定期（每月或每 1000 km）重新标定，并开发在线外参监测算法。