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

張汪洋 孫偉偉 肖琦 鹿飛

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

關(guān)鍵詞：ORB特征點(diǎn)檢測；ORB-SLAM；智能家庭清掃機(jī)器人

DOI：

10.3969/j.issn.1005-5517.2018.8.009

O 引言

近年來智能家居的發(fā)展使得我們的居家生活變得更加充滿樂趣與效率。然而對于像智能清掃機(jī)器人這類室內(nèi)移動(dòng)型機(jī)器人而言，機(jī)器人自行定位和對周邊環(huán)境的識(shí)別是其能否正常高效工作的根本。激光或相機(jī)進(jìn)行實(shí)時(shí)的定位與構(gòu)圖（simultaneous localization andmapping，SLAM）[1]一～種相對比較完善的移動(dòng)機(jī)器人自主定位的方法，當(dāng)機(jī)器人處于一個(gè)未知的環(huán)境中，通過其所攜帶的圖像采集模塊對機(jī)器人周邊的環(huán)境進(jìn)行觀測，再遞增地構(gòu)建環(huán)境地圖，并且通過構(gòu)建的環(huán)境地圖對機(jī)器人實(shí)現(xiàn)實(shí)時(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)修正方法…并采用ORB特征點(diǎn)進(jìn)行跟蹤、構(gòu)圖、重定位、閉環(huán)修正以及初始化，并且算法的環(huán)境適應(yīng)力強(qiáng)，對劇烈運(yùn)動(dòng)也很魯棒。正因?yàn)镺RB-SLAM是基于特征點(diǎn)的SLAM系統(tǒng)，因此其能夠?qū)崟r(shí)的計(jì)算出相機(jī)的運(yùn)動(dòng)軌跡，并且生成稀疏的場景三維重建結(jié)果。

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

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

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

定義矩的計(jì)算公式為：

特征點(diǎn)描述則是根據(jù)BRIEF （B；nary RobustIndependent 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 0RB-SLAM算法原理

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

2.1 追蹤階段

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

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

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

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

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

在初始姿態(tài)的估計(jì)進(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)估計(jì)的精確度與魯棒性的保證。

2.1.3 選取關(guān)鍵幀

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

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

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

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

當(dāng)追蹤完成之后我們可以得到一個(gè)新關(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，同時(shí)對關(guān)鍵幀Ki的生長樹和能夠共享地圖點(diǎn)的關(guān)鍵幀節(jié)點(diǎn)相互連接的邊進(jìn)行更新，并計(jì)算表示關(guān)鍵幀的詞袋BOW。之后依據(jù)計(jì)算詞袋的結(jié)果將地圖點(diǎn)整合到新的關(guān)鍵幀之后，對Covisibity Graph進(jìn)行更新，把新的關(guān)鍵幀插入Map中。

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

為了保證地圖點(diǎn)可追蹤，且不易在三角化時(shí)出現(xiàn)較大的誤差，則需要在創(chuàng)建點(diǎn)云的前三幀進(jìn)行約束測試，通過的地圖點(diǎn)才可以真正的被保存。通常一個(gè)點(diǎn)同時(shí)滿足：該點(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)則它必須被超過兩個(gè)關(guān)鍵幀觀察到（硬件如果是雙目攝像頭則為三個(gè)關(guān)鍵幀）。當(dāng)?shù)貓D點(diǎn)被少于三個(gè)關(guān)鍵幀能夠觀察到時(shí)該點(diǎn)就會(huì)被當(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)。通常一個(gè)地圖點(diǎn)由兩個(gè)關(guān)鍵幀所觀察得到的，并且它也可以投影到與之相連的其他關(guān)鍵幀中，此時(shí)為了得到更多的地圖點(diǎn)一般算法會(huì)使用追蹤局部地圖的方法來在附近的關(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)化中的約束，并不作為變量去改變值。同時(shí)優(yōu)化時(shí)產(chǎn)生的外點(diǎn)會(huì)在優(yōu)化的中后期進(jìn)行剔除。

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

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

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

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

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

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

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

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

最后對上述所得到的位姿圖用本質(zhì)圖（ Essential Graph）進(jìn)行優(yōu)化，將閉

環(huán)的誤差進(jìn)行分散，保證閉環(huán)檢測的準(zhǔn)確性。

3 實(shí)驗(yàn)測試

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

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

4 結(jié)論

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

參考文獻(xiàn)：

[1]周紹磊，吳修振，劉剛，等一種單目視覺ORB-SLAM，INS組臺(tái)導(dǎo)航方法[J]中國慣性技術(shù)報(bào)，2016，24（5）：633-637

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

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

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

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