本發(fā)明屬于機(jī)器人自主定位與導(dǎo)航和機(jī)器人視覺(jué)技術(shù)領(lǐng)域，具體涉及一種基于詞袋模型的機(jī)器人定位和導(dǎo)航方法；通過(guò)基于時(shí)間構(gòu)建詞袋樹群模型，在低動(dòng)態(tài)環(huán)境下基于概率估計(jì)模型對(duì)詞袋模型樹進(jìn)行選擇，詞袋樹群模型能夠?yàn)殚L(zhǎng)時(shí)間的室內(nèi)同步定位和建圖機(jī)器人的回環(huán)檢測(cè)提供依賴。

背景技術(shù)：

機(jī)器人自主定位和導(dǎo)航是一門交叉學(xué)科，近十幾年來(lái)，機(jī)器人自主定位和導(dǎo)航以及機(jī)器人視覺(jué)已經(jīng)有了很大的進(jìn)步。盡管機(jī)器人視覺(jué)在特定的場(chǎng)景和數(shù)據(jù)庫(kù)中已經(jīng)達(dá)到高的精度，也有一部分的服務(wù)機(jī)器人走進(jìn)生活，但是機(jī)器人視覺(jué)在機(jī)器人的自主導(dǎo)航與定位的應(yīng)用也只在近幾年才得以運(yùn)用，在實(shí)現(xiàn)機(jī)器人自主視覺(jué)定位和導(dǎo)航的問(wèn)題上還沒(méi)有很好的解決，特別是機(jī)器人應(yīng)對(duì)動(dòng)態(tài)復(fù)雜場(chǎng)景和長(zhǎng)時(shí)間的定位和導(dǎo)航方面仍然需要很大的努力。

目前并沒(méi)有很多針對(duì)低動(dòng)態(tài)環(huán)境下的機(jī)器人長(zhǎng)時(shí)間視覺(jué)定位和導(dǎo)航的方法，基本上所有的方法都是針對(duì)提高特征提取算子的魯棒性，雖然現(xiàn)在的Scale-invariant feature transform(SIFT)[1]的算子魯棒性很好，但是需要使用GPU加速，ORiented Brief(ORB)[2]特征提取算子效果較好并且速度較快，但是也仍然不能在實(shí)際使用中實(shí)現(xiàn)長(zhǎng)時(shí)間的定位和導(dǎo)航。其中SIFT見文獻(xiàn)“D.G.Lowe,Distinctive image features from scale-invariant keypoints,International Journal of Computer Vision,vol.60,no.2,pp.91–110,2004.”，ORB見文獻(xiàn)“E.Rublee,V.Rabaud,K.Konolige,and G.Bradski,ORB:an efficient alternative to SIFT or SURF,in IEEE International Conference on Computer Vision(ICCV),Barcelona,Spain,November 2011,pp.2564–2571.”。

目前，市場(chǎng)上能比較好的應(yīng)用在服務(wù)機(jī)器人上的方案是使用激光傳感器，這種方案在使用二維激光的情況下能獲取環(huán)境的二維平面結(jié)構(gòu)。激光傳感器不受光照的影響，同時(shí)更加高效，也不需要性能很高的處理器，嵌入式的芯片就能滿足這種計(jì)算需求。但是激光傳感器的價(jià)格非常昂貴，并且不能處理黑色吸收光的物質(zhì)或者黑色的環(huán)境；不能處理玻璃等會(huì)改變激光傳播路徑的透明物質(zhì)的建模；抖動(dòng)對(duì)建圖的影響非常大。所以使用價(jià)格低廉的視覺(jué)傳感器開發(fā)低成本同時(shí)能實(shí)現(xiàn)長(zhǎng)時(shí)間定位和導(dǎo)航的方案非常有必要性。

技術(shù)實(shí)現(xiàn)要素：

針對(duì)現(xiàn)有技術(shù)存在的技術(shù)問(wèn)題，本發(fā)明的目的是提供一種基于詞袋樹群模型的機(jī)器人定位和導(dǎo)航方法，能夠使用價(jià)格低廉的視覺(jué)傳感器實(shí)現(xiàn)機(jī)器人的長(zhǎng)時(shí)間定位和導(dǎo)航。

本發(fā)明采用的技術(shù)方案如下：

一種基于詞袋樹群模型的機(jī)器人定位和導(dǎo)航方法，包括以下步驟：

1)輸入長(zhǎng)時(shí)間的包含低動(dòng)態(tài)室內(nèi)場(chǎng)景變化的視頻流；

2)從視頻流圖像中離線抽取特征；

3)對(duì)得到的特征通過(guò)k-means++算法進(jìn)行聚類；

4)對(duì)聚類得到的子空間進(jìn)行迭代聚類；

5)對(duì)迭代聚類后的特征建立詞袋樹模型；

6)每隔一定時(shí)間保存建立的詞袋樹模型，拷貝后繼續(xù)更新，得到詞袋樹群模型；

7)通過(guò)統(tǒng)計(jì)原理對(duì)低動(dòng)態(tài)物體的運(yùn)動(dòng)規(guī)律進(jìn)行統(tǒng)計(jì)；

8)根據(jù)步驟6)得到的詞袋樹群模型，利用所述運(yùn)動(dòng)規(guī)律分析確定當(dāng)前時(shí)刻對(duì)應(yīng)的詞袋樹模型；

9)將步驟8)確定的詞袋樹模型用于回環(huán)檢測(cè)，實(shí)現(xiàn)機(jī)器人在低動(dòng)態(tài)環(huán)境下的定位和導(dǎo)航。

進(jìn)一步地，步驟5)對(duì)詞袋樹模型中的每個(gè)特征，保存其正向索引和反向索引。

進(jìn)一步地，所述正向索引中包含每一幀圖像的特征以及該特征所在樹的父節(jié)點(diǎn)，所述反向索引存儲(chǔ)每一個(gè)特征所在的圖像的序號(hào)以及在該圖像中所占的比重，其值為詞頻與反向文本頻率的乘積。

進(jìn)一步地，步驟6)間隔一定時(shí)間后保存已經(jīng)建立的樹模型，并且復(fù)制上一個(gè)樹模型，對(duì)這個(gè)復(fù)制的樹模型進(jìn)行更新，直到間隔時(shí)間到或者已經(jīng)出現(xiàn)運(yùn)動(dòng)重復(fù)，從而構(gòu)建詞袋樹群模型。

進(jìn)一步地，步驟8)根據(jù)所述運(yùn)動(dòng)規(guī)律得出三個(gè)概率最大的詞袋樹模型，并依次將此時(shí)通過(guò)環(huán)境獲得的特征向量與三個(gè)概率最大的詞袋樹模型中的特征分別進(jìn)行匹配。

進(jìn)一步地，在所述匹配過(guò)程中，通過(guò)反向索引快速定位到所需要比較的相似圖像，通過(guò)正向索引快速檢查圖像中的特征。

進(jìn)一步地，步驟9)中，成功匹配樹模型后，則將所述樹模型作為機(jī)器人定位與回環(huán)檢測(cè)的數(shù)據(jù)庫(kù)，并利用新得到的圖像不斷更新。

本發(fā)明的有益效果如下：

本發(fā)明使用價(jià)格低廉的視覺(jué)傳感器，通過(guò)圖像處理技術(shù)將圖片轉(zhuǎn)化為特征向量存儲(chǔ)在詞袋樹模型的樹結(jié)構(gòu)中，利用概率模型選擇對(duì)應(yīng)狀態(tài)下的詞袋樹來(lái)進(jìn)行定位與導(dǎo)航系統(tǒng)中的回環(huán)檢測(cè)部分，最后實(shí)現(xiàn)機(jī)器人在室內(nèi)等低動(dòng)態(tài)環(huán)境下自主定位和導(dǎo)航。本發(fā)明避免了使用昂貴的激光傳感器，以及激光帶來(lái)的問(wèn)題，同時(shí)彌補(bǔ)了現(xiàn)有的特征提取算子的缺點(diǎn)。

附圖說(shuō)明

圖1.詞袋樹群構(gòu)建流程圖。

圖2.基于狀態(tài)選擇的詞袋樹群模型使用流程圖。

圖3.基于狀態(tài)選擇的詞袋樹群模型圖。

具體實(shí)施方式

為使本發(fā)明的上述目的、特征和優(yōu)點(diǎn)能夠更加明顯易懂，下面通過(guò)具體實(shí)施例和附圖，對(duì)本發(fā)明做進(jìn)一步說(shuō)明。

1.基于DBoW2的詞袋樹群模型建立

DBoW2(參考文獻(xiàn)：Galvez-López D,Tardos J D.Bags of Binary Words for Fast Place Recognition in Image Sequences[J].IEEE Transactions on Robotics,2012,28(5):1188-1197.)將圖像進(jìn)行結(jié)構(gòu)化描述。DBoW2思想是將圖像特征整合成視覺(jué)單詞，將圖像特征空間轉(zhuǎn)化成離散的視覺(jué)字典，并用樹的結(jié)構(gòu)存儲(chǔ)視覺(jué)單詞，加快特征以及相似圖像的檢索。將新的圖像特征映射到視覺(jué)字典中最近鄰視覺(jué)字典，再通過(guò)計(jì)算視覺(jué)字典間的距離計(jì)算圖像的相似度，從而完成識(shí)別、圖像分類、檢索等任務(wù)。

本發(fā)明的基于DBoW2的詞袋樹群是基于DBoW2的原理建立多個(gè)樹形結(jié)構(gòu)用于存儲(chǔ)不同時(shí)間或者不同狀態(tài)的環(huán)境特征，以應(yīng)對(duì)復(fù)雜多變的環(huán)境。詞袋樹群的具體構(gòu)建過(guò)程如圖1所示，包括以下步驟：

a)從訓(xùn)練圖像中離線提取ORB特征；

b)將抽取的特征用k-means++算法聚類，將描述子空間劃分成K類；

c)將劃分的每個(gè)描述子空間，繼續(xù)利用k-means++算法做聚類；

d)按照上述循環(huán)，將描述子建立成樹形結(jié)構(gòu)tree1；

e)在經(jīng)過(guò)設(shè)定時(shí)間T后，保存tree1樹，并映射出樹形結(jié)構(gòu)，并命名tree2用于更新；

f)重復(fù)上述操作，優(yōu)化樹群存儲(chǔ)空間，直到環(huán)境重復(fù)，樹群treeN構(gòu)建完成。

圖3為基于狀態(tài)選擇的詞袋樹群模型示意圖，其中tree1～treeN表示形成的N個(gè)樹形結(jié)構(gòu)，Direct index image表示直接圖像索引，用來(lái)記錄樹中保存的圖像序號(hào)，Node表示樹中的節(jié)點(diǎn)，F(xiàn)eature表示節(jié)點(diǎn)下存儲(chǔ)的特征的值，反向索引中V＝0.79表示特征在對(duì)應(yīng)圖像中所占的比重。

2.基于狀態(tài)選擇的詞袋樹使用方法

針對(duì)已經(jīng)建好的詞袋樹群，其包含了室內(nèi)環(huán)境變化的多種狀態(tài)，通過(guò)統(tǒng)計(jì)可以得到室內(nèi)大部分低動(dòng)態(tài)物體的運(yùn)動(dòng)規(guī)律，例如室內(nèi)光照方向，門窗的開關(guān)以及家具的位置在一天中都符合一定的規(guī)律，以及人的活動(dòng)都滿足一定的時(shí)間規(guī)律，通過(guò)統(tǒng)計(jì)學(xué)原理對(duì)該規(guī)律進(jìn)行分析，選擇對(duì)應(yīng)不同時(shí)間段的詞袋樹模型作為室內(nèi)機(jī)器人的定位與導(dǎo)航的回環(huán)檢測(cè)的數(shù)據(jù)庫(kù)。同時(shí)由于詞袋模型都是可以線下或者在機(jī)器人空閑時(shí)刻訓(xùn)練的，能滿足實(shí)時(shí)性的要求。具體步驟如圖2所示，包括以下步驟：

a)通過(guò)統(tǒng)計(jì)學(xué)模型對(duì)家具中的門窗，光線的移動(dòng)以及低動(dòng)態(tài)特定家具的運(yùn)動(dòng)規(guī)律進(jìn)行統(tǒng)計(jì)；

b)根據(jù)以上狀態(tài)的估計(jì)確定目前狀態(tài)所在的時(shí)間范圍；

c)根據(jù)時(shí)間選擇對(duì)應(yīng)的前三個(gè)概率值最高的詞袋樹模型作為回環(huán)檢測(cè)的庫(kù)；

d)如果均匹配錯(cuò)誤進(jìn)入對(duì)所有的樹進(jìn)行一次遍歷，如果均不能匹配則建立新的詞袋樹；

e)機(jī)器人停止運(yùn)動(dòng)時(shí)對(duì)詞袋樹進(jìn)行優(yōu)化。

3.本發(fā)明方法的整體流程

參考圖1，圖2，本發(fā)明方法的流程的具體設(shè)計(jì)如下：

1)輸入長(zhǎng)時(shí)間的包含低動(dòng)態(tài)室內(nèi)場(chǎng)景變化的視頻流

本發(fā)明是針對(duì)包含低動(dòng)態(tài)的室內(nèi)場(chǎng)景的應(yīng)用，濾除掉高動(dòng)態(tài)的運(yùn)動(dòng)物體后，在對(duì)僅包含低動(dòng)態(tài)的室內(nèi)場(chǎng)景建圖后保證機(jī)器人的長(zhǎng)時(shí)間的定位與導(dǎo)航有效性，同時(shí)家具環(huán)境中光照的移動(dòng)和強(qiáng)弱變化、門窗和座椅等均符合低動(dòng)態(tài)的定義。視頻流獲取時(shí)一定要保證環(huán)境的真實(shí)性和連續(xù)性，不能出現(xiàn)對(duì)攝像頭的遮擋等行為。

2)提取特征向量，并通過(guò)k-means++算法進(jìn)行聚類

首先對(duì)輸入的視頻中的圖像提取特征向量，然后將提取的特征通過(guò)k-means++算法進(jìn)行聚類，對(duì)子空間進(jìn)行迭代聚類，這樣可以對(duì)特征進(jìn)行聚類。相同的劃分同一類，迭代次數(shù)即樹的深度需要人為定義。實(shí)際操作中，確定一個(gè)迭代次數(shù)的值，多次迭代以后仍在同一個(gè)最小子空間的特征視為同一類。

3)基于DBoW2建立詞袋樹模型，

對(duì)以上聚類后的特征建立樹結(jié)構(gòu)以保存特征，同時(shí)保存特征的正向索引(Direct index image)和反向索引(Inverse index word)。正向索引中包含每一幀圖像的特征以及該特征所在樹的父節(jié)點(diǎn)，反向索引存儲(chǔ)每一個(gè)特征所在的圖像的序號(hào)以及在該圖像中所占的比重，這里比重的值為TF*IDF，也就是詞頻TF與反向文本頻率IDF的乘積。其中詞頻是指給定的詞語(yǔ)在該文本中出現(xiàn)的頻率，反向文本頻率是指一個(gè)詞語(yǔ)普遍重要性的度量，某一特定詞語(yǔ)的反向文本頻率，可以用總文件數(shù)目除以包含該詞語(yǔ)的文件的數(shù)目，再將得到的商取對(duì)數(shù)得到。

通過(guò)反向索引能快速定位到需要比較的相似的圖像，而避免了對(duì)所有圖像進(jìn)行比較，正向索引則能快速檢查圖像中的特征。在實(shí)際操作中，對(duì)輸入的每一個(gè)圖像都要計(jì)算正向索引和反向索引。

4)建立詞袋樹群模型

間隔一定時(shí)間T后保存已經(jīng)建立的樹模型，映射出該樹模型結(jié)構(gòu)并用于之后的更新，直到經(jīng)過(guò)設(shè)定的時(shí)間T，即圖1中的Time＞T，其中Time表示累加時(shí)間，T表示設(shè)定的時(shí)間閾值。

5)重復(fù)上述操作，直到建立N個(gè)詞袋樹模型。

6)通過(guò)統(tǒng)計(jì)原理對(duì)家具等低動(dòng)態(tài)物體運(yùn)動(dòng)規(guī)律進(jìn)行統(tǒng)計(jì)

現(xiàn)有的特定的圖像處理技術(shù)，已經(jīng)能判斷門的開關(guān)以及窗戶的開關(guān)，以及家具的位置移動(dòng)。通過(guò)一定的假設(shè)，并在后期對(duì)生活家具的數(shù)據(jù)的檢驗(yàn)和校正中更正運(yùn)動(dòng)規(guī)律，對(duì)運(yùn)動(dòng)規(guī)律劃分為多個(gè)時(shí)間點(diǎn)以方便后續(xù)對(duì)應(yīng)到詞袋樹模型。

7)借助統(tǒng)計(jì)分析確定此時(shí)刻對(duì)應(yīng)的詞袋樹模型

根據(jù)以上的統(tǒng)計(jì)分析對(duì)應(yīng)的時(shí)間點(diǎn)來(lái)選擇合適的詞袋樹模型，得出三個(gè)概率最大的詞袋樹，并作為室內(nèi)機(jī)器人定位和導(dǎo)航的回環(huán)檢測(cè)模塊的數(shù)據(jù)庫(kù)。

8)依次檢驗(yàn)三個(gè)樹模型

在實(shí)際應(yīng)用中，如果當(dāng)前通過(guò)環(huán)境獲得的特征向量在概率最大的三個(gè)詞袋樹模型找到匹配，則將機(jī)器人重新定位，并優(yōu)化詞袋樹群模型，將詞袋樹群模型中不匹配或者冗余的特征點(diǎn)去除；如果不能匹配則搜索剩下所有的詞袋樹，如果仍然不能匹配，則將重新生成詞袋樹，將此時(shí)通過(guò)環(huán)境獲得的特征向量加入新的詞袋樹中。

以上實(shí)施例僅用以說(shuō)明本發(fā)明的技術(shù)方案而非對(duì)其進(jìn)行限制，本領(lǐng)域的普通技術(shù)人員可以對(duì)本發(fā)明的技術(shù)方案進(jìn)行修改或者等同替換，而不脫離本發(fā)明的精神和范圍，本發(fā)明的保護(hù)范圍應(yīng)以權(quán)利要求書所述為準(zhǔn)。