視覺(jué)SLAM中閉環(huán)檢測(cè)算法的研究

董海霞，曾連蓀

(上海海事大學(xué) 信息工程學(xué)院，上海 201306)

視覺(jué)SLAM中閉環(huán)檢測(cè)算法的研究

董海霞，曾連蓀

(上海海事大學(xué) 信息工程學(xué)院，上海 201306)

在基于人工視覺(jué)的移動(dòng)機(jī)器人導(dǎo)航中，閉環(huán)檢測(cè)是機(jī)器人準(zhǔn)確構(gòu)建地圖及定位的一個(gè)重要問(wèn)題。本文研究的閉環(huán)檢測(cè)算法結(jié)合了計(jì)算機(jī)視覺(jué)中的詞袋技術(shù)和視覺(jué)詞典技術(shù)，在對(duì)圖像進(jìn)行處理時(shí)利用了BRIEF+FAST關(guān)鍵點(diǎn)的方法，利用離線階段生成的詞典樹(shù)將二進(jìn)制描述子空間離散化。訓(xùn)練圖像生成的圖像數(shù)據(jù)庫(kù)結(jié)構(gòu)主要由等級(jí)詞袋、倒置索引和直接索引組成。倒置索引和直接索引提高了算法的效率。為了保證閉環(huán)檢測(cè)結(jié)果的可靠性，對(duì)于匹配的圖像還要進(jìn)行驗(yàn)證。重點(diǎn)詳述了一種高效的閉環(huán)檢測(cè)算法，相對(duì)一般的基于概率的閉環(huán)檢測(cè)，在硬件設(shè)備相同的情況下，本算法效率更高。

詞袋；視覺(jué)詞典；閉環(huán)檢測(cè)；圖像數(shù)據(jù)庫(kù)；匹配

0 引言

即時(shí)定位與構(gòu)圖(Simultaneous Localization and Mapping，SLAM)指的是機(jī)器人在未知的環(huán)境中，增量式地創(chuàng)建周?chē)h(huán)境的連續(xù)地圖，同時(shí)利用創(chuàng)建的地圖為自身導(dǎo)航[1]。SLAM問(wèn)題可以實(shí)現(xiàn)機(jī)器人真正的自主導(dǎo)航。數(shù)據(jù)關(guān)聯(lián)是指移動(dòng)機(jī)器人用確定傳感器觀測(cè)量與目標(biāo)源之間的對(duì)應(yīng)關(guān)系[2]。機(jī)器人進(jìn)行一段時(shí)間的運(yùn)行后，當(dāng)長(zhǎng)時(shí)間沒(méi)有被觀測(cè)到的區(qū)域被機(jī)器人自身攜帶的傳感器觀測(cè)到時(shí)，標(biāo)準(zhǔn)的匹配算法就會(huì)失敗。當(dāng)能穩(wěn)定地檢測(cè)到這些區(qū)域時(shí)，閉環(huán)檢測(cè)算法就能夠提供正確的數(shù)據(jù)關(guān)聯(lián)。

早期數(shù)據(jù)關(guān)聯(lián)算法的研究主要停留在幾何方面，Singer等人提出的最近鄰(Nearest Neighbor, NN)算法是最早也是最簡(jiǎn)單的數(shù)據(jù)關(guān)聯(lián)方法[3]。在環(huán)境特征密度較大的情況下，使用NN算法容易發(fā)生關(guān)聯(lián)失敗現(xiàn)象，由于NN算法忽略了數(shù)據(jù)之間的相關(guān)性，可能導(dǎo)致一些錯(cuò)誤的關(guān)聯(lián)。Jose Neira等人提出了聯(lián)合相容性檢驗(yàn)(Joint Compatibility Test, JC Test)算法。聯(lián)合相容分支定界(Joint Compatibility Branch and Bound, JCBB)[3]算法能排除一些NN算法無(wú)法排除的錯(cuò)誤的關(guān)聯(lián)假設(shè)[4]，基于幾何的數(shù)據(jù)關(guān)聯(lián)方法沒(méi)有充分利用到機(jī)器人周?chē)沫h(huán)境信息。隨著相機(jī)價(jià)格的下降，相機(jī)取代傳統(tǒng)的傳感器，在數(shù)據(jù)關(guān)聯(lián)方面的應(yīng)用越來(lái)越廣泛。機(jī)器人通過(guò)所攜帶相機(jī)拍攝的周?chē)h(huán)境圖片含有豐富的信息，因此可以通過(guò)對(duì)圖片的處理，判斷圖片間的相似性，看是否形成了一個(gè)閉環(huán)，從而對(duì)機(jī)器人構(gòu)建的地圖進(jìn)行修正或增廣。

本文中的閉環(huán)檢測(cè)算法主要基于視覺(jué)詞典[5]及對(duì)匹配的圖像進(jìn)行的幾何檢測(cè)，其高效性使得這種算法更適合實(shí)時(shí)應(yīng)用。

1 問(wèn)題定義

閉環(huán)檢測(cè)是指當(dāng)移動(dòng)機(jī)器人到達(dá)一個(gè)先前已經(jīng)構(gòu)建過(guò)地圖的位置時(shí)，能夠判斷出這個(gè)位置已經(jīng)構(gòu)建過(guò)地圖，然后對(duì)原來(lái)構(gòu)建的地圖進(jìn)行更新、修正[6]。

1.1 二進(jìn)制特征

在進(jìn)行兩幅圖像比較時(shí)，需要從圖像中提取關(guān)鍵點(diǎn)和局部特征，這一步是非常費(fèi)時(shí)的，這也是閉環(huán)檢測(cè)算法實(shí)時(shí)應(yīng)用的瓶頸。為了克服提取特征和關(guān)鍵點(diǎn)費(fèi)時(shí)的問(wèn)題，本文采用了FAST關(guān)鍵點(diǎn)和最優(yōu)的BRIEF描述子。FAST關(guān)鍵點(diǎn)是一些像角一樣的點(diǎn)，這些點(diǎn)是通過(guò)在一個(gè)半徑為3的Bresenham圓中，對(duì)一些像素點(diǎn)的灰度強(qiáng)度進(jìn)行比較得到的[7]。因?yàn)闄z測(cè)到的像素點(diǎn)的個(gè)數(shù)有限，所以在實(shí)時(shí)的應(yīng)用中，F(xiàn)AST關(guān)鍵點(diǎn)也能很快被檢索到。

對(duì)于每一個(gè)FAST關(guān)鍵點(diǎn)，以這個(gè)點(diǎn)為中心點(diǎn)畫(huà)一個(gè)方形的圖像塊，然后計(jì)算一個(gè)BRIEF描述子。BRIEF描述子是二進(jìn)制的比特向量，通過(guò)圖像塊(為了降低噪聲已經(jīng)事先經(jīng)過(guò)高斯平滑處理)中兩個(gè)像素點(diǎn)的強(qiáng)度比較得到比特分量的值[8]。給定圖像塊的尺度Sb，設(shè)置好用于比較的像素點(diǎn)的對(duì)數(shù)Lb(也就是描述子向量的長(zhǎng)度)，像素點(diǎn)是在離線階段隨機(jī)選擇的。對(duì)于圖像中的關(guān)鍵點(diǎn)P，它的BRIEF描述子向量B(P) 可以描述為：

(1)

Bi(P)是描述子向量中的第i個(gè)比特，I(·)表示平滑圖像中像素點(diǎn)的強(qiáng)度，ai、bi表示第i個(gè)檢測(cè)點(diǎn)相對(duì)于圖像塊中心點(diǎn)的2D偏置量，它們的取值范圍是:

(2)

這種描述子向量不需要訓(xùn)練，只是在離線時(shí)隨機(jī)選擇的點(diǎn)，占用的時(shí)間可以忽略。BRIEF描述子最大的優(yōu)點(diǎn)是計(jì)算速度快，由于這種描述子只是一個(gè)比特向量，可以通過(guò)異或比較兩個(gè)向量之間不同比特的個(gè)數(shù)(漢明距離)，在實(shí)時(shí)的應(yīng)用中計(jì)算歐氏距離要比計(jì)算漢明距離慢得多。在使用SIFT和SURF描述子時(shí)，計(jì)算兩個(gè)向量之間的距離就是計(jì)算歐氏距離。

1.2 圖像數(shù)據(jù)庫(kù)

為了讓機(jī)器人識(shí)別出目前所在的位置是否已經(jīng)構(gòu)建過(guò)地圖，本文使用了圖像數(shù)據(jù)庫(kù)。圖像數(shù)據(jù)庫(kù)主要由等級(jí)詞袋、倒置索引和直接索引三部分組成，如圖1所示。詞典中的字就是樹(shù)中的葉子節(jié)點(diǎn)。倒置索引中存放的是字的權(quán)重。直接索引中的內(nèi)容主要是圖像的特征以及與特征相關(guān)的詞典樹(shù)中的節(jié)點(diǎn)。

圖1 詞典樹(shù)、倒置索引和直接索引組成的圖像數(shù)據(jù)庫(kù)

在機(jī)器人自主導(dǎo)航過(guò)程中，機(jī)器人攜帶的相機(jī)拍攝了大量的圖片。為了能夠有序地存儲(chǔ)這些圖片，用詞袋技術(shù)通過(guò)視覺(jué)詞典把圖片轉(zhuǎn)換成稀疏的數(shù)值向量[9]。視覺(jué)詞典是在離線階段生成的，它把描述子空間離散成W個(gè)視覺(jué)字。本文所用的特征BRIEF使得二進(jìn)制描述子空間離散化，生成一個(gè)更簡(jiǎn)潔的詞典，把詞袋按等級(jí)排列，整個(gè)詞典就是一個(gè)樹(shù)形的結(jié)構(gòu)。為了生成詞典樹(shù)，從訓(xùn)練圖像(這些圖像與在線處理的圖像是相互獨(dú)立的)中提取大量的特征，它們所對(duì)應(yīng)的描述子按照K-means算法離散成Kw個(gè)二進(jìn)制的簇，這些簇就是詞典樹(shù)的一級(jí)節(jié)點(diǎn)，對(duì)于每一個(gè)節(jié)點(diǎn)再進(jìn)行K-means算法，生成第二級(jí)節(jié)點(diǎn)，依次按照這種步驟進(jìn)行Lw次，最終就會(huì)生成W個(gè)字的詞典樹(shù)[10]。根據(jù)字在訓(xùn)練集中的相關(guān)性，給每個(gè)字分配一個(gè)權(quán)重，那些出現(xiàn)次數(shù)比較頻繁、對(duì)于區(qū)分不同圖像沒(méi)有太大作用的字，分配較小的權(quán)重，對(duì)于區(qū)分圖像作用顯著的字，分配權(quán)重大一些。假設(shè)在t時(shí)刻，獲得一幅圖像It，根據(jù)圖像中特征的二進(jìn)制描述子，從根到葉子遍歷整個(gè)詞典樹(shù)，按照漢明距離最小原則在每一級(jí)選擇一個(gè)節(jié)點(diǎn)，到達(dá)葉子節(jié)點(diǎn)時(shí)就可以得到該圖像對(duì)應(yīng)的二進(jìn)制數(shù)值向量Vt∈Rw。兩幅圖像I1、I2之間的相似性可通過(guò)計(jì)算它們的數(shù)值向量之間的相似值來(lái)衡量：

(3)

在圖像數(shù)據(jù)庫(kù)中除了詞袋外還有倒置索引和直接索引。對(duì)于詞典中的一個(gè)字Wi，倒置索引中內(nèi)容是包含這個(gè)字的圖像的列表。有了這種結(jié)構(gòu)，在從數(shù)據(jù)庫(kù)中搜索與查詢(xún)圖像相似的數(shù)據(jù)庫(kù)圖像時(shí)，就可以只在與查詢(xún)圖像有共同字的數(shù)據(jù)庫(kù)圖像中進(jìn)行搜索。如果在倒置索引中存放圖像的數(shù)值向量中對(duì)應(yīng)Wi這個(gè)字的分量，還可以得到字在圖像中的權(quán)重。直接索引存儲(chǔ)的是圖像It中字的父節(jié)點(diǎn)和與節(jié)點(diǎn)相關(guān)的局部特征ftj。直接索引的結(jié)構(gòu)可以使得在幾何驗(yàn)證時(shí)，只尋找同一個(gè)字的特征間或者是有相同父節(jié)點(diǎn)的特征間的對(duì)應(yīng)性，這樣就節(jié)省了時(shí)間。但是，當(dāng)有新的圖像要加入數(shù)據(jù)庫(kù)時(shí)，倒置索引和直接索引都要進(jìn)行更新。

2 閉環(huán)檢測(cè)算法

本文研究的閉環(huán)檢測(cè)算法主要分為四步個(gè)步驟。

2.1 查詢(xún)數(shù)據(jù)庫(kù)

從圖像數(shù)據(jù)庫(kù)中檢索與給定圖像相似的圖像。當(dāng)機(jī)器人獲得最新的圖像It時(shí)，首先根據(jù)在離線階段生成的詞典樹(shù)把它轉(zhuǎn)換成詞袋向量Vt，然后在數(shù)據(jù)庫(kù)中進(jìn)行搜索得到一些候選的匹配〈Vt,Vt1〉，〈Vt,Vt2〉，…，根據(jù)公式(3)計(jì)算出這些匹配對(duì)應(yīng)的相似值s(Vt,Vtj)，相似值的變化范圍是由查詢(xún)圖像和圖像中字的分布決定的。對(duì)相似值進(jìn)行歸一化，得到歸一化相似值η:

(4)

Vt-Δt是前一幅圖像的詞袋向量。當(dāng)機(jī)器人在某一瞬間狀態(tài)急劇變化時(shí)，s(Vt,Vt-Δt)的值就會(huì)很小，導(dǎo)致歸一化相似值很高。為了避免這種錯(cuò)誤的出現(xiàn)，對(duì)s(Vt,Vt-Δt)設(shè)置一個(gè)小的門(mén)限值，忽略短時(shí)間內(nèi)與Vt相似度很低的圖像。同時(shí)，為了避免有效的圖像被錯(cuò)誤的忽略掉，門(mén)限值應(yīng)該設(shè)置得小一點(diǎn)。對(duì)于歸一化相似值η，設(shè)置一個(gè)門(mén)限α，拋棄那些沒(méi)有達(dá)到最小相似值α的匹配。

2.2 匹配組

當(dāng)相機(jī)的拍攝頻率較高時(shí)，時(shí)間間隔很短的圖像往往具有很高的相似性。為了避免在數(shù)據(jù)庫(kù)中搜索時(shí)時(shí)間間隔很短的圖像之間互相競(jìng)爭(zhēng)，這里把拍攝時(shí)間間隔很短的圖像看做是一個(gè)島，在匹配的時(shí)候把這些匹配看做是一個(gè)匹配。將時(shí)間戳tni，…，tmi，用一個(gè)時(shí)間戳Ti表示，VTi表示整個(gè)圖像島的詞袋數(shù)值向量。根據(jù)相似值H，對(duì)島的相似性進(jìn)行排序：

(5)

具有最大相似值的島作為候選的匹配組，再進(jìn)行一致性檢驗(yàn)。用圖像島的方法消除連續(xù)圖像匹配時(shí)的競(jìng)爭(zhēng)，如果It和It′是一個(gè)真正的閉環(huán)，It往往與It′±Δt，It’±2Δt，… ，也有很高的相似性。

2.3 時(shí)間上的一致性

在得到最佳的匹配島VT′后，要檢測(cè)當(dāng)前匹配與前面匹配的一致性。即匹配〈Vt，VT′〉與前面k個(gè)匹配〈Vt-Δt,VT1〉，…，〈Vt-kΔt,VTk〉必須一致，類(lèi)似于JCBB算法中匹配時(shí)聯(lián)合兼容性檢驗(yàn)。如果一個(gè)島符合一致性檢驗(yàn)，則認(rèn)為最大η值對(duì)應(yīng)的匹配〈Vt，Vt′〉是最佳匹配(t′∈T′)，把它作為一個(gè)候選的閉環(huán)，最后還要經(jīng)過(guò)幾何驗(yàn)證來(lái)判斷其是否是一個(gè)真正的閉環(huán)。

2.4 幾何驗(yàn)證

對(duì)于候選為閉環(huán)的圖像還要進(jìn)行幾何驗(yàn)證。幾何驗(yàn)證是指利用RANSAC(Random Sample Consensus)算法在匹配的圖像It和It′之間找到局部特征的對(duì)應(yīng)性。局部特征的比較有兩種方法，遞歸搜索是最簡(jiǎn)單也是最慢的方法，它是通過(guò)計(jì)算It中的每一個(gè)特征和It′中的每一個(gè)特征在描述子空間中的距離， 根據(jù)最近鄰比例策略，選擇特征之間的對(duì)應(yīng)特性。但是當(dāng)圖像中特征點(diǎn)數(shù)量很多時(shí)，遞歸搜索法的計(jì)算量是不可接受的。

另外一種方法是近似最近鄰法，當(dāng)有一幅新的圖像加入圖像數(shù)據(jù)庫(kù)中，要把成對(duì)的節(jié)點(diǎn)和特征存儲(chǔ)在直接索引中，在對(duì)It和It′進(jìn)行幾何驗(yàn)證時(shí)，查詢(xún)It′的直接索引，只比較同一個(gè)節(jié)點(diǎn)關(guān)聯(lián)的特征，節(jié)點(diǎn)在詞典樹(shù)中的級(jí)數(shù)l是事先給定的。由于進(jìn)行特征比較的次數(shù)明顯少于遞歸搜索，節(jié)省了大量時(shí)間。l的設(shè)置會(huì)影響幾何驗(yàn)證的結(jié)果，當(dāng)l=0時(shí)，只對(duì)屬于同一個(gè)字的特征進(jìn)行比較，這時(shí)幾何驗(yàn)證效率最高，但是圖像間對(duì)應(yīng)的特征對(duì)數(shù)也是最少的，這就可能導(dǎo)致正確的閉環(huán)由于缺乏點(diǎn)之間的對(duì)應(yīng)性而被拒絕。相反，當(dāng)l=LW時(shí)，全部候選的匹配都能通過(guò)幾何驗(yàn)證，但是此時(shí)，幾何驗(yàn)證的效率也沒(méi)有得到改善。

在幾何驗(yàn)證階段，只是對(duì)于匹配的圖像之間進(jìn)行驗(yàn)證，以確定兩者的相似度足夠高，在驗(yàn)證之后還要進(jìn)行正確的數(shù)據(jù)關(guān)聯(lián)。

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

整個(gè)實(shí)驗(yàn)?zāi)M的是室外靜態(tài)環(huán)境下閉環(huán)檢測(cè)算法的過(guò)程，具體步驟：首先在離線階段生成視覺(jué)詞典樹(shù)，然后在機(jī)器人運(yùn)行時(shí)利用生成的詞典樹(shù)進(jìn)行閉環(huán)檢測(cè)。整個(gè)過(guò)程共檢測(cè)到7個(gè)閉環(huán)，提取特征用的是BRIEF算法，特征提取時(shí)間為8.946 99 ms/圖，實(shí)驗(yàn)中的詞袋字典是在離線階段生成的，規(guī)模是106，詞典樹(shù)中共有754 997個(gè)字，詞典樹(shù)的級(jí)數(shù)是L=6，K-means算法中K的值設(shè)置為10。

實(shí)驗(yàn)中的原始算法是由美國(guó)計(jì)算機(jī)視覺(jué)研究者Dorian Gálvez López 提出的DLoopDetector算法，本文實(shí)驗(yàn)中不同于原始算法的地方是：在Dorian Gálvez López的實(shí)驗(yàn)中，DBow2與DLoopDetector是分立的；在本文進(jìn)行的實(shí)驗(yàn)中，把DBow2與DLoopDetector結(jié)合在一起，即利用DBow2生成的詞典樹(shù)進(jìn)行閉環(huán)檢測(cè)實(shí)驗(yàn)。本文實(shí)驗(yàn)效果圖如圖2所示。

圖2 實(shí)驗(yàn)效果圖：機(jī)器人的運(yùn)動(dòng)軌跡，其中包括檢測(cè)到的閉環(huán)

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

在視覺(jué)SLAM中，閉環(huán)檢測(cè)是數(shù)據(jù)關(guān)聯(lián)的擴(kuò)展，是一個(gè)從點(diǎn)對(duì)點(diǎn)到面對(duì)面的過(guò)程。本文研究的閉環(huán)檢測(cè)算法使用的圖像數(shù)據(jù)庫(kù)結(jié)構(gòu)除了等級(jí)詞袋、倒置索引外，還有直接索引，這種結(jié)構(gòu)使得幾何驗(yàn)證的效率更佳。二進(jìn)制描述子BRIEF的使用使提取圖像特征、計(jì)算描述子之間距離的速度更快。但是在尺度、光照、相機(jī)發(fā)生旋轉(zhuǎn)時(shí)，BRIEF描述子是不穩(wěn)定的。因?yàn)楸疚闹袑?shí)驗(yàn)是在室外靜態(tài)環(huán)境下進(jìn)行的，而且相機(jī)也只是在平面內(nèi)進(jìn)行微小運(yùn)動(dòng)，所以多數(shù)實(shí)驗(yàn)具有很高的準(zhǔn)確性。

[1] 曲麗萍. 移動(dòng)機(jī)器人同步定位與地圖構(gòu)建關(guān)鍵技術(shù)的研究[D]. 哈爾濱： 哈爾濱工程大學(xué), 2013.

[2] 柴紅霞. 移動(dòng)機(jī)器人在SLAM中數(shù)據(jù)關(guān)聯(lián)方法的研究[D]. 大連： 大連理工大學(xué), 2010.

[3] 張雪晶,孫作雷,曾連蓀，等.基于聯(lián)合相容分支定界的關(guān)聯(lián)算法研究[J].微型機(jī)與應(yīng)用,2015,34(15):82-84,88.

[4] NEIRA J， TARDS J D. Data association in stochastic mapping using the joint compatibility test[J]. Robotics and Automation, IEEE Transactions on, 2001，17(6)：890-897.

[5] 崔大成,曾連蓀.基于視覺(jué)字典的移動(dòng)機(jī)器人閉環(huán)檢測(cè)方法研究[J].微型機(jī)與應(yīng)用,2015,34(9):85-88.

[6] WILLIAMS B, CUMMINS M, NEIRA J, et al. A comparison of loop closing techniques in monocular SLAM[J]. Robotics and Autonomous Systems, 2009，57(12)：1188-1197.

[8] CALONDER M, LEPETIT V, STRECHA C, et al. Brief: binary robust independent elementary features[C]. Computer Vision-ECCV 2010, 2010:778-792.

[9] SIVIC J, ZISSERMAN A. Video google: a text retrieval approach to object matching in videos[C]. In Computer Vision, 2003. Proceedings. Ninth IEEE International Conference on, 2003：1470-1477.

[10] NISTER D， STEWENIUS H. Scalable recognition with a vocabulary tree[C]. In Computer Vision and Pattern Recognition, 2006 IEEE Computer Society Conference on, 2006：2161-2168.

Loop detection algorithm in VSLAM

Dong Haixia，Zeng Liansun

( College of Information Engineering, Shanghai Maritime University, Shanghai 201306, China)

In the mobile robot navigation based on artificial vision, loop closure detection is of vital importance for mapping and localization. Technologies in computer vision, bag-of-words and visual vocabulary, are applied in the loop closure detection algorithm in this essay. BRIEF+FAST key point is used to process images. Vocabulary tree that is created offline discretizes binary descriptor space. Training images are used to build image database, which is composed of a hierarchical bag of words, inverse indexes and direct indexes. Inverse indexes and direct indexes improve efficiency greatly. Verification has to be performed to improve reliability of loop closing candidates. Compared with probability loop detection, the algorithm in this essay is more efficient with the same hardware facilities.

bag-of-words; vision vocabulary; loop detection; image database; match

TP242

A

1674-7720(2016)05-0001-03

董海霞，曾連蓀．視覺(jué)SLAM中閉環(huán)檢測(cè)算法的研究[J] .微型機(jī)與應(yīng)用，2016，35(5)：1-3,7.

2015-12-01)

董海霞(1990-)，女，碩士，主要研究方向：移動(dòng)機(jī)器人導(dǎo)航。

曾連蓀(1962-)，男，在讀博士，教授，主要研究方向：定位導(dǎo)航系統(tǒng)。