祝曉軒　楊杰　胡繼港

摘要：針對(duì)VINS-Mono算法在弱紋理環(huán)境下運(yùn)行不穩(wěn)定、累積誤差大等問(wèn)題，提出了改進(jìn)ORB特征匹配篩選的單目視覺(jué)—慣性SLAM算法。測(cè)量數(shù)據(jù)預(yù)處理環(huán)節(jié)采用了ORB特征提取，在特征點(diǎn)匹配時(shí)，使用雙向匹配過(guò)濾和最小匹配點(diǎn)距離倍數(shù)判別剔除誤匹配，最后利用隨機(jī)采樣一致性算法完成特征匹配。實(shí)驗(yàn)結(jié)果表明，與原算法相比，改進(jìn)VINS-Mono算法的精度較高。

關(guān)鍵詞：SLAM；特征點(diǎn)提??；特征點(diǎn)匹配；匹配點(diǎn)篩選

中圖分類(lèi)號(hào)：TP242 文獻(xiàn)標(biāo)志碼：A

即時(shí)定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）指在機(jī)器人或其他載體上對(duì)各種傳感器數(shù)據(jù)進(jìn)行采集并計(jì)算，完成對(duì)其自身位姿的定位并生成周?chē)鷪?chǎng)景地圖信息。作為機(jī)器人導(dǎo)航技術(shù)研究的熱點(diǎn)之一，視覺(jué)SLAM使用的相機(jī)比激光SLAM使用的激光雷達(dá)價(jià)格低，且采集的圖像信息更為豐富，相比傳統(tǒng)的全球衛(wèi)星導(dǎo)航系統(tǒng)，視覺(jué)SLAM系統(tǒng)既可以完成高精度的定位，又可以實(shí)時(shí)生成周?chē)h(huán)境地圖［1］。但由于缺少深度信息，單目視覺(jué)SLAM存在尺度模糊性問(wèn)題，因此引入了慣性測(cè)量單元（Inertial Measurement Unit， IMU）。IMU能夠測(cè)量傳感器本身的角速度和加速度，可以與視覺(jué)傳感器采集的數(shù)據(jù)進(jìn)行互補(bǔ)，構(gòu)建出魯棒性更強(qiáng)的視覺(jué)—慣性SLAM系統(tǒng)［2］。MSCKF［3］算法和ROVIO［4］算法均是單目相機(jī)和IMU緊耦合的視覺(jué)慣導(dǎo)融合SLAM算法，但前者的后端優(yōu)化基于擴(kuò)展卡爾曼濾波器（Extended Kalman Filter， EKF），后者則基于迭代擴(kuò)展卡爾曼濾波（Iterated Extended Kalman Filter， IEKF）。從近年來(lái)國(guó)內(nèi)外經(jīng)典的視覺(jué)—慣性SLAM算法系統(tǒng)中可以看出，非線性優(yōu)化已經(jīng)成為視覺(jué)—慣性SLAM后端方案的主流理論。非線性優(yōu)化通過(guò)迭代優(yōu)化狀態(tài)估計(jì)，通常采用降低計(jì)算復(fù)雜度的滑動(dòng)窗口優(yōu)化策略。OKVIS［5］算法和VINS-Mono［6］算法均使用非線性優(yōu)化作為系統(tǒng)后端方案。相比于使用雙目相機(jī)的OKVIS，VINS-Mono具有更加完善和魯棒的初始化、重定位、閉環(huán)檢測(cè)以及全局位姿圖優(yōu)化等環(huán)節(jié)，定位精度更高，魯棒性更加優(yōu)良。但由于VINS-Mono的前端測(cè)量數(shù)據(jù)預(yù)處理中采用光流法，導(dǎo)致在弱紋理及復(fù)雜環(huán)境下存在運(yùn)行不穩(wěn)定且累積誤差大，本文針對(duì)開(kāi)源的VINS-Mono算法的前端部分進(jìn)行改進(jìn)。

1 系統(tǒng)框架構(gòu)建

基于VINS-Mono算法，本文算法采用緊耦合方案，后端使用非線性優(yōu)化完成視覺(jué)與慣性信息的深度融合，系統(tǒng)框架如圖1所示，分為五部分：測(cè)量數(shù)據(jù)預(yù)處理、系統(tǒng)初始化、后端滑動(dòng)窗口優(yōu)化、回環(huán)檢測(cè)與重定位、全局位姿圖優(yōu)化。

在測(cè)量數(shù)據(jù)預(yù)處理模塊中，相機(jī)圖像處理環(huán)節(jié)與原VINS-Mono算法不同，本文算法是對(duì)提取圖像進(jìn)行金字塔處理，然后提取ORB特征點(diǎn)，使用特征點(diǎn)法匹配相鄰幀的特征點(diǎn)，對(duì)匹配點(diǎn)采用雙向匹配過(guò)濾和最小匹配點(diǎn)距離倍數(shù)判別篩選后，使用隨機(jī)采樣一致性［7］（RANSAC）算法剔除誤匹配。同時(shí)對(duì)IMU數(shù)據(jù)進(jìn)行處理，通過(guò)對(duì)相鄰兩幀的IMU數(shù)據(jù)預(yù)積分得到當(dāng)前時(shí)刻的速度和位姿信息。

2 特征點(diǎn)提取與匹配

2.1 ORB特征點(diǎn)提取

測(cè)量數(shù)據(jù)預(yù)處理時(shí)，采用ORB特征提取處理相機(jī)圖像。ORB特征是采用Oriented FAST關(guān)鍵點(diǎn)作為特征點(diǎn)檢測(cè)方法，利用rBRIEF描述子作為特征點(diǎn)描述方法，前者是改進(jìn)的FAST角點(diǎn)，后者是速度極快且具有旋轉(zhuǎn)不變特性的二進(jìn)制描述子［8］。

ORB特征提取時(shí)，在圖片中隨機(jī)選取某一個(gè)像素點(diǎn)設(shè)為p，像素點(diǎn)的灰度為Ip，設(shè)定一個(gè)合適的灰度閾值s，以p為圓心，以r為半徑做圓，如圖2所示，選取r=3的圓上的16個(gè)像素點(diǎn)。若在這16個(gè)點(diǎn)中有超過(guò)n（n通常取9、11、12）個(gè)點(diǎn)的灰度大于Ip+s或者小于Ip－s，則點(diǎn)p為FAST角點(diǎn)，n一般取值為12［9］。

為了滿足尺度不變性，避免出現(xiàn)多個(gè)特征點(diǎn)聚集在一起的現(xiàn)象，需對(duì)圖像幀建立尺度圖像金字塔，如圖3，在不同尺度的圖像層進(jìn)行柵格處理，對(duì)每個(gè)柵格使用FAST特征提取，最后使用四叉樹(shù)的數(shù)據(jù)結(jié)構(gòu)存儲(chǔ)提取到的特征點(diǎn)，以保證特征點(diǎn)分布均勻。

為實(shí)現(xiàn)旋轉(zhuǎn)不變特性，使用灰度質(zhì)心法［10］計(jì)算提取到的每個(gè)FAST特征點(diǎn)的方向。選擇某一圖塊A，幾何中心設(shè)為O，灰度質(zhì)心設(shè)為C

通過(guò)上述匹配，A中所有匹配成功的特征點(diǎn)ai都能在B中找到唯一且對(duì)應(yīng)的特征點(diǎn)bi；遍歷集合B后，B中所有匹配成功的特征點(diǎn)bi都能在A中找到唯一且對(duì)應(yīng)的特征點(diǎn)ai。通過(guò)雙向匹配過(guò)濾完成第一次誤匹配篩選，即特征點(diǎn)ai的匹配點(diǎn)為特征點(diǎn)bi，同時(shí)特征點(diǎn)bi的匹配點(diǎn)為特征點(diǎn)ai，則認(rèn)為該匹配結(jié)果正確，否則判定為誤匹配，剔除。

將過(guò)濾后的所有匹配點(diǎn)對(duì)的Hamming距離進(jìn)行排序，得到最小Hamming距離。當(dāng)匹配點(diǎn)對(duì)之間的Hamming距離大于最小Hamming距離的設(shè)定倍數(shù)時(shí)，則認(rèn)為匹配有誤，需剔除，對(duì)特征點(diǎn)對(duì)再次篩選。但有時(shí)會(huì)出現(xiàn)最小Hamming距離過(guò)小的情況，導(dǎo)致篩選后的匹配點(diǎn)個(gè)數(shù)過(guò)少，所以設(shè)置一個(gè)經(jīng)驗(yàn)值作為下限。本文算法取的設(shè)定倍數(shù)為5，經(jīng)驗(yàn)值為30。

通過(guò)兩次對(duì)誤匹配的剔除，提高了剩余匹配點(diǎn)對(duì)的匹配質(zhì)量，有效地減少了后續(xù)使用RANSAC算法的迭代次數(shù)。由圖4可以看出，兩次篩選后，交叉匹配線（誤匹配對(duì)）明顯減少。

3 實(shí)驗(yàn)與分析

為評(píng)估本文算法的性能，在開(kāi)源數(shù)據(jù)集EuRoC上，對(duì)其定位精度、魯棒性等進(jìn)行仿真實(shí)驗(yàn)驗(yàn)證。實(shí)驗(yàn)硬件部分是CPU為Intel（R）Core（TM）i7-4720HQ@2.60GHz，內(nèi)存為16 GB的PC，操作系統(tǒng)為Ubuntu18.04。本文算法的整體構(gòu)架搭載于機(jī)器人操作系統(tǒng)ROS Melodic Morenia，基于開(kāi)源算法VINS-Mono，利用OpenCV、Ceres、DboW2等開(kāi)源庫(kù)，開(kāi)發(fā)語(yǔ)言為C++。通過(guò)ROS中的Rviz三維可視化工具對(duì)地圖可視化分析，最后使用EVO評(píng)估工具對(duì)數(shù)據(jù)集運(yùn)行結(jié)果和誤差統(tǒng)計(jì)數(shù)據(jù)完成性能評(píng)估。

圖5為本文算法在MH_01_easy序列上的絕對(duì)位姿誤差（APE），將圖5的軌跡投影到X-Z軸平面，進(jìn)一步細(xì)化軌跡誤差如圖6所示?？芍?dāng)機(jī)體快速移動(dòng)或發(fā)生較大角度轉(zhuǎn)動(dòng)時(shí)，由顏色條量化結(jié)果可知，誤差水平在26 cm以內(nèi)，平穩(wěn)運(yùn)動(dòng)部份，誤差水平縮小到1 cm左右。

為了進(jìn)一步量化誤差評(píng)估標(biāo)準(zhǔn)，使用均方根誤差（RMSE）［11］比較改進(jìn)前后算法的系統(tǒng)精度。旋轉(zhuǎn)出現(xiàn)誤差，平移上也會(huì)出現(xiàn)誤差，所以本文僅考慮平移誤差的情況作為評(píng)估指標(biāo)。Pi∈SE3，i=1，…，n，表示所有關(guān)鍵幀真值位姿，Qi∈SE3，i=1，…，n，表示所有關(guān)鍵幀估計(jì)位姿，通過(guò)式（6）求解出最終的均方根誤差

由表1數(shù)據(jù)可知，本文算法除了MH_04_diffcult序列以外，其余序列的絕對(duì)軌跡誤差的均方根誤差（RMSE）均低于0.200 m，在不同難度數(shù)據(jù)集序列上均有優(yōu)良的精度表現(xiàn)。

通過(guò)表2的數(shù)據(jù)對(duì)比，本文算法在測(cè)試的7組不同難度數(shù)據(jù)集序列上APE均方根誤差均優(yōu)于原VINS-Mono算法，誤差降低率為5%左右，在MH_02_easy中降低率可達(dá)6.75%，有效地提高了在不同場(chǎng)景下的系統(tǒng)定位精度。

4 結(jié)論

本文提出的基于VINS-Mono的改進(jìn)ORB特征匹配篩選單目視覺(jué)—慣性SLAM算法，在特征點(diǎn)匹配環(huán)節(jié)新增了兩次特征點(diǎn)誤匹配篩選剔除。該算法通過(guò)在開(kāi)源數(shù)據(jù)集EuRoC中的測(cè)試，不同光照難度的場(chǎng)景下與原VINS-Mono算法的系統(tǒng)誤差相比系統(tǒng)精度均提高，有效地降低了光照環(huán)境因素對(duì)系統(tǒng)的干擾。

參考文獻(xiàn)

［1］夏琳琳， 沈冉， 遲德儒，等. 一種基于光流—線特征的單目視覺(jué)—慣性SLAM算法［J］. 中國(guó)慣性技術(shù)學(xué)報(bào)， 2020， 28（5）： 568-575.

［2］李建禹. 基于單目視覺(jué)與IMU結(jié)合的SLAM技術(shù)研究［D］. 哈爾濱：哈爾濱工業(yè)大學(xué)， 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圖像的視覺(jué)SLAM算法研究［J］. 青島大學(xué)學(xué)報(bào)（自然科學(xué)版）， 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

通信作者：楊杰，男，副教授，主要研究方向?yàn)榛谖锫?lián)網(wǎng)的嵌入式系統(tǒng)與應(yīng)用圖像識(shí)別技術(shù)。E-mail： yangjie@qdu.edu.cn