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

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

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

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

0 引言

　　近年來智能家居的發(fā)展使得我們的居家生活變得更加充滿樂趣與效率。然而對于像智能清掃機(jī)器人這類室內(nèi)移動型機(jī)器人而言，機(jī)器人自行定位和對周邊環(huán)境的識別是其能否正常高效工作的根本。激光或相機(jī)進(jìn)行實時的定位與構(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ī)器人實現(xiàn)實時的定位。

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

1 ORB特征點提取原理

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

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

　　定義矩的計算公式為：

(1)

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

(2)

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

(3)

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

　　ORB的特征點檢測法則是將FAST特征點檢測法同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ī)器人的真實位置。這個時候就只能去與所有的關(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)生成的地圖點中找到更多的對應(yīng)關(guān)系對結(jié)果進(jìn)行精確化。由于全局圖中進(jìn)行投影的較為復(fù)雜，通常只是在局部圖中進(jìn)行投影。

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

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

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

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

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

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

　　2.2.2 冗余地圖點的剔除

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

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

　　通過檢測到的ORB特征點，查找Covisibility Graph中與之相連的關(guān)鍵幀Kc，匹配方法則是上文中提到的約束測試的方法進(jìn)行篩選匹配，并且將不滿足要求的點全部舍棄。對ORB特征點進(jìn)行三角化之后檢查正向的景深、視差、反投影誤差以及尺度一致性，如果無誤即可得到地圖點。通常一個地圖點由兩個關(guān)鍵幀所觀察得到的，并且它也可以投影到與之相連的其他關(guā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)生的外點會在優(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)檢測線程

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

　　接下來是對重復(fù)出現(xià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 實驗測試

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

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

4 結(jié)論

　　根據(jù)上述實驗結(jié)果來看，ORB-SLAM在實驗平臺上可以很好的實現(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ī)器人嵌入式實現(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十四講 從理論到實踐[M].北京:電子工業(yè)出版社,2017,3.

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

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