異步聯邦UKF的GNSS/SINS/攝影定位組合導航算法仿真

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

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

0 引 言

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

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

1 系統方案設計

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

1.1 GNSS/SINS導航子系統

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

(1)

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

SINS/GNSS松組合采用位置、速度組合方式,兩個系統的位置速度之差作為觀測量.可得到當地地理坐標系下位置速度松組合的量測方程為:

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

(2)

式中:rINS,rGNSS分別為慣導系統和GNSS系統解算的位置量;vINS,vGNSS分別指慣導系統和GNSS系統解算的速度量;H(t)為12×12為狀態轉移矩陣.由文獻[5]可直接得出當地地理坐標系下位置速度松組合的量測方程為

(3)

1.2 SINS/Photogrammetry導航子系統

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

選取的狀態量為

X(t)=

(4)

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

因此系統的狀態方程為

(5)

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

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

(6)

其中，

(7)

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

2 非等間隔聯邦濾波算法

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

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

(8)

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

考慮上文組合系統的模型,改進的非等間隔聯合UKF濾波算法為

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

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

(9)

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

(10)

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

(11)

(12)

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

(13)

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

(14)

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

(15)

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

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

(16)

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

(17)

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

(18)

3 數字仿真結果及分析

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

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

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

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

4 結束語

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