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

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

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

0 引言

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

　　近年來(lái)SLAM算法取得了巨大的發(fā)展，進(jìn)而產(chǎn)生了對(duì)單目攝像機(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)，對(duì)劇烈運(yùn)動(dòng)也很魯棒。正因?yàn)镺RB-SLAM是基于特征點(diǎn)的SLAM系統(tǒng)，因此其能夠?qū)崟r(shí)的計(jì)算出相機(jī)的運(yùn)動(dòng)軌跡，并且生成稀疏的場(chǎng)景三維重建結(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ā)展而來(lái)的，即為OFAST(FAST Key pointOrientation)也就是說(shuō)在使用FAST提取特征點(diǎn)之后再給特征點(diǎn)定義一個(gè)方向，以此來(lái)實(shí)現(xiàn)特征點(diǎn)的旋轉(zhuǎn)不變性。通常是使用灰度質(zhì)心法來(lái)計(jì)算以特征點(diǎn)為圓心半徑為r的圓形領(lǐng)域范圍內(nèi)的灰度質(zhì)心的位置，并根據(jù)從中心位置到質(zhì)心位置的向量來(lái)定義該特征點(diǎn)的主方向。

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

(1)

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

(2)

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

(3)

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

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

2 ORB-SLAM算法原理

　　ORB-SLAM算法通常利用三個(gè)線程分別進(jìn)行追蹤(Tracking)、地圖構(gòu)建(LocalMapping)和閉環(huán)檢測(cè)(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ì)在于使用上一幀的位姿與速度來(lái)估計(jì)當(dāng)前幀的位姿，而上一幀的速度則是由前面幾幀的位姿計(jì)算得出的。由于運(yùn)動(dòng)模型的計(jì)算本質(zhì)限制，運(yùn)動(dòng)模型通常適用于相對(duì)于機(jī)器人來(lái)說(shuō)勻速運(yùn)動(dòng)的物體。但是如果是運(yùn)動(dòng)相對(duì)隨意的目標(biāo)該模型就會(huì)失效，這個(gè)時(shí)候系統(tǒng)就會(huì)使用關(guān)鍵幀與重定位了。如果運(yùn)動(dòng)模型無(wú)法滿足條件時(shí)，系統(tǒng)會(huì)嘗試和距離當(dāng)前幀最近的一個(gè)關(guān)鍵幀去進(jìn)行匹配，在ORB-SLAM中利用了BOW(Bag of Words)來(lái)加速關(guān)鍵幀與當(dāng)前幀的匹配。如果當(dāng)前幀與鄰近幀的匹配也失效的話，則說(shuō)明此時(shí)的當(dāng)前幀已經(jīng)丟失，所以無(wú)法通過(guò)上述方法確定機(jī)器人的真實(shí)位置。這個(gè)時(shí)候就只能去與所有的關(guān)鍵幀進(jìn)行匹配，以此來(lái)查找合適的位置。

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

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

　　2.1.3 選取關(guān)鍵幀

　　當(dāng)追蹤階段完成之后首先對(duì)運(yùn)動(dòng)模型進(jìn)行更新，并且通過(guò)之前的計(jì)算我們得到一個(gè)對(duì)機(jī)器人位姿的初始估計(jì)，之后通過(guò)投影從已經(jīng)生成的地圖點(diǎn)中找到更多的對(duì)應(yīng)關(guān)系對(duì)結(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可見(jiàn)的每個(gè)地圖云點(diǎn)我們首先將地圖點(diǎn)對(duì)當(dāng)前幀進(jìn)行投影，倘若超出了圖像的范圍就將其舍棄;之后計(jì)算當(dāng)前視線方向向量v與地圖點(diǎn)云平均視線方向的方向向量n的夾角，舍棄n-v

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

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

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

　　為了保證地圖點(diǎn)可追蹤，且不易在三角化時(shí)出現(xiàn)較大的誤差，則需要在創(chuàng)建點(diǎn)云的前三幀進(jìn)行約束測(cè)試，通過(guò)的地圖點(diǎn)才可以真正的被保存。通常一個(gè)點(diǎn)同時(shí)滿足：該點(diǎn)應(yīng)當(dāng)在可以觀察到該點(diǎn)的關(guān)鍵幀中，并且應(yīng)有超過(guò)25%的關(guān)鍵幀可以對(duì)該點(diǎn)進(jìn)行跟蹤。若要構(gòu)建該點(diǎn)為地圖點(diǎn)則它必須被超過(guò)兩個(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)建

　　通過(guò)檢測(cè)到的ORB特征點(diǎn)，查找Covisibility Graph中與之相連的關(guān)鍵幀Kc，匹配方法則是上文中提到的約束測(cè)試的方法進(jìn)行篩選匹配，并且將不滿足要求的點(diǎn)全部舍棄。對(duì)ORB特征點(diǎn)進(jìn)行三角化之后檢查正向的景深、視差、反投影誤差以及尺度一致性，如果無(wú)誤即可得到地圖點(diǎn)。通常一個(gè)地圖點(diǎn)由兩個(gè)關(guān)鍵幀所觀察得到的，并且它也可以投影到與之相連的其他關(guān)鍵幀中，此時(shí)為了得到更多的地圖點(diǎn)一般算法會(huì)使用追蹤局部地圖的方法來(lái)在附近的關(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ì)檢測(cè)并剔除冗余的關(guān)鍵幀，這樣可以有力的控制因?yàn)殛P(guān)鍵幀數(shù)目增長(zhǎng)之后集束調(diào)整的復(fù)雜程度。

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

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

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

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

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

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

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

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

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

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

4 結(jié)論

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

　　參考文獻(xiàn)：

　　[1]周紹磊,吳修振,劉剛,等.一種單目視覺(jué)ORB-SLAM/INS組合導(dǎo)航方法[J].中國(guó)慣性技術(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]劉浩敏,章國(guó)鋒,鮑虎軍.基于單目視覺(jué)的同時(shí)定位與地圖構(gòu)建方法綜述[J].計(jì)算機(jī)輔助設(shè)計(jì)與圖形學(xué)學(xué)報(bào),2016,28(6):855-867.

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

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

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