曾 攀，潘树国，黄砺枭，王 帅，赵 涛

(东南大学仪器科学与工程学院，江苏 南京 210096)

实现高精度、强稳健性定位是完成移动智能体自主导航、探索未知区域等既定任务的重要前提[1- 2]。单目视觉传感器可获得形状、颜色及纹理等丰富的环境信息，但光照变化、无纹理对象、运动模糊和未知尺度都严重影响单目视觉定位结果。惯性测量单元(IMU)提供了移动体高采样率的测量数据，动态性能好，但存在漂移现象。利用这两类传感器的互补性，可将视觉定位与惯性导航技术融合即VIO(visual inertial odometry)[3- 4]。当单目视觉传感器无法获得有效视觉信息时，IMU继续进行运动估计并恢复尺度，而视觉过程解决了IMU的漂移问题，两者结合可构建出精度更高、稳健性更强的定位系统。因此，VIO在微型飞行器、自动驾驶、虚拟现实及增强现实等领域有着巨大潜力和广泛的应用前景[5- 8]。

文献[9]在OKVIS框架中提出了融合视觉惯性的前端模型，该模型实现了视觉和惯性数据的紧耦合，但未恢复出系统尺度和重力加速度等数据，同时不含预积分框架，因此其定位精度和稳健性较差。文献[10]提出了基于ORB_SLAM2的单目视觉惯性定位前端框架，其预积分模型采用流型预积分算法，视觉惯性初始化模型需约15 s，在该阶段无法实现实时稳健性定位，同时其跟踪模型采用简单的匀速模型，使相机剧烈运动时无法正确初始化，甚至跟踪失败，最终导致定位效果不佳。同年，文献[11]提出了VINS(visual- inertial navigation system)，其初始化模型相对较快，但并未对加速度计偏置进行标定，同时初始化精度略低，采用传统的参考帧跟踪模型限制了其定位精度。

综上，针对目前国内外学者提出的单目视觉惯性融合定位模型存在的定位精度略低、初始化时间较长及稳健性不高的问题，本文提出一种包含IMU加权预积分、快速联合初始化及视觉IMU辅助跟踪的面向单目视觉惯性紧耦合定位的改进前端模型Improved_VIO，模型框架如图1所示。

1 IMU加权预积分算法

IMU预积分旨在将相邻两个关键帧间IMU测量值进行统一积分得到运动状态的相对增量，为视觉惯性联合初始化提供初始值，并为后端优化提供IMU约束。IMU预积分精度直接影响初始化结果和系统稳健性。传统预积分模型均采用流型预积分和四元数预积分模型[12-13]，积分效果基本满足定位要求。本节针对视觉惯性融合定位强稳健性、高精度的需求，重点探讨视觉惯性数据融合同步问题，建立面向VIO的IMU高精度加权预积分模型。

1.1 加权IMU测量值

IMU的测量值包含由陀螺仪和加速度计分别提供的角速度和加速度，t时刻典型的测量值方程如下

(1)

定义t到t+Δt时刻之间真实的加权IMU测量值为这两个时刻测量值加权描述的常数，即

(2)

式中，加权系数c1(t)、c2(t)、c3(t)、c4(t)具体表示如下

c1(t)=wB(t)/(wB(t)+wB(t+Δt))

c2(t)=wB(t+Δt)/(wB(t)+wB(t+Δt))

c3(t)=aB(t)/(aB(t)+aB(t+Δt))

c4(t)=aB(t+Δt)/(aB(t)+aB(t+Δt))

1.2 加权预积分模型

当前时刻t+Δt的状态可由t时刻的状态积分得到，即

(3)

(4)

(5)

与经典的欧拉积分相比，加权欧拉积分更充分地利用了角速度和加速度测量值，可更真实地反映速度和角度的变化趋势。

2 视觉惯性松耦合的快速联合初始化方法

在视觉惯性耦合系统中，初始化是在紧耦合处理前计算绝对尺度、重力加速度、速度等信息的过程，此外初始化计算的加速度计偏置和陀螺仪偏置能使重新计算的预积分值更精确。初始化精度和时间直接影响视觉惯性紧耦合系统的稳健性、定位精度及实时性。为缩短初始化时间，本节重点研究尺度恢复问题，提出了一种视觉惯性松耦合的快速联合初始化方法。

2.1 偏置及重力加速度估计

2.1.1 陀螺仪偏置估计

由于噪声的存在，分别从陀螺仪测量值和视觉测量值得到的旋转不相等，故通过最小化相对旋转的差异来优化陀螺仪偏置，优化模型如下

(6)

2.1.2 重力加速度和加速度计偏置估计

估计从IMU坐标系到相机坐标系的变换时包含了尺度s

(7)

结合式(7)和描述了两个连续关键帧的式(3)，忽略加速度计及陀螺仪的偏置可得

(8)

估计重力加速度时，同时计算出不准确的尺度s，考虑连续3个关键帧间的式(7)、 式 (8)关系，速度项由相对速度表示，分离的变量如下

(9)

(10)

(11)

式中

结合一阶近似值,gw可表示为

(12)

结合式(8)和式 (12)并包含加速度偏置，得到

(13)

式(9)进一步转化为含有尺度、重力加速度方向修正及加速度计偏置的公式

(14)

2.2 快速尺度恢复算法

将速度、重力向量及尺度合为一个状态向量

(15)

(16)

式中

通过求解此线性最小二乘问题，可得到相机帧的速度v和优化后尺度s

(17)

3 视觉IMU辅助跟踪方法

IMU辅助跟踪模型结合了单目匀速模型和IMU先验数据，为初始化阶段及初始化完成后提供一个稳健性的视觉前端，能有效提高视觉惯性系统的整体定位性能。IMU辅助跟踪模型主要包括IMU先验数据解算和系统初始化估计两个部分，IMU先验数据解算包含在IMU预积分中，本节将主要介绍系统初始化估计的跟踪部分。

系统的初始化阶段由于无法获取速度、重力加速度及尺度，故只能使用IMU旋转数据。匀速跟踪模型视相邻关键帧间的相对变换不变，即对于3个连续帧F1、F2及F3，相互间的相对旋转分别为ΔR12和ΔR23，且有

ΔT12=ΔT23

(18)

F1和F2之间的相对变换及F3的变换矩阵分别为

(19)

(20)

本文将IMU初始的先验旋转数据由旋转矩阵间转换关系得到

(21)

将旋转矩阵代入到变换矩阵模型中

(22)

再将改进的T3作为初始值代入优化模型。当系统完成初始化后，可获得准确的重力加速度和IMU速度，因此在视觉优化前可通过IMU获得整个系统的速度信息

(23)

最终将改进的T3和v3联合作为初始状态代入优化模型。

4 试验验证与结果分析

为验证提出的单目视觉惯性改进前端模型的可行性和稳定性，本文算法基于视觉惯性ORB_SLAM2(后文记为Origin_VIO)，并采用公开的EuRoC MAV数据集中的9个测试序列[15]。该数据集包含了不同的房间和工业环境中，配备有VI- Sensor双目惯性相机的无人机动态运动。其图像采集频率为20 Hz，IMU采样频率为200 Hz，数据集提供真实轨迹。由于是单目视觉惯性平台，故仅使用数据集中左目相机测量值。为表述简便，将该数据集的数据序列简记，如MH_03_medium记为MH03，其他序列类似。本文试验使用内存为8 GB的Intel Core i7- 7000笔记本，将Improved_VIO试验结果与Origin_VIO结果作比较，并图示了V101和MH03两个数据序列以展示试验效果。

图2与图3分别为MH03和V101序列在Origin_VIO及Improved_VIO 2种系统中的定位轨迹平面图。由于系统基于关键帧进行定位跟踪，故定位轨迹会存在部分不连续的情况。由图中可知，2个系统估计的结果与实际轨迹基本重合，但Improved_VIO的轨迹较于Origin_VIO的轨迹整体更连续、稳定且偏离更小。

图4—图6为MH03序列在2种系统中定位的轨迹误差分析图。其中，图4对比了Origin_VIO和Improved_VIO的绝对定位误差，显示出Improved_VIO的定位结果整体更优。图5具体表示了2种系统在3个不同方向上的位置坐标变化情况。由图中可知，Improved_VIO及Origin_VIO的轨迹在X、Y方向上均较好地符合实际轨迹，Z方向上Improved_VIO表现更优。图6为定位误差的小提琴图，主要统计分析数据的离散程度和分布密度。Improved_VIO的轨迹误差分布主要集中在0.095 m左右，而Origin_VIO的轨迹误差主要集中在0.194 m附近，前者误差的中位数和四分位数都较小，且下侧的离散值表现也更优。

为验证本文提出的联合初始化方法的快速性，以V101序列为例做初始化状态图。图7—图10分别为初始化尺度、重力加速度、陀螺仪偏置及加速度计偏置的变化情况。由图中可知，这4类初始化量在Improved_VIO中10 s内稳定收敛，快于Origin_VIO。

表1给出9种EuRoC数据序列在Origin_VIO及Improved_VIO 2种系统中的定位结果。由表1可知，本文定位结果优于Origin_VIO的定位结果，定位精度平均提高了约30%。

表1 2种系统在EuRoC数据集上定位结果对比

5 结 语

本文提出了一种改进的单目视觉惯性定位前端模型，结合IMU加权预积分模型、快速联合初始化方法及视觉IMU辅助跟踪模型，提升了单目视觉惯性定位的精度和实时性。通过对EuRoC数据集的试验验证，本文提出的Improved_VIO相比于视觉惯性ORB_SLAM2在小尺度范围内的定位精度有较大改善。其中IMU加权预积分方法表现出良好性能，同时实现了10 s内的准确初始化，系统整体定位精度提高了约30%。本文研究模型仍有改进空间，初始位姿估计中未利用IMU预积分提供的位置数据，对传感器偏置和噪声处理较简单，未来将重点研究位置数据有效地融入初始位姿估计中，并建立偏置和噪声分析处理模型，以进一步提升单目视觉惯性紧耦合定位的精度和稳健性。