異步聯(lián)邦UKF的GNSS/SINS/攝影定位組合導(dǎo)航算法仿真

周朋進(jìn), 呂志偉, 叢佃偉, 賈錚洋

(信息工程大學(xué) 地理空間信息學(xué)院,河南 鄭州 450001)

0 引 言

飛行器的進(jìn)近過(guò)程是在飛行的各個(gè)階段中對(duì)導(dǎo)航性能要求最高的,系統(tǒng)不僅對(duì)位姿精度要求較高,對(duì)導(dǎo)航系統(tǒng)的容錯(cuò)性以及魯棒性能也有一定的要求.近年來(lái)基于差分衛(wèi)星導(dǎo)航技術(shù)的著陸系統(tǒng)研究有很大的進(jìn)步,但存在衛(wèi)星導(dǎo)航系統(tǒng)信號(hào)易受遮擋和抗干擾性差這一嚴(yán)重缺點(diǎn).在多傳感器組合導(dǎo)航用于載體位姿估計(jì)中,目前研究比較多的有基于全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)和捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)的GNSS/SINS組合導(dǎo)航、GNSS/SINS/視覺組合導(dǎo)航等.文獻(xiàn)[1]提出基于局域差分GNSS和無(wú)線電高度計(jì)(RA)的組合導(dǎo)航系統(tǒng),利用卡爾曼濾波進(jìn)行信息融合,仿真結(jié)果顯示該方案滿足著陸對(duì)導(dǎo)航系統(tǒng)精度要求.但GNSS信號(hào)易受干擾,因此在實(shí)際應(yīng)用中存在定位可靠性不足的問題.文獻(xiàn)[2]提出基于天文導(dǎo)航(CNS)的SINS/CNS緊組合方式,通過(guò)北斗修正提高SINS/CNS的定位精度,可保證SINS/CNS組合系統(tǒng)較長(zhǎng)時(shí)間內(nèi)的導(dǎo)航精度.但天文導(dǎo)航存在星光過(guò)弱或視場(chǎng)內(nèi)星數(shù)過(guò)少情況.攝影測(cè)量采用人工標(biāo)志點(diǎn),目標(biāo)的坐標(biāo)已知,在這種情況下,攝影測(cè)量系統(tǒng)無(wú)需狀態(tài)量,而成為純粹提供觀測(cè)量的導(dǎo)航方式,因此可考慮將攝影定位取代CNS系統(tǒng)進(jìn)行三系統(tǒng)組合用于進(jìn)近著陸.文獻(xiàn)[3]提出用攝影測(cè)量代替GNSS與慣導(dǎo)進(jìn)行組合來(lái)克服慣導(dǎo)系統(tǒng)的誤差發(fā)散問題,仿真實(shí)驗(yàn)表明,在飛行器進(jìn)近著陸階段通過(guò)攝影測(cè)量與慣導(dǎo)系統(tǒng)組合可達(dá)到GNSS/SINS組合系統(tǒng)的位姿精度.然而要實(shí)現(xiàn)一個(gè)高性能的GNSS/SINS/Photogrammetry組合導(dǎo)航系統(tǒng),需考慮信息融合方面的諸多實(shí)際問題,包括組合導(dǎo)航系統(tǒng)中的數(shù)據(jù)多速率、系統(tǒng)的計(jì)算實(shí)行性以及系統(tǒng)信息的融合算法等.在非線性系統(tǒng)中無(wú)跡卡爾曼濾波(UKF)具有很好的濾波效果,但標(biāo)準(zhǔn)UKF存在系統(tǒng)模型的先驗(yàn)信息不確定或噪聲變化過(guò)大時(shí)濾波發(fā)散問題.

本文提出了一種基于UKF的多傳感器信息融合算法,針對(duì)多傳感器數(shù)據(jù)異步問題,采用時(shí)間與量測(cè)更新分離的異步非等間隔聯(lián)邦濾波算法并行處理數(shù)據(jù).該方法在聯(lián)邦濾波器中利用SINS分別同GNSS與Photogrammetry構(gòu)成子濾波器進(jìn)行多傳感器信息融合,保證載體高動(dòng)態(tài)下的定位精度和可靠性,有效避免了因衛(wèi)星導(dǎo)航定位失效而導(dǎo)致系統(tǒng)定位失敗,提高了系統(tǒng)魯棒性.

1 系統(tǒng)方案設(shè)計(jì)

在GNSS/SINS/Photogrammetry的組合系統(tǒng)中,由SINS解算得到載體的姿態(tài)、速度和位置等導(dǎo)航參數(shù),GNSS得到載體的位置和速度參數(shù)信息,攝影測(cè)量獲得位置、姿態(tài)角信息.以SINS為導(dǎo)航主系統(tǒng),在同一坐標(biāo)系下,分別設(shè)計(jì)SINS/GNSS、SINS/Photogrammetry組合子濾波器,經(jīng)濾波估計(jì)獲取狀態(tài)量的兩個(gè)最優(yōu)局部狀態(tài)估計(jì)值.然后采用聯(lián)邦濾波和數(shù)據(jù)融合方法,在組合導(dǎo)航的主濾波器中,把兩個(gè)子系統(tǒng)的輸出進(jìn)行全局信息融合來(lái)獲得狀態(tài)的最優(yōu)全局估計(jì)值.最后,用獲得的系統(tǒng)狀態(tài)最優(yōu)全局估計(jì)值對(duì)SINS進(jìn)行濾波校正.本文在UKF濾波算法的基礎(chǔ)上,將其應(yīng)用在聯(lián)邦濾波器上,系統(tǒng)分為子濾波器與主濾波器兩層結(jié)構(gòu).一般認(rèn)為SINS不會(huì)出現(xiàn)系統(tǒng)故障,若檢測(cè)到GNSS或Photogrammetry系統(tǒng)故障就立即隔離其子系統(tǒng).兩個(gè)子濾波器間沒有干擾獨(dú)立工作,避免了因某個(gè)子系統(tǒng)故障而污染整個(gè)導(dǎo)航系統(tǒng),可有效提升導(dǎo)航系統(tǒng)的魯棒性.其原理如圖1所示.

1.1 GNSS/SINS導(dǎo)航子系統(tǒng)

因?yàn)閼T導(dǎo)系統(tǒng)的狀態(tài)方程中已經(jīng)含有位置,速度等參數(shù),因此僅需要在這里擴(kuò)展GNSS接收機(jī)的鐘速和鐘差項(xiàng)作為卡爾曼濾波的待估計(jì)狀態(tài)參數(shù).鐘差和鐘速的建模[4]可表示為

(1)

式中:TB為接收機(jī)鐘差等效距離;TD指接收機(jī)的等效速度;εTB和εTD指對(duì)應(yīng)的隨機(jī)噪聲項(xiàng).

SINS/GNSS松組合采用位置、速度組合方式,兩個(gè)系統(tǒng)的位置速度之差作為觀測(cè)量.可得到當(dāng)?shù)氐乩碜鴺?biāo)系下位置速度松組合的量測(cè)方程為:

=H(t)X(t)+V(t).

(2)

式中:rINS,rGNSS分別為慣導(dǎo)系統(tǒng)和GNSS系統(tǒng)解算的位置量;vINS,vGNSS分別指慣導(dǎo)系統(tǒng)和GNSS系統(tǒng)解算的速度量;H(t)為12×12為狀態(tài)轉(zhuǎn)移矩陣.由文獻(xiàn)[5]可直接得出當(dāng)?shù)氐乩碜鴺?biāo)系下位置速度松組合的量測(cè)方程為

(3)

1.2 SINS/Photogrammetry導(dǎo)航子系統(tǒng)

根據(jù)攝影測(cè)量原理,高分辨率相機(jī)能夠通過(guò)拍攝有地面標(biāo)志的標(biāo)志場(chǎng)來(lái)給出拍攝時(shí)刻的高精度飛行器位置和姿態(tài)信息.利用攝影測(cè)量信息來(lái)估計(jì)和修正慣性系統(tǒng)誤差,從而可以使修正后的慣性導(dǎo)航系統(tǒng)保持高位置精度、高姿態(tài)精度、高數(shù)據(jù)更新率、高測(cè)速信息.攝影定位和慣導(dǎo)系統(tǒng)的組合可以采用攝影解算出的相對(duì)位置和姿態(tài)角進(jìn)行組合,慣性導(dǎo)航系統(tǒng)連續(xù)提供攝像機(jī)自身姿態(tài),慣性系統(tǒng)和攝像機(jī)分別計(jì)算得載體位置之差作為濾波觀測(cè)值,經(jīng)過(guò)輸出校正的卡爾曼濾波方法修正慣性系統(tǒng)的導(dǎo)航誤差.

選取的狀態(tài)量為

X(t)=

(4)

式中:φ為姿態(tài)角誤差;δv為速度誤差;δL、δλ、δh分別為緯度、經(jīng)度和高度誤差,下標(biāo)e、n、u表示東、北、天方向,式中其余變量的意義見式(1).

因此系統(tǒng)的狀態(tài)方程為

(5)

將慣導(dǎo)系統(tǒng)與攝影定位的位置和姿態(tài)的差值作為量測(cè)值輸入給濾波器.系統(tǒng)的量測(cè)方程為

H(t)X(t)+V(t),

(6)

其中，

(7)

式中:γ、θ、ψ分別為載體的橫滾、俯仰、航向角.

2 非等間隔聯(lián)邦濾波算法

在多系統(tǒng)組合的聯(lián)邦濾波器中,一般子濾波器和主濾波器的濾波更新周期都是固定不變的.但是由于各種不同類型傳感器的數(shù)據(jù)更新率相差較大,會(huì)造成各子濾波器的量測(cè)周期不同,因此導(dǎo)致主濾波器接收到的局部濾波估計(jì)并不同步;在工程應(yīng)用中為了減輕系統(tǒng)之間計(jì)算的通信負(fù)擔(dān),主濾波器也并不是對(duì)每一個(gè)濾波周期都會(huì)進(jìn)行信息融合,這也是造成濾波不等間隔問題的一個(gè)原因.雖然可以通過(guò)采用數(shù)學(xué)手段外推和內(nèi)差的方法將量測(cè)周期不同的采樣數(shù)據(jù)轉(zhuǎn)換成相等量測(cè)周期,但這會(huì)增加很多不必要的計(jì)算負(fù)擔(dān),因此可以采用時(shí)間與量測(cè)更新分離的異步非等間隔聯(lián)邦濾波算法.文獻(xiàn)[9]針對(duì)多種傳感器的數(shù)據(jù)輸出不同步的問題,設(shè)計(jì)了量測(cè)與時(shí)間更新分離的異步非等間隔濾波算法,經(jīng)仿真驗(yàn)證該算法可以有效地實(shí)現(xiàn)對(duì)SINS/GNSS/CNS數(shù)據(jù)的異步融合,與常規(guī)的濾波方法相比較,異步濾波組合系統(tǒng)的精度有明顯提高,具有重要的工程實(shí)用價(jià)值.標(biāo)準(zhǔn)的聯(lián)邦UKF濾波算法流程如圖2所示.

在本研究方案中,由于兩個(gè)子濾波器所采用的狀態(tài)量相同,因此可對(duì)聯(lián)邦濾波器進(jìn)行適當(dāng)?shù)母倪M(jìn)以減少濾波算法的計(jì)算復(fù)雜度.首先在主濾波器中執(zhí)行誤差向量及協(xié)方差陣的時(shí)間更新和信息融合,并將更新后的信息按照一定的信息分配原則分配給兩個(gè)子濾波器;子濾波器此時(shí)根據(jù)傳遞的預(yù)測(cè)值進(jìn)行量測(cè)更新.通常情況下,子濾波器的計(jì)算周期就是各導(dǎo)航傳感器的量測(cè)周期.假設(shè)第i個(gè)子系統(tǒng)的周期為Ti(i=1,2，…，N).首先,令主濾波器的信息融合周期Tf和計(jì)算周期Tc分別為[10]:

(8)

式中: LCM(·)和GCD(·)分別為求取最小公倍數(shù)和最大公約數(shù).因此有Ti=kiTN(ki為互質(zhì)的自然數(shù)).

考慮上文組合系統(tǒng)的模型,改進(jìn)的非等間隔聯(lián)合UKF濾波算法為

1) 對(duì)于k≠LCM(k1,k2，…，kn)時(shí)的主濾波器,當(dāng)k≠ki的時(shí)候,此時(shí)子傳感器沒有新的量測(cè)信息,主濾波器只進(jìn)行時(shí)間更新;當(dāng)k=ki,即T=kiTc時(shí),這時(shí)有新的量測(cè)信息輸入,則主濾波器會(huì)同時(shí)進(jìn)行時(shí)間更新和量測(cè)更新.

當(dāng)k≠ki時(shí):主濾波器根據(jù)選取的Sigma點(diǎn)根據(jù)式(9)首先進(jìn)行時(shí)間更新:

(9)

當(dāng)k=ki時(shí):首先根據(jù)選取的Sigma點(diǎn)首先進(jìn)行時(shí)間更新:

(10)

再進(jìn)行主濾波器量測(cè)更新:

(11)

(12)

Pf(k,k)=[I-Kf(k)Hf(k)]Pf(k,k-1),

(13)

2) 當(dāng)對(duì)于k=LCM(k1,k2，…，kn)時(shí),此時(shí)對(duì)于主濾波器信息分配,有

(14)

此時(shí)的子濾波器按照上式分配原則得到的狀態(tài)預(yù)測(cè)值和協(xié)方差預(yù)測(cè)值進(jìn)行濾波算法的量測(cè)更新,對(duì)于主濾波器有如下融合策略:

(15)

在沒有攝影和GNSS觀測(cè)量的情況下,濾波器僅進(jìn)行時(shí)間更新不進(jìn)行量測(cè)方程的更新,即濾波器在t1、t2、t4時(shí)刻只進(jìn)行時(shí)間更新(如圖3).在t3時(shí)刻僅有GNSS系統(tǒng)提供觀測(cè)量時(shí),此時(shí)調(diào)整組合系統(tǒng)觀測(cè)模型,進(jìn)行狀態(tài)更新和量測(cè)更新,選擇INS/GNSS組合的觀測(cè)矩陣和向量進(jìn)行濾波[11].在t5時(shí)刻僅有攝影觀測(cè)量,則選擇INS/Photogrammetry組合子系統(tǒng)的觀測(cè)模型進(jìn)行狀態(tài)和量測(cè)更新.

針對(duì)聯(lián)邦濾波器中分配信息因子的問題,仍是國(guó)內(nèi)外研究的重要方向,還沒有形成定論[12].在實(shí)際濾波過(guò)程中,由于各個(gè)子系統(tǒng)的數(shù)據(jù)采樣率不同,一般情況下融合周期都會(huì)大于子濾波的周期,主濾波器需經(jīng)過(guò)幾次局部濾波后才進(jìn)行一次融合.此時(shí),信息守恒原則經(jīng)過(guò)多次迭代濾波會(huì)被破壞,導(dǎo)致全局濾波變?yōu)榇蝺?yōu).因此信息分配系數(shù)的不同會(huì)影響到聯(lián)邦濾波器中全局解的精度.矩陣的范數(shù)具有與絕對(duì)值類似的性質(zhì),是對(duì)矩陣大小的一種數(shù)量描述,因此可采用基于估計(jì)均方誤差矩陣范數(shù)法,使系統(tǒng)具有更好的自適應(yīng)能力.局部濾波器和主濾波器的信息分配系數(shù)分別由式(16)動(dòng)態(tài)確定,即

(16)

式中:Pi(k-1)為各個(gè)子濾波器狀態(tài)量估計(jì)誤差方差矩陣;‖·‖F(xiàn)為Frobenius范數(shù).對(duì)于任意矩陣A有

(17)

將信息反饋到各局部UKF子濾波器中策略如下式:

(18)

3 數(shù)字仿真結(jié)果及分析

利用上述方法進(jìn)行仿真實(shí)驗(yàn),包括飛行器軌跡生成、各組合系統(tǒng)解算等模塊,導(dǎo)航坐標(biāo)系采用地理坐標(biāo)系.飛行狀態(tài)包括起飛、平飛、加減速、轉(zhuǎn)彎、下降等狀態(tài),各參數(shù)及初始化狀態(tài)變量如下：飛行器的初始位置為東經(jīng)113.533°,北緯34.816°,初始速度0 m/s,航向角90°,俯仰角和橫滾角均為0°;采用中等精度的航空SINS,陀螺隨機(jī)常值漂移為0.1 (°)/h,隨機(jī)漂移為0.05 (°)/h,加速度計(jì)常值偏置為100 ug,隨機(jī)偏置為50 ug,GNSS接收機(jī)10 m的定位誤差,數(shù)據(jù)更新率10 Hz;將仿真的軌跡作為真值,在攝影節(jié)點(diǎn)處添加標(biāo)準(zhǔn)差0.5 m的隨機(jī)誤差作為攝影定位結(jié)果,攝影系統(tǒng)的姿態(tài)精度0.2°,數(shù)據(jù)更新率5 Hz,仿真時(shí)間3 600 s.本文分別針對(duì)SINS/GNSS、SINS/GNSS/Photogrammetry兩種組合系統(tǒng)進(jìn)行了數(shù)字仿真驗(yàn)證,飛行軌跡及濾波結(jié)果如圖4～11所示.

圖4示出了飛行器進(jìn)近過(guò)程中參考飛行軌跡,圖5至圖11為標(biāo)準(zhǔn)的聯(lián)邦卡爾曼濾波算法(FKF)和本文提出改進(jìn)的UKF-FKF算法解算的姿態(tài)、位置和速度誤差曲線.可以看出本文提出的算法得到的載體位姿精度在飛行器高動(dòng)態(tài)運(yùn)動(dòng)下依然具有較高的精度,可以將三個(gè)傳感器的信息進(jìn)行高效的融合.載體在高動(dòng)態(tài)下的組合導(dǎo)航系統(tǒng)具有較強(qiáng)的非線性,且過(guò)程噪聲和量測(cè)噪聲均會(huì)發(fā)生較大的變化,而提出的非等間隔聯(lián)邦UKF算法采用時(shí)間與量測(cè)更新分離的異步非等間隔算法,并通過(guò)UT變換來(lái)對(duì)非線性系統(tǒng)狀態(tài)的后驗(yàn)概率密度函數(shù)進(jìn)行近似,以此避免狀態(tài)方程線性化造成的截?cái)嗾`差,因此得到的狀態(tài)估計(jì)具有更高的精度.由仿真結(jié)果可以得出,當(dāng)載體加減速或轉(zhuǎn)彎造成狀態(tài)模型不準(zhǔn)確時(shí),傳統(tǒng)濾波算法對(duì)組合導(dǎo)航狀態(tài)估計(jì)精度迅速降低,而本文改進(jìn)的UKF-FKF算法可以有效地抵御高動(dòng)態(tài)下系統(tǒng)模型不匹配或過(guò)程噪聲過(guò)大問題.

表1 FKF和UKF-FKF算法位姿估計(jì)均方根誤差(RMSE)

表1給出了兩種方法在300～3 600 s內(nèi)載體的姿態(tài)和位置誤差的RMSE均值,由表1的統(tǒng)計(jì)結(jié)果可得出相比于標(biāo)準(zhǔn)的FKF,本文提出的方法對(duì)于高動(dòng)態(tài)下載體導(dǎo)航誤差的估計(jì)有一定優(yōu)勢(shì),特別是在高動(dòng)態(tài)時(shí)載體姿態(tài)的估計(jì)精度提升明顯.通過(guò)仿真結(jié)果分析,驗(yàn)證了基于GNSS/SINS/Photogrammetry的多系統(tǒng)組合導(dǎo)航異步聯(lián)邦UKF濾波算法的有效性和可用性,提出的異步多系統(tǒng)融合方法可以有效克服濾波的不等間隔問題和載體高動(dòng)態(tài)造成的系統(tǒng)狀態(tài)方程非線性和模型估計(jì)不準(zhǔn)確的問題.

4 結(jié)束語(yǔ)

本文提出的基于GNSS/SINS/Photogrammetry的組合導(dǎo)航異步聯(lián)邦UKF濾波算法可以把三種傳感器的優(yōu)點(diǎn)充分利用,取長(zhǎng)補(bǔ)短以提升系統(tǒng)的容錯(cuò)性和可用性.仿真結(jié)果表明,在位置、姿態(tài)和速度精度以及系統(tǒng)可用性方面,本文提出的基于GNSS/SINS/Photogrammetry的異步聯(lián)邦UKF濾波算法較標(biāo)準(zhǔn)FKF組合導(dǎo)航系統(tǒng)有明顯的提升,尤其在高動(dòng)態(tài)下對(duì)姿態(tài)的估計(jì)精度更是提升了一個(gè)數(shù)量級(jí).為多系統(tǒng)組合及數(shù)據(jù)融合工作提供了一定的理論基礎(chǔ),因此算法也具有較高的工程實(shí)用價(jià)值.