賈鶴鳴，楊澤文，宋文龍

(東北林業(yè)大學(xué) 機(jī)電工程學(xué)院，哈爾濱 150040)

?

基于中心差分卡爾曼濾波的初始對(duì)準(zhǔn)方法研究

賈鶴鳴，楊澤文，宋文龍*

(東北林業(yè)大學(xué) 機(jī)電工程學(xué)院，哈爾濱 150040)

由于建立的大方位失準(zhǔn)角下的捷聯(lián)慣性導(dǎo)航系統(tǒng)誤差模型具有非線(xiàn)性的特點(diǎn)，本文選用中心差分卡爾曼濾波(CDKF)方法對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)的控制對(duì)象進(jìn)行初始對(duì)準(zhǔn)仿真。并與無(wú)跡卡爾曼濾波(UKF)和擴(kuò)展卡爾曼濾波(EKF)兩種濾波方式進(jìn)行比較，最后得出CDKF可以提高系統(tǒng)的圖像和目標(biāo)的濾波精度的結(jié)論，并且不需要具體模型計(jì)算出解析方程，節(jié)約了計(jì)算復(fù)雜的具有驅(qū)動(dòng)約束方程的雅可比(Jacobian)矩陣的時(shí)間。最后的仿真結(jié)果證明CDKF對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法明顯優(yōu)于EKF和UKF，同時(shí)具有較高的精度和收斂性，能夠滿(mǎn)足在非線(xiàn)性模型下的系統(tǒng)對(duì)導(dǎo)航的要求。

大方位失準(zhǔn)角；捷聯(lián)慣導(dǎo)；初始對(duì)準(zhǔn)；CDKF

0 引 言

SINS(捷聯(lián)慣性導(dǎo)航系統(tǒng))中所不可或缺的一項(xiàng)技術(shù)是初始對(duì)準(zhǔn)，在初始對(duì)準(zhǔn)過(guò)程中的控制精度和所用時(shí)間會(huì)影響到整個(gè)系統(tǒng)的工作靜態(tài)和動(dòng)態(tài)性能[1-5]。擴(kuò)展卡爾曼濾波(EKF)方法在失準(zhǔn)角誤差模型的非線(xiàn)性系統(tǒng)應(yīng)用很多[6-7]，然而這種濾波方法存在精確度和準(zhǔn)確性的缺點(diǎn)，例如在將非線(xiàn)性方程問(wèn)題變成線(xiàn)性方程問(wèn)題來(lái)的求解過(guò)程中，通常會(huì)忽略泰勒級(jí)數(shù)展開(kāi)式中部分的高階項(xiàng)，因此造成EKF濾波方式出現(xiàn)了較大的線(xiàn)性化誤差，從而降低濾波精度甚至?xí)菇Y(jié)果發(fā)散，造成工作狀態(tài)難以穩(wěn)定，并且推導(dǎo)具有驅(qū)動(dòng)約束方程的雅可比(Jacobian)矩陣需要的時(shí)間長(zhǎng)，因此在實(shí)際中很難廣泛應(yīng)用。

針對(duì)EKF的不足，近年來(lái)人們提出了Sigma點(diǎn)卡爾曼濾波(SPKF)方法，其分析系統(tǒng)的性能較EKF比有較大的改善。SPKF主要分為兩種，一種是中心差分卡爾曼濾波(CDKF)，另一種是無(wú)跡卡爾曼濾波(UKF)。 CDKF 最早由ITO[8]和Norgaard[9]等人提出，他們基于卡爾曼在NASA埃姆斯研究中心時(shí)產(chǎn)生靈感發(fā)現(xiàn)的濾波方法，使CDKF避開(kāi)了EKF濾波時(shí)需要被控系統(tǒng)模型計(jì)算出具體解析方程的劣勢(shì)，并能隔離隨機(jī)變量噪聲，具有較低的線(xiàn)性化誤差和高定位準(zhǔn)確度的優(yōu)點(diǎn)。另外，CDKF比 UKF的理論精度高。因此CDKF方法在目前導(dǎo)航、航空領(lǐng)域中有諸多的應(yīng)用，效果也十分完善。

本文首先介紹了中心差分卡爾曼濾波的原理，然后從基于歐拉平臺(tái)誤差角描述實(shí)際地理坐標(biāo)系再到分析其與計(jì)算地理坐標(biāo)系的變換關(guān)系，最后創(chuàng)建一個(gè)SINS在失準(zhǔn)角為大方位條件下的非線(xiàn)性模型；研究CDKF在SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)條件下濾波效果。本文分別利用了EKF、UKF和CDKF三種方法進(jìn)行了仿真驗(yàn)證并進(jìn)行比較，仿真結(jié)果進(jìn)一步證明了中心差分卡爾曼濾波(CDKF)方法顯著優(yōu)于EKF和UKF。是一種更為優(yōu)越，實(shí)用性強(qiáng)的非線(xiàn)性濾波方法。

1 SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)誤差模型

上者的轉(zhuǎn)移關(guān)系可用以下列矩陣來(lái)表示：

(1)

(2)

(3)

(4)

當(dāng)φx，φy很小時(shí)cosφx=cosφy=1，sinφx≈φx，sinφy≈φy，則

(5)

(6)

1.1 速度誤差方程

理論上，SINS的速度微分方程表示為：

(7)

不忽略外界干擾情況下，SINS的速度微分方程為：

(8)

(9)

1.2 姿態(tài)角誤差方程

(10)

(11)

失準(zhǔn)角運(yùn)動(dòng)方程為：

(12)

(13)

(14)

(15)

1.3 建立非線(xiàn)性捷聯(lián)慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)模型

假設(shè)計(jì)算出的陀螺儀的誤差漂移εb是一個(gè)恒定值；加速度計(jì)的誤差是恒定值零偏b；假定在靜基座快速初始對(duì)準(zhǔn)條件下，所在位置是恒定不變的且已知的，此刻滿(mǎn)足=[0 ωiecosL ωiesinL]Tvt=0；其中R為地球半徑，L為緯度，忽略重力誤差項(xiàng)δgt以及不考慮δvz，則可得狀態(tài)方程為：

(16)

令狀態(tài)向量X為：

用捷聯(lián)慣性導(dǎo)航系統(tǒng)兩程度速度誤差X=δVt作為觀測(cè)誤差并測(cè)量，建立非線(xiàn)性對(duì)準(zhǔn)模型：

(17)

其中，f(X)的表達(dá)式可以參考公式(16)進(jìn)行表示，G是10×5維的矩陣系數(shù)，G(1,1)=G(2,2)=G(3,3)=G(4,4)=G(5,5)=1，H代表量測(cè)矩陣H=[02×3∶I2×2∶02×5]，V代表量測(cè)噪聲。

2 基于CDKF的狀態(tài)估計(jì)

2.1 中心差分變換 中心差分變換是研究在SINS在失準(zhǔn)角為大方位條件下中心差分卡爾曼濾波濾波器的基礎(chǔ)，同時(shí)它也是一種以插值理論為基礎(chǔ)的高效的非線(xiàn)性變換方法，是SPKF的一種，可以用來(lái)求解經(jīng)過(guò)非線(xiàn)性變換后的一些統(tǒng)計(jì)學(xué)量，例如均值和方差。研究CDKF的切入點(diǎn)是借助sterling差值定理，進(jìn)而求取逼近多項(xiàng)式非線(xiàn)性方程的導(dǎo)數(shù)，節(jié)約了進(jìn)行復(fù)雜求導(dǎo)運(yùn)算的時(shí)間，它運(yùn)用了中心差分方法代替泰勒展開(kāi)式中指數(shù)為1和2[10]。

y=f(x)。

(18)

(19)

(20)

(21)

(22)

(23)

(24)

(25)

(26)

(27)

其中Sxi表示柯勒斯基(cholesky)分解中Sx的第i列。

2.2 中心差分卡爾曼濾波方法研究

CDKF雖然表達(dá)形式與EKF類(lèi)似，實(shí)際上卻是利用先驗(yàn)分布構(gòu)造一組確定性采樣點(diǎn) Sigma 點(diǎn)，充分考慮了高斯隨機(jī)變量的統(tǒng)計(jì)特性[13-14]，并用經(jīng)過(guò)線(xiàn)性回歸變換后得到的Sigma(一組確定性采樣點(diǎn))來(lái)表示實(shí)驗(yàn)狀態(tài)的后驗(yàn)分布。針對(duì)非線(xiàn)性濾波的難題[15-16]CDKF 采用了中心差分變換方法，并且發(fā)現(xiàn)CDKF較傳統(tǒng)EKF算法具有濾波精度高，過(guò)程較穩(wěn)定的優(yōu)點(diǎn)，且不需要計(jì)算具有約束方程的Jacobian；節(jié)約了大量的時(shí)間，而與UKF濾波算法相比較，其又具有精度高，對(duì)狀態(tài)協(xié)方差敏感性要低的優(yōu)點(diǎn)。同時(shí)，在CDKF中，當(dāng)出現(xiàn)系統(tǒng)測(cè)量噪聲和系統(tǒng)本身自有噪聲均為復(fù)雜加性噪聲的情況時(shí)，為了減輕濾波計(jì)算，可不進(jìn)行狀態(tài)增廣。由此可見(jiàn)，CDKF與上面兩種濾波算法相比而言，在處理SINS大方位失準(zhǔn)角初態(tài)對(duì)準(zhǔn)方面問(wèn)題中具有相當(dāng)明顯的優(yōu)勢(shì)。

設(shè)非線(xiàn)性系統(tǒng)為：

(28)

其中，k時(shí)刻的狀態(tài)向量為xk，觀察向量用yk，wk和vk為兩兩互不關(guān)聯(lián)的均值為0的高斯噪聲。針對(duì)上述非線(xiàn)性模型，利用中心差分變換方法進(jìn)行狀態(tài)估計(jì)，推導(dǎo)出中心差分卡爾曼濾波的算法[17]如下：

(1)初始化

(29)

(30)

(2)確定權(quán)值

(31)

(32)

(3)確定sigma點(diǎn)集

(33)

(4)時(shí)間更新

(34)

(35)

(36)

(5)sigma點(diǎn)更新

(37)

(6)量測(cè)更新

yk/k-1=h(χk/k-1)。

(38)

(39)

(40)

(41)

(42)

(43)

(44)

3 仿真研究

3.1 仿真條件

在靜基座初始對(duì)準(zhǔn)狀態(tài)下，假定當(dāng)前的初態(tài)失準(zhǔn)角度φx=φy=0.6°，φz=10°；陀螺儀常值漂移為0.02°/h；隨機(jī)漂移為0.01°/h；加速度計(jì)零偏為100 μg；隨機(jī)偏差為50 μg；速度測(cè)量誤差為0.1 m/s；當(dāng)?shù)氐乩砭暥葹?5.779 6°，則P(0)、Q、R可表示為：

P(0)=diag{(0.6°)2,(0.6°)2,(10°)2,(0.1 m/s)2,(0.1 m/s)2,(0.02°/h)2,(0.02°/h)2,(0.02°/h)2,(100 μg)2,(100 μg)2}

Q=diag{(0.01°/h)2,(0.01°/h)2,(0.01°/h)2,(50 μg)2,(50 μg)2,0,0,0,0,0}

R=diag{(0.1 m/s)2,(0.1 m/s)2}。

得到上述仿真條件，分別采用擴(kuò)展卡爾曼濾波器(EKF)、無(wú)損卡爾曼濾波(UKF)和中心差分卡爾曼濾波(CDKF)三種方法對(duì)系統(tǒng)進(jìn)行濾波仿真比較，仿真時(shí)間均設(shè)定為300 s。

3.2 仿真結(jié)果分析

失準(zhǔn)角估計(jì)誤差的對(duì)比情況見(jiàn)表1。系統(tǒng)的仿真結(jié)果如圖1和圖2所示。

表1 大方位失準(zhǔn)角的估計(jì)誤差Tab.1 Estimation error of large azimuth misalignment

因?yàn)楸毕蚝蜄|向的水平失準(zhǔn)角仿真估計(jì)結(jié)果類(lèi)似，所以圖中只列出了系統(tǒng)中北向估計(jì)的結(jié)果。從北向水平失準(zhǔn)角的曲線(xiàn)圖和大方位失準(zhǔn)角的誤差表很容易得出，每個(gè)濾波算法對(duì)在兩方向失準(zhǔn)角條件下的仿真結(jié)論基本相當(dāng)，收斂速度都很快，均能得到很好很穩(wěn)定的濾波結(jié)果。從圖2和表1得出，UKF和CDKF兩者對(duì)大方位失準(zhǔn)角的估計(jì)，收斂速度、時(shí)變系統(tǒng)跟蹤速度與收斂精度優(yōu)于EKF，首要原因是 EKF在線(xiàn)性化濾波過(guò)程中存在一階截?cái)鄦?wèn)題，會(huì)產(chǎn)生截?cái)嗾`差，造成方位失準(zhǔn)角誤差的急劇增大，而UKF因?yàn)樗惴ㄖ写嬖诙A截?cái)鄦?wèn)題，會(huì)使方位失準(zhǔn)角誤差增加的更快，所以效果也不如CDKF。CDKF與UKF相比，CDKF具有在相同增長(zhǎng)時(shí)，接近穩(wěn)定值的速度較快和濾波精度高的特點(diǎn)。因此CDKF在三種濾波方式中擁有相當(dāng)顯著實(shí)用的優(yōu)點(diǎn)。

圖1 北向水平失準(zhǔn)角的誤差曲線(xiàn)Fig.1 Estimation error curves of north misalignment

圖2 天向失準(zhǔn)角預(yù)計(jì)的誤差曲線(xiàn)Fig.2 Estimation error curves of azimuth misalignment

4 結(jié) 論

本文內(nèi)容深入研究CDKF濾波，并在SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)中得到了廣泛的應(yīng)用，利用其具有精度高，穩(wěn)定性好的優(yōu)點(diǎn)，有效解決了復(fù)雜的誤差模型具有的非線(xiàn)性問(wèn)題。以上仿真出的圖形和表格表現(xiàn)出CDKF比EKF具有更高濾波精度，同時(shí)節(jié)約了求解復(fù)雜的雅可比矩陣的時(shí)間，提高了系統(tǒng)可靠性，這些優(yōu)點(diǎn)都將是CDKF成為更主流濾波方法的重要因素。CDKF與UKF相比，CDKF不但具有濾波精度高和保證收斂速度比較快的特點(diǎn)，同時(shí)減少了工作量，使數(shù)值的穩(wěn)定程度提高。仿真結(jié)果證實(shí)了CDKF的確在基于中心差分卡爾曼濾波對(duì)大方位失準(zhǔn)角初始對(duì)準(zhǔn)問(wèn)題中較其他仿真方法而言?xún)?yōu)越性非常的顯著，實(shí)現(xiàn)功能更加容易便捷，實(shí)際應(yīng)用愈加廣泛，是一種研究非線(xiàn)性系統(tǒng)濾波十分高效的算法。

[1]Ito K，Xiong K.Gaussian filters for nonlinear filtering problems[J].IEEE Transactions on Automatic Control，2000,45(5)：910-927.

[2]N?rgaard M，Poulsen N K，Ravn O.Advances in derivative-free state estimation for nonlinear systems[R].Informatics and Mathematical Modelling，Technical University of Denmark，DTU，1998.

[3]劉國(guó)海，李康吉.基于PDA的GPS定位精度提高方法[J].江蘇大學(xué)學(xué)報(bào)：自然科學(xué)版.2005，26(5)：448-452.

[4]Norgaard M，Pouisen N K，Ravn O.New Developments in State Estimation for Nonlinear System[J]．Automatica，2000，36(11)：1627-1638.

[5]Van Der Merwe R.Sigma-point Kalman filters for probabilistic inference in dynamic state-space models[D].Oreg：Oregon Health & Science University，2004.

[6]王海勃，陳紅林，韓惠珍.CDKF在GPS/SINS組合導(dǎo)航系統(tǒng)非線(xiàn)性模型中的應(yīng)用[J].現(xiàn)在電子技術(shù)，2011，34(11)：19-23.

[7]龍 瑞，秦永元，夏家和.CDKF在捷聯(lián)慣導(dǎo)系統(tǒng)大失準(zhǔn)角初始對(duì)準(zhǔn)的應(yīng)用[J].西北工業(yè)大學(xué)學(xué)報(bào)，2010，28(3)：364-368.

[8]嚴(yán)恭敏，嚴(yán)衛(wèi)生，徐德民.簡(jiǎn)化UKF濾波在SINS大失準(zhǔn)角初始對(duì)準(zhǔn)中的應(yīng)用[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2008，16(3)：253-264.

[9]黃鳳榮，孫偉強(qiáng)，翁海娜.基于UKF的旋轉(zhuǎn)式SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)方法[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2010，18(5)：513-517.

[10]Gao W，Xu B，Sun H，et al.Application of nonlinear filtering for SINS initial alignment[C].2006 International Conference on Mechatronics and Automation.IEEE，2006：2259-2263.

[11]孫 楓，唐李軍.Cubature卡爾曼濾波-卡爾曼濾波算法[J].控制與決策，2012，27(10)：1561-1565.

[12]趙 奇.卡爾曼濾波在GPS定位中的研究與實(shí)現(xiàn)[D].成都：電子科技大學(xué).2013.

[13]錢(qián)華明，葛 磊，彭 宇.多漸消因子卡爾曼濾波及其在SINS初始對(duì)準(zhǔn)中的應(yīng)用[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2012，20(3)：287-291.

[14]楊 丹.卡爾曼濾波器設(shè)計(jì)及其應(yīng)用研究[D].湘潭：湘潭大學(xué).2014.

[15]杜黨波，張 偉，胡昌華，等.含缺失數(shù)據(jù)的小波-卡爾曼濾波故障預(yù)測(cè)方法[J].自動(dòng)化學(xué)報(bào)，2014，40(10)：2115-2125.

[16]韓 萍，桑威林，石慶研.一種新型非線(xiàn)性卡爾曼濾波方法[J].儀器儀表學(xué)報(bào)，2015，36(3)：632-638.

[17]李方能，許江寧，亓洪標(biāo) 基于邊緣采樣UKF濾波的捷聯(lián)慣導(dǎo)初始對(duì)準(zhǔn)方法[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2014，22(5)：612-618.

Study on Initial Alignment Method Basedon Central Difference Calman Filter

Jia Heming ，Yang Zewen，Song Wenlong*

(College of Mechanical and Electrical Engineering，Northeast Forestry University，Heilongjiang，Harbin 150040，China)

Because the error model of the strapdown inertial navigation system is of nonlinear characteristics under the established large azimuth misalignment，the Central Difference Kalman Filter (CDKF) method was used to conduct the initial alignment simulation for controlling object of the strapdown inertial navigation system in this paper.Compared with the extended Kalman filter (EKF) and the unscented Kalman filter (UKF)，CDKF would improve the filtering precision of the system image and the objective，and did not need specific models to calculate the analytical equation，which saved the time for computing Jacobian matrix of driven complex constraint equations.The final simulation results demonstrated that the use of CDKF for the optimal estimation of system sate was better than EKF and UKF methods，the CDKF algorithm was of the high precision and convergence and could satisfy the system requirements for navigation under the nonlinear model.

Large azimuth misalignment；strapdown inertial navigation system；initial alignment；CDKF

2016-03-08

中央高?；究蒲袠I(yè)務(wù)費(fèi)專(zhuān)項(xiàng)資金資助項(xiàng)目(2572014BB03)；國(guó)家自然科學(xué)基金(31470714)

賈鶴鳴，博士，副教授。研究方向：非線(xiàn)性系統(tǒng)控制理論與應(yīng)用、智能控制與濾波技術(shù)。

*通信作者：宋文龍，博導(dǎo)，教授。研究方向：控制系統(tǒng)的檢測(cè)與濾波技術(shù)。E-mail：wlsong@126.com

賈鶴鳴，楊澤文，宋文龍.基于中心差分卡爾曼濾波的初始對(duì)準(zhǔn)方法研究[J].森林工程，2016，32(6)：66-70.

TP 273

A

1001-005X(2016)06-0066-05