周朋進, 呂志偉, 叢佃偉, 賈錚洋

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

0 引 言

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

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

1 系統(tǒng)方案設計

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

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

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

(1)

式中:TB為接收機鐘差等效距離;TD指接收機的等效速度;εTB和εTD指對應的隨機噪聲項.

SINS/GNSS松組合采用位置、速度組合方式,兩個系統(tǒng)的位置速度之差作為觀測量.可得到當?shù)氐乩碜鴺讼迪挛恢盟俣人山M合的量測方程為:

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

(2)

式中:rINS,rGNSS分別為慣導系統(tǒng)和GNSS系統(tǒng)解算的位置量;vINS,vGNSS分別指慣導系統(tǒng)和GNSS系統(tǒng)解算的速度量;H(t)為12×12為狀態(tài)轉(zhuǎn)移矩陣.由文獻[5]可直接得出當?shù)氐乩碜鴺讼迪挛恢盟俣人山M合的量測方程為

(3)

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

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

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

X(t)=

(4)

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

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

(5)

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

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

(6)

其中，

(7)

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

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

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

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

(8)

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

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

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

當k≠ki時:主濾波器根據(jù)選取的Sigma點根據(jù)式(9)首先進行時間更新:

(9)

當k=ki時:首先根據(jù)選取的Sigma點首先進行時間更新:

(10)

再進行主濾波器量測更新:

(11)

(12)

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

(13)

2) 當對于k=LCM(k1,k2，…，kn)時,此時對于主濾波器信息分配,有

(14)

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

(15)

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

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

(16)

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

(17)

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

(18)

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

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

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

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

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

4 結束語

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