OKVIS论文翻译：Keyframe-Based Visual-Inertial SLAM Using Nonlinear Optimization

论文翻译下载链接（带图）：http://download.csdn.net/detail/u014679795/9762881

论文原文：http://download.csdn.net/detail/u014679795/9762879

Keyframe-BasedVisual-Inertial SLAM Using Nonlinear Optimization

 

摘要 - 视觉和惯性传感器的融合在机器人技术中已经变得流行，因为两种感测模态的互补性质。虽然目前大多数融合策略依赖于过滤方案，但是视觉机器人社区最近已经转向用于诸如可视同步定位和映射（SLAM）之类的任务的非线性优化方法，随后发现这种方法在性能质量方面具有显着的优点和计算复杂性。遵循这一趋势，我们提出一种新颖的方法，将视觉测量与来自惯性测量单元（IMU）的读数紧密集成在SLAM中。IMU误差项以完全概率的方式与地标再投影误差集成，导致联合非线性成本函数被优化。使用“关键帧”的强大概念，我们部分地将旧状态边缘化以维持有界大小的优化窗口，从而确保实时操作。与只有视觉惯性和松散耦合的视觉惯性算法相比，我们的实验证实了紧密融合在精度和鲁棒性方面的好处。

I.    引言

结合视觉和惯性测量长期以来一直是解决常见的机器人技术任务，如运动估计，视觉测距和SLAM的流行手段。 图像中捕获的场景的丰富表示，以及存在于典型IMU中的陀螺仪和加速度计的精确短期估计已经被认为彼此互补，在空中[6,20]和汽车[14] ]导航。 此外，随着大多数智能手机中这些传感器的可用性，对于视觉惯性SLAM的有效解决方案存在极大的兴趣和研究活动。

历史上，已经通过滤波处理了视觉惯性姿态估计问题，其中传播IMU测量并且使用关键点测量来形成更新。 Mourikis和Roumeliotis [14]提出了一种基于EKF的使用单目视觉的实时融合，Jones和Soatto [8]在长的室外轨迹上提供单视觉惯性过滤结果，包括IMU到摄像机校准和环路闭合。这两个作品表现出令人印象深刻的误差低于0.5％的旅行距离。凯利和Sukhatme [9]提供校准结果和在基于过滤的visionIMU融合的上下文中的可观察性的研究。视觉惯性估计问题是偏航和位置的全局不可观察性，以及相对于初始参考姿态的不确定性的增长;这对通常依赖于某种形式的线性化的滤波方法提出了挑战。

在[18]中表明，在纯视觉SLAM基于优化的方法提供了更好的准确性相同的计算工作，和过滤方法相比。保持相对稀疏的关键帧及其相关联的地标的图经历非线性优化，因此已经非常受欢迎。

在文献中发现的视觉 - 惯性融合方法可以分类为遵循两种方法。在松耦合系统中， [10]，IMU测量结合作为独立的倾斜仪和相对偏航测量到立体视觉优化。 Weiss等人[20]使用仅视觉姿态估计作为具有间接IMU传播的EKF的更新。在[15,7]中，相对立体姿态估计被集成到包含惯性项和绝对GPS测量的因子图中。这种方法限制了复杂性，但忽略了不同传感器的内部状态之间的相关性。相反，紧耦合方法联合估计所有传感器状态。为了易于处理和作为过滤的替代，Dong-Si和Mourikis [2]提出了一种固定滞后平滑器，其中连续机器人姿势和相关状态的窗口维持，边缘化状态（在[19]之后），超出范围。类似的方法，但没有惯性术语和行星着陆的情况下使用[16]。

为了稳健和准确的视觉惯性SLAM，我们倡导紧密耦合融合，以最大限度地利用感测线索和非线性估计，而不是滤波，以减少由于线性化的次优性。我们的方法灵感来自[17]，其中建议在批量优化的SLAM中使用IMU误差项（尽管仅在初始化期间）。 我们的方法与[2]中提出的固定滞后平滑器密切相关，因为它在单个成本函数中结合惯性项和再投影误差，并且旧状态被边缘化以便限制复杂性。

关于这些作品，我们看到三个贡献：

1）我们采用关键帧范例进行无漂移估计，当慢或完全没有运动存在：而不是使用时间连续姿势的优化窗口，我们保持关键帧可能在时间间隔任意远，保持视觉约束同时仍然尊重IMU术语。 我们的关键帧的相对不确定性的公式允许建立一个姿态图，而不表示全局姿态不确定性，从RSLAM的灵感[13]。

2）我们提供IMU误差项的完全概率推导，包括相关的连续图像帧的相应信息矩阵，而没有以IMU速率明确引入状态。

3）在系统级，我们开发了用于精确实时SLAM的硬件和算法，包括使用惯性提示的鲁棒关键点匹配和异常值拒绝

在本文的剩余部分中，我们在II-B中引入了批视觉SLAM中的惯性误差项，随后概述了II-C中的实时立体声图像处理和关键帧选择以及II-D中的边缘化形式。 最后，我们展示了我们的立体视觉和IMU传感器室内和室外在III中获得的结果。

II。 紧耦合的视觉-IMU融合

在视觉SLAM中，公式化非线性优化，通过最小化在相机帧中观察到的地标的再投影误差来找到相机姿势和界标位置。图2示出了相应的图表示：其将测量显示为具有方框的边和估计的量作为圆节点。 一旦引入惯性测量，它们不仅在连续姿势之间产生时间约束，而且在加速度计和陀螺仪的连续速度和IMU偏差估计之间产生机器人状态矢量增加的时间约束。 在本节中，我们提出了将惯性测量结合到批量视觉SLAM中的方法。

A.符号和定义

1）符号：我们在整个工作中采用以下符号：表示参考帧A; 在其中向量被写为pA或任选地作为p^BC_A，其中B和C分别作为起点和终点。 帧之间的变换由齐次变换矩阵T AB表示，变换矩阵T AB将均匀点的坐标表示从到 ，其旋转矩阵部分写为CAB; 相应的四元数被写为 和η表示虚部和实部。 我们采用Barfoot等人介绍的符号。 [1]：关于四元数乘法qAC =qAB⊗qBC，我们引入左手侧复合运算符（.）+和右手侧运算符（.）⊕，使得qAC = qAB + qBC =qBC⊕qAB。

2）帧：使用图3中示意性地描绘的立体相机/ IMU设置来评估所提出的方法的性能。在被跟踪物体内部，相对于惯性帧，我们区分相机帧和IMU传感器帧，

3）状态：要估计的变量包括图像时间（索引k）xk R和地标x_L^c处的机器人状态。xR表示在惯性帧pW SW中的机器人位置，身体旋转四元数qW S，在惯性 帧的速度，以及陀螺仪的偏置bg和加速度计的偏置ba。 因此，xR被写为：

此外，我们使用分割成姿态状态和速度/偏置状态。 地标以齐次坐标表示，如[3]中所示，以便允许接近和非常远的地标的无缝集成：

我们使用状态歧管的切线空间g中的扰动，并采用组算子，指数exp和对数log。 现在，我们可以定义扰动围绕估计。 我们使用最小坐标表示法。 双射映射Φ从最小坐标变换到切线空间：

具体来说，我们使用最小（3D）轴角扰动旋转，其可以通过指数图转换成其等效四元数δq：

因此，使用组算子⊗，我们写。我们获得最小机器人误差状态向量

类似于机器人状态分解xT和xsb，我们使用姿态误差状态和速度/偏差误差状态。

 我们将齐次地标视为具有最小扰动的（非单位）四元数δβ，因此

B.具有惯性条件的批视觉SLAM

我们寻求制定视觉惯性定位和映射问题，作为包含（加权）重投影误差e_r和来自IMU的时间误差项e_s的成本函数J（x）的一个联合优化：

其中i是组件的相机索引，k表示相机帧索引，j表示地标索引。 在第k帧和第i个相机中可见的标志的索引被写为集合J（i， k）。 此外，表示相应地标测量的信息矩阵，是第k个IMU误差的信息。

固有地，纯视觉SLAM具有需要在优化期间保持固定的6自由度（DoF），即， 绝对姿势。 组合的视觉惯性问题仅具有4个自由度，因为重力使得两个旋转自由度可观察到。这使固定复杂化。 我们想要冻结围绕重力方向（世界z轴）的偏航，以及典型地第一姿态（index k1）的位置。 因此，除了将位置变化设置为零，，我们还假设

在下面，我们将呈现（标准）再投影误差公式。 之后，给出了IMU运动学与偏差项建模相结合的概述，基于此IMU误差项。

1）重投影误差公式：我们使用相当标准的重投影误差公式，适用于来自Furgale的小修改[3]：

因此，h_i（·）表示相机投影模型，表示测量图像坐标。 关于最小扰动的误差Jacobian直接来自Furgale [3]。

2）IMU运动学：在假定地球旋转的测量效应小于陀螺仪精度的假设下，我们可以将IMU运动学与简单动态偏差模型结合起来：

其中都是不相关的零均值高斯白噪声过程。 是加速度计测量值和地球重力加速度矢量。 与被建模为随机游走的陀螺偏差相反，我们使用时间常数τ> 0来将加速度计偏差建模为有界随机游走。 矩阵Ω由估计的角速率形成，用陀螺测量数据

线性化误差动力学采取形式

其中G是直接导出和：

（.）×表示与矢量关联的斜对称叉乘矩阵。

注意，可以以与在经典EKF滤波中用于平均值和协方差（P_R，在最小坐标中）的相同的方式使用等式（7）和（10）。对于实际实现，需要这些方程的离散时间版本，其中索引p表示第p个IMU测量。 对于计算复杂性的考虑，我们选择使用简单的Euler-Forward方法在时间差Δt上积分。 类似地，我们获得离散时间误差状态过渡矩阵

这导致协方差传播方程：

其中包含各个处理的所有噪声密度

3）IMU测量误差项的公式：图4示出了在时间步骤k和k + 1进行的相机测量的测量速率的差异，以及通常不与相机测量同步的较快的IMU测量。

我们需要IMU误差项是步骤k和k + 1处的机器人状态的函数以及这些时间实例之间的所有IMU测量（包括加速度计和陀螺仪读数）总结为。 因此，我们必须假设在相机测量k和k + 1的给定机器人状态下的近似正常条件概率密度f：

对于状态预测具有相关联的条件协方差，IMU预测误差项现在可写为：

这只是基于先前状态的预测和实际状态之间的差异 - 除了方向，其中我们使用简单的乘法最小误差。

接下来，在应用误差传播定律时，找到相关联的信息矩阵为：

雅可比是直接获得但不平凡，因为旋转误差一般将是非零的。

最后，关于和的雅可比矩阵将需要用于优化问题的有效解。尽管关于的微分是直接的（但不是微不足道的），但是一些注意力被给予另一个雅各比。 回想通过迭代地应用预测来计算IMU误差项（14）。 相对于状态的微分因此导致应用链式规则，产生

因此，表示特定离散步长的时间戳，并且p^k表示在获取相机帧k之后的第一IMU采样索引。

C.关键点匹配和关键帧选择

我们的处理流水线采用定制的多尺度SSE优化的Harris角点检测结合BRISK描述子提取[12]。 检测器通过逐渐抑制具有较弱分数的角落来强制图像中的关键点均匀分布，因为它们在到较强角落的小距离处被检测到。描述子被提取沿着重力方向（投影到图像中），其由于紧密的IMU融合而是可观察的

最初，关键点被立体三角化并插入到局部映射中。我们对所有地图地标执行暴力匹配;通过使用通过IMU积分获得的（不确定）姿态预测，通过在图像坐标中应用卡方检验简单地执行异常值拒绝。没有昂贵的RANSAC步骤 - 紧密的IMU参与的另一个优点。对于后续优化，保持相机帧的有界集合，即具有在该时刻拍摄的相关图像的姿态;在这些图像中可见的所有地标都保存在本地地图中。如图5所示，我们区分两种类型的帧：我们引入包括当前帧的S个最近帧的时间窗口;并且我们使用可能在过去远的N个关键帧。对于关键帧选择，我们使用一个简单的启发式：如果匹配点跨越的图像区域与所有检测到的点跨越的区域之间的比率低于50到60％，则帧被标记为关键帧。

D.部分边缘化

非线性时间约束可以驻留在的有界优化窗口中是不明显的，窗口中包含可能在时间上任意间隔开的关键帧。 在下面，我们首先提供边缘化的数学基础，即消除非线性优化中的状态，并将其应用于视觉惯性SLAM

1.非线性优化边际化的数学公式：高斯 - 牛顿方程组由所有的误差项，雅可比和信息构成：形式Hδx = b。 让我们考虑要被边缘化的一组状态，与误差项和剩余状态集合相关的所有状态的集合。 由于条件独立性，我们可以简化边缘化步骤，只将其应用于子问题：

Schur-Complement运算的应用产生：

其中和是和的非线性函数。

（18）中的方程描述了边缘化的单一步骤。 在我们的基于关键帧的方法中，必须重复应用边缘化步骤，并将结果信息作为优先级的先验，因为我们的状态估计继续改变。因此，我们固定x0周围的线性化点，边缘化时x的值。 有限偏差表示在边缘化之后发生的状态更新，其中x是我们对x的当前估计。 换句话说，x被组成为

这个通用的公式允许我们将最小坐标的先验信息应用到我们的任何状态变量 - 包括单位长度四元数。 引入Δx允许右手侧近似（到第一阶）as

现在我们可以表示高斯 - 牛顿系统（17）：

在该形式中，右侧（18）变为

在边缘化节点包括无限远（或足够接近无限远）的地标或来自单个姿势的仅在一个摄像机中可见的地标的情况下，与那些地标相关联的Hessian块将（数字地）为秩不足。 因此，我们采用伪逆，其为提供了解，给定，零分量进入零空间方向。

上述公式为边缘化xμ以及剩余状态xλ的状态引入了固定线性化点。 这也将被用作涉及这些状态的术语的所有未来线性化的参考点。在应用（18）之后，我们可以去除消耗的非线性项，并将边缘化的和作为加数，以构建整个高斯 - 牛顿系统。对正方形误差的贡献可以写为

2）边缘化应用于基于关键帧的视觉惯性SLAM：

最初边缘化误差项由前N + 1帧构成， ，如图6中以图形方式可视化.N个第一帧将全部被解释为关键帧，并且边缘化步骤包括消除相应的速度和偏置状态

当将新帧（当前帧，索引c）插入到优化窗口中时，我们应用边缘化操作。 在时间窗口中最旧的帧不是关键帧的情况下，我们将丢弃其所有的界标测量，然后将其与最旧的速度和偏置状态一起边缘化。图7示出了该过程。 下降的地标测量是次优的;

然而，它保持稀疏问题的快速解决方案。 具有关键帧的视觉SLAM成功地类似地进行，丢弃具有其地标测量的整个帧。

在是关键帧的情况下，简单地丢弃所有关键点测量的信息丢失将更显着：在共同界标观测中编码的最早的两个关键帧之间的所有相对姿势信息将丢失。因此，我们另外将在中可见但在最近的关键帧中不可见的界标边缘化。图8图形地描绘了该过程。 该问题的稀疏性再次得到保留。

IV。 结论

本文提出了一种将惯性测量紧密集成到基于关键帧的可视SLAM中的方法。 非线性优化中的误差项的组合由可用于关键点检测和IMU读数的误差统计激发，因此取代了对任何调谐参数的需要。使用提出的方法，我们获得重力方向的全局一致性和使用IMU运动学运动模型的鲁棒离群值排除。 同时，获得了基于关键帧的非线性优化的所有好处，例如静止姿态保持。使用立体摄像机和IMU传感器获得的结果证明了所提出的框架的实时操作，同时在仅视觉或松散耦合方法上表现出增加的精度和鲁棒性。