基于多传感器融合的定位和建图系统

转载自：深蓝AI
分享嘉宾：林家荣
文稿整理：William
编辑：一点人工一点智能
原文：基于多传感器融合的定位和建图系统

01  传感器介绍

IMU(加速度计)的测量频率高，即可以精确的测量到物体的姿态运动，对运动灵敏，同时成本低，体积小，抗干扰能力强，基本上在多传感器融合中是一个必备的传感器。

但IMU也有局限性，第一点是测量值存在随机游走的bias，第二点是由于通过积分的形式计算出那个姿态，因此会存在累计误差，导致长时间的积分结果不可靠。因此常是IMU+雷达或IMU+视觉相机进行计算。

因为视觉相机比较轻小，价格便宜，所以一般是基于视觉相机的SLAM被广泛应用在无人机，AR/VR等轻载重的平台上。但视觉相机也存在局限性，第一是单目相机无法直接获得可靠的3D几何信息，第二是基于多视图的方法可以获得3D几何信息，但需要消耗大量的计算资源，而且在视觉纹理缺失、大量纹理重复场景下的效果比较差，第三是相机的SLAM方法对环境光照的要求会比较苛刻。

激光雷达(LiDAR)传感器可以提供高精度稠密的3D点云，因此可以直接获得六自由度的状态估计，还能重建出周围环境高精度的3D地图。但同样也存在局限性，首先是激光雷达传感器本身就又贵又大又重，使得无法用在一些轻量的平台上，第二是分辨率相比视觉相机是比较低，同时刷新率也低，约为12Hz，第三是帧内的点云会存在运动畸变，第四点是在 周围环境特征不足的情况下，会发生退化现象。

在市区里面，到底是用视觉相机跑SLAM还是用激光雷达跑SLAM实验更合适点？这里更偏向采用视觉相机+激光雷达的方式，相机用来观测周围环境，激光雷达直接测量环境的3D几何形状。在特性排序上，相机获得的图像是有序的，连续的2D测量，雷达获得的是无序的，离散的3D测量。

02  激光雷达(惯导)SLAM

2.1 首个基于固态激光雷达的SLAM系统(loam-livox)

近期激光雷达的发展一般都致力于降低设备成本的同时提高有效性，其中备受关注的是各种固态雷达，比如微电机系统扫描，光相位阵列，Risley棱镜等。因此，在这样的背景下，loam-livox工作是以livox-MID40为代表的首个固态激光雷达的SLAM框架。livox-MID40和传统雷达对比如图1所示，可以看出来红色livox-MID40的FoV比蓝色的传统雷达是小的多，右图中是livox-MID40的扫描轨迹图。

图1 livox-MID40和传统雷达对比

在此项工作之前，有许多优秀的方法，比如：2014年提出的使用激光雷达完成定位与三维建图的LOAM算法, 提出了一种特征点的提出方法，使用点-面ICP的形式，完成了基于一个单线激光的3D定位和建图工作; 2018年提出LeGO-LOAM，其相对于LOAM的提升主要在于轻量级和地面优化，即在前端使用了分割模块来区分地面点和非地面点，接着对这些点进行特征提取，后端是设计了一个加速算法，进一步提高算法的运行速度。使得LeGO-LOAM在各种平台甚至是Nvidia的Tx2平台上实现了实时的定位和建图。

由于固态雷达的特性，例如非重复扫描的模式不能作frame-to-frame的匹配，小FoV的雷达对feature点要求更高，对异常值更敏感，更容易受动态物体的影响，因此这些优秀的算法并不能直接适用了固态激光雷达。所以，实现了首个适用于固态激光雷达，能够实时鲁棒低漂移的SLAM系统。系统框图如图2所示，先做特征点提取，然后进行面到面的匹配后去更新地图，重建KD树以供下一次的搜索。

图2 loam-livox系统框图

点云去帧内运动模糊如图3所示，一开始使用线性插值(图3(b))，发现效果不太好，后面使用分段处理(图3(c))，即把一帧分为三段，然后去重新估计姿态，因为三小段上帧内的运动模糊不明显。图3(d)是开源手持设备和采集场景。

图3 去帧内运动模糊

2.2 激光雷达-惯导紧耦合的里程计(FAST-LIO)

在此项工作之前，也存在许多优秀的工作。LIO-mapping 是香港科技大学刘明组所提出的一个紧耦合的雷达惯导融合方法，在LiDAR Odometry 融合了IMU信息以进一步提高定位精度和鲁棒性。此外，为了获取雷达位姿的更可靠估计，提出了一种旋转约束精炼算法(LIO-mapping)来进一步对齐雷达位姿和和全局地图。LIO-Mapping借鉴LOAM和VINS-Mono的思想进行的一个联合状态估计问题。工程层面和VINS-Mono一样，只不过前端视觉部分换成了lidar的前端进行特征关联和odometry的位姿估计。其主要分为两个部分：tightly coupled lidar-IMU odometry与the rotation constrained refinement。LIO-SAM是Tixiao Shan，在LeGO的基础上，提出了一种紧耦合激光-惯性里程计方法，通过优化包含LiDAR里程计因子，IMU预积分因子，GPS因子和回环因子来得到机器人的全局一致的位姿。作者使用帧-局部地图匹配代替LOAM的帧-全局地图匹配，提高了帧图匹配的效率。但是他们都或多或少存在一些问题，导致他们在实际使用中并不够鲁棒，精度不够高，运行速度慢的问题。

因此在此类背景下提出了FAST-LIO系列。FAST-LIO是一个高效的紧耦合的雷达惯导融合框架，是基于迭代卡尔曼滤波器，滤波器里提供雷达观测和IMU观测，主要贡献在于降低了大规模观测数量导致的计算量。此外，还提出了FAST-LIO2，其主要创新在于使用了一个增量式KD树去维护地图的更新，有效解决了传统方法中重建时导致地图越来越大的问题。

FAST-LIO框架如图4(a)所示，首先以雷达点云作为数据，然后进行特征提取后进行去帧内点云模糊(图4(b))，一帧内的点云通过IMU预积分的方式将其统一到帧末，以此消除掉真正的点云运动模糊。最后通过迭代卡尔曼滤波器方法去紧耦合的融合激光雷达和IMU观测。

图4 FAST-LIO框架

实验是在实验室中拿一个手持设备不断地剧烈运动来得到周围环境建图，结果如图5所示，可以看到无论是精度上，还有计算效率上都会比同行的方法有比较明显的优势。

图5 FAST-LIO实验对比

FAST-LIO2的系统框图如图6所示，其主要创新的第一点是引入增量式更新的KD树，第二点是使用的原始的雷达点，没有特定点提取的步骤，由于使用了更多的点，反而在精度上比使用特征点的方法有所提升，第三点是重力加速度上使用了S2更新。

图6 FAST-LIO2框架

增量式更新的KD树之所以会比同行的方法有明显的优势，主要因为增量式K的引进。传统的KD树里，去搜索K最近邻的点，然后构成一个点到面的残差时，会用到KD树，它是一种平衡的数据结构，也是最优的knn搜索，但并不支持增加式的。于是，提出了增量式K的数据结构，首先是支持增量式的更新，其次是可以在线的做动态的树平衡。

模型耗时评估结果如图7所示，即计算比较每雷达帧的处理时间。通过跑相同处理的雷达帧的结果对比，可以发现存在非常明显的优势。

图7 模型耗时评估对比

03  多传感器融合(激光雷达-惯导-视觉)

3.1 首个开源的多传感器紧耦合方案(R2LIVE)

现在的激光雷达--惯导—视觉的融合还是学术界的一个非常热门的方向，很多地方的问题还解决的不是很彻底。LIC-Fusion 是19年提出的雷达-惯导-相机的多传感器融合算法，可以有效地融合IMU测量，稀疏的视觉特征和提取的激光雷达点。此外，还能够在三个异步传感器之间执行在线的空间和时间传感器标定，以补偿可能的标定偏差。随后，LIC-Fusion 2.0又提出了一种新的平面特征跟踪算法来更加有效地处理激光雷达测量，以进一步提高算法的精度和鲁棒性。LVI-SAM是在LIO-SAM的基础上，融合了VINS-MONO的观测，整个系统分为VIO和LIO系统，VIO系统通过单独处理相机+IMU数据给出一个里程计结果, LIO系统通过提取匹配雷达特征给出一个雷达里程计结果，最后，通过联合优化完成系统的状态估计。

在这样的背景下，提出了R2LIVE的多传感器融合方案，系统框图如图8所示。该模型也是VIO和LIO两个子系统，LIO跟FAST-LIO框架差别不大，VIO是有两步观测，第一步是用迭代卡尔曼滤波器完成3D点的观测，第二步去更新相机的状态估计。由于卡尔曼滤波器耗时比较低，所以能够及时的计算出相机姿态，然后后端有个图优化是因为需要不断优化视觉的3D点位置，否则更新出来的地图会产生较大误差。

图8 R2LIVE框架

迭代卡尔曼滤波器如图9所示，当输入一帧雷达点云时，上一帧是相机的更新帧，预积分到一个时刻，然后提供一个姿态估计的先验，之后雷达点云到达时就完成完成一次迭达卡尔曼滤波器的更新。当几个状态估计完成之后，下一帧如果来的还是那个视觉的观测，那就把雷达观测的预积分跟到相机来的时间戳上，再去完成更新。要做视觉和雷达的融合的原因在于提高系统整体的鲁棒性。

图9 迭代卡尔曼滤波器示意图

3.2 实时真彩地图重建(R3LIVE)

R3LIVE的系统框架如图10所示，也是分为LIO和VIO两个子系统，LIO是基于FAST-LIO2改进的一个系统，VIO是考虑的一个融合视觉相机的方案。R3LIVE相比于R2LIVE的区别在于，R3LIVE会考虑一些相机的光度学模型，即相机的镜头阴影、相机的响应函数等方面。

图10 R3LIVE系统框架

光度学模型如图11所示，在建图中，认为一个点在不同相机观测中的颜色值是一样的，但是没有考虑相机曝光的影响或镜头阴影及响应应函数的影响，导致它有可能一个点插在不同的相机帧里。R3LIVE++每个点是重建的辐射光场，即反射自然光中的一个点的辐射程度通过彩色相机的模型，然后去最终计算出相机的灰度值观测。

图11 R3LIVE++光度学模型

VIO中存在两个更新，一个是frame-to-frame(图12左)，还有一个是frame-to-map(图12右)。之所以需要frame-to-frame更新是因为直接使用frame-to-map的更新是比较慢的，因为一开始可能没有收敛到一个合适的收敛值。frame-to-frame的VIO更新是上一帧跟踪的点，通过光流的方法去预测下一帧的当前帧，通过迭代优化全状态，使得两个观测点的像素距离最小，最后完成更新。

图12 VIO中的两个更新方式

04  激光雷达的实时定位和网格(mesh)重建系统(ImMesh)

4.1 ImMesh的介绍和实验演示

R3LIVE重建的彩色点云是要得到雷达在地图里面全环境的辐射信息，才能够重建出地图的纹理，且必须要求每个点覆盖到了才会有。然而，点云的密度有限，即不能在没有点的地方看着这个点，现有一种解决方案是重建地图的网格(mesh)模型，然后用相机得到图像RGB信息后贴到网格上，这样能够无损的去重建地图纹理。在这个背景下，做了一个在线激光雷达的实时定位和网格重建系统。主要贡献是完成了一个实时的、在线的网格重建。

网格重建的相关工作首先一个是泊松重建，其原理是通过一个3D的标量方程，然后去拟合3D空间中的点云，使得拟合的标量方程在每个采样点的梯度方向上和输入点的靶向量是尽可能的匹配，再通过优化3D的标量方程，之后去提取等值面的方式把这个面给提取出来，示意图如图13所示。但是泊松重建的速度比较慢，另一方面是在点间距的边缘上会出现一些莫名凸包。

图13 泊松重建示意图

离线方法中比较典型的是德劳内三角剖分+图分割的方法，如图14所示，基本原理是第一步先用3D的德劳内三角剖分对3D空间做四面体的切割，然后再通过相机和观测点的可视化关系，再去做图分割，以此区分去面片的inside和outside，最后是提取这些等值面。这些等值面就是重建的出来的mesh。

图14 离线重建方法

在线方法比较经典的是TSDF+Marching cubes，如图15所示，其原理是通过并行更新每个体素的TSDF，后面用Marching cubes的方法把面片给提取出来。TSDF的更新可以做到实时，如采用GPU加速，但在提取表面时候的速度会比较慢一些，因为是隐式的重建方法。

图15 TSDF+Marching cubes在线重建

尽管现在有这些工作，但它们在激光雷达上依旧无法做到实时性。因此，希望去构建一个实时的网格重建，因为一方面是是效率快，第二方面是能够实时的给你一个预览反馈。ImMesh系统框架如图16所示，它是直接以雷达和转换器为输入，如果有IMU则也会拿来做雷达点云的去运动模糊。定位模块第一步是去点云的运动模糊，第二步是做状态估计，然后第三步是做点云配准，完成之后存入数据结构中，即启动在线的match模块，首先取点云，然后进行降维处理，第三步是做增量式的三角形面片更新，最后把更新结果推送到全局地图里。所以，ImMesh能够同时输出系统的一个实时状态，还有实时输出雷达匹配准之后的点云，以及重建的三角形面片。

图16 ImMesh系统框架

这里在线网格重建的方法先是使用分层体素，如图17所示，首先是用分层体素对空间进行一些划分，比如说有L1，L2，L3，之后用哈希表的形式将这些体素给储存起来，然后分类管理。所以，每次雷达点云配准之后，会更新一些局部的地图，这时每个体素里存在一个状态，如果体素中插入新的顶点，体素中会有一个标志位去标志，让网格重建模块去更新这个体素里面数据。

图17 分层体素示意图

在取出点之后，将3D的一个个点通过图形的方法去投影到一个合适的平面内，这个合适的平面是由voxel-map估计的一个地图在此点的平面法向量，然后进入到那个平面去。因为雷达点本身对环境进行测量的时候，这个点本来是在一个连续的平面上，也在一个区域内，如果这时在3D上直接做match，复杂度会很高，而且目前还没有成熟的方法。所以，这里采用直接用2D的视角去解决3D的这个问题，完成降维操作，之后再去进行局部区域的更新，最后利用增量式的machine进行同步到全局地图里面。

这个系统能够实现在数据采集的时候，同时对周围环境做在线的网格重建，演示如图18所示。这里有三个视图，蓝色框的是第三人称拍的，红色框是雷达点云的录入，黄色框是前置相机的预览，然后把这三个视图通过视频剪辑的方式将时间给合在一起，进行同步。

图18 系统演示

该方法不仅仅适用于固态雷达，还有旋转机械式雷达等。在不同的数据集上测试了这个算法的性能，结果如图19所示。可以发现在不同的数据集，不同的场景和不同的雷达上对该算法进行测试，结果表明这个算法都能够在这些测试区域中实现一个实时的重建。

图19 实验结果

4.2 基于ImMesh开发的应用

1）激光雷达点云增强

激光雷达点云增强应用如图20所示，第二行的雷达的扫描是一种玫瑰花色的扫描，导致这种雷达做learning算法的时候会出现一些问题。由于该系统能够实时的重建出周围环境的三角形面片，那就可以把这个三角形面片给观察化到当前帧的坐标系里面，这时就能够得到一个规则的深度图，然后再从深度图里面反投影。所以，雷达增强之后的点云就是一个一个规则排列的增强点云。如果相机的四张角变得更大，那么就可以获得更大范围的3D感知的雷达点云，就可以对远处观测的更稠密的一些。

图20 激光雷达点云增强

2）快速无损的场景纹理重建

刚才提到R3LIVE是需要每个点去记录环境的纹理，如果有match的形式，可以直接把采集的图像给贴图到面片上，因此就能够无损的对环境进行纹理重建，这里在线的快速纹理重建如图21所示。

图21 快速场景纹理重建

1、 书籍推荐-《机器人编程：使用树莓派3和Python构建和控制自主机器人》
2、 最新｜3D SLAM关于z轴精度的优化思路整理
3、 书籍推荐-《卡尔曼滤波与信息融合》
4、 机器人运动｜浅谈Time Elastic Band算法
5、 书籍推荐-《大规模SLAM技术》
6、 ROS与移动底盘通信