張朋，譚湘霞

(北京電子工程總體研究所，北京 100854)

飛行器推力偏心及質(zhì)心漂移總體估算方法研究*

張朋，譚湘霞

(北京電子工程總體研究所，北京 100854)

對(duì)于在大氣層外采用直接側(cè)向力控制的飛行器來(lái)說(shuō)，由于推力偏心以及質(zhì)心漂移的存在，飛行器軌控發(fā)動(dòng)機(jī)在工作時(shí)，會(huì)在滾轉(zhuǎn)、俯仰、偏航等通道中產(chǎn)生附加的姿態(tài)干擾力矩。若干擾力矩過(guò)大，就會(huì)導(dǎo)致姿態(tài)失穩(wěn)。首先建立了一個(gè)完整的基于直接側(cè)向力控制的飛行器仿真模型，以動(dòng)力學(xué)模型為基本方程，分別采用擴(kuò)展卡爾曼濾波(EKF)和無(wú)跡卡爾曼濾波(UKF)推導(dǎo)出了飛行器推力偏心和質(zhì)心漂移的濾波模型。最后給出了仿真算例，驗(yàn)證了濾波模型的正確性并對(duì)2種濾波模型的特性進(jìn)行了對(duì)比。

大氣層外；直接側(cè)向力；推力偏心；質(zhì)心漂移；擴(kuò)展卡爾曼濾波；無(wú)跡卡爾曼濾波

0 引言

推力偏心和質(zhì)心漂移廣泛存在于火箭、導(dǎo)彈、衛(wèi)星當(dāng)中。以大氣層外采用直接側(cè)向力控制的飛行器為例，由于推力偏心以及質(zhì)心漂移的存在，飛行器軌控發(fā)動(dòng)機(jī)在工作的時(shí)候，會(huì)在滾轉(zhuǎn)、俯仰、偏航等通道中產(chǎn)生附加的姿態(tài)干擾力矩，從而影響飛行器的姿態(tài)控制。如果由推力偏心和質(zhì)心漂移引起的姿態(tài)干擾力矩過(guò)大，超出飛行器自身的姿態(tài)控制力矩，就會(huì)導(dǎo)致飛行器的姿態(tài)失穩(wěn)?，F(xiàn)階段國(guó)內(nèi)對(duì)推力偏心的獲得多通過(guò)對(duì)軌控發(fā)動(dòng)機(jī)進(jìn)行批次抽檢試車試驗(yàn)，而質(zhì)心漂移亦是通過(guò)試驗(yàn)獲得。文獻(xiàn)[1]提出利用濾波算法估算推力偏心和質(zhì)心漂移，但估算精度有待提高。本文則是希望尋求一個(gè)精度高、適應(yīng)性好的方法來(lái)估算飛行器的推力偏心和質(zhì)心漂移。

本文以動(dòng)力學(xué)模型為基本方程，分別采用擴(kuò)展卡爾曼濾波(extended Kalman filter，EKF)和無(wú)跡卡爾曼濾波(unscented Kalman filter，UKF)方法推導(dǎo)出了飛行器推力偏心和質(zhì)心漂移的濾波模型。最后給出了算例，驗(yàn)證了濾波模型的正確性并對(duì)2種濾波模型的特性進(jìn)行比較。

1 推力偏心和質(zhì)心漂移的干擾模型

對(duì)于大氣層外直接側(cè)向力控制的飛行器，軌控發(fā)動(dòng)機(jī)一般采用4臺(tái)發(fā)動(dòng)機(jī)正交布置，姿控發(fā)動(dòng)機(jī)則多采用4臺(tái)、6臺(tái)、8臺(tái)發(fā)動(dòng)機(jī)3種布局方式。本文以八姿控發(fā)動(dòng)機(jī)布局形式為例建立仿真模型。如圖1所示，飛行器的4個(gè)軌控發(fā)動(dòng)機(jī)呈正交布置在過(guò)質(zhì)心的平面上，飛行器的8個(gè)姿控發(fā)動(dòng)機(jī)布置在飛行器尾部。軌控發(fā)動(dòng)機(jī)和姿控發(fā)動(dòng)機(jī)推力平面均與飛行器體坐標(biāo)軸的x軸垂直。飛行器體坐標(biāo)系固連于飛行器，原點(diǎn)O位于飛行器質(zhì)心上。Ox1軸與飛行器彈體縱軸重合，指向頭部為正，Oy1軸在彈體縱向?qū)ΨQ平面內(nèi)，垂直O(jiān)x1軸，指向上方為正，Oz1軸方向按右手定則確定。發(fā)射坐標(biāo)系與飛行器未發(fā)射前的體坐標(biāo)系重合，為定坐標(biāo)系。

圖1 飛行器發(fā)動(dòng)機(jī)布局Fig.1 Position of engine

1.1 推力偏心引起的干擾力

在飛行器軌控發(fā)動(dòng)機(jī)工作時(shí)，質(zhì)心漂移只產(chǎn)生干擾力矩，推力偏心則產(chǎn)生干擾力和干擾力矩。首先，計(jì)算由推力偏心引起的干擾力。

(1)

1.2 推力偏心引起的干擾力矩

(2)

1.3 質(zhì)心漂移引起的干擾力矩

(3)

2 濾波模型建立

2.1 建立狀態(tài)方程、量測(cè)方程

本文首先研究一個(gè)軌控發(fā)動(dòng)機(jī)工作狀態(tài)下濾波模型的建立。以3號(hào)發(fā)動(dòng)機(jī)為例，3號(hào)軌控發(fā)動(dòng)機(jī)工作時(shí)，推力偏心t3x,t3z和質(zhì)心漂移xpy,zpy會(huì)產(chǎn)生干擾力矩。這些干擾力矩作用到飛行器上，便會(huì)產(chǎn)生相應(yīng)角速度wx1,wy1,wz1的變化。同時(shí)推力偏心t3x,t3z還會(huì)產(chǎn)生相應(yīng)的x，z方向的干擾力，進(jìn)而產(chǎn)生相應(yīng)的x，z方向速度vx,vz的變化。因此，本文選取(wx1,wy1,wz1,vx,vz)T為濾波的量測(cè)參數(shù)，選取量測(cè)參數(shù)再加上所要估計(jì)的t3x,t3z,xpy,zpy為狀態(tài)參數(shù)X(t)=(wx1,wy1,wz1,t3x,t3z,xpy,zpy，vx,vz)T。

根據(jù)飛行器的動(dòng)力學(xué)模塊，針對(duì)只有3號(hào)軌控發(fā)動(dòng)機(jī)工作的情況下，建立估算3號(hào)軌控發(fā)動(dòng)機(jī)推力偏心以及質(zhì)心漂移的狀態(tài)方程和量測(cè)方程。

狀態(tài)方程如下：

(4)

方程(4)可簡(jiǎn)寫(xiě)為如下形式：

X(t)=f[X(t),u(t)],

(5)

式中：X(t)為狀態(tài)向量;u(t)狀態(tài)噪聲。

測(cè)量方程為

Z(t)=H·X(t)+v(t),

(6)

式中：Z(t)=(wx1,wy1,wz1,vx,vz)T;

v(t)=(ξwx,ξwy,ξwz,ξvx,ξvz)T;

Mx1,My1,Mz1為作用在飛行器上的力矩在3個(gè)體坐標(biāo)系軸上的分量；Jx,Jy,Jz為飛行器繞3個(gè)體坐標(biāo)軸的轉(zhuǎn)動(dòng)慣量;Fx,Fz為飛行器受到的x方向和z方向的分力;ξx,ξz,ξt3x,ξt3z,ξwx,ξwy,ξwz,ξvx,ξvz為零均值的高斯白噪聲。

2.2 建立擴(kuò)展卡爾曼濾波(EKF)模型[2-4]

我們可以將上述的狀態(tài)方程和量測(cè)方程簡(jiǎn)寫(xiě)為如下的形式：

X(t)=f[X(t),u(t)],Z(t)=H·X(t)+v(t).

(7)

可以看出，上述系統(tǒng)是一個(gè)非線性系統(tǒng)。對(duì)于非線性系統(tǒng)而言，應(yīng)用擴(kuò)展卡爾曼濾波估算時(shí)需要首先將非線性函數(shù)在估計(jì)點(diǎn)附近泰勒展開(kāi)，舍棄其高階分量，使得非線性模型線性化。將狀態(tài)方程在估計(jì)值附近進(jìn)行展開(kāi)，只保留一階項(xiàng)，可得狀態(tài)轉(zhuǎn)移矩陣為Φ(t)=I+F(t)Δt，其中Δt為采樣時(shí)間，F(xiàn)(t)為非線性的狀態(tài)方程對(duì)x的各個(gè)元素求偏導(dǎo)數(shù)的雅克比矩陣。

(8)

對(duì)上述連續(xù)系統(tǒng)進(jìn)行離散化可得，狀態(tài)方程和測(cè)量方程如下：

(9)

式中：w(k)為狀態(tài)噪聲;v(k)為測(cè)量噪聲。

將方程(8)帶入到卡爾曼濾波中，可建立如下濾波模型：

狀態(tài)參數(shù)初始化：

設(shè)定初始狀態(tài)向量X0為全0的9維列向量。協(xié)方差矩陣P0為對(duì)角值都為0.001，其他元素為0的9維方陣，同時(shí)系統(tǒng)噪聲和量測(cè)噪聲的協(xié)方差矩陣Q和R為元素全為0.001的9維列向量。

預(yù)測(cè)方程：

(10)

增益矩陣：

K(k+1)=P(k+1|k)·HT(k+1)·[H(k+1)·P(k+1|k)·HT(k+1)+R(k+1)]-1.

(11)

預(yù)測(cè)誤差協(xié)方差陣：

P(k+1|k)=Φ(k+1,k)·P(k|k)·Φ′(k+1,k)+

Γ(k+1,k)Q(k)?！?k+1,k).

(12)

濾波誤差協(xié)方差陣：

P(k+1|k+1)=[I-K(k+1)·H(k+1)]P(k+1|k).

(13)

(14)

2.3 建立無(wú)跡卡爾曼濾波(UKF)模型[5-8]

UKF是一種用采樣策略來(lái)逼近非線性函數(shù)的方法，它通過(guò)UT變換選取的一組權(quán)值不同的Sigma點(diǎn)來(lái)遞推傳播系統(tǒng)狀態(tài)和量測(cè)的不確定性，將UT變換得到的均值、估計(jì)方差和量測(cè)方差引入到線性卡爾曼濾波的逐步遞推過(guò)程中，是卡爾曼濾波向非線性系統(tǒng)擴(kuò)展的一種新方法[9-10]。UKF不需計(jì)算Jacobian矩陣，該算法直接利用非線性模型，避免引入線性化誤差，提高了濾波精度。UKF的計(jì)算量與EKF相當(dāng)，但性能優(yōu)于EKF。

本文的狀態(tài)方程和量測(cè)方程可寫(xiě)成如下形式：

(15)

式中：f(X(k))為以動(dòng)力學(xué)方程為基礎(chǔ)的非線性變換。利用UKF算法處理時(shí)，不需要對(duì)上述非線性變換進(jìn)行線性化截?cái)?，直接利用UKF算法實(shí)現(xiàn)即可。具體UKF算法流程如下：

(1) 狀態(tài)參數(shù)初始化，初始參數(shù)設(shè)定與EKF相同。

(2) 對(duì)任意時(shí)刻k

(16)

2) 時(shí)間更新

χk/k-1=χk-1+f(χk-1)·h,

(17)

(18)

(19)

Z(k|k-1)=H·χ(k|k-1),

(20)

(21)

3) 測(cè)量更新

(22)

(23)

(24)

(25)

(26)

3 濾波模型的仿真分析

為了驗(yàn)證濾波模型的準(zhǔn)確性與收斂速度。本文建立了一個(gè)飛行器的仿真模型[11-15]。在仿真模型的動(dòng)力學(xué)模塊中加入推力偏心和質(zhì)心漂移作為仿真輸入。濾波模型中所需的角速度wx1,wy1,wz1和速度vx,vz可以通過(guò)仿真模型獲得。將濾波結(jié)果與所給的推力偏心和質(zhì)心漂移值進(jìn)行比對(duì)，即可驗(yàn)證兩種濾波模型的正確性和特性。

為充分驗(yàn)證濾波模型的穩(wěn)定性和正確性，本文針對(duì)估算3號(hào)發(fā)動(dòng)機(jī)，給出下列幾種不同情況下的推力偏心和質(zhì)心漂移值。

(1) 推力偏心t3x,t3z和質(zhì)心漂移xpy,zpy均為正值；

(2) 推力偏心t3x,t3z和質(zhì)心漂移xpy,zpy均為負(fù)值；

(3) 推力偏心t3x,t3z為一正一負(fù)，質(zhì)心漂移xpy一正一負(fù)。

估算結(jié)果如表1～3,圖3～8所示。

表1 情況1濾波結(jié)果Table 1 Filtration result for the first time

圖3 EKF情況1估算過(guò)程Fig.3 Estimate for the first time by EKF

圖4 UKF情況1估算過(guò)程Fig.4 Estimate for the first time by UKF

表2 情況2濾波結(jié)果
Table 2 Filtration result for the second time

項(xiàng)目t3x/radt3z/radxpy/mzpy/m仿真輸入-0．002-0．002-0．001-0．001EKF輸出-0．00196-0．00203-0．00101-0．00099UKF輸出-0．00199-0．00196-0．001003-0．001

圖5 EKF情況2估算過(guò)程Fig.5 Estimate for the second time by EKF

圖6 UKF情況2估算過(guò)程Fig.6 Estimate for the second time by UKF

表3 情況3濾波結(jié)果
Table 3 Filtration result for the third time

項(xiàng)目t3x/radt3z/radxpy/mzpy/m仿真輸入-0．0010．0020．001-0．001EKF輸出-0．000980．001980．001-0．001UKF輸出-0．00105-0．002030．00099-0．00099

圖7 EKF情況3估算過(guò)程Fig.7 Estimate for the third time by EKF

圖8 UKF情況3估算過(guò)程Fig.8 Estimate for the third time by UKF

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

(1) 通過(guò)仿真分析，EKF和UKF均可應(yīng)用于估算發(fā)動(dòng)機(jī)的推力偏心和飛行器的質(zhì)心漂移，且精度良好。相較于EKF，UKF的精度較高，收斂更為平穩(wěn)。EKF和UKF的收斂速度相當(dāng)，計(jì)算量相當(dāng)。故對(duì)于推力偏心和質(zhì)心漂移的估算，UKF濾波模型要優(yōu)于EKF濾波模型。

(2) 由于本文在濾波模型中所用的參數(shù)在現(xiàn)實(shí)的飛行試驗(yàn)中均可以通過(guò)導(dǎo)航系統(tǒng)獲得，故本方法可以應(yīng)用到采用直接側(cè)向力控制的飛行器的推力偏心和質(zhì)心漂移估算工作當(dāng)中。本論文中所有濾波初值都選為0，故本論文濾波模型同樣適應(yīng)推力偏心和質(zhì)心漂移極性未知的情況。

[1] MINJEA T, TOM T, BRIAN W. KHIT Post-Flight Data Reduction Techniques Based Extended Filter[J].AIAA，1989,7(3): 1843-1897.

[2] 侯代文，殷福亮.非線性濾波及其在說(shuō)話人跟蹤中的應(yīng)用研究[D]. 大連：大連理工大學(xué)，2008. HOU Dai-wen,YIN Fu-liang, Research on Nonlinear Filtering with Application to Speaker Tracking[D].Dalian:Dalian University of Technology,2008.

[3] 聶琦，趙琳.非線性濾波方法在導(dǎo)航系統(tǒng)中的研究[D]. 哈爾濱：哈爾濱工程大學(xué),2008. NIE Qi,ZHAO Lin. Nonlinear Filtering and Its Application in Navigation System[D]. Harbin:Harbin Engineering University,2008.

[4] 楊萌.非線性濾波及在慣導(dǎo)系統(tǒng)傳遞對(duì)準(zhǔn)中應(yīng)用研究[D]. 哈爾濱：哈爾濱工程大學(xué),2010. YANG Meng.Research on Nonlinear Filtering and the Application in Transfer Alignment of Inertial Navigation System[D]. Harbin:Harbin Engineering University,2010.

[5] 郭雪姣.非線性濾波算法研究及在衛(wèi)星高精度定軌中的應(yīng)用[D]. 長(zhǎng)沙：國(guó)防科學(xué)技術(shù)大學(xué)，2010. GUO Xue-jiao. Research on Non-linear Filter Methods for High Precision Satellites Orbit Determination[D].Changsha:National University of Defence Technology,2010.

[6] 張?chǎng)蚊?非線性濾波在通信與導(dǎo)航中的應(yīng)用研究[D]. 北京：北京郵電大學(xué),2012. ZHANG Xin-ming.Nonlinear Filtering with Applications to Communication and Navigation Systems[D].Beijing:Beijing University of Posts and Telecommunications,2010.

[7] 施巖龍，王建新.非線性濾波技術(shù)在彈道目標(biāo)跟蹤中的應(yīng)用[D]. 南京：南京理工大學(xué),2012. SHI Yan-long,WANG Jian-xin.Application of Nonlinear Filtering in Ballistic Target Tracking[D].Nanjing:Nanjing University of Science and Technology,2012.

[8] 唐燕輝，彭輝.非線性濾波算法及在神經(jīng)網(wǎng)絡(luò)與金融市場(chǎng)建模中的應(yīng)用[D].長(zhǎng)沙：中南大學(xué)，2013. TANG Yan-hui,PENG Hui. Nonlinear Filtering Algorithms and Applications in Neural Network and Financial Market Modeling[D]. Changsha:Central South University,2013.

[9] ANDREWS A.A Square Root Formulation of the Kalman filtering[J].AIAA ,1968,6(6):1165-1166.

[10] VERHAEGEN M H, DOOREN P V.Numerical Aspects of Different Kalman Filter Implementations[J].IEEE 1986,AC-31(10):907-917.

[11] 郝波.直接側(cè)向力技術(shù)的應(yīng)用研究[D]. 西安：西北工業(yè)大學(xué),2004. HAO Bo.Research on the Application of Direct Lateral Force[D]. Xi’an: College of Astronautics Northwestern Polytechnical University,2004.

[12] 程鵬. 自動(dòng)控制原理[M].北京：高等教育出版社，2010. CHENG Peng. Automatic Control Principle[M]. Beijing: Higher Education Press，2010.

[13] 錢杏芳，林瑞雄，趙亞男.導(dǎo)彈飛行力學(xué)[M].北京：北京理工大學(xué)出版社，2012. QIAN Xing-fang，LIN Rui-xiong，ZHAO Ya-nan，et al. Missile Flight Aerodynamics [M]. Beijing: Beijing Institute of Technology Press，2012.

[14] GUELMAN M.A Optimal Guidance Law in the Plane[J].Journal of Guidance,1984,7(4):471-476.

[15] GUELMAN M. A Qualitative Study of Proportional Navigation[J].IEEE Transactions on Aerospace and Electrionic Systems,1971,7(3):638-643.

Estimate Methods Research on Thrust Eccentricity and Centroid Drift of the Exoatmosphere Aircraft

ZHANG Peng,TAN Xiang-xia

(Beijing Institute of Electronic System Engineering, Beijing 100854, China)

For the exoatmosphere aircraft that are controlled by direct lateral force, divert thruster will cause them to roll, yaw and pitch and generate attitude interference torque during thruster switching on. If the disturbance torque is too large, it can lead to loss of stability attitude. Firstly, a simulation model of the aircraft based on the direct lateral force is established. Secondly, a thrust eccentricity and centroid drift model is deduced through extended kalman filter (EKF) and unscented kalman filter (UKF) based on kinetic equation. Finally, a simulation example is given to verify the correctness of the model and two filter models' characteristics was compared.

direct lateral force；thrust eccentricity；centroid drift；extended kalman filter(EKF)；unscented kalman filter(UKF)

2015-01-05；

2015-01-22

有

張朋(1990-)，男，安徽阜陽(yáng)人。碩士生，主要研究方向?yàn)轱w行器設(shè)計(jì)。

通信地址：100854 北京市142信箱30分箱 E-mail：zp_19901016@126.com

10.3969/j.issn.1009-086x.2015.04.011

TJ765.2；TP391.9

A

1009-086X(2015)-04-0062-06