基于改进ORB特征匹配的单目视觉—惯性SLAM算法

祝晓轩　杨杰　胡继港

摘要：针对VINS-Mono算法在弱纹理环境下运行不稳定、累积误差大等问题，提出了改进ORB特征匹配筛选的单目视觉—惯性SLAM算法。测量数据预处理环节采用了ORB特征提取，在特征点匹配时，使用双向匹配过滤和最小匹配点距离倍数判别剔除误匹配，最后利用随机采样一致性算法完成特征匹配。实验结果表明，与原算法相比，改进VINS-Mono算法的精度较高。

关键词：SLAM；特征点提取；特征点匹配；匹配点筛选

中图分类号：TP242 文献标志码：A

即时定位与地图构建（Simultaneous Localization and Mapping，SLAM）指在机器人或其他载体上对各种传感器数据进行采集并计算，完成对其自身位姿的定位并生成周围场景地图信息。作为机器人导航技术研究的热点之一，视觉SLAM使用的相机比激光SLAM使用的激光雷达价格低，且采集的图像信息更为丰富，相比传统的全球卫星导航系统，视觉SLAM系统既可以完成高精度的定位，又可以实时生成周围环境地图［1］。但由于缺少深度信息，单目视觉SLAM存在尺度模糊性问题，因此引入了惯性测量单元（Inertial Measurement Unit， IMU）。IMU能够测量传感器本身的角速度和加速度，可以与视觉传感器采集的数据进行互补，构建出鲁棒性更强的视觉—惯性SLAM系统［2］。MSCKF［3］算法和ROVIO［4］算法均是单目相机和IMU紧耦合的视觉惯导融合SLAM算法，但前者的后端优化基于扩展卡尔曼滤波器（Extended Kalman Filter， EKF），后者则基于迭代扩展卡尔曼滤波（Iterated Extended Kalman Filter， IEKF）。从近年来国内外经典的视觉—惯性SLAM算法系统中可以看出，非线性优化已经成为视觉—惯性SLAM后端方案的主流理论。非线性优化通过迭代优化状态估计，通常采用降低计算复杂度的滑动窗口优化策略。OKVIS［5］算法和VINS-Mono［6］算法均使用非线性优化作为系统后端方案。相比于使用双目相机的OKVIS，VINS-Mono具有更加完善和鲁棒的初始化、重定位、闭环检测以及全局位姿图优化等环节，定位精度更高，鲁棒性更加优良。但由于VINS-Mono的前端测量数据预处理中采用光流法，导致在弱纹理及复杂环境下存在运行不稳定且累积误差大，本文针对开源的VINS-Mono算法的前端部分进行改进。

1 系统框架构建

基于VINS-Mono算法，本文算法采用紧耦合方案，后端使用非线性优化完成视觉与惯性信息的深度融合，系统框架如图1所示，分为五部分：测量数据预处理、系统初始化、后端滑动窗口优化、回环检测与重定位、全局位姿图优化。

在测量数据预处理模块中，相机图像处理环节与原VINS-Mono算法不同，本文算法是对提取图像进行金字塔处理，然后提取ORB特征点，使用特征点法匹配相邻帧的特征点，对匹配点采用双向匹配过滤和最小匹配点距离倍数判别筛选后，使用随机采样一致性［7］（RANSAC）算法剔除误匹配。同时对IMU数据进行处理，通过对相邻两帧的IMU数据预积分得到当前时刻的速度和位姿信息。

2 特征点提取与匹配

2.1 ORB特征点提取

测量数据预处理时，采用ORB特征提取处理相机图像。ORB特征是采用Oriented FAST关键点作为特征点检测方法，利用rBRIEF描述子作为特征点描述方法，前者是改进的FAST角点，后者是速度极快且具有旋转不变特性的二进制描述子［8］。

ORB特征提取时，在图片中随机选取某一个像素点设为p，像素点的灰度为Ip，设定一个合适的灰度阈值s，以p为圆心，以r为半径做圆，如图2所示，选取r=3的圆上的16个像素点。若在这16个点中有超过n（n通常取9、11、12）个点的灰度大于Ip+s或者小于Ip－s，则点p为FAST角点，n一般取值为12［9］。

为了满足尺度不变性，避免出现多个特征点聚集在一起的现象，需对图像帧建立尺度图像金字塔，如图3，在不同尺度的图像层进行栅格处理，对每个栅格使用FAST特征提取，最后使用四叉树的数据结构存储提取到的特征点，以保证特征点分布均匀。

为实现旋转不变特性，使用灰度质心法［10］计算提取到的每个FAST特征点的方向。选择某一图块A，几何中心设为O，灰度质心设为C

通过上述匹配，A中所有匹配成功的特征点ai都能在B中找到唯一且对应的特征点bi；遍历集合B后，B中所有匹配成功的特征点bi都能在A中找到唯一且对应的特征点ai。通过双向匹配过滤完成第一次误匹配筛选，即特征点ai的匹配点为特征点bi，同时特征点bi的匹配点为特征点ai，则认为该匹配结果正确，否则判定为误匹配，剔除。

将过滤后的所有匹配点对的Hamming距离进行排序，得到最小Hamming距离。当匹配点对之间的Hamming距离大于最小Hamming距离的设定倍数时，则认为匹配有误，需剔除，对特征点对再次筛选。但有时会出现最小Hamming距离过小的情况，导致筛选后的匹配点个数过少，所以设置一个经验值作为下限。本文算法取的设定倍数为5，经验值为30。

通过两次对误匹配的剔除，提高了剩余匹配点对的匹配质量，有效地减少了后续使用RANSAC算法的迭代次数。由图4可以看出，两次筛选后，交叉匹配线（误匹配对）明显减少。

3 实验与分析

为评估本文算法的性能，在开源数据集EuRoC上，对其定位精度、鲁棒性等进行仿真实验验证。实验硬件部分是CPU为Intel（R）Core（TM）i7-4720HQ@2.60GHz，内存为16 GB的PC，操作系统为Ubuntu18.04。本文算法的整体构架搭载于机器人操作系統ROS Melodic Morenia，基于开源算法VINS-Mono，利用OpenCV、Ceres、DboW2等开源库，开发语言为C++。通过ROS中的Rviz三维可视化工具对地图可视化分析，最后使用EVO评估工具对数据集运行结果和误差统计数据完成性能评估。

图5为本文算法在MH_01_easy序列上的绝对位姿误差（APE），将图5的轨迹投影到X-Z轴平面，进一步细化轨迹误差如图6所示。可知，当机体快速移动或发生较大角度转动时，由颜色条量化结果可知，误差水平在26 cm以内，平稳运动部份，误差水平缩小到1 cm左右。

为了进一步量化误差评估标准，使用均方根误差（RMSE）［11］比较改进前后算法的系统精度。旋转出现误差，平移上也会出现误差，所以本文仅考虑平移误差的情况作为评估指标。Pi∈SE3，i=1，…，n，表示所有关键帧真值位姿，Qi∈SE3，i=1，…，n，表示所有关键帧估计位姿，通过式（6）求解出最终的均方根误差

由表1数据可知，本文算法除了MH_04_diffcult序列以外，其余序列的绝对轨迹误差的均方根误差（RMSE）均低于0.200 m，在不同难度数据集序列上均有优良的精度表现。

通过表2的数据对比，本文算法在测试的7组不同难度数据集序列上APE均方根误差均优于原VINS-Mono算法，误差降低率为5%左右，在MH_02_easy中降低率可达6.75%，有效地提高了在不同场景下的系统定位精度。

4 结论

本文提出的基于VINS-Mono的改进ORB特征匹配筛选单目视觉—惯性SLAM算法，在特征点匹配环节新增了两次特征点误匹配筛选剔除。该算法通过在开源数据集EuRoC中的测试，不同光照难度的场景下与原VINS-Mono算法的系统误差相比系统精度均提高，有效地降低了光照环境因素对系统的干扰。

参考文献

［1］夏琳琳， 沈冉， 迟德儒，等. 一种基于光流—线特征的单目视觉—惯性SLAM算法［J］. 中国惯性技术学报， 2020， 28（5）： 568-575.

［2］李建禹. 基于单目视觉与IMU结合的SLAM技术研究［D］. 哈尔滨：哈尔滨工业大学， 2018.

［3］MOURIKIS A I， ROUMELIOTIS S I. A multi-state constraint Kalman filter for vision-aided inertial navigation［C］// IEEE International Conference on Robotics and Automation. Rome， 2007： 3565-3572.

［4］BLOESCH M， OMARI S， HUTTER M， et al. Robust visual inertial odometry using a direct EKF-based approach［C］// IEEE/RSJ International Conference on Intelligent Robots and Systems. Hamburg， 2015： 298-304.

［5］LEUTENEGGER S， LYNEN S， BOSSE M， et al. Keyframe-based visual-inertial odometry using nonlinear optimization［J］. International Journal of Robotics Research， 2015， 34（3）： 314-334.

［6］QIN T， LI P L， SHEN S J. VINS-Mono： A robust and versatile monocular visual-inertial state estimator［J］. IEEE Transactions on Robotics， 2018， 34（4）： 1004-1020.

［7］FISCHLER M A， BOLLES R C. Random sample consensus： A paradigm for model fitting with applications to image analysis and automated cartography［J］. Readings in Computer Vision， 1981， 24（6）： 381-395.

［8］RUBLEE E， RABAUD V， KONOLIGE K， et al. ORB： An efficient alternative to SIFT or SURF［C］// IEEE International Conference on Computer Vision（ICCV）. Barcelona， 2011： 2564-2571.

［9］孟特， 李富才， 劉邦彦， 等. 基于RGB-D图像的视觉SLAM算法研究［J］. 青岛大学学报（自然科学版）， 2022， 35（1）： 79-84.

［10］ CALONDER M， LEPETIT V， STRECHA C， et al. BRIEF： Binary robust independent elementary features［C］// 11th European Conference on Computer Vision. Heraklion， 2010： 778-792.

［11］ STURM J， ENGELHARD N， ENDRES F， et al. A benchmark for the evaluation of RGB-D SLAM systems［C］// 25th IEEE/RSJ International Conference on Intelligent Robots and Systems（IROS）. Algarve， 2012： 573-580.

Monocular Visual-inertial SLAM Algorithm Based on the

Improved ORB Feature Matching

ZHU Xiao-xuan， YANG Jie， HU Ji-gang

（College of Mechanical and Electrical Engineering， Qingdao University， Qingdao 266071， China）

Abstract： Aiming at the problems of unstable operation and large accumulated error of VINS-Mono algorithm in weak texture environment， a monocular visual-inertial SLAM algorithm based on improved ORB feature matching and filtering was proposed. In the preprocessing of the measurement data， ORB feature extraction algorithm was adopted. In the feature point matching， two-way matching filtering and minimum matching point distance multiple discrimination were used to eliminate false matching. Finally， RANSAC was used to complete feature matching. The experimental results show that the improved VINS-Mono algorithm has improved accuracy compared to the original algorithm.

Keywords： SLAM; feature point extraction; feature point matching; match point filtering

收稿日期：2022-06-07

通信作者：楊杰，男，副教授，主要研究方向为基于物联网的嵌入式系统与应用图像识别技术。E-mail： yangjie@qdu.edu.cn