基于GPS/INS的自適應(yīng)無(wú)跡Kalman濾波算法

曹紅燕，劉長(zhǎng)明，沈小林，牛興龍，李大威，陳燕

(1.中北大學(xué) 電氣與控制工程學(xué)院，山西 太原 030051；2.陸軍裝備部駐北京地區(qū)軍事代表局某軍代室，山西 太原 030051)

0 引 言

隨著科技發(fā)展，軍隊(duì)建設(shè)對(duì)武器的作戰(zhàn)能力提出了更高的要求。在無(wú)人機(jī)作戰(zhàn)系統(tǒng)中，姿態(tài)信息獲取及其測(cè)量精度作為關(guān)鍵因素，一定程度上決定著戰(zhàn)爭(zhēng)的成敗，因此，需要選擇更合適的組合導(dǎo)航系統(tǒng)，獲得更高精度的姿態(tài)信息。運(yùn)載體的飛行導(dǎo)航主要分為自主性導(dǎo)航和非自主性導(dǎo)航兩種[1]，常用的導(dǎo)航系統(tǒng)有捷聯(lián)慣性導(dǎo)航系統(tǒng)(INS)，全球定位導(dǎo)航系統(tǒng)(GPS)。INS導(dǎo)航系統(tǒng)[2]不依賴(lài)于外部信息，能夠長(zhǎng)時(shí)間持續(xù)工作，可在一定速度和時(shí)間內(nèi)提供完善的導(dǎo)航信息，對(duì)姿態(tài)等信息的測(cè)量精度高且穩(wěn)定性好，但測(cè)量誤差會(huì)隨時(shí)間越來(lái)越大，導(dǎo)致對(duì)姿態(tài)角等信息的測(cè)量精度降低[3]。GPS導(dǎo)航系統(tǒng)可以實(shí)時(shí)工作，測(cè)得更加完整有時(shí)序性的信息[4]，相比INS系統(tǒng)其在長(zhǎng)時(shí)間連續(xù)工作時(shí)更加穩(wěn)定，且精度更高。將GPS與INS系統(tǒng)結(jié)合構(gòu)成組合導(dǎo)航系統(tǒng)，能夠減小測(cè)量誤差，彌補(bǔ)INS系統(tǒng)長(zhǎng)時(shí)間工作精度降低的缺陷[5]，并能解決GPS系統(tǒng)信號(hào)失真問(wèn)題，對(duì)姿態(tài)角，位置等重要姿態(tài)信息的測(cè)量具有更高的精度，更好的抗干擾性[6]。

目前國(guó)外對(duì)姿態(tài)信息測(cè)量的研究已有很多，如S.Bezick等[7]將Kalman濾波與外部非慣性導(dǎo)航系統(tǒng)相結(jié)合進(jìn)行數(shù)據(jù)處理，提高了導(dǎo)彈導(dǎo)航系統(tǒng)的對(duì)準(zhǔn)技術(shù)和精度；DENG Z[8]針對(duì)純慣性導(dǎo)航模式下的周期振蕩誤差，設(shè)計(jì)了捷聯(lián)慣性導(dǎo)航系統(tǒng)的外水平阻尼網(wǎng)絡(luò)，提出了基于外速度的水平阻尼算法，通過(guò)三軸轉(zhuǎn)臺(tái)和仿真實(shí)驗(yàn)驗(yàn)證了該算法的有效性。文獻(xiàn)[7-8]對(duì)于單導(dǎo)航系統(tǒng)進(jìn)行算法分析，雖有一定改進(jìn)，但沒(méi)有針對(duì)組合導(dǎo)航系統(tǒng)進(jìn)行算法分析與改進(jìn)。H.E.Soken等[9]處理GPS/INS組合導(dǎo)航中的數(shù)據(jù)時(shí)引入Kalman濾波算法，取得一定效果，但如果GPS測(cè)量的數(shù)據(jù)出現(xiàn)粗差，姿態(tài)信息的測(cè)量精度將會(huì)受影響。目前已有很多研究針對(duì)導(dǎo)航系統(tǒng)的Kalman濾波算法進(jìn)行了改進(jìn)，涉及到擴(kuò)展Kalman、容積Kalman、無(wú)跡Kalman等。在GPS/INS系統(tǒng)中，存在很多干擾因素，如系統(tǒng)模型擾動(dòng)，噪聲誤差等，如果不考慮這些方面影響，組合導(dǎo)航系統(tǒng)將會(huì)產(chǎn)生測(cè)姿定位錯(cuò)誤，帶來(lái)非常惡劣的影響[10]。

本文在GPS與INS緊組合的情況下，利用四元數(shù)法對(duì)無(wú)人機(jī)軌跡進(jìn)行測(cè)姿，對(duì)姿態(tài)角的信息運(yùn)用擴(kuò)展Kalman濾波、無(wú)跡Kalman濾波和改進(jìn)的算法進(jìn)行處理，并對(duì)比結(jié)果，分析實(shí)驗(yàn)數(shù)據(jù)。在無(wú)跡Kalman濾波基礎(chǔ)上，提出改進(jìn)算法，對(duì)數(shù)據(jù)進(jìn)行最優(yōu)估計(jì)和誤差補(bǔ)償。

1 基礎(chǔ)知識(shí)

1.1 主要坐標(biāo)系

姿態(tài)測(cè)量應(yīng)用的牛頓力學(xué)是在慣性空間條件下確立的，尤其是在潛艇、炮彈、航空航天以及機(jī)器人領(lǐng)域應(yīng)用十分廣泛[11]。正確的坐標(biāo)系對(duì)研究物體的運(yùn)動(dòng)非常關(guān)鍵，坐標(biāo)系的選擇會(huì)影響姿態(tài)信息的測(cè)量。本文仿真計(jì)算及試驗(yàn)涉及到的主要坐標(biāo)系如下。

(1)慣性坐標(biāo)系(oxiyizi)。此坐標(biāo)系Zi軸沿地球自轉(zhuǎn)軸極方向，xi,yi軸在赤道平面內(nèi)，分別指向空間中的兩顆恒星[12]。

(2)地球坐標(biāo)系(oxeyeze)。xe軸與格林威治子午線對(duì)準(zhǔn)，ye軸位于東經(jīng)90°，ze軸與地球自轉(zhuǎn)方向一致，坐標(biāo)系相對(duì)地心慣性坐標(biāo)系以ωe=15.041 088°/h的角速率旋轉(zhuǎn)[13]，見(jiàn)圖1。

圖1 地球坐標(biāo)系

1.2 姿態(tài)角

導(dǎo)航坐標(biāo)系(oxnynzn)經(jīng)過(guò)三次旋轉(zhuǎn)，可得載體坐標(biāo)系(oxbybzb)，轉(zhuǎn)換示意圖見(jiàn)圖2，轉(zhuǎn)換后生成的姿態(tài)角說(shuō)明如下。

圖2 導(dǎo)航坐標(biāo)系與載體坐標(biāo)系的轉(zhuǎn)換示意

(1)航向角ψ。載體縱軸在水平面上的投影與地球經(jīng)線的夾角[14]，[0°～+360°]。

(2)俯仰角θ。載體縱軸和水平面的夾角[15]，[-90°～+90°]。

(3)橫滾角γ。載體橫軸與水平面的夾角[15]，[-180°～+180°]。

本文對(duì)姿態(tài)角的更新測(cè)量采用四元數(shù)法，用四維向量的形式表示姿態(tài)信息，主要計(jì)算過(guò)程為

Q=q0+q1ib+q2jb+q3kb，

(1)

式中：q0，q1，q2，q3為實(shí)數(shù)；ib，jb，kb為載體方向的單位矢量。

四元數(shù)微分方程為

(2)

其中，ω=0+ωxib+ωyjb+ωzkb，可得矩陣形式，即

(3)

1.3 擴(kuò)展Kalman濾波

擴(kuò)展Kalman濾波是以Kalman濾波為基礎(chǔ)的一種次優(yōu)估計(jì)，利用泰勒級(jí)數(shù)展開(kāi)非線性函數(shù),并對(duì)高階項(xiàng)采用忽略或逼近措施的近似線性化方法[16]，其中重要的計(jì)算過(guò)程如下。

非線性離散系統(tǒng)的狀態(tài)方程和測(cè)量方程為

Xk=f(Xk-1)+Wk-1，

(4)

Zk=h(Xk)+Vk，

(5)

式中，Vk和Wk為高斯白噪聲。

統(tǒng)計(jì)特性為

E[Wk]=qk，cov(Wk,Wj)=QkδkJ，

(6)

E[Vk]=γk，cov(Vk,Vj)=RkδkJ，

(7)

cov(Wk,Vj)=0。

(8)

初始狀態(tài)X0獨(dú)立于Wk，Vk，其中X0的均值和協(xié)方差分別為

(9)

P0=cov(X0，X0)=

(10)

將Xk-1展開(kāi)成泰勒級(jí)數(shù)并保留一階項(xiàng)，

(11)

擴(kuò)展Kalman濾波算法展開(kāi)后僅保留泰勒展開(kāi)式的一階項(xiàng)，但高階項(xiàng)也會(huì)對(duì)數(shù)據(jù)融合結(jié)果產(chǎn)生一定影響，可參考因素的減少會(huì)導(dǎo)致結(jié)果的片面性，因此，該濾波算法是非線性函數(shù)局部線性化的結(jié)果[17]，對(duì)于非線性化程度高的系統(tǒng)，隨著時(shí)間推移，如果過(guò)程和測(cè)量噪聲協(xié)方差估計(jì)不夠準(zhǔn)確，累積的誤差會(huì)越來(lái)越大，測(cè)量精度越來(lái)越低，從而影響整個(gè)系統(tǒng)的工作。

1.4 無(wú)跡Kalman濾波

無(wú)跡變換是基于加權(quán)統(tǒng)計(jì)線性回歸計(jì)算隨機(jī)變量的后驗(yàn)分布[18]，無(wú)跡Kalman濾波融合算法是在無(wú)跡變換下進(jìn)行近似線性最小方差估計(jì)的方法[19]，計(jì)算過(guò)程如圖3所示，具體步驟如下。

圖3 無(wú)跡Kalman濾波計(jì)算過(guò)程

(1)初始化，初始狀態(tài)為

(12)

(13)

i=1,2,…,n,

(14)

i=n+1,n+2,…,2n。

(15)

(3)對(duì)采樣點(diǎn)集進(jìn)行無(wú)跡變換，得到點(diǎn)集的預(yù)測(cè)方程為

Xi(k+1|k)=f(Xi(k|k),Wk)。

(16)

(4)進(jìn)行加權(quán)處理，得到系統(tǒng)狀態(tài)量的一步預(yù)測(cè)方程和協(xié)方差方程，分別為

(17)

(18)

(5)根據(jù)一步預(yù)測(cè)估計(jì)值再次進(jìn)行無(wú)跡變換，生成新的采樣點(diǎn)集，即

(19)

i=n+1,n+2，…，2n。

(20)

(6)根據(jù)非線性測(cè)量方程進(jìn)行非線性變換，

Zi(k+1|k)=h(Xi(k+1|k),Vk)，

i=0,1，…，2n。

(21)

(7)進(jìn)行加權(quán)求和并計(jì)算系統(tǒng)的測(cè)量預(yù)測(cè)值，

(22)

(8)計(jì)算協(xié)方差PXkZk及輸出變量的方差矩陣PZkZk，

(23)

(24)

(9)計(jì)算Kalman增益矩陣，

Kk+1=PXkZkPZkZkT。

(25)

(10)狀態(tài)更新：

(26)

P(k+1|k+1)=P(k+1|k)-

Kk+1PZkZkKk+1T。

(27)

Kalman濾波、擴(kuò)展Kalman濾波以及無(wú)跡Kalman濾波3種算法各有優(yōu)缺點(diǎn)，標(biāo)準(zhǔn)Kalman濾波假設(shè)系統(tǒng)狀態(tài)和測(cè)量的數(shù)學(xué)模型是線性的，在線性系統(tǒng)中，能夠有效降低噪聲影響，但在實(shí)際工程中，真正意義上的線性系統(tǒng)并不存在，同時(shí)狀態(tài)和噪聲不一定符合高斯分布，所以標(biāo)準(zhǔn)Kalman濾波不適用于非線性系統(tǒng)。

無(wú)跡Kalman濾波計(jì)算非線性分布統(tǒng)計(jì)量的精度至少可以達(dá)到二階，在特殊采樣策略方法下能夠達(dá)到更高階的精度，在算法運(yùn)算過(guò)程中模型沒(méi)有損失，精度較高，所以本文選擇無(wú)跡Kalman濾波算法進(jìn)行主要研究，并將結(jié)果與前兩種算法進(jìn)行對(duì)比分析。雖然無(wú)跡Kalman濾波比Kalman濾波和擴(kuò)展Kalman濾波有一定的優(yōu)勢(shì)，但仍會(huì)受到高斯噪聲約束，如果涉及到復(fù)雜的待測(cè)數(shù)據(jù)，需要對(duì)算法進(jìn)行改進(jìn)。本文主要在GPS與INS緊組合的情況下進(jìn)行試驗(yàn)，具體的流程圖如圖4所示，在該框架下對(duì)新息噪聲，狀態(tài)預(yù)測(cè)，統(tǒng)計(jì)量的閾值幾個(gè)方面進(jìn)行改進(jìn)，驗(yàn)證并對(duì)比算法結(jié)果，流程圖見(jiàn)圖4。

圖4 緊組合系統(tǒng)工作流程圖

2 改進(jìn)算法

2.1 新息噪聲改進(jìn)

在無(wú)跡Kalman濾波的變換中，sigma點(diǎn)集根據(jù)系統(tǒng)方程和測(cè)量方程計(jì)算狀態(tài)估計(jì)和誤差協(xié)方差陣，其中涉及到系統(tǒng)噪聲和測(cè)量噪聲的協(xié)方差陣Q′，R，這些噪聲的狀態(tài)屬性值已知，均為預(yù)先設(shè)定的高斯白噪聲，但是實(shí)際與理論的噪聲不相符，Q′和R在算法運(yùn)行中是實(shí)時(shí)變化的，存在一定偏差，隨著時(shí)間累積容易造成濾波器的發(fā)散。針對(duì)這個(gè)問(wèn)題，目前有學(xué)者提出了衰減記憶算法、模糊自適應(yīng)算法等。衰減記憶算法主要引用衰減因子控制濾波器的發(fā)散，但測(cè)量精度大大降低。模糊自適應(yīng)算法主要通過(guò)設(shè)定一個(gè)模糊推理系統(tǒng)預(yù)測(cè)噪聲的方差值，適用于較弱的噪聲，當(dāng)噪聲很強(qiáng)時(shí)，對(duì)數(shù)據(jù)的預(yù)處理較復(fù)雜。

為了解決此問(wèn)題，本文的第一個(gè)改進(jìn)是對(duì)系統(tǒng)噪聲協(xié)方差Q′和測(cè)量噪聲協(xié)方差R在線估值。觀測(cè)新息矢量包括GPS位置、速度與校正后的INS位置、速度的差值，

eZk=Zk-HkXk|k-1。

(28)

計(jì)算最后N個(gè)觀測(cè)新息的協(xié)方差，N為自適應(yīng)窗口，取值可變，即

(29)

(30)

(31)

將式(30)和(31)分別代入式(18)和(24)中重新計(jì)算，在線估計(jì)噪聲的統(tǒng)計(jì)特性，將系統(tǒng)噪聲和測(cè)量噪聲改進(jìn)算法與導(dǎo)航系統(tǒng)融合。

2.2 狀態(tài)預(yù)測(cè)改進(jìn)

本文的第二個(gè)改進(jìn)是對(duì)狀態(tài)預(yù)測(cè)協(xié)方差矩陣引入次優(yōu)漸消因子λk，改進(jìn)后的無(wú)跡Kalman對(duì)狀態(tài)預(yù)測(cè)協(xié)方差引入的次優(yōu)漸消因子確定不需計(jì)算雅克比矩陣，具體計(jì)算過(guò)程如下。

(32)

(1)新息向量為

(33)

(2)次優(yōu)漸消因子λk按式(35)求取，

(34)

(35)

(36)

(37)

(4)改進(jìn)的狀態(tài)測(cè)量預(yù)測(cè)協(xié)方差為

(38)

(5)對(duì)式(38)兩邊求跡，可得次優(yōu)漸消因子：

(39)

(6)最后取值為

(40)

2.3 統(tǒng)計(jì)量閾值改進(jìn)

(41)

圖5 計(jì)算流程圖

3 算法驗(yàn)證及結(jié)果分析

為了對(duì)比算法結(jié)果，驗(yàn)證有效性，本文將GPS系統(tǒng)和INS系統(tǒng)進(jìn)行緊組合，在此框架下進(jìn)行算法驗(yàn)證和MATLAB試驗(yàn)仿真，設(shè)計(jì)一段無(wú)人機(jī)飛行軌跡，計(jì)算陀螺儀和加速度計(jì)的數(shù)據(jù)信息，模擬各自的誤差，與理想的慣性參數(shù)擬合在一起，解算出需要的姿態(tài)信息，利用本文所述的數(shù)據(jù)改進(jìn)算法對(duì)數(shù)據(jù)進(jìn)行處理，表1是本文試驗(yàn)的初始狀態(tài)信息。

表1 初始狀態(tài)

MATLAB仿真的無(wú)人機(jī)飛行航線軌跡如圖6所示。

圖6 總體航跡圖

緊組合情況下，INS系統(tǒng)和 GPS系統(tǒng)互相獨(dú)立工作，通過(guò)數(shù)據(jù)融合算法對(duì)其進(jìn)行校正，圖7～9是運(yùn)用緊組合的導(dǎo)航模式對(duì)無(wú)人機(jī)姿態(tài)角進(jìn)行測(cè)量并用擴(kuò)展Kalman算法、無(wú)跡Kalman算法及改進(jìn)的濾波算法處理數(shù)據(jù)的誤差曲線。

由圖7可知，緊組合導(dǎo)航系統(tǒng)測(cè)得的姿態(tài)角與真實(shí)姿態(tài)角的誤差減小，俯仰角誤差為-1°～6°，航向角誤差為0°～6°，橫滾角誤差為-4.5°～0.5°。

圖7 Kalman濾波算法補(bǔ)償后姿態(tài)角的誤差曲線

由圖9可知，紅色曲線為無(wú)跡Kalman濾波數(shù)據(jù)融合后的誤差，黑色曲線為改進(jìn)算法數(shù)據(jù)融合后的誤差，兩種算法都能將誤差減小，比擴(kuò)展Kal-man濾波后的精度高，在200 s左右姿態(tài)角絕對(duì)收斂，姿態(tài)角精度均高達(dá)0.1°，并在50 s之后絕對(duì)收斂，收斂性強(qiáng)于無(wú)跡Kalman算法，相比之下，改進(jìn)后的算法抑制隨時(shí)間漂移誤差的能力更強(qiáng)，姿態(tài)角的精度更高。

圖8 擴(kuò)展Kalman濾波補(bǔ)償后姿態(tài)角的誤差曲線

圖9 無(wú)跡Kalman濾波及改進(jìn)算法補(bǔ)償后姿態(tài)角的誤差曲線

4 結(jié) 語(yǔ)

使用常規(guī)Kalman濾波器對(duì)組合導(dǎo)航系統(tǒng)狀態(tài)進(jìn)行估計(jì)時(shí)，需先給定系統(tǒng)噪聲和測(cè)量噪聲的統(tǒng)計(jì)特性，但在實(shí)際物理過(guò)程中，無(wú)法確定噪聲統(tǒng)計(jì)特性。針對(duì)此問(wèn)題，本文提出了一種自適應(yīng)無(wú)跡Kalman濾波算法，利用觀測(cè)新息協(xié)方差對(duì)噪聲統(tǒng)計(jì)特性在線估計(jì)，對(duì)狀態(tài)預(yù)測(cè)引入漸消因子，重新計(jì)算狀態(tài)值。以緊組合導(dǎo)航系統(tǒng)為背景，使得INS與GPS兩個(gè)系統(tǒng)的優(yōu)點(diǎn)缺點(diǎn)互補(bǔ)，滿(mǎn)足了動(dòng)態(tài)性能和抗干擾能力的需求，提升導(dǎo)航定位的精確性。運(yùn)用3種濾波算法進(jìn)行補(bǔ)償分析，由結(jié)果可知，無(wú)跡Kalman和改進(jìn)的算法測(cè)姿精度大于擴(kuò)展Kalman濾波，有效抑制了INS系統(tǒng)誤差隨時(shí)間積累的現(xiàn)象，彌補(bǔ)了GPS的動(dòng)態(tài)誤差缺陷，本文的改進(jìn)算法收斂性更強(qiáng)，穩(wěn)定性更好，測(cè)姿精度更高，為了提高算法的實(shí)時(shí)性，保證算法的可用性，使算法具有說(shuō)服力，未來(lái)會(huì)繼續(xù)對(duì)算法進(jìn)行改進(jìn)，并將其應(yīng)用于實(shí)物測(cè)試，獲得更有效的數(shù)據(jù)。