馮祎,涂銳,3,韓軍強(qiáng),侯福榮,洪菊,劉金海,王星星

(1.中國(guó)科學(xué)院國(guó)家授時(shí)中心,陜西 西安 710600;2. 中國(guó)科學(xué)院大學(xué),北京 100049;3. 中國(guó)科學(xué)院精密導(dǎo)航定位與定時(shí)技術(shù)重點(diǎn)實(shí)驗(yàn)室,陜西 西安 710600)

0 引 言

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

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

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

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

1.1 視覺里程計(jì)導(dǎo)航定位原理

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

(1)

(2)

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

1.2 視覺里程計(jì)和GNSS結(jié)果處理策略

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

(3)

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

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

(4)

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

(5)

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

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

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

(6)

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

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

(7)

1.3 松組合系統(tǒng)設(shè)計(jì)

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

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

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

根據(jù)運(yùn)動(dòng)學(xué)模型來預(yù)測(cè)新的狀態(tài),設(shè)系統(tǒng)的運(yùn)動(dòng)誤差狀態(tài)向量為

(8)

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

(9)

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

E[w(t)]=0,

(10)

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

(11)

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

(12)

推導(dǎo)得到k到k+1時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣Φk+1,k為

(13)

式中,Δt為相機(jī)采集圖像的時(shí)間間隔.

觀測(cè)方程為

Zk=Hkxk+Dk.

(14)

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

E[Dk]=0，

(15)

(16)

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

(17)

(18)

式中：r和V分別表示位置和速度；上標(biāo)G和V分別表示GNSS和視覺里程計(jì)的結(jié)果．

因此k-1到k時(shí)刻的觀測(cè)方程為

(19)

狀態(tài)一步預(yù)測(cè)方程為

(20)

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

(21)

卡爾曼濾波增益為

(22)

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

(23)

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

(24)

2 模擬實(shí)驗(yàn)

2.1 實(shí)驗(yàn)平臺(tái)

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

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

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

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

(a) 視覺里程計(jì)定位 (b)GNSS信號(hào)不中斷時(shí)松組合

(c)80 s-110 s GNSS信號(hào)中斷時(shí)松組合 (d)30 s-60 s GNSS信號(hào)中斷時(shí)松組合

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

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

3 結(jié) 語

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

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