強明輝,蔣文

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

0 引 言

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

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

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

1 BDS/INS組合導(dǎo)航系統(tǒng)的誤差狀態(tài)方程

1.1 誤差方程

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

(1)

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

(2)

式中:X(t)為系統(tǒng)狀態(tài)向量;W(t)為系統(tǒng)噪聲向量;F(t)為系統(tǒng)矩陣;G(t)為系統(tǒng)噪聲矩。其中[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為姿態(tài)角誤差;δVE、δVN、δVU為速度誤差;δL、δλ、δh為緯度、經(jīng)度、高度誤差;εbx、εby、εbz為陀螺儀的隨機常數(shù)漂移;εrx、εry、εrz為陀螺儀一階馬爾科夫漂移;x、y、z為加速度一階馬爾科夫漂移;為姿態(tài)矩陣;矩陣的選取參見文獻[6]。

1.2 BDS/INS組合導(dǎo)航系統(tǒng)量測方程

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

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

(7)

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

(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的速度量測信息用其真值與相應(yīng)速度誤差之和表示為

(10)

在東北天坐標系下北斗導(dǎo)航系統(tǒng)速度量測信息用其真值與相應(yīng)速度誤差之差表示為

(11)

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

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

(12)

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

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

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

(13)

2 組合導(dǎo)航算法

2.1 衰減自適應(yīng)記憶濾波

考慮如下離散非線性系統(tǒng):

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

(14)

Zk=HkXk+νk,

(15)

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

(16)

(17)

(18)

(19)

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

(20)

(21)

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

(22)

2.2 噪聲加權(quán)自適應(yīng)卡爾曼濾波算法

定義量測信息:

rk

(23)

最佳濾波在不同的時刻余差互不相關(guān),可以得到誤差協(xié)方差矩陣:

Crk

(24)

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

(25)

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

(26)

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

(27)

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

(28)

(29)

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

(30)

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

k時刻的后驗估計協(xié)方差表達式為

(31)

k時刻的先驗估計協(xié)方差表達式為

(32)

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

(33)

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

(34)

(35)

(36)

其中,Ci(i=1,2,…)是一個適當?shù)恼麛?shù)。這種形式的加權(quán)可以減弱以前的測量值的影響。

2.3 自適應(yīng)組合濾波算法

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

(37)

(38)

(39)

(40)

(41)

(42)

(43)

3 仿真分析

3.1 自適應(yīng)濾波組合算法仿真分析

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

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

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

(44)

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

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

3.2 BDS/INS位置組合導(dǎo)航仿真分析

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

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

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

4 結(jié)束語

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