📚 课程目录第4章传感器标定与同步

4. 传感器标定与同步

在无人机多传感器三维避障融合系统中，来自相机、激光雷达、毫米波雷达、IMU 与气压计的数据必须被统一到同一时空基准下，否则再优秀的融合算法也会因“各说各话”而失效。标定与同步正是建立这一基准的关键环节：标定解决“传感器看到的世界如何映射到统一坐标系”，同步解决“不同传感器在同一时刻看到的是否是同一场景”。本节围绕内参标定、外参标定、时间同步、空间对齐与实战案例展开。

一、核心原理：内参、外参与时间戳

内参描述传感器自身的成像或测量模型。以相机为例，内参矩阵 K 包含焦距 fx, fy、主点 cx, cy 与畸变系数 k1, k2, p1, p2, k3，决定像素坐标与归一化相机坐标之间的映射。激光雷达内参则包括每束激光的垂直角、水平角偏移、距离偏差与强度响应。IMU 内参主要是零偏、尺度因子与非正交误差。

外参描述传感器之间的相对位姿，通常用旋转矩阵 R 与平移向量 t 表示，即 T = [R|t]。例如相机到激光雷达的外参 T_cam_lidar，把激光点云投影到图像平面；IMU 到相机的 T_cam_imu，把惯性预积分轨迹与视觉轨迹对齐。外参一旦标定错误，点云与图像会出现明显错位，直接影响障碍物深度估计。

时间同步包含两个层面：一是硬件触发同步，通过 PPS、GPS 秒脉冲或触发线让相机曝光、雷达扫描与 IMU 采样在同一时钟沿启动；二是软件时间戳对齐，利用 ROS 的 message_filters、时间插值或滑动窗口，把不同频率、不同延迟的数据对齐到同一时间基准。时间偏差 10 ms，在 10 m/s 飞行速度下就会带来 10 cm 的空间误差，对近距离避障不可忽视。

标定与同步的最终目标：任意时刻 t，任意传感器 i 的观测都能通过 T_i 与 t 映射到统一的世界坐标系 W，形成时空一致的三维障碍物表达。

二、方法流程：从单传感器到多传感器联合标定

完整的标定流程通常分为四步：

单传感器内参标定：相机使用棋盘格或 AprilTag，采集 20～30 张不同姿态图像，通过张正友标定法求解 K 与畸变；激光雷达使用房间墙面、地面等平面约束，优化每束激光的角度与距离修正；IMU 使用 Allan 方差分析噪声密度与随机游走。
外参初值获取：相机-雷达可用棋盘格在点云与图像中的对应角点，通过 PnP 或 ICP 求初值；相机-IMU 可用 Kalibr 工具，基于连续时间轨迹优化。
联合优化：把内参、外参与时间偏移 t_d 作为待优化变量，构建重投影误差、点面距离误差与 IMU 预积分误差，使用 Ceres 或 g2o 求解。
验证与固化：用未参与标定的数据检查重投影误差、点云-图像边缘对齐度与轨迹一致性，达标后写入配置文件并锁定。

时间同步的实现方式可按精度排序：硬件 PPS 触发优于硬件时间戳，硬件时间戳优于软件时间戳，软件时间戳优于消息到达时间。工程上常采用“硬件触发 + 软件插值”的组合：相机与雷达由同一 PPS 触发，IMU 以 200 Hz 采样，融合节点用线性插值把 IMU 对齐到图像时间戳。

# 相机内参标定示例（OpenCV）
python3 calibrate.py \
  --image_path ./calib_images \
  --pattern_size 9x6 \
  --square_size 0.025 \
  --output camera_intrinsics.yaml

# 相机-IMU 联合标定（Kalibr）
kalibr_calibrate_imu_camera \
  --bag calib.bag \
  --cam camchain.yaml \
  --imu imu.yaml \
  --target april_6x6.yaml

三、空间对齐：把点云、图像与惯性统一到同一坐标系

空间对齐的核心是坐标变换链。假设世界坐标系为 W，无人机机体坐标系为 B，相机坐标系为 C，激光雷达坐标系为 L，则点云中的点 p_L 投影到图像像素 u 的过程为：

p_C = T_C_L * p_L
p_norm = p_C / p_C.z
u = K * distort(p_norm)

其中 T_C_L 是雷达到相机的外参。若存在 IMU，还需通过 T_B_C 与 T_B_L 把观测统一到机体坐标系，再结合姿态估计得到世界坐标。空间对齐的误差来源包括：外参标定误差、时间不同步导致的运动畸变、雷达点云去畸变不彻底、相机卷帘快门效应。对于高速旋转的无人机，雷达一帧扫描期间机体已发生明显位移，必须用 IMU 数据做运动补偿，否则点云会“拖尾”。

空间对齐检查技巧：在静态场景中，把点云按深度着色投影到图像，观察物体边缘是否与图像边缘重合；若出现系统性偏移，优先检查外参平移量；若边缘随距离变化而错位，优先检查旋转量或时间偏移。

四、标定实战案例：室内避障无人机的多传感器标定

某室内避障无人机搭载双目相机、16 线激光雷达与 IMU，目标是实现 0.5～8 m 范围内的三维障碍物检测。标定过程如下：

相机内参：使用 9×6 棋盘格，方格 25 mm，采集 28 张图像，重投影误差从 0.42 px 降至 0.18 px。
雷达内参：在 3 m×4 m 平整墙面前采集 200 帧，优化后墙面厚度标准差从 3.1 cm 降至 1.2 cm。
相机-雷达外参：用带反光标记的标定板，同时提取图像角点与点云角点，ICP 初值 + 联合优化，最终点云-图像边缘平均偏差 1.6 px。
时间同步：相机 30 Hz 硬件触发，雷达 10 Hz，IMU 200 Hz；以相机时间戳为基准，IMU 线性插值，雷达帧内用 IMU 做去畸变。
验证：在 2 m/s 飞行速度下，对 3 m 处直径 0.3 m 的障碍物，深度估计误差小于 4 cm，满足避障阈值要求。

标定项	工具/方法	关键指标	常见问题
相机内参	棋盘格 + 张正友标定	重投影误差 < 0.3 px	图像太少、姿态单一
雷达内参	平面约束优化	墙面厚度 < 2 cm	标定场景不平整
相机-雷达外参	标定板 + ICP + 联合优化	边缘偏差 < 2 px	时间不同步引入运动误差
相机-IMU 外参	Kalibr 连续时间优化	重投影误差 < 0.5 px	激励不足、IMU 噪声大
时间同步	PPS + 软件插值	时间偏差 < 1 ms	时钟漂移、触发线干扰

五、误区与复盘

常见误区一：只标一次就长期使用。无人机振动、碰撞、温度变化都会导致外参漂移，建议每次飞行前做快速在线校验，或至少每周复标一次。误区二：忽视时间偏移。很多团队把外参调得很准，却忽略相机与雷达之间 20～30 ms 的固定延迟，导致高速运动时点云与图像错位。误区三：用训练数据标定、用测试数据验证时混用同一场景，造成过拟合。正确做法是标定场景与验证场景分离，并在不同距离、不同光照下检查。

复盘时建议记录每次标定的原始数据、优化残差、时间偏移估计值与验证结果，形成可追溯的标定档案。只有把内参、外参、时间同步三者作为一个整体来管理，多传感器三维避障融合才能获得稳定可靠的空间感知基础。