劉可可,馬國梁,李元生

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

?

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

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

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

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

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

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

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

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

在實(shí)際應(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)航坐標(biāo)系下[8],速度計(jì)算方程為

(10)

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

δvn+δgn

(11)

故

于是式(11)可化為

(12)

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

(13)

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

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

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

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

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

F(9,6)=-1,

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

2　基于輸出校正濾波的估計(jì)方法

2.1卡爾曼濾波基本原理

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

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

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

(16)

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

(17)

③濾波增益。

(18)

④狀態(tài)估計(jì)。

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

(19)

⑤均方差誤差估計(jì)。

(20)

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

2.2輸出校正濾波的實(shí)現(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)是量測(cè)值矩陣;V(t)是量測(cè)噪聲;L,h分別表示緯度、高度;LI,hI和LG,hG分別表示INS和GPS所輸出的緯度和高度;NE,NN,ND為GPS接收機(jī)沿東、北、地方向的距離測(cè)量誤差;vIN,vIE,vID分別為INS輸出的北向速度、東向速度和地向速度;vGN,vGE,vGD分別為GPS接收機(jī)直接輸出的運(yùn)載體的北向速度、東向速度和地向速度。

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

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

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

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

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

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

4　仿真分析

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

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。

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

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。

過程噪聲方差陣對(duì)角線上的元素設(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濾波算法的仿真對(duì)比結(jié)果。圖中,δθ,δψ,δφ分別為俯仰角誤差、偏航角誤差、滾轉(zhuǎn)角誤差；δvE,δvN,δvD分別為東、北、地速度誤差；δL，δl,δh分別為緯度、經(jīng)度和高度誤差。

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

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

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

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

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

5　結(jié)束語

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

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

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

[2]史金光,韓艷,劉世平,等.制導(dǎo)炮彈飛行姿態(tài)角的一種組合測(cè)量方法[J].彈道學(xué)報(bào),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- ),女,碩士研究生,研究方向?yàn)轱w行器導(dǎo)航、制導(dǎo)與控制。E-mail:1418839092@qq.com。

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

TJ765.1

A

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