袁丹丹,易文俊,管 軍,孫 蕾,張浩然

(南京理工大學(xué) 瞬態(tài)物理國(guó)家重點(diǎn)實(shí)驗(yàn)室,江蘇 南京 210094)

基于UKF彈體滾轉(zhuǎn)姿態(tài)測(cè)量方法研究

袁丹丹,易文俊,管 軍,孫 蕾,張浩然

(南京理工大學(xué) 瞬態(tài)物理國(guó)家重點(diǎn)實(shí)驗(yàn)室,江蘇 南京 210094)

利用磁阻傳感器和GPS等器件的測(cè)量信息,建立了地磁/GPS組合彈體姿態(tài)測(cè)量解算模型,對(duì)彈體姿態(tài)信息進(jìn)行實(shí)時(shí)測(cè)量解算。為了提高彈體飛行姿態(tài)測(cè)量解算精度,建立了彈道濾波方程,采用UKF算法進(jìn)行數(shù)據(jù)濾波處理,結(jié)合磁阻傳感器的輸出信息對(duì)彈體滾轉(zhuǎn)姿態(tài)進(jìn)行實(shí)時(shí)解算。數(shù)值計(jì)算及仿真結(jié)果表明:相比于直接使用GPS輸出的速度信息進(jìn)行姿態(tài)解算,通過(guò)UKF濾波處理后,可以使得地磁/GPS組合測(cè)量解算的結(jié)果更準(zhǔn)確,提高了彈體姿態(tài)解算精度。

彈箭;制導(dǎo);GPS;地磁;UKF濾波;滾轉(zhuǎn)姿態(tài)解算

獲取飛行彈體準(zhǔn)確的姿態(tài)信息對(duì)彈體制導(dǎo)控制存在重大的意義,若是姿態(tài)測(cè)量信息不準(zhǔn)確,將嚴(yán)重影響到控制系統(tǒng)的控制精度,甚至導(dǎo)致彈體飛行不穩(wěn)定,因此提高彈體運(yùn)動(dòng)姿態(tài)參數(shù)測(cè)量精度的研究非常有必要。在飛行彈體姿態(tài)測(cè)量中,常用的是慣性導(dǎo)航方法,即使用陀螺儀、加速度計(jì)等慣性器件進(jìn)行飛行彈體的姿態(tài)測(cè)量,其理論和技術(shù)都達(dá)到了一定的水平,但是存在著誤差隨時(shí)間累積等缺陷[1],且由于彈體的高動(dòng)態(tài)(高發(fā)射過(guò)載、高轉(zhuǎn)速等)的特點(diǎn),慣性器件很難滿足上彈應(yīng)用的要求[2]。由于磁阻傳感器可以克服誤差隨時(shí)間累積的缺陷,同時(shí)具有體積小、成本低且具有抗高過(guò)載的優(yōu)點(diǎn),也被廣泛地應(yīng)用于姿態(tài)測(cè)量中[3-4];而且考慮到地磁場(chǎng)特性,地磁場(chǎng)的強(qiáng)度和方向是關(guān)于位置的函數(shù)[5],由IGRF計(jì)算我國(guó)地磁場(chǎng)總量在各處基本相等,經(jīng)緯度每變化1°,地面距離大約變化110 km,目前炮彈射程基本在100 km以內(nèi),故可認(rèn)為在彈丸的射程范圍內(nèi),當(dāng)?shù)卮艌?chǎng)的大小和方向基本不變。此外,GPS具有全天候、高精度、連續(xù)定位的優(yōu)點(diǎn),可以為彈體實(shí)時(shí)提供時(shí)間位置和速度信息。如果單獨(dú)采用其中的一種進(jìn)行姿態(tài)測(cè)量,會(huì)受到自身?xiàng)l件的限制從而影響到測(cè)量精度,故而延伸出一些組合測(cè)量方法?？紤]到GPS和磁阻傳感器的特點(diǎn),本文研究了衛(wèi)星與地磁組合測(cè)量方案,進(jìn)行彈體姿態(tài)測(cè)量解算。文獻(xiàn)[6]采用卡爾曼濾波對(duì)彈丸的位置和姿態(tài)參數(shù)進(jìn)行濾波估計(jì),從仿真結(jié)果看,該方法的濾波估計(jì)值和理論計(jì)算值基本吻合,不過(guò)其彈道模型誤差較大[7]。本文利用GPS提供的定位信息,考慮到量測(cè)噪聲和干擾的存在,根據(jù)外彈道理論基礎(chǔ)建立數(shù)學(xué)模型,采用無(wú)跡卡爾曼濾波進(jìn)行數(shù)據(jù)濾波處理,可以有效提高測(cè)量精度;再結(jié)合地磁傳感器的輸出信息進(jìn)行滾轉(zhuǎn)姿態(tài)的解算,從而可以得到相對(duì)準(zhǔn)確的彈體姿態(tài)信息。

1 測(cè)量數(shù)據(jù)預(yù)處理

由于GPS存在著測(cè)量誤差以及會(huì)受外界干擾,且數(shù)據(jù)更新頻率不能滿足彈體的實(shí)時(shí)性要求,因此在實(shí)際過(guò)程中所得到的測(cè)量信息需要經(jīng)過(guò)數(shù)據(jù)預(yù)處理,從而得到誤差較小并且滿足實(shí)時(shí)性要求的彈丸飛行信息,便于提高彈體飛行控制性能。GPS的測(cè)量信息數(shù)據(jù)處理通常是采用最優(yōu)估計(jì)理論進(jìn)行濾波處理。常用的濾波方法包括最小二乘濾波、最大似然估計(jì)、Kalman濾波等。標(biāo)準(zhǔn)的卡爾曼濾波[8-9]是一種基于模型的線性無(wú)偏最小方差估計(jì),采用遞推計(jì)算,計(jì)算量和存儲(chǔ)量小,故而被廣泛應(yīng)用,但其只適用于系統(tǒng)狀態(tài)方程和量測(cè)方程是線性的情況;擴(kuò)展卡爾曼濾波(EKF)通過(guò)對(duì)非線性系統(tǒng)在狀態(tài)估計(jì)值附近的泰勒級(jí)數(shù)展開(kāi)式進(jìn)行一階截?cái)?轉(zhuǎn)化成線性系統(tǒng)再進(jìn)行Kalman濾波估計(jì),但是需要計(jì)算非線性函數(shù)的雅克比(Jacobian)矩陣。本文采用無(wú)跡卡爾曼濾波(UKF)對(duì)GPS測(cè)量數(shù)據(jù)進(jìn)行預(yù)處理,UKF是由Julier等[10]提出的一種基于UT變換的濾波方法,不需要進(jìn)行Jacobian矩陣和Hansen矩陣的運(yùn)算,且精度要高于EKF。

1.1 UKF的基本原理及實(shí)現(xiàn)步驟

無(wú)跡卡爾曼濾波是對(duì)后驗(yàn)概率密度分布進(jìn)行近似來(lái)得到次優(yōu)估計(jì)的濾波算法,其核心基礎(chǔ)是UT變換(用以描述高斯隨機(jī)變量在通過(guò)非線性變換之后的概率分布的一種方法),通過(guò)UT變換對(duì)非線性系統(tǒng)狀態(tài)和誤差協(xié)方差進(jìn)行遞推更新[11],每一次更新之后都要進(jìn)行UT變換,既可以保證狀態(tài)估計(jì)的精度,而且能避免對(duì)非線性方程的線性化。

設(shè)某一非線性系統(tǒng)有如下形式的狀態(tài)空間模型:

式中:Xk∈R為k時(shí)刻n×1維狀態(tài)隨機(jī)向量;Yk∈R為k時(shí)刻m×1維觀測(cè)隨機(jī)向量;mk,nk分別為過(guò)程噪聲向量和量測(cè)噪聲向量,這里假設(shè)都是均值為0的高斯白噪聲,過(guò)程噪聲的方差陣為Qk,量測(cè)噪聲的方差陣為Rk,即mk～N(0,Qk),nk～N(0,Rk)。采用UKF濾波過(guò)程如下。

1)初始化。

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

①計(jì)算Sigma點(diǎn)。

②各個(gè)Sigma點(diǎn)的權(quán)值。

③時(shí)間傳播方程。

④測(cè)量更新方程。

⑤濾波更新。

1.2GPS數(shù)據(jù)預(yù)處理

1.2.1 狀態(tài)方程的建立

考慮到彈道解算的快速性、實(shí)時(shí)性,結(jié)合外彈道理論[12],根據(jù)質(zhì)點(diǎn)彈道方程,建立系統(tǒng)的狀態(tài)模型,定義狀態(tài)變量X=(xvxyvyzvz)T,分別表示北東地坐標(biāo)系Oxyz下的位置和速度分量。由于質(zhì)點(diǎn)彈道方程式是對(duì)彈丸質(zhì)心運(yùn)動(dòng)的近似描述,存在模型誤差,需要通過(guò)引入一定的隨機(jī)噪聲來(lái)補(bǔ)償,從而系統(tǒng)的狀態(tài)方程為

1.2.2 量測(cè)方程的建立

利用GPS接收觀測(cè)到的速度參數(shù)分量建立系統(tǒng)的觀測(cè)方程為

Zk=HXk+vk

對(duì)所建立的模型,利用無(wú)跡卡爾曼濾波進(jìn)行濾波處理GPS定位信息。

2 基于地磁/GPS的姿態(tài)測(cè)量解算原理

彈丸的3個(gè)姿態(tài)角(歐拉角)描述了彈體坐標(biāo)系與地面坐標(biāo)系之間的關(guān)系。GPS速度姿態(tài)估計(jì)是利用GPS的速度量測(cè)信息,建立姿態(tài)估計(jì)模型進(jìn)行解算,此時(shí)所獲得的彈丸姿態(tài)信息通常定義為偽姿態(tài)[13]信息,其實(shí)它描述的是速度坐標(biāo)系與地面坐標(biāo)系之間的旋轉(zhuǎn)關(guān)系。基于GPS速度得到的偽姿態(tài)信息,結(jié)合三軸磁阻傳感器測(cè)得的彈丸軸向地磁分量進(jìn)行姿態(tài)矩陣的解算,解出彈丸的滾轉(zhuǎn)姿態(tài)信息,其原理如圖1所示。圖中,vx,vy,vz為地理坐標(biāo)系下3個(gè)軸向的速度分量。

2.1GPS偽姿態(tài)解算

2.2 彈丸滾轉(zhuǎn)姿態(tài)解算原理

將彈載三軸磁阻傳感器固定在彈體上,3個(gè)敏感軸分別與彈體坐標(biāo)系下的3軸平行,所以當(dāng)彈丸在空中運(yùn)動(dòng)時(shí),就可以通過(guò)磁阻傳感器3個(gè)軸上的測(cè)量信號(hào)求解彈丸的姿態(tài)信息。其原理如下:利用當(dāng)?shù)氐卮艌?chǎng)矢量、磁阻傳感器輸出的3個(gè)軸向上的地磁分量,以及地理坐標(biāo)系與彈體坐標(biāo)系之間的姿態(tài)轉(zhuǎn)換矩陣,建立如下關(guān)系:

(1)

式中:Bx,By,Bz為地理坐標(biāo)系下的地磁分量;Bxb,Byb,Bzb為地磁矢量在彈體坐標(biāo)系Oxbybzb3個(gè)軸上的投影;θ,ψ,γ分別為俯仰角、偏航角、滾轉(zhuǎn)角。

將式(1)化簡(jiǎn)得到:

所以,滾轉(zhuǎn)角的計(jì)算公式為

利用GPS輸出的信息計(jì)算的偽俯仰角θv和偽偏航角ψv代替俯仰角θ和偏航角ψ,代入滾轉(zhuǎn)角的計(jì)算公式,并結(jié)合磁阻傳感器的輸出對(duì)彈丸滾轉(zhuǎn)姿態(tài)進(jìn)行解算。

3 仿真試驗(yàn)與結(jié)果分析

針對(duì)本文所采用的地磁/GPS組合載體滾轉(zhuǎn)姿態(tài)測(cè)量方法,通過(guò)仿真試驗(yàn)來(lái)驗(yàn)證其可行性及有效性。為了進(jìn)行動(dòng)態(tài)仿真,需要給定載體的飛行軌跡,本文利用六自由度彈道模型生成彈道數(shù)據(jù)作為理論真值,并在理論真值的基礎(chǔ)上附加高斯白噪聲作為彈道濾波的量測(cè)輸出。仿真試驗(yàn)中取量測(cè)噪聲均方差:速度誤差為0.8 m/s。數(shù)值計(jì)算初始條件:彈丸初速為750 m/s,質(zhì)量為43.25 kg,直徑為130 mm。根據(jù)南京的地理位置(北緯32.028°、東經(jīng)118.854°以及海拔高度24.03 m),查得地磁要素[15]Bx=32 827.3,By=-3 153.2,Bz=37 268.2。

由圖2～圖4的誤差曲線可以發(fā)現(xiàn),通過(guò)UKF濾波處理后輸出的曲線更加平滑,由此可見(jiàn),利用UKF估計(jì)使得GPS測(cè)速精度相比于未經(jīng)消除的GPS測(cè)量精度有所提高,濾波后的速度誤差有效地消除了系統(tǒng)差,進(jìn)一步抑制了誤差的累積,使系統(tǒng)動(dòng)態(tài)性能得到了改善。圖5為彈體速度濾波前后對(duì)比。

采用GPS跟地磁組合對(duì)彈丸滾轉(zhuǎn)姿態(tài)進(jìn)行解算,結(jié)果與真實(shí)值的比較如圖6所示。圖6(a)給出了彈丸飛行全過(guò)程滾轉(zhuǎn)角的比較曲線,圖6(b)是滾轉(zhuǎn)角比較曲線的局部放大。從曲線的吻合程度可以清晰看出,采用GPS/地磁組合進(jìn)行滾轉(zhuǎn)姿態(tài)解算的方法有效、可行。

圖7給出了滾轉(zhuǎn)角度的估計(jì)誤差,即利用地磁衛(wèi)星組合解算出來(lái)的彈丸滾轉(zhuǎn)姿態(tài)與真實(shí)的滾轉(zhuǎn)姿態(tài)進(jìn)行比較,用誤差來(lái)驗(yàn)證該組合解算出來(lái)的滾轉(zhuǎn)姿態(tài)的精度。圖7(a)給出的是利用未經(jīng)過(guò)數(shù)據(jù)預(yù)處理的GPS速度信息進(jìn)行滾轉(zhuǎn)角姿態(tài)解算的誤差曲線。圖7(b)描述的是采用經(jīng)過(guò)UKF濾波處理后的GPS數(shù)據(jù)信息和磁阻傳感器的輸出信息相結(jié)合進(jìn)行滾轉(zhuǎn)姿態(tài)解算的誤差,整張圖說(shuō)明了對(duì)GPS輸出信息進(jìn)行UKF濾波預(yù)處理,可以有效減小滾轉(zhuǎn)角姿態(tài)解算的誤差。

4 結(jié)束語(yǔ)

本文基于GPS和磁阻傳感器的測(cè)量信息,采用GPS/地磁組合載體姿態(tài)測(cè)量方法,可以有效地解算彈丸滾轉(zhuǎn)姿態(tài)。首先根據(jù)載體的運(yùn)動(dòng)模式,建立彈道濾波方程,用UKF濾波估計(jì)速度參數(shù),相較于直接利用GPS接收機(jī)輸出的速度信息,提高了系統(tǒng)的穩(wěn)定性和完整性,為后續(xù)姿態(tài)解算精度的提高打下基礎(chǔ);再結(jié)合磁阻傳感器的輸出信息,進(jìn)行滾轉(zhuǎn)姿態(tài)的測(cè)量解算,而采用UKF濾波后的速度信息,有效地降低了滾轉(zhuǎn)姿態(tài)的解算誤差。

[1] 李海濤,曹詠弘,祖靜.旋轉(zhuǎn)彈姿態(tài)解算方法研究[J].兵工學(xué)報(bào),2010,31(7):987-990. LI Hai-tao,CAO Yong-hong,ZU Jing.Research on attitude algorithms for spinning projectiles[J].Acta Armamentarii,2010,31(7):987-990.(in Chinese)

[2] 黃崢,李科杰,金連寶.火炮彈丸捷聯(lián)式地磁-太陽(yáng)方位姿態(tài)測(cè)量模型研究[J].兵工學(xué)報(bào),2001,22(1):19-22. HUANG Zheng,LI Ke-jie,JIN Lian-bao.A model for the detection of geomagnetism and solar direction for use in artillery projectile attitude measurement[J].Acta Armamentarii,2001,22(1):19-22.(in Chinese)

[3] 邱榮劍.地磁傳感器測(cè)量彈體滾轉(zhuǎn)姿態(tài)方法研究[J].四川兵工學(xué)報(bào),2014,35(10):103-106. QIU Jian-rong.Research on the method of measuring projectiles roll attitude by geomagnetic sensors[J].Journal of Sichuan Ordnance,2014,35(10):103-106.(in Chinese)

[4] 龍禮,張合.三軸地磁傳感器誤差的自適應(yīng)校正方法[J].儀器儀表學(xué)報(bào),2013,34(1):161-165. LONG Li,ZHANG He.Automatic and adaptive calibration method of tri-axial magnetometer[J].Chinese Journal of Scientific Instrument,2013,34(1):161-165.(in Chinese)

[5] 李玎.基于磁傳感器組合的旋轉(zhuǎn)彈體姿態(tài)測(cè)試方法研究[D].南京:南京理工大學(xué),2009. LI Ding.Study of attitude measurement on spinning projectile based on magnetic sensors unit[D].Nanjing:Nanjing University of Science and Technology,2009.(in Chinese)

[6] 楊小軍,施坤林,汪儀林.基于磁傳感器/GPS組合制導(dǎo)飛行彈體的姿態(tài)和位置估計(jì)[J].兵工學(xué)報(bào),2008,29(2):55-55. YANG Xiao-jun,SHI Kun-lin,WANG Yi-lin.Estimate of attitude and position of flying projectile controlled by combined guidance based on magnetometer/GPS[J].Acta Armamentarii,2008,29(2):55-55.(in Chinese)[7] 牛春峰,劉世平,王中原.高速旋轉(zhuǎn)彈位置與姿態(tài)測(cè)量數(shù)據(jù)分析方法[J].火力與指揮控制,2012,37(5):89-92. NIU Chun-feng,LIU Shi-ping,WANG Zhong-yuan.Estimate position and attitude of high-speed rotating projectile[J].Fire Control & Command Control,2012,37(5):89-92.(in Chinese)

[8] 秦永元.卡爾曼濾波與組合導(dǎo)航原理[M].西安:西北工業(yè)大學(xué)出版社,2012. QIN Yong-yuan.Kalman filter and the theory of integrated navigation[M].Xi’an:Northwestern Polytechnical University Press,2012.(in Chinese)

[9] 趙琳.非線性系統(tǒng)濾波理論[M].北京:國(guó)防工業(yè)出版社,2012. ZHAO Lin.Nonlinear system filtering theory[M].Beijing:National Defense Industry Press,2012.(in Chinese)

[10] MERWE R V D,WAN E,JULIER S.Sigma-point Kalman filters for nonlinear estimation and sensor-fusion applications to integrated navigation[C]//Proceeding of the AIAA Guidance,Navigation,and Control Conference and Exhibit.Rhode Island,USA:AIAA,2004:1-30.

[11] 段笑菊,薛曉中.UKF與EKF在GPS/INS超緊組合導(dǎo)航中的應(yīng)用比較[J].火力與指揮控制,2010,35(6):60-63. DUAN Xiao-ju,XUE Xiao-zhong.Comparison between extended and unscented Kalman filtering applied to ultra-tight GPS/INS integration[J].Fire Control & Command Control,2010,35(6):60-63.(in Chinese)

[12] 宋丕極.槍炮與火箭外彈道學(xué)[M].北京:兵器工業(yè)出版社,1993. SONG Pi-ji.Exterior ballistics of guns and rockets[M].Beijing:National Defense Industry Press,1993.(in Chinese)

[13] 丁傳炳,王良明,鄭翠翠.單天線GPS姿態(tài)測(cè)量系統(tǒng)在制導(dǎo)火箭中的應(yīng)用[J].空間科學(xué)學(xué)報(bào),2010,30(6):607-611. DING Chuan-bing,WANG Liang-ming,ZHENG Cui-cui.Research on the application of single-antenna GPS measurement system on guided rocket[J].Chinese Journal of Space Science,2010,30(6):607-611.(in Chinese)

[14] KORNFELD R P,JOHN H R,DEYST J J.Single-antenna GPS-based aircraft attitude determination[J].Navigation,1998,45(1):51-60.

[15] 邱海迪,李嘉,牛春峰,等.高轉(zhuǎn)速?gòu)椡枳藨B(tài)解算方法研究[J].四川兵工學(xué)報(bào),2016,37(2):7-10. QIU Hai-di,LI Jia,NIU Chun-feng,et al.Research on attitude algorithm for projectile with high spinning speed[J].Journal of Sichuan Ordnance,2016,37(2):7-10.(in Chinese)

Study of Projectile Roll-attitude Measurement Method Based on Unscented Kalman Filter

YUAN Dan-dan,YI Wen-jun,GUAN Jun,SUN Lei,ZHANG Hao-ran

(National Key Laboratory of Transient Physics,Nanjing University of Science and Technology,Nanjing 210094,China)

A calculation model was built by using the measured information of GPS and magnetometer sensor,and the projectile attitude was calculated in real-time.In order to improve the measurement precision of the projectile attitude,the filter equation of trajectory was established,and the output values of GPS were filtered based on unscented Kalman filter(UKF).The roll angle of projectile was calculated by adopting the filter results and the values of magnetometer sensor.Numerical calculation and simulation results show that the roll angle calculated by adopting the velocity information processed by UKF is more accurate than that by directly using the velocity information from GPS output,and the estimation precision of attitude is improved.

projectile;guidance;GPS;magnetometer;unscented Kalman filter;attitude estimation

2017-01-08

國(guó)家自然科學(xué)基金項(xiàng)目(11472136);河南省自然科學(xué)基金研究項(xiàng)目資助(152300410209)

袁丹丹(1989- ),女,博士研究生,研究方向?yàn)閺椉w行控制。E-mail:tianhongjun@qq.com。

易文俊(1970- ),男,教授,博士生導(dǎo)師,研究方向?yàn)閺椉w行控制。E-mail:yiwenjun0@163.com。

V249.32

A

1004-499X(2017)02-0008-05