(東南大學(xué) 儀器科學(xué)與工程學(xué)院，江蘇 南京 210096)

單目視覺SLAM算法是機(jī)器人自主導(dǎo)航、增強(qiáng)現(xiàn)實(shí)跟蹤注冊(cè)等領(lǐng)域的研究熱點(diǎn)。SLAM的本質(zhì)是貝葉斯?fàn)顟B(tài)估計(jì)，依據(jù)觀測(cè)方程不斷更新狀態(tài)方程[1]。傳統(tǒng)的SLAM算法在環(huán)境中缺乏標(biāo)志物或者環(huán)境單一的情況下會(huì)出現(xiàn)定位不準(zhǔn)確的問題。基于此問題研究了視覺慣性SLAM算法[2]。通過在前端提取特征點(diǎn)，進(jìn)行特征點(diǎn)匹配建立地圖，同時(shí)在后端通過非線性優(yōu)化算法來求解。該組合方法一方面，相機(jī)提供了豐富的環(huán)境信息，可以構(gòu)建三維模型、定位和識(shí)別相機(jī)已經(jīng)訪問過的地點(diǎn)；另一方面，SINS傳感器提供了自運(yùn)動(dòng)信息，可以恢復(fù)單目視覺的尺度信息、估計(jì)重力方向、提供可視的絕對(duì)俯仰和滾動(dòng)[3]。但上述方法還存在以下問題：① 沒有回環(huán)檢測(cè)或缺乏地圖重利用功能，這會(huì)導(dǎo)致預(yù)測(cè)的軌跡漂移不斷增加；② 緊耦合的視覺慣性系統(tǒng)考慮了特征之間的交叉耦合使其運(yùn)算復(fù)雜度為O(M2)，這制約了緊耦合系統(tǒng)的工作范圍，當(dāng)SLAM的地圖擴(kuò)展到一定大小，實(shí)時(shí)性就很難被滿足；③ 傳統(tǒng)的SLAM/SINS組合導(dǎo)航系統(tǒng)中SINS的價(jià)格比較昂貴，無法滿足一些特定使用場(chǎng)合[4]。

針對(duì)上述問題，首先提出了一種在移動(dòng)設(shè)備上實(shí)現(xiàn)的視覺慣性SLAM松組合組合方法，降低計(jì)算復(fù)雜度；通過使用移動(dòng)設(shè)備自帶的消費(fèi)級(jí)陀螺儀滿足了特定場(chǎng)合的定位精度，如移動(dòng)端增強(qiáng)現(xiàn)實(shí)的跟蹤注冊(cè)。其次，對(duì)關(guān)鍵幀進(jìn)行回環(huán)檢測(cè)，提高了定位精度，解決了漂移問題。

本文研究單目視覺ORB-SLAM/SINS松組合的定位技術(shù)。介紹了傳統(tǒng)視覺慣性SLAM方法的不足；基于ORB-SLAM算法的工作原理，根據(jù)ORB-SLAM的輸出，結(jié)合SINS導(dǎo)航并利用最小二乘法計(jì)算出尺度因子；同時(shí)，構(gòu)建SLAM/SINS的非線性卡爾曼濾波器，將ORB-SLAM系統(tǒng)輸出的姿態(tài)信息經(jīng)過尺度變換后作為卡爾曼濾波器的觀測(cè)量；最后通過3個(gè)實(shí)驗(yàn)來驗(yàn)證本文算法的可行性。

1 整體框架

系統(tǒng)主要包括3個(gè)部分。首先通過ORB-SLAM算法求出相機(jī)的相對(duì)姿態(tài)和位置；然后結(jié)合SINS導(dǎo)航求得ORB-SLAM算法的尺度因子；最后將ORB-SLAM算法的輸出經(jīng)過尺度因子的變換后作為觀測(cè)量進(jìn)行卡爾曼濾波，從而實(shí)現(xiàn)視覺慣性SLAM定位松組合方法。算法的整體框架如圖1所示。

圖1 視覺慣性組合方法整體框架

2 單目ORB-SLAM算法及尺度因子估計(jì)

2.1 單目ORB-SLAM算法

單目視覺ORB-SLAM算法分為3個(gè)并行的線程[5]：跟蹤、地圖構(gòu)建以及回環(huán)檢測(cè)。算法的總體框架如圖2所示。

圖2 ORB-SLAM算法總體框圖

追蹤模塊首先從圖像中提取ORB[6]特征點(diǎn)，然后進(jìn)行姿態(tài)估計(jì)，利用鄰近的地圖點(diǎn)，尋找更多的能成功匹配的特征點(diǎn)以實(shí)現(xiàn)姿態(tài)優(yōu)化，最后選取關(guān)鍵幀完成地圖構(gòu)建的初始化。

地圖構(gòu)建模塊通過不斷地插入關(guān)鍵幀并對(duì)其中的特征點(diǎn)進(jìn)行極線搜索匹配。利用三角法生成新的地圖點(diǎn)并使用局部光束平差法對(duì)不符合要求的地圖點(diǎn)去除局外點(diǎn)和重復(fù)幀。最后插入新的地圖點(diǎn)，再利用光束法平差對(duì)其進(jìn)行優(yōu)化，從而實(shí)現(xiàn)對(duì)局部和全局地圖的更新。

回環(huán)檢測(cè)模塊包括回環(huán)探測(cè)和回環(huán)校正，其中，回環(huán)檢測(cè)通過Bag of Words(BOW)算法[7]實(shí)現(xiàn)，即首先匹配每個(gè)候選的回環(huán)幀和當(dāng)前幀上的特征點(diǎn)，然后用得到的特征點(diǎn)所對(duì)應(yīng)的三維點(diǎn)求解RANSAC的相似變換矩陣。回環(huán)校正通過對(duì)回環(huán)檢測(cè)的結(jié)果進(jìn)行優(yōu)化，從而提高單目ORB-SLAM算法的穩(wěn)定性。

2.1 尺度因子估計(jì)

由于單目視覺本身不具有深度信息， 尺度因子無法確定，因此尺度因子的估計(jì)問題是單目視覺ORB-SLAM/SINS組合導(dǎo)航系統(tǒng)首要解決的問題。

在傳統(tǒng)的采用人工標(biāo)記的單目視覺SLAM技術(shù)中，常用到以下4個(gè)坐標(biāo)系：圖像像素坐標(biāo)系、理想屏幕(成像平面)坐標(biāo)系、攝像頭坐標(biāo)系以及世界坐標(biāo)系。類似地，在所采用的單目視覺SLAM技術(shù)中，使用如下4個(gè)坐標(biāo)系：圖像像素坐標(biāo)系、成像平面坐標(biāo)系、相機(jī)坐標(biāo)系以及世界坐標(biāo)系。其中，視覺坐標(biāo)系(V系)的Z軸與初始化的平面垂直；相機(jī)坐標(biāo)系(C系)的原點(diǎn)為相機(jī)的鏡頭光心，坐標(biāo)系Z軸與成像平面垂直，攝影方向?yàn)樽鴺?biāo)系的正向，C系的X、Y軸分別與圖像坐標(biāo)系對(duì)應(yīng)軸平行。在此基礎(chǔ)上，將輸出結(jié)果與SINS的輸出結(jié)果進(jìn)行融合，故而在組合系統(tǒng)里，關(guān)于視覺系統(tǒng)，主要研究V系與C系。視覺系統(tǒng)參考坐標(biāo)系如圖3所示[8]

圖3 視覺系統(tǒng)參考坐標(biāo)系

(1)

(2)

式中，s為未知的尺度因子。

(3)

(4)

(5)

(6)

(7)

根據(jù)式(5)～式(7)可以計(jì)算出線性加速度和角速度。

(8)

綜上所述，通過將SLAM與SINS結(jié)合估算單目SLAM的尺度因子，從而在得到尺度因子之后將ORB-SLAM系統(tǒng)輸出的位置信息經(jīng)過尺度變換后作為觀測(cè)量進(jìn)行卡爾曼濾波更新。

3 ORB-SLAM/SINS組合濾波器設(shè)計(jì)

為了準(zhǔn)確表示SINS測(cè)量數(shù)據(jù)的誤差，對(duì)于陀螺與加速度計(jì)的測(cè)量值，采用的慣性傳感器模型如下：

(9)

該模型假設(shè)SINS的輸出ωb(角速度)與ab(加速度)包含靜態(tài)偏差b以及動(dòng)態(tài)偏差n，動(dòng)態(tài)偏差被視為高斯白噪聲，ω與a為真值。

另外b的變化是一個(gè)高斯隨機(jī)過程：

(10)

3.1 系統(tǒng)狀態(tài)方程

(11)

狀態(tài)方程的差分形式為

(12)

(13)

(14)

(15)

式中，g為導(dǎo)航坐標(biāo)系下的重力加速度向量；Ω為角速度ω的四元數(shù)乘積矩陣。

3.2 系統(tǒng)誤差方程

當(dāng)姿態(tài)誤差較小的時(shí)候，為了增加數(shù)值穩(wěn)定性采用四元數(shù)來表示誤差。此時(shí)的四元數(shù)為其最小表達(dá)形式，根據(jù)文獻(xiàn)[10]可得：

(16)

(17)

(18)

據(jù)此，系統(tǒng)的誤差狀態(tài)向量為

(19)

誤差狀態(tài)方程的差分形式為

(20)

(21)

(22)

(23)

通過四階龍格庫塔方法離散化系統(tǒng)的誤差狀態(tài)方程，并且根據(jù)SINS采集的數(shù)據(jù)，持續(xù)更新系統(tǒng)誤差狀態(tài)。

3.3 系統(tǒng)量測(cè)方程

(24)

(25)

其誤差量測(cè)方程由文獻(xiàn)[12]求得

(26)

(27)

擴(kuò)展卡爾曼濾波器可以通過每次獲得視覺系統(tǒng)的更新，計(jì)算出卡爾曼增益K，而后更新協(xié)方差P，并且利用ORB-SLAM系統(tǒng)的輸出修正慣性系統(tǒng)的預(yù)測(cè)值從而達(dá)到預(yù)期效果。

4 實(shí)驗(yàn)驗(yàn)證與分析

4.1 實(shí)驗(yàn)平臺(tái)搭建

考慮到設(shè)備的穩(wěn)定性和平衡性，采用iPhone作為實(shí)驗(yàn)平臺(tái)。并在Ubuntu 16.04的操作系統(tǒng)下利用標(biāo)準(zhǔn)數(shù)據(jù)集進(jìn)行算法驗(yàn)證，系統(tǒng)實(shí)現(xiàn)為能夠在iPhone設(shè)備上實(shí)時(shí)運(yùn)行的iOS程序。使用手機(jī)捕獲的頻率為30 Hz，分辨率為640像素×480像素的圖像，以及內(nèi)置InvenSense MP67B 六軸陀螺儀和加速度計(jì)獲得的100 Hz的IMU數(shù)據(jù)。

4.2 實(shí)驗(yàn)結(jié)果演示與分析

4.2.1 定位對(duì)比實(shí)驗(yàn)結(jié)果

采用TUM標(biāo)準(zhǔn)的數(shù)據(jù)集比較了ORB-SLAM和ORB-SLAM/SINS組合方法在X，Y，Z軸方向上的定位精度，實(shí)驗(yàn)結(jié)果如圖4所示。所提出的ORB-SLAM/SINS的定位結(jié)果與標(biāo)定好的真實(shí)數(shù)據(jù)幾乎重合，由此可見本文提出的ORB-SLAM/SINS組合導(dǎo)航方法較之前的定位算法的精度有了比較好的提升，因此采用的方法是可取的。

4.2.2 EuRoC標(biāo)準(zhǔn)數(shù)據(jù)集實(shí)驗(yàn)

本次實(shí)驗(yàn)分為兩個(gè)部分，第一部分利用EuRoC標(biāo)準(zhǔn)數(shù)據(jù)集中的單一序列(MH_01_EASY)來驗(yàn)證本文算法的可行性，實(shí)驗(yàn)結(jié)果如圖5所示(取極短時(shí)間)，軌跡①代表的是標(biāo)定好的真實(shí)數(shù)據(jù)，軌跡②表示的是利用本課題的算法預(yù)測(cè)的軌跡，實(shí)驗(yàn)結(jié)果表明，真實(shí)的軌跡與預(yù)測(cè)出的軌跡基本一致。第二部分融合數(shù)據(jù)集中的3個(gè)序列(MH_01_EASY、MH_02_EASY、MH_03_MEDIUM)對(duì)回環(huán)檢測(cè)進(jìn)行驗(yàn)證，這3個(gè)序列中包含旋轉(zhuǎn)和光照變化。實(shí)驗(yàn)結(jié)果如圖6所示，其中，軌跡①代表的是標(biāo)定好的真實(shí)數(shù)據(jù)，軌跡②表示的是利用本課題的算法預(yù)測(cè)的軌跡，而軌跡③則表示融合了3個(gè)序列的場(chǎng)景之后本文算法在運(yùn)行的過程中會(huì)進(jìn)行回環(huán)檢測(cè)。

圖4 定位誤差比較

圖5 MH_01_EASY 運(yùn)行結(jié)果

圖6 融合數(shù)據(jù)回環(huán)檢測(cè)實(shí)驗(yàn)

4.2.3 移動(dòng)增強(qiáng)現(xiàn)實(shí)實(shí)驗(yàn)

在本實(shí)驗(yàn)中，將本文的組合系統(tǒng)輸入到一個(gè)移動(dòng)設(shè)備，使用AR應(yīng)用程序演示系統(tǒng)對(duì)實(shí)時(shí)漂移消除的能力。在圖7實(shí)驗(yàn)中，將一個(gè)虛擬立方體插入到利用SLAM/SINS估計(jì)的視覺特征提取的平面上，手持手機(jī)以正常的速度在房間里走動(dòng)，由于增加了回環(huán)檢測(cè)模塊，立方體在經(jīng)過位移后可以注冊(cè)到圖像上的同一個(gè)位置，即可證明累積的漂移可以得到有效地消除。

圖7 漂移消除能力

5 結(jié)束語

本文把單目視覺ORB-SLAM與SINS相結(jié)合，利用智能手機(jī)內(nèi)置的消費(fèi)級(jí)陀螺儀研究了一種基于松組合的視覺慣性SLAM組合方法。根據(jù)慣導(dǎo)與視覺在時(shí)間與空間上的互補(bǔ)關(guān)系，實(shí)現(xiàn)了融合慣導(dǎo)與視覺數(shù)據(jù)的非線性卡爾曼濾波器。增加回環(huán)檢測(cè)功能使得系統(tǒng)具有較好的漂移消除能力。設(shè)計(jì)實(shí)驗(yàn)平臺(tái)，采用標(biāo)準(zhǔn)的數(shù)據(jù)集對(duì)本文提出的算法與開源的SLAM算法進(jìn)行比較，驗(yàn)證算法的正確性，另外，在智能手機(jī)上開發(fā)增強(qiáng)現(xiàn)實(shí)軟件，通過增強(qiáng)現(xiàn)實(shí)對(duì)跟蹤結(jié)果進(jìn)行演示，驗(yàn)證在較大的空間范圍和環(huán)境干擾下，具備較好的漂移消除能力。在SLAM未來的研究方向上計(jì)劃在地圖創(chuàng)建上采用深度學(xué)習(xí)等方法建立語義地圖。