相机与LiDAR联合标定：给机器人的“眼睛”和“耳朵”牵线

可视化演示

演示相机与LiDAR坐标系刚体变换、棋盘格特征点对应匹配及重投影误差非线性优化的完整标定流程交互式可视化。

标定必要性

老陈师傅的新徒弟小周第一次独立做面包，他左手端着盐罐，右手举着手机拍秤上的读数——“师傅，我既要看盐粒大小，又要看称重数字，忙不过来！”老陈摇头：“你得让盐罐和秤互相知道对方的位置。”在机器人世界里，相机负责看“颜色和纹理”，LiDAR负责量“距离和形状”，两者天生互补：RGB图像像彩色的秤盘刻度，点云像那捧粗粝的海盐颗粒。但二者各自为政时，数据无法融合——同一辆车在图像左下角，在点云里却在正前方。

于是，工程师们要给这对“冤家”定一门亲事：求出一个外参矩阵，描述LiDAR坐标系到相机坐标系的旋转和平移关系。这个矩阵就像老陈抄在围裙兜里的“盐粒-刻度换算表”。没有它，多模态数据就是各说各话的“罗生门”；有了它，点云才能被投影到图像上，给每个像素补上距离。

标定的最终目标，是让同一物体的图像像素与点云三维点精确对齐。从数学看，外参包括一个旋转矩阵 \( \mathbf{R} \) 和一个平移向量 \( \mathbf{t} \)，它们把LiDAR看到的点搬到相机坐标系下：

找到了这张“换算表”，机器人的眼睛和耳朵才能对同一件事物达成共识——这正是后续一切融合算法的地基。

空间坐标变换

有了“换算表”还不够，老陈还得教会小周怎么看懂它。换算表上写着：“盐罐旋转30度，再向右挪两格，秤盘刻度的读数就是盐的实际重量。”这个“旋转+挪动”，在数学里叫刚体变换。它守住的规矩是：物体的形状不变，只是位置和姿态变了。就像你拿起一罐盐，从灶台左侧放到右侧，盐粒之间的相对距离一点没变。

把刚体变换写成齐次坐标形式，好处是把“旋转”和“平移”两笔账合成一笔算。齐次坐标给三维点 \( (x, y, z) \) 添上第4个分量1，于是变换矩阵变成一个 \( 4\times 4 \) 的方阵：

学会读这张“换算表”，工程师就能把任何LiDAR点搬到相机坐标系里。可问题来了：这张表上的旋转角度和平移距离，一开始谁也不知道——必须靠“做实验”标定出来。

数据采集与同步

老陈的换算表不是拍脑袋写的，而是连着三天，每天早上六点，用同一罐盐、同一台秤，反复称量、记录、比对总结出来的。联合标定也一样：数据采集是整套流程的“原材料”。你在面包房里不同角落举起盐罐，让秤和罐子同时被看到——对应到机器人世界，就是布置一个布满了角点、边缘、平面的标定场景，让相机和LiDAR能同时观测到丰富的特征。

这一步最容易被忽视的坑是时间同步。相机按30帧每秒拍“秤盘特写”，LiDAR按10赫兹扫“盐罐轮廓”，两个数据流若不在同一时刻采样，等于老陈先撒盐、后看秤——算出来的换算表当然不准。工程上通常用硬件触发或时间戳插值，确保每一对“图像-点云”都指向同一瞬间：

采到“同频”的数据后，还得保证场景里有足够多、足够明显的“盐粒”和“刻度”。于是下一步，就是在图像里找刻度线、在点云里找盐粒的尖角——特征提取开始了。

特征点提取

老陈的换算表上画满了圈圈：盐罐棱角处标一颗星，秤盘刻度5克处画一个三角。他说：“我得先认出两边各有什么标志物，才能对账。”特征点提取就是干这个的：在相机图像里找到“刻度线交点”（图像角点），在LiDAR点云里找到“盐粒尖角”（三维角点或边缘点），为后续“对账”提供锚点。

图像侧常用的工具是棋盘格标定板，它的角点纹理清晰、亚像素级可重复检测；点云侧则利用标定板与背景的深度跳变，拟合平面并求交线、交角点。这些特征点就像换算表上的“记号笔”。特征的质量直接决定标定精度——一堆模糊、重复、分布集中的特征，就像刻度磨损的秤，怎么称都不准。直观地说，特征分布越分散、越尖锐，外参求解的约束就越强：

当两边都圈好了“标记”，下一步就是最核心的动作：把图像上的“三角”和点云里的“星星”一一对上，这一步叫对应关系建立。

对应关系建立

老陈把盐罐放到秤盘上，眼睛盯着罐子的尖角，手在秤上找到对应的刻度位置。这个“盯对”的动作，在标定流程里叫对应匹配。相机图像上的2D特征点，和LiDAR点云里的3D特征点，并非天然写着“我是谁”——必须先猜出它们谁是“同一个人”，才能用它们的配对关系反推外参。

最简单的猜法，是最近邻搜索：把LiDAR点先粗略投影到图像平面，找离它最近的图像角点作为候选配对。但盐罐尖角和秤盘刻度的“距离”，有时会被噪音、遮挡干扰，出现“认错人”的情况。于是标定流程里会加入奇异点剔除：把距离过远、角度不合理的配对当作“看走眼”扔出局。同时还要讲究匹配策略，比如多帧累计验证、交叉匹配，确保配对不只靠一次运气。数学上，对应关系可以表达为一个匹配代价最小化问题：

好，现在盐粒和刻度“对上号”了。接下来，老陈要动笔填换算表上的数字了——这一步，就是非线性优化求解外参。

外参求解与验证

老陈面前摊着几十条“盐粒-刻度”配对记录，他要从中反推出“旋转30度、平移两格”最准的数值。直接用两根记录解方程，误差太大；于是他反复微调角度和距离，让所有记录同时逼近——这正是非线性优化的思想：以重投影误差为目标函数，不断调整外参矩阵 \( \mathbf{T} \)，使配对点的投影误差整体最小：

求解这个优化问题，工程上常用梯度下降法：从初值出发，沿着误差下降最快的方向逐步修正参数。其更新规则可写为：

拿到收敛后的 \( \mathbf{T}^* \)，还不能急着庆祝。老陈会再烤一炉面包验证换算表准不准——工程师也要用重投影误差来做评估：把测试集中的LiDAR点全部投影回图像，统计它们与图像特征的平均像素距离：

如果 \( e_{rpe} \) 小于亚像素级别，说明“换算表”合格；如果误差偏大，工程师就得回头检查数据采集、特征提取、匹配任一环节。至此，从盐罐到秤盘的红线终于接通，机器人的眼睛和耳朵从此说同一种语言。

AI 应用场景

Python 演示

import numpy as np def best_fit_transform(src, dst):    """给定对应点集，用SVD求解最优旋转和平移（两步法初值估计）"""    ms = src.mean(axis=0)    md = dst.mean(axis=0)    xs = src - ms    xd = dst - md    H = xs.T @ xd    U, _, Vt = np.linalg.svd(H)    R = Vt.T @ U.T    if np.linalg.det(R) < 0:        Vt[-1, :] *= -1        R = Vt.T @ U.T    t = md - R @ ms    return R, t def icp(src, dst, max_iter=50, tol=1e-6):    """迭代最近点：重复匹配+求解变换，直到误差收敛"""    est_R = np.eye(2)    est_t = np.zeros(2)    prev_err = float("inf")    for i in range(max_iter):        # 最近邻匹配：找到每个src点对应的dst点索引        diff = dst[None, :, :] - src[:, None, :]        idx = np.argmin(np.sum(diff**2, axis=-1), axis=1)        matched = dst[idx]        # 用两步法（SVD）求解当前匹配下的最优变换        R, t = best_fit_transform(src, matched)        src = src @ R.T + t        est_R = R @ est_R        est_t = R @ est_t + t        err = np.mean(np.linalg.norm(src - matched, axis=1))        if abs(prev_err - err) < tol:            break        prev_err = err    return est_R, est_t, err if __name__ == "__main__":    rng = np.random.default_rng(42)    true_R = np.array([[0.8, -0.6], [0.6, 0.8]])    true_t = np.array([0.4, -0.3])    target = rng.uniform(-1, 1, (20, 2))    source = target @ true_R.T + true_t + rng.normal(0, 0.01, (20, 2))    R, t, err = icp(source, target)    print("估计旋转:\n", R)    print("估计平移:", t)    print("真实旋转:\n", true_R)    print("真实平移:", true_t)    print("残差:", err)

1. best_fit_transform(src, dst)用SVD分解求解最优旋转和平移，对应章节“初值估计”中的经典两步法。
2. H = xs.T @ xd构造去中心化后的协方差矩阵，是SVD求解旋转矩阵的原材料。
3. R = Vt.T @ U.T从SVD结果中恢复旋转矩阵，保证旋转是正交且行列式为1。
4. diff = dst[None, :, :] - src[:, None, :]计算每个源点到每个目标点的矢量差，为最近邻搜索做准备。
5. idx = np.argmin(...)找到最近邻索引，这是“对应关系建立”中最近邻搜索的核心。
6. src = src @ R.T + t用当前估计的变换更新源点云，对应外参矩阵的作用。
7. est_R = R @ est_R累积每次迭代的旋转，把相对变换转换成初始到当前的绝对变换。
8. err = np.mean(...)计算平均配准误差，判断是否收敛，对应重投影误差的监控。

常见误区

面试题