VIO 初始化探究：一种旋转平移解耦的高效鲁棒初始化方法

本文转载自深蓝AI

1

VIO初始化的系统介绍

首先，介绍一下VIO能做什么。假设有一系列的图像和IMU数据，现在希望把这两者数据进行融合，得到相机在空间中的一种鲁棒性好且精度较高的姿态。其中，已知IMU和相机外参、图像和IMU数据的时间戳已经同步，通过对加速度积分求解得到速度和位移，同时在图像初始速度已知的情况下能够预测图像帧的速度和位移量。当无人机或AR/VR设备悬停在空间中，重力会在加速度传感器上面存在一些分量，因此需要去估计重力方向，从而把重力从加速器的测量中移除。

所以，很多VR系统里面需要鲁棒的轨迹估计，因此需要知道图像的初始速度，或者每一帧图像的速度，然后需要知道重力方向，最后求解IMU bias。

VIO的难点在于：从用户角度来考虑，第一个是希望使用成本比较低，不会涉及一些特定的运动模式；第二个是要尽量快且准的完成初始化，因为如果VR初始化不好，那后面整个VR的轨迹可能就会容易发散。

从算法需求考虑，第一个是要求高成功率，即在快速或慢速平移情况下，能够平稳的进行初始化，还有是能用更少的图像尽快的完成初始化操作；第二个是要求高精度，使得VIO收敛快、鲁棒性好；第三个是要求高效率，即希望使用较少的资源去完成初始化。

2

详述VIO初始化原理

首先回顾一下IMU的积分以及预积分的相关知识。IMU传感器模型及预积分如图1所示。

图1 IMU预积分知识

IMU输出一般分为加速度测量值和角速度测量值，并且在角速度测量值里还包含了陀螺仪的bias以及测量噪声，而在加速度测量值中是包含了重力、加速度计的bias以及测量噪声。因此，在得到IMU的测量数据后，进行积分可以得到robot系的位置、速度和姿态。

在VIO中比较常用的一个技术是IMU预积分，如果积分的初始值改变，则普通积分需要重新计算一次，但是预积分是不依赖初始值，不用重新计算。因此，可以将IMU里的加速度和角速度数据分别写成位置速度以及旋转的预积分形式，方便求解。

基于松耦合的初始化方法（VINS-Mono, ORB-SLAM3）

VINS-Mono初始化方法如图2所示。首先，松耦合初始化方法需要构建一个SFM来获得相机姿态，但相机姿态是嵌尺度的，因此需要和IMU预积分进行对齐，从而恢复真实的轨迹。

同时，在VIO系统中，角速度是耦合的bias，所以在后续的初始化以及初始化完成之后，需要时刻去除这部分bias，再进行角速度积分。在初始化过程中还需要估计一个比较合理的陀螺仪bias，此外，也需要知道VIO的初速度，因为后续速度和位姿积分都是通过初始速度进行。

最后，在得到重力方向之后就可以将整个轨迹以及相关的初始化状态变量对齐到整个世界坐标系下。

图2 VINS-Mono初始化方法

基于松耦合的陀螺仪bias标定比较简单，如图3所示。通过SFM得到C0跟C1、C1时刻的相机的姿态，可以通过和IMU之间标定好的一个外参，可以分别得到B0跟B1之间的相对的旋转关系。

需要注意是，相对旋转关系是通过视觉进行构建，并且，还可以通过IMU预积分得到相对旋转关系，并且可以知道IMU预积分线性化后，只和bg有关系。所以，可以通过构建SFM相对旋转关系和利用IMU预积分构建总相对旋转关系方法，来进行整体的约束，从而求解整个bias。

图3 陀螺仪bias标定

VINS-Mono里进行速度、重力和尺度的初始化是通过SFM得到，那视觉位姿其实是嵌尺度，所以，需要利用视觉位姿和IMU积分的位姿进行对齐，从而来恢复尺度信息。因为在初始化时，IMU的参考坐标系是任意设置的。所以，IMU预积分是从C0(任设)坐标系开始进行，因此可简单将CO系替换成V0坐标系，得如下公式：

由于IMU的平移不仅可以通过积分得到，也可以通过视觉计算出来的位姿再乘以尺度并减去两者之间的外参关系来表示。所以，可以将视觉计算的位姿乘以尺度，然后减去它的外参，带入到IMU的预积分公式里，然后将重力跟速度相关的项放到方程右边，无关项整理到左边，进行整理得到：

最后，通过最小二乘来求得结果。

由于重力向量有两个自由度，VINS-Mono是采用了球坐标的形式来进行处理，即将三个自由度转化成两个自由度进行表示，得到

然后求解优化的W1和W2系数。b1跟b2的选择机理是：首先，如果是重力方向不等于S轴，则将重力跟X轴进行一个叉乘。之后，可以得到和平面垂直的向量，即证明跟重力方向g也垂直,就得到了b1。然后将b1跟重力方向g再进行叉乘，可以得到分别垂直于b1跟g的向量，也就是b2向量。所以，b1跟b2不仅是相互垂直，而且也都和重力方向垂直，故b1跟b2其实是在球面坐标系的一个切平面中。因此，通过微调W1跟W2，即b1跟b2的长度，进行是重力的优化。

在初始化的时候，求解的重力是在C0坐标系，即任意坐标系，而实际重力在世界系下表示是001，所以需要把整体的初始化状态变量从C0坐标系转到转到世界坐标系下。

首先是通过在C0坐标下求解重力方向，因为世界坐标下面的重力方向是001，所以可以把这两个重力方向进行叉乘以后可以得到旋转轴，另外，通过叉乘再除以点乘，然后可以得到旋转大小。然后依据旋转轴跟旋转大小可以计算出来对应的旋转矩阵，即RC0W。这样就能够通过RC0W将所有C0坐标下面的变量转到世界坐标系下。

由于在初始化中得到了尺度信息，所以可以把相机的平移和特征点的深度，然后都恢复到已知单位下面，至此的话，就可以完成VINS-Mono系统的初始化。

接下来介绍ORB-SLAM3改进的初始化方法，基本的流程和VINS-Mono相同，其中比较特殊是第三点。因为它为了减少速度求解的数量，并且降低问题的复杂性，即如果是N帧，则需要求N个速度，但这里是利用相邻两帧进行求解速度。这里速度的求解都是通过视觉位姿进行构建，由于速度与IMU有一个积分约束，所以可以分别求解得到加速度计的bias、尺度和重力。然后，对求解出来得初始化状态变量进行积分，得到就是不同帧之间的速度，最后进行对齐操作，即可完成ORB-SLAM3的初始化。

基于紧耦合的初始化方法（OpenVINS）

这里以OpenVINS为例来进行介绍，OpenVINS的估计流程如图4所示。OpenVINS的初始化认为陀螺的测量是比较准确，因此不同时刻的旋转，可以直接通过角速度的积分得到，然后不同时刻的相机的位置也可以通过IMU加速度计的二次积分得到。

因为在进行IMU位置积分的时候，积分的初始时刻速度和重力是未知，所以，可以构建相机的位姿来构建视觉的总约束关系，从而来求解初始速度、重力以及特征点的三维坐标。然后，通过一次积分以及二次积分分别得到多帧的速度以及多帧的位置关系。最后，求解世界坐标系跟初始的boby系之间的旋转矩阵关系，然后进行一个对齐操作。

图4 OpenVINS初始化

在构建视觉约束时，任选一个坐标系B0，设定为参考坐标系，那可以将特征点Fi从B0坐标系转到相机坐标系下，具体的转换过程是需要用到IMU的位姿。相机坐标系可以得到相机之间特征点的观测坐标，所以，根据观测到的规划平面上的测量值，可以来构建总方程，然后进行求解。

IMU的位置积分得到之后带入到观测方程，得到：

如果有多个特征点观测，如下公式，可以将多个特征点观测给累加起来，然后通过最小二乘的方式来进行联合求解速度。需要注意的是，由于重力的模长是一个定值，因此在求解这个方程的时候也是需要考虑该约束。

我们的方法：一种旋转平移解耦(DRT)的初始化方法

如果能够准确的估计出角速度的bias，然后去掉该bias并进行重新积分之后可以得到旋转，它的能够显著的提升精度，并且旋转精度的提升能够对后续整个速度和重力初始化带来不错的增益。DRT初始化方法如图5所示，首先是已知旋转外参数，然后提出了一种就是利用视觉约束来紧耦和估计陀螺仪bias的方法，并重新预积分得到旋转变量。之后分别提出了松耦合以及紧耦和来分别估计初始化的速度、重力，之后就可以分别进行积分，一次积分得到多帧速度，然后二次积分得到多帧的平移关系。

这个初始化方法其实并没有初始化三维特征点，因为解耦了特征点和初始化相关变量，这样做的好处的话，能够大大加快初始化的效率。最后的一步是进行三角化，把所有场景里面初始化能够观测到的特征点，求解它的三维坐标。

图5 DRT估计方法

首先，介绍一下紧耦合估计陀螺仪bias，这里采用两帧求旋转的思路，如图6所示。如果一个三维特征点P1，能够被Ci跟Cj两个相机观测到，那么这个特征点跟Ci和Cj构成了一个极平面，极平面的法向量可以通过Ci观测到的光线和Cj观测到光线进行叉乘得到。如果每一个特征点都能够计算出这样的法向量，可以看到所有特征点的法向量和平移T垂直，即所有特征点的法向量都是共面关系。

然后，将这些共面的法向量组成一个巨大矩阵N，用N乘以N的转置，用M矩阵的最小特征值来表示。也就是可以通过优化相机外参矩阵使得M矩阵最小特征值达到一个最优的结果，通过这种约束关系来求解相机外参矩阵。

图6 两帧求旋转方法

以上是纯视觉方法构建的约束关系，进一步考虑到相机和IMU之间的一个旋转关系，也就是通过IMU预积分再加上旋转外参，可以得到相机之间的旋转关系。影响预积分的精度只和角速度bias有关，因此，可以将旋转预积分进行线性化之后的方程带入到两帧约束求旋转的方程里边，得到：

在加入了IMU预积分之后，为了和之前求解的形式进行统一，所以，首先可以利用叉乘的性质，将上方公式整理为：

此外，由于旋转矩阵是一个正交矩阵，具有保角保膜长的性质，并不改变特征矩阵的特征值，所以，可以只计算上公式的中间部分。最后陀螺仪bias的估计下述公式求解得到。

在得到陀螺仪的bias之后，可以重新对角速度进行积分，然后可以得到更加准确的旋转矩阵。

这里首先介绍紧耦合初始化速度和重力，一般来说，拿到不同相机之间的R和T后进行三角化，进而求解特征点的三维坐标。因此，已知旋转向量下，三维点的位置和不同相机之间的位置紧密相关。因为特征点的位置是通过了平移进行表示，所以可以只优化连续三帧的相对平移关系来表达这样的视觉约束，这样的好处在于待求解的变量能够大大减少，从而提高计算的效率。

因此， VIO当中相机的相对位置可以通过IMU积分，再加上外参关系来得到，这样可以通过IMU预积分来构建视觉的约束关系。因此，把相机的平移关系带入到方程里进行求解：

最后，整理得到关于速度和重力的约束方程：

然后，也可以将LIGT的pose约束，利用VINS-Mono松耦合的方式，与IMU的预积分进来总对齐来进行初始化。采用前面所述的视觉约束关系来求解相机的pose，然后计算IMU的预积分并进行对齐，从而来求解尺度，速度和重力变量。

DRT中提出一种松耦合的方法，首先是估计陀螺仪的bias，所以旋转是更加准确，因此能够对后续的松耦合过程求解平移相关的状态变量。此外，因为视觉约束关系，不需要求解特征点的位置，所以能够大大加快松耦合的求解效率。

不管是松耦合还是紧耦合，需要在计算的时候考虑重力模长约束。由于重力模长约束为定值，在求解过程中加入重力约束，将其展开为：

然后，未知参数的重力约束可写为：

最后得到要求解的方程：

基于深度学习的初始化方法

在VIO初始化的过程中，如果设备处于一种时差很小或者加速度很小的过程，但在这种状态下面，还必须要进行初始化，那传统的初始化方法在这种具有挑战性场景下面无法完成，因为初始化的不确定性很大。

因此，提出了单目深度先验证的VIO初始化方法， 能够面对不同复杂场景都能顺利初始化，如图7所示。它首先利用深度学习的方式恢复单目这种没有尺度的场景深度，将其融入到整个VIO初始化系统框架中。添加尺度因子和漂移因子以后，可以恢复到米制单位的深度里，从而辅助整个VIO在一种具有挑战性的场景下面的初始化。

图7 单目深度先验的VIO初始化流程

它的整个流程和OpenVINS类似，首先进行角速度的积分来获得旋转，然后利用IMU的惯性约束、视觉约束、单目深度约束以及先验约束四种约束关系来进行初始化、速度、重力、尺度因子。视觉约束和惯性约束跟openVINS这块是比较类似。

单目深度约束是通过构建两个深度相等关系来进行构建约束方程。首先，通过尺度因子跟漂移因子来恢复米制单位下面的真实深度，然后，通过特征点估计的深度和恢复的深度的比率形式，来构建单目深度约束。

3

各方法的实验对比及结论

第一个是做了一些仿真实验，这实验纯粹是为了验证陀螺仪bias的有效性。仿真实验是让相机以20Hz进行圆周运动或是有一些起伏，IMU采样频率为200Hz，平均每帧的特征点观测为150个，并且视觉观测添加1pixel的高斯白噪声。

在Bg数据上，从0.02幅度每秒一直到0.18，间距为0.02，如表1。0.18幅度每秒大约是在半小时内产生接近10度的误差，最后依旧能够收敛到一个比较小的值。下面的重力方向也只有零点几度的偏差。

表1 模拟误差数据

其次是在真实数据上面的测试，如图8所示。VINS-Mono和AS-MLE方法两者比较接近，但基于非线性迭代的AS-MLE略优。DRT方法同在各个数据上面可以看到，精度表现基本上都是比他们要优秀，意味着DRT求的旋转更加准确。

另外，纯视觉SFM的rotation在不同的运动模式下也是比不过DRT方法，但是V103，V203数据集上松耦合方法失效，失去了这种统计的意义。整体的一个鲁棒性其实在这里也能够有一定的体现。

图8 真实数据集测试对比

精度实验测试数据如表2所示，可以发现VINS-Mono表明了一个好的旋转估计能够提升较大的系统精度，同时在DRT-1和DRT-t对比发现尺度和多速度变量的引入能够提高精度。同时消融实验证明了旋转估计的有效性，以及估计陀螺仪bias的重要性。

表2 精度实验数据对比

最后，在EuRoC数据集上10关键帧的平均初始化时间。分别统计了SfM, 陀螺仪bias估计，速度和重力估计以及特征点三角化模块的耗时，如图9所示。可以发现DRT-l 综合最优：高成功率，高效率，高精度。同时，旋转估计的精度非常重要，会影响整体的鲁棒性和成功率。

图9 时间对比