4、激光雷达与相机联合标定：联合标定原理、PnP求解外参、手眼标定法、标定板设计

各位同学，欢迎来到联合标定这一章。

说实话，在自动驾驶感知系统里，传感器融合是核心，而联合标定就是融合的基石。你想想看，如果激光雷达和相机各自看到的“世界”对不上号，那后面的目标检测、跟踪、融合全都是空中楼阁。我当年刚入行时，就吃过这个亏——标定参数差了一点点，结果融合出来的点云和图像总是错位，排查了整整两天才发现是外参矩阵里一个旋转角差了0.1度。嗯，从那以后，我对标定这件事就格外较真。

4.1 联合标定原理：为什么要做这件事？

说白了，联合标定就是求解激光雷达坐标系和相机坐标系之间的刚体变换关系。这个关系用一个4x4的外参矩阵来表示，包含旋转和平移两部分。

数学上，我们这样描述：

激光雷达坐标系下的一个点：P_lidar = (x, y, z, 1)^T
相机坐标系下的对应点：P_cam = (X, Y, Z, 1)^T
它们之间的关系：P_cam = T · P_lidar

这里的 T 就是我们要求解的外参矩阵。它由旋转矩阵 R (3x3) 和平移向量 t (3x1) 组成。

我个人习惯把标定过程分成两步：

数据采集：让激光雷达和相机同时观测同一个标定物，获取对应的3D点和2D点。
参数求解：利用这些对应关系，解算出最优的 R 和 t。

这里有个关键点——激光雷达的点云是稀疏的，而相机的图像是稠密的。怎么找到它们之间的对应关系？这就是标定板设计的用武之地了，我们后面会细说。

核心思想：联合标定本质上是一个最小化重投影误差的优化问题。我们让激光雷达点投影到图像上，然后调整外参，让投影点和图像上检测到的对应点尽可能重合。

4.2 PnP求解外参：从3D到2D的数学魔法

PnP（Perspective-n-Point）问题，说白了就是：已知n个3D点在激光雷达坐标系下的坐标，以及它们在图像上的2D投影点，求解相机的外参。

我建议你记住这个核心公式：

s · [u, v, 1]^T = K · [R | t] · [X, Y, Z, 1]^T

其中：

s 是尺度因子
[u, v] 是图像像素坐标
K 是相机内参矩阵（已知）
[R | t] 是外参矩阵（待求）
[X, Y, Z] 是激光雷达坐标系下的3D点

实际工程中，我们常用的是 EPnP 算法。为什么？因为它效率高、稳定性好。我记得有一次在实车上做标定，用传统的DLT算法，遇到噪声大的点就发散，换成EPnP后一次就收敛了。

代码实现大致是这样的：

# 使用OpenCV的solvePnP
import cv2
import numpy as np

# 3D点（激光雷达坐标系）
object_points = np.array([
    [0.5, 0.0, 0.0],   # 标定板左上角
    [0.5, 0.5, 0.0],   # 标定板右上角
    [0.0, 0.5, 0.0],   # 标定板右下角
    [0.0, 0.0, 0.0]    # 标定板左下角
], dtype=np.float32)

# 2D点（图像坐标系）
image_points = np.array([
    [320, 240],
    [420, 240],
    [420, 340],
    [320, 340]
], dtype=np.float32)

# 相机内参
camera_matrix = np.array([
    [600, 0, 640],
    [0, 600, 480],
    [0, 0, 1]
], dtype=np.float32)

dist_coeffs = np.zeros((4, 1))  # 假设无畸变

# 求解PnP
success, rvec, tvec = cv2.solvePnP(
    object_points, 
    image_points, 
    camera_matrix, 
    dist_coeffs,
    flags=cv2.SOLVEPNP_EPNP
)

# 将旋转向量转为旋转矩阵
R, _ = cv2.Rodrigues(rvec)
print("旋转矩阵 R:\n", R)
print("平移向量 t:\n", tvec)

小技巧：实际标定时，不要只用4个点。我建议至少用8-12个点，并且这些点要分布在标定板的不同区域。点太少或者分布太集中，求解结果容易不稳定。

4.3 手眼标定法：另一种思路

手眼标定法，这个名字听起来有点奇怪。它其实是从机器人领域借鉴过来的方法。在机器人里，我们要求解相机和机械臂末端之间的变换关系。在自动驾驶里，我们把它类比为求解激光雷达和相机之间的变换关系。

手眼标定的核心方程是：AX = XB

什么意思呢？

A：激光雷达在不同位姿下的变换矩阵
B：相机在不同位姿下的变换矩阵
X：激光雷达和相机之间的固定变换（就是我们要的外参）

你想想看，如果我们让传感器系统做多次运动，每次运动前后，激光雷达和相机各自的位姿变化是已知的（可以通过ICP或视觉里程计得到），那么它们之间就存在这个约束关系。

我个人觉得手眼标定法的好处是：

不需要标定板：在自然场景中就能做
可以在线标定：车辆行驶过程中也能持续优化
鲁棒性好：对噪声和异常点不敏感

但缺点也很明显——需要传感器系统有足够的运动。如果车辆一直停着不动，那这个方法就玩不转了。

注意：手眼标定法对运动质量要求很高。我曾经遇到过一个问题——车辆只在直线上运动，结果平移向量的z分量始终解不出来。后来加了转弯和上下坡的运动，才把全部参数标定好。所以，采集数据时一定要让车辆做六自由度的运动。

4.4 标定板设计：细节决定成败

标定板是联合标定的“翻译官”。它要把激光雷达的3D点和相机的2D点对应起来。

我见过很多种标定板设计，这里说几种常见的：

类型	原理	优点	缺点
棋盘格	角点检测 + 反射率差异	角点检测成熟，精度高	激光雷达对黑色区域反射弱
圆点阵列	圆心检测 + 反射率差异	圆心定位精度高	需要保证圆点完整可见
反射率标定板	高反射率区域 vs 低反射率区域	激光雷达点云特征明显	对光照敏感
3D立体标定板	多个平面组合，提供深度信息	3D-2D对应关系更丰富	制作成本高，携带不便

我个人最推荐的是 棋盘格 + 高反射率边框 的组合设计。为什么？

棋盘格的角点检测算法非常成熟，OpenCV直接支持
高反射率边框让激光雷达能清晰检测到标定板的边界
两者结合，3D点和2D点的对应关系就建立起来了

标定板的尺寸也有讲究。我建议：

棋盘格边长：10cm-20cm（根据传感器安装高度调整）
棋盘格数量：6x8 或 8x10（格子数越多，角点越多，精度越高）
边框宽度：至少5cm（太窄了激光雷达点云检测不到）

避坑指南：我曾经用过一个标定板，棋盘格是打印在普通A4纸上的。结果激光雷达打上去，黑色格子几乎不反射，点云里根本看不到标定板的轮廓。后来换了亚光喷墨打印 + 高反射率边框，效果立竿见影。所以，标定板的材质和打印质量真的很重要。

好了，关于联合标定的核心内容就讲到这里。标定这件事，理论说难不难，说简单也不简单。关键是要理解背后的几何原理，然后在实践中不断积累经验。你动手做几次，踩几个坑，自然就熟练了。

最后提醒一句：标定不是一劳永逸的。传感器在车辆行驶中可能会发生微小的位移，建议每隔一段时间重新标定一次，或者设计在线标定算法持续优化。我见过太多因为标定参数漂移导致融合效果变差的案例了。

📚 课程目录

4、激光雷达与相机联合标定：联合标定原理、PnP求解外参、手眼标定法、标定板设计

4.1 联合标定原理：为什么要做这件事？

4.2 PnP求解外参：从3D到2D的数学魔法

4.3 手眼标定法：另一种思路

4.4 标定板设计：细节决定成败