做多传感器融合的人应该都有这种经历2D激光雷达和相机背靠背装在小车上各自的数据都正常一融合就露馅。激光点打在墙上的距离明明是一米二投影到图像里却差出十来个像素避障框歪得没法看。我第一次做这类项目的时候第一反应是调代码、换算法折腾了大半天最后才意识到问题根本不在融合算法而在标定。所谓标定说白了就是确定两个传感器坐标系之间的旋转和平移关系也就是外参。外参不准后面所有工作都白搭。这事没法靠肉眼量尺子解决因为安装误差、加工公差都会让实际外参和设计值偏离很远必须靠数据反算。这篇文章把我实际跑通的基于平面约束的联合标定方案完整讲一遍。从为什么选平面约束这个方法开始到数学上怎么建模、ROS环境里怎么一步步实现和验证最后把我在项目里踩过的坑和排查经验一并放出来。适合正在做2D激光雷达和相机融合、或者被标定问题卡住的工程师参考也适合刚接触多传感器融合的初学者入门。1. 为什么做联合标定需求决定方案1.1 激光与相机天生互补但要先对齐2D激光雷达测距精度高、不受光照影响但输出的是单线扫描点没有颜色和纹理信息。相机画面信息丰富能辨认物体、车道线、文字但单目相机本身给不出可靠的深度。两者融合等于让机器人既知道前面有东西、距离多少又知道那是什么、长什么样。这个组合在很多场景都是标准配置比如室内AGV的避障导航、服务机器人的人体检测、安防巡检机器人的目标识别。但融合的前提是数据在同一个坐标系里能对上。雷达点算出来是某个位置图像里对应的像素也应该是那个位置这就需要把激光坐标系和相机坐标系之间的关系精确标定出来。这个关系就是外参一个3x3的旋转矩阵加一个3维平移向量总共6个自由度。这里有个容易混淆的点很多人以为相机内参标定好了外参随便量一量就行。真不是。相机内参描述的是光心到像素的映射外参描述的是激光坐标到相机坐标的刚体变换。安装支架的加工误差、螺丝拧紧后的微小位移、相机和雷达自身的安装偏差累积起来就足以让投影偏移十几个像素。在避障场景里差几个像素可能无所谓但在目标框精细化、动态跟踪、传感器融合测距这类任务里偏差直接决定系统能不能用。1.2 平面约束方案的优势与适用边界市面上做激光雷达和相机联合标定的思路不少。有的用标定板上的特征点、有的用环境中的边缘线、有的用互信息直接对齐点云和图像。我在实际项目里最终选了基于平面约束的方法原因是它最符合工程现场的几大诉求。第一平面是室内环境最常见的几何结构。墙面、地面、门板、柜子表面都是天然的平面。哪怕没有专门的标定板找一面平整的墙也能凑合做初步标定。第二平面约束有明确的数学表达构建优化目标很直接不容易走偏。第三对设备要求低不需要同步触发、不需要特制标定架一套棋盘格加一台能跑ROS的电脑就能开工。但它也有自己的应用边界。平面约束要求雷达扫描平面和相机视野必须同时覆盖被测平面对安装位置的几何有一定要求。如果雷达装得太低、相机装得太偏共同视野太小约束就会变弱标定出来的外参精度会打折扣。另外它对环境中的平面质量敏感如果墙面有大面积反光、玻璃墙面或者装饰条太多平面提取的稳定性会下降。这些都是选方案前需要想清楚的。还有一种情况需要注意如果你手里是3D激光雷达那有更好的标定方案可选比如基于角点或NDT的配准方法平面约束方案更多是给2D单线雷达和相机搭配时用的因为2D雷达数据本身也提取不出太多复杂特征平面恰好是最稳定、最容易获得的约束。2. 核心原理平面约束是如何工作的2.1 坐标系与位姿变换在动手写代码之前先把坐标系之间的关系理清楚。这里涉及到三个坐标系雷达坐标系、相机坐标系和世界坐标系。世界坐标系一般用标定板或者某个参照物来建立求解外参时通常先过渡到标定板坐标系。雷达坐标系的原点在激光发射中心扫描平面通常定义为z0的平面。2D激光雷达测量返回的是极坐标格式即距离和角度转换到雷达坐标系里就是一个二维坐标点、加一个恒为0的z值。也就是说激光扫描到的任何物理点在雷达坐标系里都可以表示成类似这样的形式某个(x, y, 0)。相机坐标系的原点在光心z轴指向相机前方。空间点在相机坐标系中的坐标可以通过相机内参投影到图像像素坐标。两个坐标系之间通过外参关联公式是p_c R * p_l t其中p_l是雷达系下的三维点R是旋转矩阵t是平移向量p_c是同一个点在相机系下的三维坐标。R和t就是我们要标定的目标。这里有个细节对新手很关键旋转矩阵R虽然看起来是3x3的矩阵但它只有3个自由度因为旋转矩阵必须是正交矩阵且行列式为1。实际优化时不能用9个元素直接当参数否则优化出来的结果可能根本不是合法旋转。工程上常用旋转向量so(3)的李代数或者四元数来参数化优化完再转成旋转矩阵。后面代码部分我会用旋转向量来做。2.2 把“标定”变成一个最小化问题平面约束的核心思想不复杂环境中有一些平面比如标定板表面、墙面、地面。相机可以看到这个平面雷达也能打到这个平面上。如果我们知道某一个平面在相机坐标系下的方程同时知道雷达扫描到这个平面上的一堆点的坐标那么这些点经过正确的外参变换到相机坐标系后应该恰好落在这个平面上。用数学语言说就是每个点到平面的距离应该等于0。假设第k帧数据里标定板平面在相机坐标系下的方程用n_k和d_k表示其中n_k是平面的单位法向量d_k是平面到原点的距离平面上任意点X满足n_k^T X d_k 0。假设这一帧里雷达扫描到板上的点集是P_k {p_1, p_2, ...}经过外参变换后每个点都应该满足平面方程n_k^T (R * p_i t) d_k 0由于噪声、外参不准等因素左边不会精确等于0会有一个残差。把所有帧、所有点的残差平方加起来得到一个总代价函数然后用优化算法最小化这个代价函数使R和t尽可能准确这就是平面约束标定的本质。用公式表达就是min_{R,t} Σ_k Σ_{i∈P_k} [n_k^T (R p_i t) d_k]^2这个形式非常干净也特别好写代码。需要准备的数据只有两类雷达扫描到板上的点集以及相机图像中估算出的板平面方程。2.3 为什么点多就能解出六自由度有人可能会问一个点给出一个方程那是不是凑够6个方程就能解出6个自由度理论上可以但实际操作中要复杂得多。单个点的约束对某些自由度的观测能力很弱比如当所有雷达点都在一条线上时沿这条线方向的移动很难被观测到当所有点的几何分布退化时方程组的条件数会变得很差标定结果对噪声极其敏感。举个例子如果标定板始终正对着激光雷达、所有扫描点都集中在板中央那你测到的点沿板平面法线方向几乎是共面的约束就很弱外参的某些分量根本约束不住。你可能做了标定结果是错的但残差看起来还挺小。这就是所谓的退化问题。解决办法是增加观测的多样性。我实际做的时候会把标定板放到至少6-8个不同的位置和姿态让雷达扫描线在板上的落点位置、角度尽量千差万别。有的是近处的有的是远处的有的偏左有的偏右有的稍微倾斜一点。这样能保证各个方向的约束都充分激发出来6个自由度才能被可靠地锁定。从另一个角度看这其实和相机标定很像。相机标定棋盘格要拍多角度、多位置的图像因为单张图只能确定部分参数。激光与相机联合标定同理多样化的观测是精度的重要保障。任何一个开源工具都不能在最少的输入下给你最好的结果数据质量往往决定标定上限。3. ROS实践从采集到求解的完整流程3.1 软硬件准备与环境搭建我做这套标定用的设备是一台入门级的2D激光雷达扫描频率10Hz左右和一台普通USB相机1280x720分辨率30帧机器人平台用的是树莓派加STM32控制板但标定过程其实和机器人本体关系不大核心是保证两个传感器能同时出数据就行。软件环境方面我建议直接用ROS自带的工具来录制和回放数据这样时间戳管理、话题同步都由框架处理。关于ROS版本如果是Ubuntu 20.04装ROS Noetic如果用的是Ubuntu 22.04装ROS 2 Humble。两个版本我都试过标定流程思路完全一致只是话题和命令行工具略有差异这里主要以ROS 1 Noetic为例讲。新手装ROS如果嫌麻烦可以用鱼香ROS一键安装脚本确实能省去不少环境配置的时间装完记得把工作空间也建好后面要放标定相关的节点。另外需要确认的几个依赖库OpenCV图像处理、棋盘格角点检测、numpy矩阵计算、scipy非线性优化、sensor_msgs和cv_bridgeROS消息与OpenCV图像互转。Ubuntu下用apt装OpenCV和python3-opencv就行ROS自身依赖会带cv_bridge。3.2 数据采集标定板布置与rosbag录制标定板我用的是带7x9黑白棋盘格的亚克力板边长30mm板面平整、不反光。尺寸不用太大A3纸打印的棋盘格贴到硬纸板上也够用。关键是要保证雷达扫描线能打到板面。如果雷达安装高度和板的高度对不上需要调整板的摆放高度或者垫一个三脚架把板架到相应高度。摆放位置有个原则让板尽量在雷达和相机共同视野的中间区域板面尽量垂直地面但每次采集时稍微改变板的平面朝向和距离。我一般是握着板站在机器人前方1到2米处每次拍之前先让雷达扫一下确认板的位置在扫描范围内然后用下面命令录制数据rosbag record -O calib_data.bag /scan /image_raw录完一段就移动板改变位置或朝向再录另一段。每段不用太长5到10秒就够。我建议至少录8个不同位姿也就是8段数据如果条件允许12到15段效果更好。录制时不要晃动板尽量维持板面稳定。这里有个容易忽略的点激光雷达扫描频率和相机帧率不同步。比如雷达10Hz、相机30HzROS会自动打上不同的时间戳后续处理时需要做时间对齐最简单的办法是找雷达时间戳最近的相机帧。如果你在录数据的时候板在移动时间不同步的影响会很大所以尽量保持板静止每段录制完成后人稍微移动板即可录制过程中不要动。3.3 平面检测与对应的建立数据录好之后离线处理过程分两块从雷达数据里提取板上的点集从图像数据里估计板平面的方程。先看雷达数据。ROS的LaserScan消息里包含的是极坐标格式的距离和角度先转成二维笛卡尔坐标。雷达坐标系下所有点z值都是0所以每个点的三维表示就是(x, y, 0)。然后要做的事情是从一整个扫描周期的点里把标定板对应的那一段点集分离出来。标定板在雷达反射数据上通常表现为一个连续的点段距离上和其他障碍物明显不同。我实际用的是最简单的方法设定距离阈值和连续点判断。可以从扫描点的起始位置开始当相邻两个点的距离差小于一定阈值时认为属于同一个目标同时如果某段连续点的长度在板宽的合理范围内比如0.3到0.8米就认为它是板上的扫描点。这一段内的所有点都是我们要用来做约束的点集。如果想更鲁棒一点可以用DBSCAN聚类然后按聚类的点数和宽度筛选出标定板。板上点数量太少说明板没放正或者太远建议重新采集。我在实践中的经验是每一帧至少要有10到20个板上的点太少了约束太弱。再看图像部分。利用OpenCV检测棋盘格角点得到角点在图像中的像素坐标。调用cv2.solvePnP结合相机内参和棋盘格每个角点的世界坐标以棋盘格左上角为原点边长作为比例可以得到棋盘格平面相对于相机的旋转向量和平移向量。然后从旋转向量转成旋转矩阵板的法向量就是旋转矩阵的第三列棋盘格平面的Z轴方向d -n^T * t。这样一个平面方程n^T X d 0就拿到了。这里需要提醒一下如果你用棋盘格做平面来源那么雷达扫到的必须是棋盘格所在的这个平面。棋盘格如果贴在不平的纸板上或者板子弯了平面方程和雷达点的实际位置就对不上标定精度会直接崩掉。所以务必要选平整的板拿手按压一下确认没有翘曲。3.4 外参求解与优化实现原始数据准备好之后就到了核心求解环节。我建议先用线性方法或者经验值给一个初始外参再用非线性优化精化。初始值太差的话非线性优化很容易掉进局部最优。我代码里用的初始值是直接量出来的大概安装位置雷达在相机下方大约10厘米、稍偏右5厘米朝向基本一致。转换成旋转向量就接近[0, 0, 0]平移向量就是[0.05, -0.10, 0.1]之类的粗略值。如果你的安装更随意初始值也别太离谱量个大概就行。优化目标就是把之前说的残差写出来。旋转参数用旋转向量表示这样3自由度能避开正交约束。关键代码如下import numpy as np import cv2 from scipy.optimize import least_squares def compute_plane(corners_img, corners_obj, K, dist): # 棋盘格平面在相机坐标系下的法向量n和平移距离d ret, rvec, tvec cv2.solvePnP(corners_obj, corners_img, K, dist) R_board, _ cv2.Rodrigues(rvec) n R_board[:, 2] # 棋盘格Z轴方向 d -n.dot(tvec.flatten()) # 平面方程 n^T X d 0 return n, d def residuals(params, frame_data, K): rvec params[:3] tvec params[3:6] R_lidar2cam, _ cv2.Rodrigues(rvec) res [] for (pts_lidar, n, d) in frame_data: pts_cam pts_lidar R_lidar2cam.T tvec dist pts_cam n d # 点到平面距离 res.extend(dist) return np.array(res) def solve_extrinsic(frame_data, init_rvec, init_tvec, K): init_params np.hstack([init_rvec, init_tvec]) result least_squares(residuals, init_params, args(frame_data, K), methodlm) R_opt, _ cv2.Rodrigues(result.x[:3]) t_opt result.x[3:6] return R_opt, t_opt解释几个点pts_lidar是雷达点在雷达坐标系下的三维坐标数组每一行是(x, y, 0)。pts_cam R_lidar2cam.T tvec 是把雷达点通过当前外参变换到相机坐标系这里R的用法要匹配你定义的外参方向。我代码里的R_lidar2cam满足p_cam R p_lidar t所以换算成numpy点积时要转置注意别搞反方向。残差函数里dist数组是每个雷达点到当前估计平面之间的距离。理想情况下这些距离为0。优化器通过调整参数使所有残差平方和最小。我用的scipy的least_squaresmethod设成lm也就是Levenberg-Marquardt对于这种小规模非线性最小二乘非常够用。3.5 结果验证点云投影与误差统计优化完拿到外参不能直接就算完事一定要做验证。我通常做两件事。第一件事是可视化检查。写一个节点或者脚本把雷达点变换到相机坐标系再投影到图像上在图上画出来。如果外参正确雷达点应该准确地落在对应的物体边缘。比如扫描墙面时雷达点投影到图像里应该落在墙面上而不是飘到空中。# 将雷达点云投影到图像上 def visualize_projection(frame, pts_lidar, R, t, K, dist): pts_cam pts_lidar R.T t pts_img, _ cv2.projectPoints(pts_cam, np.zeros(3), np.zeros(3), K, dist) for p in pts_img: u, v p.ravel() cv2.circle(frame, (int(u), int(v)), 2, (0, 255, 0), -1) cv2.imshow(projection, frame)用这段代码看几帧不同场景的数据如果点都贴合在物体表面心理就有底了。第二件事是定量验证。回放所有采集的数据用标定完的参数重新算一遍点到平面的平均距离和最大距离。我的经验是如果平均距离在1厘米以下说明外参基本靠谱如果平均距离超过3厘米多半还有问题。值得注意的是点到平面的平均距离只能说明标定结果和环境平面的一致性不能完全代表所有方向上的精度所以最好再结合投影可视化来综合判断。我实测下来一组采集质量好的数据优化后残差从几厘米降到几毫米投影到图像上雷达点的位置能基本贴合物体边缘避障框的像素偏差从一个物体的宽度缩小到几个像素以内。这就算通过了。4. 实战中的坑与排查方法4.1 时间同步与数据对齐我在数据处理环节遇到最多的坑就是时间同步。雷达和相机完全是两套时钟即使都挂在ROS下消息的时间戳也是各自采集时刻的近似值。如果数据处理时直接不管时间戳、按消息到达顺序读取bag很容易在板子移动的那几帧里混入状态不一致的数据。我最初写脚本时偷懒直接在遍历bag时每收到一帧雷达scan就去找上一帧图像结果标定出来的外参总是有偏差。后来改成按时间戳取最近邻的相机帧效果立刻好了很多。具体方法是在处理bag时对每一帧scan二分查找最近时间戳的图像帧再提取平面方程。如果你的传感器支持硬件同步那是最好的能彻底根治这个问题。没有硬件同步也没关系保持板子静止、用最近邻时间戳匹配精度足够满足大部分场景。4.2 雷达点聚类不稳定另一个很常见的问题是雷达点聚类不稳定。标定板如果太窄、太远或者板面反射率低激光点会断断续续连续段判断不准。这时候板上的点集可能混入背景墙面的点导致平面约束方程被污染。我排查时会先把板上的点画出来可视化看是不是真的都落在板面上。如果发现点集不干净就降低连续点判断的阈值或者改用DBSCAN聚类后按点数和宽度双重筛选。实在不行就重新采集。数据采集阶段多花两分钟比事后调试省太多时间。4.3 平面法向量方向不一致平面方程n^T X d 0里n的方向和d的符号不是唯一的。同一个平面法向量可以取反d也会跟着变方程依然成立。但在构造残差时如果某些帧的n取反了和别的帧符号不一致优化时就会出现奇奇怪怪的问题。我遇到过的现象是标定出来的旋转矩阵看起来对称但平移向量明显不对。排查后发现有的帧中检测到的板法向量朝向相机有的朝向背对相机。解决方法是统一法向量方向强制让n指向相机一侧也就是让n和相机光心到板中心的向量点积为正。加一行检查代码就能解决。4.4 初值敏感与局部最优非线性优化都有局部最优问题。标定问题本身是一个非线性较强的优化初值差太远就可能收敛到错误解。我自己踩过这个坑给了一个完全错误的初始外参优化跑完之后残差还很大投影结果稀烂。解决思路有两个方向。一是给一个尽量合理的初值哪怕只是量出来的粗略安装位置。二是在初值明显不可靠时用随机采样或者网格搜索先找一个更好的初值。另外检查一下优化后残差是否收敛到很小的数值如果残差还很大多半是初值问题或者数据质量问题。4.5 标定结果看起来“差不多”但融合还是飘有一种最磨人的情况可视化投影时雷达点大致在物体上但稍微一动就偏总觉得差了那么一点。这通常说明标定精度还不够或者数据包含退化情形。这时需要检查几个方向。首先看采集数据的多样性是否足够是不是板总是在正前方、总是同一个距离导致某个方向的约束没有被激发。其次看平面提取的精度特别是棋盘格角点检测误差如果图像分辨率太低、棋盘格太小平面方程本身就不可靠。最后看雷达和相机的时间戳同步是否精确。我在项目的第二版采集流程里增加了“距离从近到远、水平方向从中间到两侧、板面倾斜角从正对到30度”的规范性操作标定精度提升非常明显。这个方法没有任何神秘之处就是尽可能让约束覆盖到位。5. 最后说几句个人经验把整套流程跑通之后我最大的感触是标定这件事前期的数据质量决定上限后续的优化算法决定下限。数据采得好哪怕优化算法简单一点也能得到不错的结果数据采得差再高级的优化也无法补救。我的建议是正式标定前先做一遍小规模预试验采集五六帧数据跑通流程看看投影效果确认设备、环境、代码都没问题再正式采集十几帧数据做最终标定。这样能尽早发现问题避免录了半个小时的bag最后发现板没放好或者雷达没扫描到板。另外想补充的是输出外参后最好把标定结果固化到系统配置里并写一个简单的自动检查脚本。之后每次开机用一把尺子或者预设位置快速验证一下外参是否仍然有效。机械结构如果经常拆卸外参大概率会漂移定期重新标定是必要的。这套平面约束方法我已经在多个不同的小车平台上跑过从十几厘米的小型巡逻车到半米左右的送餐机器人都能得到稳定的标定效果。如果你遇到的具体问题在文章里没覆盖到欢迎在评论区留言我看到会回复。最后再分享一个小技巧把标定板固定在一个可调节高度的支架上会让数据采集过程轻松很多不然每次弯腰举着板保持静止手真的会抖。