搜救機器人視覺SLAM 回環(huán)檢測方法研究

謝鵬發(fā)，蘇衛(wèi)華，*，李世國，張世月

（1.軍事科學院國防科技創(chuàng)新研究院，北京100071；2.天津（濱海）人工智能創(chuàng)新中心，天津300457）

0 引言

在視覺同時定位與地圖構(gòu)建（simultaneous localization and mapping，SLAM）中，機器人位姿估計是一個遞推的過程，由于算法本身的局限，對一幀位姿估計產(chǎn)生的誤差將在后一幀位姿估計中延續(xù)放大，因而隨著建圖時間的增加，機器人位姿估計的累計誤差會越來越大，對機器人定位精度與地圖構(gòu)建精度將產(chǎn)生嚴重的影響。回環(huán)檢測是指機器人在進行SLAM 的過程中判斷自己是否到達之前已經(jīng)訪問過的某個場景[1]，從而根據(jù)前面誤差較小的位姿修正后面的位姿，消除累計誤差，提高機器人定位與建圖精度。對于搜救機器人而言，在復雜的災后環(huán)境中執(zhí)行搜救任務時[2]需要有更加精準的定位與環(huán)境地圖，因此研究搜救機器人視覺SLAM 回環(huán)檢測方法對于提升搜救任務的完成效率具有重大的意義。

現(xiàn)有的視覺SLAM 系統(tǒng)中，回環(huán)檢測方法主要有基于詞袋模型（bag of words，BoW）的回環(huán)檢測、基于隨機蕨法的回環(huán)檢測和基于深度學習的回環(huán)檢測等?；谏疃葘W習的回環(huán)檢測方法目前是SLAM 研究的熱門方向之一，PoseNet[3]為該種方法的代表算法，Gao 等[4-5]、何元烈等[6]、Lategahn 等[7]對深度學習用于SLAM 回環(huán)檢測方向進行了大量研究。該方法提高了SLAM 算法的運行效率以及定位與建圖的精度，但是對個人計算機的性能要求較高，需要圖形處理器（graphics processing unit，GPU）加速方能實現(xiàn)。對于搜救平臺而言，因工作環(huán)境復雜，要自主完成搜救任務需要占用大量計算資源，若SLAM 算法本身需要大量的計算資源則無法保證搜救機器人的實時工作性能。目前SLAM 的另一個發(fā)展方向是輕量化，力求在較少的計算資源下實現(xiàn)SLAM 算法的實時運行。相較于基于隨機蕨法和基于深度學習的回環(huán)檢測方法，基于BoW 的回環(huán)檢測方法通過詞袋對算法的關(guān)鍵幀進行相似度計算，不需要GPU 加速，在CPU 上即可實現(xiàn)SLAM 算法的實時運行，因此成為搜救機器人SLAM 回環(huán)檢測算法的主流方法。

ORB-SLAM2[8]是運用基于BoW 的回環(huán)檢測方法的經(jīng)典算法之一，該算法能夠有效消除累計誤差，且在相機位姿跟蹤丟失后還能迅速找回，相較于其他SLAM 算法具有很大的優(yōu)勢。但是該算法的回環(huán)檢測模塊沒有考慮到相似場景對回環(huán)準確率的影響，在相似場景中容易產(chǎn)生錯誤回環(huán)，嚴重影響算法的定位與建圖精度。因此，本文在ORB-SLAM2 算法的基礎(chǔ)上，對其回環(huán)檢測模塊進行改進，對相似場景的錯誤回環(huán)進行剔除，以提高回環(huán)準確率和算法的定位與建圖精度，進而提高搜救機器人在復雜搜救環(huán)境中執(zhí)行任務的效率。

1 相似場景回環(huán)幀剔除算法設(shè)計

1.1 回環(huán)檢測算法整體框架

基于BoW 的方法是預先加載一個詞袋字典樹，通過這個預加載的字典樹將圖像中的每一局部特征點的描述子轉(zhuǎn)換為一個單詞，字典里包含著所有的單詞，將整幅圖像的單詞統(tǒng)計為一個詞袋向量，詞袋向量間的距離代表了2 幅圖像之間的差異[9]。ORBSLAM2 算法的回環(huán)檢測模塊使用BoW 對當前幀與歷史關(guān)鍵幀進行單詞聚類，通過計算2 幀的相似性得到相似性得分較高的關(guān)鍵幀，對該關(guān)鍵幀進行連續(xù)性檢測后得到回環(huán)候選幀，之后通過計算當前幀與回環(huán)候選幀的sim3 變換以及重投影匹配等步驟得到最終的回環(huán)幀。這種方法主要利用了圖像的色彩（RGB）信息，而圖像的深度（Depth）信息并沒有得到充分利用，因此搜救機器人在建圖過程中遇到相似場景時容易產(chǎn)生錯誤回環(huán)，對定位與建圖結(jié)果產(chǎn)生嚴重影響。本文在ORB-SLAM2 算法得到回環(huán)候選幀之后，添加相似場景回環(huán)候選幀剔除模塊，通過索引當前幀與回環(huán)候選幀的深度信息對其進行比較，當結(jié)果達到設(shè)定閾值后將其剔除。改進的回環(huán)檢測算法流程如圖1 所示。

圖1 改進后的回環(huán)檢測算法流程圖

ORB-SLAM2 算法在回環(huán)檢測模塊前端得到與當前幀相似性得分較高的歷史關(guān)鍵幀后，將其共視幀中相連的幀分為一組，計算組內(nèi)最高得分的同時得到每組中得分最高的關(guān)鍵幀，通過比較組最高得分得到回環(huán)候選幀所在組，組中得分最高的關(guān)鍵幀記為回環(huán)候選幀。由此計算了回環(huán)檢測幀的連續(xù)性，避免了將單幅圖像相似但場景并不相同的關(guān)鍵幀作為回環(huán)候選幀，但是在相似場景中，其相似的圖像幀具有連續(xù)性，此處并不能將其剔除。因此，在得到回環(huán)候選幀之后添加相似場景剔除模塊，對相似場景產(chǎn)生的錯誤候選回環(huán)進行剔除。

1.2 相似場景回環(huán)剔除

由于ORB-SLAM2 算法自帶的字典是大量的室內(nèi)外場景訓練所得，字典內(nèi)容包含了常見的各種物體信息，所以本文延用ORB-SLAM2 算法的字典，對屬于同一節(jié)點（特定層）的特征點通過計算描述子距離進行匹配。采用這種BoW 加速匹配的方法分別對當前幀與每一個候選回環(huán)幀進行單詞查找匹配得到對應的地圖點。

本文提出的相似場景回環(huán)剔除算法防偽代碼如下：

算法1：相似場景回環(huán)剔除算法輸入：當前幀與回環(huán)候選幀輸出：是否剔除回環(huán)候選幀

For i=1 到n

vpMapPointMatches = SearchByBoW（Current

KF，Loop Candidates）

vpKeyFrameMP = mpCurrentKF->GetMapPoint－

Matches()

Fora=1：m

If(vpMapPointMatches(a))

pMP1=vpMapPointMatche(a)

pMP2=vpKeyFrameMP(a)

pMP1->GetWorldPos()

pMP2->GetWorldPos()

If(z1/z2>1.18||z1/z2<0.85)

相似點數(shù)量num 加1

Endif

If(num>20)

剔除當前候選回環(huán)幀

Endif

Endif

Endfor

Endfor

算法1 中，n 為候選回環(huán)幀的數(shù)量，m 為當前幀與候選回環(huán)幀地圖匹配點的數(shù)量。對于經(jīng)過相似度篩選后的回環(huán)候選幀的每一幀，通過BoW 加速匹配得到對應地圖點（vpMapPointMatches，vpKeyFrameMP）后，對于2 幀對應匹配的地圖點索引各自在世界坐標系中的z 值（z1，z2），將計算得到的比值與設(shè)定的閾值進行比較。考慮到相似場景深度值不同，但是不排除場景距離較近、深度差值小的情況，因此本文以15%的比值差值為判定條件，將閾值邊界設(shè)定為0.85 和1.18，如果比值小于0.85 或者大于1.18，則將此地圖點標記為相似點，若相似點累計超過20 個，則證明當前的回環(huán)候選幀為相似場景關(guān)鍵幀，將其剔除。

本文的改進算法是利用深度信息對相似場景的回環(huán)關(guān)鍵幀進行剔除，閾值設(shè)定的前提是ORBSLAM2 算法具有較好的定位精度與魯棒性。相較于ElasticFusion[10]和RGBD SLAM[11]等SLAM 領(lǐng)域知名的開源算法，ORB-SLAM2 算法具有更好的定位精度與魯棒性[8]，其在相機位姿跟蹤丟失后也能迅速找回，為本文改進算法提供了理論保障。

2 數(shù)據(jù)集制作

2.1 相機標定

由于每個相機的幾何模型參數(shù)不同，為得到精確的相機幾何模型參數(shù)，保證后續(xù)實時建圖的算法精度，本文使用張正友標定法[12]，在Ubuntu 16.04 系統(tǒng)中，對Kinect1.0 相機進行標定，求取相機參數(shù)。該方法克服了傳統(tǒng)標定法需要的高精度標定物的缺點，僅需要一個打印出來的棋盤格就可完成標定，因而又稱棋盤法。本文標定相機使用的棋盤格如圖2 所示。同時相對于自標定而言，棋盤法提高了精度，便于操作，因此被廣泛應用。

2.2 相似場景數(shù)據(jù)集制作

為了測試改進的回環(huán)檢測算法在相似場景中的工作性能，本文采用Kinect1.0 相機采集相似場景數(shù)據(jù)制作相似場景數(shù)據(jù)集。通過場景布置，本研究采集了場景單一且相似的會議室（如圖3 所示），通過交叉行走增加回環(huán)數(shù)量，為之后的回環(huán)性能測試提供基礎(chǔ)。以10 Hz 的幀率采集場景的色彩圖和深度圖，并將每一幅圖像的時間戳和圖像名對應保存，以便之后對深度圖像和色彩圖像進行配準建圖。最終生成相似場景數(shù)據(jù)集similar_boardroom_loop。

3 實驗與分析

3.1 準確率與召回率曲線對比

準確率和召回率是回環(huán)檢測的典型性能指標。準確率是指算法提取的所有回環(huán)中確實是真實回環(huán)的概率，召回率指的是在所有真實回環(huán)中被正確檢測出來的概率?；丨h(huán)檢測的結(jié)果分類見表1。

準確率與召回率的計算公式如下：

圖2 相機標定使用的棋盤格（7×5）

圖3 會議室場景

表1 回環(huán)檢測的結(jié)果分類

為了評價算法的有效性，本研究通過設(shè)置閾值r0（0

圖4 準確率與召回率對比曲線圖

圖4中本文改進算法剔除了ORB-SLAM2 算法在相似場景中產(chǎn)生的錯誤回環(huán)，得到的準確率與召回率曲線相較于ORB-SLAM2 算法整體偏右上方，在100%準確率下仍有50%的召回率，而ORB-SLAM2算法在100%準確率下召回率最高為30%，表明本文改進算法在準確率較高的同時還具有很好的召回率，驗證了其有效性。圖5 為2 種算法對相似場景數(shù)據(jù)集的建圖結(jié)果，可以看出，ORB-SLAM2 算法在相似場景中產(chǎn)生錯誤回環(huán)導致場景重復出現(xiàn)或者產(chǎn)生重影，而本文改進算法在剔除錯誤回環(huán)后消除了這些建圖誤差，建圖精確度更高。

3.2 定位精度

圖5 相似場景建圖結(jié)果對比

本文采用德國慕尼黑工業(yè)大學公布的TUM RGBD[13]標準數(shù)據(jù)集中的fr1-room 序列對ORBSLAM2 算法和本文改進算法進行性能測試，通過evo 評價工具對2 種算法的定位精度進行比較。

圖6 為2 種算法運行數(shù)據(jù)集的軌跡與真實軌跡（groudtruth）的對比圖，可以看出，相比于ORB-SLAM2算法，本文改進算法的運行數(shù)據(jù)集的軌跡整體接近真實軌跡，算法的定位誤差更小。分別將2 種算法的軌跡與真實軌跡進行對比，將絕對誤差可視化，如圖7、8 所示，圖中右邊是不同誤差數(shù)值對應的彩色樣條，由深藍色到深紅色，誤差數(shù)值逐漸變大?？梢钥闯?，ORB-SLAM2 算法運行數(shù)據(jù)集得到的軌跡誤差數(shù)值整體偏淺藍色和綠色，而本文改進算法運行數(shù)據(jù)集得到的軌跡誤差數(shù)值整體偏深藍色，整體誤差明顯偏小。

圖6 軌跡對比圖

結(jié)合圖9、10 和表2 可知，ORB-SLAM2 算法運行數(shù)據(jù)集得到的絕對位姿誤差的均方根誤差為0.141 m、最大誤差為0.242 m，本文改進算法運行數(shù)據(jù)集得到的絕對位姿誤差的均方根誤差為0.107 m、最大誤差為0.208 m。相較于ORB-SLAM2 算法，本文改進算法的均方根誤差減小了24.1%，最大誤差減小了14.0%，提高了算法的定位精度。

4 結(jié)論

圖7 ORB-SLAM2 算法絕對軌跡誤差可視化圖

圖8 本文改進算法絕對軌跡誤差可視化圖

圖9 ORB-SLAM2 算法絕對位姿誤差算法

本文結(jié)合圖像深度與色彩信息，對ORB-SLAM2算法的回環(huán)檢測模塊進行改進，剔除因相似場景產(chǎn)生的錯誤回環(huán)，提高了算法的回環(huán)準確率。通過場景布置，使用Kinect1.0 相機采集數(shù)據(jù)制作相似場景數(shù)據(jù)集。基于自制相似場景數(shù)據(jù)集和TUM RGBD 標準數(shù)據(jù)集對改進算法和ORB-SLAM2 算法進行測試對比，實驗結(jié)果表明改進算法能夠剔除因相似場景產(chǎn)生的錯誤回環(huán)，將改進算法應用于搜救機器人將提高搜救機器人的定位與建圖精度，進而提高搜救機器人的自主行動能力和搜救效率。

圖10 本文改進算法絕對位姿誤差

表2 2 種算法絕對位姿誤差對比單位：m

由于本文的算法是基于靜態(tài)環(huán)境的SLAM，采集的數(shù)據(jù)集環(huán)境和使用的TUM RGBD 數(shù)據(jù)集也均為靜態(tài)環(huán)境，并未考慮建圖過程中動態(tài)物體造成的建圖影響。未來工作中將考慮加入語義識別，對構(gòu)建的地圖進行語義分割和動態(tài)目標識別，將動態(tài)物體進行剔除，進一步提高搜救機器人的SLAM 建圖精度和魯棒性。