雙目視覺輔助GNSS在惡劣環(huán)境下導航定位

馮祎,涂銳,3,韓軍強,侯福榮,洪菊,劉金海,王星星

(1.中國科學院國家授時中心,陜西 西安 710600;2. 中國科學院大學,北京 100049;3. 中國科學院精密導航定位與定時技術重點實驗室,陜西 西安 710600)

0 引 言

隨著全球衛(wèi)星導航產(chǎn)業(yè)的快速發(fā)展,全球衛(wèi)星導航系統(tǒng)(GNSS)因可提供精密的位置、速度和時間信息得到了廣泛的應用．當在室外開闊的環(huán)境中時,GNSS具有高可用性和很好的精度,但是在城市峽谷、隧道等信號受到遮擋的惡劣環(huán)境中,GNSS位置估計誤差會明顯增大,結果甚至變得不可用．以往,學者們通過慣性導航系統(tǒng)(INS)輔助來改善GNSS的定位精度和可用性[1],隨著計算機圖形處理技術以及同步定位與建圖(SLAM)技術的快速發(fā)展,視覺里程計(VO)導航也成為一種新的導航定位方式,在機器人、無人機和自動駕駛車輛的多傳感器融合導航定位技術中得到了廣泛的應用．視覺里程計通過單目相機、雙目相機和深度相機以及組合導航系統(tǒng)獲取載體運動狀態(tài),實現(xiàn)導航定位功能．但是長時間單獨使用視覺里程計誤差較大,因此常與INS和GNSS等導航技術融合進行導航[2]．視覺里程計在導航過程中還能感知周圍的環(huán)境,識別周圍環(huán)境中的標志點,構建周圍環(huán)境的3D地圖,用于自動駕駛車輛和無人機等導航[3]．因此許多學者研究視覺里程計和GNSS組合系統(tǒng),Dusha[4]用類似經(jīng)典的GNSS/INS松耦合濾波器提出了GNSS和單目相機的松耦合模型,并使用一個俯瞰的飛行器實驗結果證實了其可行性．Schleicher[5]利用低成本GNSS和雙目相機實現(xiàn)了重復軌跡下實時的自動車輛導引．Aumayer[6]證實了在惡劣環(huán)境下GNSS定位精度變差時,雙目相機對GNSS定位精度具有明顯的改善作用．

V-SLAM(Visual SLAM)是指通過相機獲得的圖像估計自身的位姿,同時構建周圍環(huán)境地圖的技術．V-SLAM根據(jù)前端的跟蹤方法,可以分為直接法和特征點法:直接法根據(jù)圖像的某些像素或者全部像素不變求取幀間的位姿變化,經(jīng)典算法有SVO[7]算法和LSD-SLAM[8]算法;特征點法提取圖像的特征點,并通過匹配前后圖像的特征點來估計位姿變化,常用算法有PTAM[9]算法和ORB-SLAM[10]算法．視覺里程計在不需要初始環(huán)境信息下,能夠在小范圍和短時間內(nèi)實現(xiàn)自我運動軌跡的高精度估計．但是在復雜環(huán)境和長時間情況下其位置估計會有較大誤差,特別是在高速運動或者旋轉運動時．

本文介紹了GNSS和視覺里程計的松組合模型和方法,并通過KITTI[11]標準數(shù)據(jù)集模擬驗證了當GNSS信號中斷時,利用視覺里程計短時間高精度的特性進行持續(xù)導航定位,本文延續(xù)前人的工作,研究了GNSS信號中斷時組合導航系統(tǒng)的持續(xù)導航性能,驗證了組合系統(tǒng)在惡劣環(huán)境下能夠持續(xù)地提供導航定位．

1 系統(tǒng)整體框架及松組合濾波器設計

1.1 視覺里程計導航定位原理

相機屬于光學傳感器,其采集得到的圖像會產(chǎn)生失真的效果,需要經(jīng)過畸變校正后才能進行視覺前端處理,本文采用特征點法的視覺前端處理方法．提取圖像的ORB[12]特征點進行跟蹤,經(jīng)過剔除誤匹配點之后,利用匹配成功的特征點進行位姿R和t的估計．位姿估計中涉及到三個坐標系,其中像素坐標系原點位于圖像左上角,u軸和v軸方向分別向右和向下;相機坐標系原點位于相機光心,Z軸向前,X軸和Y軸方向分別與u軸和v軸平行;世界坐標系原點位于相機起始位置的光心,坐標系方向與相機坐標系起始時相同,在求得特征點在真實世界中的深度后,就可以確定3D點在世界坐標系下的坐標．像素坐標系下的點(u,v)和相機坐標系下的點(X,Y,Z)的關系以及世界坐標系下的點Pw到像素坐標的投影關系如式(1)～(2)所示,其中K為相機內(nèi)參矩陣,fx和fy是與相機焦距相關的參數(shù),可以通過相機標定得到．cx和cy為圖像中間位置的像素坐標,P為相機坐標系下的坐標,T為寫成齊次形式的變換矩陣．

(1)

(2)

同時在這個過程中還進行著地圖構建和回環(huán)檢測,地圖構建模塊不斷地插入關鍵幀,生成利用三角化產(chǎn)生的新的3D點并進行篩選,然后加入到局部地圖中,用于后續(xù)的特征點跟蹤和優(yōu)化估計部分．回環(huán)檢測模塊通過Bag of Words(BOW)算法,檢測是否為閉環(huán),對結果進行優(yōu)化,提高SLAM算法的精度和魯棒性．

1.2 視覺里程計和GNSS結果處理策略

在相機運動過程中,由于特征點太少或者特征點變化劇烈等原因,視覺里程計會產(chǎn)生位姿誤差,并隨著時間積累．GNSS常用的定位方法有偽距單點定位技術,精密單點定位技術和實時動態(tài)差分(RTK)定位技術．其中RTK能夠得到毫米級定位精度并且誤差不會隨著時間積累,但是在惡劣環(huán)境多路徑效應明顯時,精度會衰減到分米級甚至米級．式(3)為單頻RTK模型,

(3)

視覺里程計短時間內(nèi)具有較高的精度,基于GNSS和視覺里程計優(yōu)劣互補的特性,假設位置時間序列:

UG(t)=u(t)+EG(t),

(4)

UV(t)=u(t)+EV(t).

(5)

式中：UG(t)和UV(t)分別為RTK和視覺里程計得到的位置時間序列；u(t)為真實的位置時間序列；EG(t)和EV(t)分別為RTK和視覺里程計的位置結果誤差．由于RTK能夠達到厘米甚至毫米的精度,對于不要求特別高精度的情況下,省略掉EG(t)．通過將UG(t)和UV(t)作差,

Udif(t)=UV(t)-UG(t)

=EV(t)-EG(t)≈EV(t).

(6)

因為EG(t)≈0,在松組合系統(tǒng)中將差值Udif(t)作為確定EV(t)的觀測值,EV(t)能夠根據(jù)其時間序列進行插值或者進行外推,當GNSS信號受到遮擋或者中斷時,可以通過式(7)來估計實際的位置序列u(t)．

u(t)=UV(t)-EV(t).

(7)

1.3 松組合系統(tǒng)設計

設計一個松組合卡爾曼濾波器來組合GNSS和視覺里程計的位置速度結果,如圖1所示,流動站數(shù)據(jù)為載體上的接收機采集的GNSS觀測值,通過與附近已知的基準站的觀測值差分處理,使用RTK定位技術得到載體的位置速度信息．

圖1 松組合系統(tǒng)流程圖

視覺里程計自運動估計是以相機光心為原點,通過估計前后幀之間的相對位姿變化得到的整體位姿,在雙目相機中位姿估計原點位于雙目相機光心的連線中點．為了將視覺里程計和RTK的估計結果進行組合,將視覺里程計的定位結果通過迭代最近點(ICP)算法變換到和RTK同一東北天(ENU)坐標系下．

根據(jù)運動學模型來預測新的狀態(tài),設系統(tǒng)的運動誤差狀態(tài)向量為

(8)

式中：δrE、δrN和δrU分別表示東北天方向上的位置誤差；δVE、δVN和δVU分別表示東北天方向上的速度誤差.系統(tǒng)模型定義為

(9)

式中:G(t)為噪聲驅動陣,給定為6×6的單位矩陣;w(t)為過程激勵噪聲,其統(tǒng)計特性為

E[w(t)]=0,

(10)

E[w(t)wT(τ)]=Q(t)δ(t-τ).

(11)

式中：Q(t)為過程激勵噪聲的協(xié)方差矩陣,tk時刻離散值記為Qk；F(t)根據(jù)運動學模型表示為

(12)

推導得到k到k+1時刻的狀態(tài)轉移矩陣Φk+1,k為

(13)

式中,Δt為相機采集圖像的時間間隔.

觀測方程為

Zk=Hkxk+Dk.

(14)

通過將同一坐標系下的RTK結果和視覺里程計結果相減作為觀測值,其中Dk為觀測誤差,其統(tǒng)計特性滿足:

E[Dk]=0，

(15)

(16)

式中，Rk為觀測噪聲協(xié)方差矩陣,取為KITTI數(shù)據(jù)集中給出的RTK位置和速度精度．觀測值向量Z和觀測設計矩陣H分別為

(17)

(18)

式中：r和V分別表示位置和速度；上標G和V分別表示GNSS和視覺里程計的結果．

因此k-1到k時刻的觀測方程為

(19)

狀態(tài)一步預測方程為

(20)

一步預測后的狀態(tài)協(xié)方差矩陣:

(21)

卡爾曼濾波增益為

(22)

因此狀態(tài)估計方程為

(23)

式中,上標-和+表示量測更新前后,經(jīng)過量測更新后的狀態(tài)協(xié)方差矩陣為

(24)

2 模擬實驗

2.1 實驗平臺

KITTI數(shù)據(jù)集采集車的傳感器包括有兩對平行對齊并朝前方的灰點相機,可以分別采集灰度圖像和彩色圖像,灰度圖像的采集頻率為10 Hz,分辨率為1242像素×375像素．數(shù)據(jù)采集車在住宅區(qū)和高速公路等區(qū)域行駛,采集了多段原始數(shù)據(jù),我們采用較長和有代表性的四段數(shù)據(jù)進行模擬實驗．

2.2 實驗結果演示與分析

本文使用了上述采集車采集的兩段住宅區(qū)和兩段高速公路數(shù)據(jù)．為了說明實驗過程和效果,選定一段住宅區(qū)的數(shù)據(jù),其中采集車行駛了110 s大約1 233 m;為了展示組合系統(tǒng)在惡劣環(huán)境的持續(xù)導航性能,模擬了兩個GNSS信號中斷的實驗:1)在結尾處(80 s-110 s)GNSS信號中斷了30 s,利用卡爾曼濾波算法和組合系統(tǒng)進行持續(xù)導航；2)中途(30 s-60 s)信號中斷30 s后GNSS信號又恢復,利用卡爾曼濾波平滑算法和組合系統(tǒng)進行導航．本節(jié)的末尾還展示了其余三組數(shù)據(jù)視覺里程計定位結果誤差和模擬實驗結果．

將視覺里程計,松組合結果與RTK位置結果在采集車整個行駛路段上進行做差比較,統(tǒng)計其誤差特性．圖2示出了視覺里程計和RTK參考值的定位結果誤差,可以看到誤差隨著時間積累迅速,可能是汽車行駛速度太快,特征點變化劇烈導致視覺里程計的誤差較大．觀察松組合結果看到,開始幾秒收斂后,松組合結果和參考值相差在分米級．在模擬GNSS信號中斷后(假定在80 s-110 s共600多米路徑中無GNSS觀測值),利用松組合狀態(tài)方程繼續(xù)推導視覺里程計的誤差,然后利用視覺里程計的短時間定位結果來推導實際的位置．可以看出,組合系統(tǒng)不僅滿足持續(xù)導航,而且性能相比單視覺里程計結果得到顯著提升,U方向誤差從原來的8.5 m降低到0.2 m,E方向誤差從4.3 m降低到0.5 m,N方向誤差從9.5 m降低到0.5 m．其中N方向比E方向的視覺里程計定位誤差大,這是由于采集車朝著N方向行駛了約500 m,而只朝著E方向上行駛約49 m,N方向相比E方向速度快,且N方向上的特征點變化迅速且明顯,在左右圖像、前后幀特征點匹配時,N方向上就會多發(fā)生誤匹配而造成N方向上誤差明顯大于N方向．中途30 s-60 s GNSS信號中斷時,考慮到中斷兩端的GNSS信號是完好的,使用卡爾曼濾波平滑算法,從左側向右側計算后再從右側平滑到左側,經(jīng)過平滑后,誤差呈現(xiàn)出兩頭小中間大的趨勢,并且在30 s大約358 m的路徑中E方向誤差最大約2.7 m,N方向誤差最大約2.0 m,U方向誤差最大約3.2 m,顯著優(yōu)于單視覺里程計結果．

(a) 視覺里程計定位 (b)GNSS信號不中斷時松組合

(c)80 s-110 s GNSS信號中斷時松組合 (d)30 s-60 s GNSS信號中斷時松組合

圖3示出了其余三組的實驗結果,其中第一組為住宅區(qū)的結果,后兩組為高速公路的視覺里程計定位誤差和模擬GNSS中斷時的持續(xù)導航結果．總體比較可以看出,組合系統(tǒng)在遮擋條件下可以保持較高精度的持續(xù)導航,同時可以看到除U方向上的誤差較大外,E和N方向上的持續(xù)定位能力在米級精度．此外,與住宅區(qū)相比,高速道路的特征點較少,且大多數(shù)是樹木及防護欄,容易發(fā)生誤匹配造成較大定位誤差,并且在高速道路上汽車行駛速度較快,特征點變化劇烈,也會造成視覺里程計較大定位誤差,而組合系統(tǒng)剛好可以相互補充,實現(xiàn)高精度的持續(xù)導航．

圖3 視覺里程計定位誤差(頂端)和GNSS中斷時組合導航的定位誤差(中部、底端)

3 結 語

本文使用KITTI采集車采集的4段校正后的圖片和RTK結果數(shù)據(jù),通過ORB-SLAM算法計算了視覺里程計的定位結果,并和RTK結果進行比較說明了組合系統(tǒng)的持續(xù)導航定位能力．還利用視覺里程計和RTK結果進行松組合估計了視覺里程計隨著時間的誤差變化,并模擬了GNSS信號中斷時,利用狀態(tài)方程外推視覺里程計誤差,然后利用視覺里程計的定位結果估計實際的定位結果．實驗表明,視覺里程計的定位結果誤差較大,GNSS和視覺里程計的組合系統(tǒng)能夠很好地處理惡劣環(huán)境下GNSS信號遮擋或中斷的問題,但是長時間的導航定位GNSS系統(tǒng)還是必需的．本文的松組合系統(tǒng)使用了GNSS的RTK結果,但是GNSS得到的常常是單點定位結果,精度只能達到米級且惡劣環(huán)境下會有幾十米誤差,此時視覺里程計能短時間很好地改善GNSS的定位結果,在載體周圍特征點明顯和速度不快時能較好地實現(xiàn)持續(xù)定位功能．將來,考慮使用GNSS原始觀測值,組建一個緊組合系統(tǒng)利用視覺里程計結果改善單點定位的定位精度然后實現(xiàn)惡劣環(huán)境下的持續(xù)定位．

致謝:感謝卡爾斯魯厄理工學院開源的KITTI數(shù)據(jù)．