分段組合Kalman濾波在滾轉(zhuǎn)彈SINS/GPS系統(tǒng)中的應(yīng)用研究

劉可可,馬國梁,李元生

(1.南京理工大學(xué) 能源與動力工程學(xué)院,南京 210094;2.中國船舶工業(yè)系統(tǒng)工程研究院,北京 100094)

?

分段組合Kalman濾波在滾轉(zhuǎn)彈SINS/GPS系統(tǒng)中的應(yīng)用研究

劉可可1,馬國梁1,李元生2

(1.南京理工大學(xué) 能源與動力工程學(xué)院,南京 210094;2.中國船舶工業(yè)系統(tǒng)工程研究院,北京 100094)

為解決滾轉(zhuǎn)彈SINS/GPS系統(tǒng)的運動參數(shù)估計問題,建立了廣義誤差模型,采用輸出校正濾波對測量誤差進行估計,進一步將輸出校正濾波和反饋校正濾波進行了分段組合,給出了輸出校正和反饋校正濾波原理圖和分段組合流程圖。根據(jù)SINS/GPS組合系統(tǒng)松組合模型,以某滾轉(zhuǎn)彈六自由度仿真數(shù)據(jù)為例進行仿真。仿真結(jié)果表明,文中提出的分段組合Kalman濾波方法提高了估計精度。研究結(jié)果可為精確測量滾轉(zhuǎn)彈的運動參數(shù)問題提供有效的解決辦法。

滾轉(zhuǎn)彈;運動參數(shù);Kalman濾波;輸出校正;反饋校正

低成本的遠程精確打擊技術(shù)是目前彈箭技術(shù)的重點發(fā)展方向之一。運動參數(shù)的測量是彈箭氣動布局、彈道參數(shù)及特性研究的基礎(chǔ)。由于發(fā)射過程處于高過載、高動態(tài)條件下,獲取準確的運動參數(shù)十分困難[1-3],而單個測量系統(tǒng)亦不能滿足所需,因此有必要探討研究組合測量系統(tǒng)。捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)不受外界干擾的影響,隱蔽性好,并且數(shù)據(jù)更新率高、短期精度和穩(wěn)定性好,但導(dǎo)航信息是經(jīng)過積分得到的,其定位誤差會隨時間的推移而增大,長期精度較差。全球定位系統(tǒng)(GPS)可以連續(xù)、精確地提供三維位置信息和速度信息,并且不隨時間發(fā)散,長期穩(wěn)定性好。但是,當GPS載體作高動態(tài)運動時,接收機極易受到干擾,導(dǎo)致衛(wèi)星信號暫時丟失,使定位無法進行。由此可看出SINS和GPS的組合不是簡單相加,而是具有優(yōu)勢互補,揚長避短的特點。在SINS/GPS組合系統(tǒng)中,可以采用一種低性能的慣性導(dǎo)航系統(tǒng),利用慣導(dǎo)系統(tǒng)的速度信號解決動態(tài)跟蹤問題,而高精度定位則由GPS來實現(xiàn),這樣大大降低了成本,因此它們的組合系統(tǒng)成為近年來導(dǎo)航領(lǐng)域的熱點[4-6]。它們的組合模式有3種,分別是松組合、緊組合和深組合。但應(yīng)用時采用松組合較多,以往的這種組合主要用在飛機導(dǎo)彈上,應(yīng)用在滾轉(zhuǎn)彈上并不常見。

組合導(dǎo)航系統(tǒng)的運動參數(shù)估計主要采用的是Kalman濾波器[7],由于本文研究的滾轉(zhuǎn)彈具有高動態(tài)、高過載等特性,這就給單一的輸出校正濾波和反饋校正濾波算法帶來了較大的難度。為此本文提出了一種基于輸出校正和反饋校正的分段組合Kalman濾波方法。仿真結(jié)果表明,采用分段組合Kalman濾波不僅能使運動參數(shù)的估計精度提高,同時可以達到較好的收斂效果。

1　誤差狀態(tài)方程的建立

在實際應(yīng)用中,導(dǎo)航系統(tǒng)開始工作前傳入的數(shù)據(jù)都存在誤差,并且在SINS/GPS組合系統(tǒng)中,捷聯(lián)慣性導(dǎo)航系統(tǒng)作為主系統(tǒng),其誤差方程被作為Kalman濾波的狀態(tài)方程。因此,建立慣性導(dǎo)航系統(tǒng)的誤差方程意義重大。

1)姿態(tài)誤差方程。

(1)

將式(1)整理后,可得:

(2)

該方程微分后,得到誤差方程:

(3)

(4)

(5)

將式(1)、式(4)、式(5)代入式(3),整理后得:

(6)

(7)

(8)

(9)

式(9)即為姿態(tài)角誤差方程。

2)速度誤差方程。

在導(dǎo)航坐標系下[8],速度計算方程為

(10)

對式(10)微分即可得速度的誤差方程:

δvn+δgn

(11)

故

于是式(11)可化為

(12)

緯度L、經(jīng)度l和距地表的高度h計算公式如下[9]:

(13)

式中:vN,vE,vD分別為北向、東向、地向速度;RM為地球子午面曲率半徑;RN為地球卯酉面曲率半徑。RM,RN表達式如下:

在WGS-84中,R為地球長半軸,R=6 378 137.0m;e為地球橢球偏心率,e=0.081 819 190 842 6。式(13)的誤差方程為

(14)

綜上所述,誤差狀態(tài)方程可寫為

(15)

X(t)=(φNφEφDδvNδvEδvDδLδlδhεgxεgyεgzaxayaz)T,其中,下標N,E,D分別表示北、東、地地理坐標系的3個方向；φN,φE,φD為INS平臺誤差角;δvN,δvE,δvD分別為北、東、地速度誤差;δL,δl,δh為位置誤差;εgx,εgy,εgz為陀螺儀的固定偏差;ax,ay,az為加速度計的固定偏差。W(t)=(WgxWgyWgzWaxWayWaz)T為系統(tǒng)過程測量噪聲矢量,其中,Wgx,Wgy,Wgz為陀螺的測量噪聲;Wax,Way,Waz為加速度計的測量噪聲;F(t)為系統(tǒng)狀態(tài)矩陣,G(t)為系統(tǒng)噪聲傳播矩陣。

矩陣F(t)具有如下的形式:

式中:FN是對應(yīng)9個基本導(dǎo)航參數(shù)的矩陣,由捷聯(lián)慣導(dǎo)基本誤差方程決定,其非零元素如下：

由圖5可以看出，隨著氨水濃度的增加，磁性產(chǎn)品的產(chǎn)率也逐步增加(即赤鐵礦的回收率逐步增加)。當氨水濃度達到1.0 mol/L時，赤鐵礦回收率達到最大值。為此采用1.0 mol/L氨水濃度進行以下試驗。

F(9,6)=-1,

式中:Ω為地球自轉(zhuǎn)角速率,在WGS-84中,Ω=7.292 115×10-5rad/s。

2　基于輸出校正濾波的估計方法

2.1卡爾曼濾波基本原理

實際上,輸出校正濾波和反饋校正濾波都是在卡爾曼濾波的基礎(chǔ)上延伸得到的?？柭鼮V波算法是一個高效的最優(yōu)化自回歸數(shù)據(jù)處理算法,它實質(zhì)上是通過系統(tǒng)的量測信息重新構(gòu)造系統(tǒng)的狀態(tài)向量,并以“預(yù)測—實測—修正”的模式順序遞推,即利用前一時刻的估計值和現(xiàn)時刻的觀測值來更新對狀態(tài)變量的估計,求出現(xiàn)時刻的估計值。它主要包含2個信息更新過程:時間更新過程和量測更新過程[7]。具體計算可通過以下步驟來實現(xiàn)。

①狀態(tài)一步預(yù)測。

Xk/k-1=Φk,k-1Xk-1

(16)

②均方差一步預(yù)測。

(17)

③濾波增益。

(18)

④狀態(tài)估計。

Xk=Xk/k-1+Kk(Zk-HkXk/k-1)

(19)

⑤均方差誤差估計。

(20)

式中:Φ為式(16)對應(yīng)的離散狀態(tài)轉(zhuǎn)移矩陣;Qk為系統(tǒng)噪聲Wk的協(xié)方差陣,為非負定陣;Rk為量測噪聲Vk的協(xié)方差陣,為正定陣。

2.2輸出校正濾波的實現(xiàn)

圖1為INS/GPS組合輸出校正的濾波示意圖。

圖1　INS/GPS組合輸出校正的濾波示意圖

Z(t)6×1=H(t)6×15X(t)15×1+V(t)6×1

(21)

式中:

式中:Z(t)主要是由SINS和GPS的位置、速度的誤差量組成;H(t)是量測值矩陣;V(t)是量測噪聲;L,h分別表示緯度、高度;LI,hI和LG,hG分別表示INS和GPS所輸出的緯度和高度;NE,NN,ND為GPS接收機沿東、北、地方向的距離測量誤差;vIN,vIE,vID分別為INS輸出的北向速度、東向速度和地向速度;vGN,vGE,vGD分別為GPS接收機直接輸出的運載體的北向速度、東向速度和地向速度。

3　基于分段組合Kalman濾波校正方法

INS/GPS組合常采用的是輸出校正,但用在滾轉(zhuǎn)彈上卻不能達到滿意的效果。另一種方式是采用反饋校正濾波,其原理如圖2所示[10]。

圖2　SINS/GPS組合反饋校正的濾波示意圖

同樣,由圖1輸出校正濾波原理可知,若單純用輸出校正濾波,當上一時刻的估值誤差較大,而此刻又以上一時刻為基準,這樣誤差會越來越大。由于彈丸的轉(zhuǎn)速很高,初始誤差較大,所以不能不考慮彈丸的特殊性。同時考慮到慣性導(dǎo)航系統(tǒng)本身就有隨時間積累而發(fā)散的特點,因此單純的輸出校正濾波算法的收斂性亦難以保證。

由于以上2種濾波方法都不能達到較好的效果,因此考慮是否可以同時運用輸出校正濾波和反饋校正濾波,結(jié)合二者的優(yōu)點以提高滾轉(zhuǎn)彈運動參數(shù)的測量精度?；谝陨纤悸?本文提出了一種分段切換Kalman濾波算法,考慮到彈箭飛行過程中姿態(tài)參數(shù)的測量是其他運動參數(shù)測量的基礎(chǔ),因此切換條件就設(shè)定為姿態(tài)角,當姿態(tài)角的誤差值小于1°時用反饋校正濾波,否則切換為輸出校正濾波。其流程圖如圖3所示。

圖3　輸出反饋分段組合流程圖

4　仿真分析

根據(jù)前面給出的SINS/GPS組合系統(tǒng)松組合模型,以某滾轉(zhuǎn)彈六自由度彈道仿真數(shù)據(jù)為例,射向90°,即東向射擊,射角30°。用卡爾曼濾波算法對系統(tǒng)進行仿真,地球相關(guān)常數(shù)定義均以WGS-84模型為基準。由上面的Kalman濾波核心方程式(16)～式(19)可以看出,仿真前需提供X(0)和P(0/0)的初始值。由于隨著濾波步數(shù)的增加,任意選定的初始值對濾波值的影響會逐漸減弱直至消失,所以本文初值設(shè)定如下:X(0)初始值為0,初始方程陣P(0/0)取為對角陣,其對角線上的元素為

P1,1=(0.1°)2,P2,2=(0.1°)2,P3,3=(0.1°)2,

P4,4=(0.1)2,P5,5=(0.1)2,

P6,6=(0.1)2,P7,7=(10)2,P8,8=(10)2,

P9,9=(20)2,P10,10=(0.1°)2,

P11,11=(0.05°)2,P12,12=(0.05°)2,P13,13=

(0.1)2,P14,14=(0.05)2,P15,15=(0.05)2。

觀測噪聲方差陣設(shè)定對角陣,其元素為

R1,1=(0.1)2,R2,2=(0.1)2,R3,3=(0.1)2,

R4,4=(5)2,R5,5=(5)2,R6,6=(5)2。

過程噪聲方差陣對角線上的元素設(shè)為

Q1,1=(0.1°)2,Q2,2=(0.1°)2,Q3,3=(0.1°)2,

Q4,4=(0.1)2,Q5,5=(0.05)2,Q6,6=(0.05)2。

圖4～圖6給出了輸出校正濾波算法和切換Kalman濾波算法的仿真對比結(jié)果。圖中,δθ,δψ,δφ分別為俯仰角誤差、偏航角誤差、滾轉(zhuǎn)角誤差；δvE,δvN,δvD分別為東、北、地速度誤差；δL，δl,δh分別為緯度、經(jīng)度和高度誤差。

由于彈丸的轉(zhuǎn)速很高,所以各參數(shù)的初始誤差均較大,但隨著時間的推移,經(jīng)過濾波器的融合,誤差將漸漸地減少。從圖4(a)看出,輸出校正濾波得到的俯仰角和偏航角的誤差值一直在0上下浮動,不能收斂至0,其收斂效果不是很好。圖4(b)是切換Kalman濾波姿態(tài)角誤差曲線,從圖中可以看出,俯仰角和偏航角的誤差在20 s時基本都收斂到0,而滾轉(zhuǎn)角誤差的收斂速度則更快,在5 s左右就收斂為0。這表明切換Kalman濾波算法的收斂速度要明顯快于輸出校正濾波。

圖5為速度誤差曲線圖,從圖中可看出,2種算法的北向速度和東向速度誤差區(qū)別較小,但地向速度誤差則有明顯的不同,輸出校正濾波的最大誤差為20 m/s,而切換Kalman濾波的最大誤差則為10 m/s,其效果明顯好于輸出校正濾波算法。圖6為位置誤差曲線,從圖6(a)可以明顯地看出,輸出校正濾波算法得到的經(jīng)度和高度都隨著時間在發(fā)散,而從圖6(b)可以看出,切換Kalman濾波算法能獲得很好的收斂效果。從以上的仿真結(jié)果可以明顯看出,切換Kalman濾波算法具有更快的收斂速度和計算精度,其效果要明顯好于單純的輸出校正濾波算法。

圖4　輸出校正和切換Kalman濾波姿態(tài)角仿真對比圖

圖5　輸出校正和切換Kalman濾波速度仿真對比圖

圖6　輸出校正和切換Kalman濾波位置仿真對比圖

5　結(jié)束語

本文以滾轉(zhuǎn)彈為研究對象,建立了SINS/GPS組合系統(tǒng)的松組合模型、卡爾曼濾波的狀態(tài)方程和量測方程;分析了卡爾曼濾波的工作原理,并給出了輸出校正和反饋校正濾波原理圖及其使用范圍。分析比較表明,單獨使用反饋校正會失去穩(wěn)定性,單獨使用輸出校正濾波收斂效果卻不能滿足需要。因此,本文基于分段濾波的組合導(dǎo)航思想,提出了一種切換Kalman濾波算法。仿真結(jié)果表明,切換Kalman濾波算法比單純的輸出校正濾波算法具有更快的收斂速度和更高的計算精度,具有一定的理論參考和工程應(yīng)用價值。

[1]馬國梁.高轉(zhuǎn)速彈丸磁強計/太陽方位角傳感器組合測姿方法[J].南京理工大學(xué)學(xué)報(自然科學(xué)版),2013,37(1):140-144.

MAGou-liang.Magnetometer/yawsondeintegratedattitudemeasurementforhighspinrateprojectiles[J].JournalofNanjingUniversityofScienceandTechnology(NaturalScience),2013,37(1):140-144.(inChinese)

[2]史金光,韓艷,劉世平,等.制導(dǎo)炮彈飛行姿態(tài)角的一種組合測量方法[J].彈道學(xué)報,2011,23(3):38-41.

SHIJin-guang,HANYan,LIUShi-ping,etal.Anapproachofcombinationmeasurementforflightattitudeanglesofguidedprojectile[J].JournalofBallistics,2011,23(3):38-41.(inChinese)

[3]DAVIDE,GAYLORE,GLENNL.GPS/INSKalmanfilterdesignforspacecraftoperationintheproximityoftheinternationalspacestation[C]//AIAAGuidance,Navigation,andControlConferenceandExhibit.Ausin,Texas:AIAA,2003.

[4]PARKDB,SHINDH.DevelopmentofaGPS/INSsystemforprecisionGPSguidedbombs[J].AerospaceandElectnricSystemsMagaiche,IEEE,2012,27(3):31-39.

[5]WANGLi-jun,ZHAOHui-chang,YANGXiao-niu.ThemodelingandsimulationforGPS/INSintegratednavigationsystem[J].IEEEConfencePublications,2008,4:1 991-1 994.

[6]GROSSJ,GUY,GURURAJANS.AcomparisonofextendedKalmanfilter,sigma-pointKalmanfilter,andparticlefilterinGPS/INSsensorfusion[C]//AIAAGuidance,Navigation,andControlConference.Toronto,Ontario,Canada:AIAA,2010.

[7]秦永元,汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西安:西北工業(yè)大學(xué)出版社,2012.

QINYong-yuan,WANGShu-hua.Kalmanfilterwithintegratednavigationprinciple[M].Xi’an:NorthwesternPolytechnicalUniversityPress,2012.(inChinese)

[8]張?zhí)旃?王秀萍,王麗霞,等.捷聯(lián)慣性導(dǎo)航技術(shù)[M].北京:國防工業(yè)出版社,2010.

ZHANGTian-guang,WANGXiu-ping,WANGLi-xia,etal.Strapdowninertialnavigationtechnology[M].Beijing:NationalDefenseIndustryPress,2010.(inChinese)

[9]杜習(xí)奇.GPS與捷聯(lián)慣導(dǎo)組合導(dǎo)航系統(tǒng)研究[D].南京:南京理工大學(xué),2004.

DUXi-qi.SINScombinedwithGPSnavigationsystem[D].Nanjing:NanjingUniversityofScienceandTechnology,2004.(inChinese)

[10]以光衢.慣性導(dǎo)航原理[M].北京:航空工業(yè)出版社,1987.

YIGuang-qu.Inertialnavigationprinciple[M].Benjing:AviationIndustryPress,1987.(inChinese)

[11]SHENXiang,ZHANGYong-jun,LUXiao.AnimprovedmethodfortransformingGPS/INSattitudetonationalmapprojcetionframe[J].GeoscienceandRemoteSensingLetters,IEEE,2015,12(6):1302-306.

Research on Piecewise Integrated Kalman Filter in SINS/GPS System of Spinning Projectile

LIU Ke-ke1,MA Guo-liang1,LI Yuan-sheng2

(1.School of Power and Engineering,Nanjing University of Science and Technology,Nanjing 210094,China;2.Systems Engineering Research Institute,CSSC,Beijing 100094,China)

To solve the parameter estimation of spinning projectile SINS/GPS system,a generalized error model was established.The output correction filter was utilized to estimate the errors of measurement,and then a new filter was proposed by piecewise integrating the output correction filter with the feedback correction filter.Schematic diagrams of the output correction filter and the feedback correction filter,and the flow chart of the piecewise-integrated filter were also provided.According to the loose integrated model of SINS/GPS system,the simulation was carried out by taking the 6DOF simulation data of certain spinning projectile for instance.The numerical simulation results show that the proposed piecewise-integrated Kalman filter can improve the estimation accuracy.The results can provide an effective solution to accurately measuring the motion parameters of the spinning projectile.

spinning projectile;motion parameters;Kalman filter;output correction;feedback correction

2015-05-13

劉可可(1987- ),女,碩士研究生,研究方向為飛行器導(dǎo)航、制導(dǎo)與控制。E-mail:1418839092@qq.com。

通過作者:馬國梁(1976- ),男,副教授,研究方向為飛行器導(dǎo)航、制導(dǎo)與控制。E-mail:mgljob@163.com。

TJ765.1

A

1004-499X(2016)01-0045-07