做LVIO-SLAM有一段时间的人基本都绕不开一件事相机和激光雷达之间的外参标定。标题里这个是系列笔记的第二十一篇正好讲到相机与雷达外参标定我猜很多人在实战里遇到过这样的场景——点云投影到图像上边缘总是差那么几个像素地面勉强对齐但墙面错开定位轨迹短期看着没问题时间一长就飘。这套系统的定位精度、建图质量很大程度上取决于外参标得准不准。这篇先聊离线标定的原理部分也就是“相机与雷达外参标定1”该覆盖的内容为什么要标、标定问题怎么建模、基于标定板的方案是如何求解的。后续再展开在线标定和联合标定的实操部分。如果你正在跑LVIO框架或者打算把相机和雷达装到同一台设备上做感知和定位这篇文章的核心内容是绕不开的基础。1. 外参标定在LVIO系统里到底解决什么问题1.1 三个传感器各自“看到”了什么激光雷达、相机、IMU在LVIO系统里分工明确但也各有所短。激光雷达直接输出三维点云几何信息准确但稀疏颜色和纹理信息几乎没有相机输出图像纹理丰富但缺少深度IMU输出加速度和角速度不依赖外部环境却有积分漂移。问题就出在“各有所短还要相互配合”这句话上。要让点云的深度补到图像的纹理上或者让图像的视觉残差引导雷达的几何观测就必须知道每个传感器之间的空间变换关系雷达坐标系原点相对相机光学中心偏移了多少三个轴分别转了多少角度。这个变换关系就是外参。外参标定就是要估计出这个变换关系的精确数值。如果外参存在误差最直观的现象就是点云投影到图像上对不齐——那种“明明是一条直线从激光雷达视角看是笔直的投影到图里却变成折线”的情况基本就是旋转误差导致的。1.2 外参误差如何顺着残差链扩散到整个系统LVIO系统同时运行多条残差链。视觉观测通过特征点重投影误差约束位姿激光雷达通过点到面、点到线的距离残差约束位姿IMU通过预积分约束相邻帧之间的运动。这三条链最终都在优化后端里融合空间一致性就是通过外参来维持的。假如雷达和相机之间的外参有偏差视觉和雷达各自估计出的位姿就会不一致。优化器面对这种不一致只能把误差当作噪声或者做折中处理最终结果会演变成两种情况一种是系统把错误归结到别的变量上导致地图漂移另一种是长期运行中某个传感器的权重被削弱整个系统的鲁棒性下降。我在某设备上实跑LVIO时就观察过一个现象雷达外参的旋转分量偏了大概0.6度视觉惯导单独跑的时候轨迹精度还行但一旦开启雷达融合整体精度反而变差了而且回环检测后地图还会出现细微的重叠重影。查来查去就是外参的问题。1.3 为什么不能用“大概对齐”凑合有朋友说我手工量一下安装位置角度按说明书估一下行不行短期看勉强凑合。长期看不行。原因有两个。第一实车安装很难做到零误差。支架加工公差、螺丝锁紧后的微小形变、外壳应力都会让实际外参和设计值有偏差。第二LVIO系统对旋转外参的敏感度相当高。假设雷达位置离相机光心20厘米如果旋转外参误差仅0.5度投影到20米外的目标时位置偏差就是大约17厘米。对于建图、目标级定位这类任务这个误差完全不能接受。所以外参标定不是“可做可不做”的事而是整个传感器融合系统的前置条件。外参没标好后面跑再好的算法也白搭。2. 标定前必须建好的两个数学模型SE(3)与残差定义2.1 六个自由度的几何意义外参不是一个数而是从激光雷达坐标系到相机坐标系的一个刚体变换。刚体变换有三个旋转自由度和三个平移自由度统称六个自由度。旋转部分可以用旋转矩阵R表示属于特殊正交群SO(3)。平移部分用平移向量t表示。组合在一起就构成特殊欧氏群SE(3)T_cam_lidar [[R, t], [0, 1]]这个4x4矩阵的含义是给定激光雷达坐标系下的一个三维点p_lidar通过T_cam_lidar可以把它变换到相机坐标系下p_cam R * p_lidar t标定的目标就是估计R和t的最优值。旋转矩阵有3个独立元素但实际优化时一般不直接优化矩阵里的9个数因为旋转矩阵必须满足正交性约束R^T R Idet(R) 1直接优化会引入冗余甚至非法的旋转。工程上通常用李代数so(3)来表达旋转增量用李群SE(3)来表达完整的刚体变换。简化理解的话外参标定的本质是找到一个能准确描述“同一个三维点在两个传感器坐标系之间坐标映射关系”的刚体变换旋转决定方向平移决定原点位置。2.2 四个坐标系的链条关系LVIO系统里不止激光和相机两个坐标系实际参与运算的包括世界坐标系、相机坐标系、雷达坐标系、IMU坐标系。世界坐标系地图和轨迹的参考系通常与第一帧IMU或雷达位姿对齐。相机坐标系以相机光心为原点视觉特征的位置由它描述。雷达坐标系以雷达扫描中心为原点点云几何位置由它描述。IMU坐标系以IMU测量中心为原点加速度和角速度由它描述。把LVIO的数据流拆开看链条是这样的相机看到图像特征估计出相机在世界系中的位姿雷达扫描得到点云估计出雷达在世界系中的位姿IMU在自身坐标系里积分给出帧间运动增量。要让这三条链对齐到同一个世界系就必须知道每一对传感器之间的外参T_cam_imu和T_lidar_cam。这篇重点讨论T_lidar_cam也就是雷达在相机坐标系下的位姿。2.3 残差模型的两种主流选择点到面和点到线标定外参需要构建几何约束。激光雷达扫描到的是一个三维点集相机拍摄到的是一个二维图像平面。纯视觉特征和点云点之间没有直接对应关系所以要把几何约束建立在“点”与“几何元素”之间。点到面的残差雷达点落在某个平面附近理想情况下该点到平面的距离应为0。这个约束用于拟合标定板平面特别合适因为标定板本身就是平面点云打在标定板上点到平面的距离可以作为残差。点到线的残差雷达点落在某条直线附近理想情况下点到直线的距离应为0。这个约束适合利用环境中的边缘特征比如墙角、门框、柱子的棱线。外参标定过程中这两种残差都有使用。点到面更稳定因为平面在点云中容易提取和拟合点到线在某些场景下精度更高但线特征提取的鲁棒性不如面特征。实践中可以用标定板的平面做主要约束用环境中明显线特征做辅助约束。3. 有标定板路线的核心原理从点线面约束到非线性优化3.1 为什么平面标定板是最稳妥的出发点点云和图像的数据形式差异很大想在自然场景里做外参标定需要找两个传感器都能观测到的共同特征。标定板之所以稳妥是因为它同时提供了三种信息平面几何信息、视觉特征信息、明确的边界线信息。点云扫到标定板上通过平面拟合可以直接求出板平面在雷达坐标系下的法向量和距离。相机拍到标定板通过棋盘格角点或ArUco码可以求出板平面在相机坐标系下的位姿。同一个平面在两种传感器坐标系下各有描述这两个描述之间的变换关系正好就是外参。标定板带来的优势是简单且可控。板面是平面几何公式清晰板上的图案提供了丰富的视觉参考角点和边界都能准确提取采集时可以自由调整板在不同距离、不同角度下的姿态让约束覆盖外参的所有维度。3.2 标定板的位姿先验如何获得既然要用同一个平面在两套坐标系下的信息做约束第一步就是把这两种信息分别提取出来。雷达侧给定一帧点云中落在标定板上的点集可以用RANSAC平面拟合求解平面方程。平面方程在雷达坐标系下写成n_lidar^T * p d_lidar 0其中n_lidar是平面单位法向量d_lidar是原点到平面的距离。相机侧通过识别标定板上的棋盘格角点用PnP算法可以求解标定板平面在相机坐标系下的位姿。假设板平面的法向量在相机坐标系下为n_cam平面上某个已知点到原点的距离为d_cam。这里要强调一点雷达点云通常有噪声仅靠一帧点云拟合平面可能不稳定尤其是标定板面积小、距离远的时候。实践上一般会融合多帧点云或者对标定板进行多次扫描再拟合有效降低单帧噪声的影响。3.3 构造并求解优化问题有了同一平面在雷达坐标系和相机坐标系下的两种表达外参标定就变成了一个优化问题寻找一个T_lidar_cam使得通过该变换映射后两边的平面表达尽量一致。构造残差的方式有很多种最直接的是将雷达坐标系下的平面法向量和距离变换到相机坐标系与相机侧的观测作差r [R * n_lidar ? - n_cam; d_cam - (n_cam^T * t d_lidar)]严格推导的话平面变换公式还需要处理法向量变换的细节但原理上可以这样理解外参的正确性同时约束旋转和平移。旋转误差会体现在法向量方向不一致上平移误差会体现在平面偏移距离不一致上。通过多组“雷达点云拟合平面相机位姿估计平面”的观测对可以构建一个包含多个残差的非线性最小二乘问题T argmin Σ ||r_i||^2使用高斯牛顿法或Levenberg-Marquardt方法迭代求解。每一步迭代计算残差向量r_i关于外参增量的雅可比矩阵求解增量更新外参迭代至收敛。雅可比矩阵的推导是将外参用于SE(3)流形上的扰动模型在李代数层面做微分。实际应用中我习惯用ceres或g2o实现这套优化。这类库支持自动求导虽然雅可比推导可以省去但理解扰动模型的基本思想还是必要的否则遇到收敛失败的情况时无法定位问题。4. 跑通标定流程前必须做对的三件事数据采集、初始值和配置4.1 数据采集的实操要点场景、位姿、断档标定优化做得好不好一半取决于数据采得好不好。采集不是简单让传感器对着标定板拍几张就完事有几个要点值得注意。场景选择标定板周围不要有太多其他平面或反光物体否则点云中很难干净地分割出标定板区域。反光金属面不仅会让雷达产生异常点也会干扰相机检测。位姿覆盖标定板必须在雷达和相机的共同视野内并且要有足够多的不同姿态。我通常会让标定板在相机的四个象限里都出现分别做正视、俯仰偏转、左右旋转和前后移动。没有充足的位姿覆盖旋转自由度和平移自由度之间可能发生耦合导致解出的外参在某些方向上精度很差。数据断档问题采集时要确保每一帧数据里标定板都是完整可见的不能让板的某一角超出视野会导致平面拟合和角点检测都不稳定。宁可多采集几次也不要试图用残缺数据去凑。4.2 初值敏感性旋转和平移初始值有偏会发生什么非线性优化对初值敏感。外参的初始值如果偏差过大优化很容易落到局部极小值结果看起来残差很小实际投影仍然对不齐。在标定流程中建议先用粗略手段获得一个靠谱初值可以手动在点云和图像中选几个共同可见的角点用三点法估计初始外参也可以参考机械设计图先给出安装位置和角度再用标定数据精修。平移初值的误差容忍度比旋转初值要高一些因为平移误差主要影响残差中的常数项旋转误差则直接影响残差梯度方向。如果旋转初值差超过10度基于平面的优化很容易陷入错误解表现为收敛后残差下降缓慢或者投影效果依然错位。我在标定一个设备时就遇到过初值旋转差10度的情况。第一次跑优化残差从10降到3就降不下去了。排查后确认初值的旋转方向反了通过修正初始旋转量优化很快就收敛到残差0.5以下。这就是初值重要性的一个直观例子。4.3 传感器时间同步对结果的影响外参标定假设雷达和相机观测的是同一时刻的环境。如果时间不同步标定板在运动过程中采集到的雷达点云和图像不是同一姿态下的观测平面约束就会引入额外误差。时间同步不严的情况下标定结果可能出现一种奇怪的现象静态采集时投影完美对齐运动过程中边角出现拖影或错位。这是因为时间偏差点云和图像在时间轴上错开了导致动态场景下几何不一致。所以在正式标定前先确认传感器的时间戳是否对齐。雷达点云的时间戳、相机图像的时间戳、IMU采样的时间戳三者之间可能会有固定的延迟需要在系统里补偿。具体做法取决于硬件方案有的设备支持PTP同步有的用软件PPS同步这些时间同步的细节最好在标定前处理好。5. 结果质量判定与常见坑以实际数据为例子5.1 怎么看标定结果合不合格优化跑完之后如何判断外参标定质量最直接的方法是投影检查。把雷达点云投影到图像上观察点云边缘和图像边缘的对齐程度。如果外参准确标定板的激光点应该精准地落在图像中标定板区域内边缘线上不能有明显的错位或重叠。还有几个量化角度可以辅助判断。一是残差的最终分布残差均值接近0且标准差较小说明优化收敛得不错残差呈现明显的偏态分布说明某个方向上的约束不足或数据有误。二是用多组验证数据测试标定时用的数据在采集条件上可能和真实使用场景有差异最好在采集数据中留一部分不参与优化专门用来验证外参的泛化效果。我习惯的做法是标定完成后用另一组完全独立采集的数据做投影验证。对标定板在不同距离、不同角度下的投影效果分别检查确认都满足要求才认可这个外参可用。5.2 常见质量问题与排查方向外参标定结果质量差通常能从现象反推原因投影在图像中心对齐良好但边缘区域偏差大大概率是相机镜头畸变未被正确矫正而不是外参问题。需要先做单目相机内参标定确保畸变参数准确。投影在近距离对齐良好远距离偏差逐渐拉大旋转外参的误差通常是主因因为旋转误差产生的偏移随距离线性放大。投影整体有固定偏移但方向一致平移外参误差为主此时检查平面距离残差是否偏大。多组优化结果差异很大采集数据时标定板的位姿覆盖不足或者优化陷入局部极小值。需要重新采集更丰富位姿的数据并检查初值。5.3 一个补充思路为什么后来很多方案转向在线标定离线标定的一大缺点是繁琐且易出错需要专门采集数据、依赖标定板、处理时间同步标定一次后如果设备有轻微变形或松动外参就失效了。这也是后来很多LVIO方案在系统运行中实时估计外参的原因——外参被纳入状态向量作为隐藏状态在定位建图过程中持续优化。在线标定的好处是始终跟踪外参的变化。设备受振动、温度变化影响后外参会缓慢漂移在线估计能自动修正让长时运行的定位精度保持稳定。但离线标定仍然是基础。在线标定需要一个足够好的初始外参如果初值差太多在线优化同样会发散。因此“先离线粗标定后在线精修”是工程上比较稳妥的组合。这篇内容先把离线标定的原理和流程讲透后续再展开在线标定的实现细节。我在实际项目中最大的体会是外参标定不是一次性的工作而是一个需要纳入系统维护流程的常规步骤。设备安装完要标设备经过碰撞、维修、拆装之后要复标长期运行中也要持续关注投影对齐情况。一个负责任的外参维护策略比任何花哨的算法都更能保证系统稳定。最后分享一个实用的小技巧标定过程中把标定的中间结果都记录下来包括每帧点云拟合的平面参数、相机侧估计的板位姿、每次迭代的残差变化。一旦标定结果异常这些数据能帮助你快速定位是标定板识别失败、点云拟合异常还是收敛状态不对省掉大量重复采集的时间。