基于关键帧的视觉惯性SLAM闭环检测算法-AGV吧

1 引言

即时定位与地图构建（simultaneous localization and mapping，SLAM） [1]，指机器人在自身位置不确定的条件下，在未知环境中创建地图，同时利用地图进行自主定位与导航。由于传统视觉SLAM算法在准确性和鲁棒性方面存在问题，因此视觉惯性SLAM 算法在计算机视觉和增强现实领域越来越多地引起人们的关注[2]。视觉惯性SLAM算法即为在传统视觉SLAM算法中加入惯性测量单元（inertial measurement unit，IMU），算法中的相机可以提供丰富的环境信息，用于构建3D模型；IMU提供自主运动信息，可以在短时间内提供陀螺仪及加速计的估计值。通过相机信息与IMU信息的融合，可以增强算法的准确性及鲁棒性[3]。

视觉惯性 SLAM 算法可以分为两类：松耦合（loosely-coupled） [4-6]和紧耦合（tightly-coupled） [7-12]。其中松耦合系统为先通过视觉算法估计位姿，将得到的位姿与IMU数据进行融合，这种融合方法可以减小算法的计算复杂度，但是算法的准确度较低；紧耦合系统为相机和IMU测量数据直接进行位姿估计。由于算法的准确性较高，紧耦合算法逐渐成为视觉惯性SLAM算法研究中的一个热门方向。

紧耦合算法又可以分为两类：基于滤波和基于关键帧。2007年Mourikis和Roumeliotis [7]提出了一种基于扩展卡尔曼滤波（extended Kalman filter，EKF）的实时单目视觉融合算法，被称为多状态约束卡尔曼滤波器（multi-state constraint Kalman filter，MSCKF），提出按照时间顺序维护一个位姿的滑动窗口，如果特征点在滑动窗口的几个位姿都可被观测，就会在这几个位姿间建立约束，从而进行 EKF 的更新。 2015年Blösch等人 [8]提出了基于直接EKF的方法，通过兴趣点测量值直接使用EKF来估计相机位姿和 IMU偏差。

2010年Strasdat等人[13]提出了在使用相同计算资源的情况下，基于优化的算法比基于滤波的算法可以得到更准确的效果，使得基于优化的算法逐渐引起人们的关注。2013年Leutenegger等人[9]验证了在使用稀疏关键帧的情况下，基于非线性优化的视觉 SLAM算法比基于滤波的算法更为准确。2015年 Leutenegger等人[10]提出了一种基于关键帧的算法，在局部优化中优化相机位姿和IMU误差，跟踪和重建图像中兴趣点（interest-point）的 3D 位置。2016 年 Usenko等人[11]提出了一种类似于非线性优化的方法，通过在局部优化中使用直接图像对齐而不是间接使用兴趣点测量值来得到全局的连续地图。2017 年Forster等人[12]提出了一种基于预积分（preintegration）理论的视觉惯性里程计，将预积分理论应用于 SO(3)模型中，将IMU测量数据与图像中稀疏子集的直接跟踪相结合，从而加速了优化计算。

基于紧耦合方法的视觉惯性SLAM算法大多都不具备检测闭环和地图重利用的能力，即使相机不断重新访问相同的位置，得到的轨迹也会出现累积漂移现象。2011年Jones等人[14]提出的基于滤波的视觉惯性SLAM算法可以检测闭环，但是不能实时地建立全局地图。2017年Mur-Artal等人[3]提出的基于关键帧非线性优化的方法可以检测闭环，但是由于算法在跟踪和局部优化过程中会固定状态量，因此对初值的要求很高，需要一个复杂繁琐的初始化过程。

视觉SLAM算法另一重要环节为视觉定位，即为在离线环境下构建地图，然后在已构建地图内进行定位的算法[15-17]。Mur-Artal等人[17]在2014年首次提出了基于关键帧技术的SLAM算法的重定位，可以以图像帧速率处理包含数千个关键帧的地图的重定位问题。Lynen等人[16]在2015年提出一种在移动设备上运行的视觉惯性定位算法，使用地图压缩和图像匹配技术在预先建立的基于兴趣点的地图上来定位相机。但是，这些算法仅实现了设备的重定位，并不具有继续建图的能力。

本文算法在边缘化图像帧之后，通过非线性优化技术和闭环检测技术来得到并行的全局连续轨迹，从而可以有效地检测闭环和全局优化，建立连续的全局地图，此外算法可以在已获得的地图进行重定位和具有继续建图的能力。

本文算法分为五部分：

（1）提取特征点进行特征点匹配，运用RANSAC[18]（random sample consensus）算法排除误匹配得到位姿，并通过匹配特征点所占的图像区域与所有检测到特征点的图像区域的比率选取关键帧。

（2）进行局部优化，最小化图像位姿信息的二次投影误差和IMU误差，得到局部连续轨迹估计。

（3）边缘化图像帧和关键帧，保持局部优化计算量在一个恒定的范围。

（4）在全局地图中通过图像检索策略找到与当前关键帧匹配的关键帧，从而形成闭环，然后利用RANSAC算法验证匹配并得到位姿，最后进行全局优化得到较准确的地图。

（5）重定位和继续建图。使用和闭环检测类似的图像检索算法找到一系列连续的匹配关键帧对作为重定位假设，并根据新的关键帧与已构建的地图关键帧之间的约束关系继续进行SLAM地图的构建。算法框图如图1所示。