ORB-SLAM算法在智能家庭清掃機器人中的應用　

作者 張汪洋1 孫偉偉1 肖琦1 鹿飛2

　　1.中國礦業(yè)大學(xué)徐海學(xué)院(江蘇 徐州 221000)2.上海東軟載波微電子有限公司(上海 200000)

摘要：對于家庭智能清掃機器人而言，機器人自我定位與路徑規劃是其實(shí)現智能化、全自動(dòng)化的理論基礎。實(shí)時(shí)的定位與構圖(simultaneous localization and mapping，SLAM)技術(shù)可以很好的解決這個(gè)問(wèn)題。該技術(shù)可以利用機器人自身所攜帶的各式傳感器對周?chē)h(huán)境進(jìn)行采集，并利用采集結果遞增地創(chuàng )建環(huán)境地圖，同時(shí)使用該地圖實(shí)現機器人對自身的定位與導航功能。本文將通過(guò)討論ORB-SLAM算法系統的執行過(guò)程，并進(jìn)行仿真實(shí)驗，驗證該算法在對家庭清掃機器人的自我定位與導航的實(shí)用性。

0 引言

　　近年來(lái)智能家居的發(fā)展使得我們的居家生活變得更加充滿(mǎn)樂(lè )趣與效率。然而對于像智能清掃機器人這類(lèi)室內移動(dòng)型機器人而言，機器人自行定位和對周邊環(huán)境的識別是其能否正常高效工作的根本。激光或相機進(jìn)行實(shí)時(shí)的定位與構圖(simultaneous localization and mapping，SLAM)^[1]是一種相對比較完善的移動(dòng)機器人自主定位的方法，當機器人處于一個(gè)未知的環(huán)境中，通過(guò)其所攜帶的圖像采集模塊對機器人周邊的環(huán)境進(jìn)行觀(guān)測，再遞增地構建環(huán)境地圖，并且通過(guò)構建的環(huán)境地圖對機器人實(shí)現實(shí)時(shí)的定位。

　　近年來(lái)SLAM算法取得了巨大的發(fā)展，進(jìn)而產(chǎn)生了對單目攝像機支持度較高的ORB-SLAM算法。ORB-SLAM是由Raul Mur-Artal，J. M. M. Montiel和Juan D. Tardos于2015年發(fā)表在IEEE Transactions on Robotics上。該算法融合了PTAM算法的主要思想以及Strasdat提出的閉環(huán)修正方法[1]并采用ORB特征點(diǎn)進(jìn)行跟蹤、構圖、重定位、閉環(huán)修正以及初始化，并且算法的環(huán)境適應力強，對劇烈運動(dòng)也很魯棒。正因為ORB-SLAM是基于特征點(diǎn)的SLAM系統，因此其能夠實(shí)時(shí)的計算出相機的運動(dòng)軌跡，并且生成稀疏的場(chǎng)景三維重建結果。

1 ORB特征點(diǎn)提取原理

　　ORB(Oriented FAST and Rotated BRIEF)是一種快速特征點(diǎn)的提取與描述的算法。ORB算法通常分為兩部分，分別是特征點(diǎn)提取和特征點(diǎn)描述。

　　特征點(diǎn)提取是由FAST(Features from Accelerated Segment Test)算法發(fā)展而來(lái)的，即為OFAST(FAST Key pointOrientation)也就是說(shuō)在使用FAST提取特征點(diǎn)之后再給特征點(diǎn)定義一個(gè)方向，以此來(lái)實(shí)現特征點(diǎn)的旋轉不變性。通常是使用灰度質(zhì)心法來(lái)計算以特征點(diǎn)為圓心半徑為r的圓形領(lǐng)域范圍內的灰度質(zhì)心的位置，并根據從中心位置到質(zhì)心位置的向量來(lái)定義該特征點(diǎn)的主方向。

　　定義矩的計算公式為：

(1)

　　在上式中I(x,y)為圖像的灰度表達式，則該矩質(zhì)心為：

(2)

　　假設點(diǎn)坐標為O，則向量角即為該特征點(diǎn)的方向。計算公式為：

(3)

　　特征點(diǎn)描述則是根據BRIEF(Binary Robust Independent Elementary Features)特征描述算法改進(jìn)而來(lái)的。BRIEF為了解決旋轉不變性將特征點(diǎn)的Patch旋轉至主方向上(steered BRIEF)。具體操作步驟不再贅述。

　　ORB的特征點(diǎn)檢測法則是將FAST特征點(diǎn)檢測法同BRIEF特征描述算法相互結合起來(lái)并加以改進(jìn)與優(yōu)化。

2 ORB-SLAM算法原理

　　ORB-SLAM算法通常利用三個(gè)線(xiàn)程分別進(jìn)行追蹤(Tracking)、地圖構建(LocalMapping)和閉環(huán)檢測(LoopClosing)。如圖1所示。

　　2.1 追蹤階段

　　在攝像頭輸入圖像幀之后的追蹤階段通常分為以下幾步進(jìn)行運行，即ORB特征提取、幀間初始姿態(tài)估計/重定位、姿態(tài)優(yōu)化、選取新的關(guān)鍵幀[2]。

　　2.1.1 初始姿態(tài)估計/重定位

　　ORB-SLAM在初始姿態(tài)估計部分主要使用了以下幾種運動(dòng)模型(Tracking with motion model)、關(guān)鍵幀(Tracking with reference key frame)和重定位(Relocation)^[3]。

　　運動(dòng)模型的本質(zhì)在于使用上一幀的位姿與速度來(lái)估計當前幀的位姿，而上一幀的速度則是由前面幾幀的位姿計算得出的。由于運動(dòng)模型的計算本質(zhì)限制，運動(dòng)模型通常適用于相對于機器人來(lái)說(shuō)勻速運動(dòng)的物體。但是如果是運動(dòng)相對隨意的目標該模型就會(huì )失效，這個(gè)時(shí)候系統就會(huì )使用關(guān)鍵幀與重定位了。如果運動(dòng)模型無(wú)法滿(mǎn)足條件時(shí)，系統會(huì )嘗試和距離當前幀最近的一個(gè)關(guān)鍵幀去進(jìn)行匹配，在ORB-SLAM中利用了BOW(Bag of Words)來(lái)加速關(guān)鍵幀與當前幀的匹配。如果當前幀與鄰近幀的匹配也失效的話(huà)，則說(shuō)明此時(shí)的當前幀已經(jīng)丟失，所以無(wú)法通過(guò)上述方法確定機器人的真實(shí)位置。這個(gè)時(shí)候就只能去與所有的關(guān)鍵幀進(jìn)行匹配，以此來(lái)查找合適的位置。

　　2.1.2 姿態(tài)優(yōu)化

　　在初始姿態(tài)的估計進(jìn)行完畢之后就是進(jìn)行姿態(tài)優(yōu)化，即是在當前幀與地圖之間盡可能多的查找對應關(guān)系，來(lái)對當前幀的位姿進(jìn)行優(yōu)化。位姿優(yōu)化是非關(guān)鍵幀姿態(tài)估計的精確度與魯棒性的保證。

　　2.1.3 選取關(guān)鍵幀

　　當追蹤階段完成之后首先對運動(dòng)模型進(jìn)行更新，并且通過(guò)之前的計算我們得到一個(gè)對機器人位姿的初始估計，之后通過(guò)投影從已經(jīng)生成的地圖點(diǎn)中找到更多的對應關(guān)系對結果進(jìn)行精確化。由于全局圖中進(jìn)行投影的較為復雜，通常只是在局部圖中進(jìn)行投影。

　　將局部地圖中和當前幀相類(lèi)似的點(diǎn)關(guān)鍵幀序列稱(chēng)為K1，在Covisibility Graph中與K1相鄰的關(guān)鍵幀序列稱(chēng)為K2。局部地圖有意者參考關(guān)鍵幀Kref∈K1，該關(guān)鍵幀具有和當前幀最多的共同地圖云點(diǎn)。

　　為了搜索K1、K2可見(jiàn)的每個(gè)地圖云點(diǎn)我們首先將地圖點(diǎn)對當前幀進(jìn)行投影，倘若超出了圖像的范圍就將其舍棄;之后計算當前視線(xiàn)方向向量v與地圖點(diǎn)云平均視線(xiàn)方向的方向向量n的夾角，舍棄n-v

　　2.2 地圖構建階段

　　當追蹤完成之后我們可以得到一個(gè)新關(guān)鍵幀Ki，接下來(lái)將根據Ki進(jìn)行局部建圖。在局部建圖通常包括關(guān)鍵幀插入，冗余地圖點(diǎn)和關(guān)鍵幀剔除以及局部集束調整等步驟。

　　2.2.1 關(guān)鍵幀的插入

　　將由追蹤階段所得到的關(guān)鍵幀Ki作為新的節點(diǎn)加入Covisibility Graph，同時(shí)對關(guān)鍵幀Ki的生長(cháng)樹(shù)和能夠共享地圖點(diǎn)的關(guān)鍵幀節點(diǎn)相互連接的邊進(jìn)行更新，并計算表示關(guān)鍵幀的詞袋BOW。之后依據計算詞袋的結果將地圖點(diǎn)整合到新的關(guān)鍵幀之后，對Covisibility Graph進(jìn)行更新，把新的關(guān)鍵幀插入Map中。

　　2.2.2 冗余地圖點(diǎn)的剔除

　　為了保證地圖點(diǎn)可追蹤，且不易在三角化時(shí)出現較大的誤差，則需要在創(chuàng )建點(diǎn)云的前三幀進(jìn)行約束測試，通過(guò)的地圖點(diǎn)才可以真正的被保存。通常一個(gè)點(diǎn)同時(shí)滿(mǎn)足：該點(diǎn)應當在可以觀(guān)察到該點(diǎn)的關(guān)鍵幀中，并且應有超過(guò)25%的關(guān)鍵幀可以對該點(diǎn)進(jìn)行跟蹤。若要構建該點(diǎn)為地圖點(diǎn)則它必須被超過(guò)兩個(gè)關(guān)鍵幀觀(guān)察到(硬件如果是雙目攝像頭則為三個(gè)關(guān)鍵幀)。當地圖點(diǎn)被少于三個(gè)關(guān)鍵幀能夠觀(guān)察到時(shí)該點(diǎn)就會(huì )被當作冗余點(diǎn)剔除。這樣有效的防止了外點(diǎn)影響效果。

　　2.2.3 新的地圖點(diǎn)創(chuàng )建

　　通過(guò)檢測到的ORB特征點(diǎn)，查找Covisibility Graph中與之相連的關(guān)鍵幀Kc，匹配方法則是上文中提到的約束測試的方法進(jìn)行篩選匹配，并且將不滿(mǎn)足要求的點(diǎn)全部舍棄。對ORB特征點(diǎn)進(jìn)行三角化之后檢查正向的景深、視差、反投影誤差以及尺度一致性，如果無(wú)誤即可得到地圖點(diǎn)。通常一個(gè)地圖點(diǎn)由兩個(gè)關(guān)鍵幀所觀(guān)察得到的，并且它也可以投影到與之相連的其他關(guān)鍵幀中，此時(shí)為了得到更多的地圖點(diǎn)一般算法會(huì )使用追蹤局部地圖的方法來(lái)在附近的關(guān)鍵幀中找到可以與之匹配的關(guān)鍵幀。

　　2.2.4 局部集束調整

　　局部集束調整就是將當前處理的關(guān)鍵幀Ki進(jìn)行優(yōu)化，通常參與優(yōu)化的關(guān)鍵幀只參與優(yōu)化中的約束，并不作為變量去改變值。同時(shí)優(yōu)化時(shí)產(chǎn)生的外點(diǎn)會(huì )在優(yōu)化的中后期進(jìn)行剔除。

　　2.2.5 局部關(guān)鍵幀的剔除

　　為了控制地圖重建的緊湊度，在這一階段會(huì )檢測并剔除冗余的關(guān)鍵幀，這樣可以有力的控制因為關(guān)鍵幀數目增長(cháng)之后集束調整的復雜程度。

　　在結束完這一系列步驟之后，關(guān)鍵幀會(huì )被記錄到數據庫列表中，并將標志設置為Set Accept Key Frames(true)以允許追蹤線(xiàn)程可以繼續得到關(guān)鍵幀。自此完成局部地圖構建的全部工作。

　　2.3 閉環(huán)檢測線(xiàn)程

　　在實(shí)際應用情況下即使在之前的步驟中我們也利用了諸如集束調整等方法去消減誤差，但隨著(zhù)運行時(shí)間的增加依然會(huì )存在累積誤差[5]。消除累積誤差最為有效的方法是對之前運行得出的結果進(jìn)行閉環(huán)檢測。

　　首先算法會(huì )進(jìn)行閉環(huán)條件檢測，計算出當前關(guān)鍵幀與Covisibility Graph中與其相連的關(guān)鍵幀之間的詞袋(BOW)的相似度。通常使用DBoW2庫進(jìn)行閉環(huán)條件檢測。

　　之后算法將進(jìn)行Sim3的計算。由于針對一個(gè)三維的點(diǎn)而言，單目相機不能對其在三角化中的具體位置進(jìn)行確定。因此單目SLAM是一個(gè)Sim3的相似變換群，擁有7個(gè)自由度，即為[sR|t]。

　　接下來(lái)是對重復出現的點(diǎn)云進(jìn)行閉環(huán)融合，同時(shí)在Covisibility Graph中插入新的邊，連接閉環(huán)。

　　最后對上述所得到的位姿圖用本質(zhì)圖(Essential Graph)進(jìn)行優(yōu)化，將閉環(huán)的誤差進(jìn)行分散，保證閉環(huán)檢測的準確性。

3 實(shí)驗測試

　　為了驗證ORB-SLAM在掃地機器人上進(jìn)行自我定位的實(shí)用性，我們使用了單目攝像頭與樹(shù)莓派3平臺進(jìn)行了搭載ORB-SLAM2系統。

　　實(shí)驗過(guò)程：將搭建好的掃地機器人平臺放在一個(gè)簡(jiǎn)單模擬家居環(huán)境中，并操作使其對周?chē)沫h(huán)境圖像進(jìn)行采集并生成一個(gè)稀疏的三維環(huán)境地圖以及機器人行進(jìn)軌跡。將地圖傳回PC機并與實(shí)際地形進(jìn)行對比。

4 結論

　　根據上述實(shí)驗結果來(lái)看，ORB-SLAM在實(shí)驗平臺上可以很好的實(shí)現機器人的自我定位與導航。這驗證了ORB-SLAM在掃地機器人上推廣使用的可行性。同時(shí)減少了日后掃地機器人進(jìn)行視覺(jué)定位與導航的成本，大大加快了掃地機器人真正普及千家萬(wàn)戶(hù)的步伐。?

　　參考文獻：

　　[1]周紹磊,吳修振,劉剛,等.一種單目視覺(jué)ORB-SLAM/INS組合導航方法[J].中國慣性技術(shù)報,2016,24(5):633-637.

　　[2]林連秀,葉蕓,姚劍敏,等.基于ORB-SLAM的移動(dòng)機器人嵌入式實(shí)現與優(yōu)化[J].微機與應用,2017,36(5):50-53.

　　[3]劉浩敏,章國鋒,鮑虎軍.基于單目視覺(jué)的同時(shí)定位與地圖構建方法綜述[J].計算機輔助設計與圖形學(xué)學(xué)報,2016,28(6):855-867.

　　[4]高翔,張濤,劉毅,等.視覺(jué)SLAM十四講 從理論到實(shí)踐[M].北京:電子工業(yè)出版社,2017,3.

　　[5]劉浩敏,章國鋒,鮑虎軍.面向大尺度場(chǎng)景的單目同時(shí)定位與地圖構建[J].中國科學(xué),2016,46(12):1748-1761.

　　本文來(lái)源于《電子產(chǎn)品世界》2018年第9期第40頁(yè)，歡迎您寫(xiě)論文時(shí)引用，并注明出處。