转载自：深蓝 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 与挪动底盘通信