運動曲率自適應V-SLAM算法研究

趙榮亮，王紅旗，劉群坡，張 巖

運動曲率自適應V-SLAM算法研究

趙榮亮，王紅旗，劉群坡，張 巖

（河南理工大學電氣工程與自動化學院，河南 焦作 454000）

傳統室內視覺同時定位與建圖（V-SLAM）存在特征點誤匹配剔除耗時長、較大轉彎時定位精度低等問題。為了提升匹配速度，提出一種基于特征點的余弦相似度直方圖剔除誤匹配方法。為了避免大幅度轉彎時關鍵幀丟失，提出一種基于運動曲率的關鍵幀自適應提取V-SLAM算法（MSAV-SLAM）。該算法通過高效多點透視（EPnP）計算幀間位姿，以融合了運動曲率、幀間位移和旋轉矩陣的復合運動量為參考變量，自適應選擇關鍵幀。室內數據集實驗表明，所提出的算法與基于固定時間、空間和原ORB-SLAM2算法相比，平均每幀追蹤時間減少為原算法的57.62%，且絕對軌跡誤差為改進前的81.45%。

視覺同步定位與地圖創建；關鍵幀；特征點匹配；運動曲率；室內定位

0 引言

近年來，室內同時定位與建圖（simultaneous localization and mapping，SLAM）進展很大，在室內服務機器人、智慧工廠和智能倉儲[1]等領域發揮著重要作用。根據室內定位SLAM系統的傳感器類型可分為視覺同時定位與建圖（visual-SLAM，V-SLAM）和激光SLAM 2大類，與激光傳感器相比，相機獲取的信息豐富，便于提取環境中深層次的信息，因此V-SLAM成為當前的研究熱點[2]。而在V-SLAM中，依據視覺前端對相機圖像的處理方式，一般可分為特征點法和直接法。直接法通過最小化光度誤差的方法計算相機的運動軌跡，計算速度很快，但光照變化對其影響很大。特征點法需要計算每幀圖像中的特征點與描述子，對環境中光照強度和尺度等因素的變化不敏感[3]。

文獻[4]提出了首個基于單目相機的單目同時定位與建圖算法（monocular simultaneous localization and mapping, MonoSLAM），利用擴展卡爾曼濾波器（extended Kalman filter, EKF）實時計算出相機的運動軌跡。但隨著地圖規模變大，系統狀態變量維數不斷增多，計算復雜度會急速增加。文獻[5]提出的并行追蹤與建圖（parallel tracking and mapping,PTAM）通過光束法平差（bundle adjustment, BA）優化關鍵幀，避免對每幀圖像進行全局優化，并將追蹤線程與建圖線程分離，可以進行較大規模建圖。2015年，文獻[6]在PTAM算法的基礎上進行大量改進，提出了尺度不變特征變換同時定位建圖算法（oriented fast and rotated brief-SLAM，ORB-SLAM），采用寬松的關鍵幀選擇和剔除機制，以減少追蹤丟失。文獻[7]增加了雙目相機和深度相機的接口，優化了特征點的存儲方式，為減少誤匹配點對位姿求解的干擾，使用隨機采樣一致法（random sample consensus, RANSAC）計算最優位姿。文獻[8]在其基礎上使用漸進采樣一致算法（progressive sample consensus，PROSAC），緩解了前端的計算壓力，但對特征點要求較高。

V-SLAM中的關鍵幀機制是影響系統精度的重要因素，吸引了許多研究者的注意。傳統關鍵幀選擇算法如基于固定時間、空間等，這2種算法都屬于在時域或空域的均勻采樣[9]，計算速度快，在相機運動軌跡簡單時表現良好，但忽略了關鍵幀之間的共視關系，定位效果很差。ORB-SLAM2結合時間和空間等特征設置多個判斷條件選擇關鍵幀[10]，避免了上述問題；但在相機旋轉角度大時易漏選關鍵幀，定位精度降低，甚至造成追蹤失敗。文獻[11]根據非關鍵幀滑動窗口幀間約束關系對關鍵幀篩選機制進行優化，減少了后端優化的計算量。文獻[12]通過同時維護直接法與特征點法選擇的2種關鍵幀，保證了系統的全局一致性。文獻[13]對關鍵幀再次篩選，解決了往返運動存在重復關鍵幀的問題。

綜上所述，ORB-SLAM2使用RANSAC算法多次迭代確立最優運動模型，特征點的計算復雜，耗時較長；當相機旋轉角度大時易漏選關鍵幀。本文提出的運動狀態自適應的視覺同時定位與建圖算法（motion state adaptive V-SLAM，MSAV-SLAM），在特征追蹤階段先計算出特征點方向向量，構建余弦相似度直方圖，對誤匹配進行初步剔除，以降低迭代次數，減少特征點的計算量。在局部優化階段，使用運動曲率自適應的關鍵幀選取策略，可以避免關鍵幀漏失，提升室內定位精度。

1 V-SLAM算法框架

本文在ORB-SLAM2算法框架的基礎上進行改進，主要由追蹤模塊、局部建圖模塊和回環檢測模塊3部分組成。

追蹤模塊先提取出每幀圖像中的ORB特征點，再根據對應幀之間匹配成功的特征點分別計算基礎矩陣與單應矩陣，并選擇運動追蹤模型求解初始位姿與特征點在地圖中的投影點坐標。本文算法先計算出特征點的主方向向量，再建立余弦相似度直方圖進行誤匹配剔除。局部建圖模塊使用運動曲率自適應的關鍵幀選取機制。在相機旋轉幅度大時可準確選取關鍵幀，再根據關鍵幀進行局部優化和地圖創建。回環檢測模塊通過詞袋模型獲取當前幀與關鍵幀之間的共視關系，檢測是否發生回環。若發生回環，則根據該幀與地圖的約束關系進行全局BA優化修正整體運動軌跡，減少累積漂移。

2 誤匹配剔除與關鍵幀選取

2.1 余弦相似度直方圖誤匹配剔除

特征點匹配是室內V-SLAM算法實現圖像幀之間數據關聯的關鍵步驟，準確的特征點匹配對前端求解位姿以及后端的全局優化十分重要；但是在特征點匹配時常常會出現錯誤的匹配，需要進行誤匹配剔除。一般的，匹配正確的特征點遠多于誤匹配的點，且在發生旋轉時對應特征點的方向一致；而在追蹤過程中相鄰二幀的相機旋轉角度小于90°，否則視角會完全丟失，無法進行定位。余弦相似度可以衡量2個向量方向的相似程度，因此，本文利用相鄰圖像幀之間的特征點方向向量的余弦相似度直方圖來剔除誤匹配的特征點。

圖1 未剔除誤匹配特征點

圖2 剔除誤匹配特征點

2.2 運動曲率自適應關鍵幀選擇策略

V-SLAM中關鍵幀直接影響系統的性能，一般從以下二方面選取關鍵幀：一方面是關鍵幀必須是清晰的圖像，且擁有分布均勻的高質量特征點，

能滿足建圖和位姿優化的要求；另一方面是關鍵幀必須能夠充分代表相鄰普通幀，且要與其他關鍵幀有一定的共視關系。

代入針孔相機模型為

圖3 運動曲率自適應選取關鍵幀

3 實驗與結果分析

本文所有實驗在處理器為AMD Ryzen74800U，內存4 GB，操作系統為Ubuntu 16.04.6 LTS 64-bit的計算機上進行。為驗證算法在室內定位的性能，先確立評價算法定位精度與實時性的指標。在歐盟機器人挑戰賽（European robotics challenge，EuEoC）的室內數據集上進行實驗，并與多種關鍵幀選擇算法進行對比。

3.1 評估標準

本文采用絕對軌跡誤差（absolute trajectory error，ATE）的平均值評價定位精度，在慕尼黑工業大學（Technische Universit?t München，TUM）數據集的基準測試中首次使用該指標。使用評估里程計(evaluation of odometry and SLAM，EVO）工具計算不同關鍵幀算法的ATE，并繪制軌跡圖。SLAM算法在追蹤階段計算相機位姿耗時較長，是影響系統整體效率的關鍵因素。因此選用SLAM系統在追蹤階段處理每幀圖像的平均時間作為實時性指標。為避免參數調整的影響，在不同關鍵幀選取算法中，每幀圖像的特征點提取數量等參數都使用默認值。

3.2 結果與分析

EuRoc數據集由蘇黎世聯邦理工學院的無人駕駛系統實驗室發布，包含微型無人機在真實廠房環境和2個不同房間內采集并對齊后的雙目圖像。相機使用全局快門曝光，采集幀率為20 Hz，分辨率為752×480個像素。無人機的真實運動軌跡由動作捕捉系統提供，誤差在1 mm左右。根據無人機飛行速度和環境等因素，將不同序列分為簡單、中等和困難3個等級。

為避免系統波動的影響，每種算法運行10次以上，取中位數進行對比。基于時間與空間的算法在追蹤模塊與原ORB-SLAM2算法相同，因此只對比本文算法與原ORB-SLAM2算法在追蹤模塊每幀圖像的處理時間。2種算法在MH-05序列上每幀圖像的追蹤時間對比如圖4所示。可以看出，本文算法耗時整體上低于原算法。

圖4 追蹤時間對比

為驗證本文算法在特征提取模塊的速度優勢，使用EuRoC數據集中不同場景下的序列進行實驗，包括光線較暗的廠房內的MH系列和2個不同房間內的V1、V2系列，結果如表1所示。

表1 不同序列平均每幀追蹤時間 ms

從表1可知：在不同場景下，本文算法比ORB-SLAM2算法速度都有提升，平均追蹤時間為原算法的57.62%；而MH-05序列的光線較暗，提取的特征點數量少，因此速度提升較小。

為驗證本文算法在相機轉彎幅度較大時的性能，選取運動速度較快、軌跡復雜、困難等級的MH-05序列進行實驗。該序列飛行距離為97.8 m，時間為111 s，平均速度為0.88 m/s。分別使用時域均勻、空域均勻、原ORB-SALM2和融合曲率 4種不同的關鍵幀機制實驗。為避免關鍵幀數量的影響，保證基于時域和空域算法所選取的關鍵幀數量與融合曲率后選取的數量大致相等，約占總幀數的20%。融合曲率后選取的關鍵幀數量略大于原關鍵幀策略，不影響系統整體。4種算法的估計軌跡與軌跡真值的對比如圖5所示。

圖5中虛線GroundTruth代表數據集提供的軌跡真值；曲線D-SLAM為基于固定距離選擇關鍵幀算法的軌跡；曲線T-SLAM為基于固定時間選擇關鍵幀算法的軌跡；以及本文MSAV-SLAM的軌跡曲線，和原ORB-SLAM2的軌跡曲線都有表示。整體來看，基于固定時間與空間的算法定位精度較低。無人機在和處的旋轉幅度較大；從局部放大圖中可以直觀地看出，本文算法比其他3種算法更接近軌跡真值。

圖5 不同關鍵幀選擇策略的運動軌跡

通過EVO工具計算4種不同關鍵幀選取算法的ATE，如表2所示，T-SLAM和D-SLAM分別為固定時間和空間的關鍵幀選擇算法，加粗表示該項最低值。

表2 不同關鍵幀選擇策略的絕對軌跡誤差（ATE） mm

從表2可以看出：由于無人機的速度變化大，因此基于時間選擇關鍵幀的T-SLAM算法精度最低；基于空間的D-SLAM算法受速度影響較小，精度高于T-SLAM算法；本文MSAV-SLAM與其他3種算法相比絕對軌跡誤差的平均值最小，最優結果僅為原ORB-SLAM2算法誤差的81.45%。

綜合各項指標，本文的MSAV-SLAM算法在不同的室內場景中，在特征追蹤模塊的耗時優于ORB-SLAM2，且相機軌跡更加符合真值，可以滿足V-SLAM系統實時性和定位精度的要求。

4 結束語

本文的MSAV-SLAM算法根據余弦相似度剔除誤匹配點，可以減少特征點的運算量，提升室內定位系統效率。使用運動曲率自適應的關鍵幀選擇機制，解決了ORB-SLAM2算法室內在轉彎幅度較大時定位精度降低的問題。選取室內EuRoC數據集上相機旋轉角度大、運動軌跡復雜的序列進行實驗。結果表明，本文的算法定位精度和追蹤速度均優于原ORB-SLAM2算法。

[1] 王雅儀, 余萌, 朱鋒. 線面特征結合的三維激光SLAM算法及性能評估[J]. 導航定位學報. 2021, 9(1): 122-129.

[2] TAKETOMI T, UCHIYAMA H, IKEDA S. Visual SLAM algorithms: a survey from 2010 to 2016[J]. Ipsj Transactions on Computer Vision & Applications. 2017, 32(1): 1-19.

[3] 方琪, 王曉華, 蘇杰. 基于分組策略的點線特征融合同步定位與地圖構建算法[J]. 激光與光電子學進展, 2021, 58(14): 405-413.

[4] DAVISON A J, REID I D, MOLTON N D, et al. MonoSLAM: real-time single camera SLAM[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence. 2007, 29(6): 1052-1067.

[5] KLEIN G, MURRAY D. Parallel tracking and mapping for small AR workspaces[EB/OL]. [2022-05-29]. http: //www. robots. ox. ac. uk/～gk/publications/KleinMurray2007ISMAR. pdf.

[6] MUR-ARTAL R, MONTIEL J M M, TARDOS J D. ORB-SLAM: a versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics, 2015, 31(5): 1147-1163.

[7] MUR-ARTAL R, TARDOS J D. ORB-SLAM2: an open-source SLAM system for monocular, stereo and RGB-D cameras[J]. IEEE Transactions on Robotics, 2017, 33(5): 1255-1262.

[8] 曹蜜, 胡凌燕, 熊彭文, 等. 基于PROSAC算法與ORB-SLAM2的RGB-D相機室內視覺定位研究[J]. 傳感技術學報, 2019, 32(11): 1706-1712.

[9] 張新良, 李陽, 趙運基. 一種基于時空切片的SLAM關鍵幀提取方法[J]. 系統仿真學報. 2018, 30(5): 1724-1729.

[10] ZIELI?SKI K, STASZAK R, NOWACZYK M, et al. 3D dense mapping with the graph of keyframe-based and view-dependent local maps[J]. Journal of Intelligent & Robotic Systems. 2021, 103(2): 28.

[11] 張小國, 劉啟漢, 李尚哲, 等. 基于雙步邊緣化與關鍵幀篩選的改進視覺慣性SLAM方法[J]. 中國慣性技術學報, 2020, 28(5): 608-614.

[12] 谷曉琳, 楊敏, 張燚, 等. 一種基于半直接視覺里程計的RGB-D SLAM算法[J]. 機器人, 2020, 42(1): 39-48.

[13] XIE P, SU W, LI B, et al. Modified keyframe selection algorithm and map visualization based on ORB-SLAM2[C]//TheInstitute of Electrical and Electronic Engineers (IEEE). Proceedings of the 20204th International Conference on Robotics and Automation Sciences. Wuhan, China: IEEE, 2020: 142-147[2022-05-29].

Motion curvature adaptive V-SLAM algorithm research

ZHAO Rongliang, WANG Hongqi, LIU Qunpo, ZHANG Yan

（School of Electrical Engineering and Automation, Henan Polytechnic University, Jiaozuo, Henan 454000，China）

Traditional indoor vision simultaneous localization and mapping (V-SLAM) has many problems, such as time-consuming feature point mismatch elimination and low localization accuracy in large turns. In order to improve the matching speed, a method of eliminating false matching based on cosine similarity histogram of feature points is proposed. In order to avoid the loss of key frames in large turns, a motion state adaptive key frame extraction V-SALM (MSAV-SLAM) algorithm was proposed. This algorithm calculates the interframe pose by efficient perspective-n-point (EPnP), and adaptively selects key frames by taking the combined motion curvature, interframe displacement and the compound motion of rotation matrix as reference variables. Laboratory data set experiments show that compared with the original ORB-SLAM2 algorithm based on fixed time and space, the average tracking time per frame of the proposed algorithm is reduced to 57.62%, and the absolute trajectory error is 81.45%.

visual simultaneous localization and mapping; key frame; feature point match; motion curvature; indoor localisation

TP242

A

2095-4999(2022)06-0053-06

趙榮亮，王紅旗，劉群坡，等. 運動曲率自適應V-SLAM算法研究[J]. 導航定位學報, 2022, 10(6): 53-58.（ZHAO Rongliang, WANG Hongqi, LIU Qunpo, et al. Motion curvature adaptive V-SLAM algorithm research[J]. Journal of Navigation and Positioning, 2022, 10(6): 53-58.）

10.16547/j.cnki.10-1096.20220607.

2022-05-25

趙榮亮（1997—），男，河南南陽人，碩士研究生，研究方向為基于視覺的移動機器人定位。

王紅旗（1973—），男，河南周口人，博士，副教授，碩士研究生導師，研究方向為機器人控制。