论文笔记_S2D.62_VIL-SLAM_立体视觉惯性激光雷达同时定位和建图

小灰灰 2023-01-01 03:55 316阅读 0赞

**目录**

基本情况

摘要

I. 介绍

II. 相关工作

III. 系统概述

IV. 视觉前端

V. 视觉惯导里程计

VI. 激光雷达建图

VII. 基于雷达增强的回环检测

VIII. 实验结果

IX. 总结

参考

--------------------

# 基本情况 #

*  标题：Stereo Visual Inertial LiDAR Simultaneous Localization and Mapping
 *  作者：Weizhao Shao, Srinivasan Vijayarangan∗, Cong Li∗, and George Kantor
 *  出处：Shao, W., Vijayarangan, S., Li, C., & Kantor, G. (2019, November). Stereo visual inertial lidar simultaneous localization and mapping. In *2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)* (pp. 370-377). IEEE.

对比的开源代码：[https://github.com/laboshinl/loam\_velodyne][https_github.com_laboshinl_loam_velodyne]

# 摘要 #

同步定位和地图绘制（SLAM）是移动和空中机器人技术的一项基本任务。 与基于视觉的系统相比，**基于LiDAR的系统**由于其准确性和鲁棒性已被证明具有优越性。 尽管其优越性，基于纯LiDAR的系统在某些退化的情况下仍会失败，例如穿越隧道。 我们提出了立体视觉惯性激光雷达（VIL）SLAM，（论文效果：）在这些退化情况下性能更好，并且在所有其他情况下具有可比的性能(comparable performance)。

VIL-SLAM通过将

*  紧耦合的**立体视觉惯性里程计**（VIO）
 *  与**激光雷达建图**
 *  和激光雷达增强的**视觉闭环**

相结合来实现这一目标。该系统实时生成环闭合校正的6自由度**激光雷达姿态**和接近实时的**1cm体素**稠密点云。与最先进的激光雷达方法相比，VIL-SLAM显示了更高的精确度和鲁棒性。

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70][]

**（a）传感器设备（b）构建室外场景模型**

# **I. 介绍** #

SLAM解决了在估计机器人状态的同时映射未知环境的问题。尽管SLAM在过去的几十年中得到了积极的研究，但Cadena等人仍然对此进行了研究。 \[1\]注意到在处理**不同的环境**和**长期连续运行**方面仍然存在挑战。 SLAM系统在多种传感器模式下运行，每种传感器试图利用其优势。在过去的几年中，基于LiDAR的SLAM系统由于其对环境变化的鲁棒性，已经超越了基于视觉的系统。

但是，基于纯LiDAR的系统有其不足之处。

*  它们在具有**重复结构**（如隧道或走廊）的环境中会失败。这些环境很难映射和定位，因此必须部署能够利用所有传感器模式优势的系统才能成功。

我们建议使用VIM-SLAM，它使用IMU，立体摄像机和LiDAR，并共同开发其优势。

我们的实验表明，在大多数情况下，VIL-SLAM的性能可与基于纯LiDAR的系统相提并论，并且在纯基于LiDAR的系统完全失效的情况下，性能更好。 VILSLAM通过将立体VIO和LiDAR映射与闭环集成在一起来实现这一目标。据我们所知，这是此类工作的第一个。此外，我们介绍了一种使用**飞行时间激光扫描仪（Faro）评估地图结果**的方法。我们还将在EuRoC MAV数据集上提供VIO验证结果。

VIL-SLAM使用

*  **紧耦合的立体VIO**，该VIO执行固定**滞后的位姿图优化**，
 *  使用**稀疏3D**功能进行**地图配准**的LiDAR建图，
 *  以及将**视觉回环检测**与**稀疏点云**对齐，以实现回环闭合(loop closure)。回环闭合使用**增量解算器**优化**全局位姿图**。

VIL-SLAM旨在在不同环境中长期稳定运行。

*  **高频IMU**测量产生的估计值对于较短的时间间隔是合理的，但会很快漂移。当受**立体视觉测量的约束**时，我们可以纠正偏差并估计准确的相对运动（称为VIO）。
 *  **相对运动估计**用于辅助**LiDAR扫描匹配**，然后累积高保真3D点云以形成准确的地图。

机器人的状态估算值会在长距离遍历期间累积漂移。

**闭环**

*  通过使用视觉或LiDAR方法识别重新访问的站点来解决此问题。视觉方法包括使用词袋\[2\]识别位置，使用透视n点（PnP）算法估计姿势校正。
 *  在LiDAR方法中，使用基于片段的算法（例如**SegMatch** \[3\]）识别位置，并使用**迭代最近点（ICP）**\[4\]算法估计姿势校正。

虽然词袋法快速且通用，但它缺乏使用ICP的慢速但稳健的LiDAR方法的准确性。 VIL-SLAM使用一种混合方法，

*  首先使用Bagof-Words技术找到**闭环候选者**，
 *  然后使用**Perspective-n-Point（PnP）算法**生成姿态校正的粗略估计，
 *  最后使用**ICP细化**粗略估计。

# II. 相关工作 #

当前的VIO文献介绍了各种公式，以整合视觉和惯性数据。文献中有

*  紧耦合系统\[5\]-\[7\]（其中视觉信息和惯性测量被共同优化）
 *  或松耦合系统\[8\]-\[11\]（其中IMU是一个单独的模块，与仅视觉状态估计器相融合)。

这些方法可以进一步分为

*  基于滤波的\[11\] – \[16\]
 *  或基于图优化的\[5\] – \[7\]，\[17\]，\[18\]。

基于紧密耦合的**基于优化**的方法，利用迭代地最小化残差的优势，通常可以以更高的计算成本获得更好的精度和鲁棒性。

在我们的工作中，我们通过以无结构的方式表达路标(landmarks)，并仅针对固定大小的姿态图进行优化，以实现实时性能来限制计算成本。

仅使用**激光**扫描仪的当前最先进的SLAM系统是\[19\] – \[23\]，其中需要运动模型，即恒速模型或高斯过程。文献\[24\]中的方法**结合了立体相机和激光扫描仪**。它具有从视觉测距法（VO）生成的运动估计，并通过匹配激光扫描进行完善。

*  我们系统的区别在于它们使用**多分辨率网格地图**表示，而我们使用**稀疏点云**来定位和输出稠密点云。

而且，与VO相比，VIO通常更健壮和准确\[25\]。使用IMU，单眼相机和激光扫描仪的VLOAM \[26\]与我们的系统最相似。

*  一个区别是，我们使用**紧密耦合的VIO作为运动模型**来初始化LiDAR mapping算法，而VLOAM使用**松散耦合的IMU和摄像机**。

尽管我们的VIO更加强大，但VLOAM拥有一个更具**交互性**的系统，可以将来自摄像头和LiDAR模块的信息用于IMU偏差校正。

*  VIL-SLAM具有的另一项功能是LiDAR增强的环路闭合功能。

# III. 系统概述 #

该系统具有四个模块，如图2所示。

*  **视觉前端**从立体相机获取立体图像对。它执行帧到帧跟踪和立体匹配，并输出立体匹配作为视觉测量。
 *  **立体VIO**进行立体匹配和IMU测量，在姿势图上执行IMU预积分和紧耦合的**固定滞后平滑**。该模块以IMU速率和摄像机速率输出VIO姿态。
 *  **LiDAR mapping 模块**使用来自VIO的运动估计并执行LiDAR点变形（dewarping）和扫描帧到地图的注册(scan to map registration)。
 *  **闭环模块**进行视觉环路检测和初始环路约束估计，并通过稀疏点云ICP对齐进一步精细配准。约束所有**LiDAR姿势**的全局姿势图被增量式优化，以获取（**输出:**）**全局校正的轨迹**和**实时的LiDAR姿势校正**。它们被发送回LiDAR mapping 模块以进行地图更新和重新定位。

在**后期处理**中，我们将**扭曲的LiDAR扫描**与**最佳估计的LiDAR姿态**拼接在一起，以获得密集的映射结果（图5）。

![图片][19dc0c34568a229dff31bc19453f5d77.png]

**图2. VIL-SLAM系统图 传感器为灰色，模块为绿色。箭头指示消息在系统中的流动方式。深色粗箭头**表示系统实时输出**，浅色粗箭头**表示近实时后处理生成的输出**。**

![图片][0b8895ca882efa667ef93c13dd6e5ebb.png]

**图6 实验装置 使用两个VPs校准相机机**

## **IV. 视觉前端** ##

视觉前端接受一个立体图像对，并执行**帧到帧跟踪**和**立体匹配**，以生成一组立体匹配的稀疏特征点，即立体匹配(stereo matches)。立体匹配可以是前一个立体对中的一个，也可以是该对中新提取的一个。逐帧跟踪性能直接影响时间约束质量，而立体匹配则有助于限制比例。这两个任务对于任何立体视觉测距法都是至关重要的。

直接方法在最近几年显示出强大而有效的时间跟踪结果\[8\]，\[27\]。因此，我们使用Kanade Lucas Tomasi（KLT）特征跟踪器\[28\]来跟踪先前的立体匹配中的所有特征点（在左侧还是右侧图像中）。仅当它们都被跟踪时，我们才有跟踪的立体匹配，并将其推送到输出中。较大的立体基线有助于比例估计，并减少了因遥远特征而引起的退化问题。我们使用基于特征的方法，比KLT更适合处理大型基准。如果跟踪的立体匹配项的数量低于阈值，我们将使用Shi-Tomashi Corner检测器\[29\]执行特征提取，然后执行特征消除过程，在该过程中，删除与任何现有特征的像素坐标距离小于阈值的特征。然后，对所有幸存特征计算ORB（定向的FAST和Rotated Brief）\[30\]描述符，然后进行蛮力立体匹配以获得新的立体匹配。系统通过在第一对立体对上执行立体匹配来初始化。

## **V. 视觉惯导里程计** ##

立体VIO的目标是在相对较高的频率上**提供实时准确的状态估计**，作为**LiDAR建图算法**的运动模型。在位姿图上运行的紧耦合固定滞后平滑器在精度和效率之间是一个很好的权衡。基于优化的方法通常允许多次线性化以接近全局最小值。固定滞后位姿图优化器进一步限定了变量的最大数目，因此计算成本是有界的。由于糟糕的视觉测量会导致收敛问题，对视觉测量强制执行严格的异常值拒绝机制。系统通过检查**平均重投影误差**来消除异常值。

![图片][6386971a5dd86e403e3fe0252be95496.png]

**该算法以IMU预积分因子和无结构视觉因子作为约束条件**

## **VI. 激光雷达建图** ##

激光雷达建图在进行**激光雷达点提取**和**扫描帧点云到地图配准**之前，使用高频IMU速率VIO姿态作为运动。将扫描c表示为从一个完整的激光雷达旋转获得的点云。从c中提取几何特征，包括锐边上的点和平面上的点。然后，基于当前扫描到地图的特征点（所有先前的特征点），通过最小化特征点形成的欧几里德距离残差，将其作为一个**优化问题**来解决。

## **VII. 基于雷达增强的回环检测** ##

回环检测对任何一个SLAM系统都是至关重要的，因为长期运行会引入漂移。环路闭合的目的是通过对**全局位姿图进行优化**来消除漂移，该全局位姿图结合了LiDAR建图的回环约束和相对变换信息。为了更好地辅助激光雷达制图，校正后的激光雷达姿态被实时发送回来，以便新扫描的特征点被配准到重新访问的地图上。

*  本文提出在**视觉词袋回环检测**和**PnP回环约束**公式的基础上**增加ICP对齐**。该系统采用增量求解器iSAM2对全局姿态图进行优化，实现了实时性。

![图片][6118427f96399afbce47c9b0030ce04d.png]

# VIII. 实验结果 #

评估了VIL-SLAM，并将其与基于最佳实时激光雷达的系统LOAM2在自定义数据集上进行了比较。还使用EuRoC MAV数据集评估立体VIO子模块（VIL-VIO）。

这里提供了五个典型环境的结果，包括无特色的走廊、杂乱无章的高台、隧道和室外环境。所有这些序列的数据收集在同一点开始和结束。基于最终漂移误差（FDE）对里程计（LiDAR建图姿态）进行评估。以Faro扫描为基本真实值，用平均配准误差（MRE）对建图结果进行评估。首先将地图与模型对齐（Faro扫描），然后计算地图点与模型中最近点之间的欧几里德距离。里程表FDE和建图结果如表一所示，较好的用粗体显示。

![图片][5a7c5d778a361f1a5eec5c561a60e008.png]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 1][]

**图6. 地图比较：VIL-SLAM（右）和LOAM（左）与模型相比的地图配准误差。0.3m以上的误差（a-b）用红色表示，0.5m以上的误差（c）用红色表示。蓝色和绿色内部不连续的红色区域是由于扫描设备的遮挡导致的模型缺失。**

# IX. 总结 #

VIL-SLAM是一种先进的里程计和地图构建系统，旨在在**不同的环境中长期稳定**地运行。当前的框架将VIL-VIO和激光雷达建图松耦合在一起。我们正在将其扩展到一个紧密耦合的框架中，以便从**LiDAR建图得到的精确姿态估计**可以用于**IMU偏差校正**。在循环闭合中对**扫描帧之间**的**稀疏特征点进行ICP精细化**获得更好的回环约束。

# 参考 #

*  [立体视觉+惯导+激光雷达SLAM系统 点云PCL][SLAM_ _PCL]

[https_github.com_laboshinl_loam_velodyne]: https://github.com/laboshinl/loam_velodyne
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70]: /images/20221120/ae1c2fbd7f0d4e64af00428794636b75.png
[19dc0c34568a229dff31bc19453f5d77.png]: /images/20221120/70f7f2b5ed204a3eb1c9c078a41c58f8.png
[0b8895ca882efa667ef93c13dd6e5ebb.png]: /images/20221120/f33c232cf5f840008e42bd77bf89242b.png
[6386971a5dd86e403e3fe0252be95496.png]: /images/20221120/02006502e0254963922f3aa845cdf0ea.png
[6118427f96399afbce47c9b0030ce04d.png]: https://img-blog.csdnimg.cn/img_convert/6118427f96399afbce47c9b0030ce04d.png
[5a7c5d778a361f1a5eec5c561a60e008.png]: https://img-blog.csdnimg.cn/img_convert/5a7c5d778a361f1a5eec5c561a60e008.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 1]: https://img-blog.csdnimg.cn/2021071620484064.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ==,size_16,color_FFFFFF,t_70
[SLAM_ _PCL]: https://mp.weixin.qq.com/s/cFzj-fRs1AlCooLKRlr2BA