基于多尺度光流融合特征点视觉-惯性SLAM方法

王通典, 刘洁瑜, 吴宗收, 沈 强, 姚二亮

(火箭军工程大学导弹工程学院, 陕西 西安 710025)

0 引 言

同时定位与地图(simultaneous localization and mapping, SLAM)构建中核心的问题是通过视觉里程计(visual odemetry, VO)完成移动载体的位姿估计。目前VO的实现方案可以分很多种,其中将相机结合惯性测量单元(inertial measurement unit, IMU)的实现方案称为视觉惯性里程计(visual-inertial odemetry, VIO)。依靠纯视觉的VO虽然可以自主的实现位姿解算,但是仍受制于相机本身的特性,易受外界环境干扰。IMU测量频率高于相机且受外界干扰较小,但是长时间导航存在漂移的问题。考虑两者之间的互补性,融合视觉和IMU实现的VIO可以发挥相互之间的优势,弥补单一传感器的不足,从而实现更高的测量精度。

融合IMU和图像信息的视觉-惯性SLAM(visual-inertial-SLAM, VI-SLAM)分类方式有很多种,根据数据耦合方式可分为:基于松耦合和基于紧耦合。松耦合通过独立步骤融合VO估计的位姿和IMU测量的状态,紧耦合则将VO和IMU的状态量统一进行估计。按照后端优化方法可以分为:基于滤波的方法和基于非线性优化的方法。早期的SLAM主要基于扩展卡尔曼滤波(extented Kalman filter, EKF)实现,文献[7]基于EKF搭建SLAM系统,实现状态在线估计。随后文献[8]采用无迹卡尔曼滤波(unscented Kalman filer, UKF)构建VSLAM系统,该方法实现了位姿的在线更新。文献[9]提出了一种多状态约束卡尔曼滤波(multi-state constraint Kalman filter, MSCKF)实现IMU和视觉的融合,将视觉信息作为状态量,使用IMU进行状态预测,在卡尔曼滤波的框架下实现紧耦合的SLAM。文献[10]提出了一种基于EKF进行状态估计的鲁棒VIO(robut VIO, ROVIO),将光度误差作为观测量进行更新,但该算法对特征点的数量依赖较大。文献[11]提出了一种基于关键帧的视觉惯性SLAM算法,将视觉重投影误差结合惯性误差在滑动窗口中进行非线性优化,虽取得不错的效果,但系统整体效率较低。

按照VO前端还可分为:基于图像特征点的间接法(特征点法)和基于图像灰度变化的直接法。文献[13]在ORB-SLAM(oriented FAST and rotated BRIEF SLAM)的基础上融合惯性信息提出了紧耦合的VI-SLAM算法,实现了较高的定位精度并且具有地图重用和回环检测等功能,但基于特征点法的SLAM算法相比直接法较为耗时,占用大量的系统资源。半直接法VO(semi-direct VO, SVO)采用半直接法实现同时定位与建图,仅在关键帧进行特征点提取从而具有较高的实时性。文献[17]在SVO的基础上,提出了一种半直接视觉惯性里程计,通过IMU信息弥补视觉里程计的缺陷,有效提高跟踪精度与系统鲁棒性,同时较准确地恢复了环境尺度信息,但是基于半直接法的视觉前端,在相机快速运动情况下鲁棒性较差。为了解决直接法依靠梯度搜索计算位姿容易陷入局部最优的问题,文献[18]将IMU数据紧密关联到图像跟踪过程中,提出了一种结合直接法和IMU的视觉里程计,并取得了较小的累计误差,但是考虑IMU自身漂移的特性,仍不能较好地解决直接法视觉前端陷入局部最优的问题。

文献[19]基于滑动窗口非线性优化的后端提出了结合Shi-Tomasi光流法和IMU的单目视觉惯性系统(monocular visual-inertial system, VINS-Mono)算法,在非线性优化之前创新性的采用松耦合方式进行视觉惯性联合初始化,使得VINS-Mono的效果优于其他基于非线性的VI-SLAM算法,但基于光流法的SLAM算法易受光照变化影响,容易丢失特征,因此环境鲁棒性较差。

与使用人工设置的特征点不同,文献[20]基于卷积神经网络进行位姿估计提出了一种端到端的卷积神经网络SLAM(convolutional neural networks SLAM, CNN-SLAM)算法,由于通过网络模型提取图像更深层次的特征信息,该SLAM算法对光照变化更加鲁棒。但网络训练的效果依赖大量的数据样本,实时性较差。

基于光流法的SLAM算法具有较高计算速度,但需满足很强的灰度不变假设,实际中难以满足。在光照条件较差、相机运动过快导致图像模糊以及弱纹理的情况下鲁棒性较差,容易出现累计误差较大甚至位姿估计失效的问题。

为提高视觉惯性导航系统实时定位精度和鲁棒性,针对基于光流法的视觉前端鲁棒性较差容易产生累计误差的问题,借鉴开源算法VINS-Mono的系统框架,本文结合LK(Lucas and Kanade)光流法和ORB特征点法融合IMU信息,提出了一种多尺度光流融合特征点法的视觉-惯性SLAM方法。主要改进如下。

(1) 通过多尺度四叉树均匀化的方法改进ORB特征提取过程,提高特征提取的分布性和鲁棒性,从而提高位姿解算精度,减少累积误差。

(2) 提出了一种光流融合特征点法的VIO,根据是否是关键帧进行自适应特征匹配和光流追踪,为解决帧间位姿估计失效的问题,融合惯性信息利用帧间IMU预积分位姿估计结果预测,减少累积误差对初始位姿的影响。

(3) 整合ORB特征匹配重投影误差、IMU预积分误差和双向滑动窗口先验误差构建目标优化函数,采用滑动窗口非线性优化进行位姿求解,保证实时性的同时提高定位精度。

1 系统框架

本文设计的改进算法如图1所示。主要由5个部分组成:数据预处理、视觉惯性初始化、后端非线性优化、闭环检测以及全局位姿图优化。本文的主要改进在数据预处理、视觉惯性初始化以及后端非线性优化3部分。

图1 算法框图Fig.1 Algorithmic framework

(1) 数据预处理:首先采用改进的算法进行特征点的提取,关键帧采用ORB特征点法建立数据关联,普通帧采用LK光流追踪建立数据关联保证实时性的同时提高鲁棒性。

(2) 初始化:基于LK光流建立的数据关联进行初始位姿估计为后端非线性优化提供初值,并联合惯性信息进行视觉惯性初始化,估计IMU偏置、尺度、重力等初始状态;针对在相机运动过快、特征缺少等情况下丢失跟踪从而导致位姿估计失效的问题,本文通过融合惯性信息,利用帧间IMU预积分结果对失败的位姿估计进行预测,从而为非线性后端优化提供初始位姿估计。

(3) 后端非线性优化:联合ORB特征点重投影误差、IMU预积分误差、先验构建后端优化目标函数,采用滑动窗口非线性优化进行位姿求解。

2 多尺度光流融合特征点法和惯性信息融合方法

2.1 改进ORB特征提取算法

光流融合特征点法前端算法首先需要对ORB特征点进行提取,以便后续的LK光流追踪和特征匹配。传统的ORB算法为了获得稳定的特征点,提取结果往往分布不均匀、冗余特征较多。这会导致帧间位姿解算误差较大,从而使得定位精度降低。本文对ORB特征提取过程进行改进,整个流程如图2所示。

图2 改进ORB特征提取流程Fig.2 Improved ORB feature extraction process

为了使ORB特征具有尺度不变性首先构建高斯图像金字塔,以1.2尺度因子缩放原图像,设置图像金字塔的层数为6层;其次对金字塔的每层图像进行网格划分,设置网格大小为30;在每个网格内,进行自适应FAST角点的提取;对每层图像提取的FAST角点进行四叉树均匀化,四叉树划分如图3所示，划分节点并筛选出节点内响应值最大的特征点;最后对得到的所有特征点在原图像层再次进行四叉树划分,以实现特征点均匀化和减少特征冗余性。

图3 四叉树节点划分Fig.3 Quadtree node division

采用自适应的FAST角点提取,根据每个网格内提取特征点的数量自适应选择提取阈值。降低阈值提取的特征点存在冗余性,四叉树分配特征点可以均匀的筛选出响应值最大的特征点。

四叉树的思想是对数据区域进行递归,划分为4个节点的树结构。首先按照图像的宽高比设置初始节点为1,随后划分为4个子节点,判断每个节点中特征点的数量,大于1的节点继续划分为4个子节点,依次不断划分。因为每层图像金字塔需要提取的特征点数量不同,所以每层金字塔中四叉树节点深度也会自适应的发生变化,当节点的总数大于当前层设置的特征点数或者子节点内的特征点数等于1时,将不再继续划分节点。当进行完每一层的四叉树分配后,对所有提取的特征点在原尺度图像进行四叉树分配,进一步提高特征分布性,整个改进ORB特征提取法流程由算法1所示。

算法1 改进ORB特征提取输入:图像img-金字塔层数levels-当前层提取特征点nums-阈值threshold1、threshold2输出:提取的特征点mFor j=0;j

2.2 光流融合特征点视觉惯性里程计算法

光流法相比特征点法,虽然具有较高的速度,但是该方法基于的3个假设实际中难以满足,易受光照变化影响鲁棒性较差,跟踪结果不稳定从而导致位姿估计精度低甚至位姿估计失效,而特征点法较为准确且鲁棒性高。

结合LK光流速度快和ORB特征点法精度高的特点同时考虑位姿估计失效问题,本文提出了一种光流融合特征点法视觉惯性里程计。在采用改进特征提取算法得到特征点后,帧间进行光流追踪,关键帧进行BRIEF描述子的提取以及特征匹配。同时考虑帧间的光流追踪在光照较差环境下容易丢失特征,导致位姿估计失败,本文采用IMU预积分信息对帧间位姿进行预测。最后构建特征匹配点的重投影误差并加入到滑动窗口,连同IMU残差以及先验进行非线性优化,算法流程示意图如图4所示。

图4 光流融合特征点视觉惯性里程计Fig.4 Optical flow fusion feature point visual inertial odometer

(1) 采用改进的特征提取算法进行特征点的提取。

(2) 得到提取的特征点后,利用LK光流法进行帧间追踪,从而建立图像帧之间的数据关联,并根据对极几何、PNP等方法恢复帧间位姿、为后端非线性优化提供初值,同时利用三角测量的方法得到特征点逆深度的估计值;针对位姿估计失效问题,利用IMU预计分结果进行预测。

(3) 联合惯性信息进行视觉惯性初始化,恢复尺度、IMU零偏、重力加速度、速度等视觉惯性系统的初始状态。

(4) 根据图像帧之间的视差以及帧间共视特征点的个数,进行关键帧的选择。

(5) 在关键帧对提取的特征点计算描述子,并进行特征匹配,建立关键帧间的数据关联;构建重投影误差联合IMU预积分误差、先验加入到滑动窗口中进行非线性优化。

2.3 IMU预积分结果预测位姿估计

第2.2节的帧间初始位姿恢复依靠光流追踪的特征点采用对极几何、PNP(perspective-N-point)方法进行求解。因光流法易受光照变化的影响,在图像模糊、相机运动过快的情况下易发生特征缺失从而导致初始位姿估计失效。针对该问题,考虑短期内IMU位姿估计结果较为准确,本文在初始位姿估计过程中引入惯性信息,利用IMU的预积分以及上一图像帧位姿预测当前帧的位姿。设位姿失效发生在+1帧,图像帧从世界坐标系到相机坐标系的位置wc、速度wc以及姿态wc,和从世界坐标系到IMU坐标系的位置wb、速度wb和姿态wb已知,根据IMU预积分公式可推导+1帧从世界坐标系到IMU坐标系的位置wb+1、速度wb+1和姿态wb+1,可由

wc+1=wb+1

(1)

wc+1=wb+1⊗

(2)

得到+1图像帧从世界坐标系到相机坐标系的位置wc、速度wc和姿态wc,其中和为IMU坐标系和相机坐标系之间的平移、旋转量可通过标定得到。

2.4 基于关键帧的滑动窗口非线性优化

紧耦合的非线性优化是使所有测量残差马氏范数的和最小,从而得到最大状态后验估计。本文对关键帧进行特征描述与匹配,并将由关键帧间匹配的特征点构建的重投影误差,联合IMU预积分残差、边缘化先验构成最小化目标函数,加入到滑动窗口进行后端非线性优化如图5所示,滑动窗口中目标函数()形式为

(3)

图5 VI-SLAM后端算法示意图Fig.5 Schematic diagram of VI-SLAM back-end algorithm

滑动窗口中待优化的系统状态向量在时刻定义为

(4)

式中:为IMU状态向量,包含IMU在{}系中的位置w b、速度w b、姿态w b以及加速度零偏值和角速度零偏值;为特征点逆深度。

对目标函数()采用误差状态进行线性化,并利用Gauss-Newton方法迭代求解和Huber鲁棒核函数剔除异值,则系统状态向量可表示为

(5)

δ=

(6)

式中:是滑动窗口先验、IMU预积分、视觉测量的信息矩阵,=()();=-()();()为目标函数()对状态量的雅可比。

3 实验与分析

为验证本文算法的有效性,分别从ORB特征提取效果,以及光流特征点视觉惯性里程计的定位精度和鲁棒性进行验证。采用公共数据集EuRoC对算法进行评估,该数据集包含11个数据序列,分为简单、中等和困难3个等级,综合考虑光照、运动快慢等飞行条件,可以全面评估算法的性能。实验硬件平台参数如表1所示。

表1 硬件平台参数

3.1 改进ORB特征提取实验验证

算法的验证实验采用数据集中MH-05序列图像,综合考虑选取相机运动过快、光照较弱以及光照发生变化3个场景进行特征点的提取与光流追踪,对比算法采用Shi-Tomasi光流法和原始ORB算法进行主观视觉分析如图6所示。

图6 特征提取结果对比Fig.6 Comparison of feature extraction results

设置提取特征点数量为500个,从图6中可以看出原始ORB算法提取特征点主要集中在物体边缘,离散性差,因此采用该方法提取的特征点无法反应图像的整体变化;基于Shi-Tomasi光流跟踪的特征点易受光照变化影响,在弱光场景下跟踪的特征点最少,光照变化场景下特征点分布在图像下方,分布不均匀。而相比之下本文提出的算法提取特征点具有较好的分布性,受光照变化和快速运动影响较小。

为进一步客观量化本文所提ORB特征提取算法具有良好的分布性和鲁棒性,设置特征点提取阈值为500,对以上3种算法在不同的场景下进行多次实验并计算平均值,从特征提取误匹配率、算法耗时以及分布均匀度进行对比。分布均匀度采用统计的方法得到,利用30×30大小的网格对提取到特征点图像进行划分,共可划分为个网格。分别统计每个网格中特征点的数量,记提取特征点数量不为0的网格数为,则分布均匀度的评价指标可表示为

(7)

该值越接近1则分布均匀性越好。多次实验平均结果如表2所示。

表2 多场景算法数据指标对比

从表2中对比数据可知,ORB算法在3种场景下提取特征点最多且匹配率较为稳定,因增加了特征描述的过程,算法平均耗时较多,且均匀度较低这与图6中结果一致。Shi-Tomasi光流法耗时最低且均匀度适中,但是在光照变化场景和弱光照场景特征点最少,这是因为光流法易受光照变化影响。本文的改进方法整体耗时介于ORB算法和Shi-Tomasi光流法之间,在弱光照场景和快速运动场景下特征点提取数量适中,因增加了特征点均匀化的过程,在3个场景下均匀度均高于其他两个算法,且匹配率有所提升。

综上,本文改进的特征提取算法在保证实时性的同时,提高特征提取离散性且有较高的匹配率,在多场景表现均优于对比算法,具有较强的鲁棒性。较好的特征离散性能反应整幅图像的变化提高数据关联的准确性,从而降低累计误差。

3.2 光流融合特征点的视觉惯性SLAM算法

针对本文提出的光流融合特征点法的视觉惯性SLAM算法,从导航定位精度和鲁棒性进行实验分析与评估。综合考虑实验采用EuRoC数据集中V2_01_medium序列,同等硬件条件下,采用VINS-Mono算法与本文算法进行对比。

图7为本文SLAM算法和VINS-mono在MH_05_difficult序列上的绝对位姿误差(APE),图中颜色条显示了误差整体分布情况。从图7中可知,VINS的平均误差在44.8 cm,而本文的平均误差为35 cm,本文算法的误差极值比VINS的更小,因此本文的算法在该序列定位精度更高。将三维绝对误差轨迹投影到-平面分析可得,相比VINS本文算法整体位姿误差更小(表现为蓝色线段占比更大),且在相机快速运动时(红色线段)最大轨迹误差有所降低,在该序列本文算法定位精度更高。

图7 MH-05序列绝对位姿误差对比Fig.7 Comparison of absolute pose error of MH-05 sequence

为了说明本文算法定位具有更好的鲁棒性。选取不同的定位难度,分别在V1_02_medium和V2_01_easy序列将本文算法和VINS-Mono进行对比如图8所示。由图8可知,本文算法在以上两个序列的平均位姿误差均有所降低,在不同难度的数据集上运行均有更好的表现,表现出了更强的鲁棒性。

图8 两种序列绝对位姿误差对比Fig.8 Comparison of absolute pose errors in two sequences

为了进一步说明本文算法具有更好的鲁棒性和定位精度,对以上3个序列(MH_05_difficult、V1_02_medium、V2_01_easy)的定位误差进行量化,主要从均方根误差(Rmse)、误差平方和(Std)、误差极值(Max、Min)以及误差中值(Median)进行分析,本文SLAM算法和VINS的定位误差统计特性对比如表3所示。

表3 本文算法和VINS-Mono的误差量化指标对比

分析表3中数据可得,在3个序列的误差均值较VINS平均降低12.4%,误差中值平均降低12.3%,且误差极值的表现也优于VINS-Mono,进一步对轨迹误差的均方误差进行对比如表4所示,相比之下本文算法的轨迹误差更小,绝对轨迹误差的均方误差平均降低16.7%。

表4 绝对轨迹误差的RMSE对比

综上可以说明本文算法定位精度和鲁棒性均优于VINS-Mono,这是因为本文提出的光流融合特征点的SLAM算法降低了位姿的累积误差,特征提取的均匀化过程提高了位姿估计精度,且在光照较差或相机快速运动等场景下具有较强的鲁棒性,其中MH序列绝对轨迹误差降低更加明显,这是因为MH序列存在较多处光照条件较差的飞行轨迹,而本文面对较差的光照条件更加鲁棒,且在利用IMU预积分结果预测了估计失效的位姿。

4 结 论

结合光流法速度快和ORB特征点法精度高的特点,本文提出了一种光流融合特征点法的视觉-惯性SLAM方法。改进ORB特征提取过程,采用多尺度网格化的方法提取特征点,并利用四叉树均匀分配特征点从而提高特征点的分布离散性和位姿估计的精度。基于改进特征提取过程融合光流和特征点法实现了视觉-惯性SLAM算法,基于光流法进行帧间的数据关联从而进行位姿估计为后端优化提供初值,针对帧间位姿估计失效的问题,采用IMU预积分结果预测帧间的位姿估计,基于ORB特征点算法对关键帧进行数据关联,联合ORB特征点的重投影误差、预积分误差、边缘化先验构建目标函数进行非线性优化。实验表明,改进的ORB特征提取算法在保证实时性的同时具有较高的分布离散性和鲁棒性,从而降低累计误差。光流融合特征点的VI-SLAM算法定位精度和鲁棒性均优于VINS-Mono。