[论文阅读]：A Linear-Complexity EKF for Visual-Inertial Navigation with Loop Closures

摘要： 在未知环境中实现实时视觉惯性导航，同时实现有界误差性能，在机器人应用中具有巨大潜力。为此，本文提出了一种新的linear-complexity EKF用于视觉惯性定位，该EKF可以有效地利用回环检测约束，从而实现长期持续导航。关键思想是将Schmidt-Kalman公式应用到多状态约束卡尔曼滤波器（MSCKF）框架中，在这个框架中， 我们选择性地将关键帧作为状态向量中的扰动参数用于回环检测，但不更新它们的估计值和协方差，从而节省计算资源，同时仍然跟踪它们与当前导航状态的交叉相关性。因此，所提出的Schmidt-MSCKF仅具有O（n）的计算复杂度，同时仍将回环检测纳入系统。所提出的方法在大规模真实世界实验中得到了广泛验证，与标准MSCKF相比，性能有了显著提高，同时只产生了微小的计算开销。

I. Introdution

  视觉惯性导航系统（VINS）已成为最有前景的低成本3D定位解决方案之一，它融合了单目或立体相机的图像和IMU测量[1-7]。这种定位解决方案的优点是使用廉价且十分普遍的传感器，并且由于传感器的互补性，有可能提供与LiDAR等更昂贵的传感器在精度方面相当的姿态估计。虽然当前的VINS方法可以在短时间内表现良好（例如，参见[2-4,7,8]和其中的参考文献），但由于VINS可用的感知、内存和计算资源有限，它们对于长期和大规模部署来说不够稳健和准确。
  在本文中，我们试图通过开发高效的VINS来解决这个问题，将回环检测纳入系统中。回环检测约束能够提供有效的信息来修正随着时间累积的漂移，从而即使在未知环境中进行长期导航后，仍然能够保持有界的定位误差【9–11】。特别是，我们没有执行基于特征点的SLAM来构建环境的大型3D地图，而是基于标准的多状态约束卡尔曼滤波器（MSCKF）[1]， 并选择性地保持关键帧（发生回环的地方）的姿态，从而限制了状态的增长，因为单个关键帧可能会观察到许多特征。通过这样做，可以在EKF更新中利用当前窗口中的特征测量和回环检测特征观测。然而，随着轨迹长度的增长和关键帧不断添加到状态中，由于EKF协方差更新的二次复杂性，MSCKF的这种朴素扩展将很快在计算上变得难以实现实时性能。例如，我们根据经验观察到，标准EKF只能保留200多个关键帧并保持实时性。为了克服这个问题，我们利用Schmidt KF（SKF）[12]将状态向量中的关键帧姿态视为“干扰”参数，不更新它们的估计和协方差，同时仍然正确地跟踪它们与其他导航状态的互相关系。因此，所提出的Schmidt-MSCKF在有效利用循环闭合约束的同时，对于添加的关键帧数量只有O（n）的计算复杂度。

II. RELATED WORK

 SLAM同时估计机器人的位置及其周围环境的结构，并在过去三十年中吸引了大量的研究工作[13]。在轨迹上，定位误差可能会无限增长，除非检测到并使用一些全局信息（例如GPS或先验地图）或对先前位置的约束（即回环闭合）。许多方法利用来自不同关键帧的特征观测来限制轨迹上的漂移[4,14]。大多数都有一个双线程系统，可以通过优化一个小窗口的“局部”关键帧和特征，在短期内限制漂移，而后台进程则优化了一个包含循环回环检测的长期稀疏姿态图，以实现长期一致性[15-19]。例如，VINS-Mono[18,19]在局部滑动窗口和全局批处理优化中都使用了循环闭合约束。具体来说，在局部优化过程中，关键帧的特征观测提供了隐式的回环检测约束，同时，问题大小仍然很小，因为通过假设关键帧姿态是完美的（从而将其从优化中删除）。
 由于无法在不做出不一致的假设（如将关键帧姿势视为真实或重用信息）的情况下保持计算效率，因此包含长期循环闭合信息具有挑战性。Mourikis和Roumeliotis[20]介绍了一种混合估计器，该估计器使用MSCKF进行实时局部估计，并在环路闭合检测时触发全局束调整(BA)。这允许以一致的方式重新搜索和包含回环检测约束，同时要求过滤器等待束调整完成的大量额外开销时间。Lynen等人[21]开发了一种基于大规模地图的VINS，该VINS使用包含特征位置及其不确定性的压缩先验地图，并采用与先验地图中特征的匹配来全局约束定位。Zheng等人[22]开发了一种点线视觉惯性里程计（VIO）系统，该系统将边缘化关键点的3D位置视为未来环路闭合观测的真实位置，这可能会导致不一致。Julier[23]开发了一种二维SKF，可以跟踪地标的位置和协方差，同时通过次优更新规则保持计算效率。最近，DuToit等人[24]引入了Cholesky-Schmidt卡尔曼滤波器，然而，该滤波器使用具有完全不确定性的先验图，并放宽了映射特征与当前状态变量之间的所有相关性。与这些工作相比，我们积极估计与当前状态的相关性，并且与映射点相比，只添加关键帧姿势，限制了计算增长。

III. P ROBLEM S TATEMENT

 在本节中，我们描述了在标准MSCKF框架内具有环路闭合的视觉惯性定位问题[1]，揭示了其（至少）二次复杂度不适合实时性能。

A. MSCKF-based VIO

 基于标准MSCKF的VIO[1]的状态向量包含IMU导航状态x I和克隆的过去IMU（或相机）姿态x C的滑动窗口，即。

其中${}_G^I\overline{q}$从全局参考系{G}到IMU局部坐标系{I}[25]的旋转${}_G^{I}R$的单位四元数。$b_{\omega }$和$b_a$是陀螺仪和加速度计的偏差，${}^G{v}_I$和${}^G{p}_I$分别是IMU在全局坐标系中表示的速度和位置。克隆状态$x_C$以滑动窗口方式包含m个历史IMU姿态

 1)Propagation: 在MSCKF中，状态估计和相应的协方差块通过基于以下通用非线性IMU运动学对线性加速度（$a_m$）和角速度（${\omega }_m$）的输入IMU测量值进行积分而随时间传播

其中$n_a$和$n_{\omega }$是IMU测量的零平均高斯白噪声。我们在当前估计下线性化这个非线性模型，然后使用标准EKF[1]传播状态估计和协方差矩阵。
 2）Update： 当传感器平台在3D空间中移动时，使用KLT光流在图像平面上跟踪环境特征[27]。当特征丢失或达到滑动窗口大小时，我们使用特征轨迹信息进行更新。具体来说，我们假设以下堆叠非线性相机测量模型：

其中$n_f$是具有协方差$R_f$的高斯白噪声，${}^G{p}_f$是特征的3D位置。我们在当前状态估计下线性化该测量模型，并获得以下测量残差：

其中，$H_x$和$H_f$是关于状态和特征的测量雅可比矩阵， $\tilde{x}$和 ${}^G{\tilde{p}}_f$分别是状态和特征误差。由于特征不包含在状态向量中，我们通过将$r_f$投影到$H_f$的左零空间（i.e.,$N^T{H}_f=0$）来边缘化${}^G{p}_f$，以获得与特征无关的测量残差：

其中$n_f^{{}^{\prime }}=N^T$是协方差$R_f^{{}^{\prime }}=N^T{R}_{f}N$的高斯白噪声，可用于EKF更新[1]

B. Incorporating Loop Closures

 基于标准MSCKF的VIO[1]及其变体[2,3,7,28]已被证明能够在相对较短的时间内提供准确的定位解决方案，但对于长期大规模部署来说不够稳健和准确，因为这些系统本质上是open-loop里程计，其导航误差随时间无限增长。为了限制定位误差，即使在先验映射不可用的情况下，我们也应该加入回环检测约束。为此，我们可以连续包含可以检测到回环闭合事件的关键帧姿势，即状态向量：

其中$x_{S_i}=[{}_G^{I_i}\overline{q}{}^G{q}_{I_i}^T{]}^T$是第i个关键帧姿势。例如，如果一个人天真地将所有姿势都保留在状态向量中，他们基本上构建了批处理图SLAM优化，并可以包含所有可能的回环闭合，但这会导致问题大小随时间无限增长，不适合实时性能[29]。即使我们在MSCKF框架内仅将关键帧姿态连续包含在状态向量中，执行EKF更新仍然会产生状态向量维度dim（$x_k$）的二次成本，该成本会随着时间的推移而增加，并可能很快变得计算成本过高。

IV. SCHMIDT -MSCKF VINS WITH L OOP C LOSURES

 从上一节可以清楚地看出，简单地在MSCKF状态向量中包含关键帧以利用环路闭合信息不适合长期实时导航。为了解决这个问题，在本节中，我们详细介绍了所提出的线性复杂度的新型一致施密特MSCKF，同时将循环闭包有效地结合到基于EKF的VINS中

A. Overview

 如前所述，在将循环闭合约束纳入VINS时，瓶颈问题之一是关键帧姿态和/或关键点特征的存储和估计带来的计算复杂性。随着关键帧在轨迹长度上的添加，需要估计的状态大小将随着时间的推移而增长，威胁到实时VINS性能，尽管它的增长速度比将关键点特征添加到状态向量中要慢得多。一种避免在以后估计关键帧的方法是简单地假设关键帧姿势是“真实的”，并忽略与这些估计相关的不确定性，这将导致过度自信（不一致）的过滤器。相比之下，我们利用SKF进行高效估计，同时仍然跟踪状态中所有关键帧的不确定性。
 具体来说，我们通过随机克隆（stochastic cloning）方法【30】，在状态向量中保留了一组可能发生回环闭合的关键帧位姿（公式10），并持续跟踪它们与其他状态变量的相关性。我们通过在MSCKF的滑动窗口中添加来自历史关键帧位姿的附加观测值，对当前跟踪的特征隐式施加回环闭合约束。需要注意的是，这种方法不需要估计三维特征的位置，因为这些观测值仅是滑动窗口中的位姿和处理过的历史关键帧位姿的函数。这种做法带来了显著的计算节省，因为在轨迹上的关键帧数量通常远小于从这些帧中观测到的特征数量。此外，我们对SKF【12】进行了调整，将旧的关键帧位姿视为“扰动参数”，从而显著限制由于额外关键帧状态增加而带来的计算复杂度。这是因为在这种框架下，我们不会更新这些状态的估计值和边缘协方差，而仅更新它们与滑动窗口中活动状态变量的交叉相关性。通过这种方式，我们成功地将计算复杂度从二次减少到线性，同时保持了一致性。

B. Schmidt-MSCKF

  所提出的包含回环闭合的视觉惯性导航系统（VINS）的关键思想是将施密特-卡尔曼滤波器（SKF）应用到多状态约束卡尔曼滤波器（MSCKF）框架中，用于实时状态估计。具体而言，我们首先定义活动状态$x_A:=[x_I^T{x}_C^T{]}^T$由于滑动窗口边缘化的作用，其大小在整个轨迹上保持不变；而关键帧状态$x_S$随着新关键帧的插入线性增长（参见公式(10)）。我们相应地将第 $k$时刻的协方差矩阵进行分块划分。

  1) Propagation: 与标准MSCKF【1】类似，IMU状态估计通过基于IMU运动学（公式(4)）的积分进行传播（参见第III-A.1节），而所有其他变量由于具有零动态性保持不变。协方差矩阵的传播方式如下：

其中${\Phi }_{k-1}$和$Q_{k-1}$分别是活动状态的系统雅可比矩阵和离散噪声协方差矩阵。请注意，与低速率相机图像（例如20Hz）相比，由于高速率IMU测量（例如200Hz），重复计算${\Phi }_{k-1}{P}_{A{S}_{k-1|k-1}}$的计算成本很高。为了降低成本，我们逐步整合两个图像时间（比如m）之间可用的IMU测量值：${\Phi }_{k-1}={\Phi }_{k-1，m}\cdot \cdot {\Phi }_{k-1,0},$初始条件${\Phi }_{k-1,0}=I$。同样，噪声协方差$Q_{k-1}$被递归复合。

  2) Visual tracking and loop closing: 当IMU相机传感器平台在环境中导航时，随着新图像的到来，滑动窗口会顺序向前移动。与标准MSCKF VIO（见第III-A.2节）一样，采用基于KLT的视觉跟踪在当前滑动窗口中构建特征轨迹。然而，我们没有像标准MSCKF那样边缘化旧的克隆相机姿势，而是选择某些克隆作为状态向量中保留的关键帧进行循环闭合。虽然可以使用不同的启发式方法来选择新的关键帧，例如基于图像视差[18]或特征跟踪质量[4]，但在这项工作中，为了验证概念，我们只是在固定的时间间隔内添加新的关键框。一旦选择克隆的姿势作为关键帧，其相应的状态将成为关键帧状态的一部分[见公式（10）]，其互相关（而不是自协方差）将在未来更新。这可以通过这样一个事实来证明，当克隆的姿势到达滑动窗口的末端并被选为关键帧时，它们的估计通常是准确的，可以假设它们已经达到了具有成熟但非零不确定性的稳定状态，这将在我们的估计器中得到正确有效的跟踪，而不是像[18]中那样天真地假设它们是完美的，具有零不确定性。
  为了执行基于关键帧的回环检测，我们利用最先进的DBoW2方法【31】来寻找回环闭合候选关键帧。当插入一个新关键帧时，通过提取300个FAST特征【32】及其ORB描述子【33】，将该关键帧的图像更新到DBoW2数据库中。为了检测当前相机图像的回环闭合，我们查询DBoW2数据库以检索与其在视觉上最相似的关键帧。检索完成后，通过几何验证数据库中排名靠前的候选关键帧，确保查询图像与候选关键帧之间可以计算出基础矩阵。
 一旦确定了回环闭合的关键帧，我们便将滑动窗口中活动特征与该关键帧中的特征匹配。例如，如图2所示，如果检测到帧 {C3} 与关键帧 {K1} 匹配，则 {C3} 中提取的所有特征都会尝试与 {K1} 中提取的特征进行匹配。具体来说，当一个活动特征与关键帧特征匹配时，我们将关键帧的附加观测值添加到该特征的轨迹中。需要特别注意的是，每个活动特征只能匹配一个关键帧一次，也就是说，一个特征测量只能参与一个特征轨迹，以防止信息重复使用，从而确保一致性。需要注意的是，为了节省计算开销，当前图像只与一个关键帧进行匹配，但这一过程可以很容易地修改为允许多个回环闭合关键帧的匹配。因此，我们将来自回环闭合关键帧的附加观测值与滑动窗口中的特征轨迹结合起来，当活动特征丢失跟踪或达到滑动窗口的大小时，执行MSCKF更新。下一节将详细说明，我们如何更新活动状态估计 $x_A$其协方差，以及活动状态与关键帧状态 $x_S$之间的交叉相关性

  3)Update: 一旦包括活动特征测量和（如果有的话）循环闭合约束的特征轨迹准备好在当前滑动窗口中处理，我们就会根据这些视觉特征测量在MSCKF框架（以及Schmidt MSCKF）内执行SKF更新。具体来说，与标准MSCKF一样，我们首先对当前窗口中的所有特征测量值进行BA，以三角测量3D特征位置G p f。在线性化非线性相机测量值时，需要它们的估计量G p̂f（6）[注意，我们的状态向量中没有保持特征（10）]。对于给定的特征z k，我们对其位置进行线性边缘化（即零空间运算）[34]，并对公式（9）进行如下划分：

理想情况下，我们现在可以使用上述独立于特征的测量残差（13）来执行标准MSCKF更新。然而，由于状态向量$x_S$中的关键帧会随着时间的推移而增长，即使特征已经从MSCKF中的状态中排除，它也可能很快变得太大，无法实时更新完整的状态估计和协方差。因此，我们改为执行SKF更新。具体来说，我们首先注意到标准卡尔曼增益的计算如下：

其中$S_k=H_x^{{}^{\prime }}{P}_{k|k-1}{H}_x^{{}^{\prime }}+R_f^{{}^{\prime }}$是残差协方差的度量[见公式（9）]。为了降低计算复杂度，我们不更新关键帧状态干扰参数，因此，与SKF一样，我们将与关键帧状态对应的卡尔曼增益设置为零，即$K_{S_k}=0$。在这种情况下，可以证明活动状态的最佳增益与其相应的标准卡尔曼增益$K_{A_k}=0$相同。因此，状态估计更新如下：

请注意，在标准EKF中，协方差由以下公式更新：

可以看出，在标准公式中，$K_{S_k}{S}_k{K}_{S_k}^T$项包含最大的计算成本。相比之下，SKF通过设置施密特卡尔曼增益$K_{S_k}=0$来节省计算量，同时仍能确保一致性[24]

在上述表达式中，第二等式（19）揭示了计算复杂度变为$O(n)$，而不是如（17）中的$O(n^2)$，接下来将对此进行详细分析。总之，算法1概述了所提出的Schmidt-MSCKF VINS方法的主要步骤

  4) Computational complexity: 在propagation过程中，计算的主要开销集中在矩阵乘法${\Phi }_k{P}_{A{S}_{k|k}}$[见公式（12）]主导。由于${\Phi }_k$是大小固定的方阵，而$P_{A{S}_{k|k}}$是具有列数为$O(n)$的“胖矩阵”，因此这个矩阵乘法的计算复杂度为 $O(n)$。propagation后，我们会克隆当前时刻的IMU状态。这涉及简单地复制$P_{A{A}_{k+1|k}}$中的相应行和列，这是一个$O(1)$操作，同时将对应状态的行复制到$P_{A{S}_{k+1|k}}$的末尾，这是一个$O(n)$的操作。滑动窗口中与每个特征关联的图像最多只能匹配到一个旧关键帧。由于每次匹配都会添加一个方位测量，这意味着所有被处理的特征在有限大小的位姿集合中具有恒定数量的测量值。考虑到被跟踪特征的数量是有上限的，在每次更新中，最多只涉及恒定数量的测量值和变量，这立即意味着计算Sk的复杂度为$O(1)$。通过利用测量雅可比矩阵的稀疏性，我们可以计算出$O(n)$的代价计算$L_{A_k}=P_{A{A}_{k|k-1}}{H}_{A_k}^T+P_{A{S}_{k|k-1}}{H}_{S_k}^T$和$L_{S_k}$（参见公式14）。类似的$L_{S_k}$也具有$O(n)$的复杂度。Update阶段的剩余部分主要由$L_{A_k}{S}_k^{-1}{L}_{S_k}$支配（参见公式19）。由于$L_{S_k}$是一个有$O(n)$行的“高矩阵”，这一操作的计算复杂度同样为$O(n)$。
  在边缘化过程中，将变量 $x_{C_i}$从活动状态 $x_A$移动到关键帧状态 $x_S$时，需要注意操作的计算复杂度保持为 $O(n)$。具体来说，我们将 $P_{AA}$,$P_{AS}$和 $P_{SS}$分别作为独立的矩阵进行管理。当将 $x_{C_i}$ 移动到施密特状态时，我们首先将 $P_{AA}$,中对应的列复制为 $P_{AS}$中的一列。这需要分配$O(n)$ 的空间，并且复制操作的计算开销为 $O(n)$。然后，我们将 $P_{AS}$中与 $x_{C_i}$对应的行的条目复制，以在 $P_{SS}$的末尾形成新的行和列。同样，这需要分配新的行和列（$O(n)$ 的空间），并且复制的计算开销为 $O(n)$ 最后，我们删除 $P_{AA}$中与 $x_{C_i}$对应的列和行，这是一项 $O(1)$的操作；同时删除 $P_{AS}$中与 $x_{C_i}$相关的行，这是一项 $O(n)$ 的操作。关于每一步计算复杂度的完整分析，可以参考我们的技术报告【35】，其中提供了详细说明。

V. EXPERIMENTAL RESULTS

为了验证所提出的具有环路闭合的Schmidt-MSCKF VINS，我们在不同的传感器平台上进行了真实世界的实验。在下文中，我们展示了手持传感器的两组结果，证明了所提出的方法比标准MSCKF（没有环路闭合）实现了更高的精度，同时只产生了微不足道的计算开销。

A. Vicon Loops

  我们首先在Vicon回环数据集【4】上测试我们的VINS系统，该数据集包含了一个13分钟、1.2公里长的轨迹，使用手持的VI传感器（立体相机和IMU）进行多次回环。完整的6自由度地面真值由Vicon运动跟踪系统提供，采样频率为200Hz。在此测试中，我们以每两秒一个的固定速率选择回环闭合的关键帧。对于下面展示的结果，我们开发并验证了立体视觉和单目视觉（即仅使用其中一只立体相机）VINS算法。具体来说，比较的估计器包括：（i）不使用回环闭合的标准MSCKF【1】；（ii）使用回环闭合并对关键帧进行完整协方差更新的标准MSCKF（参见第III-B节）；（iii）提出的结合回环闭合的Schmidt-MSCKF VINS；以及（iv）开源的VINS-Mono【18, 19】。所使用的性能度量标准包括：（i）根均方误差（RMSE），用于衡量估计的准确性；（ii）CPU运行时间，用于量化计算成本。
  表I显示了在10次实验中平均得到的RMSE值（为了考虑算法的重复性）。轨迹通过使用前两分钟的地面真值进行对齐。图3展示了估计的轨迹，清晰地表明，回环闭合测量提供的信息显著限制了所提方法在时间上的漂移。在使用回环闭合约束的两个MSCKF系统中，更新完整状态估计和协方差的系统，如预期的那样，获得了略好的性能；而所提出的Schmidt-MSCKF在精度上紧随其后，同时显著节省了计算开销。具体而言，图4展示了在数据集上进行单次代表性实验时，单目VINS的CPU运行时间。正如预期的那样，所提出的Schmidt-MSCKF（上图）在执行EKF更新所需的时间上只有线性增长，而使用完整回环闭合更新的标准MSCKF（中图）在传播和更新的计算时间上都显示出二次增长。所提出的方法甚至在实时阈值0.05秒（相机帧率为20Hz）以下，能够实时处理400个关键帧，特别是在轨迹的末端。相比之下，VINS-Mono姿态优化后台（非实时线程）在轨迹结束时需要超过2秒，这显著延迟了回环闭合信息在当前状态估计中的纳入。

B. UD Spencer

我们进一步在特拉华大学（UD）斯宾塞实验室进行了多层室内实验。使用Intel Realsense ZR300传感器收集了一个20分钟、1.5公里长的数据集，包含单目鱼眼图像和IMU测量数据。使用制造商提供的外部标定作为初始估计，并在线进一步优化。关键帧每0.75秒插入一次，最多总共插入886个关键帧，在更新过程中，26%的特征包含至少一个关键帧的观测值。图5展示了标准MSCKF、所提单目Schmidt-MSCKF和VINS-Mono的估计轨迹。显然，所提方法能够在整个轨迹中以最小的漂移进行定位，而标准MSCKF则出现了显著的漂移。由于此数据集没有地面真值，我们通过返回起始位置并评估起止误差，得到标准MSCKF、所提Schmidt-MSCKF、使用完整协方差更新的MSCKF和VINS-Mono（带非实时优化线程）的方法的起止误差分别为3.58米（0.2%）、0.64米（0.04%）、0.26米（0.02%）和0.16米（0.01%）。从定性上看，所提方法的轨迹与VINS-Mono相当，同时仅需一个线程进行实时估计。

VI. CONCLUSIONS AND FUTURE WORK

  在本文中，我们提出了线性复杂度的Schmidt-MSCKF用于VINS，该方法能够高效地在单线程估计器中整合回环闭合约束，并提供（几乎）有界误差的定位性能。所提方法的关键思想是选择性地将关键帧（而不是大量地图特征）保留在状态向量中，以便利用回环闭合约束，并利用SKF公式，在不更新关键帧（作为扰动参数）及其协方差的情况下，仍然正确地跟踪它们与活动导航状态的交叉相关性。我们已经在单目和立体VINS中广泛验证了所提的Schmidt-MSCKF，结果表明，与标准MSCKF相比，性能显著提升，同时计算开销仅为边际。未来，我们将选择性地添加和删除关键帧，并评估不同关键帧选择标准的性能。