單目視覺慣性SLAM與UWB數(shù)據(jù)融合的精確定位*

李玉卿， 鮑 泓， 徐 成

(1.北京聯(lián)合大學(xué)智慧城市學(xué)院,北京 100101； 2.北京聯(lián)合大學(xué) 北京市信息服務(wù)工程重點(diǎn)實(shí)驗(yàn)室,北京 100101)

0 引 言

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

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

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

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

1 VIORB-SLAM與UWB定位

1.1 VIORB-SLAM

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

圖1 VIORB算法流程

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

2)局部建圖：優(yōu)化前N個(gè)關(guān)鍵幀以及這些關(guān)鍵幀看到的點(diǎn)，當(dāng)前關(guān)鍵幀N+1固定不變，提供IMU預(yù)積分約束。連續(xù)關(guān)鍵幀之間的時(shí)間差異越長，IMU提供的信息越弱。

3)回環(huán)檢測：IMU提供尺度信息，全局位姿優(yōu)化下降到6個(gè)自由度。完成全局位姿優(yōu)化后，再對速度進(jìn)行矯正，執(zhí)行一次全局優(yōu)化，優(yōu)化所有的系統(tǒng)狀態(tài)。

1.2 UWB定位

UWB通信系統(tǒng)通過信號在兩個(gè)異步收發(fā)機(jī)之間飛行時(shí)間之差來測量節(jié)點(diǎn)間的距離。TDOA定位方式利用多個(gè)基站接收到信號的時(shí)間差來確定標(biāo)簽的位置，不需要檢測信號傳輸時(shí)間，對時(shí)間同步的要求大大降低。

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

(1)

式中c為光速，τi為第i個(gè)基站收到標(biāo)簽發(fā)送信號的時(shí)間，消除式時(shí)間差Δt得到

(2)

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

2 視覺與UWB融合

2.1 融合算法框架

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

圖2 數(shù)據(jù)融合算法流程

2.1 EKF

EKF主要包括預(yù)測更新和測量校正。通過前一時(shí)刻狀態(tài)向量xk-1對當(dāng)前的狀態(tài)xk進(jìn)行預(yù)測，根據(jù)預(yù)測的估計(jì)量對當(dāng)前的狀態(tài)修正，最后得到當(dāng)前最優(yōu)狀態(tài)向量yk。滿足如下關(guān)系

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

(3)

式中wk-1為過程噪聲，vk為量測噪聲。預(yù)測更新包括狀態(tài)預(yù)測和誤差協(xié)方差預(yù)測

(4)

Pk=FPk-1FT+Qk

(5)

Kk=PkHT(HPkHT+Rk)-1

(6)

(7)

(8)

式中Rk為觀測噪聲協(xié)方差矩陣,yk為k時(shí)刻系統(tǒng)觀測到的狀態(tài)向量。

2.2 數(shù)據(jù)融合

將VIORB與UWB的坐標(biāo)信息經(jīng)過空間變換轉(zhuǎn)換到全局坐標(biāo)系下，通過EKF將兩種數(shù)據(jù)進(jìn)行融合。算法的具體步驟為:

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

(9)

2)預(yù)測更新：以k-1時(shí)刻的最優(yōu)狀態(tài)向量對k時(shí)刻的狀態(tài)向量進(jìn)行預(yù)測估計(jì)，如式(3)中，則

(10)

通過狀態(tài)預(yù)測和誤差協(xié)方差預(yù)測方程，獲得對當(dāng)前時(shí)刻狀態(tài)向量的估計(jì)。

3)測量校正：預(yù)測獲得k時(shí)刻的估計(jì)狀態(tài)向量對當(dāng)前觀測到的狀態(tài)向量修正。根據(jù)式(6)計(jì)算測量校正方程組，獲得當(dāng)前最優(yōu)的狀態(tài)向量。式(6)中的H為

(11)

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

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

圖3 實(shí)驗(yàn)場景示意

實(shí)驗(yàn)在室內(nèi)環(huán)境下，分為正常光照與弱光光照。實(shí)驗(yàn)過程中采集的數(shù)據(jù)通過EVO與固定軌跡進(jìn)行對比，結(jié)果如圖4所示。從誤差最值、誤差平均值、均方根誤差及標(biāo)準(zhǔn)差4個(gè)方面進(jìn)行比較。如表1和表2所示。

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

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

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

UWB定位精度較高，當(dāng)存在障礙物遮擋等時(shí)，定位精度會受到影響出現(xiàn)較大誤差。VIORB旋轉(zhuǎn)時(shí)，相機(jī)視野內(nèi)的場景變化較大，易發(fā)生特征點(diǎn)丟失導(dǎo)致跟蹤失敗，且IMU

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

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

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

4 結(jié) 論

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