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

運(yùn)動(dòng)曲率自適應(yīng)V-SLAM算法研究

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

（河南理工大學(xué)電氣工程與自動(dòng)化學(xué)院，河南 焦作 454000）

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

視覺(jué)同步定位與地圖創(chuàng)建；關(guān)鍵幀；特征點(diǎn)匹配；運(yùn)動(dòng)曲率；室內(nèi)定位

0 引言

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

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

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

綜上所述，ORB-SLAM2使用RANSAC算法多次迭代確立最優(yōu)運(yùn)動(dòng)模型，特征點(diǎn)的計(jì)算復(fù)雜，耗時(shí)較長(zhǎng)；當(dāng)相機(jī)旋轉(zhuǎn)角度大時(shí)易漏選關(guān)鍵幀。本文提出的運(yùn)動(dòng)狀態(tài)自適應(yīng)的視覺(jué)同時(shí)定位與建圖算法（motion state adaptive V-SLAM，MSAV-SLAM），在特征追蹤階段先計(jì)算出特征點(diǎn)方向向量，構(gòu)建余弦相似度直方圖，對(duì)誤匹配進(jìn)行初步剔除，以降低迭代次數(shù)，減少特征點(diǎn)的計(jì)算量。在局部?jī)?yōu)化階段，使用運(yùn)動(dòng)曲率自適應(yīng)的關(guān)鍵幀選取策略，可以避免關(guān)鍵幀漏失，提升室內(nèi)定位精度。

1 V-SLAM算法框架

本文在ORB-SLAM2算法框架的基礎(chǔ)上進(jìn)行改進(jìn)，主要由追蹤模塊、局部建圖模塊和回環(huán)檢測(cè)模塊3部分組成。

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

2 誤匹配剔除與關(guān)鍵幀選取

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

特征點(diǎn)匹配是室內(nèi)V-SLAM算法實(shí)現(xiàn)圖像幀之間數(shù)據(jù)關(guān)聯(lián)的關(guān)鍵步驟，準(zhǔn)確的特征點(diǎn)匹配對(duì)前端求解位姿以及后端的全局優(yōu)化十分重要；但是在特征點(diǎn)匹配時(shí)常常會(huì)出現(xiàn)錯(cuò)誤的匹配，需要進(jìn)行誤匹配剔除。一般的，匹配正確的特征點(diǎn)遠(yuǎn)多于誤匹配的點(diǎn)，且在發(fā)生旋轉(zhuǎn)時(shí)對(duì)應(yīng)特征點(diǎn)的方向一致；而在追蹤過(guò)程中相鄰二幀的相機(jī)旋轉(zhuǎn)角度小于90°，否則視角會(huì)完全丟失，無(wú)法進(jìn)行定位。余弦相似度可以衡量2個(gè)向量方向的相似程度，因此，本文利用相鄰圖像幀之間的特征點(diǎn)方向向量的余弦相似度直方圖來(lái)剔除誤匹配的特征點(diǎn)。

圖1 未剔除誤匹配特征點(diǎn)

圖2 剔除誤匹配特征點(diǎn)

2.2 運(yùn)動(dòng)曲率自適應(yīng)關(guān)鍵幀選擇策略

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

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

代入針孔相機(jī)模型為

圖3 運(yùn)動(dòng)曲率自適應(yīng)選取關(guān)鍵幀

3 實(shí)驗(yàn)與結(jié)果分析

本文所有實(shí)驗(yàn)在處理器為AMD Ryzen74800U，內(nèi)存4 GB，操作系統(tǒng)為Ubuntu 16.04.6 LTS 64-bit的計(jì)算機(jī)上進(jìn)行。為驗(yàn)證算法在室內(nèi)定位的性能，先確立評(píng)價(jià)算法定位精度與實(shí)時(shí)性的指標(biāo)。在歐盟機(jī)器人挑戰(zhàn)賽（European robotics challenge，EuEoC）的室內(nèi)數(shù)據(jù)集上進(jìn)行實(shí)驗(yàn)，并與多種關(guān)鍵幀選擇算法進(jìn)行對(duì)比。

3.1 評(píng)估標(biāo)準(zhǔn)

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

3.2 結(jié)果與分析

EuRoc數(shù)據(jù)集由蘇黎世聯(lián)邦理工學(xué)院的無(wú)人駕駛系統(tǒng)實(shí)驗(yàn)室發(fā)布，包含微型無(wú)人機(jī)在真實(shí)廠房環(huán)境和2個(gè)不同房間內(nèi)采集并對(duì)齊后的雙目圖像。相機(jī)使用全局快門曝光，采集幀率為20 Hz，分辨率為752×480個(gè)像素。無(wú)人機(jī)的真實(shí)運(yùn)動(dòng)軌跡由動(dòng)作捕捉系統(tǒng)提供，誤差在1 mm左右。根據(jù)無(wú)人機(jī)飛行速度和環(huán)境等因素，將不同序列分為簡(jiǎn)單、中等和困難3個(gè)等級(jí)。

為避免系統(tǒng)波動(dòng)的影響，每種算法運(yùn)行10次以上，取中位數(shù)進(jìn)行對(duì)比?；跁r(shí)間與空間的算法在追蹤模塊與原ORB-SLAM2算法相同，因此只對(duì)比本文算法與原ORB-SLAM2算法在追蹤模塊每幀圖像的處理時(shí)間。2種算法在MH-05序列上每幀圖像的追蹤時(shí)間對(duì)比如圖4所示。可以看出，本文算法耗時(shí)整體上低于原算法。

圖4 追蹤時(shí)間對(duì)比

為驗(yàn)證本文算法在特征提取模塊的速度優(yōu)勢(shì)，使用EuRoC數(shù)據(jù)集中不同場(chǎng)景下的序列進(jìn)行實(shí)驗(yàn)，包括光線較暗的廠房?jī)?nèi)的MH系列和2個(gè)不同房間內(nèi)的V1、V2系列，結(jié)果如表1所示。

表1 不同序列平均每幀追蹤時(shí)間 ms

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

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

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

圖5 不同關(guān)鍵幀選擇策略的運(yùn)動(dòng)軌跡

通過(guò)EVO工具計(jì)算4種不同關(guān)鍵幀選取算法的ATE，如表2所示，T-SLAM和D-SLAM分別為固定時(shí)間和空間的關(guān)鍵幀選擇算法，加粗表示該項(xiàng)最低值。

表2 不同關(guān)鍵幀選擇策略的絕對(duì)軌跡誤差（ATE） mm

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

綜合各項(xiàng)指標(biāo)，本文的MSAV-SLAM算法在不同的室內(nèi)場(chǎng)景中，在特征追蹤模塊的耗時(shí)優(yōu)于ORB-SLAM2，且相機(jī)軌跡更加符合真值，可以滿足V-SLAM系統(tǒng)實(shí)時(shí)性和定位精度的要求。

4 結(jié)束語(yǔ)

本文的MSAV-SLAM算法根據(jù)余弦相似度剔除誤匹配點(diǎn)，可以減少特征點(diǎn)的運(yùn)算量，提升室內(nèi)定位系統(tǒng)效率。使用運(yùn)動(dòng)曲率自適應(yīng)的關(guān)鍵幀選擇機(jī)制，解決了ORB-SLAM2算法室內(nèi)在轉(zhuǎn)彎幅度較大時(shí)定位精度降低的問(wèn)題。選取室內(nèi)EuRoC數(shù)據(jù)集上相機(jī)旋轉(zhuǎn)角度大、運(yùn)動(dòng)軌跡復(fù)雜的序列進(jìn)行實(shí)驗(yàn)。結(jié)果表明，本文的算法定位精度和追蹤速度均優(yōu)于原ORB-SLAM2算法。

[1] 王雅儀, 余萌, 朱鋒. 線面特征結(jié)合的三維激光SLAM算法及性能評(píng)估[J]. 導(dǎo)航定位學(xué)報(bào). 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] 方琪, 王曉華, 蘇杰. 基于分組策略的點(diǎn)線特征融合同步定位與地圖構(gòu)建算法[J]. 激光與光電子學(xué)進(jìn)展, 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ī)室內(nèi)視覺(jué)定位研究[J]. 傳感技術(shù)學(xué)報(bào), 2019, 32(11): 1706-1712.

[9] 張新良, 李陽(yáng), 趙運(yùn)基. 一種基于時(shí)空切片的SLAM關(guān)鍵幀提取方法[J]. 系統(tǒng)仿真學(xué)報(bào). 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] 張小國(guó), 劉啟漢, 李尚哲, 等. 基于雙步邊緣化與關(guān)鍵幀篩選的改進(jìn)視覺(jué)慣性SLAM方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2020, 28(5): 608-614.

[12] 谷曉琳, 楊敏, 張燚, 等. 一種基于半直接視覺(jué)里程計(jì)的RGB-D SLAM算法[J]. 機(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

趙榮亮，王紅旗，劉群坡，等. 運(yùn)動(dòng)曲率自適應(yīng)V-SLAM算法研究[J]. 導(dǎo)航定位學(xué)報(bào), 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—），男，河南南陽(yáng)人，碩士研究生，研究方向?yàn)榛谝曈X(jué)的移動(dòng)機(jī)器人定位。

王紅旗（1973—），男，河南周口人，博士，副教授，碩士研究生導(dǎo)師，研究方向?yàn)闄C(jī)器人控制。