基于松組合的視覺慣性SLAM方法

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

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

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

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

1 整體框架

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

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

2 單目ORB-SLAM算法及尺度因子估計

2.1 單目ORB-SLAM算法

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

圖2 ORB-SLAM算法總體框圖

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

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

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

2.1 尺度因子估計

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

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

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

(1)

(2)

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

(3)

(4)

(5)

(6)

(7)

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

(8)

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

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

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

(9)

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

另外b的變化是一個高斯隨機過程：

(10)

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

(11)

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

(12)

(13)

(14)

(15)

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

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

當姿態(tài)誤差較小的時候，為了增加數(shù)值穩(wěn)定性采用四元數(shù)來表示誤差。此時的四元數(shù)為其最小表達形式，根據(jù)文獻[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)量測方程

(24)

(25)

其誤差量測方程由文獻[12]求得

(26)

(27)

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

4 實驗驗證與分析

4.1 實驗平臺搭建

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

4.2 實驗結(jié)果演示與分析

4.2.1 定位對比實驗結(jié)果

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

4.2.2 EuRoC標準數(shù)據(jù)集實驗

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

圖4 定位誤差比較

圖5 MH_01_EASY 運行結(jié)果

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

4.2.3 移動增強現(xiàn)實實驗

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

圖7 漂移消除能力

5 結(jié)束語

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