基于改進ORB-SLAM2的機器人同步定位與建圖技術研究

陽霜，羅 堪，韋曉慧，周志偉，彭國超

(1.湖南機電職業(yè)技術學院，湖南長沙 410151；2.長沙師范學院，湖南長沙 410100)

0 引言

隨著機器人技術的飛速發(fā)展，如何實現(xiàn)機器人在未知環(huán)境下的定位與導航，并自主地完成特定任務，是一個火熱的研究方向。在深海、隧道、室內(nèi)等應用場景，GPS等全球衛(wèi)星定位系統(tǒng)的信號存在高衰減現(xiàn)象，機器人面臨“在哪里”“去哪里”“怎么去”等問題。為了實現(xiàn)這些功能，讓移動機器人能夠同步定位與建圖(SLAM)，需要讓其能夠明確自身的位置和位姿，感知周圍環(huán)境的信息并創(chuàng)建地圖。這一技術的發(fā)展，對于實現(xiàn)機器人的智能定位與導航具有非常重要的意義。

目前，SLAM技術主要分為兩類：基于激光雷達的SLAM[1]和基于視覺的SLAM[2]，這兩種技術在機器人定位和地圖構建方面都有廣泛的應用。其中，激光SLAM 是一種較為成熟的方法，已成功應用于多種商業(yè)產(chǎn)品。然而，激光SLAM 構建的地圖信息過于簡單，無法讓機器人獲取更豐富的環(huán)境信息[3]。視覺SLAM 是近十年來發(fā)展迅速的SLAM 解決方案，可以使用攝像頭重建3D 地圖。此外，圖像中包含豐富的物體信息，可以幫助機器人完成各種基于視覺的智能任務[4]。

Cesar Cadena 等人將視覺SLAM 的發(fā)展，劃分為三個代表性的時代[5]，第一個時代“經(jīng)典時代”，主要解決SLAM 問題，憑借提出的數(shù)學公式，SLAM問題首次得到了有效應用。第二個時代“算法改進時代”，引入GPU、RGB-D、立體相機等新硬件，用于改進視覺SLAM 算法。第三個時代“提高魯棒性時代”，主要是針對特定場景，提高系統(tǒng)魯棒性。比較有代表的是基于稀疏直接法的視覺里程計(DSO, Direct Sparse Odometry)[5]，ORB-SLAM[7]等算法，這一時期SLAM 問題最大的挑戰(zhàn)，是在控制計算量大小的情況下，構建一致收斂的軌跡和地圖，保證移動機器人定位精度的同時，提高算法的運行效率。2017年Mur-Artal R等，在ORB-SLAM 算法基礎上，開源了ORB-SLAM2[8]方案，新增了對雙目相機、RGB-D 相機的源生支持，是目前市場上主流的具有實時性好、地圖重用和動態(tài)管理關鍵幀等優(yōu)點的完整視覺SLAM 系統(tǒng)。D3VO[9]是一種基于深度學習的視覺里程計框架，由Stumberg等人在2020年提出。該框架將深度學習的深度估計、位姿估計、不確定度估計應用于視覺里程計方法中，以優(yōu)化前端追蹤效果，并提高后端非線性優(yōu)化的性能[10]，但目前的深度學習解決方案仍處于探索階段，對數(shù)據(jù)和計算機設備要求較高，且現(xiàn)有的模型還不夠成熟，運行效果有待提高。

綜上所述，ORB-SLAM2 是目前視覺SLAM 中，最前沿的同步定位與建圖算法之一，本文選擇ORBSLAM2 算法，對機器人進行同步定位與建圖，提出自適應降低閾值的方法提取特征點，當通過高響應閾值搜索，未搜索到特征點時，自適應逐級降低閾值搜索特征點，提高所提取特征點的響應值，從而提高移動機器人在未知環(huán)境下，進行同步定位與建圖的精度。

1 ORB-SLAM2簡介

ORB-SLAM2由跟蹤、局部建圖、閉環(huán)優(yōu)化檢測三大線程組成，如圖1所示。

圖1 ORB-SLAM2框架圖

在跟蹤線程中，首先從圖像中提取ORB 特征點，并運用四叉樹法對這些特征點進行篩選。然后，根據(jù)上一幀的位姿信息，進行位姿估計或進行重定位以初始化位姿。接下來，跟蹤線程會跟蹤重建局部地圖，并進一步優(yōu)化位姿以確定新的關鍵幀。

在局部建圖線程中，機器人會將跟蹤線程生成的新的關鍵幀插入，并驗證最新獲取的地圖點，進而構建新的地圖點。這些地圖點通過局部捆集調(diào)整(Local BA)進一步優(yōu)化，最后機器人會對插入的關鍵幀進行篩選，刪除冗余關鍵幀，實現(xiàn)局部地圖構建。

為了減小之前步驟所積累產(chǎn)生的誤差，閉環(huán)優(yōu)化線程的主要任務是通過詞袋庫判別閉環(huán)幀。然后，利用Sim3算法計算相似變換，對地圖進行矯正及優(yōu)化，從而減小之前步驟中，所積累產(chǎn)生的誤差。

系統(tǒng)的地圖由4個部分組成，即地圖點、關鍵幀、一致圖和生成樹。其中，一致圖是由節(jié)點和邊構成的數(shù)據(jù)結構，節(jié)點指的是關鍵幀，邊是關鍵幀之間的聯(lián)系。生成樹則是按照地圖點和關鍵幀之間的關系，以樹狀結構進行存儲，用于查找相鄰的關鍵幀。

位置識別模塊包括視覺詞袋和識別數(shù)據(jù)庫兩部分。視覺詞袋利用ORB特征信息對特征點進行分類，實現(xiàn)位置的快速匹配。識別數(shù)據(jù)庫是利用關鍵幀所匹配的特征點，與視覺詞袋進行關聯(lián)，將相似度得分最高的，形成關鍵幀數(shù)據(jù)庫，運用到重定位及回環(huán)檢測。

2 ORB特征點提取優(yōu)化

原算法在跟蹤線程中提取ORB特征點時，爭取特征點在圖像的所有部分分布均勻，將圖像分為30*30大小的CELL 區(qū)域，然后對每個CELL 搜索特征點，優(yōu)先使用高響應閾值搜索特征點，若使用高響應閾值沒找到特征點，則將搜索閾值調(diào)整為低響應閾值，重新對該CELL 進行搜索，直至將所有CELL 搜索完畢后，特征點提取工作結束。

由于原算法，在ORB 特征點提取環(huán)節(jié)，若通過高響應閾值沒有搜索到特征點，直接采用低響應閾值進行搜索，由于高響應閾值與低響應閾值之間的跨度較大，因此在搜索特征點時，提取了較多低響應值特征點，將增大后續(xù)特征點篩選及匹配誤差，從而影響機器人在進行同步定位與建圖的精度。

本文對提取ORB特征點算法進行改進，若通過高響應閾值搜索，未搜索到特征點，則將搜索響應值在原搜索響應值的基礎上，降低t來搜索特征點。如果此時搜索到特征點，則記錄特征點后，來到下一CELL進行特征點搜索；若未搜索到特征點，且此時搜索響應值大于低響應閾值，則在此搜索響應值的基礎上，再次降低t來搜索特征點。由此，通過將響應值逐次降低t來搜索特征點，直至遍歷完所有的CELL，如圖2所示。通過對響應值進行自適應調(diào)整，在特征點提取時，將有效減少低響應值的特征點，在降低后續(xù)對特征點進行篩選與匹配的誤差的同時，還可以減少特征點匹配的工作量，提高系統(tǒng)運行效率。

圖2 特征點提取優(yōu)化流程圖

3 實驗與分析

3.1 數(shù)據(jù)準備

EuRoC MAV 數(shù)據(jù)集是微型飛行器(MAV)上收集的視覺慣性數(shù)據(jù)集。數(shù)據(jù)集包含立體圖像、同步IMU測量以及準確的運動和結構地面實況。本文采用Eu-RoC MAV 數(shù)據(jù)集中的01、03、05 號場景，各數(shù)據(jù)集特點如表1所示。通過對三個數(shù)據(jù)集進行比較，得知廠房01的數(shù)據(jù)拍攝條件最好，飛行器運行速度低，拍攝場景紋理及光照均較好，廠房03 的數(shù)據(jù)較廠房01 而言，運行速度有所加快，而廠房05的數(shù)據(jù)最難，不僅運行速度快，其場景的紋理及光照條件均較差。因此，筆者利用這三個數(shù)據(jù)集，來運行原始ORB-SLAM2 算法及改進后的ORB-SLAM2 算法，有助于分析算法對高速運行、弱紋理、光照較差的不良條件的魯棒性。

表1 數(shù)據(jù)集介紹

3.2 評價標準

針對改進算法和原始算法，分別計算相對定位誤差(RPE，Relative Positioning Error) 和絕對定位誤差(APE，Absolute Positioning Error) 的 均 方 根 誤 差(RMSE)、標準差(σ)。

APE是估計位置(2D 或3D)與真實位置之間的歐幾里得距離，用于檢驗算法的全局準確性和穩(wěn)定性。其RMSE計算如式(1)，RMSE越小，估計軌跡越接近真實軌跡，標準差計算如式(2):

式中，n表示關鍵幀數(shù)目，i表示第i個關鍵幀，xobs.i表示第i個關鍵幀姿態(tài)真實值，xmodel.i為第i個關鍵幀姿態(tài)的估計值。

式中，n表示關鍵幀數(shù)目，i表示第i個關鍵幀，xmodel.i表示第i個關鍵幀姿態(tài)真實值，μ為xmodel.i為的平均值。

RPE是連續(xù)位置估計(2D 或3D)之間的歐幾里得距離，用于計算漂移，即每米的平移或旋轉(zhuǎn)誤差。第i幀的RPE定義如下：

式中，Pi表示第i幀的估計位姿，Qi表示第i幀的真實位姿，Δt表示間隔時間。

式中，n表示觀測次數(shù)，Δt表示時間間隔t，m=n- Δt×RPE，Ei表 示i時 刻 相 對 位 姿 估 計 誤 差，trans(Ei)代表相對位姿誤差中的平移部分。

相對定位誤差、絕對定位誤差的具體計算，可運用開源的EVO Python計算包進行計算。

最后運用式(5)計算估計軌跡與真實軌跡長度的相對誤差，用于評價算法對軌跡尺度估計的能力。

3.3 誤差分析

運用原算法及改進算法，對廠房01、03、05號數(shù)據(jù)集進行同步定位與建圖仿真，其絕對定位誤差及相對定位誤差比較，分別如表2、表3所示。

表2 不同數(shù)據(jù)集下APE對比表

表3 不同數(shù)據(jù)集下RPE對比表

從表2、表3 可知，改進算法的相對定位誤差的RMSE與σ值，略小于原算法，而改進算法的絕對定位誤差的RMSE與σ值，遠優(yōu)于原算法，三個場景中，其絕對定位誤差的RMSE的平均值下降了15.87%。由此可知，改進算法在進行機器人同步定位與建圖時，其漂移誤差略微減小，而其全局準確性和穩(wěn)定性得到大幅提升。

對改進算法在廠房01、03、05，三個不同場景下的APE、RPE進行比較，其中廠房01 和廠房03 的APE、RPE相差較小，而廠房05 的APE、RPE較另兩種相差略大，說明改進算法對高速運動的魯棒性較好，而對于弱紋理的魯棒性有待提高。

運用ORB-SLAM2改進算法，對廠房01、03、05數(shù)據(jù)集進行同步定位與建圖仿真，其軌跡仿真如圖3所示，圖中X、Y、Z 為三維空間坐標軸，圖例data 為真實軌跡，圖例KeyFrameTrajectory 為估計軌跡。通過對圖3進行觀察，可分析估計軌跡與真實軌跡的擬合程度，其中廠房01與03的軌跡仿真圖中，估計軌跡與真實軌跡無較大軌跡錯位，幾乎重合，而廠房05的估計軌跡與真實軌跡在某一區(qū)域內(nèi)，存在一定偏差。再次驗證了，改進算法對高速運動的魯棒性較好，而對于弱紋理的魯棒性有待提高。

圖3 軌跡仿真圖

ORB-SLAM2 改進算法對廠房01、03、05 數(shù)據(jù)集仿真，其真實軌跡與估計軌跡長度，及軌跡長度誤差計算，如表4所示。從表中數(shù)據(jù)分析可知，估計軌跡長度均與真實軌跡長度相近，且誤差較小。結合軌跡仿真圖及軌跡長度比較表，總體來說，對ORB-SLAM2算法進行ORB特征點提取優(yōu)化后，能夠較好地實現(xiàn)機器人同步定位與建圖。

表4 軌跡長度比較表

4 結論與展望

通過上述實驗分析，在提取ORB 特征點時，通過對響應值進行自適應調(diào)整，對ORB-SLAM2 算法進行改進，能夠較好地應用于未知環(huán)境下，機器人同步定位與建圖。通過實驗數(shù)據(jù)對比分析可知，改進算法的相對定位誤差的RMSE與σ值，略小于原算法，而改進算法的絕對定位誤差的RMSE與σ值，遠優(yōu)于原算法，三個場景中，其APE的RMSE的平均值下降了15.87%。由此可知，通過對ORB-SLAM2算法進行改進，在進行同步定位與建圖時，每米的平移或旋轉(zhuǎn)誤差略微有所下降，準確性及穩(wěn)定性得到較大提高，估計軌跡更接近于真實軌跡。因此，運用改進算法使機器人在未知環(huán)境下，進行同步定位與建圖的精度得到提高。

通過對廠房01 和廠房03 的絕對定位誤差、相對定位誤差、軌跡仿真圖進行對比，得知改進算法，對于高速運動的魯棒性較好，但對廠房05與廠房01、03場景的絕對定位誤差、相對定位誤差、軌跡仿真圖進行對比，得知后續(xù)對于環(huán)境光照變化、低紋理環(huán)境等問題，如何提升系統(tǒng)魯棒性，提高智能移動機器人定位與導航精度，仍有待加強研究。

另外，視覺SLAM 主要用于機器人或無人機等嵌入式平臺的場景識別、深度學習、3D重建等應用。如何在提高機器人或無人機進行實時定位與導航精度的同時，加快運行速度，還有待進一步探索。