高精度多传感融合SLAM SOTA：可直接用于导航探索的实时地图

本文链接：https://blog.csdn.net/soaring_casia/article/details/136706498

论文标题：

Tightly-Coupled LiDAR-Visual-Inertial SLAM and Large-Scale Volumetric Occupancy Mapping

论文作者：

Simon Boche, Sebastian Barbas Laina, Stefan Leutenegger

1. 论文速览

本文提出了一种包括激光雷达、视觉传感器、惯性传感器三者在内的SLAM系统，在OKVIS的基础之上加入了LiDAR因子构建残差函数，同时使用局部子图构建全局一致地图策略完成了高质量的3D场景搭建，不仅使原框架在定位精度上得到进一步提升，同时在3D场景重建上也有着较高的性能表现。

2. 方案提出

自主导航是移动机器人在真实世界中完成各类任务的关键技术之一，除了高精度的状态估计之外，准确合适且具有全局一致性的3D场景表示也是必不可少的。本文提出了一种完全紧耦合的LiDAR-Visual-Inertial SLAM（激光雷达-视觉-惯性SLAM）和3D建图框架，该框架应用了局部子图构建全局地图的策略来实现其在大规模场景下的拓展，同时提出了一种新的（无对应关系、具有固定概率值的）激光雷达残差公式，其表达方式仅与占用场和相对应的占用场梯度有关。这些残差可以添加到因子图优化问题中：不仅可以作为实时位姿估计中帧到建图的因子，还可以作为调整各子图之间相互对齐的因子。实验结果表明，本文提出的方案在位姿估计精度上达到现阶段最优，同时也说明全局一致的基于体积的占用子图可以直接用于导航或探索等下游任务。

3. 技术要点

3.1 System Overview（框架总览）

在这里插入图片描述
图1｜LVI SLAM整体框架©️【深蓝AI】编译

系统输入为IMU测量值，双目相机采集的图片及无序的LiDAR点云；将状态估计和IMU测量值都集成在对应各点的时间戳上，考虑到激光雷达传感器的动态运动，加入了运动补偿模块；将测量值转换为对应的地图帧后，更新当前活跃子图的占用场。子图构建模块和LiDAR-Visual-Inertial状态估计器（以下简称为LVI）之间的紧耦合通过如图1中的两种方式进行实现。下面将对框架中的主要技术要点分别进行阐述。

3.2 Occupancy Mapping and Submapping（占用地图与子图构建）

本文提出的3D地图构建框架是一种基于体积的占位地图构建方法，其思想借鉴了基于八叉树的多分辨率体积占用建图框架Supereight2[1]：一种模板类型的高性能八叉树库，主要用于3D稠密建图下的SLAM系统，本文对该模板库进行了简要介绍，包括动态激光雷达优势及贝叶斯更新（相关论文及开源代码库地址置于文章末尾，供各位学习研究）。

在子图构建策略方面，主要思想是将SLAM系统中可用的传感器信息作为生成新子图时的决策标准：记录已观测空间和测量总数之间的比率，并将其对应与每帧之上，如果该比率低于某一个规定阈值时，则会使用下一个视觉关键帧创建新的子图（这一部分借鉴的思想来自于OKVIS2[2]）。

3.3 LiDAR-Visual-Inertial Estimator（激光雷达-视觉-惯性状态估计器）

状态估计器的baseline为OKVIS2，这一部分将会从两个方面（因子图及其优化、残差构建）展开叙述：

1）Factor Gragh and Optimisation Problem（因子图与优化问题）

在这里插入图片描述
图2｜因子图示意图©️【深蓝AI】编译

在使用视觉、惯性和位姿图因子的基础上，图2上添加了两种不同类型的LiDAR因子，对于实时优化窗口的每个状态量，都添加了关于最后一个完整子图的LiDAR因子，同时添加了地图到地图的LiDAR因子来约束各子图。状态量表示为：

$x=[ _Wr_S^T,q_{WS}^T,_Wv^T,b_g^T,b_a^T]$

状态量从左到右依次为在固定世界坐标系下IMU的位置与方向，IMU相对于世界坐标系的速度，陀螺仪和加速度计的bias。

2）Error Residuals（关于误差的残差函数）

这一部分包括了视觉重投影误差、IMU误差、相对位姿误差和基于子图的激光雷达误差四个部分，由于本文主要创新点在于LiDAR函数部分且前三种残差表示与OKVIS2中保持一致（文末关于OKVIS论文中对上述三种残差构建有详细描述），因此主要介绍LiDAR Residuals。

本文提出了两种类型的LiDAR残差，分别对应了两种不同类型的状态量：一方面，对于实时残差，这两个状态量选取的是当前帧与最后一个生成的子图相关联的关键帧；另一方面，地图到地图的残差构建都由子图关键帧组成的，从关于误差的残差函数来说，两者没有区别。

对于一个给定的子图关键帧位姿和另一个已知位姿的点云图，残差表示为：

$e_l^{a,b}( _{s^{\alpha}}p)=\frac{d}{\sigma}=\frac{L( _{s^{\alpha}}p)}{\sqrt{\frac{L^2_{min}}{9}}+\sigma^2_z|\nabla L( _{s^{\alpha}}p)|^2}$

其中 $_{s^{\alpha}}p=T_{_{s^as^b}p}$ ，这里所表示的测量点均在3D场景（地图）中的表面上，并且点与最近表面的距离d可以通过占用值和占用场梯度进行推断，该公式推导建立在线性传感器模型基础之上，下面公式分别计算的是距离d和地图的不确定度：

$d=\frac{L}{|\nabla L|},\sigma_{map}=\frac{L_{min}}{3|\nabla L|}$

其中 $L_{min}$ 是一个配置参数，表示的是最小对数占用值，整体的不确定度由地图不确定度和传感器测量不确定度两部分加权表示：

$\sigma=\sqrt{\sigma^2_{map}+\sigma^2_z}$

残差雅可比的解析推导详细过程在原文中有推导介绍，内容与多源传感器下状态误差函数的推导过程大同小异，对严谨数学公式推导有研究兴趣的同学可以阅读原文学习。

4. 实验结果

本文实验分为LVI定位精度对比实验和地图构建实验，从定量角度分析了本文提出的LVI SLAM在定位精度上的优势，从定性角度分析了本文地图构建框架在全局一致性上的优越性能。

4.1 LVI SLAM 定位精度对比实验

从表1的对比实验可以看出在OKVIS2添加了LiDAR因子后，显著提高了OKVIS2在定位上的准确性，同时也可以看出本文提出的框架实现了最为准确的位姿估计。

在这里插入图片描述
表1｜LVI SLAM定位精度对比实验©️【深蓝AI】编译

4.2 地图构建效果图

图3展示的是Exp21序列所有生成子图的叠加效果（水平切片图），其中蓝色部分表示自由区域，绿色部分表示估计的运动轨迹，可以证明本文提出的框架可以提供准确的3D重建及自由活动区域，并且可以直接供机器人进行导航等任务，子图之间几乎不存在明显的不一致现象。

图4处理了来自Leica BWK2Fly无人机上记录的数据，以此定性分析本文提出的方法可以适用于真实场景下搭载不同LiDAR传感器的无人设备。

4.3 计算效率（耗时）：

5. 结论

5.1 本文的主要贡献

（1）提出了一种基于紧耦合优化的LVISLAM和地图构建框架，利用局部子地图来确保在构建场景时地图的全局一致性。

（2）引入了一种新的激光雷达残差公式，该公式直接对地图占用值和占用场梯度进行操作，避免了计算开销较大的数据关联步骤，且这些残差可以通过两种方式添加至因子图中进行优化。

（3）通过设计实验将不同的基于激光雷达传感器的地图构建方法在定位精度和地图质量两个方面进行对比分析，证明本文框架在这两种评价指标下为SOTA。

5.2 未来工作展开

（1）探索一种更接近真实场景下的基于激光雷达传感器的不确定性模型，尝试在激光雷达扫描结果下采样时结合几何特征。

（2）针对于视觉前端容易出现跟踪丢失问题，优化框架以提高其鲁棒性。

引用：
【1】https://arxiv.org/abs/2010.07929
【2】https://supereight2.readthedocs.io/en/latest/
【3】https://arxiv.org/abs/2202.09199

编译｜Lean_loves_lulu

审核｜Los

移步公众号【深蓝AI】，第一时间获取自动驾驶、人工智能与机器人行业最新最前沿论文和科技动态。