前言

Estimator Initialization in Vision-aided Inertial Navigation with Unknown Camera-IMU Calibration这篇论文是2012年IROS上的一篇文章，针对相机和IMU的初始估计问题，提出了在没有任何先验知识的情况下，直接从传感器测量值计算系统可观测量(平台姿态和速度,特征位置,以及IMU相机标定)的算法。在OpenVINS中，如果平台初始时不是静止的，则利用动态初始化来尝试恢复初始状态，就是利用的这篇论文中的方法，该方法已经在OpenVINS中实现并开源。

一、问题背景

当没有系统初始状态的先验知识时，(例如,在系统运行的开始阶段)，现有利用惯性测量和自然发生的点特征的观测进行运动估计的方法是不适用的。

使用相机和IMU测量的导航的绝大多数现有技术采用递归贝叶斯估计方法[ 1 ] - [ 4 ]或平滑公式[ 5 ]。在这两种情况下，准确的初始猜测(先验估计)对于状态的可靠估计是必要的。这是由于这两类方法都依赖于测量模型的线性化，因而在缺乏精确初始估计的情况下，较大的线性化误差会导致发散。在当前的实践中，为了初始化上述讨论的任何状态估计方法，通常会根据具体情况使用特定领域的知识。例如，在某些应用中，可能会有额外的传感器(例如,测斜仪和/或GPS)可用，或者已知平台最初处于静止状态。然而，这类方法并不具有普遍适用性。

为了解决这个局限性，在最近的工作[ 8 ] - [ 10 ]中提出了仅基于自然特征的观测来初始化系统状态的算法。这些方法提供了运动平台的姿态(横摇和纵摇)，速度，相机观测特征的位置的初始估计，以及在[ 9 ]的情况下，加速度计的偏差。但是现有方法为了计算这些估计值，假设相机到IMU的变换(旋转和平移)是先验已知的。最简单的方法是使导航算法能够估计所有必要的参数，包括相机到IMU的转换，而不需要先验的初始猜测。这就是本文所要解决的问题。

二、主要假设

视觉辅助惯性导航系统的可观测性已经在[ 1 ]，[ 2 ]中进行了研究。这些工作表明，在缺少全局坐标已知的参考点的情况下，IMU的全局位置以及绕重力轴(即,偏航)的转动是不可观测的。另一方面，下列量是一般可观测的：

( O1 ) IMU相对于水平面(即,横滚和俯仰)的姿态，

( O2 ) IMU相对于初始IMU框架的轨迹(位置、速度和方向)，

( O3 )相对于初始IMU框架的特征位置。

( O4 ) IMU和相机帧之间的转换(即,相机到IMU的标定)，

( O5 ) IMU陀螺仪和加速度计的偏差。

正如第一节所讨论的，我们在本文中的重点是估计上面的量( O1 ) - ( O4 )。我们假设对IMU偏差的估计已经可用(例如,这些偏差最初可以假设为接近于零,然后在第六节描述的MLE精化中计算它们的高精度估计)。文献[ 1 ]，[ 2 ]的可观测性结果证明了(限制奇异轨迹,如恒速运动)我们能够估计感兴趣的量，但没有说明如何从传感器数据中直接计算这些量，并且不需要事先估计。

本文提出的方法不需要先验知识，仅利用自然特征的观测值，直接从传感器测量值估计相机到IMU的转换。

三、理论流程

1、问题建模

IMU测量值：

将忽略测量中的噪声，并建立一个线性方程组，该方程组将允许我们估计除了相机和IMU之间的旋转矩阵以外的所有量。
IMU测量的全局位置：

一个标定好内参的相机，在时刻对第j个特征的观测由透视相机模型描述：

为第j时刻特征相对于第i时刻相机坐标系的位置。

利用坐标系转换可以得到：

未知量为向量和相机IMU之间的变换，方程为

方程Aijx = bij是由第i幅图像中第j个特征的观测值导出的。通过收集所有特征和IMU测量得到的方程，我们得到了线性系统

现在考察( 15 )式中线性系统的性质。首先，我们注意到向量x包含了恢复可观测项( O1 ) - ( O4 )所必需的所有未知量，除了相机到IMU的旋转。具体来说，知道局部框架内的重力方向就相当于知道IMU相对于水平面的姿态(量( O1 ) )。此外，如果B0v0和B0g是已知的，那么我们可以使用( 6 )估计局部帧(数量( O2 ) )中的IMU轨迹。特征位置( O3 )和相机到IMU的转换( (部分( O4) )显式地包含在x中。那么，求解这个线性系统就可以确定我们所求的所有参数，除了相机到IMU的旋转。从( 12 ) - ( 14 )可以看出，矩阵A和向量b可以使用特征测量，IMU测量和旋转矩阵（相机到IMU的旋转）来计算。因此，我们看到，如果我们能够确定相机到IMU的旋转，我们可以很容易地恢复所有剩余的量。

一个重要的考虑是确定所需的图像和特征的数量，以便能够估计系统中的所有未知参数。我们考察的第一个问题是图像的最小数量。在[ 19 ]中，我们证明了如果象的个数N≤3，则线性系统( 15 )中的矩阵A至少是3秩亏的.也就是说，即使已知CBC，我们仍然无法唯一确定系统中的未知参数。因此，所需的最少图像数量为N = 4。

为了确定所需特征的数量，我们采用了一个计数参数：对于每幅图像中的每个特征测量，我们从Aijx = bij中获得2个标量方程。因此，对于N幅图像和M个特征，测量约束个数为2N M。另一方面，可观测的未知量个数为3M + 11 ( 3M为特征位置, 6为相机到IMU的转换, 3为初始速度, 2为横滚和俯仰)。为了能够唯一地确定所有的未知数，我们必须有2M N≥3M + 11，从中我们可以看出，所需的最小特征数是M = 3，如果有4张图像，M = 2，如果有5张图像，M = 1，如果有7张或更多的图像。在所有这些最小的情况下，测量值的数量都高于未知量的数量，因此问题是过约束的，并且可以计算出唯一的解。

2、相机-IMU变换的估计

在这一部分，我们提出了两种计算相机到IMU旋转矩阵的方法。当观察到至少M = 5个特征时，第一种方法是适用的。在这种情况下，我们可以使用基于图像的运动估计算法(如[ 20 ] (如果观察到M = 4的特征,同样可以完成,但所涉及的算法要复杂得多) )来估计不同图像之间的相对相机方位，Ci Cj C。当M < 4时，单独使用特征测量无法估计相机旋转。对于这种情况，我们在第IV - B节中描述了一种只需要M≥2个特征就可以恢复相机到IMU旋转矩阵的方法。

（1）当M≥5时的解

相机和IMU各自估计自己的相对变换，通过下式关联：

转换为等价四元数：

式中：( 18 )是一个形如的线性系统，其中未知量是描述相机到IMU旋转的四元数。通过使用所有可用的图像对，我们可以构造一个过约束的线性系统：

最小二乘解是B的最小奇异值对应的右单位奇异向量，由此可直接恢复旋转矩阵 [ 24 ]。为了得到唯一的解，至少需要两幅图像，其中系统绕不同的轴旋转[ 23 ]。因此，我们看到，该方法至少需要3幅图像，其中至少有5个特征被跟踪，以便能够确定相机到IMU的旋转。

（2）当M≥2时的解

我们现在提出了另一种方法，它可以操作任意数量的点M≥2，也就是说，它可以恢复矩阵CBC，即使单独的特征测量不能用于确定图像之间的相对相机旋转。

根据叉积的性质，对于相机在i时刻观察到的任意两个特征有

式中，为利用特征测量值计算的已知向量，为利用IMU测量值计算的已知矩阵。矩阵和向量是未知的。

我们可以收集所有未知数到向量y中：

现在看到( 25 )式在y的元素上是一元二次方程。从每幅图像中我们可以提取一个这样的方程，从而从N幅图像中我们可以得到N个关于y元素的二次方程。另外，旋转矩阵必须满足正交性约束，用c中的6个二次方程表示。最后，由于( 25 )式在Δ p上是齐次的，因此必须对该向量施加范数约束：

因此，我们看到，总共用N幅图像，我们得到了关于y的N + 7个二次约束。由于y是一个12 × 1的向量，当至少有5幅图像可用时，我们有多个方程等于未知数的个数。

到目前为止，只考虑了两个特征。为了将我们的提法推广到观察到M≥2特征的情形，我们注意到( 25 )可以改写为：

类似地，我们根据下面的未知量向量得到一个二次方程组：

向量y包含n = 3M + 6个未知数。

A、利用凸迭代求解方程组

以上推导的方程构成了多元二次方程组。在代数意义下求解这样的系统是NP -完全的[ 25 ]，因此在这里我们将采用基于迭代凸近似的求解方法因此，可以通过求解可行性问题找到y：

上述可行性问题可以通过将秩约束松弛为一个不等式来精确地重新表述：

由于秩为零的唯一矩阵为零矩阵(这不是一个解决办法)，因此上述问题将有与式( 31 )相同的解。为了得到上述问题的解，我们将使用[ 26 ]的结果.具体地，文献[ 26 ]表明，通过迭代求解以下两个凸优化问题可以找到( 32 )的解：

其过程如下：对所谓的方向矩阵W (在我们的实现中设置为零)选取一个初始值，求解问题( 33 )得到Y ?。然后我们利用这个值在问题( 34 )中找到一个新的值W，并且这个过程重复到收敛.文献[ 26 ]表明，当该迭代收敛时，得到的解就是原问题( 32 ) (从而,对我们原创性的寻找y的问题)的精确解。我们注意到问题( 33 )和( 34 )都是半定规划( SDPs )，并且可以用现有的算法有效地解决。

B、解决噪声的存在

式中：( 25 )只有在测量是完美的情况下才成立。当存在噪声时，有

为了能够使用上面提出的凸迭代公式来解决这个问题，我们可以计算出一个误差的上界，使得：

因此，我们现在可以把估计未知向量y的问题写成可行性问题：

其中不等式约束来自于测量，而等式约束是由于对y的元素的正交性和单位范数约束。值得注意的是，如( 31 )中一样，这些约束定义了Y的一个凸集，因此[ 26 ]的方法仍然可以应用.因此，为了找到问题( 37 )的解，我们使用类似于( 33 )和( 34 )的凸迭代，唯一的不同是，( 33 )中的等式约束现在被上面所示的等式和不等式约束的组合所取代。

估计向量y (因此旋转矩阵CBC ,是我们感兴趣的)的精度取决于边界eibj的选择。如果选择过于宽松的界限，那么得到的解将是不准确的。另一方面，如果选择太小的界，问题将变得不可行。为了解决这个问题，在我们的实现中，我们使用传感器噪声的已知统计量来计算边界的初始低估计。我们开始使用这些值进行凸迭代，如果问题是不可行的(导致迭代停止)，我们逐渐增加边界，直到收敛成功。

3、线性系统的求解

添加重力范数已知的约束，有

4、最大似然精细化估计

针对这一点提出的直接解决方案提供了无需任何先验初始猜测的结果的优势。然而，它们在统计上并不是最优的，因为噪声的存在并没有被正确地建模。为了适当地考虑测量中的噪声，我们建立了一个用于估计参数向量θ的MLE，其中包括( i )在每一时刻记录图像的IMU状态(位置、速度、方向)，( ii )所有特征的位置，pj，j = 1 .. M，( III ) Camera - To - Imu变换。量( i ) - ( ii )表示关于一个框架，其原点与初始IMU框架的原点重合，而其z轴与重力方向对齐。

按照标准的做法，我们将图像测量噪声向量nij建模为一个高斯零均值随机变量，其协方差矩阵为Rij。具体来说，我们使用时间间隔[ ti , ti + 1]内的IMU测量值来计算IMU状态的变化：

其中wi是一个噪声向量，建模为零均值的高斯随机变量，其协方差矩阵为Qi。函数f和协方差矩阵Qi是用连续时间运动方程的数值积分来计算的[ 18 ]。

最大化测量值的似然函数等价于最大化对数似然函数，而对数似然函数又等价于最小化代价函数：

成本函数c ( θ )是非线性的，它的最小化是通过应用Levenberg - Marquardt方法[ 27 ]迭代进行的。在我们的测试中，我们观察到，如果使用第四节和第五节中描述的直接解来为迭代提供一个初始猜测，则收敛是迅速的，并且只需要几个(通常小于10)次迭代。我们还注意到，即使在直接解的推导中假设IMU的偏差是已知的，如果需要的话，这些偏差可以作为未知数包含在MLE中，并与所有其他参数一起估计。

四、实验方案

蒙特卡罗模拟结果

接下来，我们研究了改变图像数量N和特征数量M对估计精度的影响。对

五、创新总结

本文的主要贡献是提供初始估计的方法。正如以前的工作[ 8 ]，[ 9 ]所示，摄像机的速度和姿态以及特征点的位置可以通过求解一个适当的线性系统来解析地计算。在这里，我们证明了相机到IMU的平移也可以通过类似的线性系统来计算，而对于相机到IMU的旋转，我们提出了两种不同的方法：第一种方法需要在图像中观察到五个或更多的特征，并计算表示相机到IMU旋转的单位四元数作为线性最小二乘问题的解。第二种方法只需要两个(或更多)特征，并使用一系列凸问题来获得解决方案。

上述得到的初始估计值具有直接利用传感器数据计算的优点，但并不是统计意义上的最优。因此，随后使用最大似然估计器( MLE )对其进行改进，其公式导致非线性最小二乘问题，并使用Levenberg-Marquaurdt算法求解。除了提供未知参数的统计最优估计外，MLE还为我们提供了估计误差的协方差矩阵。这使得在不同设定下检验估计的准确性成为可能，并得出人们可以预期的关于估计质量的实际有用的结论。

本文提出了在视觉辅助惯性导航应用中，在没有关于系统初始状态的任何先验知识的情况下，初始化估计器的方法。我们直接使用相机和惯性测量来计算系统的可观测量，即平台姿态和速度，特征位置，以及IMU -相机标定。这项工作的一个关键贡献是基于凸优化的算法，用于计算相机和IMU之间的旋转矩阵，即使在少量特征( M≥2)的情况下也是可行的。在计算出旋转矩阵之后，其他所有可观测量都可以通过求解一个二次约束最小二乘问题来确定。最后，将这些估计值作为迭代极大似然估计量的起点，以获得更准确的、统计上最优的估计值。通过蒙特卡洛试验，证明了所提算法适用于需要在没有任何先验信息的情况下进行状态估计的应用场合。