本文定义了3个坐标系：世界坐标系F_W、相机坐标系F_C以及IMU坐标系F_S. 其中F_W的原点设置为IMU的初始位置，z轴的方向与重力相同. 两图像帧之间的变换用齐次矩阵T表示，如T_WS代表IMU坐标系到世界坐标系的位姿转换，其中旋转部分记作R_WS，也可以用四元数表示为q_WS.

本文提出了一种基于视觉惯性融合的双目视觉里程计，该算法的主要框架如图 1所示. 前端的跟踪线程中，将IMU先验位姿作为初值，使用半直接法进行相邻图像帧之间的运动估计. 系统初始化模块主要用于对齐视觉和惯性信息，并构建高精度初始地图. 后端进行局部优化，结合视觉和惯性信息构建联合优化模型，在滑动窗口中对关键帧位姿以及地图点进行优化. 本文的主要贡献如下：

1) 提出了一种半直接法和IMU融合的双目视觉惯性里程计算法框架;

2) 使用逆光流法跟踪均匀化的FAST关键点，提高图像处理的计算效率;

3) 基于双目相机的特点，提出一种简化的双目视觉模型以减少图 1右半部图像中的多余测量值的计算;

4) 结合视觉和惯性测量信息，构建高精度的初始化地图，为之后的运动估计和位姿优化提供基础.

通常情况下，IMU的采样频率远高于相机，因此在两相邻图像帧之间存在多个IMU测量数据，文献[12]中给出了IMU预积分的详细推导过程. 参考预积分理论，通过对两相邻图像帧i和i+1之间的多个IMU测量值积分，可以得到Δt时间段内IMU的状态增量，包括位移p，速度v以及旋转矩阵对应的四元数q：

其中，R_SW表示从世界坐标系到IMU坐标系的变换矩阵，g^W是世界坐标系下的重力分量.

需要注意的是，传感器零偏b_g和b_a在两图像帧之间也在缓慢地发生变化. 为了保持较快的处理速度，在下个时刻对其求一阶泰勒展开得到一个近似偏差值，则传感器偏差改变时测量值的更新式为

其中，$ \partial \alpha_{i}$是q_i对应的欧拉角，$\left\{\frac{\partial p}{\partial b}, \frac{\partial v}{\partial b}, \frac{\partial \alpha}{\partial b}\right\} $是相关状态量的变化梯度，可由Δt之间的IMU测量值计算求出.

本文使用FAST角点快速检测算法对左右相机采集到的图像提取关键点，为了避免提取的关键点过于集中，除了非极大值抑制以外，还采用均匀化提取的策略. 首先，设置阈值Td对视觉帧图像粗提取一批角点，然后将该图像分割成固定大小的G个网格，并设置每个网格中的最大角点数量M，使用Harris角点评价准则计算每个网格中角点的评价得分，若某网格中的角点数量大于M，则选取评分高的前M个角点作为特征关键点，若网格内的角点数量极小，则忽略这片特征.

实验对比结果如图 2所示. 左图是传统FAST特征检测算法提取结果，右图是结合均匀化策略的特征提取结果. 通过对比可以看出，右图中的角点提取数量明显少于传统FAST特征检测算法，且角点分布较为均匀; 左图中门框、三脚架以及桌子上的角点提取过于密集，而在右图中得到了明显改善，地面和墙上的部分零星角点被舍弃. 表 1记录了两种角点检测的耗时情况，从表中数据来看，改进后算法的角点检测数量远低于传统FAST检测算法，虽然平均耗时有所增加，但总耗时与传统FAST算法差距不大，这种策略可以在很大程度上避免由于特征提取过于集中而造成的误匹配，还能减少特征提取的数量，提高特征匹配的计算效率.

与采用单一传感器的纯视觉里程计不同，本文算法结合了两种传感器的信息来实现对图像特征的跟踪. 针对两相邻图像帧，通过IMU预积分可以为特征跟踪提供一个良好的先验位姿，如下式：

其中，T_WS表示通过IMU预积分求得的相邻图像帧之间的变换矩阵，T_SC表示相机与IMU之间的外参矩阵.

在此基础上，使用逆光流法对相邻图像帧中的关键点进行跟踪，如图 3所示. 假设像素a是上一图像帧中的稳定角点，利用光流跟踪可以获得下一图像帧上对应的像素b，然后再反向追踪像素b在上一帧中的对应像素c，由于噪声的影响，a和c的位置不可能完全重合，通过比较两者之间的像素距离d与设定阈值的大小，判断此次跟踪的正确性. 然而在多数的场景中，载体相机采集的图像无法完全满足光流法的条件，因此会出现像素点跟踪错误甚至跟踪失败的情况，本文使用RANSAC(随机抽样一致)算法对光流跟踪结果进行过滤^[13]，以消除异常点.

系统初始化是视觉和惯性信息之间松耦合的过程. 首先需要对视觉部分进行初始化，得到三维空间中一系列的相机位姿，同时IMU持续进行预积分，获取图像帧之间的预积分量，之后把视觉初始化得到的相机位姿和预积分结果进行对齐，估计出初始化过程中图像帧速度、陀螺仪的零偏以及重力矢量等.

本文系统中，IMU与双目相机可以看作一个整体，所以相机从i到i+1帧图像的旋转变换应与IMU在对应时间内的旋转增量相同，忽略传感器的观测噪声将两者的结果对齐，通过最小化相对旋转误差得到陀螺仪零偏的初始值：

其中，N是图像帧数目，ΔR_i，i+1为相邻图像帧之间的陀螺仪积分，R_S^W为IMU坐标系到世界坐标系的旋转矩阵.

针对要求解重力矢量以及各图像帧时刻的速度，设目标状态量为$ \chi=\left[v_{1}, \cdots, v_{i} \cdots, v_{n}, g\right]$，其中v_i代表第i帧图像对应的速度，g代表重力矢量. 由预积分公式可得：

其中，v_i+1ⁱ和p_i+1ⁱ可以通过预积分求出，p_i和p_i+1可以由视觉初始化求出.

根据多帧图像间的上述约束关系联立方程组，通过SVD分解即可得到初始化过程中的速度和重力矢量的估计值. 此外，根据重力矢量相对世界坐标系z轴的旋转，可以将系统初始位姿与世界坐标系对齐，并以此位姿作为参考构建初始地图，为之后的运动估计提供精确的初值. 为了提高初始地图的精度，采用以下3个步骤构造和维护地图点：

1) 构造地图点

地图点的位置可以用关键点的归一化坐标z和深度d表示为向量d·z，其计算过程如下所示：

整理后可得：

其中，z₁和z₂表示两个相匹配关键点的归一化坐标，R₁₂和t₁₂表示两个图像帧之间的旋转矩阵和平移向量.

此外，为了验证初步构建地图点的有效性，本文使用卡方检验标准来评估其位置向量. 位置向量的偏差定义为：e=dz－d₁z₁，测量的距离定义为：D=dz－t₁₂/2，地图点的位置偏差应限制在测量距离的范围之内，其卡方标准可以定义如下：

其中，σ是对应关键点的标准方差. 如果e_chi2大于预定阈值，则表示估计偏差很大或该地标点非常接近相机，这两种情况下都会导致初始地图一定程度的失真.

2) 深度更新

本文使用多对具有丰富特征的连续图像帧来构造初始地图，因此在初始化的过程中可以得到同一地图点的多个深度值，每个地图点的深度应不断进行更新，使得估计结果的不确定性逐渐变小. 根据文献[14]中的理论，本文使用高斯+均匀混合分布建模，当出现新的深度估计时，使用递归贝叶斯方法更新此混合分布中的参数^[15]，使得期望值更接近真实深度，如公式(9)所示：

3) BA优化

在创建足够多的地图点之后，先对初始地图中的所有图像帧进行一次分类，若某图像帧中的可视地图点数量达到设定阈值，便将其标记为有效图像帧，反之，则标记为无效图像帧. 然后，联合初始地图中所有有效图像帧的位姿及其可见地图点的三维坐标构建重投影误差项，其目标函数可以定义为

其中，e_x=u_ij－h(T_i，z_j)表示相机在位姿T_i处对地图点l的观测误差，h是相机投影模型，Ω是相关地图点的信息矩阵.

通过最小化此目标函数可以获得优化的图像帧位姿以及地图点坐标，由于先对参与优化的图像帧进行了一次筛选，提高了优化的效率和精度，为后续运动估计提供了良好的初值.

根据相机投影模型可以将任何可见的3D地标点投影到像素坐标系中. 左侧相机的重投影误差可表示为

其中，u_l和v_l是左图像帧中的观测坐标，f，c_x和c_y是相机内参.

可以看到，如果按照传统的方式构建左右相机的重投影误差，会产生一个四维的误差向量，这无疑增加了计算的复杂度. 本文使用一种简化的视觉模型来解决这个问题：假定双目相机已经严格校正，则左右两幅图像中的投影点的纵坐标是相同的，某一时刻下的左右图像匹配后，可获取右图像中对应特征点横坐标的观测值^[16]，根据双目相机投影模型的特点，可以计算得到右图像中地标点投影位置的横坐标，然后将其与观测值相比较，便得到右侧相机的重投影误差：

其中，u_r是右图像中的横坐标观测值，b是双目相机的基线. 此时，可将双目重投影误差用一个三维向量表示：

IMU的测量误差主要分为两部分，包括预积分项的R，v，p的误差以及b_g和b_a的误差. 根据预积分模型，将各误差分量表示为

因此，IMU误差项可表示为

基于上述的视觉重投影误差与IMU传播误差，构建联合优化的模型，以求解更准确的相机位姿和地图点三维信息，构建视觉惯性联合优化的目标函数：

其中，M是当前窗口中的关键帧集合，L(i)是两个相机在第i帧中同时观察到的地图点集合，Ω_C与Ω_S分别代表视觉信息矩阵和惯性信息矩阵， $ e_{C}^{T} \mathit{\Omega}_{C} e_{C}$和 $e_{S}^{T} \mathit{\Omega}_{S} e_{S} $表示窗口中对应时刻的视觉误差和IMU误差的二次形式.

在不改变一致性的前提下，对滑动窗口内最旧的关键帧状态边缘化处理，可以避免丢失该关键帧包含的关联信息，有效地保留历史信息对窗口内的其他状态的影响，该过程的本质是一个最小二乘问题，本文用高斯牛顿迭代法求解，定义如下：

在边缘化过程中，不直接删除被边缘化的状态量以及与其相关的地标点，否则会减少优化过程中的约束. 假定δx_a是要被边缘化的状态量，δx_b是需要被保留的约束，根据舒尔补公式对优化量进行边缘化操作，该过程可简化为

整理得：

其中，$H_{b b}-H_{b a} H_{a a}^{-1} H_{a b} $和$ b_{b}-H_{b a} H_{a a}^{-1} b_{a} $表示被边缘化的H矩阵，状态量δx_a被边缘化.

本文通过以下实验来评估所提出双目视觉惯性里程计的性能. 首先，使用EuRoC MAV数据集对算法性能进行评估，然后将所提出算法与ORB-SLAM2以及VINS-Fusion算法进行比较分析. 实验所用笔记本配置为六核i5-8400处理器，满载频率为3.8 GHz.

EuRoC MAV数据集由安装在微型飞行器上的各种传感器收集而得到，该数据集分为3个系列的飞行场景，包含一系列由简单到复杂的运动序列. 选取EuRoC MAV数据集中较有代表性的MH_01，MH_04，V2_01以及V2_03运动序列作为实验场景，本文算法的运行结果如图 4-图 7所示：

图中显示了本文算法在所选数据集中的运动轨迹和定位误差. 其中，MH_01序列中的定位误差保持在0.361 m内，V2_01序列中的定位误差保持在0.262 m内，这表明本文算法在运动相对平稳，光照变化较小的场景中具有较高的定位精度. 在无人机运动较快，光照条件较差的MH_04序列与V2_03序列中，本文算法的定位误差分别保持在0.699 m和0.527 m内，且保持在一个相对平稳的状态，表明本文提出双目惯性里程计算法在恶劣场景下的鲁棒性较好.

为了进一步验证算法性能，选择ORB-SLAM2和VINS-Fusion算法同本文算法作比较，对应的均方根误差(RMSE)如表 2所示，需要注意的是，实验时关闭了VINS-Fusion算法中的回环检测功能. 从表中数据对比可以看出，ORB-SLAM2在M系列以及V1_01，V1_02和V2_01数据集中的RMSE值是最小的，这是基于特征法的纯视觉里程计的优势：适用于光照条件较好的大场景运动估计; 而在光照条件差、运动较快的场景中，另外两种视觉惯性里程计的算法表现更好，这也是两种传感器互补的优势. 与VINS-Fusion算法对比，本文算法在MH_03和V1_02两个数据集中的表现更好，其运动轨迹对比如图 8-图 9所示，其中，虚线代表该运动序列的真实参考轨迹，橘色的线代表VINS-Fusion算法在该序列下的运动轨迹，而蓝色的线代表本文算法在该序列下的运动轨迹，对比两者的运动轨迹可以看出，本文算法在两个运动序列中定位精度的表现都优于VINS-Fusion算法，尤其是在V1_02这种较为复杂的场景中. 在V1_03，V2_03的数据集中，本文算法获得的RMSE值最小，这是由于本文算法提供了更精确的先验信息并改进了光流匹配过程，为之后的位姿优化过程提供了可靠的基础，而且以强约束关键帧作为滑动窗口的优化项，进一步提高了位姿估计的精度.

为了验证算法的实时性，表 3记录了本文算法和VINS-Fusion算法在数据集上的CPU负载和运行帧率. EuRoC MAV数据集中所使用相机的帧率为20 Hz，所以若某算法的运行帧率低于20 Hz，整个系统的数据处理会出现明显的延迟. 从表中数据对比可以看出，相比于VINS-Fusion，本文的算法降低了约8%CPU负载，且运行帧率更高.

本文提出了一种基于半直接法的双目视觉惯性里程计算法，在系统初始化阶段，结合惯性测量数据和双目相机的图像信息构建高精度的初始化地图，为后端的位姿优化提供良好的初值; 使用简化的双目视觉模型构建重投影误差，减少了对右相机图像中多余的测量值的计算; 在滑动窗口边优化的过程中选择性地剔除部分图像帧信息，确保在优化过程中拥有足够参考信息的同时尽可能地减少优化计算量; 与ORB-SLAM2和VINS-Fusion算法的实验对比表明，本文算法在定位精度上已达到主流的视觉里程计的水平，并且在计算效率方面有一定程度的提高.