VINS-Mono 论文翻译

VINS-Mono: A Robust and Versatile MonocularVisual-Inertial State Estimator

摘要

单目视觉惯性系统（VINS）由相机和低成本惯性测量单元（IMU）组成，构成了用于六自由度状态估计的最小传感器模组。然而，缺乏直接测量距离的手段在IMU数据处理、估计器初始化、外参标定和非线性优化方面提出了重大挑战。在这项工作中，我们提出了VINS-Mono：一种鲁棒且通用的单目视觉惯性状态估计器。我们的方法从一个鲁棒的初始化和故障恢复过程开始。采用一种基于非线性优化的紧耦合方法，通过融合预积分测量值和特征观测值，获得高精度的视觉惯性里程计。一个闭环检测模块，结合我们的紧耦合方式，可以以最小的计算开销实现重定位。此外，我们还进行了四自由度位姿图优化，以增强全局一致性。我们在公共数据集和真实实验上验证了系统的性能，并与其他最先进的算法进行了比较。在MAV平台上实现了机载闭环自主飞行，并将该算法移植到基于iOS平台的演示中，验证了该算法是一个可靠、完整、通用的系统，适用于需要高精度定位的不同应用场合。我们为PC和iOS移动设备提供了开源的实现。

I 引言

状态估计无疑是机器人导航、自主驾驶、虚拟现实和增强现实（AR）等广泛应用中最基本的模块。由于其体积小、成本低且易于硬件设置，仅使用单目相机的方法在该领域获得了极大的关注[1]-[5]。然而，单目纯视觉系统无法恢复真实，因此限制了其在现实机器人应用中的使用。最近，我们看到了一个发展趋势，即使用低成本的惯性测量单元(IMU)辅助单目视觉系统。这种单目视觉-惯性系统(VINS)的主要优点是可以观测真实尺度以及横滚和俯仰角。这将使其可以用于需要真实尺度状态估计的导航任务。此外，IMU测量值的集成可以通过弥补由于照明变化、无纹理区域或运动模糊造成的部分区域的视觉轨迹丢失，显著提高运动跟踪性能。单目VINS不仅广泛应用于地面机器人和无人机，而且在移动设备上也很实用。它在尺寸、重量和功耗方面对自我和环境感知都有很大的优势。

然而，有几个问题影响单目VINS的使用。第一个是严格的初始化。由于缺乏直接的距离测量，很难将单目视觉结构与惯性测量直接融合。同时认识到VINS是高度非线性的这一事实，我们发现在估计器初始化方面存在重大挑战。在大多数情况下，系统应从已知的静止位置启动，并在开始时缓慢小心地移动，这限制了其在实践中的使用。另一个问题是视觉惯性里程计(VIO)的长期漂移是不可避免的。为了消除漂移，必须进行闭环检测、重定位和全局优化。除了这些关键问题外，地图保存和重用的需求正在增长。

为了解决所有这些问题，我们提出了VINS-Mono，这是一种鲁棒且通用的单目视觉-惯性状态估计器，它是我们之前三项工作的组合和扩展[6]–[8]。VINS-Mono包含以下功能：

1. 鲁棒的初始化过程，能够从未知初始状态引导系统；
2. 紧耦合、基于优化的单目VIO，具有相机–IMU外参标定和IMU零偏矫正；
3. 在线重定位和四自由度全局位姿图优化；
4. 位姿图重用，可以保存、加载和合并多个局部位姿图。

在这些特性中，鲁棒的初始化、重定位和位姿图重用是我们的技术贡献，这些贡献来自我们以前的工作[6]–[8]。工程贡献包括开源系统集成、无人机导航实时演示和移动应用。整个系统已成功应用于小型AR场景、中型无人机导航和大型状态估计任务，如图1所示。

I 引言

状态估计无疑是机器人导航、自主驾驶、虚拟现实和增强现实（AR）等广泛应用中最基本的模块。由于其体积小、成本低且易于硬件设置，仅使用单目相机的方法在该领域获得了极大的关注[1]-[5]。然而，单目纯视觉系统无法恢复真实，因此限制了其在现实机器人应用中的使用。最近，我们看到了一个发展趋势，即使用低成本的惯性测量单元(IMU)辅助单目视觉系统。这种单目视觉-惯性系统(VINS)的主要优点是可以观测真实尺度以及横滚和俯仰角。这将使其可以用于需要真实尺度状态估计的导航任务。此外，IMU测量值的集成可以通过弥补由于照明变化、无纹理区域或运动模糊造成的部分区域的视觉轨迹丢失，显著提高运动跟踪性能。单目VINS不仅广泛应用于地面机器人和无人机，而且在移动设备上也很实用。它在尺寸、重量和功耗方面对自我和环境感知都有很大的优势。

然而，有几个问题影响单目VINS的使用。第一个是严格的初始化。由于缺乏直接的距离测量，很难将单目视觉结构与惯性测量直接融合。同时认识到VINS是高度非线性的这一事实，我们发现在估计器初始化方面存在重大挑战。在大多数情况下，系统应从已知的静止位置启动，并在开始时缓慢小心地移动，这限制了其在实践中的使用。另一个问题是视觉惯性里程计(VIO)的长期漂移是不可避免的。为了消除漂移，必须进行闭环检测、重定位和全局优化。除了这些关键问题外，地图保存和重用的需求正在增长。

为了解决所有这些问题，我们提出了VINS-Mono，这是一种鲁棒且通用的单目视觉-惯性状态估计器，它是我们之前三项工作的组合和扩展[6]–[8]。VINS-Mono包含以下功能：

1. 鲁棒的初始化过程，能够从未知初始状态引导系统；
2. 紧耦合、基于优化的单目VIO，具有相机–IMU外参标定和IMU零偏矫正；
3. 在线重定位和四自由度全局位姿图优化；
4. 位姿图重用，可以保存、加载和合并多个局部位姿图。

在这些特性中，鲁棒的初始化、重定位和位姿图重用是我们的技术贡献，这些贡献来自我们以前的工作[6]–[8]。工程贡献包括开源系统集成、无人机导航实时演示和移动应用。整个系统已成功应用于小型AR场景、中型无人机导航和大型状态估计任务，如图1所示。

II 相关工作

关于基于单目视觉的状态估计/里程计/SLAM的学术工作非常广泛。值得注意的方法包括PTAM[1]、SVO[2]、LSD-SLAM[3]、DSO[5]和ORBSLAM[4]。显然，任何进行全面相关概述的尝试都是不完整的。然而，在本节中，我们跳过了对纯视觉方法的讨论，只关注与单目视觉-惯性状态估计最相关的结果。

处理视觉和惯性测量的最简单方法是松耦合的传感器融合[9]，[10]，其中IMU被视为一个独立的模块来辅助视觉结构。融合通常由扩展卡尔曼滤波器(EKF)完成，其中IMU用于状态传播，纯视觉位姿用于更新。此外，紧耦合的视觉-惯性算法基于EKF[11][13]或图优化[14]-[19]，其中相机和IMU测量从原始测量的级别联合优化。一种流行的基于EKF的VIO方法是MSCKF[11]，[12]。MSCKF在状态向量中保持了多个以前的相机位姿，并在多个相机视角中使用相同特征的视觉测量来形成多约束更新。SR-ISWF[20]，[21]是MSCKF的一个扩展。它使用平方根形式[14]来实现单精度表示，并避免较差的数值特性。该方法使用逆滤波器进行迭代重新线性化，使其与基于优化的算法相同。批处理图优化或BA技术维护和优化所有测量值，以获得最佳的状态估计。为了实现常量的处理时间，基于图的VIO方法[15]、[17]、[18]通常通过边缘化过去的状态和测量值，来在最近状态的有限大小滑动窗口上进行优化。由于非线性系统迭代求解的计算量很大，很少有基于图优化的方法能够在资源受限的平台(如手机)上实现实时性能。

对于视觉测量处理，根据残差模型的定义，算法可以分为直接法和间接法。直接法[2]、[3]、[22]使光度误差最小化，而间接方法[12]、[15]、[17]使几何位移最小化。直接方法由于其较小的吸引域而需要良好的初始猜测，而间接方法在提取和匹配特征时消耗额外的计算资源。间接法由于其成熟性和鲁棒性，在实际工程部署中更为常见。然而，直接方法更容易扩展用于稠密建图，因为它们直接在像素级别上操作。

IMU通常以比摄像机高得多的速率获取数据。人们提出了不同的方法来处理高速率IMU测量值。最直接的方法是在基于EKF的方法中使用IMU进行状态传播[9]，[11]。在图优化形式中，为了避免重复的IMU重积分，开发了一种称为IMU预积分的有效的技术。这项技术最早在[23]中介绍，它使用欧拉角参数化旋转误差。Shen等人[16]利用连续时间误差状态动力学推导了协方差传播。在[19]和[24]中，通过添加后验IMU零偏校正，进一步改进了预积分理论。

准确的初始值对于引导任何单目VINS至关重要。[17]和[25]中提出了一种利用短期IMU预积分的相对旋转的线性估计器初始化方法。该方法无法对原始投影方程中的陀螺仪零偏和图像噪声进行建模。[26]中介绍了单目视觉-惯性初始化问题的闭式解。稍后，[27]中提出了通过添加陀螺仪零偏校准来对此闭式解进行扩展的解决方案。这些方法无法对惯性积分中的不确定性进行建模，因为它们依赖于IMU测量值在较长时间内的双重积分。在[28]中，提出了一种基于SVO[2]的重新初始化和故障恢复算法。需要一个额外的向下距离传感器来恢复真实尺度。[18]中介绍了一种建立在流行的ORB-SLAM[4]之上的初始化算法。据报道，尺度收敛所需的时间可能超过10秒。这可能会给需要在开始时就获得尺度估计的机器人导航任务带来问题。

里程计方法，无论其所依赖的基本数学公式如何，都会受到全局平移和旋转长期漂移的影响。为此，闭环矫正在长期操作中起着重要作用。ORB-SLAM[4]能够矫正闭环并重用地图，它利用了视觉词袋的优势[29]。闭环检测之后是7-DOF[30] (位置、旋转和尺度)位姿图优化。

II 系统概述

本文所提出的单目视觉-惯性状态估计器的结构如图2所示。

系统从测量值预处理(见第IV节)开始，在其中提取和跟踪特征，并预积分两个连续帧之间的IMU测量值。初始化程序(见第V节)提供了所有必要的值，包括位姿、速度、重力矢量、陀螺仪零偏和三维(3-D)特征位置，用于引导后续的基于非线性优化的VIO。VIO(见第VI节)与重定位(见第VII节)模块紧密融合了IMU测量值的预积分和特征观测。最后，位姿图优化模块(见第VIII节)接收几何验证后的重定位结果，并执行全局优化以消除漂移。它页实现了位姿图的重用。VIO和位姿图优化模块在单独的线程中并行运行。

与OKVIS[15]相比(OKVIS是一种最先进的VIO算法，适用于双目相机)我们的算法是专门为单目相机设计的。因此，我们特别提出了一个初始化过程、关键帧选择标准，以及使用和处理大视场(FOV)相机来获得更好的跟踪性能的方法。此外，我们的算法还提供了一个完整的系统，带有闭环矫正和位姿图重用模块。

现在我们定义了本文中使用的符号和坐标系定义。我们认为$(\cdot {)}^w$是世界坐标系。重力方向与世界坐标系的z轴对齐。$(\cdot {)}^b$是机体坐标系，我们将其定义为与IMU坐标系相同。$(\cdot {)}^c$是相机的坐标系。我们使用旋转矩阵$R$和哈密顿四元数$q$来表示旋转。我们主要在状态向量中使用四元数，但旋转矩阵也用于方便的三维向量旋转。$q_b^w$和$p_b^w$是从机体坐标系到世界坐标系的旋转和平移。$b_k$是拍摄第$k$张图像时的机体坐标系。$c_k$是拍摄第$k$张图像时的相机坐标系。$\otimes$表示两个四元数之间的乘法运算。$g^w=[0,0,g{]}^T$是世界坐标系中的重力矢量。最后，我们将$(\hat{\cdot })$表示为特定量的噪声测量或估计。

测量值预处理

视觉前端

对于每一帧新图像，KLT稀疏光流算法跟踪现有特征。同时，检测新的角点特征，以保持每个图像中特征的最小数目（100-300）。检测器通过在两个相邻特征点之间设置像素位置的最小间隔来实施特征分布均匀化。二维特征首先被去畸变，然后经过离群点剔除后投影到单位球面上。使用RANSAC和基础矩阵模型进行离群点剔除。

这一步还选择了关键帧。在VINS视觉前端中有两个关键帧选择标准。第一个是前一个关键帧的平均视差。如果被跟踪的特征在当前帧和上一帧之间的平均视差并且超过某个阈值，则将该帧视为新的关键帧。请注意，不仅平移而且旋转都会导致视差。但是，不能在仅旋转运动中对特征进行三角化。为了避免这种情况，我们在计算视差时使用陀螺测量值的短期积分来补偿旋转。请注意，此旋转补偿仅用于关键帧选择，不涉及VINS公式中的旋转计算。因此，即使陀螺仪含有较大的噪声或者零偏，也只会产生次优的关键帧选择结果，而不会直接影响估计质量。另一个标准是跟踪质量。如果被跟踪特征的数量低于某个阈值，将此帧视为新的关键帧。这个标准是为了避免特征跟丢。

IMU预积分

我们遵循我们之前基于连续时间四元数的惯性测量单元预积分的推导[16]，并包括惯性测量单元零偏的处理[19]和[24]。我们注意到，我们当前的预积分过程与[19]和[24]共享几乎相同的数值结果，但使用了不同的推导。所以，我们在这里只简单介绍一下。关于基于四元数的推导的细节可以在附录A中找到。

1）IMU噪声和零偏

IMU测量值在机体坐标系中测量，结合了反重力力和机体动力学，并受到加速度零偏$b_a$、陀螺仪零偏$b_w$和附加噪声的影响。原始陀螺仪和加速度计的测量值$\hat{\omega }$和$\hat{a}$由下式给出
$$\begin{gathered} {\hat{a}}_t=a_t+b_{a_t}+R_w^t{g}^w+n_a \\ {\hat{\omega }}_t={\omega }_t+b_{{\omega }_t}+n_{\omega }\text{\hspace{1em}(1)} \end{gathered}$$
我们假设加速度和陀螺仪测量中的加性噪声为高斯白噪声，$n_a\sim \mathcal{N}(0,{\sigma }_a^2)$，$n_{\omega }\sim \mathcal{N}(0,{\sigma }_{\omega }^2)$。加速度零偏和陀螺仪零偏被建模为随机游走，其导数是高斯白噪声，$n_{b_a}\sim \mathcal{N}(0,{\sigma }_{b_a}^2)$，$n_{b_{\omega }}\sim \mathcal{N}(0,{\sigma }_{b_{\omega }}^2)$
$${\dot{b}}_{a_t}=n_{b_a}，{\dot{b}}_{{\omega }_t}=n_{b_{\omega }}\text{\hspace{1em}(2)}$$

2）预积分

对于两个连续时间帧$b_k$和$b_{k+1}$，在时间间隔$[t_k，t_{k+1}]$中存在若干个惯性测量单元测量值。给定零偏估计，我们将它们整合到局部坐标系$b_k$中

$$\begin{gathered} {\alpha }_{b_{k+1}}^{b_k}=\int {\int }_{t\in [t_k,t_{k+1}]}{R}_t^{b_k}({\hat{a}}_t-b_{a_t})d{t}^2 \\ {\beta }_{b_{k+1}}^{b_k}={\int }_{t\in [t_k,t_{k+1}]}{R}_t^{b_k}({\hat{a}}_t-b_{a_t})dt \\ {\gamma }_{b_{k+1}}^{b_k}={\int }_{t\in [t_k,t_{k+1}]}\frac{1}{2}\Omega ({\hat{\omega }}_t-b_{{\omega }_t}){\gamma }_t^{b_k}dt\text{\hspace{1em}(3)} \end{gathered}$$
这里
$$\Omega (\omega )=\left[\begin{array}{cc}-{\omega }_{\times }& \omega \\ -{\omega }^T& 0\end{array}\right],{\omega }_{\times }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right].\text{\hspace{1em}(4)}$$
$\alpha$，$\beta$和$\gamma$的协方差矩阵$P_{b_{k+1}}^{b_k}$也是一致传播的。可以看出，预积分项(3)可以通过在给定偏差参考帧的零偏$b_k$来从惯性测量单元的测量值单独获得。

3）零偏矫正

如果零偏的估计值发生了轻微的变化，我们通过关于零偏的一阶近似来调整${\alpha }_{b_{k+1}}^{b_k}$、${\beta }_{b_{k+1}}^{b_k}$和${\gamma }_{b_{k+1}}^{b_k}$，如下所示
$$\begin{gathered} {\alpha }_{b_{k+1}}^{b_k}\approx {\hat{\alpha }}_{b_{k+1}}^{b_k}+J_{b_a}^{\alpha }\delta b_{a_k}+J_{b_{\omega }}^{\omega }\delta b_{{\omega }_k} \\ {\beta }_{b_{k+1}}^{b_k}\approx {\hat{\beta }}_{b_{k+1}}^{b_k}+J_{b_a}^{\beta }\delta b_{a_k}+J_{b_{\omega }}^{\beta }\delta b_{{\omega }_t} \\ {\gamma }_{b_{k+1}}^{b_k}\approx {\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes [\begin{array}{c}1\\ \genfrac{}{}{0ex}{}{}{2}\end{array} \end{gathered}$$