ORB-SLAM算法在智能家庭清掃機(jī)器人中的應(yīng)用　

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

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

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

0 引言

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

　　近年來SLAM算法取得了巨大的發(fā)展，進(jìn)而產(chǎn)生了對單目攝像機(jī)支持度較高的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)行跟蹤、構(gòu)圖、重定位、閉環(huán)修正以及初始化，并且算法的環(huán)境適應(yīng)力強(qiáng)，對劇烈運(yùn)動也很魯棒。正因為ORB-SLAM是基于特征點(diǎn)的SLAM系統(tǒng)，因此其能夠?qū)崟r的計算出相機(jī)的運(yùn)動軌跡，并且生成稀疏的場景三維重建結(jié)果。

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ā)展而來的，即為OFAST(FAST Key pointOrientation)也就是說在使用FAST提取特征點(diǎn)之后再給特征點(diǎn)定義一個方向，以此來實(shí)現(xiàn)特征點(diǎn)的旋轉(zhuǎn)不變性。通常是使用灰度質(zhì)心法來計算以特征點(diǎn)為圓心半徑為r的圓形領(lǐng)域范圍內(nèi)的灰度質(zhì)心的位置，并根據(jù)從中心位置到質(zhì)心位置的向量來定義該特征點(diǎn)的主方向。

　　定義矩的計算公式為：

(1)

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

(2)

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

(3)

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

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

2 ORB-SLAM算法原理

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

　　2.1 追蹤階段

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

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

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

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

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

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

　　2.1.3 選取關(guān)鍵幀

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

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

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

　　2.2 地圖構(gòu)建階段

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

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

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

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

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

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

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

　　2.2.4 局部集束調(diào)整

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

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

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

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

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

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

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

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

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

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

3 實(shí)驗測試

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

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

4 結(jié)論

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

　　參考文獻(xiàn)：

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

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

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

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

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

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