基于双目视觉与IMU的组合导航算法

谭 静，赵健康，崔 超

(上海交通大学 电子信息与电气工程学院，上海 200240)

0 引 言

精确的导航数据是保证机器人稳定运行的核心。现有惯性导航技术的累积误差限制了该方法的精度和应用。INS/GPS组合导航技术[1,2]在室内等GPS失效场景中又无法提供可靠的导航信息。区域无线导航技术[3,4]又受限于繁琐的基站布置工作无法用于大型场景。视觉惯性里程计通过融合视觉信息与IMU数据，可以在GPS失效时提供精确的导航数据，主动的信息摄入方式又避免了繁琐的基站布置。但现有方法计算复杂，对主控设备性能要求高。基于此，本文提出一种基于误差状态粒子滤波器(ESPF)的双目相机与IMU融合的组合导航方法。将双目视觉和IMU解耦为两个独立的模块，针对传感器观测周期不一致问题，采用欧拉积分对无观测时的系统状态进行航迹推算。通过把状态量映射到李代数空间，将无法直接获得四元数随机粒子分布的过程转化为向量空间的随机粒子求解问题，以融合相机解算的位姿信息与IMU测量数据实现小型无人平台运动信息的实时估计。为了在保证更好的跟踪载体运动状态的同时，减小航迹推算阶段的累积误差，提高状态信息的输出频率，在滤波算法中实时估计了IMU的漂移。方法中将相机与IMU解耦为独立模块的算法结构降低了计算的复杂程度，同时更方便进行传感器的故障检测与隔离，有利于提高算法的鲁棒性。实验结果表明，本文提出的算法具有良好的实时性与鲁棒性，能够满足低成本小型无人平台导航的需求。

1 相关研究

以滤波方法为主的MSCKF[5]和基于非线性优化理论的VINS[6]被认为是目前主流的两种融合方法的代表。Mur-Artal R等提出的ORB_SLAM2算法[7,8]将相机与IMU联合初始化，结合g2o实现局部的BA优化，利用ORB提供的描述子在大范围运动时实现回环检测和重定位，但三线程的系统结构加重了CPU的负担。MSCKF中提出一种多状态约束的双目视觉惯性里程计，以类似滑动窗口的形式，在观察到同一特征点的多个位姿之间建立约束，再进行滤波更新，但以高维度的特征点为状态量增加了计算的复杂度。文献[9]根据IMU与相机的更新频率，设计多速率卡尔曼滤波器实现状态估计，充分利用了IMU数据，但其没有考虑线性加速度引入的有色噪声的影响，使用相机计算的位姿增量与滤波器结果构建观测量的方法，也使得加速度计的漂移在位置估计中不断累积。沈绍劼等提出的VINS[6]，使用IMU预积分构建紧耦合框架，将边缘化的先验信息、IMU测量残差以及视觉重投影误差进行联合优化，实现了载体的位姿估计。文献[10]提出的基于非线性优化的OKVIS框架，通过最小化地标重投影误差和IMU误差项寻求最优的相机位姿和地标位姿。

虽然目前存在很多高精度的VIO算法，但它们都是以提高系统的计算复杂度为代价来实现，系统中所采用的惯性器件也大多是价格昂贵的高精度IMU，不完全适用于搭载低成本、低精度IMU的小型移动平台，而且高负荷的运算量也使得其无法在低性能的计算设备上使用。此外，现有的基于滤波器的方法，如文献[9]提出的多速率卡尔曼滤波器保持了IMU的动态特性，但忽略了线性加速度引入的有色噪声影响，由于采用的是增量式构建观测量的方法，使得位置估计含有加速度计的漂移累计误差。基于上述分析，本文提出一种低计算成本的双目视觉与IMU融合的组合导航技术。

2 坐标系描述

图1 系统坐标系关系

3 双目视觉与IMU组合导航算法

本文所提出的基于误差状态粒子滤波器的双目视觉与IMU组合导航算法框架如图2所示，首先将IMU测量的加速度与角速度进行平滑滤波，过滤掉测量数据中的野值；在获得相机图像后，构建图像金字塔并完成左右图像、前后帧图像特征点的提取与匹配，再根据剔除误匹配之后的特征点对解算相机的运动信息，最后采用基于误差状态的粒子滤波器将对齐到导航坐标系的运动数据与IMU测量数据进行融合，以实现载体运动信息的最优估计。

图2 双目视觉与IMU组合导航算法框架

3.1 IMU测量模型与预处理

不考虑地球自转角速度的影响，将重力加速度看作常量，可以得到加速度计的测量模型为

(1)

其中，α、β、γ代表载体绕x、y、z三轴旋转的角度，Ra、Rβ、Rγ代表旋转矩阵，旋转顺序定义为z-y-x，g为重力加速度，Vx、Vy、Vz为载体的线性速度。

通常在小型移动平台上多使用基于MEMS技术的传感器，这种传感器具有低成本、低功率的特点，但也伴随着相对高水平的测量噪声和偏差，忽略非线性误差源(比如比例因子、正交误差)，陀螺仪与加速度计的输出结果可以表示为

(2)

(3)

(4)

其中，D为正整数，为滤波器滑动窗口大小，y(k)为k时刻的滤波输出。经过上述滤波器处理后，可以滤除传感器测量数据中的高频噪声。

3.2 双目视觉运动信息估计算法

该双目视觉运动状态估计算法在大多数情况下已经可以实现较精确的运动估计，但此时估算的位姿信息并未与重力对齐，无法直接用于导航，且纯双目的方法仍然对环境中的许多因素十分敏感。而IMU能够帮助载体恢复运动信息，有助于提高算法的鲁棒性，其敏感于重力加速度的特征，也能够辅助实现相机位姿信息与导航坐标系的对齐，这也是在双目视觉运动估计算法中引入IMU的原因。

3.3 基于误差状态的双目视觉与IMU融合算法

3.3.1 航迹推算

本文所采用融合算法的状态量共有16维

(5)

(6)

(7)

本系统所采用的算法将陀螺仪和加速度计的测量偏差视为常值漂移，则bω、bf的微分为0。最终基于惯性传感器的航迹推算方程为

(8)

其中，dt为IMU的更新间隔，Xk|k-1代表由k-1时刻获得的航迹推算结果，Xk-1为上一时刻的载体状态信息。

3.3.2 基于误差状态的粒子滤波融合方法

为了实现对载体运动信息的精确有效估计，本系统中引入了基于误差状态的粒子滤波融合算法。粒子滤波的核心思想是根据建议密度函数选取离散的随机采样点，通过对粒子集合求均值获得滤波器的估计结果。但是状态量中表示姿态的四元数受到模长为1的额外约束，并且对加法运算不具有封闭性，无法直接求解随机粒子集合。

通常，由航迹推算得到的四元数q与采样四元数qrandom之间的误差可以视为小量，则可以表示为

qrandom=q⊗δq

(9)

(10)

要获得当前时刻的误差状态，算法中采用了扩展卡尔曼滤波实时维护和更新δX，并为求解随机误差四元数提供合理的建议密度函数，以减小滤波过程中退化现象对估计结果的影响。

在估计误差四元数的过程中，误差状态的微分方程可以定义为

(11)

其中

(12)

当相机的测量数据更新后，通过融合相机数据可以对维护的随机粒子的误差状态进行校正，由于相机计算的位姿信息并未与导航坐标系对齐，因此在算法中利用加速度计敏感于重力的特性，在系统运行开始阶段，将载体静置一段时间，随后取多个加速度计测量数据的平均值，求得相机位姿数据与导航坐标系的对齐矩阵，完成相机测量数据与IMU数据的对齐。测量经过上述转化后代表当前时刻载体的位姿信息，则误差状态的测量转移矩阵H简化为单位矩阵。

经过更新获得随机粒子的误差状态和建议密度分布函数后，根据一般向量空间的随机变量计算方法，求解得到随机误差状态带入式(9)即可获得状态量的随机后代粒子。最后整个IMU与双目视觉融合算法流程如下：

(1)采用欧拉积分推算载体运动信息；

(5)根据建立密度分布函数产生随机的误差状态δXk～q(δXk-1,Zk)=N(δXk,Pk)，并根据式(9)计算后代随机粒子xk，针对每一个粒子重复上述步骤；

4 实验结果

为了验证本文中方法的可行性及有效性，本节会详细阐述将所推荐的方法运行于EuRoC数据集[11]的实验结果，并从精度、运行性能和实时性等方面进行分析。

EuRoC是基于小型无人机平台所采集的视觉惯性里程计数据集，其中包含以20 Hz频率更新的752×480分辨率的双目图像(MT9V034)以及200 Hz更新率的陀螺仪和角速度计测量数据(ADIS16448)；位置和姿态的真值分别由Leica MS50和Viconc采集得到，精度约1 mm。数据集中提供的11个测试情景，包含从良好视觉条件下的慢速飞行到运动模糊或者光线条件较差的快速飞行等多个场景，能够更全面的对算法进行评估。

4.1 算法精度

此外在本次实验中，将所推荐的算法运行于EuRoC数据集，并与Vins Fusion[12]和OKVIS进行了比较。其中Vins Fusion是在2019年提出的一种通用的多传感器里程计估计框架，OKVIS是基于滑动窗口的视觉里程计算法。

本实验采用文献[13]所推荐的工具对各算法的性能进行评估，该工具是基于Python语言开发，能够全面地分析各算法精度，最终提供所测试算法估计精度的定量分析结果与常用的误差评价结果。表1是各算法在EuRoC数据集上运行结果的均方根误差(RMSE)。本文的算法在Vicon Room场景中具有较好的定位精度，其中在V1-01和V2-02中的精度最高。经分析，主要是Machine Hall数据集中的环境条件、飞行路径都更加复杂；过快的飞行速度和较长的飞行距离也是MH-03和MH-04估计精度下降的原因之一；此外，本文的算法无法在V1-03、V2-03以及MH-05上实现载体的信息估计，除了飞行速度过快之外，还有光照条件太差或者存在运动模糊的情况导致，从而导致两帧图像之间的共识区域太小，无法追踪特征点。

表1 各算法估计结果均方根误差(RMSE)

图3表明了在V1-01和V2-02数据集上各算法估计的轨迹信息的效果图，其中点线为轨迹真值，实线是本文算法的实验轨迹估计结果，点划线和虚线分别为OKVIS和Vins Fusion的估计结果。从图3可以看出V2-02相比于V1-01飞行轨迹更复杂，飞行路径也更长。而从图4(a)中可以明显看出随着飞行轨迹增加，估计轨迹和真值之间的误差也不断增加，这主要是由于本文中的算法没有加入闭环检测和后端优化，在长时间运行情况下，累计误差不断增加，无法得到校正的缘故。但是从图4(b)可以看出，尽管估计轨迹的绝对误差在不断增大，但是其相对误差始终保持在4%以内。

图3 V1-01和V2-02中各算法轨迹估计结果

图4 V2-02中各算法在轨迹不同长度误差结果

4.2 运行性能

本文所推荐的算法主要是应用于无法携带高性能计算设备的小型无人平台上，因此除了对估计精度进行分析，算法的运行性能也是需要考量的重要元素之一。在实验中分别将各算法在数据上运行了6次，并统计了相关算法的CPU占用率。从图5中可以看出，OKVIS有很高的CPU占用率，经分析，认为主要是由于在两帧图像匹配的过程中计算了BRISK描述子导致的。而本文所采用的相机与IMU解耦的融合算法相比于另外两个算法具有更低的CPU占用率，相比于Vins Fusion降低了约29.7%，相比OKVIS降低了约57.2%，表明本文所用算法在运行性能上具有一定的优势。

图5 各算法在EuRoC数据集运行的CPU占用率

4.3 实时性

本节对所提出的算法的运行时间进行了测试，以说明所推荐的算法能够在线实时估计载体的运动信息。在实验过程中，将所推荐的算法与对比算法分别运行于EuRoC数据集上6次，其中实验平台为基于Ubuntu16.04的机器人操作系统，CPU型号为AMD Ryzen1600，记录程序每获取到一次数据之后程序的开始时间和运行时间，统计后总结于表2中。

表2 各算法平均运行时间

从表2中可以看出，本文所推荐的方法与Vins Fusion和OKVIS都能够实时估计载体的运动信息，但是本文所推荐的算法在运行时间上的表现更好，相较于Vins Fusion减少了约42.4%，相比OKVIS减少了约15%，能够保证稳定实时输出对载体的运动信息估计结果。及时追踪和估计载体的运动信息，利于平台根据载体数进行有效的控制，也使得该算法在低成本、低性能设备中的推广更具有竞争力。

5 结束语

本文提出了基于误差状态粒子滤波器的双目视觉与IMU融合算法实现了载体运动信息的实时估计。根据部分数据集失败的原因分析，未来考虑在相机位姿估算时引入IMU信息以进一步提高算法在高速运动场景下的鲁棒性。实验结果表明，与现有的方法相比，本文的算法在保证必要的导航精度和实时性的前提下，具有更高的计算效率和更快的运行速度，使其在无法携带高性能计算设备的小型无人平台部署中更具竞争力。