双目视觉与惯导融合的移动机器人定位方法

许智宾，李宏伟，张 斌，肖志远，邓 晨

1. 郑州大学信息工程学院，河南 郑州 450052； 2. 郑州大学地球科学与技术学院，河南 郑州 450052； 3. 郑州大学水利科学与工程学院，河南 郑州 450001

随着社会的进步和人工智能技术的快速发展，自主智能机器人受到了广泛关注。移动机器人就是自主智能机器人的一种，其应用领域众多，如仓储物流、自动驾驶、服务机器人等。研究移动机器人的同时定位与建图技术对于提高移动机器人的自主化、智能化等具有重要意义[1]。

视觉SLAM使用的视觉传感器主要有单目相机，双目相机和RGB-D相机[2]。单目相机无法独立地从一帧图像中恢复出周围环境的尺度信息[3]，RGB-D相机在室外环境中容易受到光照环境的影响[4]，而双目相机可以做到通过数学方法得到准确的尺度信息，使其可以克服RGB-D相机在获得尺度信息时容易受到光线的影响，以及单目相机产生的尺度不一致的问题。

视觉里程计是视觉SLAM系统中不可或缺的组成部分[5]。在视觉里程计中，根据其实现方法的不同可以分为基于特征法的视觉里程计和基于直接法的视觉里程计。基于特征法的视觉里程计是在图像中提取特征点并计算描述子，通过描述子实现特征匹配，通过计算最小化重投影误差来估计当前帧的位姿。基于特征法的视觉里程计具有定位精度较高的特点，但是由于需要计算图像帧之间的特征点和描述子，会占用部分计算资源，影响视觉里程计的实时性。如ORB-SLAM2[6]、ProSLAM[7]、OpenVSLAM[8]等都是比较优秀的基于特征法的视觉SLAM系统。基于直接法的视觉里程计仅提取特征点而不计算描述子，通过计算光流得到相邻两个图像帧之间特征点的匹配关系，之后通过计算最小化光度误差估计移动机器人的位姿信息[9]。如LSD-SLAM[10]、DSO[11]、DVO-SLAM[12]、RGBD-SLAM-V2[13]等都是比较优秀的基于直接法的视觉SLAM系统。基于直接法的视觉里程由于不需要计算特征点的描述子，使其运行实时性较好，但是光流法的运动跟踪策略使其定位精度往往不如基于特征法的视觉里程计。

近年来，惯导设备与视觉传感器融合的SLAM系统已经成为一个热门的SLAM技术研究课题[14]。视觉惯导融合[15]的SLAM系统现在有很多优秀的开源方案，如MSCKF[16]、ROVIO[17]、OKVIS[18]、StructVIO[19]、VINS-Mono[20]等。MSCKF是一种基于滤波的视觉惯导融合的视觉SLAM系统，它设计了一种滑动窗口模型，将滑动窗口中被多个图像帧观测到的特征点及其对应的相机位姿作为扩展卡尔曼滤波的更新。但是MSCKF需要一个良好的初始深度值以及初始协方差，否则将会导致位姿估计误差较大。ROVIO也是一种基于扩展卡尔曼滤波的视觉惯导融合的SLAM系统，在运动跟踪阶段使用基于图像块的光流法进行特征跟踪，将IMU数据用于状态传递，将视觉数据用于状态更新。OKVIS是一种基于非线性优化的视觉惯导融合的SLAM系统，它在滑动窗口模型种构建非线性优化函数进行位姿优化，包括了视觉重投影误差和IMU测量误差。OKVIS在前端视觉里程计部分采用基于特征法的视觉里程计，在图像中提取Harris角点[21]并提取BRISK描述子[22]。VINS-Mono是一种视觉惯导融合的视觉SLAM系统，使用光流法进行运动跟踪，使用IMU预积分技术对IMU数据进行预处理，同时使用一种滑动窗口模型将视觉数据与IMU数据进行紧耦合方式的非线性优化[20]。

由于视觉惯导融合的视觉SLAM系统中实现运动跟踪方式的不同，仍然受到特征法视觉里程计与直接法视觉里程计缺点的影响，如特征法计算描述子会影响其实时性，直接法带来的累计误差会影响其定位精度。针对上述问题，本文采用一种融合特征法与直接法的半直接法视觉里程计进行运动跟踪，提高其运行效率，同时采用视觉惯导融合的方式，构建基于滑动窗口的非线性优化的方式，进一步提高位姿估计的精度。

1 方法与原理

本文在基于特征法[23]的视觉SLAM系统和基于直接法的视觉SLAM系统的基础上提出一个融合了特征法与直接法的视觉里程计，并在此基础上融合视觉传感器与惯性测量单元，提出一种视觉惯导融合的视觉SLAM框架。在融合算法中，利用光流法[24]进行运动跟踪得到移动机器人的初始姿态，并将地图点投影到当前帧中，通过构建重投影误差函数估计位姿。同时利用IMU的瞬时高精度测量数据来弥补稳健性较低的视觉数据，以及利用长时间视觉数据的相对稳健性来抑制IMU数据的累计漂移误差。双目视觉与IMU融合的SLAM系统框架如图1所示。

1.1 融合特征法与直接法的半直接法位姿估计方法

为了克服直接法和特征法自身的缺点，并且保持直接法计算速度快，特征法精度高的特点，提出一种融合直接法和特征法的半直接法双目视觉里程计。融合算法的流程如图2所示。

融合算法主要包括双目视觉初始化、直接法估计初始位姿、特征法优化位姿3个模块。

1.1.1 双目视觉初始化

由于单目相机无法从一帧图像中恢复出观测到的目标点的尺度，因此单目SLAM系统通常采用三角化的方式，通过连续几帧图像恢复尺度信息，导致视觉里程计在开始工作之前需要有一个初始化的阶段，为之后的位姿估计提供尺度信息。而对于双目相机，可以根据双目相机模型使用单帧图像计算得到图像中观测到的特征点的尺度。使用双目相机进行视觉里程计的实现可以简化初始化的过程，提高运行效率。

图1 视觉惯导融合的SLAM系统框架Fig.1 SLAM system framework of visual inertial navigation fusion

图2 融合算法流程Fig.2 Flow chart of the fusion algorithm

在初始化阶段，首先在左目图像中提取特征点，然后使用光流法在右目图像中跟踪左目图像中的特征点，获取左右两幅图像中的匹配点对。

双目相机在标定之后其基线b和焦距f已知。假设对应一组匹配正确的点对，在左目图像中的坐标为UL，在右目图像中的坐标为UR。则根据双目相机模型有

(1)

式中，z为观测到的特征点在相机坐标系下的尺度信息；b为双目相机基线；f为焦距。

在有了尺度信息之后，可以得到特征点在相机坐标系下的世界坐标。假设空间点在相机坐标系下的坐标为(X,Y,Z)，特征点的像素坐标为(u,v)，则有

(2)

即可求解得到相机坐标系下的三维坐标(X,Y,Z)，得到特征点的尺度信息，为双目视觉里程计提供一个良好的初始值。

1.1.2 直接法估计初始位姿

在视觉里程计通过双目相机获取到最新一帧的图像之后，首先使用光流法跟踪前一个图像中的特征点，得到它们在当前图像中的位置信息，并且使用逆向光流的方法去除误匹配的特征点对，然后根据最小化光度误差来计算初始位姿。

(3)

光度误差δI可以表示为

δI(Tt,t+1,u)=It+1(πTt,t+1·π-1(u,dp))-It(u)

(4)

式中，dp是特征点在前一帧图像中的深度信息；It表示滑动窗口中第t帧图像。

为了简化计算，使在计算增量过程中优化方程的雅克比矩阵保持不变，本文采用逆计算的方法计算光度误差

δI(Tt,t+1,u)=It+1(πTt,t+1·π-1(u,dp))-

It(π(T(ε)·π-1(u,dp)))

(5)

由于构建的计算最小化光度误差的优化问题是一个非线性方程，因此这里使用高斯-牛顿的方法求解，通过不断迭代的方式，从相机的一个初始位姿开始，不断地计算更新增量来更新位姿的估计值

Tt,t+1←Tt,t+1·T(ε)-1

(6)

1.1.3 特征法优化位姿

使用直接法估计初始位姿，在长时间跟踪后容易产生累计误差，此时可以通过跟踪地图点，将当前图像中观察到的地图点投影到当前帧中，通过计算视觉重投影误差的方式来消除累计误差。

将滑动窗口中所有帧观测到的特征点记为地图点，然后将所有地图点根据当前帧的初始位姿投影到当前帧，得到当前帧中特征点与地图点的匹配关系。为了使投影位置更加精确，使重投影误差计算有一个良好的初值，本文通过构建特征点位置与地图点的投影坐标的光度误差函数来优化当前帧中所有投影点的位置

(7)

对于最小化光度误差这个优化问题，可以使用迭代高斯-牛顿的方法来求解，最终可以得到更为精确的地图点在当前帧中的投影位置。

在得到更为精确的地图点在当前帧中的投影位置之后，通过构建视觉重投影误差，使用迭代高斯-牛顿法求解，来进一步优化相机位姿

(8)

1.2 IMU数据预处理

IMU传感器通常是由三轴陀螺仪和三轴加速度计组成的，可以测量得到移动机器人运动的角速度和加速度，但是IMU在获取数据时会产生测量偏差和噪声，因此，IMU的数据测量模型可以表示为

(9)

得到IMU的运动模型为

(10)

式中，q表示旋转；v表示速度；p表示位移。

由于实际使用的情况下，IMU的数据采集频率要远大于相机采集数据的频率，因此需要在离散的时间下对IMU的数据进行积分处理。这里采用一种中值积分的方法对IMU数据进行积分处理，即对于k帧和k+1帧，二者之间的位姿可以使用两帧之间IMU采集到的所有数据的平均值表示，也就是i时刻与i+1时刻之间的所有IMU数据的平均值

(11)

式中

由于每次计算新的图像帧的位姿，都需要对所有IMU数据进行积分处理，因此增加了系统的计算量，会导致SLAM系统的运行效率降低。采用IMU预积分可以通过计算两帧之间p、v、q的增量更新积分值，以降低SLAM系统的计算量。

在连续时间下，IMU预积分的值只与不同时刻的IMU的测量值和IMU偏差有关。根据中值积分的方法，IMU的预积分值在离散时间下的形式为

(12)

式中

1.3 视觉惯导联合初始化

由于SLAM系统本质上是一个位姿状态估计器，因此一个良好的初始值能够很好地提高系统的定位精度，提高系统的稳健性。本文采用松耦合的方式来设计实现双目视觉与惯导融合的SLAM系统的初始化：首先，仅适用基于纯视觉的SfM和双目相机模型来恢复相机运动并计算图像中观测到的特征点的深度信息；然后，将视觉SfM计算得到图像的旋转、平移以及特征点的尺度信息通过松耦合的方法与IMU数据进行对齐，完成视觉惯导联合初始化，为系统提供良好的视觉位姿初始值和IMU初始值。视觉惯导联合初始化的流程如图3所示。

图3 视觉惯导联合初始化流程Fig.3 Visual inertial navigation joint initialization process

使用半直接法估计相机位姿的流程与第2节中介绍的方法相同，得到当前相机位姿之后根据双目相机模型恢复特征点的尺度信息，然后通过松耦合的方式将IMU预积分数据与半直接法计算得到旋转、平移和尺度信息对齐，估计重力、速度及IMU的偏差。

1.4 基于滑动窗口的后端优化方法

设计一种基于滑动窗口的后端优化方法的目的是为了减少SLAM系统位姿计算的累计误差的同时通过限制处理数据的规模减少计算量，提高SLAM系统的运行效率。

在滑动窗口中仅维持少量的图像帧，同时记录滑动窗口中所有的图像帧的视觉观测数据与图像帧之间IMU的预积分数据。

滑动窗口中主要保存了图像帧之间的IMU数据即滑动窗口中第k帧图像对应的IMU状态包括了速度、旋转、加速度计的偏置及陀螺仪的偏置，相机外参即相机与惯性测量单元之间旋转和平移，以及特征点的逆深度信息。这些都是要在后端优化阶段进行优化的信息。将优化信息表示为

(13)

式中，xk表示了滑动窗口中第k帧图像对应的IMU状态；n表示滑动窗口中关键帧的总数；m是滑动窗口中所有图像帧观测到的特征总数；λi是第i个特征点在第1次观测到它的图像帧中计算得到的逆深度。

对于优化变量的计算可以使用非线性优化的方法，通过构建滑动窗口中的视觉观测值误差，IMU测量误差以及滑动窗口中的边缘化误差进行优化变量的计算。滑动窗口内误差函数为

(14)

1.4.1IMU测量误差

由于惯性测量单元与视觉传感器采集数据的频率不同，可以使用流形预积分的方式在离散时间对IMU数据进行预积分，将IMU数据与图像帧中视觉观测数据对齐。所以IMU测量误差可以表示为IMU的实际测量值与IMU预积分的差值

(15)

1.4.2 视觉测量误差

对于视觉观测误差，可以使用计算重投影误差的方式表示。为了使计算方法适用于多种类型的视觉传感器，这里采用一种在球面坐标系下进行误差计算方法

(16)

2 试验与分析

为了验证本文提出的双目视觉与惯导融合的视觉SLAM系统方案的可行性和有效性，本文使用EuRoc这一个公共数据集进行相关试验分析。EuRoc数据集使用一种搭载双目视觉传感器和惯性测量单元的六翼飞行器采集数据，同时使用一种Vicon动作捕捉系统实时捕捉飞行器飞行时的位姿，并将其保存为六自由度的位姿数据作为数据集中飞行器飞行的真实轨迹。

EuRoc一共包含了11个图像序列，5个机械厂房图像数据(MH01-MH05),2个普通房间图像序列(VR1和VR2)，每一个数据集官方都给了一个跟踪难度，见表1。

表1 EuRoc图像序列

2.1 基于半直接法的位姿估计方法试验分析

基于半直接法的视觉里程计与基于特征法的ORB-SLAM2在EuRoc数据集[25]上进行测试，计算相对轨迹误差与绝对轨迹误差的均方根误差，验证方法的有效性。其中试验数据集选择MH01、MH03、MH05这3个代表不同跟踪难度的数据集。

以MH01为例，本文提出的基于半直接法的视觉里程计与ORBSLAM2的MH01数据集上的轨迹对比如图4所示。

根据SLAM系统估计的位姿信息与真实轨迹对比，计算相对轨迹误差与绝对轨迹误差如图5、图6所示。

图4 轨迹对比Fig.4 Trajectory comparison

两种SLAM系统在MH01、MH02、MH03这3个数据集中相对轨迹误差与绝对轨迹误差的对比结果见表2、表3。

表2 相对轨迹误差

图5 两种算法在MH01上的APEFig.5 APE of two algorithms on MH01

图6 两种算法在MH01上的RPEFig.6 RPE of two algorithms on MH01

表3 绝对轨迹误差

由表2和表3可以看出，半直接法视觉里程计的定位精度在绝对轨迹误差和相对轨迹误差方面都远不如基于特征法的ORB-SLAM2，为了进一步提高定位精度，本文在半直接法视觉里程计的基础上构建了视觉惯导融合的视觉SLAM系统，其定位精度将在2.2节进行详细分析。

表4是计算不同的SLAM系统对数据集中每幅图像帧的跟踪时间，反映SLAM系统的运行效率。由表4中可以看出，基于直接法的DSO在3种跟踪难度的数据集中处理图像帧的时间都是最少的，因此其运行效率是最高的，而视觉惯导融合的视觉SLAM系统的运行效率比半直接法视觉里程计的运行效率略低，是因为视觉惯导融合的视觉SLAM系统中增加了对IMU数据的处理以及回环检测模块，占用了一些计算资源。虽然视觉惯导融合的视觉SLAM系统的运行效率不如基于直接法的DSO，但是相比于基于特征法的ORB-SLAM2系统，其运行效率提升了约50%，具有较好的实时性。

表4 图像帧的平均跟踪时间

2.2 视觉惯导融合的视觉移动机器人定位系统试验分析

为了验证本文构建的视觉惯导融合的视觉SLAM系统的可行性，使用EuRoc数据集中不同的试验场景对其进行验证，选取数据集中MH01、VR12、MH05这几个代表不同场景不同跟踪难度的图像序列。本文构建的视觉惯导融合的视觉SLAM系统在数据集中运行结果如图7所示。

图7表示在该数据集中SLAM系统估计的相机位姿与数据集中真实轨迹的对比结果，其中彩色线条表示位姿的估计结果，不同颜色代表与真实轨迹不同的差值，灰色线条代表数据集中由动作捕捉系统捕捉到的真实轨迹数据。可以看出，视觉惯导融合的SLAM系统在机械厂房和普通房间这两个场景中都可以有效地进行运动跟踪并且估计相机位姿，而且可以看出，在跟踪难度较低的数据集中，位姿的估计结果与真实轨迹的误差值要小于跟踪难度较高的数据集，这是由于难度较高的数据集中相机运动较快且场景光照较差导致的。

图7 数据集运行结果Fig.7 Data set running results

此外，还计算了目前主流的几个视觉惯导融合的视觉SLAM系统在EuRoc数据集中其他几个图像序列上的绝对轨迹误差，见表5。

由表5可以看出，在上述图像序列中，本文视觉惯导融合的视觉SLAM系统的绝对轨迹误差均要优于OKVIS、ROVIO、VINS-Mono这3个主流的视觉SLAM系统方案。同时相比于本文采用的半直接法视觉里程计，在融合了IMU数据之后，定位精度得了约50%的提升。证明本文系统可以在提高SLAM系统运行效率的同时，仍然具有良好的定位精度。同时本文系统在EuRoc数据集中MachineHall与Vicon Room1、Vicon Room2这两个不同场景的图像序列中都可以良好运行，并保持良好的稳健性，证明本文系统可以适用在不同的应用场景中。

表5 绝对轨迹误差

3 总 结

本文提出的双目视觉与IMU融合的视觉SLAM系统使用基于半直接法的视觉里程计，提高了运动跟踪效率。首先，利用光流法进行相机运动追踪，再利用帧间特征点的匹配关系计算移动机器人初始位姿。其次，将滑动窗口中的三维路标点投影到当前图像帧中，并构建重投影误差函数估计相机位姿。然后，将视觉数据与IMU数据相互融合，利用IMU的瞬时高精度测量数据来弥补稳健性较低的视觉数据，同时也利用长时间视觉数据的相对稳健性来抑制IMU数据的累计漂移误差。最后，在EuRoc数据集中对本文算法进行试验验证。试验表明本文提出的双目视觉与IMU融合的视觉SLAM系统在不同的场景下都可以保持较高的定位精度。