第4章多传感器标定

第4章：多传感器标定——激光雷达与相机联合标定、IMU与激光雷达标定、标定工具与实战流程

各位同学，欢迎来到多传感器标定这一章。

说实话，标定这件事，在很多人眼里是「脏活累活」。但我个人觉得，它恰恰是高精地图最核心的基石。传感器没对齐，后面建图、定位全是白搭。我见过太多项目，算法调得天花乱坠，最后发现是标定参数偏了0.1度，整个地图就歪了。

这一章，咱们就把激光雷达、相机、IMU这三兄弟的标定讲透。我会结合我实际踩过的坑，给你一套能直接上手的实战流程。

4.1 为什么需要多传感器标定？

说白了，每个传感器都有自己的坐标系。激光雷达有激光坐标系，相机有相机坐标系，IMU有IMU坐标系。它们各自看世界，但我们需要把它们看到的融合到一起。

举个例子：激光雷达扫到一个障碍物，坐标是(10, 2, 0)。相机在同一时刻拍到同一个障碍物，像素坐标是(320, 240)。这两个数据如果不做坐标变换，你根本不知道它们说的是同一个东西。

标定的目的，就是求出这些坐标系之间的旋转矩阵R和平移向量t。有了这个，我们才能把激光点投影到图像上，或者把IMU的加速度转换到车体坐标系。

核心公式：

P_camera = R * P_lidar + t

其中P_lidar是激光雷达坐标系下的点，P_camera是相机坐标系下的点。

4.2 激光雷达与相机联合标定

这是高精地图里最常用的标定组合。激光雷达提供精确的3D点云，相机提供丰富的纹理信息。两者结合，才能做出带语义的高精地图。

4.2.1 标定原理

激光雷达和相机的联合标定，本质上是找一个3D-2D的投影关系。激光点云中的每个点，通过外参投影到图像上，应该落在对应的像素位置。

我习惯用棋盘格来做标定物。为什么？因为棋盘格的角点检测非常成熟，OpenCV里直接有函数。激光雷达扫到棋盘格上，能提取出棋盘格平面的点云；相机拍到棋盘格，能提取出角点的像素坐标。这两组数据一对应，就能解出外参。

4.2.2 实战流程

准备标定板：建议用1.2m x 0.9m的棋盘格，格子大小10cm。太小了激光点云稀疏，检测不准。
数据采集：把标定板放在不同位置、不同角度，至少采集10-15组数据。注意：标定板要完整出现在相机视野和激光雷达扫描范围内。
提取特征：对每帧图像，用OpenCV的findChessboardCorners提取角点。对每帧点云，用RANSAC拟合棋盘格平面，提取平面上的点。
优化求解：用PnP算法或非线性优化，最小化重投影误差。

我的经验：采集数据时，标定板距离传感器不要太远。我建议3-5米最佳。太远了激光点稀疏，角点检测容易失败。太近了视野受限，标定参数容易过拟合。

4.2.3 代码示例（Python）

import cv2
import numpy as np
from scipy.optimize import minimize

# 假设已经提取了棋盘格角点的3D点云和2D像素坐标
# points_3d: Nx3, points_2d: Nx2

def project_points(points_3d, rvec, tvec, camera_matrix, dist_coeffs):
    """将3D点投影到2D图像平面"""
    points_2d_proj, _ = cv2.projectPoints(
        points_3d, rvec, tvec, camera_matrix, dist_coeffs
    )
    return points_2d_proj.reshape(-1, 2)

def reprojection_error(params, points_3d, points_2d, camera_matrix, dist_coeffs):
    """计算重投影误差"""
    rvec = params[:3]
    tvec = params[3:6]
    points_2d_proj = project_points(points_3d, rvec, tvec, camera_matrix, dist_coeffs)
    error = np.mean(np.linalg.norm(points_2d - points_2d_proj, axis=1))
    return error

# 初始猜测（通常用PnP初始化）
_, rvec_init, tvec_init, _ = cv2.solvePnPRansac(points_3d, points_2d, camera_matrix, dist_coeffs)

# 非线性优化
result = minimize(
    reprojection_error,
    np.concatenate([rvec_init.flatten(), tvec_init.flatten()]),
    args=(points_3d, points_2d, camera_matrix, dist_coeffs),
    method='Levenberg-Marquardt'
)

print(f"优化后的旋转向量: {result.x[:3]}")
print(f"优化后的平移向量: {result.x[3:6]}")

注意：我曾经遇到过一个问题——优化一直不收敛。后来发现是相机内参没标准。内参不准，外参怎么优化都是错的。所以建议先单独标定相机内参，再联合标定外参。

4.3 IMU与激光雷达标定

IMU和激光雷达的标定，很多人容易忽略。但做实时更新时，IMU的加速度和角速度数据，是激光雷达运动畸变校正的关键。

4.3.1 标定原理

IMU和激光雷达的标定，主要是求两个坐标系之间的旋转和平移。但IMU输出的是角速度和加速度，激光雷达输出的是点云。这两者怎么对应？

我常用的方法是：让传感器做特定的运动，比如绕某个轴旋转。IMU能测出角速度，激光雷达通过连续帧的点云配准（比如ICP），也能算出角速度。这两组角速度一对比，就能解出旋转关系。

4.3.2 实战流程

固定传感器：把激光雷达和IMU刚性固定在一个平台上。
采集运动数据：手持平台做6个自由度的运动，包括旋转和平移。注意：运动要足够丰富，但不要太剧烈，避免IMU饱和。
预处理：对IMU数据进行积分，得到姿态变化。对激光雷达连续帧做ICP，得到相对位姿变化。
联合优化：构建一个图优化问题，同时优化IMU和激光雷达的外参。

避坑指南：我曾经犯过一个错误——IMU和激光雷达的时间戳没对齐。IMU的采样频率是200Hz，激光雷达是10Hz。如果不做时间同步，标定结果会差很多。建议用硬件触发或软件插值来对齐时间戳。

4.4 标定工具与实战工具推荐

说实话，自己从头写标定工具太累了。我建议直接用现成的开源工具，然后根据自己的传感器做微调。

工具名称	适用场景	优点	缺点
Kalibr	相机-IMU标定	支持多相机、多IMU，精度高	配置复杂，依赖ROS
lidar_camera_calibration	激光雷达-相机标定	基于棋盘格，操作简单	对棋盘格尺寸敏感
LiDAR_IMU_Calib	激光雷达-IMU标定	支持连续运动标定	需要手动调参
OpenCalib	多传感器通用标定	集成多种算法，可视化好	文档较少

我个人最常用的是Kalibr做相机-IMU标定，然后用lidar_camera_calibration做激光雷达-相机标定。这两个工具配合，基本能覆盖高精地图的标定需求。

4.5 标定质量评估

标定完了，怎么知道标得好不好？我一般看三个指标：

重投影误差：激光点投影到图像上，和实际像素位置的偏差。一般小于2个像素就算不错。
视觉一致性：把标定板放在不同位置，看投影误差是否稳定。如果某个位置误差突然变大，说明标定参数有局部过拟合。
泛化能力：用没参与标定的数据做验证。如果误差和训练数据差不多，说明标定参数泛化好。

我的标准：

重投影误差 < 1.5像素 → 优秀

重投影误差 1.5-3像素 → 可用

重投影误差 > 3像素 → 重新标定

4.6 实战中的常见问题与解决方案

嗯，这里我总结几个我实际遇到过的问题，希望能帮你少走弯路。

问题1：标定结果不稳定——每次标定出来的参数都不一样。原因通常是数据量不够或运动不够丰富。我建议至少采集20组不同姿态的数据。
问题2：重投影误差很小，但实际效果差——这往往是过拟合了。标定板只在某个区域出现，导致参数只对那个区域有效。解决方案：让标定板覆盖整个视野。
问题3：IMU和激光雷达标定后，运动畸变校正效果差——大概率是时间同步没做好。我建议用硬件触发信号，或者用PTP协议做时间同步。

重要提醒：标定不是一劳永逸的。传感器在车辆行驶过程中，可能会因为振动、温度变化而发生微小的位移。我建议每3个月或每行驶5000公里重新标定一次。特别是在发生碰撞或更换传感器后，必须重新标定。

好了，这一章的内容就到这里。多传感器标定是门手艺活，光看理论不够，一定要动手做。下一章我们会讲高精地图的数据采集与预处理，到时候会用到今天标定的参数。记得把标定结果保存好。

📚 课程目录