一種結(jié)合ORB特征和視覺詞典的RGB-D SLAM算法

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

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

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

1 引言

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

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

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

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

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

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

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

2.1 ORB特征提取

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

圖1 算法整體流程圖

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

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

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

因此，特征點的方向就可以簡單地表示為：

隨后，BRIEF描述符被用來生成對檢測到的特征點的描述。BRIEF描述符具有計算速度快，表示簡單和占用空間小的特點，但是不具備方向不變性。對于特征點周圍大小為N的點集，定義S為一個2×N的矩陣：

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

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

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

（1）采用ORB算法提取相鄰幀的特征點并計算特征描述符。

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

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

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

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

圖2 匹配結(jié)果

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

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

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

（3）在n個點中選取m個點，然后采用透視n點（PnP）算法計算變換矩陣T。

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

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

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

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

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

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

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

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

圖3 詞匯樹

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

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

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

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

2.4 全局優(yōu)化

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

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

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

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

這樣，對于兩個由邊連接的節(jié)點，會有一個誤差：

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

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

2.5 丟失恢復(fù)

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

3 實驗結(jié)果與分析

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

為了驗證本文算法的有效性，本文選取上述測試集中的數(shù)據(jù)進(jìn)行了定位與建圖實驗，同時與文獻(xiàn)[22-23]進(jìn)行了對比。算法的實驗平臺為：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個數(shù)據(jù)集中的全部幀進(jìn)行實驗，每組數(shù)據(jù)集進(jìn)行5次重復(fù)實驗，記錄每個關(guān)鍵幀對應(yīng)的相機(jī)的位姿估計并采用文獻(xiàn)[21]提供的評估工具計算算法的RMSE。同時，在實驗過程中記錄了算法的整體運(yùn)行時間以便對算法的運(yùn)行速度進(jìn)行評估。最后，根據(jù)得到的相機(jī)軌跡與原始的圖像數(shù)據(jù)生成三維點云圖。表1為本文算法的詳細(xì)實驗結(jié)果。

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

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

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

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

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

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

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

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

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

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

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

圖5 fr1/desk點云圖

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

圖6 fr1/floor點云圖

4 結(jié)束語

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

由于本文算法采用ORB特征，相較于SIFT與SURF特征存在魯棒性不足的缺點。因此對于缺少路徑回環(huán)的場景來說，需要做進(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]付夢印，呂憲偉，劉彤，等.基于RGB-D數(shù)據(jù)的實時SLAM算法[J].機(jī)器人，2015，37（6）：683-692.