三维视觉惯性SLAM的有效Schmidt-EKF
An Efficient Schmidt-EKF for 3D Visual-Inertial SLAM
论文地址：
摘要
为移动和可穿戴传感器系统实现厘米级精确定位具有重要的实际应用意义。本文提出了一种新的、高精度、高效的视觉惯性（VI）SLAM算法，称为Schmidt-EKF-VI-SLAM（SEVIS），该算法以紧密耦合的方式将IMU测量和单目图像最佳融合，以提供具有有界误差的三维运动跟踪。采用Schmidt-Kalman滤波公式，在状态向量中选择性地包含信息特征，同时在它们成熟后将它们作为干扰参数（或Schmidt状态）处理。建模的这种变化使得不再需要不断更新施密特状态（或它们的协方差），允许EKF正确地解释它们与活动状态的相互关系，节省了大量的计算量。在地图大小方面获得线性计算复杂度，而不是标准SLAM系统中的二次型。为了充分利用地图信息来约束导航漂移，提倡有效的关键帧辅助的二维到二维特征匹配，以找到当前二维视觉测量和三维地图特征之间的可靠对应。所提出的SEVIS在仿真和实验中都得到了广泛的验证。
本文创新点
该文件的主要贡献包括：
•设计了一种高精度、高效的基于Schmidt-e KF的VI-SLAM（即SEVIS）算法，该算法利用Schmidt-KF公式，允许对用于长期环路闭合的环境地图进行并行估计，以线性计算复杂度将导航漂移绑定。
•提出了一种关键帧辅助的2D-to-2D匹配方案，用于在不执行3D-to-2D匹配（这可能不适用于稀疏3D环境地图）的情况下，将2D视觉测量与3D地图特征匹配的具有挑战性的数据关联问题。这种二维到二维匹配不受估计性能的影响，允许长期的环路闭合和从极端漂移中恢复。
•在蒙特卡罗模拟和实际实验中广泛验证了所提出的SEVIS算法，显示了的系统提供的适用性和性能增益。对计算要求的实验研究进一步表明，所提出的SEVIS在建立和维护基于三维特征的地图时仍然是实时的。
Visual-Inertial SLAM
VI-SLAM过程将摄像机图像和IMU（陀螺仪和加速度计）测量数据进行最佳融合，提供传感器平台的6自由度姿态估计以及环境特征（map）的三维位置重建。将简要介绍EKF框架中的VI-SLAM，是SEVIS算法的基础。VI-SLAM的状态向量包含IMU导航状态xI和克隆的过去IMU（或相机）姿势xC的滑动窗口，如MSCKF[30]中所示，以及在全局帧中表示的地图特征的位置xS：
Schmidt-EKF based VI-SLAM
状态估计和协方差的EKF更新在map特征的数量方面具有二次复杂性[38]，这使得VI-SLAM的天真实现太昂贵而无法实时运行。利用SKF[44]，提出了一种新的用于VI-SLAM（SEVIS）的Schmidt-EKF算法，降低了这种二次复杂度。关键思想是有选择地将map特征视为状态向量（即Schmidt state xS（1））中的干扰参数，均值和协方差将不再被更新，与激活状态xA的互相关仍被利用和更新。提议的sevi的IMU传播与标准EKF相同。在接下来的内容中，主要关注单目图像的更新，这是SEVIS的核心，但是这种方法很容易扩展到立体系统。当相机IMU传感器对在环境中移动时，使用基于描述符的跟踪来跟踪特征。首先检测到快速特征[41]，并为每个特征提取ORB描述符[43]。OpenCV[36]“BruteForceHamming”KNN描述符匹配器用于查找对应关系，在前两个返回之间执行比率测试以确保有效匹配，并使用8点RANSAC来拒绝任何额外的异常值。一旦发现视觉轨迹，将使用三种类型的跟踪特征来有效地更新状态估计和协方差：
（i）具有机会性且只能在短时间内跟踪的VIO特征。
（ii）比上述特征更稳定且可以在当前滑动窗口之外跟踪的SLAM特征。
（iii）地图特征，是成熟和信息量大的SLAM特征，在一段独立的时间内保持在Schmidt状态。
为了执行SLAM和利用地图约束来限制导航错误的同时获得显著的计算节省，采用SKF方法[44]并将map特征视为干扰参数，跟踪它们与活动状态的相互关系，同时在更新期间仍允许它们的概率包含。特别是，计算施密特科夫的增益矩阵如下：
已经充分利用当前相机测量信息来更新SEVIS状态估计和协方差。算法1概述了该算法的主要步骤。
为了验证所提出的SEVIS的后端估计引擎，首先对具有已知测量特征对应关系的视觉惯性SLAM进行Monte Carlo模拟，其中单目视觉惯性传感器平台在圆柱竞技场内的圆形轨道上移动，观察一系列环境特征。传感器和弹道的仿真参数见表1。
特别是，比较了三种VIN算法，以揭示所提出的SEVIS的优点：
（i）基线VIO方法，包括增加6个SLAM特征的MSCKF（见[23]）。这些大满贯的特点是明确边缘化时，他们离开视野。
（ii）基线SLAM方法，使用相同的MSCKF窗口，但增加了90个SLAM特征。与上面的VIO不同，在这种情况下SLAM特性从未被边缘化，因此它们可以用于（隐式）循环闭包。
（iii）提出的SEVIS算法，由与基线VIO相同的MSCKF窗口和6个SLAM特征组成，同时增加了一组90个地图特征，这些特征被建模为干扰参数。当SLAM特征离开视场时，它们被移动到Schmidt状态，成为算法1中描述的地图特征。50次蒙特卡罗模拟运行的平均平方根误差（RSSE）性能如图2所示。
正如预期的那样，基线VIO随着时间的推移在方向和位置上累积漂移，而基线SLAM在没有长期漂移的情况下提供有界误差性能。有趣的是指出，位置RSSE根据相对于初始循环闭包的位置略有振荡。这是因为EKF与基于优化的方法不同，它不能重新线性化过去的测量值，因此修正这些误差的能力有限[48]。更重要的是，很明显，所提出的SEVIS算法也不会累积长期漂移，尽管其精度略低于基线SLAM。然而，考虑到SEVIS相对于map特征的数量具有线性计算复杂性，而基线SLAM具有二次复杂性，这种精度的降低是一个很小的代价。
Real-World Experimental Results
使用两种不同的误差度量：绝对轨迹误差（ATE）和相对误差（RE）来评估所提出的方法。将读者引向[54]了解这些错误度量的详细定义。除了的基线和建议的方法之外，还对VINSMono[40，39]进行了额外的评估，以提供与利用循环闭合信息的当前最先进方法的比较。如表2和图4所示，所提出的SEVIS能够高精度定位并在全基线VI-SLAM系统的水平上执行。从RE来看，很明显，包含map特征可以防止长期漂移，并且随着轨迹段长度的增长，RE几乎恒定，从而提供更高的精度。提议的SEVIS提供了一个计算上可行的滤波器，其精度与全基线VI-SLAM相似，具有与VINS Mono类似的竞争性能（尽管VINS Mono利用了批处理优化）。与全协方差SLAM相比，所提出的SEVIS算法的主要优点是降低了计算复杂度。这种方法的实用性在不同算法的运行时间中是显而易见的。如图5所示，评估了这三个系统并收集了实现的时序统计信息。
进一步挑战了在困难的室内夜间多楼层数据集上提出的系统，该数据集具有多个挑战，包括低光环境、长曝光时间和运动模糊的低对比度图像不适合正确的特征提取（见图6）。如果能够提取特征，由于噪声大、梯度小，得到的描述符匹配较差，与Vicon循环数据集相比，更新过程中使用的异常值较多，导致估计量跳变大，校正不正确。由于关键帧辅助的2D-to-2D匹配对估计性能差或漂移和地图特征更新具有不变性，因此所提出的SEVIS可以在这些场景中恢复，并防止错误的漂移。
Realsense ZR300传感器3用于采集20分钟的灰度单目菲舍耶图像，具有惯性读数，1.5公里的轨迹跨越两层楼。执行了摄像头到IMU外部的在线校准，以进一步确定制造商驱动程序提供的转换。最多允许700个地图点，以便充分覆盖地图区域，在4Hz时插入查询数据库以确保所有地图要素的充分覆盖的关键帧，以及一次处于活动状态的两个SLAM要素。由基线VIO和提议的sevi生成的轨迹如图7所示。显然，地图特征的加入防止了基线VIO所经历的长期漂移，这种漂移在偏航和z轴方向上都显示出很大的误差。由于此数据集没有可用的基本事实，作为一种常见的做法，计算了当传感器平台返回到起始位置时理想情况下应等于零的轨迹起点-终点误差。基线VIO的误差为4.67m（弹道距离的0.31%），而建议的SEVIS的误差仅为0.37m（弹道距离的0.02%）。