基于双步边缘化与关键帧筛选的改进视觉惯性SLAM方法

张小国，刘启汉，李尚哲，王 庆

（东南大学仪器科学与工程学院，南京 210000）

近些年，同时定位与建图技术[1,2](Simultaneous localization and mapping,SLAM) 逐渐成为一个非常重要的研究方向，其中单目SLAM 因其方案简单、成本低廉等优点在移动机器人领域得到广泛应用。但单目SLAM 在弱纹理区域难以工作，且存在快速运动时图像模糊、动态场景易发生匹配错误等问题[3]。针对上述问题，学者们提出了将惯性传感器与单目相机融合的方案[4,5]。这一类方案在视觉难以工作时可采用IMU 积分保持定位能力，在两者均能工作时可互相校正提供更好的定位和制图能力。将视觉里程计与惯性传感器的数据结合统称为视觉-惯性里程计(Visual-Inertial Odometry，VIO/VI-SLAM)。VISLAM 的组合导航技术已成为当前SLAM 领域的一大研究热点。根据不同需求有不同VI-SLAM 的研究方法，如研究视觉惯导融合的SLAM，或研究视觉惯导融合里程计，前者注重定位和建图，后者注重导航。根据图像和惯性测量单元（IMU）数据的耦合程度不同，又分为松耦合和紧耦合。其中，松耦合通常将两种传感器单独计算出导航信息，再将导航信息融合，如半直接视觉里程计(SVO)[6]算法；而紧耦合通常将两种传感器数据同时放在一起做优化，最后得出导航信息，如VINS-Mono 后端非线性优化算法[7]。在数据的融合方法中，可以分为基于滤波和基于图优化两大类方法。基于滤波的方法以扩展卡尔曼滤波(EKF)为代表[8]，S.Lynen 等人提出了多状态限制卡尔曼滤波(MSCKF)[9]算法，并且这种方法后来与传统的EKF-SLAM 相结合，使系统性能得到很大提升，适合计算资源有限的移动设备。但MSCKF 使用全局信息优化，缺少回环检测。随后Apostolopoulos 等人提出了鲁棒的视觉惯性里程计ROVIO[10]，在扩展的卡尔曼滤波器更新中使用直接强度误差作为视觉度量，但ROVIO 只适用于视觉特征点非常多的环境。除了滤波算法外，基于图优化中的非线性优化方法被越来越多广泛应用于SLAM系统。S.Leutenegger 等人提出了OKVIS[11]，建立了兼顾视觉重投影误差和惯性误差的优化函数，在滑动窗口上进行非线性优化，提高了OKVIS 的定位精度，但由于非线性优化增大了计算量，系统效率较低。相较于OKVIS，秦天等人提出了基于非线性优化的滑动窗口估计系统VINS-Mono，该系统在前端增加了一种动态的松耦合传感器融合初始化程序，使IMU 数据预积分后才与图像数据紧耦合，降低了对系统初始条件的限制。VINS-Mono 不仅从输出结果精度上优于基于EKF 的MSCKF、ROVIO，且在运行效率上远优于OKVIS。

VINS-Mono 虽然能在保持一定的运行效率的基础上实现较高精度SLAM，但存在非线性优化运算量较大的问题。为进一步提高系统的运行效率和定位精度，研究人员开展了两个方向的研究：一是提高硬件配置，如采用GPU 加速[12]或多平台协作运行[13]，二是改进软件算法，通过增加数据的有效利用以提高精度或改进运算方法减少计算时间，如设计混合滑动窗口优化器[14]，优化数据处理方式，或是改进后端算法[15]等。

基于VINS-Mono 系统原有算法，本文在系统后端使用双步边缘化的算法[16]，将Hessian 矩阵中需要边缘化的矩阵分步边缘化，从而将大矩阵拆分计算提高系统运行效率和集束调整的精度。同时，在系统前端，本文改进了图像关键帧筛选策略，提升图像特征点选取的可靠性并且更有效利用前端数据，从而提高了SLAM 解算的精度和鲁棒性。

1 系统框架

本文算法基于VINS-mono 系统改进，系统框架如图1所示。系统主要包括四个线程：前端图像跟踪(数据预处理和初始化)、后端非线性优化、闭环检测和闭环优化。跟踪线程采用光流跟踪图像相邻帧，后端优化用于优化误差保证最优估计，闭环检测和闭环优化用于检测整个系统环路并较正误差。

本文采用双步边缘化的方法，通过边缘化Hessian 矩阵进行非线性优化，降低矩阵的计算量。此外，在数据预处理阶段，采用了添加非关键帧滑动窗口的关键帧筛选策略来保证相邻关键帧之间的视差大小，能够适当降低系统的数据冗余，提高运行速度，保证系统平稳运行。

图1 系统框架Fig.1 System framework diagram

2 改进方法

2.1 基于双步边缘化的集束调整

VINS 系统在后端非线性优化时采用了固定的滑动窗口大小，当新关键帧进入滑动窗口时，需要舍弃滑动窗口里的最旧帧，以确保Hessian 矩阵的稀疏性。但是若直接删除最旧帧，会直接破坏最旧帧对滑动窗口内相邻帧的约束。

因此采用边缘化的方法将最旧帧包含的约束信息继续保存在Hessian 矩阵中，使系统能够在不计算最旧帧帧内信息的前提下，保留最旧帧对滑动窗口内相邻帧的约束。同时，本文采用了一种双步边缘化的方法，相关流程图如图2所示。当滑动窗口有新的关键帧进入时，系统需要边缘化窗口中的最旧帧，根据因子图显示的关系将需要边缘化的矩阵块移动至Hessian 矩阵左上角。

同时，为了减少计算成本，本文根据状态之间的相关性将要边缘化的状态分为独立状态或从属状态。根据Schur 边缘化的方法，按照先边缘化独立状态，再边缘化从属状态的方法分步计算，以此来降低矩阵计算量，提高集束调整的精度。

图2 双步边缘化算法流程图Fig.2 Flow chart of two-step marginalization algorithm

2.1.1 系统后端误差优化方程的构建

基于视觉/惯性里程计前端得到的跟踪定位结果，在后端对系统状态量和观测量进行优化。系统定义的状态变量主要包括：滑动窗口内n+1 个相机的位姿xi（0≤i≤n）、加速度计偏差ba和陀螺仪偏差bg、以及相机到IMU 的外参（包括位置和旋转）和m+1 个3D 点的逆深度λj（0≤j≤m），因此可以表示为：

本文中，为了求解最优的状态变量X*，定义系统后端的误差优化方程(后续简称为目标函数)为：

式(1)中，目标函数中包含的三项依次为边缘化先验误差、IMU 测量残差和视觉重投影误差，使目标函数取最小值的X即为最优状态变量X∗。

目标函数的优化求解使用 Levenberg-Marquardt(LM)法。假设系统有一个较好的初始变量，系统通过最小化目标函数，求解最优状态变量X*。将目标函数记为f（X），进行Taylor 展开后得：

其中J（X）为f（X）关于X的导数。因此求解的非线性最小二乘问题为：

对式(3)关于ΔX求导，并取零，得：

式(4)可简写为：

其中，H=J（X）TJ（X），b=-J（X）f（X）。

2.1.2 基于Schur 的边缘化

当新的关键帧进入滑动窗口，系统开始边缘化最旧帧。边缘化的对象包括最旧帧的相机位姿x和观测到的部分路标点。当最旧帧与滑动窗口内其他帧存在共视，即观测到相同的路标点，边缘化会将约束保留在与最旧帧存在共视的其他关键帧上。

确定需要边缘化的路标点和最旧帧位姿后，将优化方程HΔX=b根据边缘化状态，写为：

式(6)将优化方程中的∆X按照最旧帧中需要边缘化和保留的部分分为δxa、δxb，对应的H矩阵块分别是Λa、Λb和Λc，对应的b矩阵块分别是ga和gb。

对式(6)采用Schur 消元：

式(6)～(8)，实现了Schur 边缘化。

2.1.3 双步边缘化算法

在Hessian 矩阵中，主要的元素包括路标点、相机位姿以及速度、加速度和IMU 偏差。

为了减少计算量，本文根据状态之间的相关性，将需要边缘化的状态分为独立状态或从属状态。独立集合中的任何两个状态都不相关，即独立集内的每个状态仅与从属状态相关。本文定义路标点的状态是独立的，而相机位姿以及速度、加速度和IMU偏差是从属状态。同时，本文以分步的方式边缘化状态量，即独立状态在从属状态之前被边缘化。

如图3所示，虚线框住的部分表示要边缘化的状态，其中Li（1≤i≤4）为路标点，Pj（1≤j≤4）为相机位姿，bk（1≤k≤4）为IMU 偏差。其对应的Hessian矩阵如图4所示。边缘化的状态为x1=[L1,L2,L3,P1,b1]，其中仅P1和b1是从属状态的。直接与边缘化状态x1的有关联的状态为x2=[P2,b2,P3,b3,P4]。图4-6 矩阵中的黄色块代表路标点自相关的矩阵块，绿色块表示将边缘化的路标点与位姿之间的矩阵块，其余蓝色块表示非零矩阵块。红色框表示会被边缘化影响的部分。

图3 含边缘化部分的示意图Fig.3 Schematic diagram with marginalized parts

图4 对应于图3的Hessian 矩阵Fig.4 The Hessian matrix corresponding to Fig.3

图5 边缘化L1后的Hessian 矩阵Fig.5 The Hessian matrix after marginalization of L1

在边缘化L1后，再计算实现对路标点L2的边缘化，同理将所有路标点边缘化后的H矩阵如图6所示。

图6 边缘化所有路标点后的Hessian 矩阵Fig.6 The Hessian matrix after marginalization of all landmark points

完成所有独立状态的边缘化后，再对图6矩阵中的P1和b1进行边缘化，其中Ann、Anm、Amn为需要边缘化的H矩阵，Amm为保留的H矩阵。通过计算完成边缘化，即一步完成所有从属状态边缘化。以上就是双步边缘化算法，其算法步骤可以由算法1 表示。

算法1:Two-step marginalization of Hessian matrix Input：-边缘化变量的维数m-全体变量的维数pos-滑动窗口中的关键帧矩阵H.-边缘化路标点个数n-当前边缘化的路标点Li Output:-完成双步边缘化的矩阵H根据m 和pos 计算第二步边缘化矩阵的维数r根据n 个独立状态和r 维从属状态排列pos 维的边缘化数据for 0;;ii n i=＜++根据Schur 计算路标点 i LP 的边缘化end for计算r 维从属状态的边缘化

2.2 基于滑动窗口的关键帧筛选

图像帧输入频率变高会提高输入数据的冗余程度，但冗余信息不能提升系统性能且浪费了计算资源，因此需要在众多图像帧中筛选出关键帧。筛选关键帧的目的是降低图像信息冗余度，提高系统运行效率，该过程一般遵循两个最基本的原则：其一是保持滑动窗口内相邻的关键帧存在一定的视差，防止信息冗余；其二是当快速运动导致采集的相邻帧视差过大时，确保跟踪的特征点不会丢失。

在筛选关键帧过程中，涉及到关键帧滑动窗口和非关键帧滑动窗口，在本方法中只有关键帧滑动窗口参与后端非线性优化和边缘化，而非关键帧滑动窗口不参与，且窗口大小固定为2。当前帧定义为非关键帧滑动窗口内的最新帧，当有图像帧进行关键帧判断时，无论是不是关键帧，非关键帧滑动窗口都进行帧的替换。当图像帧参与关键帧筛选时，该帧已经通过了系统前端的特征点提取和光流跟踪。系统通过遍历当前帧的特征点计算数值参数作为关键帧的判定条件。在满足以下条件中任意一项时，定义当前帧为关键帧，并触发边缘化：

算法通过遍历当前帧的特征点，计算当前帧内信息并判断以下条件：

1）如果关键帧滑动窗口内关键帧数量小于两帧或者共视点数n1（滑窗中的特征点在当前帧中继续被追踪到的点）小于threshold1，则视当前帧为关键帧，在实验测试中根据测试效果阈值threshold1 最优为20。

2）如果当前特征点在关键帧滑动窗口最新帧和关键帧滑动窗口最新帧之前任一帧内存在，那么该特征点就定义为平行特征点。当平行特征点数n2为0，则视当前帧为关键帧。

3）定义非关键帧滑动窗口内两帧之间匹配的特征点数为n3，当前帧内的所有特征点数nnum。当则视当前帧为关键帧。实验测试中根据测试效果阈值threshold2 最优为0.001。

4）若当前特征点为平行特征点，则通过计算当前特征点在当前帧和关键帧滑动窗口最新帧内的归一化坐标的欧式距离作为视差l。设当前帧和滑动窗口最新帧匹配的平行特征点在归一化平面的坐标分别为 （x1,y1） 和 （x2,y2），视差若≥threshold4，则视当前帧为关键帧。

关键帧筛选策略的算法步骤可以由算法2 表示。

算法2：Key frame selection strategy Input：-当前帧K 已完成特征点提取-滑动窗口大小N-在关键帧滑动窗口中的帧 （）iK≤i ＜N-在非关键帧滑动窗口的帧T-当前帧K 中特征点jP 的数量n（0≤ j ＜）n Output:-当前帧K 是否为关键帧For ;;0 jj n j=＜++0计算共视点的数量 n1计算平行特征点的数量 2n计算前一个非关键帧T 与当前帧K 之间匹配特征点的数量 n3 ifjP 是平行特征点计算t 和K0 中jP 的归一化坐标的欧几里得距离lj l+=l总距离 j end if end for if 1 2 Nn threshold1＜＜或return true elseif n2=0 return true else if 2 2 n / n threshold＜return true else if 2 4 l n threshold≥return true end if

3 实验与分析

为验证本文算法对应的运算速度和定位精度，采用面向视觉惯性SLAM 算法的EuRoC 数据集进行验证[17]。实验硬件平台参数如表1所示。为了评估改进了关键帧筛选和边缘化算法的系统，使用绝对轨迹误差（ATE）度量。本文将输出结果与SVO-MSF[18]、OKVIS、ROVIO 和VINS-Mono进行了比较[19]。SVO-MSF 是使用半直接视觉里程计（SVO）和多传感器融合（MSF）[20]结合的松耦合算法。OKVIS 是基于开放关键帧的视觉惯性SLAM，它在关键帧姿势的滑动窗口上利用了非线性优化。ROVIO 是鲁棒的视觉惯性里程计，是基于扩展卡尔曼滤波器（EKF）的视觉惯性状态估计器。VINSMono 是使用预积分IMU 因子的基于非线性优化系统，是目前综合性能较优的SLAM/IMU 组合导航算法。如表2和表3所示，在测试的5 个EuRoC 数据集中，本文方法相较于VINS-Mono 在运行速度上平均提升14.91%，在输出轨迹精度上提升率有17.59%。

表1 硬件平台参数表Tab.1 Hardware platform parameters

为了获得更详细的系统效果，本文将估计的轨迹与EuRoC 数据集的轨迹真值（GroundTruth）进行比较，输出结果如图7所示，图7(a)是VINS-Mono系统估计的轨迹结果，图7(b)是本文的系统估计的轨迹结果。需要指出的是，在图7中可以通过观察输出轨迹与数据集真值的误差看到改进后精度的提升,由于轨迹图路径较为复杂，在图7第二列和第三列放大了局部轨迹以显示本文算法的优化效果。图8比较了本文的方法与VINS-Mono 在V1_01.easy 数据集中的误差，其中蓝色线表示本文的方法，红色线表示VINS-Mono。从图8可以看出，本文的系统比VINSMono 性能更稳定和准确。

表2 EuRoC 测试中运行时间的比较Tab.2 Comparison of running time in EuRoC test

图7 VINS-Mono 与本文系统在V1&V2&MH 测试数据集的输出轨迹图Fig.7 The output trajectory graphs of VINS-Mono and our method in V1&V2&MH test data set

表3 EuRoC 测试中绝对轨迹误差的RMSE 比较（单位：米）Tab.3 RMSE comparison of absolute trajectory error in EuRoC test (Unit：m)

图8 数据集EuRoCV1_01.easy的各方向误差对比图Fig.8 Comparison chart of each direction error of thedata set EuRoCV1_01.easy

4 结论

针对目前主流视觉/惯导组合SLAM 系统后端优化中计算代价较大的问题，本文提出了一种改进边缘化策略和关键帧筛选策略的SLAM 方法，支持以更高的效率输出更高精度的位姿计算。该系统通过增加非关键帧滑动窗口之间的图像帧约束关系，使图像数据更有效地被使用，基于Schur 边缘化的原理，通过将因子图中的因子分类，并根据因子类别对应的矩阵进行分步边缘化的方法，将高维矩阵分步拆解进行运算，降低了运算难度，并提高了运算精度。

采用公开数据集EuRoC验证本文算法。实验表明，本文的方法相较于VINS-Mono系统在平均运行速度上平均提高了14.91%，且定位精度也有一定改善。