黃春梅，　王　杰，　鄒慧瑩

(長(zhǎng)春工業(yè)大學(xué) 計(jì)算機(jī)科學(xué)與工程學(xué)院， 吉林 長(zhǎng)春　130012)

?

改進(jìn)的CKF在SINS/GPS組合導(dǎo)航中的應(yīng)用

黃春梅，王杰，鄒慧瑩

(長(zhǎng)春工業(yè)大學(xué) 計(jì)算機(jī)科學(xué)與工程學(xué)院， 吉林 長(zhǎng)春130012)

摘要：為了降低動(dòng)態(tài)擾動(dòng)誤差對(duì)容積卡爾曼濾波(Cubature Kalman Filter, CKF)精度的影響，采用新息自適應(yīng)估計(jì)(Innovation-based Adaptive Estimation, IAE)理論構(gòu)造改進(jìn)的CKF濾波模型，利用新息序列在線估計(jì)和修正噪聲統(tǒng)計(jì)特性，自適應(yīng)地調(diào)整量測(cè)噪聲。通過(guò)SINS/GPS組合導(dǎo)航系統(tǒng)對(duì)標(biāo)準(zhǔn)CKF與改進(jìn)的CKF進(jìn)行仿真驗(yàn)證，結(jié)果表明,改進(jìn)的CKF算法能夠在一定程度上提高組合導(dǎo)航系統(tǒng)對(duì)不同隨機(jī)噪聲的適應(yīng)能力，有效地降低了濾波誤差，提高了解算精度。

關(guān)鍵詞：容積卡爾曼濾波； SINS/GPS組合導(dǎo)航； 非線性系統(tǒng)； 自適應(yīng)濾波

0引言

在組合導(dǎo)航系統(tǒng)中，提高導(dǎo)航定位的解算精度是導(dǎo)航領(lǐng)域的關(guān)鍵問(wèn)題。由于SINS/GPS組合導(dǎo)航系統(tǒng)的狀態(tài)方程在直接濾波算法中具有非線性形式，所以常規(guī)的卡爾曼濾波算法已不再適用[1]。因此，擴(kuò)展卡爾曼濾波(Extend Kalman Filter, EKF)、無(wú)跡卡爾曼濾波(Unscented Kalman Filter, UKF)、粒子濾波(Particle Filter, PF)等非線性濾波方法被相繼提出。EKF算法實(shí)現(xiàn)簡(jiǎn)單、應(yīng)用廣泛，但是計(jì)算量大，且濾波結(jié)果易發(fā)散。UKF采用UT變換，克服了EKF的局限性，但在高維非線性系統(tǒng)中，缺乏對(duì)異常擾動(dòng)的自適應(yīng)調(diào)節(jié)能力[2]。CKF將基于Bayesian理論的非線性濾波歸結(jié)于求解非線性函數(shù)與高斯概率密度乘積的積分問(wèn)題，相比EKF、UKF濾波方法，CKF具有更好的濾波性能，適用于SINS/GPS組合導(dǎo)航非線性濾波的要求[3]。

CKF是在狀態(tài)變量和量測(cè)變量符合相互獨(dú)立的高斯分布前提下得到的，它導(dǎo)出了狀態(tài)變量的預(yù)測(cè)密度函數(shù)和濾波密度函數(shù)。根據(jù)三階球面-相徑容積規(guī)則確定的均值、方差是對(duì)非線性系統(tǒng)狀態(tài)的最佳估計(jì)[4],其估計(jì)精度高、不容易發(fā)散且計(jì)算量小，使得CKF的應(yīng)用越來(lái)越廣泛。由于在濾波過(guò)程中存在異常擾動(dòng)問(wèn)題，為了降低動(dòng)態(tài)異常擾動(dòng)誤差對(duì)CKF濾波精度的影響，文中嘗試?yán)米赃m應(yīng)算法調(diào)整量測(cè)噪聲，通過(guò)仿真結(jié)果分析，改進(jìn)的CKF算法能夠消除異常誤差的出現(xiàn)，改善了濾波精度及可靠性。

1SINS/GPS緊組合導(dǎo)航模型

緊組合導(dǎo)航模型將捷聯(lián)慣導(dǎo)系統(tǒng)和GPS接收機(jī)進(jìn)行一體化設(shè)計(jì)，主要由SINS系統(tǒng)、GPS系統(tǒng)和組合導(dǎo)航濾波器3部分組成，SINS與GPS兩系統(tǒng)相互輔助[5-6]，如圖1所示。

圖1　SINS/GPS緊組合系統(tǒng)結(jié)構(gòu)框圖

1.1緊組合導(dǎo)航系統(tǒng)狀態(tài)方程

對(duì)于SINS/GPS緊組合導(dǎo)航模型，系統(tǒng)狀態(tài)方程建立如下：

加速度計(jì)誤差▽E、▽N、▽U主要含有零位偏置▽AE、▽AN、▽AU和白噪聲ωAE、ωAN、ωAU，即

(1)

可以得到濾波器的狀態(tài)方程為：

(2)

其中，狀態(tài)向量X含有9個(gè)狀態(tài)

(3)

W(t)為噪聲向量：

(4)

系數(shù)矩陣F為:

(5)

其中Fb(6×6)為慣導(dǎo)誤差方程式系數(shù)所確定的矩陣，F(xiàn)s(6×3)、FG(3×6)分別為

(6)

(7)

1.2緊組合導(dǎo)航系統(tǒng)量測(cè)方程

假設(shè)Lt、λt、ht、VSEt、VSNt、VSUt為載體的真實(shí)位置和速度，以慣導(dǎo)的定位誤差和GPS的定位誤差作為濾波器的輸入量，定義量測(cè)向量為

(8)

其中

(9)

(10)

2標(biāo)準(zhǔn)CKF

CKF算法通過(guò)選取2n個(gè)相同權(quán)值的容積點(diǎn)來(lái)計(jì)算推演出下一時(shí)刻系統(tǒng)狀態(tài)的濾波估計(jì)。采用三階球面-相徑容積規(guī)則來(lái)計(jì)算高斯加權(quán)積分，而貝葉斯濾波簡(jiǎn)化為求解非線性函數(shù)與高斯概率密度乘積的積分問(wèn)題。

針對(duì)離散非線性狀態(tài)空間模型：

(11)

xk∈Rn×1，zk∈Rm×1

vk-1～N(0,Qk),wk-1～N(0,Rk)

1)分解

(12)

2)形成容積點(diǎn)

(13)

(14)

其中，ei是單位列矢量的第i元，唯獨(dú)它等于1，其余元素等于0。

3)計(jì)算傳播的容積點(diǎn)

(15)

4)計(jì)算預(yù)測(cè)的狀態(tài)

(16)

5)計(jì)算預(yù)測(cè)的狀態(tài)誤差矩陣

(17)

量測(cè)更新過(guò)程如下：

1)分解

(18)

2)形成容積點(diǎn)

(19)

3)計(jì)算傳播的容積點(diǎn)

(20)

4)計(jì)算測(cè)量的預(yù)測(cè)值

(21)

5)計(jì)算新的量測(cè)誤差協(xié)方差矩陣

(22)

6)計(jì)算互協(xié)方差矩陣

(23)

7)計(jì)算濾波增益

(24)

8)計(jì)算濾波狀態(tài)

(25)

9)計(jì)算濾波后的協(xié)方差矩陣

(26)

3改進(jìn)的CKF算法

基于新息自適應(yīng)估計(jì)算法，以新息序列協(xié)方差匹配原理作為依據(jù)，將卡爾曼濾波器參數(shù)進(jìn)行自適應(yīng)調(diào)整[7-8]。用新息序列的統(tǒng)計(jì)特性來(lái)對(duì)濾波器的工作狀態(tài)進(jìn)行檢測(cè)，當(dāng)濾波器工作正常時(shí)，新息序列將是零均值的高斯白噪聲，但是當(dāng)濾波器工作不正常時(shí)，新息序列的統(tǒng)計(jì)就不滿足上述條件。正是基于這種思想，IAE方法要通過(guò)對(duì)量測(cè)新息統(tǒng)計(jì)的計(jì)算，自適應(yīng)的調(diào)整模型中的噪聲協(xié)方差陣。

假設(shè)估計(jì)窗口長(zhǎng)度為m,tk時(shí)刻的新息序列為：

(27)

由IAE方法計(jì)算新息序列的協(xié)方差矩陣為：

(28)

利用tk-1時(shí)刻之前m個(gè)時(shí)刻的信息進(jìn)行求解，可避免利用t時(shí)刻新信息值，則

(29)

隨著系統(tǒng)的不斷運(yùn)行，量測(cè)噪聲方差陣被自適應(yīng)的調(diào)整，系統(tǒng)模型的濾波效果更符合實(shí)際情況。

4仿真實(shí)驗(yàn)及結(jié)果分析

對(duì)含有SINS/GPS組合導(dǎo)航系統(tǒng)的飛行器分別采用CKF算法和改進(jìn)的CKF算法進(jìn)行導(dǎo)航計(jì)算[9]。設(shè)定運(yùn)載器工作的初始位置為: 東經(jīng)125°，北緯43°。飛行器初始狀態(tài)為:東、北、天三方向速度的初始偏差均為0，東向加速度初始偏差為1.0×10-4m/s2；北向加速度初始偏差為0；天向加速度初始偏差為0。三方向速度的測(cè)量誤差均為0.01 m/s。隨機(jī)偏差為1.0×10-6，刻度誤差為0.000 1，經(jīng)度λ、緯度L、高度h的初始誤差量分別為1′、1′、1 m。SINS與GPS的采樣周期分別為0.01、1 s,仿真時(shí)間為200 s。

標(biāo)準(zhǔn) CKF和文中提出的改進(jìn)的CKF計(jì)算得到的定位誤差如圖2～圖5所示。

圖2　CKF速度誤差仿真圖

圖3　改進(jìn)的CKF速度誤差仿真圖

圖4　CKF位置誤差仿真圖

圖5　改進(jìn)的CKF位置誤差仿真圖

具體實(shí)驗(yàn)對(duì)比數(shù)據(jù)見(jiàn)表1和表2。

表1　兩種算法的速度誤差實(shí)驗(yàn)仿真數(shù)據(jù)比較

表2　兩種算法的位置誤差實(shí)驗(yàn)仿真數(shù)據(jù)比較

從濾波輸出結(jié)果中可以看出，改進(jìn)的CKF算法可以自適應(yīng)地更新量測(cè)噪聲的統(tǒng)計(jì)特性，能夠在一定程度上抑制異常量測(cè)對(duì)濾波精度的影響，因而得到的定位精度比標(biāo)準(zhǔn)CKF更高。由表1和表2可以看出，改進(jìn)的CKF的速度誤差與位置誤差均明顯小于標(biāo)準(zhǔn)的CKF，表明當(dāng)噪聲統(tǒng)計(jì)不準(zhǔn)確時(shí)，改進(jìn)的CKF濾波算法可以有效地提高濾波精度，減小SINS/GPS組合導(dǎo)航非線性濾波的定位誤差。

5結(jié)語(yǔ)

為了提高組合導(dǎo)航系統(tǒng)的濾波解算精度，基于IAE自適應(yīng)估計(jì)原理提出一種改進(jìn)的CKF濾波算法。該算法能夠在線地估計(jì)和調(diào)整量測(cè)噪聲的統(tǒng)計(jì)特性，使系統(tǒng)模型更加符合實(shí)際情況，提高了CKF的自適應(yīng)能力。

實(shí)驗(yàn)結(jié)果表明，當(dāng)導(dǎo)航系統(tǒng)噪聲統(tǒng)計(jì)不準(zhǔn)確時(shí)，改進(jìn)的CKF濾波性能明顯優(yōu)于標(biāo)準(zhǔn)CKF濾波，可以有效地提高SINS/GPS組合導(dǎo)航系統(tǒng)的定位精度和可靠性。

參考文獻(xiàn)：

[1]李建文,郝順義.高動(dòng)態(tài)GPS/SINS組合導(dǎo)航精確定位方法研究[J].信息與控制，2009，38(3)：304-308.

[2]黃春梅,尹新.GPS/SINS組合導(dǎo)航系統(tǒng)中自適應(yīng)濾波器[J].長(zhǎng)春工業(yè)大學(xué)學(xué)報(bào),2015，36(6):687-691.

[3]徐慧娟.EKF與UKF在INS/GPS組合導(dǎo)航中的應(yīng)用分析[J].航天控制，2006，24(6):7-10.

[4]葛磊.容積卡爾曼濾波算法研究及其在導(dǎo)航中的應(yīng)用[D].哈爾濱:哈爾濱工程大學(xué)，2013.

[5]唐季軍.Cubature卡爾曼濾波及其在導(dǎo)航中的應(yīng)用研究[D].哈爾濱:哈爾濱工程大學(xué)，2012.

[6]K Pakki, B Chandra, D W Gu. Cubature inforamtion filter and its applications[C]//Proceeding of the 2011 American Control Conference,2011.

[8]張靈利.捷聯(lián)慣導(dǎo)兩種姿態(tài)算法抗干擾性比較[J].長(zhǎng)春工業(yè)大學(xué)學(xué)報(bào):自然科學(xué)版,2012,33(6):30-35.

[9]Xiaoyu Zhan， Chunlei Song. Research and design of MEMS SINS/GPS ntegrated navigation system based on adaptive filter [J].Proceedings of the 2nd International Conference on Systems Engineering and Modeling，2013(1):229-233.

Improved CKF application in SINS/GPS integrated navigation system

HUANG Chunmei,WANG Jie,ZOU Huiying

(Chool of Computer Science & Engineering, Changchun University of Technology, Changchun 130012， China)

Abstract：An improved Cubature Kalman Filter (CKF) model using Innovation-based Adaptive Estimation (IAE) is implemented to reduce abnormal dynamic error of CKF. The innovation information is used to on-line estimate and correct noise statistical characteristics, automatically adjust measurement. Both the CKF and improved CKF are applied to the SINS/GPS integrated navigation system, and the simulation results show that the improved CKF algorithm can improve the adaptability to different stochastic disturbances, decrease filtering error and improve the calculation accuracy.

Key words：Cubature Kalman Filter (CKF); SINS/GPS integrated navigation; nonlinear system; adaptive filtering.

收稿日期：2016-01-07

作者簡(jiǎn)介：黃春梅(1965-)，女，漢族，吉林長(zhǎng)春人，長(zhǎng)春工業(yè)大學(xué)副教授，博士，主要從事嵌入式控制方向研究,E-mail:huangchunmei@ccut.edu.cn.

DOI:10.15923/j.cnki.cn22-1382/t.2016.2.07

中圖分類號(hào)：TP 273

文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1674-1374(2016)02-0134-07