SR-CDKF在組合導(dǎo)航直接法濾波中的應(yīng)用

逯 嶠,馬國(guó)梁,張 平,葛敬飛

(1.南京理工大學(xué) 能源與動(dòng)力工程學(xué)院,江蘇 南京 210094;2.中國(guó)人民解放軍63863部隊(duì),吉林 白城137001;3.中國(guó)人民解放軍65156部隊(duì),遼寧 凌源 122521)

SR-CDKF在組合導(dǎo)航直接法濾波中的應(yīng)用

逯 嶠1,馬國(guó)梁1,張 平2,葛敬飛3

(1.南京理工大學(xué) 能源與動(dòng)力工程學(xué)院,江蘇 南京 210094;2.中國(guó)人民解放軍63863部隊(duì),吉林 白城137001;3.中國(guó)人民解放軍65156部隊(duì),遼寧 凌源 122521)

為了解決SINS/GPS組合導(dǎo)航系統(tǒng)姿態(tài)、速度和位置等導(dǎo)航參數(shù)的非線性估計(jì)問(wèn)題，提出了一種基于平方根中心差分卡爾曼濾波(SR-CDKF)的直接式濾波估計(jì)方法。系統(tǒng)狀態(tài)方程選取慣導(dǎo)力學(xué)編排方程,系統(tǒng)狀態(tài)向量和觀測(cè)向量分別選取SINS導(dǎo)航參數(shù)和GPS輸出參數(shù),構(gòu)建了SR-CDKF濾波器。該濾波器可以使導(dǎo)航參數(shù)動(dòng)態(tài)過(guò)程得到直接反映,并使得直接式濾波計(jì)算流程得以實(shí)現(xiàn)。以MATLAB中areoblk_HL20模塊為基礎(chǔ)構(gòu)建了仿真系統(tǒng)并進(jìn)行了仿真。仿真結(jié)果表明,該算法具有較高的濾波精度、良好的濾波收斂性和穩(wěn)定性,能使非線性模型下SINS/GPS組合導(dǎo)航系統(tǒng)的導(dǎo)航要求得到滿足。

組合導(dǎo)航;中心差分;卡爾曼濾波;直接校正

SINS/GPS組合導(dǎo)航系統(tǒng)是目前一種比較理想且應(yīng)用廣泛的導(dǎo)航系統(tǒng)。組合導(dǎo)航系統(tǒng)信息處理方法有很多種,Kalman濾波及其擴(kuò)展方法是目前應(yīng)用比較廣泛的一種方法[1]。根據(jù)所選取的被估計(jì)系統(tǒng)狀態(tài)不同,估計(jì)方法分為以導(dǎo)航參數(shù)為狀態(tài)變量的直接法和以導(dǎo)航參數(shù)誤差量為狀態(tài)變量的間接法2種[2]。如果采用直接估計(jì)法,導(dǎo)航參數(shù)的動(dòng)態(tài)變化過(guò)程可以被系統(tǒng)方程直接而準(zhǔn)確地描述,在解算導(dǎo)航參數(shù)的同時(shí)又進(jìn)行了濾波估計(jì)計(jì)算,比間接法簡(jiǎn)化了計(jì)算。

毫無(wú)疑問(wèn),實(shí)際的SINS/GPS組合導(dǎo)航系統(tǒng)的數(shù)學(xué)模型是非線性方程,而擴(kuò)展卡爾曼濾波(EKF)是解決非線性系統(tǒng)估計(jì)問(wèn)題應(yīng)用最多的一種方法。EKF的主要思路是采用Taylor展開(kāi)將非線性方程線性化后再用標(biāo)準(zhǔn)Kalman濾波作處理,因此EKF的估計(jì)誤差會(huì)隨系統(tǒng)非線性程度增強(qiáng)而加大,甚至有時(shí)會(huì)導(dǎo)致計(jì)算發(fā)散[3-4],另外EKF需要求Jacobian矩陣,過(guò)程繁瑣且易出錯(cuò)。

針對(duì)EKF的這些不足,由于一個(gè)高斯分布的逼近比一個(gè)非線性函數(shù)的逼近更容易,人們提出了Sigma-point卡爾曼濾波(SPKF)[5]。SPKF主要包括中心差分卡爾曼濾波(CDKF)和無(wú)跡卡爾曼濾波(UKF)。其中,CDKF的理論精度比UKF稍高,避開(kāi)了復(fù)雜的Jacobian矩陣計(jì)算,更易于實(shí)現(xiàn)[6-8]。SR-CDKF不僅繼承了CDKF的優(yōu)點(diǎn),而且通過(guò)直接更新?tīng)顟B(tài)變量協(xié)方差矩陣的平方根來(lái)進(jìn)行濾波,計(jì)算量有所減少,提高了數(shù)值計(jì)算的穩(wěn)定性[9-12],因此本文提出將SR-CDKF方法應(yīng)用到SINS/GPS組合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)估計(jì)中。

1 SR-CDKF算法原理及步驟

1.1 SR-CDKF原理

SR-CDKF的理論基礎(chǔ)是中心差分變換,中心差分變換根據(jù)數(shù)學(xué)中的插值理論來(lái)對(duì)非線性函數(shù)進(jìn)行近似,將Taylor級(jí)數(shù)展開(kāi)式中的一階和二階導(dǎo)數(shù)用中心差分來(lái)取代,非線性方程的導(dǎo)數(shù)用基于Sterling插值公式的多項(xiàng)式逼近,由此來(lái)解決解析微分運(yùn)算問(wèn)題[13]。

1.2 SR-CDKF濾波步驟

設(shè)狀態(tài)方程和量測(cè)方程具有一般形式:

xk=F(xk-1,uk-1,vk-1)

yk=H(xk,nk)

式中:xk為狀態(tài)變量,uk為系統(tǒng)輸入?yún)?shù),vk和nk是期望為0且互不相關(guān)的高斯白噪聲,它們的協(xié)方差陣分別為Rv和Rn,即有:

設(shè)xk,vk和nk的維數(shù)分別為L(zhǎng)x、Lv和Ln,則執(zhí)行SR-CDKF濾波計(jì)算的步驟如下[14]。

①確定濾波初值。

當(dāng)k=0時(shí),濾波初值為

式中:chol{*}表示對(duì)矩陣進(jìn)行cholesky分解。

②計(jì)算時(shí)間更新所需要的sigma點(diǎn)集。

③確定權(quán)值。

④時(shí)間更新。

式中:qr(A)表示對(duì)矩陣A進(jìn)行QR分解后得到的R矩陣下三角部分。

⑤計(jì)算量測(cè)更新所需要的sigma點(diǎn)。

⑥量測(cè)更新。

式中:

⑦計(jì)算增益和更新?tīng)顟B(tài)。

式中:cholupdate{*}表示矩陣的cholesky更新。

2 SINS/GPS組合導(dǎo)航非線性模型

2.1 直接式SINS/GPS組合導(dǎo)航系統(tǒng)濾波器結(jié)構(gòu)

在直接式SINS/GPS組合導(dǎo)航濾波器中[15],首先由慣性器件和GPS分別測(cè)量運(yùn)載體的運(yùn)動(dòng)狀態(tài),其次SR-CDKF濾波器接受慣性器件測(cè)得的角速率、比力和GPS測(cè)得的位置速度信息,經(jīng)SR-CDKF濾波器處理計(jì)算后輸出組合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)最優(yōu)估計(jì)值,如圖1所示。

2.2 組合系統(tǒng)狀態(tài)方程

本文根據(jù)文獻(xiàn)[16],捷聯(lián)慣導(dǎo)系統(tǒng)(SINS)導(dǎo)航坐標(biāo)系(n系)選用“北東地”地理坐標(biāo)系建立的非線性姿態(tài)、速度和位置方程如下。

姿態(tài)方程為

(1)

(2)

陀螺儀測(cè)量值模型為

(3)

速度方程為

(4)

加速度計(jì)的測(cè)量模型為

(5)

位置方程為

(6)

式中:RN和RM分別為參考橢球卯酉圈和子午圈曲率半徑;L,λ,h分別為載體所在的緯度、經(jīng)度和高度。

④關(guān)于陀螺常值漂移ε和加速度計(jì)零偏Δ,本文中用隨機(jī)常數(shù)來(lái)表示:

(7)

以式(1)～式(7)為狀態(tài),選取組合導(dǎo)航系統(tǒng)方程狀態(tài)變量x(t)為

x(t)=(φθψvNvEvDLλh
εxεyεzΔxΔyΔz)T

因此,系統(tǒng)狀態(tài)方程可以表示為

2.3 觀測(cè)方程

量測(cè)值y取GPS輸出的速度參數(shù)和位置參數(shù),即y=(vN,GPSvE,GPSvD,GPSLGPSλGPShGPS)T,基于前面所選取的系統(tǒng)方程狀態(tài)變量x和量測(cè)值y,觀測(cè)方程可寫(xiě)成

yk=H(xk,nk)=HPVxk+nk

式中:HPV=(O6×3I6×3O6×6),yk和xk分別為y和x在k時(shí)刻的值,nk=(nvNnvEnvDnLnλnh)T為測(cè)量噪聲。

3 仿真分析

1)真值的生成。

為了對(duì)SR-CDKF濾波算法作進(jìn)一步分析,本文采用MATLAB中aeroblk_HL20模塊中的子模塊6DOF(Euler Angles)產(chǎn)生的數(shù)據(jù)作為原始數(shù)據(jù)。由于該模塊輸出的運(yùn)動(dòng)參數(shù)均是在忽略地球自轉(zhuǎn)和地球曲率的假設(shè)下得出的,因此如果采用這些數(shù)據(jù)作為參考真值,需要將前述的慣性導(dǎo)航方程(即系統(tǒng)狀態(tài)方程)簡(jiǎn)化,方程的非線性也會(huì)減弱,體現(xiàn)不出SR-CDKF方法相對(duì)于其他非線性濾波方法的優(yōu)勢(shì)。另外,簡(jiǎn)化的導(dǎo)航方程由于忽略了地球曲率,因此只適合于短距離導(dǎo)航,而完整的導(dǎo)航方程無(wú)此限制,拓寬了參考真值的數(shù)據(jù)適用范圍。綜上所述,需要將原始數(shù)據(jù)進(jìn)行轉(zhuǎn)化,以得到完整的慣性導(dǎo)航方程下的運(yùn)載體真實(shí)的運(yùn)動(dòng)參數(shù),具體的轉(zhuǎn)換思路和方法參見(jiàn)文獻(xiàn)[16]。

2)仿真初始條件。

設(shè)定SINS和GPS的采樣周期分別為0.01 s和0.1 s,仿真時(shí)間70 s,設(shè)置仿真初始條件:初始滾轉(zhuǎn)角、俯仰角、偏航角誤差分別為3°,3°和5°;初始經(jīng)度、緯度、高度誤差為10 m,10 m和20 m;初始北向、東向、地向速度誤差均為0.3 m/s。陀螺儀三軸白噪聲均方差均為0.1°/s;三軸常值漂移分別為-0.1 °/s,0.1 °/s和-0.1 °/s;加速計(jì)白噪聲均方差均為0.05 m/s2;三軸零偏分別為-0.01 m/s2,0.01 m/s2和-0.02 m/s2;GPS位置和速度精度分別取5 m和0.1 m/s,初始協(xié)方差矩陣P為對(duì)角陣,除了對(duì)角線元素,其他元素均為0,其對(duì)角線元素為

P(1,1)=P(2,2)=P(3,3)=52
P(4,4)=P(5,5)=P(6,6)=0.32
P(7,7)=P(8,8)=P(9,9)=102
P(10,10)=P(11,11)=P(11,12)=0.22
P(13,13)=P(14,14)=P(15,15)=0.32

3)仿真結(jié)果。

在前文給出的仿真條件下,由于UKF、CDKF仿真曲線與SR-CDKF仿真曲線比較接近,在同一幅圖中會(huì)相互重疊,不能明顯看出優(yōu)劣,因此只給出了SINS/GPS組合導(dǎo)航系統(tǒng)基于SR-CDKF的濾波仿真曲線,如圖2～圖6所示。

從圖2～圖6可看出,組合導(dǎo)航系統(tǒng)的運(yùn)載體姿態(tài)角參數(shù)在10 s左右就收斂了,速度和位置參數(shù)則收斂得更快,陀螺和加速度計(jì)零偏基本都在20 s內(nèi)收斂,且各參數(shù)的計(jì)算均未發(fā)散,表明計(jì)算穩(wěn)定,從一定程度上驗(yàn)證了SR-CDKF方法的數(shù)值穩(wěn)定性。從圖中還可看出,在有測(cè)量噪聲和固定零偏的情況下,各導(dǎo)航參數(shù)(姿態(tài)、速度、位置等)上下波動(dòng)范圍較小,表明濾波計(jì)算的誤差比較小。

在仿真條件下,UKF、CDKF和SR-CDKF 3種估計(jì)方法得到的運(yùn)動(dòng)參數(shù)估計(jì)誤差均方差如表1～表5所示。

表1 姿態(tài)角估計(jì)誤差標(biāo)準(zhǔn)差比較

表2 速度估計(jì)誤差標(biāo)準(zhǔn)差比較

表3 位置估計(jì)誤差標(biāo)準(zhǔn)差比較

表4 陀螺儀漂移估計(jì)值標(biāo)準(zhǔn)差比較

表5 加速度計(jì)零偏估計(jì)值標(biāo)準(zhǔn)差比較

由誤差數(shù)據(jù)表1～表5看出,CDKF比UKF估計(jì)誤差標(biāo)準(zhǔn)差要小,精度稍微好一些,驗(yàn)證了CDKF理論精度稍高于UKF這一事實(shí)。而總體上看SR-CDKF對(duì)各個(gè)運(yùn)動(dòng)參數(shù)的估計(jì)誤差的標(biāo)準(zhǔn)差均小于UKF和CDKF,即估計(jì)精度均高于CDKF和UKF,同時(shí)也表明SR-CDKF的數(shù)值穩(wěn)定性好于UKF和CDKF。綜上,SR-CDKF能提高非線性系統(tǒng)的濾波精度,使復(fù)雜的非線性系統(tǒng)濾波估計(jì)問(wèn)題得以有效解決,同時(shí)使組合導(dǎo)航系統(tǒng)具有較高的導(dǎo)航精度。

4 結(jié)論

本文采用平方根中心差分卡爾曼濾波方法,研究了直接式的SINS/GPS組合導(dǎo)航估計(jì)問(wèn)題,分別以姿態(tài)、速度、位置和零偏等導(dǎo)航參數(shù)和GPS的速度和位置為系統(tǒng)狀態(tài)向量和觀測(cè)向量,將慣導(dǎo)力學(xué)編排方程作為系統(tǒng)狀態(tài)方程,以MATLAB中aeroblk_HL20模塊產(chǎn)生的數(shù)據(jù)經(jīng)過(guò)適當(dāng)轉(zhuǎn)換后作為仿真的參考真值,并在此基礎(chǔ)上生成SINS和GPS的測(cè)量值,經(jīng)濾波器估計(jì)后得到系統(tǒng)輸出,即導(dǎo)航參數(shù)的最優(yōu)估計(jì)值。仿真結(jié)果表明,SR-CDKF可以較好地處理非線性模型的濾波問(wèn)題,擺脫了傳統(tǒng)的對(duì)非線性方程進(jìn)行Taylor展開(kāi)的線性化過(guò)程,計(jì)算量較小,且能保證濾波過(guò)程的數(shù)值穩(wěn)定,簡(jiǎn)化了濾波過(guò)程,使系統(tǒng)導(dǎo)航精度得到了提高。

[1] 付心如,徐愛(ài)功,孫偉.抗差自適應(yīng)UKF的INS/GNSS組合導(dǎo)航算法[J].導(dǎo)航定位學(xué)報(bào),2017,5(2):111-116.

FU Xin-ru,XU Ai-gong,SUN Wei.An INS/GNSS integrated navigation algorithm based on robust adaptive UKF[J].Journal of Navigation and Positioning,2017,5(2):111-116.(in Chinese)

[2] 孟陽(yáng),高社生,高兵兵,等.基于UKF的INS/GNSS/CNS組合導(dǎo)航最優(yōu)數(shù)據(jù)融合方法[J].中國(guó)慣性技術(shù)學(xué)報(bào),2016,24(6):746-751.

MENG Yang,GAO She-sheng,GAO Bing-bing,et al.UKF-based optimal data fusion method for integrated INS/GNSS/CNS[J].Journal of Chinese Inertial Technology,2016,24(6):746-751.(in Chinese)

[3] 袁贛南,趙自超,賈鶴鳴,等.CDKF算法在四旋翼飛行器組合導(dǎo)航系統(tǒng)中應(yīng)用[J].吉林大學(xué)學(xué)報(bào)(信息科學(xué)版),2015,33(2):161-167.

YUAN Gan-nan,ZHAO Zi-chao,JIA He-ming,et al.CDKF algorithm applied to quadrotor integrated navigation system[J].Journal of Jilin University(Information Science Edition),2015,33(2):161-167.(in Chinese)

[4] 潘加亮,熊智,王麗娜,等.一種簡(jiǎn)化的發(fā)射系下SINS/GPS/CNS組合導(dǎo)航系統(tǒng)無(wú)跡卡爾曼濾波算法[J].兵工學(xué)報(bào),2015,36(3):484-491.

PAN Jia-liang,XIONG Zhi,WANG Li-na,et al.A simplified UKF algorithm for SINS/GPS/CNS integrated navigation system in launch inertial coordinate system[J].Acta Armamentarii,2015,36(3):484-491.(in Chinese)

[5] JULIER S,UHLMANN J,DURRANT-WHYTE H F.A new method for the nonlinear transformation of means and covariances in filters and estimators[J].IEEE Transactions on Automatic Control,2000,45(3):477-482.

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

JIA He-ming,YANG Ze-wen,SONG Wen-long.Study on initial alignment method based on central difference Calman filter[J].Forest Engineering,2016,32(6):66-70.(in Chinese)

[7] 張召友,郝燕玲,吳旭.3種確定性采樣非線性濾波算法的復(fù)雜度分析[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào),2013,45(12):111-115.

ZHANG Zhao-you,HAO Yang-ling,WU Xu.Complexity analysis of three deterministic sampling nonlinear filtering algorithms[J].Journal of Harbin Institute of Technology,2013,45(12):111-115.(in Chinese)

[8] 韓萍,干浩亮,何煒琨,等.基于迭代中心差分卡爾曼濾波的飛機(jī)姿態(tài)估計(jì)[J].儀器儀表學(xué)報(bào),2015,36(1):187-193.

HAN Ping,GAN Hao-liang,HE Wei-kun,et al.Iterated central difference Kalman filter based aircraft attitude estimation[J].Chinese Journal of Scientific Instrument,2015,36(1):187-193.(in Chinese)

[9] 陳曉威,張國(guó)毅,李彥志,等.基于SRCDKF的相位差變化率無(wú)源定位算法[J].電子信息對(duì)抗技術(shù),2016,31(1):8-11.

CHEN Xiao-wei,ZHANG Guo-yi,LI Yan-zhi,et al.Passive location algorithm of phase difference rate of change based on SRCDKF[J].Electronic Warfare Technology,2016,31(1):8-11.(in Chinese)

[10] 賈鶴鳴,宋文龍,牟宏偉,等.ISR-CDKF在SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)中的應(yīng)用[J].吉林大學(xué)學(xué)報(bào)(信息科學(xué)版),2013,31(2):196-202.

JIA He-ming,SONG Wen-long,MOU Hong-wei,et al.Application of ISR-CDKF in initial alignment of large azimuth misalignment in the SINS[J].Journal of Jilin University(Information Science Edition),2013,31(02):196-202.

[11] DUAN J,LIU D,YU H,et al.An improved fast SLAM algorithm for autonomous vehicle based on the strong tracking square root central difference Kalman filter[C]//2015 IEEE 18th International Conference on Intelligent Transportation Systems(ITSC).Spain:IEEE,2015:693-698.

[12] TANG X,YAN J,ZHONG D.Square-root sigma-point Kalman filtering for spacecraft relative navigation[J].Acta Astronautica,2010,66(5-6):704-713.

[13] 郝燕玲,牟宏偉.自適應(yīng)平方根中心差分卡爾曼濾波算法在捷聯(lián)慣性導(dǎo)航系統(tǒng)大方位失準(zhǔn)角初始對(duì)準(zhǔn)中的應(yīng)用[J].吉林大學(xué)學(xué)報(bào)(工學(xué)版),2013,43(1):261-266.

HAO Yang-ling,MOU Hong-wei.Application of adaptive SRCDKF in SINS initial alignment with large azimuth misalignment[J].Journal of Jilin University(Engineering and Technology Edition),2013,43(1):261-266.(in Chinese)

[14] HAO Y L,YANG J W,CHEN L,et al.Initial alignment of large azimuth misalignment based on square root central difference Kalman filter[J].Journal of Chinese Inertial Technology,2011,19(2):180-184.

[15] 王林,林雪原.基于UKF的GPS/SINS組合導(dǎo)航直接式濾波算法[J].導(dǎo)航定位與授時(shí),2015(3):43-48.

WANG Lin,LIN Xue-yuan.Direct Kalman filtering algorithm for the GPS/SINS integrated navigation based on UKF[J].Navigation Positioning and Timing,2015(3):43-48.(in Chinese)

[16] 劉可可.基于Kalman濾波的滾轉(zhuǎn)彈運(yùn)動(dòng)參數(shù)GPS/IMU組合測(cè)量方法研究[D].南京:南京理工大學(xué),2016.

LIU Ke-ke.Research on GPS/IMU integrated measurement method of spinning projectile motion parameters based on Kalman filter[D].Nanjing:Nanjing University of Science and Technology,2016.(in Chinese)

ApplicationofSR-CDKFinDirectFilteringofIntegratedNavigation

LU Qiao1,MA Guo-liang1,ZHANG Ping2,GE Jing-fei3

(1.School of Energy and Power Engineering,Nanjing University of Science and Technology,Nanjing 210094,China;2.Unit 63863 of PLA,Baicheng 137001,China;3.Unit 65156 of PLA,Lingyuan 122521,China)

In order to solve the nonlinear estimation problem of the navigation-parameters about SINS/GPS integrated navigation system such as attitudes,speeds and positions,a direct filtering estimation method was proposed based on Square Root Center Differential Kalman Filter(SR-CDKF).The system state vectors and observation vectors respectively consisted of SINS navigation-parameters and GPS output parameters,and the SR-CDKF filter was constructed.The filter can directly reflect the dynamic process of navigation parameters and realize the direct filtering calculation-process.Based on the areoblk_HL20 module in the MATLAB,the simulation system was built,and the simulation was carried out.The simulation results show that the algorithm has high filtering-accuracy,good convergence and stability of filtering,and the navigation requirements of SINS/GPS integrated navigation system under the condition of the nonlinear model can be satisfied.

integrated navigation;central difference;Kalman filter;direct correction

TP273

A

1004-499X(2017)04-0029-06

2017-05-10

逯嶠(1990-),男,碩士研究生,研究方向?yàn)榭柭鼮V波與組合導(dǎo)航。E-mail:lqnust900605@126.com。

馬國(guó)梁(1976-),男,副教授,博士,研究方向?yàn)閷?dǎo)航、制導(dǎo)與控制。E-mail:mgl@njust.edu.cn。