Robust Initialization of Monocular Visual-Inertial Estimation on Aerial Robots

VINS初始化论文详解

I.Motivation

单目相机在视觉系统应用中的短板

尺度不确定性
光照变化，弱纹理以及运动模糊等导致视觉跟踪丢失

IMU作为视觉互补的传感器的好处

恢复尺度，同时roll和pitch都可以直接获取
可以帮助系统在高速飞行下有更好的稳定性，弥补视觉跟踪丢失等情况

视觉惯导系统是一个非线性的系统，一个良好的初始值严重影响系统的性能，一个不良的初始值可能使估计器发散或者陷入局部最小化；

由于初始状态的R,tR, tR,t是一个可变的原点状态（即不需要考虑位置），因此实际需要估计的是重力向量，速度，传感器偏差和特征点深度；

视觉惯导初始化的难点：

单目无法恢复尺度
IMU需要非零加速度运动才可估计尺度
- 因为IMU只有加速度计，需要积分才能得到速度和位移
- 但由于加速度计获得的数据是包含重力向量的，因此加速度与重力向量是耦合的
  - 因为是加速度运动，所以无法估计重力向量
  - 无法估计重力向量就无法得到得到加速度
任务中由于时间等限制因素，难以保证无人机能静止或者按设定的运动进行初始化
- 特别是在运动过程中，发生估计器失效需要重启的情况
- 因此凸显了飞行中自动重新初始化的重要性

文章贡献点：

未知先验信息情况下，实现运动中的初始化速度，重力向量，尺度和陀螺仪的偏置
将中初始化算法应用与基于滑窗的单目视觉惯导系统并开源

II.Related Work

早期的视觉惯导系统大多假设某些初始状态是已知的或者忽略某些初始状态，抑或是需要系统在启动前先保持静止或者水平状态。在没有先验信息下，大多数系统都无法适应动态起飞或运动中的初始化

S Shen 等人早期利用陀螺仪短暂的积分获取相对旋转实现对线性估计器的初始化，该法能很好的应用与室内环境，然而在特征点分布在大范围的室外由于没能覆盖传感器噪声而表现不理想。

[7] S. Shen, Y. Mulgaonkar, N. Michael, and V. Kumar, “Initialization-free monocular visual-inertial estimation with application to autonomous MAVs,” in Proc. of the Int. Sym. on Exp. Robot., Marrakech, Morocco, Jun. 2014.

[12] Z. Yang and S. Shen, “Monocular visual–inertial state estimation with online initialization and camera–imu extrinsic calibration,” IEEE Transactions on Automation Science and Engineering, vol. 14, no. 1, pp. 39–51, 2017.

通过分析SfM和惯导积分实现初始化

[23] L. Kneip, S. Weiss, and R. Siegwart, “Deterministic initialization of metric state estimation filters for loosely-coupled monocular visioninertial systems,” in Proc. of the IEEE/RSJ Int. Conf. on Intell. Robots and Syst., Sep. 2011, pp. 2235–2241.

基于ORB-SLAM的惯导初始化，通过视觉估计得到估计值后进行初始化，然而据说其尺度收敛耗时往往多于10s，因此并不适合于一开始就需要尺度信息的机器人导航

[24] R. Mur-Artal and J. D. Tardos, “Visual-inertial monocular SLAM with map reuse,” arXiv preprint arXiv:1610.05949, 2016.

本文的方法于上一个方法比较相似，我们忽略了加速度计的偏置以确保初始化过程的快速性，同时发现加速度计的偏置与重力向量耦合，往往不可观。关于加速度计偏置的估计会在Sect. IV-E中讨论

III.Overview总体思想

纯视觉可通过八点法，五点发，找本质矩阵或基础矩阵获得帧间相对位姿

[22] A. Heyden and M. Pollefeys, “Multiple view geometry,” Emerging Topics in Computer Vision, 2005.

[27] D. Nister, “An efficient solution to the five-point relative pose prob- ´ lem,” IEEE transactions on pattern analysis and machine intelligence, vol. 26, no. 6, pp. 756–770, 2004.

然后使用BA去优化，以得到尺度未知的地图

[28] B. Triggs, P. F. McLauchlan, R. I. Hartley, and A. W. Fitzgibbon, “Bundle adjustmenta modern synthesis,” in International workshop on vision algorithms. Springer, 1999, pp. 298–372.

通过与imu测量值进行对齐进而提取初始化值（重力向量，速度和偏置）

构造一个松耦合的视觉惯导初始化过程

先进行纯视觉的位姿估计
然后跟IMU预积分的值对齐恢复所需初始化参数

IV.Methodology

符号定义：

(⋅)v(\cdot)^v(⋅)v表示以SfM结果为参考系
(⋅)^\hat{(\cdot)}(⋅)^表示以传感器测量数据
(⋅)ˉ\bar{(\cdot)}(⋅)ˉ表示SfM尺度未知的估计参数

A. Vision-Only Structure

搜索出与最新帧有足够视差的关键帧
通过五点法（即用五对点求解本质矩阵）求解两帧间的相对位姿
随机设置一个尺度sss并将特征点全部三角化
利用PnP（Perspective-n-Point，3D-2D）来恢复滑窗内的其他帧的位姿
进行全局BA最小化重投影误差
根据外参，将变量都转移到imu坐标系下

(⋅)v(·)^v(⋅)vis the reference frame in SfM, which is an arbitrarily fixed frame in visual structure, irrelevant to inertial measurement.

注意这里都是在基于SfM的参考坐标系下

B. IMU Pre-Integration

IMU反馈的频率比视觉的高许多，因此在两帧间存在着大量的imu数据；对这些数据进行预积分，同时以预积分的结果作为增量度量约束

IMU数据测量方程：

受偏置bias bbb 和噪声 η\etaη 影响

对于两连续帧bkb_kbk和bk+1b_{k+1}bk+1，可以对线加速度和角速度在参考坐标系bkb_kbk下进行预积分，相对位移αbk+1bk\alpha^{b_k}_{b_{k+1}}αbk+1bk，相对速度βbk+1bk\beta^{b_k}_{b_{k+1}}βbk+1bk和相对旋转γbk+1bk\gamma^{b_k}_{b_{k+1}}γbk+1bk表示如下

可以知道这些相对变量独立于参考系bkb_kbk的初始的位置和速度

IMU预积分补充知识（参考VINS-MONO）

IMU测量方程：

两帧间[tk,tk+1][t_k,t_{k+1}][tk,tk+1]世界坐标系下的普通积分方程：

补充：基于扰动模型的四元数导数

Sola J. Quaternion kinematics for the error-state Kalman filter[J]. arXiv preprint arXiv:1711.02508, 2017.

P44, P7

从上述世界坐标系下的积分可以看出，每当初始状态改变都需要重新积分，特别是基于优化的算法，意味着每调整一次位姿都需要重新进行积分，极其消耗算力，因此预积分方法应运而生。

通过将参考坐标系由世界坐标系改为相对参考系（local frame）bkb_kbk，即两帧间积分中的前一帧为参考系，这样只需要预积分一次在该参考系下的线加速度和角速度则在以后都可以复用。

这些预积分项现在只与偏置和噪声有关，如果偏置变化很小，我们可以通过一阶近似调整这些预积分项；如果变化较大的话，还是需要重新进行积分。

离散时间预积分

欧拉法（Euler）
中值积分法（mid-point）
龙格库塔积分法（RK4）

以欧拉法为例作讲解，代码使用的是中值积分法：

初始状态下，αbkbk=0\alpha^{b_k}_{b_{k}}=0αbkbk=0, βbkbk=0\beta^{b_k}_{b_{k}}=0βbkbk=0, γbkbk=[1,0v]T\gamma^{b_k}_{b_{k}}=[1,\mathbf{0}_v]^Tγbkbk=[1,0v]T

噪声项未知，设其为0

后续协方差传播和动态误差方程有缘再更，可自行看vins-mono的论文

C. Visual-Inertial Alignment

前面已经获得了由SfM得到的尺度未知的相机姿态和IMU的预积分。

1) Gyroscope Bias Calibration

已知：

由视觉估计获取的相对姿态qbkv\mathbf{q}^v_{b_k}qbkv和qbk+1v\mathbf{q}^v_{b_{k+1}}qbk+1v
由预积分得到的姿态变换约束γbk+1bk\gamma^{b_k}_{b_{k+1}}γbk+1bk

最小化误差以获取陀螺仪偏置

稍微解释一下误差函数的构造
qbk+1v−1⊗qbkv⊗γbk+1bk=qbk+1v−1⊗q^bk+1v=γˉbk+1bk−1⊗γbk+1bk{\mathbf{q}_{b_{k+1}}^{v}}^{-1} \otimes \mathbf{q}_{b_{k}}^{v} \otimes \gamma_{b_{k+1}}^{b_{k}} \\ ={\mathbf{q}_{b_{k+1}}^{v}}^{-1} \otimes \mathbf{\hat q}_{b_{k+1}}^{v} \\ ={\bar\gamma_{b_{k+1}}^{b_{k}}}^{-1} \otimes \gamma_{b_{k+1}}^{b_{k}} qbk+1v−1⊗qbkv⊗γbk+1bk=qbk+1v−1⊗q^bk+1v=γˉbk+1bk−1⊗γbk+1bk
综上可以获得偏置的更新量δbg\delta b_gδbg，将此更新量加到初始偏置上bg0=0b_{g_0}=0bg0=0上获得估计的陀螺仪偏差，使用估计的陀螺仪偏差重新更新预积分

至此视觉与IMU已经完成了初略的对准，即姿态匹配上了

2) Initializing Velocity, Gravity Vector and Metric Scale

定义待估计变量：

对应关键帧的速度，重力向量和尺度

定义残差为预积分（IMU测量值）与视觉估计的误差

其中z^bk+1bk=[α^bk+1bkβ^bk+1bk]\hat{\mathbf{z}}_{b_{k+1}}^{b_{k}}=\left[\begin{array}{l}\hat{\boldsymbol{\alpha}}_{b_{k+1}}^{b_{k}} \\ \hat{\boldsymbol{\beta}}_{b_{k+1}}^{b_{k}}\end{array}\right]z^bk+1bk=[α^bk+1bkβ^bk+1bk]为预积分数据，Hbk+1bkXI+nbk+1bk\mathbf{H}_{b_{k+1}}^{b_{k}} \mathcal{X}_{I}+\mathbf{n}_{b_{k+1}}^{b_{k}}Hbk+1bkXI+nbk+1bk为SfM得到的数据，这里依然设定噪声项nbk+1bk=0\mathbf{n}_{b_{k+1}}^{b_{k}}=0nbk+1bk=0，其中尺度sss隐含在SfM测量函数中，由以下公式关联

由此可以构造最小二乘问题求解得速度，重力向量以及尺度

这里省略亿点点过程，主要是如何从SfM估计的位姿转化为预积分对应项，以及代码中如何将最小二乘问题转化为构造并求解Ax=bAx=bAx=b，基本原理还是很简单的，有空再更新下过程。

3) Gravity Refinement

由于一个地方的重力是可以测量得到的，也即重力的模已知的；因此，优化重力向量从三维变为二维。然而若把重力的模作为约束加入到上述问题中将会使其转变为非线性问题，也即无法利用Ax=bAx=bAx=b来进行高效的求解。

将重力向量在切线空间（tangent space）上进行参数化
g⋅g^ˉ+w1b1+w2b2g \cdot \bar{\hat{\mathrm{g}}}+w_{1} \mathbf{b}_{1}+w_{2} \mathbf{b}_{2} g⋅g^ˉ+w1b1+w2b2
ggg为重力的模，g^ˉ\bar{\hat{\mathrm{g}}}g^ˉ为当前估计的重力方向，b1,b2\mathbf{b}_1,\mathbf{b}_2b1,b2为切线空间中的两个正交基，ω1,ω2\omega_1,\omega_2ω1,ω2为对应方向b1,b2\mathbf{b}_1,\mathbf{b}_2b1,b2上的位移

将g⋅g^ˉ+w1b1+w2b2g \cdot \bar{\hat{\mathrm{g}}}+w_{1} \mathbf{b}_{1}+w_{2} \mathbf{b}_{2}g⋅g^ˉ+w1b1+w2b2，带回z^bk+1bk=Hbk+1bkXI+nbk+1bk\hat{\mathbf{z}}_{b_{k+1}}^{b_{k}}=\mathbf{H}_{b_{k+1}}^{b_{k}} \mathcal{X}_{I}+\mathbf{n}_{b_{k+1}}^{b_{k}}z^bk+1bk=Hbk+1bkXI+nbk+1bk，依然可以得到一个线性的最小二乘问题，通过迭代或者构造Ax=bAx=bAx=b问题求解出ω1,ω2\omega_1,\omega_2ω1,ω2，即可以得到精细化后的重力向量

根据估计的重力向量，可以将之前估计的所有状态都转换到世界坐标系下，综上完成了VIO的初始化过程。

D. Nonlinear VINS Estimator

基于图优化的紧耦合视觉惯导系统

对应的状态表达如下（忽略转置）：

Xk\mathbf{X}_{k}Xk对应IMU在第kthk^{th}kth帧的状态，nnn为滑窗内关键帧总数，mmm为特征点个数，用逆深度λi\lambda_iλi表示，xcb\mathbf{x}_c^bxcb为外参

将状态估计问题构造为非线性最小二乘问题如下：

第一项为边缘化的先验信息，第二项为IMU测量残差，第三项为视觉重投影残差。具体看VINS-MONO论文

E. Discussions

为了保证系统的能观性（除了全局位置漂移和yaw角），视觉及IMU初始化都需要充分的激励。

其中视觉可以选取拥有足够视差的关键帧来保证，而对于IMU测量来说，能保证SfM成功的那些帧对应的IMU测量数据并不一定能保证对应IMU初始化的成功，对于某些直线或匀速运动是无法避免的。因此直觉上，可以通过检查加速度及角速度的方差或者标准差来判断IMU的激励程度，来决定是否需要进行IMU的初始化

然而代码里即便在IMU未完全激励下，仍不会阻止系统进行初始化，而对应结果是运动状态初始化无太大影响，而在静止状态强行进行初始化，会因为加速度计无有效数据进行优化导致系统漂移，此时补充一个静止状态下的加速度计偏置估计能有效解决问题

论文进行对加速度计偏置也做了与角速度计偏置对应的处理来进行求解，通过仿真实验发现需要在短时间内至少30°的旋转才能很好的估计加速度计的偏置，论证了初始化阶段难以对加速度计偏置进行估计

而另一个仿真实验，忽略了加速度计偏置，考虑角速度计偏置，发现即便忽略加速度计的偏置，仍能获得一个良好的初始估计

综上，在由足够运动激励下，能够在初始化阶段忽视加速度计偏置

V. EXPERIMENTAL RESULTS

A. Performance on Public Datasets

1) Initial Values Recovery

分别进行初始时刻初始化以及随机选时间进行初始化以验证飞行途中重新初始化的性能，下图展示了在MH 01 easy数据集上进行25次随机时间初始化结果，若以尺度估计的误差低于10%为成功，则成功率高达84%，同时，即便误差高达30%非线性估计器也能够成功运行

2) Overall Performance

以提出的VINS系统对比了OKVIS，一个双目视觉惯导系统

可以看到即便VINS是单目系统，但总体性能优于双目的OKVIS；

同时即便初始化阶段忽略了加速度计偏置的估计，但相应偏差项都能够收敛于真值。

B. Real World Experiment

1) Indoor Closed-Loop Control

2) Outdoor Environments