改進的自適應濾波算法在BDS/INS組合導航中的應用

強明輝,蔣文

(1.蘭州理工大學 電氣工程與信息工程學院,甘肅 蘭州 730050; 2.甘肅省工業過程先進控制重點實驗室,甘肅 蘭州 730050; 3.蘭州理工大學 電氣與控制工程國家級實驗教學示范中心,甘肅 蘭州 730050)

0 引 言

組合導航系統有利于充分利用各導航系統的優點進行信息互補與信息融合,成為導航系統發展的方向。組合導航系統是利用兩種或兩種以上的導航設備提供多重信息,構成一個多功能、高精度的冗余系統。慣性導航系統(INS)具有自主性、隱蔽性、強干擾性、高機動和完整的導航信息等優點,但是導航誤差隨時間積累而增長,這使其應用受到了一定的限制。全球定位系統(GPS)是一種全球、全天候、高精度的定位系統,誤差不隨時間積累,但易受干擾、數據更新率低[1],且GPS受美國國防部直接控制,使用受制于人。北斗導航系統是我國正在建設和運行且擁有絕對自主權的全球衛星導航系統的重要組成部分。因此,將北斗系統(BDS)與INS結合起來進行組合導航,已成為導航系統發展的必然趨勢和研究熱點,對我國國防建設具有極大的現實意義。

為了提高組合導航的精度,研究人員開始探索不同的算法進行線性化導航的數學模型。基于組合導航的估計算法已經被廣泛研究。近幾年,陳美華等利用UKF算法進行了MEMS/BD組合導航數據融合研究[2],提高了組合導航系統的精度,使系統具有更好的魯棒性。為了將EKF應用于非平滑系統,Chatzis提出了不連續性擴展卡爾曼濾波器[3]。

雖然上述方法在一定程度上取得了較好的效果,但仍有一些問題有待解決,如1) BDS的噪聲成分復雜,不僅包括白噪聲,還包括周圍建筑物引起的其他隨機環境噪聲和電磁干擾;2) INS模型和噪聲不確定性;上述問題給信息融合估計帶來了一些困難,目前還沒有得到很好地解決。本文采用松散組合方式進行研究,以位置、速度作為觀測量進行仿真分析。在卡爾曼濾波中引入系統的量測噪聲協方差(R)的計算,并對其指數加權,有效地抑制濾波發散。引入衰減記憶因子可以達到較小的當前信號偏差值,增加新觀測數據的影響是克服濾波偏差的有效方法,減少了過去收集的數據對濾波的影響。此算法不僅可以避免濾波散度,而且可以有效地減弱環境噪聲對組合導航系統的影響,提高導航精度。

1 BDS/INS組合導航系統的誤差狀態方程

1.1 誤差方程

由于位置、速度組合反饋校正模式的組合算法在較長時間內工作時,濾波精度高,組合效果良好,故而采用該組合方案[4]。組合導航系統采用卡爾曼濾波進行設計,首先需要對組合導航系統建模,確定需要估計的狀態,建立狀態方程與量測方程。本文采用北斗系統的位置和速度誤差作為誤差狀態,其中位置誤差δLBE，δLBN，δLBU,速度誤差δVBE，δVBN，δVBU.其狀態方程為

(1)

式中:XB為n維狀態向量;FB為北斗系統的(n×n)階動態矩陣;GB=I為單位矩陣;WB為n維系統噪聲矩陣。采用位置,速度組合方式,選擇位置與速度作為觀測量,則BDS/INS組合導航系統誤差狀態方程為

(2)

式中:X(t)為系統狀態向量;W(t)為系統噪聲向量;F(t)為系統矩陣;G(t)為系統噪聲矩。其中[5]:

X(t)= [δφEδφNδφUδVEδVNδVUδLδλδh

εbxεbyεbzεrxεryεrzxyz]T,

(3)

W(t)= [ωgxωgyωgz,ωbxωbyωbz,ωaxωayωaz]T,

(4)

(5)

(6)

式中: E、N、U分別表示東、北、天方向;δφEδφNδφU為姿態角誤差;δVE、δVN、δVU為速度誤差;δL、δλ、δh為緯度、經度、高度誤差;εbx、εby、εbz為陀螺儀的隨機常數漂移;εrx、εry、εrz為陀螺儀一階馬爾科夫漂移;x、y、z為加速度一階馬爾科夫漂移;為姿態矩陣;矩陣的選取參見文獻[6]。

1.2 BDS/INS組合導航系統量測方程

BDS與INS以位置、速度的組合方式進行組合導航必然出現兩組量測值:位置量測差值,即北斗接收機給出的位置信息與INS系統輸出的經度、緯度與高度信息的差值;另外是速度量測的差值,即這兩個系統給出的相應速度信息的差值[7]。

在東北天坐標系下INS的輸出位置信息用其真值與相應誤差之和表示為:

(7)

在東北天坐標系下北斗接收機的輸出位置信息用其真值與相應誤差之差表示為

(8)

式中：Lt,λt,ht為真實位置;NE,NN,NU分別為北斗接收機沿地理坐標系各個方向的位置誤差;RM為量測點沿北方向的曲率半徑;RN為量測點沿東西方向的曲率半徑;L為量測點的緯度。則位置量測誤差矢量為:

=HP(t)X(t)+VP(t) ，

(9)

其中:Hp=[03×6diag[RMRNcosL1] 03×9];VP=[NNNENU]T為濾波器噪聲; 其中RM=Re(1-2f+3fsin 2L),RN=Re(1+2fsin2L),f=1/289.257,Re=6378137m.

在東北天坐標系下INS的速度量測信息用其真值與相應速度誤差之和表示為

(10)

在東北天坐標系下北斗導航系統速度量測信息用其真值與相應速度誤差之差表示為

(11)

其中:VN,VE,VU是飛機沿東北天地理坐標系各個坐標軸的真實速度;MN,ME,MU為北斗接收機在東北天坐標系中的各個坐標軸上的測速誤差分量。則速度測量矢量定義為:

=HV(t)X(t)+VV(t),

(12)

式中,HV=[03×6diag[1 1 1] 03×9].

由以上推導可知,將位置量測方程與速度量測方程合并,即可得到BDS/INS組合導航系統的量測方程,如下式所示:

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

(13)

2 組合導航算法

2.1 衰減自適應記憶濾波

考慮如下離散非線性系統:

Xk=Φk|(k-1)Xk-1+Γk|((k-1)ωk-1,

(14)

Zk=HkXk+νk,

(15)

式中:Xk∈Rm和uk-1∈Rc分別為系統的狀態向量和控制向量;Zk∈Rn為量測向量;Hk∈Rp×n為量測矩陣;ω(k)～N(0Qk)為高斯過程噪聲;νk為量測噪聲,對其統計特性不做假設。傳統常規卡爾曼非線性濾波方法可以總結為[8]

(16)

(17)

(18)

(19)

Pk=(I-KkHk)Pk|(k-1),

(20)

(21)

式中,λk≥1為時變衰減因子。λk的更新公式為[8]

(22)

2.2 噪聲加權自適應卡爾曼濾波算法

定義量測信息:

rk

(23)

最佳濾波在不同的時刻余差互不相關,可以得到誤差協方差矩陣:

Crk

(24)

根據歷史誤差的記錄,可以得到Crk的估計為

(25)

上式用到全部的信息,會增加算法的計算量,會使算法的實時性有所下降,故引入濾波窗口。

(26)

這樣能較好地抑制歷史數據對狀態的影響。將上式(24)帶入卡爾曼增益可得新的卡爾曼增益:

(27)

引入新息后根據傳統卡爾曼濾波算法可以得到噪聲矩陣的更新方程[9]:

(28)

(29)

其中,Δxk=Kkrk.將式(24)帶入式(27)可以得到自適應卡爾曼濾波增益的計算公式為

(30)

要想進一步得到當前時刻最優的估計,需得出卡爾曼增益(K)與統計噪聲(R)之間的關系,那么必須知道k時刻的后驗估計協方差與k時刻的先驗估計協方差。

k時刻的后驗估計協方差表達式為

(31)

k時刻的先驗估計協方差表達式為

(32)

把k時刻的后驗估計協方差矩陣的跡記為T[Pk]:

(33)

確定卡爾曼增益的表達式:

(34)

(35)

(36)

其中,Ci(i=1,2,…)是一個適當的整數。這種形式的加權可以減弱以前的測量值的影響。

2.3 自適應組合濾波算法

針對衰減自適應記憶濾波和噪聲加權自適應濾波的特點,可對這2種算法進行組合,實現優勢互補,從而滿足實際需要。首先對系統噪聲方差Qk和濾波的量測噪聲方差Rk進行在線估計,然后應用衰減自適應記憶濾波;推導出自適應組合濾波算法如下:

(37)

(38)

(39)

(40)

(41)

(42)

(43)

3 仿真分析

3.1 自適應濾波組合算法仿真分析

針對衰減自適應記憶濾波和噪聲加權自適應濾波的特點,可對這2種算法進行組合,實現優勢互補,從而滿足對動態模型噪聲干擾不確定的復雜BDS/INS系統。首先對不準確的集成系統進行了濾波,由于環境噪聲的不確定性,本文前期驗證了此算法的合理性。設目標的運動狀態的x=4t+1,其狀態方程和測量方程為

x(k+1)=x(k)+ω(k)，

z(k)=x(k)+υ(k) ，

(44)

式中:x(k)為目標的運動狀態;z(k)為目標運動狀態的測量值;ω(k)為過程噪聲；υ(k)為運動目標的過程噪聲。采樣周期為1。傳統卡爾曼濾波與衰減自適應卡爾曼算法對上述系統狀態估計的軌跡和誤差如圖1、2所示。

從上圖可以看出采用衰減自適應記憶算法在一定程度上可以提高了濾波器的精度。當環境噪聲變化時,衰減自適應記憶算法可以更好地調節濾波噪聲方差Rk,從而卡爾曼增益K調節更加靈敏。

3.2 BDS/INS位置組合導航仿真分析

INS和BDS各有優缺點,但在誤差傳播性能上正好是互補的。INS/BDS組合導航系統可以優勢互補,取長補短。對BDS/INS組合導航進行仿真研究時,用BDS的高精度定位信息通過衰減記憶自適應卡爾曼濾波來補償INS系統的積累誤差,提高導航精度。同時,用INS系統的速度和加速度信息對BDS接收機進行速度輔助,以提高BDS接收機的抗干擾能力和動態性能。將此算法用于BDS/INS組合導航。

仿真選取一段無人機飛行的數據,INS輸出相應的位置、速度、四元數等,通過其輸出的四元數得到姿態矩陣;計算出組和導航系統的轉移矩陣F、G以及得出組合導航位置量測方程H。把衛星接收機和INS的輸出的位置差值作為量測值,輸入到卡爾曼濾波器中,經過濾波后估計INS的誤差,進而對INS進行校正。得到導航系統的位置輸出。在計算機上進行仿真,得到不同濾波算法定位誤差圖,如圖4、5、6所示。

圖3示出了INS系統和衛星導航的原始定位誤差。從圖3可以看出慣導系統在前100 s定位誤差相對較低;但是隨時間的變化,定位誤差逐漸增加,故單一導航系統難以為無人機提供精確的導航。對比改進的自適應算法(圖4)與衰減因子卡爾曼算法(圖5)和量測噪聲加權算法(圖6)的定位誤差,可以驗證改進的自適應算法可以在一定程度上提高導航的性能和系統的魯棒性。

4 結束語

本文首先對比了卡爾曼濾波算法和衰減自適應記憶卡爾曼濾波算法對勻速運動的預測;本文提出的衰減自適應記憶卡爾曼算法可以使濾波保持最優。該算法不斷更新量測噪聲方差,在線自適應估計衰減因子。使預測值更加精確,有效地抑制了卡爾曼濾波估計的發散。其次采用一組飛行數據作為仿真的原始數據,驗證了衰減自適應記憶濾波和噪聲加權自適應濾波在難以確定噪聲的BDS/INS組合導航的適用性。