改進(jìn)粒子濾波的全捷聯(lián)導(dǎo)引頭制導(dǎo)信息估計(jì)*

龐威,謝曉方,劉家祺,孫濤

(海軍航空工程學(xué)院 兵器科學(xué)與技術(shù)系，山東 煙臺 264001)

0 引言

新型滑翔制導(dǎo)炸彈是在普通航彈上加裝被動(dòng)式紅外圖像導(dǎo)引頭，實(shí)現(xiàn)防區(qū)外投放和打擊目標(biāo)的。而導(dǎo)引頭分為框架式導(dǎo)引頭和全捷聯(lián)導(dǎo)引頭：傳統(tǒng)框架式紅外圖像導(dǎo)引頭具有較大的視場角，且能夠直接提取比例制導(dǎo)等所需的視線角速率信息，但由于框架的存在，結(jié)構(gòu)復(fù)雜，成本較高，可靠性較低。隨著非制冷紅外探測器、CCD探測器等為代表的紅外圖像制導(dǎo)系統(tǒng)具有結(jié)構(gòu)簡單、成本低、設(shè)計(jì)方便等優(yōu)點(diǎn)，其在精確制導(dǎo)系統(tǒng)中應(yīng)用成為目前研究的熱點(diǎn)。全捷聯(lián)紅外圖像導(dǎo)引頭是將光學(xué)系統(tǒng)、相機(jī)和圖像處理模塊等直接固聯(lián)在制導(dǎo)彈藥基座上，降低了復(fù)雜度和成本，提高了系統(tǒng)的可靠性；采用高分辨率的成像器件提高了角度分辨率以彌補(bǔ)無框架時(shí)視場角的降低；但無法直接測量制導(dǎo)所需的視線角速率信息，且由于視場角較大會(huì)引入較大的測量噪聲，因此需要設(shè)計(jì)合理的算法對其進(jìn)行解耦和信息估計(jì)。

相關(guān)文獻(xiàn)已開展了全捷聯(lián)式圖像導(dǎo)引頭視線角速率解耦和制導(dǎo)信息估計(jì)的研究。Maley James[1]和Jacques Waldman[2]分別采用卡爾曼濾波和擴(kuò)展卡爾曼濾波(extended Kalman filter, EKF)方法研究了視線角速度估計(jì)問題；Smita Sadhu[3]采用擾動(dòng)觀測器研究了視線角速度估計(jì)問題；曹衛(wèi)衛(wèi)[4]通過構(gòu)建基于FPGA的硬件卡爾曼濾波系統(tǒng)實(shí)現(xiàn)對彈體視線角速度的求導(dǎo)計(jì)算；袁亦方[5]提出了一種采用EKF濾波估計(jì)方法，用以解決單兵全捷聯(lián)圖像導(dǎo)引頭體視線角速度估計(jì)問題；孫婷婷[6]通過建立全捷聯(lián)導(dǎo)引頭的數(shù)學(xué)模型，分別采用微分+穩(wěn)態(tài)卡爾曼濾波方法和無跡卡爾曼濾波方法估計(jì)體視線角速度；王小剛[7]采用Huber-Based魯棒濾波研究了大氣層外飛行器捷聯(lián)導(dǎo)引頭體視線角估計(jì)問題；張韜[8]提出了一種基于無跡施密特卡爾曼濾波方法，并對落角約束導(dǎo)引律下對機(jī)動(dòng)目標(biāo)進(jìn)行了仿真。上述是基于觀測噪聲為高斯白噪聲條件下的視線角速度估計(jì)問題，實(shí)際上由于導(dǎo)引頭和炸彈制作工藝、導(dǎo)引頭安裝誤差、以及大的視場角導(dǎo)致體大量觀測噪聲等問題，并使其呈現(xiàn)非高斯特性從而導(dǎo)致視線角速度估計(jì)精度的降低。

針對上述問題，本文以滑翔制導(dǎo)炸彈中全捷聯(lián)導(dǎo)引頭的應(yīng)用為研究對象，通過推導(dǎo)視線角速度解耦公式，建立了視線角和角速度的狀態(tài)模型和觀測模型，然后利用改進(jìn)的粒子濾波算法估計(jì)視線角速度，并進(jìn)行了半實(shí)物仿真實(shí)驗(yàn)。

1 相關(guān)坐標(biāo)系和角度的定義

1.1 坐標(biāo)系的定義

為方便提取視線角速率，首先定義本文所需的坐標(biāo)系。

(1) 地面標(biāo)系Oxyz

與地表固連，取炸彈投放點(diǎn)在地面的投影點(diǎn)為坐標(biāo)原點(diǎn)O；彈道面與地面水平面的交線為Ox軸，正方向指向目標(biāo)；Oy軸沿當(dāng)?shù)卮咕€指向天頂；Oz軸根據(jù)右手法則定義。

(2) 彈體坐標(biāo)系Oxdydzd

與彈體固連，取炸彈質(zhì)心為坐標(biāo)原點(diǎn)O；炸彈縱軸為Oxd軸，正方向指向炸彈頭部；Oyd軸位于炸彈縱向?qū)ΨQ剖面內(nèi)，指向上為正；Ozd根據(jù)右手法則確定。

(3) 視線坐標(biāo)系Oxsyszs

取炸彈質(zhì)心為坐標(biāo)原點(diǎn)O；Oxs軸為炸彈和目標(biāo)的連線，正方向指向目標(biāo)；Ozs軸位于地面坐標(biāo)系中Oxz面內(nèi)，與Oxs軸垂直；Oys軸根據(jù)右手法則確定。

(4) 體視線坐標(biāo)系Oxtytzt

取炸彈質(zhì)心為坐標(biāo)原點(diǎn)O；Oxt軸為炸彈與目標(biāo)的連線，正方向指向目標(biāo)；Ozt軸位于彈體坐標(biāo)系中Oxdzd面內(nèi)，與Oxt軸垂直；Oyt根據(jù)右手法則確定。

1.2 角度的定義

qγ為視線高低角：表示視線坐標(biāo)系中Oxs與地面坐標(biāo)系中Oxz面之間的夾角。

qα為體視線高低角：表示體視線坐標(biāo)系Oxt軸與彈體坐標(biāo)系Oxdzd面之間的夾角。

qst為視線變換角：表示視線坐標(biāo)系Oys軸到體視線坐標(biāo)系Oyt軸之間的變換角。

?,ψ,γ分別為俯仰角、偏航角與滾轉(zhuǎn)角，其定義參見文獻(xiàn)[9]。

各坐標(biāo)系之間的轉(zhuǎn)換關(guān)系如圖1所示。其中L(*)表示轉(zhuǎn)換矩陣。

圖1 坐標(biāo)系之間的轉(zhuǎn)換關(guān)系Fig.1 Transformation between coordinate systems

2 視線角速度提取

2.1 視線角和角速度解耦方法

滑翔制導(dǎo)炸彈在末制導(dǎo)過程中，彈目相對地面坐標(biāo)系始終是運(yùn)動(dòng)的，全捷聯(lián)紅外圖像導(dǎo)引頭只能夠測量出體視線角的大小，其含有視線角和彈體姿態(tài)角，而比例導(dǎo)引方式所需的是視線坐標(biāo)系下的視線高低角和方位角以及視線高低角速度和方位角速度，因此需要將導(dǎo)引頭測量信號中耦合的彈目運(yùn)動(dòng)信息去除。

設(shè)向量ωd為彈體坐標(biāo)系相對于地面坐標(biāo)系3個(gè)方向上的旋轉(zhuǎn)角速度，則其在彈體坐標(biāo)系中可表示為

ωd=(ωx,ωy,ωz)T.

(1)

設(shè)向量ωst表示視線坐標(biāo)系相對于體視線坐標(biāo)系3個(gè)方向上的旋轉(zhuǎn)角速度，則其在視線坐標(biāo)系中可表示為

(2)

設(shè)向量ωtd表示體視線坐標(biāo)系相對于彈體坐標(biāo)系3個(gè)方向上的旋轉(zhuǎn)角速度，其在彈體坐標(biāo)系下可表示為

(3)

設(shè)向量ωse表示視線坐標(biāo)系相對地面坐標(biāo)系3個(gè)方向上的旋轉(zhuǎn)角速度，則其在地面坐標(biāo)系下可表示為

ωse=L(γ,?,ψ)(ωd+ωtd)+L(qγ,qλ)ωst.

(4)

此外，根據(jù)視線坐標(biāo)系和地面坐標(biāo)系之間的轉(zhuǎn)換關(guān)系，ωse也可表示為

(5)

根據(jù)式(4)和式(5)可得

(6)

式中：

設(shè)彈目距離在某時(shí)刻為R，由彈目相對運(yùn)動(dòng)關(guān)系和前文定義的坐標(biāo)系可知目標(biāo)在體視線坐標(biāo)系和視線坐標(biāo)系上的坐標(biāo)均為(R,0,0)T，根據(jù)圖1可得目標(biāo)在地面坐標(biāo)系和彈體坐標(biāo)系中的坐標(biāo)分別為

(7)

(8)

同時(shí)，又根據(jù)地面坐標(biāo)系和彈體坐標(biāo)系的轉(zhuǎn)換關(guān)系，可得式(9)。

(9)

化簡,得

式中：

M= -cos ?sinψcosqαcosqβ+

(sin ?sinψcosγ+cosψsinγ)sinqα-

(-sin ?sinψsinγ+cosψcosγ)·

cosqαsinqβ；

N= cos ?sinψcosqαcosqβ+

(-sin ?cosψcosγ+sinψsinγ)sinqα-

(sin ?cosψsinγ+sinψcosγ)·

cosqαsinqβ.

至此，已推導(dǎo)出視線角高低角和視線角方位角度求解公式。

2.2 視線角速度估計(jì)模型

全捷聯(lián)紅外圖像導(dǎo)引頭測量的信息既包含體視線高低角qα和體視線方位角qβ，又包含大量的非高斯噪聲[10]；而制導(dǎo)信息為視線高低角、視線方位角和視線高低速度、視線方位角速度，因此本文以這4個(gè)量為狀態(tài)變量建立系統(tǒng)的狀態(tài)方程。

圖2 彈目運(yùn)動(dòng)關(guān)系Fig.2 Relationship between missile and target

(11)

觀測方程為

(12)

式中：

v1和v2為觀測噪聲。由于全捷紅外圖像導(dǎo)引頭相對于普通導(dǎo)引頭具有更大的視場，因此也導(dǎo)致其觀測噪聲的增大，尤其是非高斯噪聲的增大，嚴(yán)重影響了EKF方法的估算精度和穩(wěn)定特性；同時(shí)，從式(11)和(12)中也可看出狀態(tài)方程和觀測方程具有較強(qiáng)的非線性，而粒子濾波對非線性系統(tǒng)、非高斯噪聲具有較高的估計(jì)精度，因此本文采用粒子濾波求解體視線角速度。

3 基于改進(jìn)粒子濾波的視線角速率求解

與EKF方法不同，粒子濾波是一種以蒙特卡羅仿真和遞推貝葉斯估計(jì)為基礎(chǔ)的統(tǒng)計(jì)濾波方法[12]。其主要思想是首先在狀態(tài)空間中產(chǎn)生一組以系統(tǒng)狀態(tài)向量的經(jīng)驗(yàn)條件分布為依據(jù)的粒子集合，并設(shè)定粒子的初始權(quán)重；然后根據(jù)系統(tǒng)的觀測值不斷調(diào)整集合中粒子的權(quán)重和位置用以修正初始設(shè)定的經(jīng)驗(yàn)條件分布。其本質(zhì)是用由粒子集合和對應(yīng)權(quán)重來組成離散隨機(jī)測度，用以近似系統(tǒng)狀態(tài)的相關(guān)概率分布，然后通過遞推算法來更新下一步的離散隨機(jī)測度。當(dāng)集合中的粒子數(shù)量很大時(shí)，這種蒙特卡羅描述即可近似地表示系統(tǒng)狀態(tài)變量真實(shí)的后驗(yàn)概率密度函數(shù)。

3.1 標(biāo)準(zhǔn)粒子濾波算法

(13)

式(13)中權(quán)值可通過重采樣法選擇，可表示為

(14)

(15)

將式(15)帶入到式(14)中，可求得重要權(quán)值為

(16)

將權(quán)值歸一化，即

(17)

式(13)～(17)表示基本粒子濾波中權(quán)重的求解方法。采用式(16)求解的粒子權(quán)重可能會(huì)隨著時(shí)間的增加，導(dǎo)致重要性權(quán)值集中在少數(shù)粒子，僅靠這些粒子已經(jīng)不能夠有效地表達(dá)出后驗(yàn)概率密度函數(shù)，即產(chǎn)生匱乏現(xiàn)象。為克服粒子匱乏，本文提出一種分層采用的粒子濾波算法。

3.2 基于分層采樣的粒子濾波算法

分層采樣中，將采樣空間劃分為多個(gè)部分，每個(gè)部分稱為層。在蒙特卡羅積分應(yīng)用中，把一個(gè)積分空間分成幾個(gè)子空間，然后分別計(jì)算每個(gè)子空間的積分值。因此，相對于隨機(jī)采樣，分層采樣可有效的減少采樣誤差。

考慮到函數(shù)P(x)具有如下的形式：

(18)

(19)

根據(jù)比例分配原則，每層采樣的粒子個(gè)數(shù)為

Ni=floor(Nγi).

(20)

由此，后驗(yàn)概率可近似地表示為

(21)

綜上所述，當(dāng)N→∞時(shí)，可由大數(shù)定理保證式(21)的值逼近于真實(shí)后驗(yàn)概率。

3.3 改進(jìn)粒子濾波算法步驟

由3.2節(jié)，將改進(jìn)的粒子濾波流程總結(jié)如圖3所示。

圖3 改進(jìn)粒子濾波算法流程圖Fig.3 Flow chart of improved particle filter algorithm

在本文中采用改進(jìn)粒子濾波的步驟如下：

Step 1:初始化。

Step 2:重要性采樣。

在k時(shí)刻，采用式(22)更新粒子權(quán)值：

i=1,2,…,N,

(22)

其中:h(·)為測量方程，即式(12)；并采用式(23)對權(quán)值歸一化：

(23)

那么可得k時(shí)刻未知參數(shù)x的最小均方誤差估計(jì)為

(24)

Step 3：分層采樣。

選取重要性函數(shù)為P(X(k)|X(k-1))，并根據(jù)式(11)進(jìn)行一步預(yù)測得到新的粒子集合：

根據(jù)分層采樣理論，構(gòu)造出不同層次之間的高斯混合概率密度函數(shù)為

(25)

并由此重新獲得粒子集合

Step 4:輸出結(jié)果：k=k+1，轉(zhuǎn)入Step 2。

4 半實(shí)物仿真實(shí)驗(yàn)與分析

4.1 角速度估計(jì)半實(shí)物仿真實(shí)驗(yàn)

4.2 仿真設(shè)置

某新型滑翔制導(dǎo)炸彈采用三維比例制導(dǎo)方式打擊目標(biāo)[14]，末制導(dǎo)啟控點(diǎn)坐標(biāo)為(0,2 000,0)m；初始速度vm0=(100,10,10) m/s,此刻俯仰高低角?m0=5.68°；初始航向角ψm0=5.71°，3個(gè)方向的初始加速度均為0。目標(biāo)初始位置為：(400,0,600)m,初始航向角為ψt0=0°，目標(biāo)作蛇形機(jī)動(dòng)，其運(yùn)動(dòng)規(guī)律如式(26)所示。

圖4 半實(shí)物仿真硬件圖Fig.4 Hardware of the semi-physical simulation

圖5 半實(shí)物仿真實(shí)驗(yàn)原理圖Fig.5 Schematic diagram of semi-physical simulation

(26)

設(shè)定本文的采樣時(shí)間dt=0.01 s，粒子個(gè)數(shù)為1 000。為驗(yàn)證本文方法的有效性，同時(shí)采用EKF算法[5]、標(biāo)準(zhǔn)粒子濾波方法(particle filter,PF)與理想無干擾理論計(jì)算比例導(dǎo)引律作為真值進(jìn)行對比[15]；攻擊目標(biāo)過程中，末制導(dǎo)方式為比例導(dǎo)引律，其仿真結(jié)果如圖6～8所示。

定義角度和角速度的均方誤差作為算法的評價(jià)指標(biāo)，即

(27)

4.3 仿真分析

圖7表示采用改進(jìn)粒子濾波得到的視線角和視線角速度的估計(jì)值和真值的對比結(jié)果。圖8表示分

別采用EKF濾波方法和標(biāo)準(zhǔn)粒子濾波(圖中PF線)以及本文改進(jìn)的粒子濾波(圖中IPF線)方法得到的濾波誤差，表1和表2分別表示采用3種方法得到最大估計(jì)誤差和均方誤差。從表中可以看出,采用改進(jìn)粒子濾波方法估計(jì)精度高于EKF和PF方法。

4.4 原因分析

采用改進(jìn)粒子濾波方法的估計(jì)精度高于EKF和PF方法，主要原因是由于導(dǎo)引頭測量的噪聲并非高斯白噪聲，同時(shí)由于組合導(dǎo)航模塊中陀螺和慣導(dǎo)測量誤差以及三軸轉(zhuǎn)臺控制精度等影響，導(dǎo)致觀測方程中測量噪聲含有大量非高斯噪聲；同時(shí)由于EKF濾波方法中的狀態(tài)轉(zhuǎn)移矩陣和觀測矩陣采用一階Taylor展開近似，忽略了二階以上的高階項(xiàng)，因此不可避免地引入了線性化誤差，從而造成精度的下降。在PF方法中，由于粒子在迭代中發(fā)生匱乏現(xiàn)象，造成濾波精度低于改進(jìn)的粒子濾波方法。

圖6 攻擊彈道Fig.6 Attack trajectory

方法qγmax/(°)qλmax/(°qγmax/((°)·s-1qλmax/((°)·s-1)PF0.70120.853213.010216.5813EKF2.33321.859023.390317.8363IPF0.20470.28853.45274.7816

表2 均方誤差對比

圖7 視線角度和角速度估計(jì)Fig.7 Estimation LOS and velocity of LOS

圖8 誤差分析Fig.8 Error analysis

5 結(jié)束語

為加快推進(jìn)全捷聯(lián)紅外圖像導(dǎo)引頭在新型滑翔制導(dǎo)炸彈中的應(yīng)用，本文推導(dǎo)出了視線角和角速度的解耦公式，并提出了基于改進(jìn)粒子濾波的體視線角速度估計(jì)方法，設(shè)計(jì)了半實(shí)物仿真實(shí)驗(yàn)進(jìn)行驗(yàn)證，主要結(jié)論如下：

(1) 相比于EKF和PF方法，采用基于改進(jìn)粒子濾波的體視線角速度估計(jì)方法能夠得到較高精度的彈目視線角和角速度信息，能滿足滑翔制導(dǎo)炸彈對海/地平面攻擊所需比例導(dǎo)引律視線角速度的要求。

(2) 半實(shí)物仿真實(shí)驗(yàn)結(jié)果表明：基于改進(jìn)粒子濾波方法的全捷聯(lián)紅外圖像末制導(dǎo)導(dǎo)引頭制導(dǎo)信息精度較高，具有較強(qiáng)的實(shí)用價(jià)值，可為新型滑翔紅外圖像制導(dǎo)炸彈的研制提供一定的參考。

[1] MALEY J M.Line of Sight Rate Estimation for Guided Projectiles with Strapdown Seekers[J].AIAA Guidance,Navigation and Control Conference,2015,32(6):2346-2351.

[2] JACQUES W.Line-of-Sight Rate Estimation and Linearizing Control of an Imaging Seeker in a Tactical Missile Guided by Proportional Navigation[J].IEEE Transactions on Control Systems Technology,2002,10(4):556-567.

[3] SMITA S,GHOSHAL T K.Sight Line Rate Estimaion in Missile Seeker Using Disturbance Observer-Based Technique[J].IEEE Transactions on Control Systems Tehcnology,2011,19(2):449-454.

[4] 曹衛(wèi)衛(wèi).捷聯(lián)導(dǎo)引頭視線角速率估計(jì)方法研究[D].南京:南京理工大學(xué),2012. CAO Wei-wei,Study on Estimation Method of Los Rate of Strapdown Seeker[D].Nanjing:Nanjing University of Science and Technology,2012.

[5] 袁亦方,林德福,祁載康,等.單兵全捷聯(lián)圖像制導(dǎo)彈藥制導(dǎo)信息估計(jì)技術(shù)[J].紅外與激光工程,2015,44(1):370-376. YUAN Yi-fang,LIN De-fu,QI Zai-kang,et al.Techniques on Estimating Guidance Information for Strapdown Image Guided Man Portable Munitions[J].Infrared and Laser Engineering,2015,44(1):370-376.

[6] 孫婷婷,儲海榮,賈宏光,等.捷聯(lián)式光學(xué)導(dǎo)引頭視線角速率解耦與估計(jì)[J].紅外與激光工程,2014,43(5):1587-1593. SUN Ting-ting,CHU Hai-rong,JIA Hong-guang,et al.Line-of-Sight Angular Rate Decoupling and Estimationof Strapdown Optical Seeker[J].Infrared and Laser Engineering,2014,43(5):1587-1593.

[7] 王小剛,胡智勇,于洋,等.基于魯棒濾波的捷聯(lián)導(dǎo)引頭視線角速度估計(jì)方法[J].中國慣性技術(shù)學(xué)報(bào),2016,24(2):251-256. WANG Xiao-gang,HU Zhi-yong,YU Yang,et al.Line-of-Sight Angular-Rate Estimation of Strapdown Seeker Based on Robust Filter[J].Journal of Chinese Inertial Technology,2016,24(2):251-256.

[8] 張韜,張希銘,王民鋼.全捷聯(lián)導(dǎo)引頭制導(dǎo)信息濾波算法及仿真[J].西北工業(yè)大學(xué)學(xué)報(bào),2015,33(5):744-749. ZHANG Tao,ZHANG Xi-ming,WANG Min-gang.Algorithm and Simulation for Guidance Information Filter for Strapdown Seekers[J].Journal of Northwestern Polytechnical University,2015,33 (5) :744-749.

[9] 李新國,方群.有翼導(dǎo)彈飛行動(dòng)力學(xué)[M].西安: 西北工業(yè)大學(xué)出版社,2005:28-36. LI Xin-guo,FANG Qun.Winged Missile Flight Dynamics[M].Xi′an: Northwestern Polytechnical University Press,2005:28-36.

[10] 魯天宇,尹健,夏群利,等.基于波束角誤差補(bǔ)償?shù)南嗫仃噷?dǎo)引頭解耦算法[J].系統(tǒng)工程與電子技術(shù),2015,37(9):2123-2128. LU Tian-yu,YIN Jian,XIA Qun-li,et al.A Kind of Decoupling Algorithm of Phased Array Seeker Based on Beam Angle Error Compensation[J].Systems Engineering and Electronics,2015,37(9):2123-2128.

[11] ZHANG Guo-jiang,YAO Yu,MA Ke-mao.Line of Sight Rate Estimation of Strapdown Imaging Guidance System Based on Unscented Kalman Filter[C]∥Proceedings of Fourth International Conference on Machine Learning and Cybernetics,Guangzhou 2005:1574 -1578.

[12] 朱志宇.粒子濾波算法及其應(yīng)用[M].北京: 科學(xué)出版社,2010:27-35. ZHU Zhi-yu.Particle Filter and Its Application[M].Beijing: Science Press,2010:27-35.

[13] 李偉,曹潔,李軍,等.噪聲相關(guān)粒子濾波算法[J].電子科技大學(xué)學(xué)報(bào),2016,45(3):436-441. LI Wei,CAO Jie,LI Jun,et al.Particle Filter Algorithm with Correlative Noises[J].Journal of University of Electronic Science and Technology of China,2016,45(3):436-441.

[14] 袁宴波,張科,薛曉東.基于Radau偽譜法的制導(dǎo)炸彈最優(yōu)滑翔彈道研究[J].兵工學(xué)報(bào),2014,35(8):1179-1186.

YUAN Yan-bo,ZHANG Ke,XUE Xiao-dong.Optimization of Glide Trajectory of Guided Bombs Using a Radau Pseudo-spectral Method[J].Acta Armamentarii,2014,35(8):1179-1186.

[15] 歐陽中輝,劉家祺,張龍杰,等.基于矢量運(yùn)算的三維真比例導(dǎo)引彈道仿真[J].彈箭與制導(dǎo)學(xué)報(bào),2013,31(1):53-56. OUYANG Zhong-hui,LIU Jia-qi,ZHANG Long-jie,et al.Simulation of Three-Dimensional TPN Trajectory Based on Vector Operation[J].Journal of Projectiles,Rockets,Missiles and Guidance,2013,31(1):53-56.