單目視覺慣性SLAM與UWB數據融合的精確定位*

李玉卿， 鮑 泓， 徐 成

(1.北京聯合大學智慧城市學院,北京 100101； 2.北京聯合大學 北京市信息服務工程重點實驗室,北京 100101)

0 引 言

室內或封閉場景下的無人駕駛，主要通過同步定位與建圖(simultaneous localization and mapping,SLAM)的方法實現定位。單純依靠視覺傳感器的SLAM[1,2]不能滿足實際需求。視覺SLAM存在傳統算法和深度學習兩種方法。傳統視覺SLAM分成特征點法和直接法。特征點法：首次實現跟蹤和建圖的并行化PTAM[3]，其特征跟蹤在前端，地圖優化在后端；ORB-SLAM2[4]在PATM的基礎上添加閉環修正線程，場景相對單一或速度較快時效果變差；VINS-Mono[5]加入了慣性數據，光流跟蹤的特征匹配易受光照變化影響。直接法包括DTAM[6]，特征缺失、圖像模糊下仍能運行，但計算量過大；LSD-SLAM[7]直接估計關鍵幀之間相似變換與尺度感知，對于相機的內參和曝光要求十分嚴格；SVO[8]與DSO的運行速度較快，但都舍棄了回環檢測，累積誤差會隨著時間增大。

基于深度學習的視覺SLAM也是研究的熱門。PoseNet[9]在GoogleNet[10]的基礎上將6自由度位姿作為回歸問題進行網絡改進；LIFT[11]利用深度神經網絡學習圖像中的特征點；CNN-SLAM[12]在LSD-SLAM基礎上用卷積神經網絡進行深度估計以及圖像匹配；UndeepVO[13]也使用深度神經網絡估計單目相機的自由度位姿及深度。但基于深度學習的視覺SLAM過于依賴訓練集，當實際運動速度、幀頻率與訓練數據存在差異時，誤差會迅速增大。

超寬帶(ultra-wide band，UWB)定位技術，具有分辨率高、穿透力強、功耗低等優點。其定位原理與GPS相似，一定范圍內可獲得較高的測距和定位精度。UWB定位方式中信號到達時間差(time difference of arrival，TDOA)法，通過測量移動終端到不同發送端的距離或距離差進而進行定位，相較于其他方法對周圍環境的抗干擾能力更強。

目前單純視覺SLAM不能滿足實際需求[14]，存在定位精度不高魯棒性差的問題。UWB定位技術精度較高，但信號受到干擾或物體遮擋時，會產生較大的誤差。因此本文將單目視覺慣性ORB(VIORB)[15]與UWB獲取定位數據進行融合提升定位的精度與魯棒性，實現高精度室內實時定位。

1 VIORB-SLAM與UWB定位

1.1 VIORB-SLAM

單目相機在拍攝過程中會丟失場景的深度信息，建圖時存在尺度不確定性的問題，對于單目VIORB定位算法，在ORB-SLAM2的基礎上加入慣性測量單元(inertial mea-surement unit，IMU)信息通過IMU預積分獲取尺度信息，得到更精確的相機的位姿。ORB-SLAM2是由特征跟蹤、局部建圖、閉環檢測三個線程構成。VIORB分別進行了修改，融合了IMU數據信息。VIORB算法的流程如圖1所示。

圖1 VIORB算法流程

1)特征跟蹤：加入IMU數據信息，得到更加精確的數據，基于最小化重投影誤差和最小化IMU誤差，建立幀與幀之間的約束關系，從而優化當前幀。

2)局部建圖：優化前N個關鍵幀以及這些關鍵幀看到的點，當前關鍵幀N+1固定不變，提供IMU預積分約束。連續關鍵幀之間的時間差異越長，IMU提供的信息越弱。

3)回環檢測：IMU提供尺度信息，全局位姿優化下降到6個自由度。完成全局位姿優化后，再對速度進行矯正，執行一次全局優化，優化所有的系統狀態。

1.2 UWB定位

UWB通信系統通過信號在兩個異步收發機之間飛行時間之差來測量節點間的距離。TDOA定位方式利用多個基站接收到信號的時間差來確定標簽的位置，不需要檢測信號傳輸時間，對時間同步的要求大大降低。

已知標簽到基站間的距離差為常數，時間差也為常數，標簽的位置一定處于以這兩點為焦點的雙曲線上。三個基站就存在三條雙曲線，曲線相交的點就是標簽的位置。標簽相對于基站A(x1,y1),B(x2,y2),C(x3,y3)的距離差

(1)

式中c為光速，τi為第i個基站收到標簽發送信號的時間，消除式時間差Δt得到

(2)

通過求解式(2)中方程組,即可得到標簽位置的坐標(x,y)。

2 視覺與UWB融合

2.1 融合算法框架

對于單目視覺慣性SLAM與UWB組合的非線性系統，通過擴展卡爾曼濾波器(extended Kalman filter,EKF)進行融合。數據融合算法流程如圖2所示。

圖2 數據融合算法流程

2.1 EKF

EKF主要包括預測更新和測量校正。通過前一時刻狀態向量xk-1對當前的狀態xk進行預測，根據預測的估計量對當前的狀態修正，最后得到當前最優狀態向量yk。滿足如下關系

xk=f(xk-1)+wk-1,yk=h(xk)+vk

(3)

式中wk-1為過程噪聲，vk為量測噪聲。預測更新包括狀態預測和誤差協方差預測

(4)

Pk=FPk-1FT+Qk

(5)

Kk=PkHT(HPkHT+Rk)-1

(6)

(7)

(8)

式中Rk為觀測噪聲協方差矩陣,yk為k時刻系統觀測到的狀態向量。

2.2 數據融合

將VIORB與UWB的坐標信息經過空間變換轉換到全局坐標系下，通過EKF將兩種數據進行融合。算法的具體步驟為:

1)數據初始化：將VIORB與UWB獲得的定位數據傳入。輸入數據分別為

(9)

2)預測更新：以k-1時刻的最優狀態向量對k時刻的狀態向量進行預測估計，如式(3)中，則

(10)

通過狀態預測和誤差協方差預測方程，獲得對當前時刻狀態向量的估計。

3)測量校正：預測獲得k時刻的估計狀態向量對當前觀測到的狀態向量修正。根據式(6)計算測量校正方程組，獲得當前最優的狀態向量。式(6)中的H為

(11)

3 實驗結果與分析

實驗硬件環境為i7—7700，GTX1050Ti。單目USB攝像頭，六軸加速度計MPU6050模塊，Link Track定位測距模塊。首先固定設備，使其重心位于同一直線，調整相機與IMU的坐標軸方向一致。調節UWB頻率與相機幀率一致。軟件環境為Ubuntu16.04，ROS-kinetic，OpenCV3.4.5，軌跡評估工具EVO。將UWB定位基站布置在實驗區域的四角上，按照預設軌跡均勻低速采集數據。實驗場景如圖3所示。

圖3 實驗場景示意

實驗在室內環境下，分為正常光照與弱光光照。實驗過程中采集的數據通過EVO與固定軌跡進行對比，結果如圖4所示。從誤差最值、誤差平均值、均方根誤差及標準差4個方面進行比較。如表1和表2所示。

表1 正常光照下三種方法軌跡誤差比較

表2 弱光光照下三種方法軌跡誤差比較

圖4 正常、弱光光照下三種方法軌跡對比

UWB定位精度較高，當存在障礙物遮擋等時，定位精度會受到影響出現較大誤差。VIORB旋轉時，相機視野內的場景變化較大，易發生特征點丟失導致跟蹤失敗，且IMU

誤差也隨之增大。本文提出的數據融合的方法在實驗過程中定位精度較高，在方向改變的場景下，沒有出現明顯的跳變及顯著漂移。表1給出了正常光照下VIORB，UWB及本文算法的誤差統計。受非視覺誤差的影響，UWB的誤差最值較大，但系統運行過程中的魯棒性較好；VIORB魯棒性較差且環境因素對定位精度的影響較大。本文算法定位精度的最大誤差為0.101 316 m，RMSE為0.035 965，相較于前兩種方法的最大誤差分別為0.146 437 m與0.153 275 m，融合算法的軌跡更契合實際軌跡，且沒有顯著的漂移誤差。

弱光光照條件下，雖然VIORB使用的是ORB特征提取算子，具有一定的光照不變性，但較于正常光照還是產生了部分誤差，仍能完成整個定位過程，相應因累積誤差而產生的漂移要大于正常光照。相較于VIORB與UWB，本文方法的定位精度仍要優于單一傳感器方式的定位方法。

表2給出了弱光光照下單目VIORB、UWB及本文算法的誤差統計。受弱光光照的影響，定位過程中因累積誤差而產生的漂移導致了誤差增大，但仍能完成定位過程，最大誤差為0.136 103 m，RMSE為0.039 132，前兩種方法的最大誤差分別為0.195 078 m與0.153 275 m，定位精度優于前兩種方法。

4 結 論

本文提出一種將單目VIORB與UWB數據融合的定位方法可有效解決視覺SLAM定位精度不高，魯棒性不強的問題,且抑制UWB定位過程中非視距誤差的影響。通過VIORB與UWB的對比,證明提出的融合算法改善了定位的效果，提高了系統定位的精度及魯棒性。