做LVIO-SLAM的人迟早都会撞上外参标定这堵墙。相机拍出的彩色图像要和雷达撒出去的三维点云对齐再和惯导积分出的轨迹融合前提是先把传感器之间的相对位姿关系搞清楚。所谓相机与雷达外参就是两个传感器之间的旋转和平移关系它在LVIO-SLAM框架里决定了一个视觉特征点和一个激光点能否落到同一个空间位置。这篇笔记是我在实际的室内机器人项目中从“直接调现成工具”到“自己把外参标定流程跑通、看懂每一步在做什么”的第一部分总结适合刚接触LVIO-SLAM、需要把多传感器标定原理和实操串起来的开发者参考。1. 外参标定到底解决什么问题1.1 LVIO框架中相机和雷达为什么需要“对齐”LVIO是激光-视觉-惯性融合SLAM名字里已经写清楚了一个系统里同时存在三种异构传感器。每个传感器都有自己独立的坐标系相机输出像素坐标雷达输出三维点云坐标惯导输出加速度和角速度积分后的位姿。这些数据放到同一套状态估计流程里之前必须知道同一个空间点在各个传感器坐标系里分别长什么样。比如一个墙角点在雷达坐标系里的坐标是(1.2, -0.4, 2.1)在相机坐标系里可能是(1.1, -0.5, 1.9)两者之间的差值就是由相机和雷达的安装位姿差异造成的。如果直接用未对齐的点云和图像做特征匹配视觉特征投影到点云上会产生明显的偏移轻则地图分层重则回环检测直接错乱。外参标定解决的就是这个问题求出一个刚体变换矩阵把一个传感器坐标系下的点变换到另一个传感器坐标系下让两种异构观测在几何上一致。LVIO里还有一个额外的刚体变换是IMU到雷达的但很多框架会先标定相机-雷达外参再通过相机-IMU标定把三者串起来。相机与雷达外参是整个链路的第一环也是后面所有环节能成立的前提。1.2 外参的数学本质和坐标系约定外参通常写成一个4x4的齐次变换矩阵T里面包含一个3x3旋转矩阵R和一个3x1平移向量t。假设我们要求的是相机坐标系到雷达坐标系的变换记为T_cl那么对一个相机坐标系下的三维点p_c它在雷达坐标系下的坐标p_l满足p_l R * p_c t写成齐次形式就是p_l_4 T_cl * p_c_4。旋转R描述两个坐标系之间的姿态差异平移t描述原点之间的相对位移。这个变换在物理上对应的是传感器硬件在设备上的安装角度和安装距离所以标定结果其实就是对物理安装状态的一种数学描述。这里有一个特别容易踩的坑工具和代码里外参的符号约定不统一。有的框架写T_cl意思是“相机到雷达”有的写T_lc意思是“雷达到相机”两个是互逆关系。我在第一次用某个开源的LVIO配置时直接把别人给的外参填进去结果点云投影到图像上一团乱后来才发现那个文件里存的是T_lc而我填的位置期望的是T_cl。遇到外参文件先别急着用把变换方向搞清楚否则后续所有问题都会被误判成标定不准。1.3 旋转和平移在标定中的“分工”旋转和平移虽然合在一起叫外参但实际标定过程中它们的影响方式很不一样。旋转误差对远距离点的影响是发散的雷达测到20米外的点外参旋转差0.5度点云投影到图像上可能偏好几个像素近距离标定时这种误差又容易被噪声淹没。平移误差则主要影响近距离的绝对位置对远距离影响相对稳定。这也是为什么采集标定数据时要让标定板出现在不同距离和不同角度。只有数据里同时覆盖近距离、远距离、左侧、右侧、俯仰变化旋转和平移的约束才是解耦的。如果标定板始终放在同一个位置同一姿态那你大概率只能把外参的某几个分量标出来其他分量在优化里处于弱约束状态看起来残差很小但换到新场景立刻露馅。2. 标定方案选型与思路拆解2.1 标定板选型棋盘格、圆环板还是白板相机-雷达联合标定需要一个能被两种传感器同时“看到”的目标。最常见的选择是棋盘格标定板原因在于视觉端有成熟的角点检测算法图像里的内角点位置可以达到亚像素精度。雷达端虽然看不到棋盘格里的黑白格但可以区分出标定板所在平面和背景只要把落在板面上的点云提取出来就能拟合出一个平面。圆环标定板也是常见方案圆环中心在图像里可以用椭圆拟合获得理论精度不差。但问题出在雷达端16线雷达扫到圆环板上只有几行点想要准确拟合圆环圆心几乎不可能在点云里圆环目标太小很容易被当作杂点滤掉。ArUco码标定板同理视觉端识别稳如老狗雷达点云里却几乎没有可供几何约束的特征。所以主流的可靠方案仍然是一块足够大的平板正面贴棋盘格图案四周留一圈白色或浅色边框。图像端用棋盘格角点做精确匹配点云端用整块板的平面点做几何约束。棋盘格只是给相机看平板才是给雷达看的东西两者配合等于把视觉特征和几何平面绑定在了一起。2.2 基于平面约束的核心思想雷达点云能提供最可靠的信息是“这些点在一个平面上”。假设雷达坐标系下标定板平面的方程为n_l * p_l d_l 0而相机看到的棋盘格在相机坐标系下的空间姿态是已知的。如果我们知道相机到雷达的外参T_cl就可以把相机坐标系下的棋盘格平面变换到雷达坐标系去和雷达点云拟合出的平面比较。进一步往细里说实际操作中一般先分别估计“标定板-雷达”变换和“标定板-相机”变换。棋盘格角点在图像坐标系里有已知像素坐标同时它们在世界/标定板坐标系里的物理坐标也已知可以用PnP方法直接估计出标定板坐标系到相机坐标系的变换T_cb。雷达端则对提取出的板面点云做平面拟合估计出板的平面方程再结合板本身已知的尺寸和朝向得到标定板坐标系到雷达坐标系的变换T_lb。如果两个变换都完全准确那么T_cl T_lb * T_cb_inv应该是一个严格恒定的矩阵。但现实中两个变换都受噪声影响不同位姿下算出来的T_cl会略有差异所以需要用多组位姿做联合优化把T_cl以及每帧的T_cb、T_lb一起作为待优化变量让所有约束同时满足得尽可能好。2.3 初值估计和两步式优化为什么这么重要直接在一个高维非线性优化问题里硬猜外参十有八九会收敛到局部极小值。更稳妥的方法是分成两步先拿到一个不太差的初值再做精细化优化。初值来源主要有几个途径一是按机械设计图纸或者安装支架尺寸手工量出大致的平移和角度这种方式误差在厘米级和几度级虽然不精确但足以让优化进入正确的收敛域二是用“手眼标定”的思路利用上面提到的T_cb和T_lb组合出外参三是对环境几何特征比较明显的场景直接用ICP类方法把相机点云从图像反投影得到和雷达点云做粗配准。我个人的习惯是无论初值来自哪里都要用多组不同初值做一遍优化看最后收敛到的外参是否一致。如果从明显不同的初值出发最终都收敛到相近的结果说明这个解比较可信如果几组初值得到截然不同的外参说明约束不够强数据采集有问题这时候去调优化参数是没用的得回去补数据。3. 实操流程与核心环节3.1 数据采集采集姿势比想象中重要数据采集是外参标定里最容易被低估的环节。我见过很多人拿工具跑几秒就得到结果看起来残差很小但放到实际SLAM里点云和图像就是对不上。问题基本都出在采集阶段。采集时应该让标定板固定在某个位置移动机器人本体或者反过来机器人不动、移动标定板。无论哪种方式都要保证标定板相对于传感器有足够多的姿态变化俯仰角最好覆盖正负30度到60度之间偏航角覆盖左右各30度以上距离从近到远拉开2到8米的范围。这样做的目的是让每个外参分量都被约束到。只有平移误差会造成近距离投影偏差只有旋转误差会造成远距离投影偏差数据分布太集中于某个范围就会把约束“锁死”。另外要注意标定板表面不能太反光。强反光材质在雷达点云里会产生大量飞点尤其是16线低分辨率雷达一个飞点就能让平面拟合偏移。最好用哑光板材贴纸也用哑光面。棋盘格的黑色格子在常规雷达波长下反射率确实偏低但单独靠黑白格本身不会完全丢点关键还是板子整体要有足够的尺寸让雷达至少有几十个点落在平板区域内。3.2 雷达点云预处理和板面点提取拿到原始点云后第一步是把标定板上的点从场景里分离出来。如果环境干净可以简单用直通滤波按距离框出标定板区域如果背景复杂可以先做一次RANSAC平面分割提取出所有平面点再根据面积、法向量和已知板位姿粗筛出标定板平面。点云平面拟合可以用PCL或Open3D核心参数是两个距离阈值和迭代次数。距离阈值决定了多少点算作内点通常按雷达点云噪声设1到2厘米迭代次数不要省1000次起步。以下是一段Open3D实现平面拟合的核心代码import open3d as o3d # 假设已经完成了点云去畸变和区域裁剪 pcd o3d.io.read_point_cloud(scan.pcd) plane_model, inliers pcd.segment_plane( distance_threshold0.015, # 1.5cm以内算内点 ransac_n3, # 拟合平面最少3个点 num_iterations1500 # 迭代次数 ) [a, b, c, d] plane_model # 平面方程: a*x b*y c*z d 0 print(normal:, [a, b, c], offset:, d) plane_cloud pcd.select_by_index(inliers)如果雷达线数很低单帧点云中板面上的点只有几十个建议把连续多帧点云按雷达自身运动变换到同一坐标系后累积起来再做平面拟合等效于提高了点云密度。这个方法在16线雷达上实测非常有效但前提是机器人移动时位姿估计准确否则累积引入的误差比单帧噪声还大。雷达点云还有一个必须做的处理运动畸变去除。这个问题在旋转采集时尤其严重。如果机器人是静止的只是旋转雷达或旋转平台点云没问题如果机器人在运动中采集一帧点云里的每个点对应不同的传感器位姿扫描同一块平面会因为自身运动被拉成曲面或者出现重影直接拿去拟合平面会产生很大的斜率误差。3.3 棋盘格角点检测和图像姿态估计图像端用OpenCV的findChessboardCorners就能拿到棋盘格内角点关键是调用参数和前后处理要正确。棋盘格尺寸必须以“物理格数”为准比如9列6行内角点对应标定板图纸上精确的格子边长如果格子尺寸标错PnP解出来的标定板姿态尺度就错了雷达和相机的平移外参会直接偏一个比例。角点检测完成后要细化到亚像素。findChessboardCorners加上cornerSubPix能让角点位置误差从像素级降到0.1像素以下。然后用solvePnP求解棋盘格坐标系到相机坐标系的变换。OpenCV的solvePnP返回的是旋转向量和平移向量旋转向量用Rodrigues转成旋转矩阵。代码片段如下import cv2 import numpy as np # 假设图像已经转成灰度图gray棋盘格内角点数为 (9, 6) ret, corners cv2.findChessboardCorners(gray, (9, 6), None) if ret: criteria (cv2.TERM_CRITERIA_EPS cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) corners_sub cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria) # 生成棋盘格物理坐标 objp np.zeros((9 * 6, 3), np.float32) objp[:, :2] np.mgrid[0:9, 0:6].T.reshape(-1, 2) * square_size # square_size 是每个格子的边长单位米 # 相机内参K和畸变系数dist来自单目内参标定结果 success, rvec, tvec cv2.solvePnP(objp, corners_sub, K, dist) R_cb, _ cv2.Rodrigues(rvec) # 标定板坐标系到相机坐标系的旋转 t_cb tvec.reshape(3, 1) # 对应平移需要提醒的是相机内参一定要先标好而且使用标定时的分辨率。很多LVIO系统跑的是半分辨率或裁剪后的图像如果图像尺寸和相机内参不匹配PnP解出的姿态会有系统性误差外参标定结果跟着一起偏。3.4 联合优化怎么把两类约束融合起来当每一帧都得到T_cb和T_lb之后可以先用T_cl T_lb * T_cb_inv求一组初值。把这组初值作为所有帧共同外参的初始值然后进入联合优化阶段。优化目标由两类残差构成第一类是视觉残差已知棋盘格角点的像素坐标按当前估计的T_cb把物理角点投影到图像上比较投影坐标和检测坐标的差值。这个残差约束的是“相机-标定板”部分。第二类是雷达几何残差把雷达点云中属于标定板平面的内点按照当前估计的T_lb变换到标定板坐标系再检查它们在z0平面附近的距离。优化过程会调整T_cl、T_cb和T_lb让这两类残差的总和最小。这里有一个实现技巧不要把所有数据一次性扔进优化器。先用距离阈值宽松一点跑几轮剔除明显的外点再收紧阈值精化。比如点面残差先允许0.05米等优化收敛后把残差大于0.02米的点剔除再跑一轮。这样能有效避开标定板边缘那些半落在板外的点。列文伯格-马夸尔特算法在这个尺度的问题上足够稳定。残差雅可比可以用数值微分也可以用解析式但数值微分对步长比较敏感。我的建议是先做解析式或者用自动微分库比如在Python里用ceres的Python绑定、或者用g2o的Python重实现总之不要让数值微分的步长影响收敛结果。3.5 标定结果怎么才算合格标定完成后不能只看总残差要有几个独立的验证手段。最直观的是把雷达点云投影到图像上叠加显示点云的深度颜色。此时标定板边缘应该和图像边缘严格重合远处物体的点云投影误差应该小于3到5个像素。更严格的量化指标是重投影平均误差和点面距离误差。棋盘格角点重投影误差一般在0.3像素以内算优秀0.5像素以内算可用雷达点云到板面的平均距离误差通常要小于1.5厘米。如果16线雷达的拟合残差到了3厘米以上先别急着优化多半是板面点提取出了问题或者数据采集阶段运动畸变没消除。还可以做一次“交叉验证”保留几帧没有参与标定的数据用标定出的外参把图像和点云做投影对齐看这些新鲜数据上是否依然对齐良好。如果参与优化的数据上都完美没参与的数据却漂了说明标定过拟合了当前数据集数据多样性不够这是最常见的问题。4. 常见问题与排查技巧4.1 时间同步偏差导致“标定不出来”相机和雷达如果时间戳没有对齐等于用不同时刻的观测去求同一个不变的外参。只要设备有一点运动哪怕是手抖点云和图像之间就会引入一个随机的相对位移。这个问题在采集数据时很难直接发现因为看起来只是噪声大了一点但优化残差会异常高。排查方法是做一个静态验证把机器人完全静止拿同一时刻的一帧图像和一帧点云做投影如果时间同步是好的静止时点云和图像边缘应该非常对齐。如果静止时也不对齐说明不是时间同步问题而是外参本身错了如果静止时对齐、一动就糊那基本就是时间同步偏差。解决时间同步的思路分硬件和软件两层。硬件上相机触发可以接PTP同步信号雷达用自身时钟给所有传感器统一授时软件上则要做时间戳插值把相机帧时间戳对应的雷达位姿从前后两帧点云位姿中插值出来。很多LVIO框架本身有传感器时间同步模块但前提是数据采集时把时间戳记录精度做对。4.2 点云运动畸变旋转采集时最明显标定数据如果是在机器人旋转运动时采集的雷达一帧扫描内的每个点实际对应了不同的机体位姿。例如16线雷达扫描周期约100毫秒这一百毫秒里机器人转了5度点云就成了扭曲过的空间表达。拿扭曲的点云去拟合平面求出来的法向量方向都有偏外参标定自然不准。常用的解决方法是纯估计法利用LVIO系统自身的里程计位姿把一帧点云中的每个点按照它的时间戳和对应位姿补偿回起始时刻坐标系。如果系统还没有可靠位姿也可以用平滑运动的假设做线性插值补偿但精度有限。还有一种更省事的方式采集时让机器人保持静止只移动标定板这样一帧点云本身没有运动畸变。4.3 标定板遮挡和边缘杂点雷达扫到标定板边缘时有的激光束会打在板边缘的圆弧面上产生半遮挡点这些点既不属于板平面也不属于背景平均值落在板边界附近。如果平面拟合时把这些点算进去平面会稍微倾斜。边缘杂点是点面残差图里最突出的那一批点通常残差远大于中间点。解决策略是在提取板面点云后做一个形态学收缩沿板的平面边界向内收缩5到10厘米只保留远离边缘的内点。或者根据点云在平面上的投影范围去掉最靠近边界的点。不要贪心保留所有内点标定板中间区域的点才是最有价值的。4.4 低线数雷达点云稀疏拟合不稳16线雷达在3米外的一块1米乘0.8米标定板上实际扫到的点可能只有几十个而且分布不均匀。这种情况下拟合出的平面法向量方差很大组合出的T_lb噪声直接传导到外参。如果用的又是Velodyne VLP-16这类低线雷达每线之间的垂直角度分辨率很大板面点会形成一条条明显的横向“条纹”而不是均匀分布。应对办法是增加雷达点云在板面上的覆盖密度。一个是让标定板离雷达更近距离从5米拉近到2米密度按平方关系提升二是做多帧累积把多次静止观测的点云叠加三是让标定板相对雷达转动改变激光线打在板上的位置这样多帧联合起来等效于提高了垂直方向的分辨率。低线雷达标定时不要用单帧点云做平面拟合最少累积三到五帧。4.5 弱反射棋盘格导致雷达“看不清”板面棋盘格黑色格子的反射率比白色格子低但低线雷达通常仍能返回有效距离只是强度值有明显差异。问题出在有些标定板的黑色格子用的是导电炭黑或深色油墨红外反射率极低可能导致部分点在雷达端被当成无效点丢弃在点云里形成“黑洞”。这会让点云平面产生空洞平面拟合虽然没有大问题但点云边缘和内点统计会受影响。最简单的规避方法是买标定板时选印刷棋盘格贴纸而不是喷涂油墨或者在标定板四周加一圈白色边框确保雷达点云里至少能形成完整的板面形状。如果板面点还是缺得厉害可以考虑在板后面垫一块高反射材料利用反射强度差异来辅助分割板面区域但切记不要让背景反光点混入平面拟合。5. 踩坑后的几点体会外参标定做得多了以后我最大的体会是标定工具永远是次要的数据质量才是决定成败的关键。很多人在优化参数和算法实现上折腾很久回过头来才发现是采集数据时标定板姿态太单一或者点云运动畸变没处理。宁可多花半小时把数据采好也比在错误数据上反复调参强。第二个体会是标定结果一定要可视化验证。数字指标再漂亮也不如一眼看到点云边缘和图像边缘对齐来得直观。我每次标定完都会保存几组原始数据用标定好的外参做点云投影然后一帧一帧翻过去看。只要有一帧在近处或远处出现系统性偏移这组外参就要打问号。第三个体会是外参不是标一次就一劳永逸的。机械结构受热膨胀、螺丝松动、拆卸后再装回都可能导致外参漂移。LVIO系统运行一段时间之后如果发现地图出现轻微分层或者视觉和激光里程计输出不一致先检查外参是不是该重新标定了。这也是很多在线标定方法试图解决的问题后续我打算把在线外参估计再单独整理一篇把这部分内容续上。 
长河君 · 2026/10/22 16:35:47 · 阅读 1.2k LVIO-SLAM相机雷达外参标定：从原理到实操避坑指南
做LVIO-SLAM的人迟早都会撞上外参标定这堵墙。相机拍出的彩色图像要和雷达撒出去的三维点云对齐再和惯导积分出的轨迹融合前提是先把传感器之间的相对位姿关系搞清楚。所谓相机与雷达外参就是两个传感器之间的旋转和平移关系它在LVIO-SLAM框架里决定了一个视觉特征点和一个激光点能否落到同一个空间位置。这篇笔记是我在实际的室内机器人项目中从“直接调现成工具”到“自己把外参标定流程跑通、看懂每一步在做什么”的第一部分总结适合刚接触LVIO-SLAM、需要把多传感器标定原理和实操串起来的开发者参考。1. 外参标定到底解决什么问题1.1 LVIO框架中相机和雷达为什么需要“对齐”LVIO是激光-视觉-惯性融合SLAM名字里已经写清楚了一个系统里同时存在三种异构传感器。每个传感器都有自己独立的坐标系相机输出像素坐标雷达输出三维点云坐标惯导输出加速度和角速度积分后的位姿。这些数据放到同一套状态估计流程里之前必须知道同一个空间点在各个传感器坐标系里分别长什么样。比如一个墙角点在雷达坐标系里的坐标是(1.2, -0.4, 2.1)在相机坐标系里可能是(1.1, -0.5, 1.9)两者之间的差值就是由相机和雷达的安装位姿差异造成的。如果直接用未对齐的点云和图像做特征匹配视觉特征投影到点云上会产生明显的偏移轻则地图分层重则回环检测直接错乱。外参标定解决的就是这个问题求出一个刚体变换矩阵把一个传感器坐标系下的点变换到另一个传感器坐标系下让两种异构观测在几何上一致。LVIO里还有一个额外的刚体变换是IMU到雷达的但很多框架会先标定相机-雷达外参再通过相机-IMU标定把三者串起来。相机与雷达外参是整个链路的第一环也是后面所有环节能成立的前提。1.2 外参的数学本质和坐标系约定外参通常写成一个4x4的齐次变换矩阵T里面包含一个3x3旋转矩阵R和一个3x1平移向量t。假设我们要求的是相机坐标系到雷达坐标系的变换记为T_cl那么对一个相机坐标系下的三维点p_c它在雷达坐标系下的坐标p_l满足p_l R * p_c t写成齐次形式就是p_l_4 T_cl * p_c_4。旋转R描述两个坐标系之间的姿态差异平移t描述原点之间的相对位移。这个变换在物理上对应的是传感器硬件在设备上的安装角度和安装距离所以标定结果其实就是对物理安装状态的一种数学描述。这里有一个特别容易踩的坑工具和代码里外参的符号约定不统一。有的框架写T_cl意思是“相机到雷达”有的写T_lc意思是“雷达到相机”两个是互逆关系。我在第一次用某个开源的LVIO配置时直接把别人给的外参填进去结果点云投影到图像上一团乱后来才发现那个文件里存的是T_lc而我填的位置期望的是T_cl。遇到外参文件先别急着用把变换方向搞清楚否则后续所有问题都会被误判成标定不准。1.3 旋转和平移在标定中的“分工”旋转和平移虽然合在一起叫外参但实际标定过程中它们的影响方式很不一样。旋转误差对远距离点的影响是发散的雷达测到20米外的点外参旋转差0.5度点云投影到图像上可能偏好几个像素近距离标定时这种误差又容易被噪声淹没。平移误差则主要影响近距离的绝对位置对远距离影响相对稳定。这也是为什么采集标定数据时要让标定板出现在不同距离和不同角度。只有数据里同时覆盖近距离、远距离、左侧、右侧、俯仰变化旋转和平移的约束才是解耦的。如果标定板始终放在同一个位置同一姿态那你大概率只能把外参的某几个分量标出来其他分量在优化里处于弱约束状态看起来残差很小但换到新场景立刻露馅。2. 标定方案选型与思路拆解2.1 标定板选型棋盘格、圆环板还是白板相机-雷达联合标定需要一个能被两种传感器同时“看到”的目标。最常见的选择是棋盘格标定板原因在于视觉端有成熟的角点检测算法图像里的内角点位置可以达到亚像素精度。雷达端虽然看不到棋盘格里的黑白格但可以区分出标定板所在平面和背景只要把落在板面上的点云提取出来就能拟合出一个平面。圆环标定板也是常见方案圆环中心在图像里可以用椭圆拟合获得理论精度不差。但问题出在雷达端16线雷达扫到圆环板上只有几行点想要准确拟合圆环圆心几乎不可能在点云里圆环目标太小很容易被当作杂点滤掉。ArUco码标定板同理视觉端识别稳如老狗雷达点云里却几乎没有可供几何约束的特征。所以主流的可靠方案仍然是一块足够大的平板正面贴棋盘格图案四周留一圈白色或浅色边框。图像端用棋盘格角点做精确匹配点云端用整块板的平面点做几何约束。棋盘格只是给相机看平板才是给雷达看的东西两者配合等于把视觉特征和几何平面绑定在了一起。2.2 基于平面约束的核心思想雷达点云能提供最可靠的信息是“这些点在一个平面上”。假设雷达坐标系下标定板平面的方程为n_l * p_l d_l 0而相机看到的棋盘格在相机坐标系下的空间姿态是已知的。如果我们知道相机到雷达的外参T_cl就可以把相机坐标系下的棋盘格平面变换到雷达坐标系去和雷达点云拟合出的平面比较。进一步往细里说实际操作中一般先分别估计“标定板-雷达”变换和“标定板-相机”变换。棋盘格角点在图像坐标系里有已知像素坐标同时它们在世界/标定板坐标系里的物理坐标也已知可以用PnP方法直接估计出标定板坐标系到相机坐标系的变换T_cb。雷达端则对提取出的板面点云做平面拟合估计出板的平面方程再结合板本身已知的尺寸和朝向得到标定板坐标系到雷达坐标系的变换T_lb。如果两个变换都完全准确那么T_cl T_lb * T_cb_inv应该是一个严格恒定的矩阵。但现实中两个变换都受噪声影响不同位姿下算出来的T_cl会略有差异所以需要用多组位姿做联合优化把T_cl以及每帧的T_cb、T_lb一起作为待优化变量让所有约束同时满足得尽可能好。2.3 初值估计和两步式优化为什么这么重要直接在一个高维非线性优化问题里硬猜外参十有八九会收敛到局部极小值。更稳妥的方法是分成两步先拿到一个不太差的初值再做精细化优化。初值来源主要有几个途径一是按机械设计图纸或者安装支架尺寸手工量出大致的平移和角度这种方式误差在厘米级和几度级虽然不精确但足以让优化进入正确的收敛域二是用“手眼标定”的思路利用上面提到的T_cb和T_lb组合出外参三是对环境几何特征比较明显的场景直接用ICP类方法把相机点云从图像反投影得到和雷达点云做粗配准。我个人的习惯是无论初值来自哪里都要用多组不同初值做一遍优化看最后收敛到的外参是否一致。如果从明显不同的初值出发最终都收敛到相近的结果说明这个解比较可信如果几组初值得到截然不同的外参说明约束不够强数据采集有问题这时候去调优化参数是没用的得回去补数据。3. 实操流程与核心环节3.1 数据采集采集姿势比想象中重要数据采集是外参标定里最容易被低估的环节。我见过很多人拿工具跑几秒就得到结果看起来残差很小但放到实际SLAM里点云和图像就是对不上。问题基本都出在采集阶段。采集时应该让标定板固定在某个位置移动机器人本体或者反过来机器人不动、移动标定板。无论哪种方式都要保证标定板相对于传感器有足够多的姿态变化俯仰角最好覆盖正负30度到60度之间偏航角覆盖左右各30度以上距离从近到远拉开2到8米的范围。这样做的目的是让每个外参分量都被约束到。只有平移误差会造成近距离投影偏差只有旋转误差会造成远距离投影偏差数据分布太集中于某个范围就会把约束“锁死”。另外要注意标定板表面不能太反光。强反光材质在雷达点云里会产生大量飞点尤其是16线低分辨率雷达一个飞点就能让平面拟合偏移。最好用哑光板材贴纸也用哑光面。棋盘格的黑色格子在常规雷达波长下反射率确实偏低但单独靠黑白格本身不会完全丢点关键还是板子整体要有足够的尺寸让雷达至少有几十个点落在平板区域内。3.2 雷达点云预处理和板面点提取拿到原始点云后第一步是把标定板上的点从场景里分离出来。如果环境干净可以简单用直通滤波按距离框出标定板区域如果背景复杂可以先做一次RANSAC平面分割提取出所有平面点再根据面积、法向量和已知板位姿粗筛出标定板平面。点云平面拟合可以用PCL或Open3D核心参数是两个距离阈值和迭代次数。距离阈值决定了多少点算作内点通常按雷达点云噪声设1到2厘米迭代次数不要省1000次起步。以下是一段Open3D实现平面拟合的核心代码import open3d as o3d # 假设已经完成了点云去畸变和区域裁剪 pcd o3d.io.read_point_cloud(scan.pcd) plane_model, inliers pcd.segment_plane( distance_threshold0.015, # 1.5cm以内算内点 ransac_n3, # 拟合平面最少3个点 num_iterations1500 # 迭代次数 ) [a, b, c, d] plane_model # 平面方程: a*x b*y c*z d 0 print(normal:, [a, b, c], offset:, d) plane_cloud pcd.select_by_index(inliers)如果雷达线数很低单帧点云中板面上的点只有几十个建议把连续多帧点云按雷达自身运动变换到同一坐标系后累积起来再做平面拟合等效于提高了点云密度。这个方法在16线雷达上实测非常有效但前提是机器人移动时位姿估计准确否则累积引入的误差比单帧噪声还大。雷达点云还有一个必须做的处理运动畸变去除。这个问题在旋转采集时尤其严重。如果机器人是静止的只是旋转雷达或旋转平台点云没问题如果机器人在运动中采集一帧点云里的每个点对应不同的传感器位姿扫描同一块平面会因为自身运动被拉成曲面或者出现重影直接拿去拟合平面会产生很大的斜率误差。3.3 棋盘格角点检测和图像姿态估计图像端用OpenCV的findChessboardCorners就能拿到棋盘格内角点关键是调用参数和前后处理要正确。棋盘格尺寸必须以“物理格数”为准比如9列6行内角点对应标定板图纸上精确的格子边长如果格子尺寸标错PnP解出来的标定板姿态尺度就错了雷达和相机的平移外参会直接偏一个比例。角点检测完成后要细化到亚像素。findChessboardCorners加上cornerSubPix能让角点位置误差从像素级降到0.1像素以下。然后用solvePnP求解棋盘格坐标系到相机坐标系的变换。OpenCV的solvePnP返回的是旋转向量和平移向量旋转向量用Rodrigues转成旋转矩阵。代码片段如下import cv2 import numpy as np # 假设图像已经转成灰度图gray棋盘格内角点数为 (9, 6) ret, corners cv2.findChessboardCorners(gray, (9, 6), None) if ret: criteria (cv2.TERM_CRITERIA_EPS cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) corners_sub cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria) # 生成棋盘格物理坐标 objp np.zeros((9 * 6, 3), np.float32) objp[:, :2] np.mgrid[0:9, 0:6].T.reshape(-1, 2) * square_size # square_size 是每个格子的边长单位米 # 相机内参K和畸变系数dist来自单目内参标定结果 success, rvec, tvec cv2.solvePnP(objp, corners_sub, K, dist) R_cb, _ cv2.Rodrigues(rvec) # 标定板坐标系到相机坐标系的旋转 t_cb tvec.reshape(3, 1) # 对应平移需要提醒的是相机内参一定要先标好而且使用标定时的分辨率。很多LVIO系统跑的是半分辨率或裁剪后的图像如果图像尺寸和相机内参不匹配PnP解出的姿态会有系统性误差外参标定结果跟着一起偏。3.4 联合优化怎么把两类约束融合起来当每一帧都得到T_cb和T_lb之后可以先用T_cl T_lb * T_cb_inv求一组初值。把这组初值作为所有帧共同外参的初始值然后进入联合优化阶段。优化目标由两类残差构成第一类是视觉残差已知棋盘格角点的像素坐标按当前估计的T_cb把物理角点投影到图像上比较投影坐标和检测坐标的差值。这个残差约束的是“相机-标定板”部分。第二类是雷达几何残差把雷达点云中属于标定板平面的内点按照当前估计的T_lb变换到标定板坐标系再检查它们在z0平面附近的距离。优化过程会调整T_cl、T_cb和T_lb让这两类残差的总和最小。这里有一个实现技巧不要把所有数据一次性扔进优化器。先用距离阈值宽松一点跑几轮剔除明显的外点再收紧阈值精化。比如点面残差先允许0.05米等优化收敛后把残差大于0.02米的点剔除再跑一轮。这样能有效避开标定板边缘那些半落在板外的点。列文伯格-马夸尔特算法在这个尺度的问题上足够稳定。残差雅可比可以用数值微分也可以用解析式但数值微分对步长比较敏感。我的建议是先做解析式或者用自动微分库比如在Python里用ceres的Python绑定、或者用g2o的Python重实现总之不要让数值微分的步长影响收敛结果。3.5 标定结果怎么才算合格标定完成后不能只看总残差要有几个独立的验证手段。最直观的是把雷达点云投影到图像上叠加显示点云的深度颜色。此时标定板边缘应该和图像边缘严格重合远处物体的点云投影误差应该小于3到5个像素。更严格的量化指标是重投影平均误差和点面距离误差。棋盘格角点重投影误差一般在0.3像素以内算优秀0.5像素以内算可用雷达点云到板面的平均距离误差通常要小于1.5厘米。如果16线雷达的拟合残差到了3厘米以上先别急着优化多半是板面点提取出了问题或者数据采集阶段运动畸变没消除。还可以做一次“交叉验证”保留几帧没有参与标定的数据用标定出的外参把图像和点云做投影对齐看这些新鲜数据上是否依然对齐良好。如果参与优化的数据上都完美没参与的数据却漂了说明标定过拟合了当前数据集数据多样性不够这是最常见的问题。4. 常见问题与排查技巧4.1 时间同步偏差导致“标定不出来”相机和雷达如果时间戳没有对齐等于用不同时刻的观测去求同一个不变的外参。只要设备有一点运动哪怕是手抖点云和图像之间就会引入一个随机的相对位移。这个问题在采集数据时很难直接发现因为看起来只是噪声大了一点但优化残差会异常高。排查方法是做一个静态验证把机器人完全静止拿同一时刻的一帧图像和一帧点云做投影如果时间同步是好的静止时点云和图像边缘应该非常对齐。如果静止时也不对齐说明不是时间同步问题而是外参本身错了如果静止时对齐、一动就糊那基本就是时间同步偏差。解决时间同步的思路分硬件和软件两层。硬件上相机触发可以接PTP同步信号雷达用自身时钟给所有传感器统一授时软件上则要做时间戳插值把相机帧时间戳对应的雷达位姿从前后两帧点云位姿中插值出来。很多LVIO框架本身有传感器时间同步模块但前提是数据采集时把时间戳记录精度做对。4.2 点云运动畸变旋转采集时最明显标定数据如果是在机器人旋转运动时采集的雷达一帧扫描内的每个点实际对应了不同的机体位姿。例如16线雷达扫描周期约100毫秒这一百毫秒里机器人转了5度点云就成了扭曲过的空间表达。拿扭曲的点云去拟合平面求出来的法向量方向都有偏外参标定自然不准。常用的解决方法是纯估计法利用LVIO系统自身的里程计位姿把一帧点云中的每个点按照它的时间戳和对应位姿补偿回起始时刻坐标系。如果系统还没有可靠位姿也可以用平滑运动的假设做线性插值补偿但精度有限。还有一种更省事的方式采集时让机器人保持静止只移动标定板这样一帧点云本身没有运动畸变。4.3 标定板遮挡和边缘杂点雷达扫到标定板边缘时有的激光束会打在板边缘的圆弧面上产生半遮挡点这些点既不属于板平面也不属于背景平均值落在板边界附近。如果平面拟合时把这些点算进去平面会稍微倾斜。边缘杂点是点面残差图里最突出的那一批点通常残差远大于中间点。解决策略是在提取板面点云后做一个形态学收缩沿板的平面边界向内收缩5到10厘米只保留远离边缘的内点。或者根据点云在平面上的投影范围去掉最靠近边界的点。不要贪心保留所有内点标定板中间区域的点才是最有价值的。4.4 低线数雷达点云稀疏拟合不稳16线雷达在3米外的一块1米乘0.8米标定板上实际扫到的点可能只有几十个而且分布不均匀。这种情况下拟合出的平面法向量方差很大组合出的T_lb噪声直接传导到外参。如果用的又是Velodyne VLP-16这类低线雷达每线之间的垂直角度分辨率很大板面点会形成一条条明显的横向“条纹”而不是均匀分布。应对办法是增加雷达点云在板面上的覆盖密度。一个是让标定板离雷达更近距离从5米拉近到2米密度按平方关系提升二是做多帧累积把多次静止观测的点云叠加三是让标定板相对雷达转动改变激光线打在板上的位置这样多帧联合起来等效于提高了垂直方向的分辨率。低线雷达标定时不要用单帧点云做平面拟合最少累积三到五帧。4.5 弱反射棋盘格导致雷达“看不清”板面棋盘格黑色格子的反射率比白色格子低但低线雷达通常仍能返回有效距离只是强度值有明显差异。问题出在有些标定板的黑色格子用的是导电炭黑或深色油墨红外反射率极低可能导致部分点在雷达端被当成无效点丢弃在点云里形成“黑洞”。这会让点云平面产生空洞平面拟合虽然没有大问题但点云边缘和内点统计会受影响。最简单的规避方法是买标定板时选印刷棋盘格贴纸而不是喷涂油墨或者在标定板四周加一圈白色边框确保雷达点云里至少能形成完整的板面形状。如果板面点还是缺得厉害可以考虑在板后面垫一块高反射材料利用反射强度差异来辅助分割板面区域但切记不要让背景反光点混入平面拟合。5. 踩坑后的几点体会外参标定做得多了以后我最大的体会是标定工具永远是次要的数据质量才是决定成败的关键。很多人在优化参数和算法实现上折腾很久回过头来才发现是采集数据时标定板姿态太单一或者点云运动畸变没处理。宁可多花半小时把数据采好也比在错误数据上反复调参强。第二个体会是标定结果一定要可视化验证。数字指标再漂亮也不如一眼看到点云边缘和图像边缘对齐来得直观。我每次标定完都会保存几组原始数据用标定好的外参做点云投影然后一帧一帧翻过去看。只要有一帧在近处或远处出现系统性偏移这组外参就要打问号。第三个体会是外参不是标一次就一劳永逸的。机械结构受热膨胀、螺丝松动、拆卸后再装回都可能导致外参漂移。LVIO系统运行一段时间之后如果发现地图出现轻微分层或者视觉和激光里程计输出不一致先检查外参是不是该重新标定了。这也是很多在线标定方法试图解决的问题后续我打算把在线外参估计再单独整理一篇把这部分内容续上。