本發(fā)明屬于基于單攝像頭的移動機(jī)器人自主定位與導(dǎo)航領(lǐng)域，特別涉及一種基于重跟蹤策略的單目視覺同步定位與地圖構(gòu)建方法。

背景技術(shù)：

隨著計(jì)算機(jī)技術(shù)和人工智能的發(fā)展，機(jī)器人的研究得到越來越多的關(guān)注和投入，智能自主移動機(jī)器人成為機(jī)器人領(lǐng)域的一個(gè)重要研究方向和研究熱點(diǎn)。同步定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)問題是指移動機(jī)器人在未知環(huán)境中，利用自身的傳感器(如激光雷達(dá)、里程計(jì)、攝像頭、超聲波等)創(chuàng)建一個(gè)與環(huán)境相一致的地圖，并同時(shí)利用該地圖對機(jī)器人的位置和姿態(tài)進(jìn)行估計(jì)。該問題被認(rèn)為能否真正實(shí)現(xiàn)機(jī)器人自主導(dǎo)航的關(guān)鍵問題。

近年來，隨著計(jì)算機(jī)視覺技術(shù)的發(fā)展以及計(jì)算機(jī)運(yùn)算能力的增強(qiáng)，出現(xiàn)了大量利用攝像機(jī)作為傳感器的視覺SLAM方法。視覺傳感器具有信息量大、靈活性高、成本低等優(yōu)點(diǎn)，這是其他傳感器所無法比擬的。單目視覺SLAM僅使用一個(gè)攝像頭作為傳感器完成同步定位與地圖創(chuàng)建操作，具有系統(tǒng)結(jié)構(gòu)簡單、成本低且易實(shí)現(xiàn)等優(yōu)點(diǎn)。因此，基于單目視覺SLAM技術(shù)具有廣闊的應(yīng)用前景。

現(xiàn)有的解決單目SLAM的方法主要分為基于概率框架的單目視覺SLAM方法和非概率框架的單目視覺SLAM方法?；诟怕士蚣艿膯文恳曈XSLAM方法主要通過遞歸的貝葉斯濾波方法估計(jì)相機(jī)姿態(tài)和地圖特征的空間位置，常用的解決方法有基于擴(kuò)展卡爾曼濾波器(extended Kalman filter)或粒子濾波器(particle filter)。非概率框架的單目視覺SLAM系統(tǒng)目前主要是基于關(guān)鍵幀和稀疏捆綁調(diào)整等優(yōu)化算法實(shí)現(xiàn)，常用的解決方法有直接法或基于特征的方法。

非概率框架的單目視覺SLAM系統(tǒng)要求相鄰兩幀的基線較短(small baseline)，以便系統(tǒng)能夠估計(jì)每幀的姿態(tài)。當(dāng)跟蹤丟失(tracking lost)，即當(dāng)前幀的姿態(tài)無法被正確估計(jì)時(shí)，系統(tǒng)采用重定位策略，只有當(dāng)機(jī)器人回到曾經(jīng)經(jīng)過的地點(diǎn)時(shí)，SLAM系統(tǒng)才能繼續(xù)定位相機(jī)的位置并重建周圍環(huán)境。這種策略的缺點(diǎn)在于從跟蹤丟失到重定位成功之間，相機(jī)的姿態(tài)和地圖無法重建出來。

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

針對現(xiàn)有技術(shù)存在的技術(shù)問題，本發(fā)明的目的是提供一種基于重跟蹤策略的單目視覺機(jī)器人同步定位與地圖構(gòu)建方法；本發(fā)明致力于解決傳統(tǒng)方法中由于使用重定位策略導(dǎo)致跟蹤丟失后部分相機(jī)姿態(tài)和環(huán)境無法估計(jì)與建模的問題，創(chuàng)新性地提出了一種重跟蹤策略，提高了相機(jī)姿態(tài)估計(jì)的成功率以及地圖重建的完整率。

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

一種基于重跟蹤策略的單目視覺機(jī)器人同步定位與地圖構(gòu)建方法，其步驟為：

1)輸入攝像頭捕獲的場景圖像，對每張圖片提取ORB特征；

2)利用相鄰圖片的特征匹配跟蹤相機(jī)姿態(tài)并估計(jì)特征點(diǎn)的深度信息；因特征匹配失敗而導(dǎo)致相機(jī)姿態(tài)無法被估計(jì)定義為跟蹤丟失；

3)對所述跟蹤丟失的圖片執(zhí)行重定位與重跟蹤策略；所述重跟蹤策略采用局部初始化算法；

4)若所述重定位成功執(zhí)行，則估計(jì)當(dāng)前相機(jī)的姿態(tài)，且停止執(zhí)行所述重跟蹤策略并刪除其產(chǎn)生的臨時(shí)變量；

5)若所述重跟蹤成功執(zhí)行，則產(chǎn)生一條新的軌跡；

6)對所述產(chǎn)生的軌跡數(shù)量進(jìn)行判定，若數(shù)量超過閾值，則將最初始的軌跡剔除；

7)對每個(gè)關(guān)鍵幀進(jìn)行閉環(huán)檢測，檢測成功后進(jìn)行軌跡融合；

8)定位系統(tǒng)結(jié)束時(shí)，對所述軌跡進(jìn)行篩選，得到一條姿態(tài)正確的軌跡。

上述方法對跟蹤丟失這種情況采用了重定位與重跟蹤(局部初始化)兩種策略。下面進(jìn)一步具體說明本發(fā)明的技術(shù)內(nèi)容：

1.局部初始化

本發(fā)明的局部初始化算法改進(jìn)于ORB-SLAM(參考文獻(xiàn)：R.Mur-Artal,J.M.M Montiel,and J.D.Tardos.ORB-SLAM:a versatile and accurate monocular SLAM system.IEEE Transactions on Robotics,2015)的自動地圖初始化算法。當(dāng)系統(tǒng)檢測到跟蹤丟失時(shí)，局部初始化的過程如下：

a)查找初始匹配點(diǎn)對：

提取當(dāng)前幀的ORB特征，搜索當(dāng)前幀與參考幀(首次迭代時(shí)，以跟蹤丟失前的最后一個(gè)關(guān)鍵幀為參考幀)的特征點(diǎn)匹配個(gè)數(shù)。若匹配數(shù)量大于設(shè)定閾值，則執(zhí)行b)；否則，將當(dāng)前幀設(shè)置為參考幀，重新執(zhí)行a)。

b)模型選擇：

采用RANSAC(Random Sample Consensus，隨機(jī)抽樣一致性)策略，分別用八點(diǎn)法與四點(diǎn)法估計(jì)基本矩陣F(Foundamental matrix)與單應(yīng)矩陣H(Homography matrix)，并計(jì)算相應(yīng)的重投影誤差，選擇誤差小的矩陣作為系統(tǒng)模型。

c)運(yùn)動估計(jì)與地圖構(gòu)建：

利用上述b)中選擇的模型，計(jì)算相機(jī)的姿態(tài)以及構(gòu)建環(huán)境地圖。

2.軌跡修正與融合

Loop closure即軌跡閉環(huán)，是指相機(jī)捕獲的當(dāng)前場景與先前走過的場景一樣，即相機(jī)回到曾經(jīng)經(jīng)過的地方。利用該閉環(huán)，可以大大消除累計(jì)誤差。具體步驟如下：

a)計(jì)算當(dāng)前關(guān)鍵幀c與閉環(huán)關(guān)鍵幀l的相似變換S_c,l，獲取累計(jì)誤差：

其中，s_c,l為正實(shí)數(shù)，表明尺度因子；R_c,l與t_c,l分別為旋轉(zhuǎn)矩陣與平移矩陣，Sim(3)為相似變換群。

b)用S_c,l修正當(dāng)前關(guān)鍵幀的姿態(tài)T_c,w：

其中w表示世界坐標(biāo)系，c表示當(dāng)前關(guān)鍵幀，l表示閉環(huán)關(guān)鍵幀，T_l,w為閉環(huán)關(guān)鍵幀l的姿態(tài)，為修正后的當(dāng)前關(guān)鍵幀的姿態(tài)。

c)修正當(dāng)前關(guān)鍵幀所在軌跡的其余關(guān)鍵幀。該步驟分為兩種情況：一、若當(dāng)前關(guān)鍵幀與閉環(huán)關(guān)鍵幀屬于同一條軌跡，則修正當(dāng)前關(guān)鍵幀的所有鄰接關(guān)鍵幀，該過程與ORB-SLAM的方法一致；二、若當(dāng)前關(guān)鍵幀與閉環(huán)關(guān)鍵幀屬于不同的軌跡，則按從后向前的順序修正當(dāng)前軌跡上的所有關(guān)鍵幀，并將當(dāng)前軌跡融合進(jìn)閉環(huán)軌跡。該過程分兩步執(zhí)行，首先計(jì)算當(dāng)前軌跡上相鄰關(guān)鍵幀的相對姿態(tài)：

其中σ＝(1,2,...,n-1),n為當(dāng)前軌跡的關(guān)鍵幀數(shù)量；是T_c-σ,w的逆矩陣，T_c-σ,w是第c-σ?guī)谑澜缱鴺?biāo)系w的姿態(tài)。

其次，利用計(jì)算出的相對姿態(tài)從后向前修正當(dāng)前軌跡剩余關(guān)鍵幀的姿態(tài)：

其中σ＝(1,2,...,n-1),n為當(dāng)前軌跡的關(guān)鍵幀數(shù)量；是ΔT_c-σ+1,c-σ的逆矩陣

d)將當(dāng)前軌跡的標(biāo)簽更改為閉環(huán)關(guān)鍵幀所在軌跡的標(biāo)簽。

3.軌跡刪除

實(shí)際應(yīng)用中，遮擋、抖動、紋理不足等狀況可能使跟蹤丟失的情況頻繁出現(xiàn)，進(jìn)而使SLAM系統(tǒng)產(chǎn)生多條軌跡。然而，SLAM系統(tǒng)只需要輸出一條準(zhǔn)確的相機(jī)軌跡，因此需要刪除多余的相機(jī)軌跡。

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

本發(fā)明提供了一種基于重跟蹤策略的單目視覺機(jī)器人同步定位與地圖構(gòu)建方法，能夠解決傳統(tǒng)方法中由于使用重定位策略導(dǎo)致跟蹤丟失后部分相機(jī)姿態(tài)和環(huán)境無法估計(jì)與建模的問題，通過重跟蹤策略提高了相機(jī)姿態(tài)估計(jì)的成功率以及地圖重建的完整率。本發(fā)明的優(yōu)勢在于能在快速移動、遮擋、紋理不足、光照變化的條件下，定位完整的相機(jī)軌跡。

附圖說明

圖1.本發(fā)明方法的總體框架圖；

圖2.本發(fā)明方法中生成新軌跡以及修正與融合軌跡的示意圖。

具體實(shí)施方式

下面通過具體實(shí)施例和附圖，對本發(fā)明做進(jìn)一步說明。

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

1.輸入攝像頭捕獲的場景圖像：

本發(fā)明以30fps的速率處理普通數(shù)碼攝像機(jī)采集的圖像。需要注意的是，由于純旋轉(zhuǎn)運(yùn)動沒有時(shí)差，不能估計(jì)特征點(diǎn)的深度，因此要求系統(tǒng)開始運(yùn)行時(shí)攝像機(jī)有一定的水平運(yùn)動，且運(yùn)動速度不宜太快，以便于完成系統(tǒng)的初始化。

2.判斷是否跟蹤丟失

若通過上一幀圖像無法估計(jì)出相機(jī)姿態(tài)，則認(rèn)為系統(tǒng)跟蹤丟失，執(zhí)行步驟4；否則，執(zhí)行步驟3。

3.提取ORB特征并估計(jì)相機(jī)姿態(tài)

ORB是一種具有尺度與旋轉(zhuǎn)不變性的特征提取算法。本發(fā)明利用開源計(jì)算機(jī)視覺數(shù)據(jù)庫(OpenCV)中自帶的特征點(diǎn)提取算法提取每幀圖像的ORB特征。用RANSAC迭代算法與PnP(Perspective-N-Points)算法獲取該圖像與參考關(guān)鍵幀的特征點(diǎn)匹配以及相機(jī)的姿態(tài)。若特征點(diǎn)匹配個(gè)數(shù)太少導(dǎo)致無法估計(jì)出相機(jī)姿態(tài)，則跟蹤丟失。

4.判斷重定位是否成功

當(dāng)系統(tǒng)處于跟蹤丟失的狀態(tài)時(shí)，需要對每一幀圖像執(zhí)行重定位算法(relocalization)，該算法為現(xiàn)有的一種算法，其將圖像轉(zhuǎn)換成詞袋模型(bag of words)并且在識別詞典(recognition database)中查詢匹配的關(guān)鍵幀(keyframe candidates)，若查詢到相匹配的關(guān)鍵幀，則說明重定位成功，刪除由局部初始化產(chǎn)生的臨時(shí)變量，然后估計(jì)相機(jī)的姿態(tài)與恢復(fù)地圖構(gòu)建，執(zhí)行步驟6；若查詢不到相匹配的關(guān)鍵幀，則說明重定位失敗，執(zhí)行步驟5。

5.判斷局部初始化是否成功

若局部初始化成功，系統(tǒng)首先判斷軌跡數(shù)量是否超過設(shè)定閾值T_r，如超過閾值，則首先刪除由局部初始化生成的標(biāo)簽最小的軌跡，然后再生成新的軌跡，否則直接從當(dāng)前位置生成新的軌跡。如圖2(a)所示，l₀是系統(tǒng)跟蹤的原始軌跡，在幀i處跟蹤丟失，由于重定位與局部初始化均失敗，相機(jī)軌跡出現(xiàn)中斷，如虛線b₁所示。在虛線末尾，局部初始化成功，生成新的軌跡l₁。

6.修正與融合軌跡

當(dāng)系統(tǒng)檢測到閉環(huán)時(shí)，需要修正軌跡的累計(jì)誤差。若當(dāng)前關(guān)鍵幀與閉環(huán)關(guān)鍵幀分別屬于不同的軌跡，還需要將當(dāng)前軌跡融合進(jìn)閉環(huán)軌跡。如圖2(b)所示，當(dāng)前關(guān)鍵幀j與閉環(huán)關(guān)鍵幀k形成閉環(huán)，兩幀之間存在累計(jì)誤差，如j與k之間虛線所示。此時(shí)，需要修正當(dāng)前軌跡l₂的誤差，并將其融合進(jìn)軌跡l₀，表示為l'₀，如圖2(c)所示。

7.刪除冗余軌跡

實(shí)際應(yīng)用中，遮擋、抖動等狀況可能使跟蹤丟失的情況頻繁出現(xiàn)，進(jìn)而使SLAM系統(tǒng)產(chǎn)生多條軌跡。然而，對軌跡跟蹤或者場景重建這類任務(wù)來說，SLAM系統(tǒng)只需要輸出一條準(zhǔn)確的相機(jī)軌跡。在這里，冗余軌跡定義為未能與初始軌跡形成閉環(huán)的軌跡，若第一次跟蹤丟失前的軌跡標(biāo)簽為0，由局部初始化算法生成的軌跡的標(biāo)簽逐漸遞增，則在系統(tǒng)即將結(jié)束時(shí)，需要將標(biāo)簽不為0的軌跡刪除。具體方法為：首先在關(guān)鍵幀數(shù)據(jù)庫(keyframe database)上刪除冗余軌跡上所有的關(guān)鍵幀，然后在重建地圖中(map point)中刪除這些關(guān)鍵幀對應(yīng)的點(diǎn)云圖。

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