基于改進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