1. 为什么是这四种传感器组合先搞清楚各自在系统里干什么活做过多传感器融合项目的人应该都有这种体会单靠激光雷达做SLAM建出来的地图局部看着没问题跑到楼道拐角或者长走廊里就开始飘回环一闭合误差直接把人整崩溃。我一开始接触三维重建项目时也觉得激光雷达不是精度很高吗为什么实际跑出来的点云还是歪的等到把视觉、IMU和RTK加进去之后才明白这套组合真正解决的不是“传感器数量”而是每个传感器的短板被其他传感器补上了。激光雷达干的事是提供几何结构。市面上常见的机械式或者固态式雷达比如Livox MID-360这类近距离相对精度能做到2厘米以内测距稳定、不受光照影响这是视觉系统体会不到的优势。但它也有天生缺陷在长走廊、开阔场地、缺少特征的环境下点云配准会退化因为没有足够多的几何约束来锁定位姿。另外一个问题是激光扫描出来的点云没有颜色信息想要做带纹理的三维模型光靠雷达是远远不够的。视觉相机的任务刚好反过来它能提供高分辨率的纹理信息和丰富的特征点在激光退化的时候视觉特征还能帮一把。但单目相机没有尺度信息双目或RGB-D相机又受到光照、反光、视野范围的限制。你让相机单独跑估计几秒钟之内漂移就能到不可用的程度。IMU在这个系统里承担的是短时间高频率的运动预测。IMU输出频率能到200Hz甚至更高在两次激光帧之间、两次图像帧之间用IMU积分可以推算出相对运动。但IMU的致命弱点是积分漂移加速度计的零偏和陀螺仪的温漂会让积分结果在10秒钟内就不可信。RTK解决的是绝对位置问题。前面几类传感器做出来的轨迹都是相对轨迹也就是说在某个起点之后系统只知道“相对于起点走了多远”并不知道自己在世界坐标系下的绝对坐标。RTK通过接收卫星信号和差分改正数据可以得到厘米级绝对定位这个精度在开阔环境下非常可靠但在建筑遮挡、桥下、树下会退化成浮点解或单点解所以它不能单独使用。把这四种传感器放在一起形成互补激光提供高精度几何和相对里程计视觉提供纹理和视觉特征约束IMU提供高频短时运动预测RTK提供低频绝对位置钉桩。这套组合下来前端的短时精度由激光和IMU保证中长期漂移由回环检测和RTK约束拉回最终输出带绝对坐标的厘米级三维点云。这套系统的典型精度预期是这样的在开阔环境下融合轨迹的绝对误差可以控制在3到5厘米内点云重建在近距离10米内的相对精度可以做到2厘米左右。如果场地里RTK信号一般或者遮挡多绝对误差会放宽到10到15厘米但相对形貌仍然能保持厘米级。2. 样机搭建选型、供电和固定方式里的工程细节很多第一次做多传感器融合的人上来就把传感器往机架上一绑然后开始刷代码结果数据跑出来各种诡异问题。我在这上面吃亏吃得很惨后来总结出一套从选型到装配的完整流程。2.1 传感器选型按预算和场景需求做取舍硬件选型没有绝对最优只有最合适。我用过的几套方案各有特点这里列出来给大家参考。层级激光雷达视觉相机IMURTK适用场景入门验证Livox MID-360 / 速腾聚创HeliosRealSense D455 / 双目模组雷达自带IMU或单独BMX160千寻/华测RTK模块室内外验证、校园场景工程级速腾聚创RS-Helios-5515 / 禾赛XT32工业全局快门相机 广角镜头战术级6轴如ICM-20602或更高端双天线RTK/INS组合变电站、厂区、市政测绘深度研究机械式64线/128线多相机阵列高精度光纤陀螺加速度计组合基站自建PPP-RTK高精度工程测量、科研入门级别用雷达自带的IMU省钱省事但要注意自带IMU通常性能一般振动较大时容易激励出噪声。做研究项目的话我建议单独配一块IMU尺寸小、重量轻安装位置固定后续标定和调试会省很多心。相机方面如果只是做纹理贴图卷帘快门相机也能凑合但要做运动场景下的双目深度估计全局快门是底线。2.2 机械固定刚体约束是融合的前提传感器融合算法里默认一个前提所有传感器是刚性固连的它们的相对位姿不随时间变化。这句话看起来简单实际工程中却容易被忽视。我在第一版样机上犯过一个很低级的错误IMU直接通过双面胶粘在碳纤维板上激光雷达支架用的是单点云台快拆。跑了一个上午的数据位置精度开始漂查来查去最后发现IMU在剧烈加速过程中居然有微小的松动外参标定结果完全失效。硬件上不刚算法再努力也没用。正确的安装方式是这样的所有传感器统一固定在一块加工过的金属基板或碳纤维板上安装面做去毛刺和平面度检测IMU用螺丝固定最好加一个小的定位销防止转动雷达和相机支架采用三点定位加锁紧螺钉的方式确保拆装之后能回到同一个位置。如果预算允许直接CNC加工一个集成传感器仓把相机、IMU、雷达都嵌进去这才是最省心的方式。固定稳定之后还需要记录每个传感器在机架上的大概位置关系。比如雷达的坐标系原点在机器人的什么位置、相机的光心在哪个方向、IMU安装在雷达左侧还是右侧这些信息在前期标定和问题排查时特别有用。2.3 供电与数据带宽最容易翻车的隐形工程多传感器系统的能耗比很多人想象中大。Livox MID-360功耗大概十几瓦工业相机两三瓦工控机几十瓦RTK模块几瓦加起来看着不多但如果用电池供电线缆压降和电流纹波会导致传感器复位或数据丢包。我给这套系统配过12V锂电池组和稳压模块实测下来发现一个问题激光雷达启动瞬间电流尖峰很高如果稳压器不给力电压跌落会让雷达重启重启的时间戳会出现跳变整个数据链路就断了。后来换了带缓启动功能的宽压稳压模块问题才解决。数据带宽上激光雷达点云数据量和相机数据量叠加起来普通的USB集线器和百兆网口根本扛不住。激光雷达一般走网口相机走USB3.0或GigEIMU走串口或USBRTK走串口。做系统集成时建议把相机单独接USB3.0独立控制器雷达接千兆网口避免共用带宽导致丢帧。3. 时间同步毫米级精度系统的地基工程时间同步是我在做这套系统过程中踩过最深的一个坑也是很多开源方案没有仔细讲清楚的部分。简单说一下为什么要做时间同步传感器数据融合的前提是知道每帧数据是在哪个时刻采集的。如果时间戳不对齐激光帧和图像帧之间的对应关系全乱了融合算法拿到的是一堆错位的信息这比没有同步还要糟糕。3.1 时间戳不对齐会造成什么后果举一个实际现象来说明。我用没有做硬件同步的数据跑过一次融合定位跑完看轨迹整体形态是对的但每秒会出现微小跳变点云地图上出现了规律性的重影。进一步对照原始数据才发现激光雷达和IMU的时间戳差了大约20毫秒20毫秒在车辆正常行驶状态下意味着几厘米到十几厘米的运动误差。这个量级刚好和厘米级重建的目标冲突。时间误差还会出现在相机上。相机曝光时间如果是10毫秒那么图像内容的实际时刻应该是曝光开始到曝光结束的中间时刻而不是数据包到达电脑的时刻。不处理曝光中点时间动态场景下的像素位置就会和IMU预测的位姿存在系统偏差。3.2 硬件同步和软件同步两条路硬件同步是专业系统的主流做法核心思路是给所有传感器一个共同的时钟基准。RTK模块一般会输出PPS脉冲信号和GPRMC语句PPS是秒脉冲它上升沿对应的整秒时刻是高精度授时源。把PPS接到主控板或转接板上通过GPIO触发IMU采样同时给激光雷达和相机提供外部触发信号这样所有传感器的内部时钟就和UTC时间对齐了。软件同步不需要额外接线逻辑是用各传感器自己的时间戳通过插值或最近邻查找把不同频率的数据在算法层面对齐。这套方案实现方便但精度一般到10到20毫秒级别对于低速移动的静态重建场景够用一旦运动速度上去误差就不可控。实测下来我强烈建议做硬件同步。具体的接线方式不复杂RTK模块的PPS输出接到一个STM32单片机或者主控板的定时器捕获引脚单片机记录PPS时刻同时给相机发出硬件触发信号触发拍照给IMU发出采样脉冲。激光雷达如果是非触发式的它的数据自带高精度时间戳主控板只需要在收到PPS之后做一次本地时钟校准即可。时间同步做好之后判断数据质量的简单方法是查看融合算法输出的时间标签是否平滑用软件画出各传感器时间戳间隔曲线正常情况下应该是稳定且没有突跳的。4. 空间标定把传感器坐标系统一到同一把尺子下硬件和时间都解决之后接下来是空间标定。多传感器系统里每个传感器都有自己独立的坐标系激光雷达的原点在雷达中心相机光心在镜头内部IMU在芯片位置。融合之前必须精确知道这些坐标系之间的旋转和平移关系。这个关系就是外参。外参精度直接影响融合质量。以我做的项目为例激光雷达和IMU之间的外参旋转误差如果超过0.1度融合后的轨迹在10米距离上就会产生接近2厘米的偏差。所以标定不是跑个脚本出来就行要做准确性验证。4.1 激光雷达和IMU的外参标定流程激光雷达和IMU之间的标定目前比较成熟的做法是用连续时间优化或者批处理的方式估计外参。核心原理是在手持设备快速运动的过程中IMU积分得到的轨迹和激光雷达点云配准得到的位姿之间的关系能够反演出两个坐标系之间的相对变换。实际操作中我一般按照这个流程走准备一个纹理丰富、几何结构明显的室内环境至少包含墙面夹角、柱体、箱子、桌椅这类有清晰几何特征的对象。如果场地太空旷激光配准会退化标定数据基本报废。手持设备做连续激励运动要让六个自由度都能被激发水平平移、前后平移、上下平移、绕三个轴的旋转。建议多做一些快速转弯、点头、画8字这些动作每个动作持续十几秒。整个采集过程大概两三分钟。采集时要注意雷达不能照射到强反射性材料比如镜面、玻璃幕墙这些会造成配准异常。采集完成后用LVI-SAM、lidar_inertial_align或者直接使用FAST-LIO的标定模式跑优化。输出结果一般包含外参的平移向量和旋转四元数。标定完成后要做验证。怎么验证呢把标定好的外参给LIO系统重新跑一遍数据看前端里程计的残差是不是显著降低。如果标定正确轨迹应该平滑点云拟合法线的一致性要提高。还有一种直观验证法把该帧点云用外参变换到IMU坐标系下再和IMU短时间预测的姿态对比重合程度高说明外参准。4.2 相机和IMU的联合标定要点相机和IMU的标定最常用的工具是Kalibr。它利用Apriltag或棋盘格标定板在相机保持静止、IMU连续运动的状态下估计相机到IMU的外参以及相机内参。关键点在于运动激励方式。和激光雷达与IMU标定类似相机-IMU标定也需要充分激励加速度计和陀螺仪。我试过把设备放在桌面上平移和旋转跑出来的标定残差很大后来把设备拿在手里快速旋转、加速标定质量明显改善。一个经验是每次运动方向改变之后稍稍停留一下可以给算法提供更丰富的约束。相机内参不建议用标定板做一次就算完。工业镜头在运输过程中受振动或者温度变化影响内参可能轻微变化建议每隔一段时间重新做一次。特别在室外温差大的环境里作业热胀冷缩带来的内参变化会导致视觉重投影误差明显上升。4.3 激光雷达到相机的坐标对齐激光雷达和相机的外参标定在实际项目里容易让人绕圈。方法不少可以用目标板方法把标定板同时放在相机视野和激光雷达视野内提取板上角点和雷达扫描线上的对应关系然后求PNP解算外参。这个方法精度高但操作起来比较繁琐每次更换设备位置都要重新标。在工程现场我更喜欢用自然特征方法直接采集一帧点云和一帧图像人工在两者之间点选十几个共同特征点比如墙角边缘、杆子顶端、路缘石拐角然后做一个基于点线约束的非线性优化。对于大多数场景这种标定的精度足够把纹理映射的误差控制在几个像素以内。如果项目涉及纹理重建相机的标定还有一个额外要求镜头畸变参数必须非常准否则纹理贴到三维模型上会出现扭曲和错位。建议标定图像采集时让标定板尽可能覆盖图像各个区域图像数量不少于30张。5. RTK绝对约束接入把局部地图钉到世界坐标上RTK接入是整个系统里让重建结果拥有“绝对地址”的关键步骤。没有RTK的时候系统能输出相对精确的地图但这个地图的坐标原点是你自己定义的和真实世界没有任何关联。测绘或者巡检场景里需要把重建结果放进统一的坐标系RTK就是那把钥匙。5.1 坐标转换流程从卫星坐标到当地平面坐标RTK模块输出的是WGS84坐标系下的经纬度高程或者在RTK模块里开启固定转换后直接输出当地平面坐标。在机器人系统里通常需要的是UTM坐标或者自定义的当地坐标系。我常用的流程是RTK原始输出的经纬度高程先通过标准椭球参数和高斯投影/UTM投影转换成平面坐标再根据测区情况做一个七参数或四参数转换把结果转到项目要求的坐标系。这个转换参数从哪里来要么由当地测绘部门提供要么在现场用多个已知控制点自己解算。有一种做法是直接用RTK模块内置的坐标转换参数很多国产RTK模块支持直接输出地方坐标系坐标。节省了很多软件工作但前提是模块内的转换参数配置正确否则绝对位置可能偏差几十厘米。5.2 RTK数据进入融合算法的工程实现方式融合架构上RTK属于低频绝对观测值典型输出频率是1Hz到10Hz而激光里程计是10HzIMU是200Hz视觉是30Hz。低频观测怎么和高频状态估计融合呢最常用的方案是因子图优化把激光里程计当成连续帧间约束把RTK位置当成全局位置约束每次收到RTK数据时往因子图里加入一个位置因子。因子的噪声设置很关键。RTK固定解的水平精度标称是1到2厘米加1ppm但实际使用中受多路径效应影响开阔程度不同会导致精度差异巨大。我的做法是开启动态协方差模式RTK上报固定解时给2厘米的位置噪声浮点解时给50厘米到1米的噪声单点解时直接不给这个因子建约束。如果RTK解状态频繁跳变融合算法会自适应降低RTK权重避免错误观测污染轨迹。差分龄期也要监控。RTK的差分龄期是指当前使用的差分改正数据距离基站发布时间过去了多久。龄期越大改正数越陈旧精度越差。一般差分龄期小于5秒时解算是可靠的超过10秒就要警惕超过30秒建议将该观测的协方差放大到浮点解级别。我在软件里写了个判断逻辑根据差分龄期动态计算RTK因子噪声大小差分龄期每增加1秒定位噪声对应增加20%。5.3 遮挡环境下的降级策略RTK在城市峡谷、隧道、桥下很容易失锁。这种情况下融合系统要能平滑降级保持在无RTK条件下的相对定位精度。我的策略是给建图模块一个绝对位置参考标记。在RTK信号还好的时候每隔一段距离给地图插入定位锚点当RTK失锁时系统靠激光里程计和回环检测维持位姿等RTK重新恢复固定解后新观测会通过因子图把全局误差重新拉回来。关键在于RTK恢复瞬间不要把整个轨迹一下子拉到新位置否则点云地图会出现断裂我使用渐变的鲁棒核函数控制位置修正的幅度。6. 从融合轨迹到三维重建点云坐标、后处理与精度验证融合定位做完了接下来是三维重建。很多人以为定位精度高重建的点云就一定好实操中并不是这样。点云重建的质量还取决于点云拼接方式、滤波策略、纹理映射和后处理。6.1 点云拼接与畸变补偿激光雷达在扫描过程中设备本身是运动的这就导致一帧点云里不同角度的点实际上是在不同时刻被扫描到的。机械式雷达通过旋转扫描扫描完一整圈需要50到100毫秒在这段时间里设备已经移动了好几厘米。如果不做运动畸变补偿直接把这帧点云当作刚体数据使用重建出来的墙面会扭曲。解决思路是激光点云中的每个点都对应一个采集时刻通过IMU和LIO系统提供的高频位姿把每个点都变换到统一时刻坐标系下。Livox MID-360这类非重复扫描雷达它的处理方式和机械式雷达不同需要按照每个点的精确时间来补偿。这一步做与不做重建质量天差地别。6.2 点云后处理的标准流程经过运动畸变补偿和坐标系转换后原始点云仍然会有大量噪声和离群点。我一般按下述流程处理后才能出模型按距离或者栅格做体素降采样把点云密度统一到可控水平。体素尺寸根据目标分辨率调整做厘米级重建时体素设为1到2厘米。使用统计滤波或半径滤波去除离群点。空气中漂浮的灰尘、树叶上的杂点、玻璃面的镜面反射点都是干扰源。对地面做平面拟合或者使用区域生长算法分割出来根据项目需求决定保留还是剔除。做室内重建时地面是重要结构做部件测量时通常需要去掉地面对轮廓的干扰。进行法向量估计和曲面重建。常用的有Ball Pivoting和Poisson表面重建。Poisson重建对噪声的鲁棒性更好但细节保留能力稍弱Ball Pivoting细节保留好但要求点云密度均匀。纹理映射是视觉传感器的重头戏。把高分辨率图像的颜色映射到三维网格模型上需要先通过相机的位姿计算可见性然后对每个三角面片选择最佳视角的纹理信息。这个环节如果相机位姿或者标定参数有偏差纹理就会错位墙面上的裂缝和管道上的文字会模糊。6.3 精度验证方法内符合与外符合重建完成后必须做精度验证不能凭肉眼觉得“看起来还行”。我在实际项目中使用两种方法。第一种是内符合检验即同一场景扫描两遍将两遍重建结果做点云配准看两份点云在大部分区域的偏差分布。如果两遍结果的均值和标准差都小于2厘米说明系统重复性好这是最基本的指标。第二种是外符合检验使用RTK或全站仪测量场景中的一些特征点比如墙角坐标、管道端头坐标然后重建点云中提取对应的点的坐标计算差值的均方根。实测中开阔场景下外符合精度一般能控制在3到5厘米封闭室内环境由于RTK信号遮挡绝对精度会稍差但相对精度仍然能到2厘米左右。有一组我从现场记录的数据可以分享在一个大约500平方米的厂房里用这套融合系统做重建布设了12个全站仪控制点作为验证融合重建点云与全站仪坐标的平均三维偏差为3.8厘米最大偏差为6.2厘米厂房内部墙面结构的相对偏差则为1.6厘米。7. 一路实测下来最值得记住的几条经验做这套融合系统前前后后折腾了几个月积累了一些可能对后来人更有价值的经验。这里挑几条最典型的来说。第一传感器之间的数据链路在正式跑场景之前必须反复验证。我遇到过一次很诡异的现象在实验室里一切正常出外场跑了20分钟轨迹开始漂移查了很久发现是RTK和IMU共用了一个USB转串口芯片外场电磁干扰导致串口数据偶尔被截断时间戳缺失了。软件上加了CRC校验和缓存重发机制之后才稳定下来。第二IMU的稳定性和温度关系非常大。在室内常温下标定得好好的IMU拿到室外暴晒或者冬天低温环境下零偏会发生明显漂移。这个不是标定一次就一劳永逸的建议在每次出测前做一次短暂静止初始化让系统重新估计零偏。如果设备刚从车上搬下来还是热的等温度平衡了再初始化。第三RTK的部署位置尽量离激光雷达远一点。RTK天线会引入金属反射体放在雷达附近会带来额外的多路径反射在点云里形成一圈一圈的干扰。我在平台车顶测试过两种位置天线紧挨着雷达时点云中的墙边出现一条虚幻的弧线把天线支架升高到雷达上方40厘米后这个问题就消失了。第四标定数据不要采集一次就永久使用。传感器经过运输、温度变化、多次拆装后外参可能会发生变化。我建议定期用自动监测脚本跑一遍标定验证看外参是否有漂移的迹象。如果项目要求极高精度每一次外场作业前做一个快速验证采集采集几十秒运动数据用LIO跑一下残差确认外参无显著变化再开始正式作业。第五回环检测在纯激光系统里相对难触发视觉特征能极大提升回环率。我做过一个地下车库项目纯激光跑完整圈回环没有触发加上视觉词袋匹配之后回环在很多帧都能检测到最终轨迹误差从1米以上降到10厘米以内。所以融合系统里的视觉部分不仅是做纹理用的它在定位方向上的价值同样巨大。第六关于数据回放和问题定位一定要把原始数据全部录下来。很多融合问题是在事后分析中才发现的如果只保存了融合后的轨迹和地图没有保存传感器原始数据那么出问题之后基本没办法排查。我现在的习惯是每次采集都同时记录rosbag文件、各传感器时间戳曲线和RTK状态日志有问题随时可以回放分析。目前这套系统还在迭代中。下一步我琢磨的方向有两个一个是接入更高级的语义分割信息把动态物体自动从重建场景中剔除让静态结构模型更干净另一个是把RTK失效场景下的融合策略做得更细用视觉和雷达联合的置信度估计来动态切换融合模式。这些内容等有了新进展再继续跟大家分享。