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

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

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

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

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

1 整體框架

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

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

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

2.1 單目ORB-SLAM算法

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

圖2 ORB-SLAM算法總體框圖

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

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

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

2.1 尺度因子估計

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

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

圖3 視覺系統參考坐標系

(1)

(2)

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

(3)

(4)

(5)

(6)

(7)

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

(8)

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

3 ORB-SLAM/SINS組合濾波器設計

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

(9)

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

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

(10)

3.1 系統狀態方程

(11)

狀態方程的差分形式為

(12)

(13)

(14)

(15)

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

3.2 系統誤差方程

當姿態誤差較小的時候，為了增加數值穩定性采用四元數來表示誤差。此時的四元數為其最小表達形式，根據文獻[10]可得：

(16)

(17)

(18)

據此，系統的誤差狀態向量為

(19)

誤差狀態方程的差分形式為

(20)

(21)

(22)

(23)

通過四階龍格庫塔方法離散化系統的誤差狀態方程，并且根據SINS采集的數據，持續更新系統誤差狀態。

3.3 系統量測方程

(24)

(25)

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

(26)

(27)

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

4 實驗驗證與分析

4.1 實驗平臺搭建

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

4.2 實驗結果演示與分析

4.2.1 定位對比實驗結果

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

4.2.2 EuRoC標準數據集實驗

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

圖4 定位誤差比較

圖5 MH_01_EASY 運行結果

圖6 融合數據回環檢測實驗

4.2.3 移動增強現實實驗

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

圖7 漂移消除能力

5 結束語

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