VINS-MONO翻译以及理论推导系列(二)：V估计器初始化

V 估计器初始化

紧耦合单目VIO是一个高度非线性的系统.由于单目相机无法直接观测到尺度，因此，如果没有良好的初始值，很难直接将这两种测量结果融合在一起。人们可以假设一个静止的初始条件来启动单目vins估计器。然而，这种假设是不合适的，因为在实际应用中经常会遇到运动下的初始化.当IMU测量结果被大偏置破坏时，情况就变得更加复杂了。事实上，初始化通常是单目vins最脆弱的步骤。需要一个鲁棒的初始化过程来确保系统的适用性。

我们采用松耦合的传感器融合方法得到初始值.我们发现，纯视觉的SLAM，或SfM，具有良好的初始化性质。在大多数情况下，视觉系统可以通过从相对运动方法(如八点[32]或五点算法[33]或估计齐次矩阵)中导出初始值来引导自己。通过将度量IMU预积分与纯视觉SfM结果相匹配，我们可以粗略地恢复尺度、重力、速度，甚至偏置。这足以引导非线性单目vins估计器，如图4所示。

与在初始阶段同时估计陀螺仪和加速度计偏置的[17]相比，我们在初始阶段选择忽略加速度计偏置项。加速度计偏置与重力耦合，由于重力向量相对于平台动力学的大量级，以及初始阶段相对较短，这些偏置项很难被观测到。我们以前的工作对加速度计偏置标定进行了详细的分析。

A.纯视觉SFM滑窗

初始化过程开始于一个纯视觉的SfM来估计一个高比例相机的姿势和特征位置的图。

我们保持了一个帧的滑动窗口来限制计算复杂度。首先，我们检查了最新帧与之前所有帧之间的特征对应。如果我们能找到稳定的特征跟踪(超过30个跟踪特征)和足够的视差(20个以上的旋转补偿像素)在滑动窗口中的最新帧和任何其他帧之间，我们使用五点法[33]恢复相对旋转和这两个帧之间的尺度平移。否则，我们会将最新的帧保存在窗口中，并等待新的帧。如果五点法成功的话，我们可以任意设置尺度，并对这两个帧中观察到的所有特征进行三角化。基于这些三角化特征，采用(Pnp)[35]方法来估计窗口中所有其他帧的姿态。最后，应用全局光束平差[36]将所有特征观测的总重投影误差降到最小。由于我们还没有任何世界坐标系的信息，我们设置了第一个相机帧 $\left (\cdot \right )^{c_0}$ 作为参考帧的SfM。所有帧的姿态 $\left(\overline{p}_{c_k}^{c_0},q_{c_k}^{c_0} \right )$ 和特征位置表示相对于 $\left(\cdot \right )^{c_0}$ 。假设摄像机和IMU之间有一个粗糙测度的外部参数 $\left(p_c^b,q_c^b \right )$ ，我们可以将姿态从C帧转换到B帧(IMU)。

其中s是匹配视觉结构与米制尺度的尺度参数，解决这个缩放参数是实现成功初始化的关键。推导过程：不过有一个疑问就是 $R_{b_k}^{c_0}$ 和 $q_c^b$ 的关系是什么是逆的关系吗？

在这里插入图片描述

B. 视觉惯性对齐

1)陀螺仪偏置标定：考虑窗口连续两帧 $b_k$ 和 $b_{k+1}$ ，我们从视觉sfM中得到旋转 $q_{b_k}^{c_0}$ 和 $q_{b_{k+1}}^{c_0}$ ，以及通过IMU预积分得到的相对约束 $\hat{\gamma}_{b_{k+1}}^{b_k}$ 。我们将IMU预积分项线性化，使陀螺仪偏置最小，并将下列代价函数降到最小：

其中 $\mathcal{B}$ 代表窗口中的所有帧。利用第四部分导出的偏置雅可比，给出了 ${\gamma}_{b_{k+1}}^{b_k}$ 对陀螺仪偏置的一阶近似。这样，我们得到了陀螺仪偏置 $b_{\omega}$ 的初始校准。然后我们用新的陀螺仪偏置重新传播所有的imu预积分项。

其中式（15）中陀螺仪的误差有两部分组成分别为：测量噪声和陀螺仪偏置，而视觉的误差就只有观测噪声，忽略噪声的话（因为很小）两者差值的绝对值是陀螺仪偏置，将整个滑动窗口的所有的旋转做差构成了一个最小化误差模型。由于四元数最小的值为 $\left[ \begin{array}{c} 1\\ 0\\ \end{array} \right]$ ,因此有：

在这里插入图片描述

只考虑虚步的话，得到上式子。其中： ${q_{b_{k+1}}^{c_0}}^{-1}q_{b_k}^{c_0}$ 表示相机从 $b_k$ 到 $b_{k+1}$ 的相对旋转， ${\gamma}_{b_{k+1}}^{b_k}$ 表示陀螺仪从 $b_{k+1}$ 到 $b_k$ 的相对旋转。然后取最小二乘，当然也可以使用SVD分解等方法求解，得到了陀螺仪偏置 $b_{\omega}$ 的初始校准。

2)速度、重力向量和米制尺度初始化：在陀螺仪偏置初始化后，我们继续初始化导航的其他基本状态，即速度、重力向量和度量尺度：

其中，当取第k帧图像时， $v_{b_k}^{b_k}$ 是B帧中的速度， $g^{c_0}$ 是 $c_0$ 帧中的重力向量，s将单目SfM缩放为度量单位。

考虑窗口中两个连续的帧 $b_k$ 和 $b_{k+1}$ ，那么(5)可以写成：

推导过程：

我们可以将(14)和(17)合并成以下线性测量模型：

可以看出， $R_{b_k}^{c_0},R_{b_{k+1}}^{c_0},\overline{p}_{c_k}^{c_0},\overline{p}_{c_{k+1}}^{c_0}$ 是从上尺度单目视觉 $\Delta t_k$ 中得到的， $\Delta t_k$ 是两个连续帧之间的时间间隔。通过求解这个线性最小二乘问题：

我们可以得到窗口中每一帧的物体帧速度，视觉参照系 $\left ( \cdot \right )^{c_o}$ 中的重力向量，以及尺度参数。

3)重力细化：通过约束量级，可以对原线性初始化步骤得到的重力向量进行细化。在大多数情况下，重力向量的大小是已知的。这导致重力向量只剩2自由度。因此，我们在其切线空间上用两个变量重新参数化重力。参数化将重力向量表示为 $g.{\overline {\hat {\mathrm g}}}+\omega_1b_1+\omega_2b_2$ ，其中 $g$ 是已知的重力大小， $\hat {\overline{\mathrm g}}$ 是表示重力方向的单位向量， $b_1$ 和 $b_2$ 是跨越切平面的两个正交基，如图5所示， $\omega_1$ 和 $\omega_2$ 分别是 $b_1$ 和 $b_2$ 的对应位移。通过算法1的交叉乘积运算，可以找到一组 $b_1$ , $b_2$ 。然后用 $g.{\overline {\hat {\mathrm g}}}+\omega_1b_1+\omega_2b_2$ 代替(17)中的g，并与其它状态变量一起求解 $\omega_1$ 和 $\omega_2$ 。此过程迭代到 $\hat {\mathrm g}$ 收敛为止。

4)完成初始化：经过对重力向量的细化，通过将重力旋转到z轴上，得到世界帧与摄像机帧 $c_o$ 之间的旋转 $q_{c_o}^\omega$ 。然后我们将所有变量从参考框架 $\left (\cdot \right )^{c_0}$ 旋转到世界框架 $\left (\cdot \right )^\omega$ 。本体坐标系下的B帧的速度也将被旋转到世界框架。视觉SfM的转换组件将被缩放为公制单位。此时，初始化过程已经完成，所有这些度量值都将被输入到一个紧耦合的单目VIO中。

参考：

1.https://blog.csdn.nt/weixin_42905141/article/details/102782800

2.https://blog.csdn.net/pancheng1/article/details/81008081

VINS-MONO翻译以及理论推导系列(二)：V估计器初始化

V 估计器初始化

猜你喜欢