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

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

(南京理工大學(xué) 瞬態(tài)物理國家重點實驗室,江蘇 南京 210094)

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

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

(南京理工大學(xué) 瞬態(tài)物理國家重點實驗室,江蘇 南京 210094)

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

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

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

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

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

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

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

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

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

1)初始化。

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

①計算Sigma點。

②各個Sigma點的權(quán)值。

③時間傳播方程。

④測量更新方程。

⑤濾波更新。

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

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

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

1.2.2 量測方程的建立

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

Zk=HXk+vk

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

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

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

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

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

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

(1)

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

將式(1)化簡得到:

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

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

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

針對本文所采用的地磁/GPS組合載體滾轉(zhuǎn)姿態(tài)測量方法,通過仿真試驗來驗證其可行性及有效性。為了進(jìn)行動態(tài)仿真,需要給定載體的飛行軌跡,本文利用六自由度彈道模型生成彈道數(shù)據(jù)作為理論真值,并在理論真值的基礎(chǔ)上附加高斯白噪聲作為彈道濾波的量測輸出。仿真試驗中取量測噪聲均方差:速度誤差為0.8 m/s。數(shù)值計算初始條件:彈丸初速為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),通過UKF濾波處理后輸出的曲線更加平滑,由此可見,利用UKF估計使得GPS測速精度相比于未經(jīng)消除的GPS測量精度有所提高,濾波后的速度誤差有效地消除了系統(tǒng)差,進(jìn)一步抑制了誤差的累積,使系統(tǒng)動態(tài)性能得到了改善。圖5為彈體速度濾波前后對比。

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

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

4 結(jié)束語

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

[1] 李海濤,曹詠弘,祖靜.旋轉(zhuǎn)彈姿態(tài)解算方法研究[J].兵工學(xué)報,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)式地磁-太陽方位姿態(tài)測量模型研究[J].兵工學(xué)報,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] 邱榮劍.地磁傳感器測量彈體滾轉(zhuǎn)姿態(tài)方法研究[J].四川兵工學(xué)報,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é)報,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)測試方法研究[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].兵工學(xué)報,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)測量數(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].北京:國防工業(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)測量系統(tǒng)在制導(dǎo)火箭中的應(yīng)用[J].空間科學(xué)學(xué)報,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)速彈丸姿態(tài)解算方法研究[J].四川兵工學(xué)報,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

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

袁丹丹(1989- ),女,博士研究生,研究方向為彈箭飛行控制。E-mail:tianhongjun@qq.com。

易文俊(1970- ),男,教授,博士生導(dǎo)師,研究方向為彈箭飛行控制。E-mail:yiwenjun0@163.com。

V249.32

A

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