第七期：VINS系统构建

本节课的内容主要分三个部分，第一对VIO相关知识回顾，第二是整个系统要初始化的话，我们需要哪些参数，比如陀螺仪的bias，重力向量，速度和尺度等等，最后会讲解一下VINS系统的流程和给出整个VINS系统的代码示例。

VIO相关知识回顾

首先IMU传感器测量的是物体的角速度和加速度，加速度的模型和世界坐标系（东北天）下的重力加速度$g$有关系，那么将测量到的加速度和重力加速度进行向量相加，再旋转到body坐标系得到测量值，加上bias和高斯白噪声等误差就得到了实际的角速度和加速度的传感器模型。

接下来学习了IMU预积分，预积分就是将一段时间的数据进行积分。为什么要进行预积分，因为IMU和摄像头的频率相差大，IMU会在同一时间段内产生成倍的数据，增加了数据处理的复杂程度，所以就考虑将一片连续帧的IMU数据进行积分得到两个时刻$i,j$之间的位移、速度和旋转角度等数据的测量约束。这里面涉及了很多细节比如协方差矩阵的传递等。预积分的作用是能约束相邻两个或多个时刻图像之间的相对运动的变化约束。

接下来就是视觉技术的知识，具体可以看视觉前端笔记。

因为IMU和图像分别都能得到旋转平移量，那么两个传感器分别能得到两套轨迹，那这两套轨迹如何得到最优融合是个问题。

VINS鲁棒初始化

这里b和c数据分表代表IMU和相机得到的数据，则$b_2$坐标系如何表示到$c_0$坐标系去，有两条路可以走：利用IMU和相机的外参得到$b_2$和$c_2$之间的变换关系，将$b_2$变到$c_2$，再将$c_2$变到$c_0$去。这里要注意旋转和平移的转换不完全等价，平移会存在尺度的问题，bc之间因为有标定，尺度是确定的，其他的是有尺度因子；或者第二条路先得到$b_2$到$b_0$的变换，再加上b到c的变换。

因为平移$p_{bc}$的实际距离也不会很远，并且它可以手动测量出来，它的重要性远没有旋转外参数$q_{bc}$来的高。第一步先估计旋转外参数$q_{bc}$，第二步利用旋转约束估计陀螺仪的bias，接下来第三步再估计重力方向，速度以及相机运动尺度的初始值，第四步做refine优化的操作，最后通过求解世界坐标系和初始相机body坐标系间的旋转矩阵，将轨迹对齐到世界坐标系。

第一步：利用旋转约束估计外参数旋转$q_{bc}$

注意这里讲解人手绘图示和（5）式是相反的。

$q_{bc}$的求解推导

第二步：基于旋转约束的陀螺仪bias

陀螺仪bias的求解思路。到这里我们能做到将IMU和相机的数据对齐，接下来开始初始化第一帧的速度、重力向量在相机坐标系下的表示以及尺度因子。

第三步：初始化速度、重力和尺度因子

初始化参数的最小二乘求解思路。不过这一步中会存在一些问题，估计重力向量的时候，是假设的它为三维向量，然而在某一个地区运动的时候，它的模长其实是固定的，也就是重力加速度的值是固定不动的，所以应该只有两个自由度。

第四步：优化重力向量$g^{c_0}$

将公式(19)表示的$g^{c_0}$带入公式(16)，改变待优化的变量。其中待优化的变量会增加$w1$和$w2$。

第五步：将相机坐标系对齐世界坐标系

到此就完成了VINS系统的初始化过程。

VINS初始化拓展

论文《Robust initialization of monocular visual-inertial estimation on aerial robots》中解释到了为什么加速度bias没有被估计，因为相比于陀螺仪的bias，加速度的bias太小了，加速度的bias对初始化的影响很小，所以一般不估计加速度的bias。第二个平移外参数$p_{bc}$，相比于旋转参数来说对系统的影响不大，不需要单独估计初始化，就用测量得到的物理值就行，在后续优化过程再对其进行优化。

VINS系统

IMU残差项主要是预积分估计出来的值和两状态之间IMU算出来的值的误差，视觉残差项加上鲁棒核函数是因为视觉跟踪的时候往往会跟踪错误，跟踪到了错误的帧，需要对这些outlier进行抑制所以要引入鲁棒核函数。