張 震 ，鄭 宏 ，周 璇，張生群

1.武漢大學(xué) 電子信息學(xué)院，武漢 430072

2.武漢大學(xué) 大疆創(chuàng)新無(wú)人機(jī)實(shí)驗(yàn)室，武漢 430072

1 引言

當(dāng)機(jī)器人處于一個(gè)完全陌生的環(huán)境中時(shí)，如何通過(guò)自身傳感器獲取到的信息，完成自身的定位和對(duì)周?chē)h(huán)境的建模，是一項(xiàng)具有重要意義同時(shí)又極具挑戰(zhàn)性的工作，一般將這個(gè)問(wèn)題稱(chēng)為同時(shí)定位與建圖（Simultaneous Localization and Mapping，SLAM）。SLAM被認(rèn)為是真正實(shí)現(xiàn)機(jī)器人自主運(yùn)動(dòng)最核心的部分[1]。目前常用的方法采用激光雷達(dá)獲取周?chē)系K物信息，但受限于激光雷達(dá)昂貴的價(jià)格，很難進(jìn)行廣泛的應(yīng)用。微軟于2010年推出了可同時(shí)獲得彩色圖與深度圖的相機(jī)——Kinect。由于其低廉的價(jià)格，迅速在計(jì)算機(jī)視覺(jué)以及機(jī)器人領(lǐng)域得到了廣泛的應(yīng)用。

當(dāng)前求解SLAM問(wèn)題主要可以分為兩種方法。傳統(tǒng)的方法將定位看作一個(gè)狀態(tài)估計(jì)問(wèn)題：在給定了從初始時(shí)刻到當(dāng)前時(shí)刻的控制輸入以及觀測(cè)數(shù)據(jù)的條件下，構(gòu)建一個(gè)聯(lián)合后驗(yàn)概率密度函數(shù)來(lái)描述當(dāng)前位姿和地圖特征的空間位置。通過(guò)遞歸的貝葉斯濾波方法對(duì)此概率密度函數(shù)加以估計(jì)，從而實(shí)現(xiàn)同步定位和地圖創(chuàng)建。根據(jù)濾波器選擇的不同，可以分為基于擴(kuò)展卡爾曼濾波（EKF）的方法，基于擴(kuò)展信息濾波（EIF）的方法和基于粒子濾波（PF）的方法等?；跒V波器的方法主要存在兩個(gè)問(wèn)題：一是線性化模型導(dǎo)致的累計(jì)誤差，二是高計(jì)算復(fù)雜度導(dǎo)致更新效率緩慢，因此很難被應(yīng)用到大規(guī)模環(huán)境地圖創(chuàng)建中去。這都是由濾波器模型本身的缺陷所造成的。Lu和Milios于1997年首次提出了一種基于圖優(yōu)化的SLAM方法[2]，他們以激光傳感器為基礎(chǔ)，闡述了圖優(yōu)化SLAM方法的基本思想。這種方法很快在基于視覺(jué)的SLAM方法中（Visual-based slam，V-slam）得到了廣泛的應(yīng)用?；趫D優(yōu)化的SLAM方法將相機(jī)的位姿軌跡描述成圖的形式，其中，圖的節(jié)點(diǎn)代表相機(jī)的位姿估計(jì)或環(huán)境特征點(diǎn)，圖的邊表示位姿之間的約束關(guān)系。通過(guò)對(duì)不一致的約束進(jìn)行優(yōu)化，可以得到更加精確的相機(jī)位姿。

Henry[3]等人最早提出了一種使用深度相機(jī)的SLAM算法。該算法首先提取圖像的SIFT特征點(diǎn)，然后采用迭代最近點(diǎn)（Iterative Closest Point，ICP）算法求解兩幀圖像之間的運(yùn)動(dòng)變換，最后利用圖優(yōu)化工具TORO進(jìn)行全局優(yōu)化。Engelhard等人[4]采用SURF（Speed-Up Robust Features）特征點(diǎn)取代SIFT特征點(diǎn)進(jìn)行特征匹配，然后結(jié)合深度信息，獲得三維的匹配點(diǎn)對(duì)集合，接著采用隨機(jī)采樣一致性（Random Sample Consensus，RANSAC）算法求出相鄰兩幀的初始運(yùn)動(dòng)變換，并將其作為ICP算法的初值進(jìn)一步優(yōu)化，最后采用Hogman位姿圖優(yōu)化方法求解出全局最優(yōu)位姿。Endres[5]等人采用環(huán)境評(píng)估模型（Environment Measurement Model，EMM）來(lái)進(jìn)一步判斷RANSAC和ICP求解出的運(yùn)動(dòng)變換是否正確，在后端則采用通用圖優(yōu)化工具g2o優(yōu)化全局位姿。Gao[6]等人認(rèn)為物體邊緣附近的深度值通常是不準(zhǔn)確的，而特征點(diǎn)又往往分布在物體的邊緣，這種矛盾會(huì)影響ICP算法的準(zhǔn)確性，所以提出了一種算法來(lái)篩選出平面特征點(diǎn)（planar point features）。ElGhor等人[7]則通過(guò)提取平面來(lái)給平面邊緣的特征點(diǎn)賦予一個(gè)更加可信的深度值來(lái)解決上述問(wèn)題。還有一些研究人員從其他角度入手改進(jìn)算法。Houben等人[8]通過(guò)使用多個(gè)攝像頭來(lái)解決無(wú)結(jié)構(gòu)的平面缺少特征點(diǎn)和重復(fù)的紋理難以進(jìn)行匹配等問(wèn)題。Ma等人[9]通過(guò)融合慣性測(cè)量單元（Inertial Measurement Unit，IMU）的數(shù)據(jù)來(lái)解決戶外大場(chǎng)景下的SLAM問(wèn)題。

由于采用SIFT或SURF特征，所以計(jì)算速度較慢，一般需要通過(guò)GPU進(jìn)行加速，所以無(wú)法應(yīng)用到?jīng)]有GPU或者GPU運(yùn)算能力弱的設(shè)備上。此外，上述算法在閉環(huán)檢測(cè)上的效率也較低。

針對(duì)上述問(wèn)題，本文提出了一種結(jié)合ORB特征和視覺(jué)詞典的RGB-D SLAM算法。該算法采用ORB特征描述符，不需要經(jīng)過(guò)GPU加速，使其可以應(yīng)用于沒(méi)有GPU或者GPU運(yùn)算能力弱的設(shè)備上。同時(shí)由于采用了視覺(jué)詞典，使得閉環(huán)檢測(cè)的效率和實(shí)時(shí)性得到了提高。

2 基本原理和實(shí)現(xiàn)方法

算法的整體流程如圖1描述。在前端，首先提取圖像的ORB特征，然后利用kNN匹配找到對(duì)應(yīng)的最臨近與次臨近匹配，然后采用比值檢測(cè)與交叉檢測(cè)剔除誤匹配點(diǎn)，接著采用改進(jìn)的PROSAC-PnP算法計(jì)算相鄰幀之間位姿變換，并判斷該幀能否成為關(guān)鍵幀。在后端，利用基于BoW（Bag of Words）的閉環(huán)檢測(cè)算法進(jìn)行閉環(huán)檢測(cè)，采用倒排索引記錄每個(gè)單詞對(duì)應(yīng)的關(guān)鍵幀集合，從倒排索引中找出與當(dāng)前幀具有共同單詞較多的幀計(jì)算得分，挑選得分高的進(jìn)行特征匹配，判斷是否發(fā)生回環(huán)，將回環(huán)作為新的邊約束添加到姿態(tài)圖中。最后，利用通用圖優(yōu)化工具g2o對(duì)姿態(tài)圖進(jìn)行全局優(yōu)化，得到全局一致的位姿與點(diǎn)云地圖。

2.1 ORB特征提取

在SLAM領(lǐng)域中，前端（Front End）一般研究相鄰兩幀之間的配準(zhǔn)，以此來(lái)獲得相機(jī)位置的估計(jì)。在前端的處理中有兩種主要的方法：直接法（Direct Method）和基于特征的方法（Feature Based Method）。對(duì)于基于特征的方法而言，特征與描述符的選擇對(duì)配準(zhǔn)的速度與準(zhǔn)確性有著至關(guān)重要的影響。在傳統(tǒng)的RGB-D SLAM算法中，SIFT[10]和SURF[11]特征描述符是兩種非常流行的選擇。但是無(wú)論是SIFT還是SURF都要求很長(zhǎng)的時(shí)間來(lái)計(jì)算特征描述符。一些算法采用GPU加速的SIFT算法（SiftGPU）[12]來(lái)加快特征描述符的計(jì)算，但是這種方法對(duì)于嵌入式設(shè)備或者缺乏GPU運(yùn)算功能的設(shè)備來(lái)說(shuō)并不適用。Rublee等人[13]于2011年提出了一種新的特征描述符計(jì)算方法，稱(chēng)為ORB算法。ORB算法采用改進(jìn)的FAST角點(diǎn)檢測(cè)方法獲取特征點(diǎn)，采用BRIEF特征描述子生成特征描述符。由于FAST和BRIEF的計(jì)算速度都非常的快，所以相較于SIFT和SURF而言，ORB算法在速度上具有絕對(duì)的優(yōu)勢(shì)，不需要經(jīng)過(guò)GPU加速，適合在嵌入式設(shè)備或缺少GPU運(yùn)算能力的設(shè)備上使用。

圖1 算法整體流程圖

ORB算法包含oFAST特征點(diǎn)檢測(cè)和rBRIEF描述符提取兩個(gè)步驟。FAST特征點(diǎn)不具備尺度不變性，同時(shí)也不具有方向信息。為了解決這兩個(gè)問(wèn)題，ORB算法用圖像金字塔方法處理圖像并在每一層金字塔尺度上檢測(cè)特征點(diǎn)，之后再對(duì)特征點(diǎn)加上方向信息，構(gòu)成了具備尺度不變性與方向信息的oFAST特征點(diǎn)。

ORB算法采用intensity centroid[14]計(jì)算方向信息。Rosin定義圖像塊的矩為：

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

因此，特征點(diǎn)的方向就可以簡(jiǎn)單地表示為：

隨后，BRIEF描述符被用來(lái)生成對(duì)檢測(cè)到的特征點(diǎn)的描述。BRIEF描述符具有計(jì)算速度快，表示簡(jiǎn)單和占用空間小的特點(diǎn)，但是不具備方向不變性。對(duì)于特征點(diǎn)周?chē)笮镹的點(diǎn)集，定義S為一個(gè)2×N的矩陣：

根據(jù)圖像塊的旋轉(zhuǎn)信息θ和相應(yīng)的旋轉(zhuǎn)矩陣Rθ，計(jì)算帶旋轉(zhuǎn)信息的矩陣Sθ=RθS。新的特征描述符可由如下的公式表示：

2.2 基于PROSAC-PnP的圖像配準(zhǔn)

在特征匹配時(shí)，誤匹配是經(jīng)常出現(xiàn)的，并會(huì)對(duì)圖像配準(zhǔn)的準(zhǔn)確性產(chǎn)生巨大的影響。因此，如何去除誤匹配點(diǎn)，得到魯棒性強(qiáng)的匹配顯得至關(guān)重要。本文所采用的算法步驟如下描述：

（1）采用ORB算法提取相鄰幀的特征點(diǎn)并計(jì)算特征描述符。

（2）采用kNN算法計(jì)算兩幅圖像的最臨近匹配以及次臨近匹配，得到匹配向量M1、M2。

（3）移除最臨近匹配與次臨近匹配的距離比值大于給定閾值ratio的匹配，得到過(guò)濾后的匹配向量FM1、FM2。

（4）對(duì)FM1、FM2進(jìn)行交叉匹配：只有同時(shí)出現(xiàn)在FM1和FM2中的配對(duì)點(diǎn)才被認(rèn)為是合法的匹配，得到最終的魯棒性強(qiáng)的匹配GM，GM被用于下一步的圖像配準(zhǔn)。

如圖2顯示的是原始的匹配結(jié)果與經(jīng)過(guò)過(guò)濾后的匹配結(jié)果，可以看到經(jīng)過(guò)算法過(guò)濾后，得到的配對(duì)點(diǎn)質(zhì)量有了明顯的提高。

圖2 匹配結(jié)果

傳統(tǒng)的算法大多采用ICP與RANSAC結(jié)合的方法進(jìn)行圖像配準(zhǔn)。RANSAC算法存在的問(wèn)題是它隨機(jī)選取匹配點(diǎn)進(jìn)行計(jì)算，而忽略了匹配點(diǎn)的差異性。事實(shí)上在得到的配對(duì)點(diǎn)中，有一些也是誤匹配的外點(diǎn)，即匹配點(diǎn)的質(zhì)量存在差別。為此，本文結(jié)合PROSAC[15]算法，提出了一種RPOSAC-PnP算法，來(lái)進(jìn)行圖像配準(zhǔn)，算法步驟如下描述：

（1）對(duì)得到的匹配點(diǎn)按照最臨近與次臨近距離的比值進(jìn)行排序，即認(rèn)為最臨近與次臨近距離相差越大的匹配點(diǎn)匹配質(zhì)量越高。

（2）在排序后的點(diǎn)中選取前n個(gè)點(diǎn)作為高質(zhì)量匹配點(diǎn)作為RANSAC算法的輸入，不足n點(diǎn)則全部選取。

（3）在n個(gè)點(diǎn)中選取m個(gè)點(diǎn)，然后采用透視n點(diǎn)（PnP）算法計(jì)算變換矩陣T。

（4）對(duì)于剩余的未參與計(jì)算的點(diǎn)，分別應(yīng)用T去測(cè)試，當(dāng)誤差小于某個(gè)閾值時(shí)，認(rèn)為該點(diǎn)適用于當(dāng)前模型參數(shù)T，將其視為內(nèi)點(diǎn)。

（5）統(tǒng)計(jì)測(cè)試得到的內(nèi)點(diǎn)個(gè)數(shù)，當(dāng)內(nèi)點(diǎn)數(shù)目大于給定的閾值并且當(dāng)前模型內(nèi)點(diǎn)數(shù)目大于上一次模型內(nèi)點(diǎn)數(shù)目的時(shí)候，將模型參數(shù)替換為當(dāng)前計(jì)算得到的參數(shù)，反之，則舍棄。

（6）如果迭代次數(shù)達(dá)到上限，則退出，否則重復(fù)從步驟（3）執(zhí)行。

PROSAC-PnP算法基于一種假設(shè)，該假設(shè)認(rèn)為最臨近距離與次臨近距離差別越大，得到的匹配點(diǎn)匹配質(zhì)量越高。采用PROSAC算法后，可以適當(dāng)減小算法的迭代次數(shù)，又因?yàn)檫x取的都是高質(zhì)量的匹配內(nèi)點(diǎn)，所以計(jì)算的速度和魯棒性都會(huì)有所提升。

2.3 基于BOW的閉環(huán)檢測(cè)

在相機(jī)姿態(tài)的計(jì)算過(guò)程中，由于只用到了上一幀的數(shù)據(jù)來(lái)進(jìn)行計(jì)算，所以計(jì)算的結(jié)果會(huì)存在累計(jì)誤差。典型的情況是當(dāng)相機(jī)運(yùn)動(dòng)了一圈又回到起點(diǎn)的時(shí)候，相機(jī)的位姿估計(jì)會(huì)與初始位姿存在明顯的誤差。如果機(jī)器人可以識(shí)別出一個(gè)曾經(jīng)來(lái)到過(guò)的地方，那么在當(dāng)前幀與歷史幀之間就可以建立起一種約束，利用這種約束就可以修正累計(jì)誤差，得到更為精確的相機(jī)姿態(tài)。這個(gè)過(guò)程在SLAM中稱(chēng)為閉環(huán)檢測(cè)。

圖優(yōu)化理論產(chǎn)生以來(lái)，出現(xiàn)了很多閉環(huán)檢測(cè)的方法[16-18]。目前流行的閉環(huán)檢測(cè)方法主要有兩種：基于圖像特征匹配的方法和基于詞袋（BoW）的方法?；趫D像特征匹配的方法將當(dāng)前幀與已有的關(guān)鍵幀進(jìn)行特征匹配，根據(jù)匹配的結(jié)果判斷是否發(fā)生了閉環(huán)。隨著算法的運(yùn)行，關(guān)鍵幀會(huì)變得越來(lái)越多，而與每一幀都進(jìn)行匹配的策略將導(dǎo)致算法的運(yùn)行時(shí)間越來(lái)越長(zhǎng)。為了解決這個(gè)問(wèn)題，可以從關(guān)鍵幀中選取一個(gè)較小的子集進(jìn)行匹配，閉環(huán)檢測(cè)的質(zhì)量也將嚴(yán)重依賴于子集的選取。Nister[19]等人于2006年提出了一種基于詞袋模型的圖像檢索方法，該方法在原始詞袋模型的基礎(chǔ)上采用N層的樹(shù)形結(jié)構(gòu)描述和構(gòu)建詞匯樹(shù)，這一方式大大提高了檢索效率。

詞匯樹(shù)的構(gòu)建過(guò)程是一個(gè)遞歸的過(guò)程。首先利用K-means聚類(lèi)方法將訓(xùn)練集中的所有特征向量分成K類(lèi)，然后對(duì)于每一類(lèi)，遞歸的采用同樣的方式進(jìn)行聚類(lèi)，直到達(dá)到所設(shè)定的層數(shù)。這樣，整個(gè)詞匯樹(shù)就被表示成了一個(gè)K叉樹(shù)的結(jié)構(gòu)，如圖3所示。

圖3 詞匯樹(shù)

在構(gòu)建完成整個(gè)詞匯樹(shù)之后，需要對(duì)每個(gè)葉節(jié)點(diǎn)賦予一個(gè)權(quán)重w，該權(quán)重的值是基于單詞的逆向文本頻率，也就是說(shuō)：頻繁出現(xiàn)的詞匯將具有較小的權(quán)重，而出現(xiàn)次數(shù)很少的具有代表性的詞匯具有較大的權(quán)重。權(quán)重w可通過(guò)下式計(jì)算：

其中，N是數(shù)據(jù)庫(kù)中圖片數(shù)目的總和，Ni是數(shù)據(jù)庫(kù)中所有具有詞匯i的圖片的總和。然后待檢索圖片的特征向量和數(shù)據(jù)庫(kù)圖片的特征向量就可以表示成如下形式：

其中，ni和mi分別表示待檢索圖片和數(shù)據(jù)庫(kù)圖片中具有單詞i的數(shù)目。然后，待檢索圖片與數(shù)據(jù)庫(kù)圖片之間的相似程度可通過(guò)下式得到：

有了上式，就可以計(jì)算出當(dāng)前幀與舊的關(guān)鍵幀的相似程度。將當(dāng)前幀與舊關(guān)鍵幀的每一幀都進(jìn)行比較是不明智的，應(yīng)該選取具有共同單詞數(shù)較多的關(guān)鍵幀來(lái)進(jìn)行比較。為此需要建立關(guān)鍵幀的倒排索引，來(lái)記錄包含特定單詞的所有關(guān)鍵幀。這樣對(duì)于新來(lái)的一幀，首先計(jì)算出該幀包含的單詞表，然后對(duì)于每一個(gè)單詞，通過(guò)倒排索引查找具有該單詞的所有關(guān)鍵幀，同時(shí)記錄每個(gè)關(guān)鍵幀與當(dāng)前幀的共同單詞的個(gè)數(shù)。隨后在所有與當(dāng)前幀具有共同單詞的關(guān)鍵幀中選取那些公共單詞個(gè)數(shù)超過(guò)給定個(gè)數(shù)N的關(guān)鍵幀，分別計(jì)算得分。最后再選取得分超過(guò)給定閾值的關(guān)鍵幀，作為候選的可能與當(dāng)前幀構(gòu)成回環(huán)的幀，再進(jìn)行常規(guī)的特征匹配判斷，得到最后的回環(huán)的判定。若在隨后的處理中當(dāng)前幀被選定為關(guān)鍵幀，則需要更新關(guān)鍵幀的倒排索引。閉環(huán)檢測(cè)的流程圖如圖4所示。

2.4 全局優(yōu)化

圖4 閉環(huán)檢測(cè)流程圖

在視覺(jué)SLAM中建立的三維的點(diǎn)云地圖，對(duì)姿態(tài)一般采用6自由度建模描述。在這之中，又涉及到是否需要將圖像的特征點(diǎn)作為節(jié)點(diǎn)加入到圖結(jié)構(gòu)，并在特征點(diǎn)節(jié)點(diǎn)之間與關(guān)鍵幀節(jié)點(diǎn)之間添加邊約束。這會(huì)在圖結(jié)構(gòu)中生成更多的約束，所以會(huì)使優(yōu)化的準(zhǔn)確性提高。但是由于每幀圖像都包含數(shù)量眾多的特征點(diǎn)（幾十甚至上百個(gè)），這會(huì)使得整個(gè)圖結(jié)構(gòu)變得巨大無(wú)比，給優(yōu)化帶來(lái)不便。因此，本文只對(duì)姿態(tài)圖（即不包括特征點(diǎn)節(jié)點(diǎn)的圖）進(jìn)行優(yōu)化。

姿態(tài)圖的節(jié)點(diǎn)代表相機(jī)的6自由度姿態(tài)：

姿態(tài)圖的邊代表兩個(gè)姿態(tài)的變換關(guān)系：

這樣，對(duì)于兩個(gè)由邊連接的節(jié)點(diǎn)，會(huì)有一個(gè)誤差：

其中，i,j屬于節(jié)點(diǎn)集合C,Ωi,j表示xi和xj之間的信息矩陣，E(x)就是要優(yōu)化的誤差函數(shù)，即找到一個(gè)序列X，使得：

整個(gè)問(wèn)題被轉(zhuǎn)化成了一個(gè)非線性最小二乘問(wèn)題。g2o[20]是一個(gè)流行的圖優(yōu)化求解工具，可用來(lái)求解上述優(yōu)化問(wèn)題。優(yōu)化過(guò)程中大部分時(shí)間都消耗在線性方程組的求解上，g2o提供了三種不同的線性求解器：CSparse和CHOLMOD基于喬里斯基分解（Cholesky decomposition），PCG則基于預(yù)處理共軛梯度算法（preconditioned conjugate gradient）。CSparse和CHOLMOD受初始值設(shè)定的影響較小，PCG在給定較好初值的情況下可以很快的收斂。本文采用CSparse求解器進(jìn)行求解。

2.5 丟失恢復(fù)

在相機(jī)運(yùn)動(dòng)過(guò)程中，由于種種原因可能會(huì)出現(xiàn)圖像失配的情況。如果不對(duì)這種情況進(jìn)行處理，將嚴(yán)重影響以后的定位，甚至?xí)斐烧麄€(gè)算法崩潰。為此，本文加入了丟失恢復(fù)功能：當(dāng)連續(xù)的N幀圖像都出現(xiàn)失配的時(shí)候，立即將當(dāng)前幀圖像設(shè)為關(guān)鍵幀，并對(duì)當(dāng)前幀進(jìn)行閉環(huán)檢測(cè)，以期待找到所有的與當(dāng)前幀相近的場(chǎng)景并建立起幀間約束，降低運(yùn)動(dòng)丟失對(duì)定位造成的影響。

3 實(shí)驗(yàn)結(jié)果與分析

Sturm等人[21]使用Kinect記錄了一系列的圖像序列，同時(shí)用一個(gè)運(yùn)動(dòng)捕捉系統(tǒng)記錄了相機(jī)的精確位姿信息。圖像序列包括彩色圖和深度圖，分辨率為640×480，幀率為30 Hz，真實(shí)軌跡值由8個(gè)高速追蹤相機(jī)（100 Hz）獲得。整個(gè)數(shù)據(jù)庫(kù)包括39個(gè)序列，包括辦公室場(chǎng)景和工廠廠房。同時(shí)，他們還提供了一個(gè)自動(dòng)評(píng)估工具，該工具可以自動(dòng)計(jì)算出真實(shí)軌跡與算法估計(jì)軌跡之間的均方根誤差（RMSE）。

為了驗(yàn)證本文算法的有效性，本文選取上述測(cè)試集中的數(shù)據(jù)進(jìn)行了定位與建圖實(shí)驗(yàn)，同時(shí)與文獻(xiàn)[22-23]進(jìn)行了對(duì)比。算法的實(shí)驗(yàn)平臺(tái)為：Intel i5 6200U處理器（2.3 GHz），4 GB RAM，64位Ubuntu16.04操作系統(tǒng)。

3.1 定位誤差分析

本文選取了fr1數(shù)據(jù)包中的fr1/xyz、fr1/rpy、fr1/desk、fr1/desk2、fr1/floor、fr1/room這6個(gè)數(shù)據(jù)集中的全部幀進(jìn)行實(shí)驗(yàn)，每組數(shù)據(jù)集進(jìn)行5次重復(fù)實(shí)驗(yàn)，記錄每個(gè)關(guān)鍵幀對(duì)應(yīng)的相機(jī)的位姿估計(jì)并采用文獻(xiàn)[21]提供的評(píng)估工具計(jì)算算法的RMSE。同時(shí)，在實(shí)驗(yàn)過(guò)程中記錄了算法的整體運(yùn)行時(shí)間以便對(duì)算法的運(yùn)行速度進(jìn)行評(píng)估。最后，根據(jù)得到的相機(jī)軌跡與原始的圖像數(shù)據(jù)生成三維點(diǎn)云圖。表1為本文算法的詳細(xì)實(shí)驗(yàn)結(jié)果。

表1 本文算法的詳細(xì)結(jié)果

為了體現(xiàn)本文算法的優(yōu)越性，本文與其他算法做了對(duì)比。表2為本文算法與文獻(xiàn)[22]和文獻(xiàn)[23]算法的均方根誤差對(duì)比。

表2 本文算法與文獻(xiàn)[22]、[23]均方根誤差對(duì)比m

由表2可以看出，對(duì)于fr1/rpy、fr1/desk、fr1/desk2、fr1/room數(shù)據(jù)包，本文算法都優(yōu)于其他兩種算法。對(duì)于fr1/rpy，本文算法誤差比文獻(xiàn)[22]低1.3 cm，比文獻(xiàn)[23]低0.3 cm；對(duì)于fr1/desk，本文算法誤差比文獻(xiàn)[22]低1.0 cm，比文獻(xiàn)[23]低0.6 cm；對(duì)于fr1/desk2，本文算法誤差比文獻(xiàn)[22]低2.5 cm，比文獻(xiàn)[23]低0.5 cm；對(duì)于fr1/room，本文算法誤差比文獻(xiàn)[22]低1.0 cm，比文獻(xiàn)[23]低4.1 cm。對(duì)于fr1/xyz數(shù)據(jù)包，由于該數(shù)據(jù)包本身的運(yùn)動(dòng)范圍不大，對(duì)于該數(shù)據(jù)包的位姿估計(jì)已經(jīng)比較精確了，本文的算法相比文獻(xiàn)[23]來(lái)講也沒(méi)有提高，與其持平。而對(duì)于fr1/floor數(shù)據(jù)包，本文算法弱于其他兩種算法，這是由于fr1/floor數(shù)據(jù)包的運(yùn)動(dòng)沒(méi)有太多的回環(huán)，所以算法的性能更多地依賴于算法前端的特征匹配與圖像配準(zhǔn)。ORB特征相較于SIFT與SURF而言，雖然速度較快但是相應(yīng)的魯棒性不如前兩者高，所以本文算法在該數(shù)據(jù)集上表現(xiàn)略差。從總體上來(lái)看，本文算法略優(yōu)于其他兩種算法。

3.2 運(yùn)行速度對(duì)比

由于文獻(xiàn)[23]只進(jìn)行了特征點(diǎn)的提取，而沒(méi)有進(jìn)行描述符提取和特征匹配，所以本文只與文獻(xiàn)[22]進(jìn)行了速度對(duì)比，對(duì)比結(jié)果如表3所示。

表3 本文算法與文獻(xiàn)[22]算法運(yùn)行速度對(duì)比s

文獻(xiàn)[22]的算法運(yùn)行在一個(gè)4核處理器的電腦上，本文算法運(yùn)行在一個(gè)雙核處理器的電腦上，主頻只有2.3 GHz。然而，本文算法的運(yùn)行速度要優(yōu)于文獻(xiàn)[22]。文獻(xiàn)[22]處理一幀數(shù)據(jù)平均用時(shí)0.35 s，即每秒鐘最多只能處理3幀數(shù)據(jù)，而本文算法處理一幀數(shù)據(jù)平均需要0.11 s，每秒可處理約10幀數(shù)據(jù)，速度是文獻(xiàn)[22]的3倍。文獻(xiàn)[22]已經(jīng)通過(guò)并行加速獲得了2～2.5倍的速度提升，而本文算法沒(méi)有采用任何形式的加速，這些都表明了本文算法的實(shí)時(shí)性相較于文獻(xiàn)[22]有了很大程度的提高。

3.3 地圖構(gòu)建分析

本文選取了fr1/desk和fr1/floor數(shù)據(jù)包進(jìn)行建圖分析。

fr1/desk是一個(gè)辦公室場(chǎng)景，為手持kinect拍攝所得。由圖5可以看到，該場(chǎng)景內(nèi)的辦公桌，電腦以及房間的墻壁都得到了比較好的重建。圖像左邊地面部分有一片黑色的空洞，這是由于拍攝過(guò)程中沒(méi)有掃過(guò)那塊地面造成的。

圖5 fr1/desk點(diǎn)云圖

fr1/floor數(shù)據(jù)集由機(jī)器人裝載kinect采集所得，共1 200余幀。由圖6可以看出，整個(gè)空間的輪廓也得到了較好的重建。

圖6 fr1/floor點(diǎn)云圖

4 結(jié)束語(yǔ)

本文提出了一種改進(jìn)的RGB-D SLAM算法來(lái)進(jìn)行同時(shí)定位與地圖構(gòu)建應(yīng)用研究。在SLAM前端，本文采用ORB特征取代傳統(tǒng)的SIFT和SURF特征，使其可以應(yīng)用在嵌入式設(shè)備或缺乏GPU運(yùn)算能力的設(shè)備上；同時(shí)采用改進(jìn)的PROSAC-PnP算法增強(qiáng)相機(jī)位姿計(jì)算的魯棒性。在SLAM后端，采用基于視覺(jué)詞典的閉環(huán)檢測(cè)算法進(jìn)行閉環(huán)檢測(cè)，通過(guò)通用圖優(yōu)化工具g2o對(duì)位姿圖進(jìn)行優(yōu)化，得到了全局一致的位姿。最后，本文選取了fr1數(shù)據(jù)包中的6個(gè)測(cè)試數(shù)據(jù)集進(jìn)行實(shí)驗(yàn)，并與文獻(xiàn)[22-23]進(jìn)行了誤差對(duì)比，表明了本文算法的優(yōu)越性。

由于本文算法采用ORB特征，相較于SIFT與SURF特征存在魯棒性不足的缺點(diǎn)。因此對(duì)于缺少路徑回環(huán)的場(chǎng)景來(lái)說(shuō)，需要做進(jìn)一步的改進(jìn)以提高算法可靠性。

[1]Durrant-Whyte H，Bailey T.Simultaneous localization and mapping：Part I[J].IEEE Robotics&Automation Magazine，2006，13（2）：99-110.

[2]Lu F，Milios E.Globally consistent range scan alignment for environment mapping[J].Autonomous Robots，1997，4（4）：333-349.

[3]Henry P，Krainin M，Herbst E，et al.RGB-D mapping：Using depth cameras for dense 3D modeling of indoor environments[C]//The 12th International Symposium on Experimental Robotics，2010，20：22-25.

[4]Engelhard N，Endres F，Hess J，et al.Real-time 3D visual SLAM with a hand-held RGB-D camera[C]//Proceedings of the RGB-D Workshop on 3D Perception in Robotics at the European Robotics Forum，Vasteras，Sweden，2011：1-15.

[5]Endres F，Hess J，Sturm J，et al.3-D mapping with an RGB-D camera[J].IEEE Transactions on Robotics，2014，30（1）：177-187.

[6]Gao X，Zhang T.Robust RGB-D simultaneous localization and mapping using planar point features[J].Robotics&Autonomous Systems，2015，72：1-14.

[7]Elghor H E，Roussel D，Ababsa F，et al.Planes detection for robust localization and mapping in RGB-D SLAM systems[C]//International Conference on 3D Vision，2015：452-459.

[8]Houben S，Quenzel J，Krombach N，et al.Efficient multicamera visual-inertial SLAM for micro aerial vehicles[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems，2016.

[9]Ma L，F(xiàn)alquez J M，Mcguire S，et al.Large scale dense visual inertial SLAM[M]//Field and Service Robotics.[S.l.]：Springer International Publishing，2016.

[10]Lowe D G.Distincitive image features from scale-invariant keypoints[J].International Journal of Computer Vision，2004，60（2）：91-110.

[11]Bay H，Tuytelaars T，Gool L V.SURF：Speeded up robust features[J].Computer Vision&Image Understanding，2006，110（3）：404-417.

[12]Wu Changchang.SiftGPU：A GPU implementation of scale invariant feature transform（SIFT）[EB/OL].（2011-04-19）.http：//www.cs.unc.edu/～ccwu/siftgpu/.

[13]Rublee E，Rabaud V，Konolige K，et al.ORB：An efficient alternative to SIFT or SURF[C]//International Conference on Computer Vision，2011：2564-2571.

[14]Rosin P L.Measuring corner properties[J].Computer Vision&Image Understanding，1999，73（2）：291-307.

[15]Chum O，Matas J.Matching with PROSAC-progressive sample consensus[C]//IEEE Computer Society Conference on Computer Vision and Pattern Recognition，2005：220-226.

[16]Labbe M，Michaud F.Online global loop closure detection for large-scale multi-session graph-based SLAM[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems，2014：2661-2666.

[17]Khan S，Wollherr D.IBuILD：Incremental bag of binary words for appearance based loop closure detection[C]//IEEE International Conference on Robotics and Automation，2015：5441-5447.

[18]Volkov M，Rosman G，Dan F，et al.Coresets for visual summarization with applicationsto loop closure[C]//IEEE International Conference on Robotics and Automation，2015：3638-3645.

[19]Nister D，Stewenius H.Scalable recognition with a vocabulary tree[C]//2006 IEEE Computer Society Conference on Computer Vision and Pattern Recognition，2006：2161-2168.

[20]Kümmerle R，Grisetti G，Strasdat H，et al.G2o：A general framework for graph optimization[C]//IEEE International Conference on Robotics and Automation，2011：3607-3613.

[21]Sturm J，Engelhard N，Endres F，et al.A benchmark for the evaluation of RGB-D SLAM systems[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems，2012：573-580.

[22]Endres F，Hess J，Engelhard N，et al.An evaluation of the RGB-D SLAM system[C]//IEEE International Conference on Robotics and Automation，2012：1691-1696.

[23]付夢(mèng)印，呂憲偉，劉彤，等.基于RGB-D數(shù)據(jù)的實(shí)時(shí)SLAM算法[J].機(jī)器人，2015，37（6）：683-692.