張光耀，倪益華，呂艷，倪忠進(jìn)，黃通交

(浙江農(nóng)林大學(xué) 工程學(xué)院，浙江 杭州 310000)

0 引 言

隨著移動(dòng)機(jī)器人技術(shù)發(fā)展，同時(shí)定位與建圖(simultaneous localization and mapping，SLAM)受到越來越多的關(guān)注。SLAM按傳感器種類分為激光SLAM與視覺SLAM兩大類[1]。激光SLAM目前較為成熟，已進(jìn)入商業(yè)應(yīng)用階段。但由于激光雷達(dá)價(jià)格高、體積大、安裝復(fù)雜等原因，使激光SLAM應(yīng)用受到很大限制。視覺SLAM因視覺攝像頭價(jià)格低、機(jī)器視覺技術(shù)飛速發(fā)展而成為研究熱點(diǎn)。

2007年，A.J.Davisin等[2]提出了第一個(gè)視覺SLAM：MonoSLAM,其前端為Shi-Tomasi特征提取，后端為擴(kuò)展卡爾曼濾波優(yōu)化算法，該方法存在計(jì)算量大、精度低、實(shí)用性差等問題；J.Stückler等[3]2012年提出了DVO SLAM，其前端為稠密的視覺里程計(jì)，后端采用g2o算法框架，回環(huán)檢測是對關(guān)鍵幀的遍歷特征檢測，構(gòu)建的地圖為點(diǎn)云地圖，但其精度不高，在TUM數(shù)據(jù)集上平均精度僅0.416 m[4]，且地圖為點(diǎn)云地圖，不能直接用于后續(xù)導(dǎo)航等，實(shí)用性低；M.Labbe等[5]2014年提出了RTABMAP，其前端為SIFT特征，后端為圖優(yōu)化，回環(huán)檢測基于詞袋模型，輸出地圖為點(diǎn)云地圖。該方法計(jì)算量仍較大，精度不高，在TUM數(shù)據(jù)集上平均精度為0.22 m[4]，地圖為點(diǎn)云地圖，實(shí)用性差；R.Mur-Artal等[6]2015年提出了ORB-SLAM，其前端為ORB特征，后端為圖優(yōu)化，回環(huán)檢測基于詞袋模型，輸出地圖為點(diǎn)云地圖，該方法計(jì)算量較小，精度不高，地圖為稀疏點(diǎn)云地圖，實(shí)用性差?？偠灾?，精度較低、實(shí)用性較差是目前大部分視覺SLAM存在的共同問題。

針對這些問題，可以在回環(huán)檢測和構(gòu)建地圖部分進(jìn)行改進(jìn)。回環(huán)檢測是消除誤差的一個(gè)重要環(huán)節(jié)。J.Sivic等[7]提出了詞袋模型，它是將提取到的圖像特征進(jìn)行離散化處理，并統(tǒng)計(jì)不同特征出現(xiàn)的頻率以描述機(jī)器人周圍的環(huán)境。該方法計(jì)算量巨大、匹配速度慢、相似度計(jì)算能力較低、效果較差;D.Niser等[8]針對詞袋模型計(jì)算量大、匹配速度慢等問題，對詞袋模型進(jìn)行改進(jìn)，增加了樹形結(jié)構(gòu)，提高了特征聚類速度，減少了計(jì)算量，提高了匹配速度，但仍存在相似度計(jì)算能力較低的問題；C.Cadena等[9]通過將詞袋模型與雙目相機(jī)結(jié)合的方法進(jìn)行回環(huán)檢測，該方法在當(dāng)時(shí)效果良好，但仍存在相似度計(jì)算能力不強(qiáng)的問題；C.Gong等[10]采用遞歸貝葉斯方法進(jìn)行回環(huán)檢測，該方法效果一般；羅順心等[11]將SSD神經(jīng)網(wǎng)絡(luò)用于回環(huán)檢測，效果較好，但其效果和訓(xùn)練樣本關(guān)系密切。地圖構(gòu)建部分是SLAM輸出信息的體現(xiàn)。視覺SLAM中，常見的地圖形式為點(diǎn)云地圖。點(diǎn)云地圖規(guī)模龐大，包含很多無用信息，且無法被用于導(dǎo)航，實(shí)用性較差。八叉樹地圖將地圖信息進(jìn)行壓縮，大大減小了規(guī)模，且可以記錄一個(gè)空間點(diǎn)是否被占據(jù)的信息，能用于導(dǎo)航等，實(shí)用性較高。

本文基于以上研究，對詞袋模型進(jìn)行改進(jìn)，增加節(jié)點(diǎn)距離，以期提高相似度計(jì)算能力，將改進(jìn)后的詞袋模型用于回環(huán)檢測，以減少累計(jì)誤差，提高精度，且通過八叉樹模型將點(diǎn)云圖變成可用于導(dǎo)航的八叉樹地圖，提高實(shí)用性。

1 算法框架

本文提出的RGB-D SLAM算法框架如圖1所示。首先，提取ORB特征點(diǎn)并匹配，結(jié)合深度信息通過PnP算法估計(jì)相機(jī)位姿，對圖像進(jìn)行篩選，得到關(guān)鍵幀并完成幀與幀之間的配準(zhǔn)；其次，通過改進(jìn)的詞袋模型進(jìn)行回環(huán)檢測，結(jié)合圖優(yōu)化得到相機(jī)全局最優(yōu)位姿，再通過去外點(diǎn)濾波器與體素濾波器減少點(diǎn)云規(guī)模；最后，通過octree轉(zhuǎn)化，生成八叉樹地圖。

圖1 RGB-D SLAM算法框架

2 算法流程

2.1 特征提取與匹配

每一幀640像素×480像素圖像產(chǎn)生約30萬個(gè)空間點(diǎn)[12]，若直接使用整幀圖像點(diǎn)，會因數(shù)據(jù)規(guī)模過大而導(dǎo)致算法速度變慢、實(shí)時(shí)性減弱。因此，需要提取點(diǎn)特征，以減小計(jì)算量。

采用oriented fast and rotated brief(ORB)特征點(diǎn)。ORB特征點(diǎn)通過Oriented FAST檢測特征與Steer BRIEF描述，能保證特征點(diǎn)具有良好的旋轉(zhuǎn)不變性與尺度不變性[13]。且ORB特征點(diǎn)具有提取速度快、匹配能力較好等優(yōu)點(diǎn)。

點(diǎn)特征提取后，采用快速近似最近鄰(fast library for approximate nearest neighbors，F(xiàn)LANN)算法，結(jié)合隨機(jī)抽樣一致性算法進(jìn)行匹配?？焖俳谱罱忂m用于高維數(shù)據(jù)的匹配[14]。隨機(jī)抽樣一致性算法(random sample consensus，RANSAC)基于迭代的思想，在每次迭代過程中隨機(jī)從三維坐標(biāo)點(diǎn)對中小規(guī)模采樣，計(jì)算出變換模型，然后比較全局與該模型，得到滿足該模型的內(nèi)點(diǎn)集合，重新求變換模型[15-16]。隨機(jī)抽樣一致性算法用于剔除誤匹配點(diǎn)。

2.2 位姿估計(jì)與優(yōu)化

完成相鄰幀間的匹配后，需要求解相鄰幀間的變換矩陣。該變換矩陣T由一個(gè)旋轉(zhuǎn)矩陣R和平移矩陣t組成，即

(1)

深度相機(jī)所輸出的圖像中，一幀圖像的特征點(diǎn)三維坐標(biāo)可以直接由depth圖確定。在這種情況下，Perspective-n-Point(PnP)算法只需3對點(diǎn)就可以計(jì)算相機(jī)的運(yùn)動(dòng)，大大減小了計(jì)算量。故本文采用PnP算法計(jì)算變換矩陣T。

隨時(shí)間推移，算法產(chǎn)生的誤差不斷堆積，這就需要回環(huán)檢測減少堆積的誤差。本文提出一種改進(jìn)的詞袋模型，其算法流程如下。

(1)隨機(jī)選取n個(gè)中心點(diǎn),且確定一個(gè)閾值。

(2)對每一個(gè)特征，計(jì)算它與各個(gè)中心點(diǎn)間的距離，取最小的作為該特征的類。

(3)計(jì)算每個(gè)類新的中心點(diǎn)。

(4)迭代(2)、(3)步，直到每個(gè)特征與它的類中心點(diǎn)的距離小于閾值。

(5)設(shè)置節(jié)點(diǎn)權(quán)重，并計(jì)算Ddistance。

詞典生成后，使用K叉樹表達(dá)，如圖2所示，步驟如下。

圖2 K叉樹

(1)對根節(jié)點(diǎn)用上述聚類方法將特征聚成n類，得到第一層。

(2)對每一層的n個(gè)節(jié)點(diǎn)，再聚類成n類，得到下一層。

(3)重復(fù)(2)，最后得到葉子層，即words。

使用Term Frequency-Inverse Document Frequency(TF-IDF)計(jì)算節(jié)點(diǎn)權(quán)重，即

(2)

式中：ηi為某葉子節(jié)點(diǎn)wi的權(quán)重；ni為wi中的特

征數(shù)量；n為所有特征數(shù)量。

選擇權(quán)重最大的兩個(gè)節(jié)點(diǎn)ai與bi，計(jì)算兩個(gè)節(jié)點(diǎn)間的距離，即

(3)

節(jié)點(diǎn)權(quán)重與節(jié)點(diǎn)距離Ddistance組合,形成詞袋模型(Bag-of-Words)，即

Bag-of-words={(w1，η1)，(w2，η2)，…，(wn，ηn)，Ddistance}?v。

(4)

使用L1范數(shù)形式計(jì)算相似度s，即

(5)

式中：s(vA-vB)為圖像A與B的相似度；Ai為圖像A的Bag-of-Words中節(jié)點(diǎn)；Bi為圖像B的Bag-of-Words中節(jié)點(diǎn)。

2.3 位姿圖優(yōu)化

采用圖優(yōu)化的方法對整個(gè)位姿圖進(jìn)行優(yōu)化。圖優(yōu)化方法中，圖的定義是點(diǎn)和邊的集合，點(diǎn)代表優(yōu)化變量，邊代表誤差項(xiàng)[17]。在SLAM問題中，位置圖的點(diǎn)是每個(gè)時(shí)刻相機(jī)的位置，邊是不同時(shí)刻相機(jī)的位置變換關(guān)系。本文使用g2o求解位姿圖。該方法將位姿圖優(yōu)化問題轉(zhuǎn)化為非線性最小二乘法問題。在SLAM問題中，求解相機(jī)最優(yōu)位姿x*。如式(6)所示，目標(biāo)函數(shù)F(x)定義為

(6)

(7)

(8)

式中：xk為k時(shí)刻所處的位置；uk為k時(shí)刻傳感器的輸入；zk,j為k時(shí)刻j路標(biāo)的觀測值；yj為路標(biāo)j。

2)課程服務(wù)于在校學(xué)生、中山及周邊通信企業(yè)，以崗位需求和職業(yè)發(fā)展為導(dǎo)向，注重職業(yè)能力培養(yǎng)，參考通信工程師職業(yè)資格標(biāo)準(zhǔn)，以運(yùn)營商商用設(shè)備為基礎(chǔ)，以真實(shí)工作任務(wù)和工作過程為載體，并結(jié)合知識的認(rèn)知規(guī)律和學(xué)生的學(xué)習(xí)特點(diǎn)構(gòu)建基于工作過程系統(tǒng)化的教學(xué)任務(wù)。

2.4 八叉樹地圖

采用統(tǒng)計(jì)濾波器方法去除孤立點(diǎn)，再使用體素濾波器降采樣處理龐大的點(diǎn)云數(shù)據(jù)?？紤]到后續(xù)導(dǎo)航等需求，選擇將處理后的點(diǎn)云存入octree提供的點(diǎn)云結(jié)構(gòu)中，通過轉(zhuǎn)化，輸出八叉樹地圖。八叉樹，如圖3所示，是很多體素表達(dá)空間三維模型，將體素不斷均分成8個(gè)小體素，直到最小的體素滿足要求的精度[18]。在八叉樹地圖中，每個(gè)節(jié)點(diǎn)都存儲了該節(jié)點(diǎn)是否被占據(jù)的概率，P(n)為節(jié)點(diǎn)n的占據(jù)概率，取[0，1]，使用概率對數(shù)值logit(n)描述，即

圖3 八叉樹示意圖

(9)

其反變換為

(10)

由式(9)可知，logit(n)從-∞到+∞時(shí)，P(n)從0到1。當(dāng)不斷觀測到節(jié)點(diǎn)n被占據(jù)時(shí)，logit(n)增加1，反之則減小1，再求出P(n)。對于t時(shí)刻，節(jié)點(diǎn)n的觀測數(shù)據(jù)為z，概率對數(shù)值為logit(n|z1:t)，t+1時(shí)刻為

logit(n|z1:t+1)=logit(n|z1:t-1)+
logit(n|zt)，

(11)

則相應(yīng)的概率形式為

(12)

式中,P(n)為節(jié)點(diǎn)n的先驗(yàn)概率，通常取0.5。選擇合適的閾值，當(dāng)節(jié)點(diǎn)所對應(yīng)的概率大于閾值時(shí)，認(rèn)為該節(jié)點(diǎn)為占據(jù)狀態(tài)，反之，則為是空閑狀態(tài)。

3 結(jié)果與分析

使用傳統(tǒng)的詞袋模型與改進(jìn)的詞袋模型進(jìn)行相似度對比實(shí)驗(yàn)，傳統(tǒng)的詞袋模型即DBoW2庫，該庫已集成引言中提及的詞袋模型成果。通過kinect v1相機(jī)在數(shù)控車床實(shí)驗(yàn)室拍攝12 000張圖像，做成數(shù)據(jù)集，然后使用該數(shù)據(jù)集對詞袋字典進(jìn)行訓(xùn)練，圖4(a)所示為訓(xùn)練的圖像。改進(jìn)的詞袋模型訓(xùn)練生成K=10，L=8，單詞數(shù)4 704 257的字典；原始的詞袋模型訓(xùn)練生成K=10，L=8，單詞數(shù)4 015 782的字典。然后對如圖4(b)所示的5張圖像進(jìn)行相似度計(jì)算，相似度取[0，1]，完全一樣時(shí)的相似度為1，完全不同時(shí)的相似度為0，結(jié)果如表1～2所示。

圖4 訓(xùn)練與實(shí)驗(yàn)圖像

表1 傳統(tǒng)的詞袋模型實(shí)驗(yàn)結(jié)果

由表1～2實(shí)驗(yàn)結(jié)果可知，傳統(tǒng)的詞袋模型中，圖像2和5的相似度高達(dá)0.575 98，圖像3和6的相似度高達(dá)0.349 599，改進(jìn)后的詞袋模型中，圖像2和5的相似度為0.423 769，圖像3和6的相似度為0.308 652，均有顯著下降，說明改進(jìn)的詞袋模型相似度計(jì)算精度更高、效果更好，表明改進(jìn)的詞袋模型有較好的實(shí)際應(yīng)用價(jià)值。

表2 改進(jìn)的詞袋模型實(shí)驗(yàn)結(jié)果

rgbd_dataset_freiburg1_xyz數(shù)據(jù)集的實(shí)驗(yàn)結(jié)果如表3和圖5所示，平均每幀處理時(shí)間為0.066 s。

表3 rgbd_dataset_freiburg1_xyz位姿估計(jì)(部分)

圖5中的藍(lán)色曲線為本文算法估計(jì)出的軌跡，紅色曲線為數(shù)據(jù)集自帶的標(biāo)準(zhǔn)軌跡。由圖5可知，本文算法對于該數(shù)據(jù)集表現(xiàn)良好，誤差較小，由表3數(shù)據(jù)可以求得ATERMSE，為0.076 7 m。

圖5 xyz數(shù)據(jù)集估計(jì)軌跡與標(biāo)準(zhǔn)軌跡對比

rgbd_dataset_freiburg1_desk數(shù)據(jù)集的實(shí)驗(yàn)結(jié)果如表4、圖6所示，平均每幀的處理時(shí)間為0.118 s。

圖6中，藍(lán)色曲線為本文算法的估計(jì)軌跡，紅色曲線為數(shù)據(jù)集自帶的標(biāo)準(zhǔn)軌跡。由圖6可知，本文算法對該數(shù)據(jù)集表現(xiàn)一般，由表4數(shù)據(jù)可以求得ATERMSE,為0.261 3 m。

表4 rgbd_dataset_freiburg1_desk位姿估計(jì)(部分)

圖6 desk數(shù)據(jù)集估計(jì)軌跡與標(biāo)準(zhǔn)軌跡對比

rgbd_dataset_freiburg1_360數(shù)據(jù)集的實(shí)驗(yàn)結(jié)果表5和圖7所示，平均每幀圖像實(shí)際處理時(shí)間為0.050 s。

圖7中，藍(lán)色曲線為本文算法計(jì)算出的估計(jì)軌跡，紅色曲線為數(shù)據(jù)集自帶的標(biāo)準(zhǔn)軌跡。由圖7可知，本文算法在周圍環(huán)境有回環(huán)存在的情況下，表現(xiàn)良好，能與標(biāo)準(zhǔn)軌跡基本一致。由表5數(shù)據(jù)可求ATERMSE，為0.091 3 m。

圖7 360數(shù)據(jù)集估計(jì)軌跡與標(biāo)準(zhǔn)軌跡對比

表5 rgbd_dataset_freiburg1_360位姿估計(jì)(部分)

結(jié)合這3個(gè)數(shù)據(jù)集的仿真實(shí)驗(yàn)數(shù)據(jù)，綜合分析可知，本文算法在相機(jī)運(yùn)動(dòng)較慢或者環(huán)境存在回環(huán)的情況下，效果良好，誤差較小，為0.076 7 m和0.091 3 m；在相機(jī)運(yùn)動(dòng)較快的情況下，效果較差，誤差較大，為0.261 3 m。

與使用傳統(tǒng)詞袋模型的本文算法和開源算法DVO SLAM、LSD-SLAM進(jìn)行對比實(shí)驗(yàn)，結(jié)果如表6所示。

由表6分析可知，本文算法在周圍環(huán)境有回環(huán)時(shí)，精度比兩種開源算法和本文使用原始詞袋模型算法高，表現(xiàn)好；在相機(jī)運(yùn)動(dòng)較慢時(shí)，精度比LSD-SLAM高，比DVO SLAM低，表現(xiàn)較好；在相機(jī)運(yùn)動(dòng)較快時(shí)，精度比兩種開源算法低，表現(xiàn)差。

表6 不同算法實(shí)驗(yàn)結(jié)果對比

在數(shù)控車床實(shí)驗(yàn)室進(jìn)行測試。實(shí)驗(yàn)室環(huán)境如圖8所示，實(shí)驗(yàn)設(shè)備為Kinect V1相機(jī)、I5-7300 CPU筆記本電腦。實(shí)地測試結(jié)果如圖9所示。圖9(a)為本文算法所輸出的數(shù)控車床實(shí)驗(yàn)室室內(nèi)環(huán)境八叉樹地圖，圖9(b)為相機(jī)的估計(jì)軌跡。由圖9可知，本文的算法實(shí)用性較高，能滿足后續(xù)的導(dǎo)航要求。

圖8 數(shù)控車床實(shí)驗(yàn)室室內(nèi)環(huán)境

圖9 數(shù)控車床實(shí)驗(yàn)室實(shí)驗(yàn)結(jié)果

4 結(jié) 語

針對室內(nèi)環(huán)境下視覺SLAM精度不高和實(shí)用性較差等問題，對傳統(tǒng)詞袋模型進(jìn)行改進(jìn)，增加了一個(gè)節(jié)點(diǎn)距離，提出了一種基于改進(jìn)詞袋模型的室內(nèi)無源自然信標(biāo)RGB-D SLAM算法，并進(jìn)行了傳統(tǒng)詞袋模型與改進(jìn)詞袋模型的對比實(shí)驗(yàn)、基于TUM數(shù)據(jù)集的精度實(shí)驗(yàn)和實(shí)地測試。詞袋模型的對比實(shí)驗(yàn)結(jié)果顯示，改進(jìn)的詞袋模型在相似度計(jì)算上更優(yōu)。精度實(shí)驗(yàn)結(jié)果顯示本文算法在環(huán)境有回環(huán)和相機(jī)運(yùn)動(dòng)較慢的情況下表現(xiàn)較好，對室內(nèi)環(huán)境適應(yīng)性好。實(shí)地測試結(jié)果顯示本文算法實(shí)用性較高，為后續(xù)的導(dǎo)航奠定了較好的基礎(chǔ)，但當(dāng)相機(jī)運(yùn)動(dòng)較快時(shí)，本文算法表現(xiàn)一般，誤差較大，因此，需要進(jìn)一步探究。