VINS-初始化:【翻译】Robust Initialization of Monocular Visual-Inertial Estimation on Aerial Robots
目录
- 用于空中机器人的单目视觉惯性估计的鲁棒初始化
-
- 摘要
- 简介
- 综述
- 方法
-
- 纯视觉结构
- IMU预积分
- 视觉-惯性对齐
- 非线性VINS估计
- 讨论
- 实验结果
-
- 在公共数据集上的性能
- 真实世界的实验
- 结论
用于空中机器人的单目视觉惯性估计的鲁棒初始化
摘要
在本文中，我们提出了一种鲁棒的在线估计器初始化算法，为单目视觉惯性系统（VINS）提供高质量的初始状态。由于VINS的非线性，较差的初始化可能会严重影响基于滤波的或基于图像的方法的性能。我们的方法从纯视觉运动恢复结构（SFM）开始，构建相机位姿和特征位姿的尺度结构。通过使用预集成的IMU测量松散地对齐这种结构，我们的方法恢复了公制尺度，速度，重力矢量和陀螺仪偏置，用于作为初始值，以引导非线性紧耦合优化框架。我们的方法可以在不使用关于系统状态和移动的任何先验信息的情况下在各种场景中进行在线初始化。我们的方法通过公共UAV数据集和实时机载实验验证了所提出的方法的性能。
简介
视觉惯性融合是目前机器人领域的一个研究热点。在航空制图、自动驾驶、交通运输、监视和救援等领域都需要精确的状态估计。传统的纯视觉算法（如[1]-[5]）可以对环境进行姿态估计和结构构建。然而，由于单台摄像机无法恢复尺度，这些算法不能直接应用于实际应用中，特别是在自主导航中。通常，惯性测量单元（IMU）被视为基于视觉算法的补充传感器[6]-[12]。通过融合IMU的公制测量值，可以完全恢复比例、横滚角和俯仰角。此外，当由于光照变化、无纹理区域和aggressive运动引起的运动模糊而导致跟踪丢失时，IMU测量可以辅助视觉。在IMU的帮助下，性能得到了极大的提高。一个摄像头和一个低成本的IMU构成了最小的传感器组，以提供足够的自我和环境意识，可以在任何智能平台上很容易地获得。
由于视觉惯性系统的非线性，单目估计器的性能在很大程度上依赖于初始值（重力、速度、偏差和特征深度）的精度。较差初始化会降低收敛速度甚至导致完全不正确的估计，特别是对于完全六自由度的空中机器人，精确的初始值是至关重要的。然而，单目视觉惯性系统的初始状态是难以精确地得到的。一方面，不能从单目摄像机获得度量尺度。另一方面，需要非零加速度运动来初始化度量尺度。这导致未知初始姿态（重力矢量）和速度。特别是在有限的搜索和救援任务中，让MAV静止或沿特定模式移动时进行初始化，通常是不可行的。快速启动MAV并初始化估算器，而无需任何关于动态运动的先验知识是很有必要的。另外，视觉算法在运动速度快或光照变化大的情况下都是脆弱的。当视觉跟踪丢失时，估计器将很容易失败。这表明需要开发机上的自动重新初始化功能。所有这些问题都促使我们找到一个健壮的系统，它能够实时初始化以恢复所有关键状态。
在本文中，我们提出了一种允许在飞行中初始化单目视觉惯性系统的方法。初始速度，重力矢量，尺度以及陀螺仪偏差在初始化过程中可以得到校准。我们首先执行纯视觉运动恢复结构（SFM），然后用SFM结果松散地对齐IMU测量，以获得度量初始状态。我们的方法的性能由公共数据集和室内和室外环境的实时实验证明。
我们的贡献有三点：
-
鲁棒的视觉惯导初始化过程，可以为空中机器人提供在线初始状态
-
我们将所提出的方法应用于基于滑动窗口的单目惯性系统，以执行闭环自主飞行实验
-
开源
综述
视觉和惯性测量是两种互补的资源。视觉代表了最新的全局结构，IMU引入了尺度增量信息。绝对尺度和速度在单目相机中不可实现。为了初始化度量尺度，需要非零加速运动，这会导致初始姿态（重力矢量）和初始速度未知。所以我们不能假设估计器是从平稳状态开始的。更糟糕的是，IMU通常受到不可忽视的bias（偏差）的影响。如果没有一个好的初始估计，很难将这两个因素直接融合在一起。一个不好的初值会导致估计量发散或陷入局部极小值。为了提高单目视觉惯性系统的成功率，需要一个鲁棒的初始化过程。
单目视觉SLAM或运动结构（SfM）比视觉惯性系统更容易初始化。纯视觉系统通过八点法[22]、五点法[27]、齐次法和基本法很容易bootstrap自己。然后进行BA[28]，以细化结构。给定一个有尺度的视觉结构，我们可以将IMU测量值对准这个结构来提取视觉惯性系统的初始值（重力、速度和偏差）。受此启发，我们采用了松耦合的视觉惯性初始化过程。首先构造纯视觉结构，然后将该结构与IMU预积分对齐，恢复初始值。我们提出的方法的pipeline如图1（a）所示。
图一：(a)初始化过程的主要结构 (b)自行研制的用于室内闭环实验的带有一个前视摄像头的四旋翼飞行器
方法
我们从定义符号开始。我们认为 ( . ) w (.)^w (.)w是世界坐标系，重力矢量沿z轴方向。 ( . ) v (.)^v (.)v是SfM中的参考坐标系，是视觉结构中的任意固定坐标系，与惯性测量无关。 ( ⋅ ) b w (\cdot)_{b}^{w} (⋅)bw是相对于世界坐标系的body坐标系。我们将IMU框架视为body框架，即IMU框架与（body）主体框架对齐。 b k b_k bk是第k帧图像的body坐标系. ( ⋅ ) v c (\cdot)_{v}^{c} (⋅)vc是相对于视觉参考系的相机坐标系。 c k c_k ck是第k帧图像的相机坐标系。我们使用 ( ⋅ ^ ) (\hat{\cdot}) (⋅^)表示可能受噪声和偏差影响的传感器测量值。我们使用 ( ⋅ ˉ ) ({\bar\cdot}) (⋅ˉ) 表示SfM结构中的尺度参数。我们用四元数 q q q来表示旋转。⊗ 是两个四元数的乘法运算。 g w g^w gw=[0，0，g] T ^T T是世界坐标系中的重力矢量。 g v g^v gv是视觉参考系中的重力矢量。
我们假设在初始化步骤中已知摄像机的内部校准和摄像机与IMU之间的外部校准。事实上，我们不需要非常精确的外部校准，因为我们将在非线性优化（Sect.IV-D）中不断地改进它。
纯视觉结构
初始化过程以纯视觉结构开始，该结构估计尺度级相机姿势和特征位置的图.我们的方法基于滑动窗口的方法[8,12]，其维护了若干空间独立的图像帧。空间帧的选择是通过邻近区域有足够的视差来决定的。在这些帧中提取并跟踪稀疏特征[29]。相对应的特征将用于构造窗口内部的视觉结构。
作为计算机视觉中常用的一种技术，我们首先选择两帧具有足够特征视差的图像。然后使用五点法[27]来恢复这两帧之间的相对旋转和尺度平移。然后我们任意设置比例，并对这两帧中观察到的所有特征进行三角剖分。基于这些三角化特征，采用透视n点（PnP）方法估计窗口中其他帧的姿态。最后，应用全局BA[28]，以最小化所有特征观测值的总重投影误差。在那之后，我们得到所有的框架姿势( p ‾ c k v , q c k v \overline{\mathbf{p}}_{c_{k}}^{v}, \mathbf{q}_{c_{k}}^{v} pckv,qckv）和特征位置。假设我们在相机帧和IMU（body）帧之间有外部参数（ p b c , q b c \mathbf{p}_{b}^{c}, \mathbf{q}_{b}^{c} pbc,qbc）的先验，所有变量都可以从相机帧转换到IMU帧，
相对于参考坐标系的第k帧body坐标系的旋转 = 相对于参考坐标系的第k帧相机坐标系的旋转 ⊗ 相对于相机坐标系的body坐标系的旋转
相对于参考坐标系的第k帧body坐标系的尺度平移 = 相对于参考坐标系的第k帧相机坐标系的尺度平移 +相对于参考坐标系的第k帧相机坐标系的旋转 * 相对于相机坐标系的body坐标系的平移
其中，S是未知的规模，将在下一步解决。
IMU预积分
IMU测量的频率比视觉测量高得多。通常，在两个连续的视觉帧之间存在几十个IMU的测量值。我们在局部帧中预积分[26]这些IMU测量值，并将积分结果视为增量度量约束（incremental metric constraint.）。
我们将IMU测量值（角速度和加速度）表示为 ω ^ b , a ^ b \hat{\boldsymbol{\omega}}^{b}, \hat{\mathbf{a}}^{b} ω^b,a^b 这些测量值受偏差 b b b和噪声 η η η的影响,
IMU的角速度测量值=IMU的角速度真值+陀螺仪的偏差+陀螺仪的高斯白噪声
IMU t时刻加速度的测量值 =相对世界坐标系的t时刻body坐标系的旋转 *（IMU t时刻加速度的真值+世界坐标系下重力加速度）+ 加速度计的偏差+ 加速度计的高斯白噪声
给定对应于图像帧 b k b_k bk和 b k + 1 b_k+1 bk+1的两个时间阶段，我们可以在局部帧bk中预积分线加速度和角速度：
b k b_k bk和 b k + 1 b_k+1 bk+1帧之间IMU的相对位置（位移）= t帧到k帧的相对旋转约束*IMU在第t时刻的加速度在k到k+1帧时刻对t的平方做积分
b k b_k bk和 b k + 1 b_k+1 bk+1帧之间IMU的相对速度 = t帧到k帧的相对旋转约束*IMU在第t时刻的加速度在k到k+1帧时刻对t做积分
b k b_k bk和 b k + 1 b_k+1 bk+1帧之间IMU的相对旋转约束= t帧到k帧的相对旋转约束对时间的导数在k到k+1帧时刻对t做积分
α b k + 1 b k , β b k + 1 b k , γ b k + 1 b k \boldsymbol{\alpha}_{b_{k+1}}^{b_{k}}, \boldsymbol{\beta}_{b_{k+1}}^{b_{k}}, \boldsymbol{\gamma}_{b_{k+1}}^{b_{k}} αbk+1bk,βbk+1bk,γbk+1bk 分别表示相对位置、相对速度和相对旋转约束。可以看出，惯性测量通常被积分以形成相对运动约束，其与帧 b k b_k bk的初始位置和速度无关
视觉-惯性对齐
我们从SfM（Sect. IV A）得到尺度级相机位姿和IMU预积分的测量（Sect. IV -B）。在本节中，我们将详细介绍如何对齐这两个部分的方法。
（1）陀螺仪偏差标定：考虑窗口中连续两帧bk和bk+1，从视觉结构上可以得到相对旋转 q b k v \mathbf{q}_{b_{k}}^{v} qbkv 和 q b k + 1 v \mathbf{q}_{b_{k+1}}^{v} qbk+1v，以及来自IMU预积分的相对（旋转）约束 γ ^ b k + 1 b k \hat{\gamma}_{b_{k+1}}^{b_{k}} γ^bk+1bk。我们通过最小化这两项之间的误差来估计陀螺仪偏差：
最小化 b k b_k bk和 b k + 1 b_k+1 bk+1帧之间的相对旋转：获得使窗口所有帧中相对于参考坐标系的bk+1帧的旋转四元数的倒数 ⊗ 相对于参考坐标系的bk帧的旋转四元数 ⊗ b k b_k bk和 b k + 1 b_k+1 bk+1帧之间IMU的相对旋转约束的总误差 最小的陀螺仪的偏差值
线性化相对于陀螺仪偏差的旋转约束
其中 B \mathcal{B} B是窗口中的所有的帧。在第二个方程中，我们线性化了相对于陀螺仪偏差的旋转约束。基于相对约束的视觉结构旋转对齐γ, 我们可以得到 b g b_g bg的估计值。然后我们更新相对于 b g b_g bg的 α ^ b k + 1 b k , β ^ b k + 1 b k \hat{\boldsymbol{\alpha}}_{b_{k+1}}^{b_{k}}, \hat{\boldsymbol{\beta}}_{b_{k+1}}^{b_{k}} α^bk+1bk,β^bk+1bk。
(2)初始化速度、重力矢量和公制比例: 我们将在这一步中估计的变量定义为
[相对于参考坐标系的b0帧的速度，相对于参考坐标系的b1帧的速度，…
，相对于参考坐标系的bn帧的速度，相对于参考坐标系的重力矢量，公制比例参数]
其中s是将视觉结构与IMU测量隐式提供的实际尺度对齐的比例参数。下面的等式描述了度量位置和视觉结构的速度约束之间的关系，
[ b k b_k bk和 b k + 1 b_k+1 bk+1帧之间IMU的相对位置（位移）测量值， b k b_k bk和 b k + 1 b_k+1 bk+1帧之间IMU的相对速度测量值] T ^T T = b k b_k bk和 b k + 1 b_k+1 bk+1帧之间的系数矩阵（？） * 视觉结构中待估计变量 + b k b_k bk和 b k + 1 b_k+1 bk+1帧之间的噪声
在上述公式中， q b k v , p ‾ b k v , p ‾ b k + 1 v \mathbf{q}_{b_{k}}^{v}, \overline{\mathbf{p}}_{b_{k}}^{v}, \overline{\mathbf{p}}_{b_{k+1}}^{v} qbkv,pbkv,pbk+1v来自视觉结构。 q b k v \mathbf{q}_{b_{k}}^{v} qbkv是 q v b k \mathbf{q}_{v}^{b_{k}} qvbk的逆旋转。 ∆ T k ∆T_k ∆Tk是两个连续帧之间的时间间隔。通过求解这个最小二乘问题
我们可以得到在视觉参考系（.） v ^v v中的速度，重力矢量，以及尺度参数。从视觉结构中得到平移分量 p ‾ v \overline{\mathbf{p}}^{v} pv将放缩到公制单位。通过范数约束，估计得到的重力将再次经过一轮细化。
(3) 重力细化：通过约束重力矢量的大小，可以细化上一步得到的重力矢量。在大多数情况下，重力矢量的大小是已知的。然而，如果直接将这个范数约束加入到（7）中的优化问题中，它将变得非线性，难以求解。在这里，我们使用一种方法，通过优化其切线空间上的二维误差状态来实行重力范数（约束）。由于重力的大小是已知的，所以重力的自由度是2，我们可以在它的切线空间上用两个变量来参数化重力。我们把重力参数化为 g ⋅ g ^ ‾ + w 1 b 1 + w 2 b 2 g \cdot \overline{\hat{\mathrm{g}}}+w_{1} \mathbf{b}_{1}+w_{2} \mathbf{b}_{2} g⋅g^+w1b1+w2b2，其中g是重力的大小， g ^ ‾ \overline{\hat{\mathrm{g}}} g^是当前估计的方向向量，b1和b2是跨越切平面的两个正交基。W1和W2分别是朝向B1和b2的相应位移。我们可以用Gram-Schmidt过程很容易地找到一组b1，b2。通过这种方式，我们在重力的切线空间上通过两种状态来重新参数化重力，如图2所示。然后用g代替（6）中的 g ⋅ g ^ ‾ + w 1 b 1 + w 2 b 2 g \cdot \overline{\hat{\mathrm{g}}}+w_{1} \mathbf{b}_{1}+w_{2} \mathbf{b}_{2} g⋅g^+w1b1+w2b2，而且它也是线性形式。将此过程反复多次，直到ˆ g收敛。
在细化重力矢量之后，我们根据重力矢量将所有变量从可视参考框架（.） v ^v v旋转到世界框架（.） w ^w w。此时，初始化过程完成，这些度量值将被输入到紧耦合非线性视觉惯性估计器中。
图2:因为重力的大小是已知的，所以重力的自由度是2。g位于半径为已知大小|g|的球体上。我们把当前估计的重力参数化为 g ⋅ g ^ ‾ + w 1 b 1 + w 2 b 2 g \cdot \overline{\hat{\mathrm{g}}}+w_{1} \mathbf{b}_{1}+w_{2} \mathbf{b}_{2} g⋅g^+w1b1+w2b2 ，其中b1和b2是跨越切线空间的两个正交基。
非线性VINS估计
获得所有必要的初始值后，我们可以启动我们的紧耦合的VINS估计器[8,31]。在这里，我们简要地描述基于图的非线性视觉惯性系统的解决方案。
在具有N个IMU帧和M个特征的滑动窗口中，所有状态的定义为（忽略转置）：
[第0帧的IMUPVQ bias的集合，第1帧的IMUPVQ bias的集合…，第n帧的IMUPVQ bias的集合，相机坐标系到body坐标系的外部转换矩阵，第0个三维特征的逆深度，第1个三维特征的逆深度，第m个三维特征的逆深度]
[相对于世界坐标系的第k帧body坐标系的平移，相对于世界坐标系的第k帧body坐标系的速度，相对于世界坐标系的第k帧body坐标系的旋转，加速度计的bias，陀螺仪的bias]
[相机坐标系到body坐标系的旋转，相机坐标系到body坐标系的平移]
其中，第k个IMU的状态包括位置 P b k w \mathbf{P}_{b_{k}}^{w} Pbkw、速度 v b k w \mathbf{v}_{b_{k}}^{w} vbkw、body坐标系bk相对于世界框架w的方向 q b k w \mathbf{q}_{b_{k}}^{w} qbkw以及IMU偏移 b a b_a ba、 b g b_g bg。当第一次在相机帧中观测时，三维特征由其逆深度λ参数化， x c b \mathbf{x}_{c}^{b} xcb是相机坐标系c到body坐标系b的外部变换。VINS估计被定义为一个非线性最小二乘问题，
计算使度量位置和视觉结构的速度约束的残差+惯性非线性残差+视觉测量的非线性残差的误差值最小二乘法最小的IMU P，V，Q，bias的值。
其中 r B ( z ^ b k + 1 b k , X ) r_{\mathcal{B}}\left(\hat{\mathbf{z}}_{b k+1}^{b_{k}}, \mathcal{X}\right) rB(z^bk+1bk,X) 和 r C ( z ^ l c j , X ) r_{\mathcal{C}}\left(\hat{\mathbf{z}}_{l}^{c_{j}}, \mathcal{X}\right) rC(z^lcj,X)是惯性和视觉测量的非线性残差函数。 ∣ ∣ . ∣ ∣ ||.|| ∣∣.∣∣是用协方差P加权的马氏距离。具体而言， r B r_B rB是惯性测量积分中连接一对连续帧 b k b_k bk和 b k + 1 b_k+1 bk+1的IMU因子的残差 z ^ b k + 1 b k \hat{\mathbf{z}}_{b k+1}^{b_{k}} z^bk+1bk。 r C r_C rC是视觉因子的残差，通过重投影函数它建立了landmark测量 z ^ l c j \hat{\mathbf{z}}_{l}^{c_{j}} z^lcj和状态之间的联系。 ρ ( ⋅ ) ρ(·) ρ(⋅) 是鲁棒的huber范数[32]。过去的状态被边缘化，并转换为先前的信息{ r p ， H p {r_p，H_p} rp，Hp}。优化的细节可以在[33]找到。
讨论
为了实现单目VINS除全局位置偏移和yaw角外的完全可观测性，需要在视觉和IMU因子上都有足够的激励。视觉模块的可观测性可以通过选择多个具有足够视差的空间分离帧来保证。然而，在SfM窗口内的IMU测量可能不会使整个系统可见。对于旋翼机，退化运动如直线轨迹运动或零加速度运动是不可避免的。当我们在空中机器人上使用VINS时这些都是常见的问题。直观地说，我们可以在初始化阶段通过检查加速度的变化来拒绝小加速度运动 α ^ b k + 1 b k , β ^ b k + 1 b k \hat{\boldsymbol{\alpha}}_{b_{k+1}}^{b_{k}}, \hat{\boldsymbol{\beta}}_{b_{k+1}}^{b_{k}} α^bk+1bk,β^bk+1bk。初始化程序仅在窗口中的IMU测量包含足够的激励时才开始。
加速度偏差在初始化过程中很难标定，因为加速度通常和重力在小转角下耦合。为了研究加速度偏差随运动的可观测性，我们设计了以下模拟实验。在仿真环境中，空中机器人进行不同旋转水平的加速运动。加速度偏差为常数[0.1,0.1,0.1] m ⋅ s − 2 m·s^{−2} m⋅s−2,噪声标准差为0.01～0.03 m ⋅ s − 2 m·s^{−2} m⋅s−2.窗口中保留15个空间上分离的帧。图像噪声不包括在内，这意味着一个准确的视觉结构。同样的，不考虑陀螺仪的偏置和噪声，消除了陀螺仪的影响。为了校正框架中的加速度偏差，我们可以将相对于加速度偏差的位移和速度约束 α ^ b k + 1 b k , β ^ b k + 1 b k \hat{\boldsymbol{\alpha}}_{b_{k+1}}^{b_{k}}, \hat{\boldsymbol{\beta}}_{b_{k+1}}^{b_{k}} α^bk+1bk,β^bk+1bk线性化，该过程与等式4中的步骤类似。然后将加速度偏差代入公式6。加速度偏差的标定结果如图3（a）所示。x轴是窗口中沿三个轴的平均旋转变化，y轴是校准偏差的误差大小。从图中我们可以看出，如果我们想在较短的初始化过程中完全校准加速度偏差，至少需要30度的旋转。在实际场景中，由于机器人平台的动力学约束，这种主动旋转运动一开始是不可行的。
在另一个模拟中，我们测试了忽略加速度偏差时视觉惯性对齐的性能。在这个模拟中，我们考虑了图像噪声（0.5像素),陀螺仪偏差（[0.003,0.02,0.08] r a d . s − 1 rad.s^{-1} rad.s−1） 陀螺仪噪声（0.0024 r a d . s − 1 rad.s^{-1} rad.s−1) 。尺度误差和加速度偏差如图3（b）所示。x轴是加速度偏差的大小，y轴是尺度误差的百分比。在这个范围内，加速度的影响是线性的。如果我们能接受10%的尺度误差，则加速度偏差的公差范围最高达 0.3 m . s − 2 0.3m.s^{-2} 0.3m.s−2。 0.3 m . s − 2 0.3m.s^{-2} 0.3m.s−2的偏差通常是正常IMU的极值。忽略加速度偏差不会显著影响初始化结果。
从模拟中我们可以看出，除非执行充分激励的旋转，否则很难区分加速度偏差和重力偏差。由于机器人平台的动力学约束，这在实际中很难实现。忽略加速度偏差不会对初始化结果造成显著的负面影响。为此，我们将加速度偏差估计留给非线性优化（Sect.IV-D）。
图3: (a) x轴表示初始化步骤中的平均旋转变化。y轴表示不同测量噪声下加速度偏差的估计误差。可以看出，除非运动中有足够的旋转，否则很难区分加速度偏差
(b) x轴表示不同的加速度偏差。y轴表示所提出算法的估计尺度误差，不考虑加速度偏差。可以看出，有限的加速度偏差不会严重破坏算法。如果在模拟过程中，我们在初始化假设中可以接受10%的视觉比例误差，那么可承受的加速度偏差的裕度可达 0.3 m / s − 2 0.3m/s^{−2} 0.3m/s−2
实验结果
我们首先使用ASL DataSet中的公开可用的MAV Visual-Inertial数据集进行验证算法[34]。然后我们将我们的方法应用于现实世界室内和室外环境。
在公共数据集上的性能
ASL数据集中的MAV视觉惯性数据集是在微型飞行器上采集的。数据集包含立体图像（Aptina MT9V034全局快门，WVGA单色，20 FPS）、同步IMU测量（ADIS16448，角速率和加速度，200 Hz）和地面真实状态（VICON和Leica MS50）。我们只使用双目图像集中的一个相机
(1) 初始值恢复：我们使用MH_01_easy数据集进行评估。在这个数据集中，陀螺仪偏差和加速度计偏差在x、y和z轴上分别约为 [ − 0.0032 , 0.021 , 0.078 ] r a d . s − 1 [-0.0032,0.021,0.078]rad.s^{-1} [−0.0032,0.021,0.078]rad.s−1和 [ − 0.0032 、 0.026 、 0.076 ] m . s − 2 [-0.0032、0.026、0.076]m.s^{-2} [−0.0032、0.026、0.076]m.s−2。在我们的算法中，我们保持了至少15帧的初始视觉结构。我们在初始化阶段估计陀螺仪的偏差，而将加速度计的偏差估计留给后续的非线性优化。为了验证在飞行中的初始化的能力，我们在数据集中随机选择起始时间，这意味着我们的算法在无人机飞行时不需要任何先验信息就可以启动。图4显示了MH_01_easy数据集中25个不同开始时间测试的陀螺仪偏差和尺度校准的性能。这四个子图形分别是xyz轴上的陀螺仪偏差和尺度比例。陀螺在x、y两个主方向的平均偏差分别为[8.59,1.82]%。平均标度误差为8.09%。如果我们定义小于10%的比例误差为成功初始化，那么我们的过程在这个数据集中执行成功率为84%。事实上，即使初始尺度误差超过30%，非线性估计器也能成功地初始化。
图4：在MH_01_easy数据集的初始化过程中恢复陀螺仪偏差和尺度。该图包含25个不同开始时间的测试结果。
（2）整体性能：我们选择两个数据集，MH_01_easy，V1_03_difficult来展现我们的视觉惯性里程计的整体性能。在这些实验中，我们将提出的方法与OKVIS[6]进行比较，这是使用双目相机的最先进的视觉惯性算法。整个轨迹如图5所示。我们的单目系统可以达到与双目系统相同的准确性。
MH_01_easy的误差图如图6所示。x轴表示距离，y轴分别表示平移误差和旋转误差。所提出的算法在平移时获得了与双目系统几乎相同的精度。陀螺仪偏差估计如图7所示。首先，由于初始化过程的良好初始假设，我们提出了一种稳定的陀螺偏差估计方法。图8展示了加速度的偏差估计。虽然在初始化过程中忽略了加速度偏差，但偏差估计会随着运动的进行而逐渐收敛。
我们的算法在v1_03_diffcult数据集上也有很好的表现，v1_03_diffcult数据集是最具挑战性的具有积极的运动和较大的光照变化的数据集。该方法具有良好的初始假设能力，可以补偿不稳定特征跟踪带来的负面影响。与双目OKVIS相比的误差图如图9所示。我们的系统几乎达到了与这种立体算法相同的准确性。
真实世界的实验
我们还在一个空中机器人上测试了我们的算法，如图1（b）所示。一个前视全局快门相机（MatrixVision mvBlueFOX-MLC200w），配备752×480分辨率。它配备了一个190度的鱼眼镜头。DJI A3飞行控制器使用惯性测量单元（IMU，ADXL278和ADXRS290，100Hz）和姿态稳定控制。板载计算资源包括运行在3.00 GHz的Intel i7-5500U CPU。这个实验的录像可以在补充材料中找到。
（1）室内闭环控制：我们在这个室内实验中执行实时闭环控制。 视觉惯性里程计在控制回路中提供位置，姿态和速度反馈。为了测试动态初始化能力，当空中机器人在空中飞行时，我们开始视觉惯性系统（VINS），而不是稳定地在地上发射系统。启动视觉惯性系统后，我们将VINS 里程计添加到控制回路中。最后，我们将空中机器人切换为自动飞行模式，让其遵循设计的轨迹飞行。
空中机器人的飞行轨迹如图10所示。随时间变化的位置如图11所示。首先，我们手动控制四旋翼飞行。在7.5秒时，当四旋翼在空中飞行时，我们启动视觉惯性系统。估计器在初始化后1秒内输出里程计信息。在19.0s时，我们将视觉惯性估计器加入到控制回路中，这有助于稳定空气中的四旋翼。我们手动控制四转子到起始点。最后，我们在31.0s时将空中机器人切换到自主飞行模式。在视觉惯性反馈下，无人机按照轨迹自主飞行。
OptiTrack系统提供的真值进行了比较。蓝线是提出的估计器的结果。红线是真值。三条虚线代表三个时间戳，当提出的估计器启动时，估计器被加入控制回路，四旋翼开始自主飞行。室内实验全长58.12米。沿x、y和z方向的最终漂移分别为[-0.13、-0.24、-0.16]m，百分比为0.55%。
图10：室内机载闭环实验的轨迹。视觉惯性系统飞行初始化完成后，手动控制四旋翼到边界，切换到自主飞行模式。四旋翼按设计轨迹飞行。设计的轨迹是8的数字。
图11：在室内实验中，比较了该系统与OptiTrack的位置（数据）。三条虚线表示启动视觉惯性系统，将里程计加入控制回路，切换到自主飞行的时间戳。
（2） 室外环境：大尺度环境对单目视觉惯性系统来说是一个挑战。对于视觉测量，需要一个较长的运动来保证帧之间有足够的视差，这意味着空间帧之间存在一个较长的时间间隔。然而，较长时间间隔的积分会严重影响惯性测量单元的测量精度。更糟糕的是，当无人机以大俯仰角平稳飞行时，惯性测量单元的测量值会退化，除了重力外，输出几乎为零的读数，这将导致激励不足，无法完全恢复标度。
为了避免较长周期积分，我们在初始化过程中维护所有帧，而不仅仅是空间帧。此外，我们拒绝退化的运动且只有在充分激励惯性测量后才会启动所提出的程序。这些自适应使得我们的算法在大规模环境中表现良好。
我们在海拔40-90m的100m×80m户外区域验证了算法。当无人机在40米高飞行时，我们初始化视觉惯性系统，如图12所示。并与GPS进行了比较。轨迹全长575m，最终沿x，y，z方向的偏移量分别为[-3.03、0.18、2.07]m，百分比为0.64%。
图12：户外实验(a） 无人机在高空飞行的视图，带有45度前视鱼眼摄像头(b） 并与GPS进行了轨迹比较。
结论
本文提出了一种新的单目视觉惯性估计器初始化算法。我们的初始化程序为非线性VINS估计器提供初始猜测（速度、重力矢量、陀螺仪偏差和特征深度）。这些初步猜测有助于提高VINS的性能，使其能够进行动态初始化。在室内闭环控制和室外环境下，利用实际数据验证了该方法的实用性。
最后，我们对基于单目视觉惯性系统（VINS）的全自主导航进行了研究。我们的第一个结果，即一个无人机导航应用，具有单目稠密建图，避障和路径规划，在[33]提出。我们将进行广泛的研究，进一步提高系统的准确性和鲁棒性。