基于容積卡爾曼濾波的全捷聯(lián)制導(dǎo)信息估計(jì)算法

， ，

(解放軍軍械工程學(xué)院，河北 石家莊 050003)

0 引言

傳統(tǒng)的平臺(tái)式制導(dǎo)彈藥在導(dǎo)引頭內(nèi)存在慣性穩(wěn)定平臺(tái)，通過萬向支架將導(dǎo)引頭與彈體運(yùn)動(dòng)隔離，避免了彈體姿態(tài)對(duì)于導(dǎo)引頭信息的擾動(dòng)，且具有較大的視場(chǎng)，但成本高，可靠性低[1]。全捷聯(lián)制導(dǎo)彈藥去掉了萬向支架以及穩(wěn)定平臺(tái)，具有體積小，質(zhì)量輕、可靠性高等優(yōu)點(diǎn)，滿足精確制導(dǎo)武器低成本、小型化的特點(diǎn)，逐漸應(yīng)用于各種“定點(diǎn)打擊”式的小規(guī)模作戰(zhàn)。但由于導(dǎo)引頭測(cè)量得到的體視線角運(yùn)動(dòng)信息是由彈體姿態(tài)運(yùn)動(dòng)信息與彈目視線角運(yùn)動(dòng)信息耦合而成的，不能直接應(yīng)用于比例導(dǎo)引等導(dǎo)引方法以及控制策略中；并且彈目相對(duì)運(yùn)動(dòng)模型的非線性程度較高。因此必須尋求合適的濾波算法對(duì)彈目視線角及角速度進(jìn)行準(zhǔn)確估計(jì)。

目前，國內(nèi)外學(xué)者對(duì)捷聯(lián)制導(dǎo)信息估計(jì)問題已經(jīng)開展了研究，并取得了一定的進(jìn)展。Vaddi等人針對(duì)非線性目標(biāo)運(yùn)動(dòng)模型，利用擴(kuò)展卡爾曼濾波的方法對(duì)目標(biāo)運(yùn)動(dòng)信息進(jìn)行了估計(jì)[2]；Waldamanm等人針對(duì)運(yùn)動(dòng)模型高非線性特點(diǎn)，采用無跡卡爾曼濾波技術(shù)估計(jì)彈目視線角速率[3]；Jaipal等人通過對(duì)擴(kuò)展卡爾曼濾波、無跡卡爾曼濾波、中心差分卡爾曼濾波以及粒子濾波在目標(biāo)跟蹤問題中的性能進(jìn)行了分析，為在實(shí)際應(yīng)用中如何選擇非線性濾波器提供了理論依據(jù)[4]。李璟璟等人對(duì)非線性濾波在量測(cè)噪聲為高斯和非高斯情形的濾波性能進(jìn)行了分析，得出了無跡卡爾曼濾波適合于捷聯(lián)成像導(dǎo)引系統(tǒng)視線角速率估計(jì)的結(jié)論[5]；孫婷婷等人針對(duì)光學(xué)導(dǎo)引頭不能直接測(cè)量體視線角速率問題，利用微分+穩(wěn)態(tài)卡爾曼濾波器估計(jì)體視線角速率[6]；林喆等人針對(duì)捷聯(lián)成像尋的器的慣性視線重構(gòu)問題，提出了一種基于自適應(yīng)中心差分卡爾曼濾波的慣性視線重構(gòu)濾波器的設(shè)計(jì)方案[7]。

上述對(duì)于捷聯(lián)制導(dǎo)信息估計(jì)問題的研究都需要利用慣性測(cè)量組件對(duì)彈體運(yùn)動(dòng)信息進(jìn)行測(cè)量，但測(cè)量元件精度具有穩(wěn)定性差、抗干擾能力低等局限性，并且可適用于制導(dǎo)彈藥中的測(cè)量元件甚少。因此，為滿足低成本精確制導(dǎo)彈藥小體積、低成本特點(diǎn)，本文提出了基于CKF的全捷聯(lián)制導(dǎo)信息估計(jì)算法。

1 坐標(biāo)系定義及變換

彈體在飛行過程中與目標(biāo)形成如圖1所示的彈目相對(duì)位置關(guān)系幾何示意圖，其中彈體質(zhì)心位置O為坐標(biāo)原點(diǎn)，T為目標(biāo)點(diǎn)，O-XgYgZg為慣性坐標(biāo)系，OT為彈目視線，其在慣性系下的傾角和偏角分別為qγ和qλ。O′為激光探測(cè)器中心位置，OZ為彈軸方向，O′ab平面為激光探測(cè)器平面位置，O′a軸與彈體縱向平面重合且與彈軸垂直，O′b軸與O′Za平面垂直，彈目視線在彈體坐標(biāo)系投影Oc與彈軸O′Z夾角為體視線偏角qα，與彈目視線OT夾角為體視線傾角qβ。OV為彈體的速度矢量，其在慣性系下的傾角和偏角分別為θ和ψv。彈軸OZ在慣性系下形成俯仰角φ、偏航角ψ和滾轉(zhuǎn)角γ。 為便于分析問題，根據(jù)圖1中彈目視線相對(duì)位置關(guān)系建立慣性坐標(biāo)系、彈體坐標(biāo)系、彈道坐標(biāo)系、彈目視線坐標(biāo)系以及體視線坐標(biāo)系。

如圖2(a)中所示，O-XgYgZg為慣性坐標(biāo)系，坐標(biāo)系原點(diǎn)O為彈體質(zhì)心位置，OXg軸沿水平線指向射擊方向，OYg軸與OXg軸垂直鉛直向上，OZg根據(jù)右手法則垂直于OXgYg平面指向右方。O-X2Y2Z2為彈道坐標(biāo)系，坐標(biāo)原點(diǎn)O取彈體質(zhì)心，OX2軸沿速度方向，OY2位于鉛垂平面內(nèi)且與OX2軸垂直向上，OZ2軸與其他兩軸垂直并構(gòu)成右手坐標(biāo)系。

如圖2(b)中所示，O-XbYbZb為彈體坐標(biāo)系，坐標(biāo)系原點(diǎn)O為彈體質(zhì)心位置，OXb軸沿彈體軸線方向，OYb軸在彈體的縱向?qū)ΨQ面內(nèi)且與OXb軸垂直向上，OZb根據(jù)右手法則與OXlYl平面垂直指向右方。O-XlYlZl為體視線坐標(biāo)系，坐標(biāo)原點(diǎn)O為彈體質(zhì)心，OXl軸沿彈目視線方向指向目標(biāo)，OZl軸在彈體系OXbYb平面內(nèi)且與OXl軸垂直，OYl軸按照右手法則與OXlZl平面垂直向上。

如圖2(c)中所示，O-XqYqZq為視線坐標(biāo)系，坐標(biāo)原點(diǎn)O為彈體質(zhì)心，OXq軸沿彈目視線方向指向目標(biāo)，OZq軸在慣性系OXgZg平面內(nèi)且與OXq軸垂直，OYq軸按照右手法則與OXqZq平面垂直向上。

上述五種坐標(biāo)系關(guān)系如圖3所示，其中qc表示視線坐標(biāo)系中OYq軸與體視線坐標(biāo)系中OYl軸之間的夾角，稱為視線變換角。L()表示各坐標(biāo)系之間的轉(zhuǎn)換矩陣。

2 基于CKF的全捷聯(lián)制導(dǎo)信息估計(jì)算法

2.1 濾波模型狀態(tài)方程建立

2.1.1彈體運(yùn)動(dòng)模型

對(duì)彈體進(jìn)行受力分析，將彈體飛行過程中的位置、速度、姿態(tài)角作為系統(tǒng)的狀態(tài)變量Xm(t)=[v,θ,ψv,φ,ψ,γ]T，建立彈體六自由度運(yùn)動(dòng)模型[8]。其中，f1(·)表示六自由度非線性微分方程組，ac(t)表示彈體控制指令，w(t)表示風(fēng)速等擾動(dòng)因素。

(1)

2.1.2目標(biāo)運(yùn)動(dòng)模型

由于制導(dǎo)彈藥打擊的目標(biāo)為陸地目標(biāo)，因此目標(biāo)機(jī)動(dòng)具有加速度小且變化緩慢的特點(diǎn)?，F(xiàn)采用勻速(CV)運(yùn)動(dòng)模型建立如下目標(biāo)運(yùn)動(dòng)模型。其中，vt(0)為目標(biāo)初始運(yùn)動(dòng)速度值，w(t)為隨時(shí)間變化的擾動(dòng)值。

vt(t)=vt(0)+wt(t)

(2)

2.1.3彈目相對(duì)運(yùn)動(dòng)模型

彈目相對(duì)運(yùn)動(dòng)矢量OT變化規(guī)律可利用下式表示：

r=Vm-Vt

(3)

為便于分析彈目視線角的變化規(guī)律，將OT用極坐標(biāo)表示形式表達(dá)。將彈體速度矢量OV與目標(biāo)速度矢量OVt分別投影到視線坐標(biāo)系OXq、OYq與OZq三軸，根據(jù)彈目相對(duì)運(yùn)動(dòng)角速度與線速度的關(guān)系，可得如下彈目相對(duì)運(yùn)動(dòng)速度表達(dá)式[9]。其中，Vx、Vy與Vz為彈體速度在實(shí)現(xiàn)坐標(biāo)系下的投影，Vtx、Vty與Vtz為目標(biāo)運(yùn)動(dòng)速度在視線坐標(biāo)系下的投影。

(4)

已知彈體速度在彈道坐標(biāo)系下的坐標(biāo)為(V,0,0)，為求得彈體速度矢量在視線坐標(biāo)系下的投影，根據(jù)圖4中坐標(biāo)系間的轉(zhuǎn)換關(guān)系，可得到慣性系下彈體以及目標(biāo)的速度矢量在視線坐標(biāo)系下坐標(biāo)，如下式：

(5)

將坐標(biāo)系轉(zhuǎn)換矩陣帶到式(5)中，結(jié)合式(4)，可得到彈目相對(duì)運(yùn)動(dòng)速度表達(dá)式(6)。

(6)

(7)

2.2 濾波模型量測(cè)方程建立

將彈目相對(duì)距離矢量OT在慣性系下的投影與在彈體坐標(biāo)系下的投影根據(jù)圖4中坐標(biāo)系間的關(guān)系進(jìn)行轉(zhuǎn)換，可得到等式(8)，最終化簡(jiǎn)得到全捷聯(lián)導(dǎo)引頭量測(cè)模型式(9)。其中，Rij為慣性坐標(biāo)系至彈體坐標(biāo)系轉(zhuǎn)換矩陣中的元素，v1和v2為導(dǎo)引頭量測(cè)噪聲。

(8)

(9)

2.3 容積卡爾曼濾波

卡爾曼濾波是線性條件下的最優(yōu)濾波，被廣泛應(yīng)用于目標(biāo)跟蹤與狀態(tài)估計(jì)。根據(jù)得出的濾波模型可以看到，全捷聯(lián)制導(dǎo)彈藥濾波模型具有高非線性特點(diǎn)，目前解決方法主要有將非線性方程線性化方法與非線性濾波兩種方法。由于容積卡爾曼濾波方法不需要求解雅格比矩陣，且濾波精度能達(dá)到三階擴(kuò)展卡爾曼濾波精度，并且在估計(jì)狀態(tài)變量個(gè)數(shù)多于3時(shí)比無跡卡爾曼濾波能達(dá)到更好的收斂效果。因此，本文采用容積卡爾曼濾波方法對(duì)彈體運(yùn)動(dòng)參數(shù)以及彈目視線角進(jìn)行估計(jì)。

容積卡爾曼濾波(Cubature Kalman Filter,CKF)是由Arasaratnam等人于2009年提出[10]。針對(duì)無跡卡爾曼濾波(Unscented)在狀態(tài)變量高維時(shí)可能出現(xiàn)濾波發(fā)散的問題，Arasaratnam利用球面徑向準(zhǔn)則來逼近非線性系統(tǒng)的后驗(yàn)分布，帶入卡爾曼濾波框架中，最終得到容積卡爾曼濾波。

2.3.1容積卡爾曼濾波初始化

(10)

(11)

2.3.2時(shí)間更新方程

1)計(jì)算容積點(diǎn)

(12)

(13)

n表示狀態(tài)變量的個(gè)數(shù)，[1]i表示一個(gè)全對(duì)陣點(diǎn)集。例如，當(dāng)n=2時(shí)，全對(duì)陣點(diǎn)集可表示為：

(14)

2)容積點(diǎn)傳播

Xi,k+1/k=f(Xi,k/k,uk)

(15)

其中，f()為系統(tǒng)的狀態(tài)方程。

3)狀態(tài)預(yù)測(cè)以及誤差協(xié)方差

(16)

(17)

2.3.3量測(cè)更新方程

1)計(jì)算容積點(diǎn)

(18)

2)容積點(diǎn)傳播

Zi,k+1/k=h(Xi,k+1/k,uk)

(19)

其中，h()為系統(tǒng)的量測(cè)方程。

3)量測(cè)值估計(jì)、量測(cè)協(xié)方差以及狀態(tài)量測(cè)交叉協(xié)方差

(20)

(21)

(22)

4)濾波更新方程

(23)

(24)

Pk+1/k+1=Pk+1/k-Kk+1Pzz,k+1/kKk+1T

(25)

結(jié)合彈體運(yùn)動(dòng)方程、目標(biāo)運(yùn)動(dòng)方程以及彈目視線運(yùn)動(dòng)方程，將式(7)與式(9)作為濾波方程的狀態(tài)方程與量測(cè)方程，帶入至容積卡爾曼濾波方程式(10)—式(25)中，設(shè)置濾波初始值，即導(dǎo)引頭開始工作時(shí)系統(tǒng)的狀態(tài)，最終可得到基于CKF的全捷聯(lián)制導(dǎo)信息估計(jì)算法，如圖4所示。

3 實(shí)驗(yàn)驗(yàn)證

3.1 末段飛行分析

由于彈體在飛行末段飛行速度快，彈目視線角度變化迅速，因此對(duì)彈體末段飛行情況以及與目標(biāo)的遭遇情形進(jìn)行分析。建立圖5所示的彈體末段飛行示意圖，圖中OXY為慣性坐標(biāo)系，T為目標(biāo)點(diǎn)位置。M1、M2以及M3分別為落點(diǎn)小于目標(biāo)點(diǎn)位置，正中目標(biāo)點(diǎn)位置以及落點(diǎn)大于目標(biāo)點(diǎn)位置三種情形下的彈體飛行末段位置，qγ1、qγ2和qγ3分別為彈體飛行末段的彈目視線傾角。如圖所示，當(dāng)彈體落點(diǎn)小于目標(biāo)點(diǎn)位置，此時(shí)的彈目視線傾角始終為銳角并最終趨近于0°；當(dāng)彈體正中目標(biāo)，彈目視線傾角最終趨于-90°；但彈體落點(diǎn)大于目標(biāo)點(diǎn)位置，飛行末端彈目視線角為鈍角并最終趨于-180°。

彈體在末段飛行過程中，飛行速度快，并且彈目相對(duì)距離減小，導(dǎo)致彈目視線角變化迅速，因此需要濾波器穩(wěn)定并且滿足濾波性能指標(biāo)。

3.2 數(shù)字仿真分析

為驗(yàn)證彈道參數(shù)及彈目視線角估計(jì)精度以及算法的性能，構(gòu)建圖6所示的仿真驗(yàn)證模型。

由于迫擊炮彈在無控段彈道參數(shù)基本固定，因此在實(shí)驗(yàn)仿真中將容積卡爾曼濾波制導(dǎo)信息估計(jì)方法應(yīng)用在120 mm迫擊炮彈道模型中。根據(jù)迫擊炮彈體氣動(dòng)參數(shù)以及實(shí)際打靶測(cè)得的射擊環(huán)境構(gòu)建彈體六自由度運(yùn)動(dòng)模型，其中設(shè)定發(fā)射條件以及風(fēng)速為擾動(dòng)因素。根據(jù)目標(biāo)勻速運(yùn)動(dòng)模型以及彈體運(yùn)動(dòng)模型構(gòu)建彈目相對(duì)運(yùn)動(dòng)模型。利用彈體姿態(tài)角以及彈目視線角構(gòu)建導(dǎo)引頭模型，將導(dǎo)引頭測(cè)量得到的體視線角進(jìn)行濾波得到彈目視線估計(jì)角度并通過微分器可得到彈目視線角速率。

由于迫彈在飛行末端速度約為200 m/s，而導(dǎo)引頭參測(cè)距離約為2 km，導(dǎo)引頭工作時(shí)間約為10 s，無控迫彈飛行時(shí)間約為50 s，因此在濾波初始化階段，設(shè)定導(dǎo)引頭在迫彈飛行40 s后打開導(dǎo)引頭，為簡(jiǎn)化問題，假設(shè)導(dǎo)引頭開始工作即搜索到目標(biāo)。設(shè)定目標(biāo)點(diǎn)坐標(biāo)為(1 000,-3.3,0)，目標(biāo)沿x軸勻速運(yùn)動(dòng)速度為10 m/s。濾波模型狀態(tài)變量初始條件設(shè)定為無擾動(dòng)彈道在40 s時(shí)刻的值，將系統(tǒng)噪聲都假定為高斯白噪聲，考慮彈體初速擾動(dòng)、射向偏差以及風(fēng)速擾動(dòng)，設(shè)定濾波器工作時(shí)間為10 s進(jìn)行實(shí)驗(yàn)。

3.3 實(shí)驗(yàn)結(jié)果與分析

按照?qǐng)D示的實(shí)驗(yàn)原理進(jìn)行數(shù)字建模并進(jìn)行實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如圖7至圖10以及表1所示。彈目視線傾角最大誤差為8.61°，誤差均方根為5.37°；彈目視線偏角最大誤差為2.03°，誤差均方根為1.11°；彈體俯仰角最大誤差為8.59°，誤差均方根為5.36°；偏航角最大誤差為1.17°，誤差均方根為0.14°。從實(shí)驗(yàn)結(jié)果可以看出，由于設(shè)定濾波初始值為無擾動(dòng)彈道在濾波初始時(shí)刻的值，因此在彈體飛行過程中彈目視線角及彈體姿態(tài)角估計(jì)值會(huì)在濾波初始時(shí)刻產(chǎn)生最大誤差，隨著迭代步數(shù)的增加，濾波器會(huì)使估計(jì)值逐漸收斂于真實(shí)值。由實(shí)驗(yàn)結(jié)果可以看出，在彈目視線角變化迅速以及彈體姿態(tài)角穩(wěn)定的情況下，濾波器對(duì)于角度的估計(jì)值都具有良好的穩(wěn)定性。由于濾波器設(shè)計(jì)過程中，建立系統(tǒng)模型存在不確定性以及噪聲干擾，導(dǎo)致濾波結(jié)果沒有完全收斂于真實(shí)值，但通過對(duì)實(shí)驗(yàn)結(jié)果的分析，可以看出濾波結(jié)果能夠滿足制導(dǎo)與控制所需信息的精度，并且該非線性濾波器具有一定的穩(wěn)定性能。因此，該濾波器具有一定的工程應(yīng)用價(jià)值。

表1 估計(jì)最大誤差及誤差均方根Tab.1 Estimated maximum errors and root mean square error

4 結(jié)論

本文提出了基于CKF的全捷聯(lián)制導(dǎo)信息估計(jì)算法。該算法通過分析彈目相對(duì)運(yùn)動(dòng)關(guān)系建立濾波模型；針對(duì)制導(dǎo)彈藥彈道穩(wěn)定的特點(diǎn)采用容積卡爾曼濾波方法對(duì)彈體姿態(tài)角以及彈目視線角進(jìn)行了聯(lián)合估計(jì)。通過仿真驗(yàn)證結(jié)果表明，該算法能夠滿足制導(dǎo)信息精度的要求，并且具有一定的穩(wěn)定性。本文提出的全捷聯(lián)制導(dǎo)信息估計(jì)方法可為全捷聯(lián)激光制導(dǎo)技術(shù)在制導(dǎo)彈藥中的應(yīng)用提供理論依據(jù)。

[1]姚郁, 章國江. 捷聯(lián)成像制導(dǎo)系統(tǒng)的若干問題探討[J]. 紅外與激光工程, 2006, 35(1): 1-6.

[2]Vaddi S S, Menon P K. Target state estimation for integrated guidance-control of missiles[C]// AIAA Guidance, Navigation and Control Conference and Exhibit. Hilton head, South Carolina: 2007: 1-22.

[3]JacquesWaldmann. Line-of-sight rate estimation and linearizing control of an imaging seeker in a tactical missile guided by proportional navigation[J]. IEEE Transaction on Control Systems Technology, 2002, 10(4): 556-567.

[4]Katkuri J R, Jilkov V P, Li X R.A comparative study of nonlinear filters for target tracking in mixed coordinates[C]// Proc of the 42nd South Eastern Symposium on System Theory. Tyler: Institute of Electrical and Electronics Engineers Inc, 2010: 202-207.

[5]李璟璟. 捷聯(lián)成像導(dǎo)引頭視線角速率估計(jì)方法研究[D].哈爾濱: 哈爾濱工程大學(xué), 2008.

[6]孫婷婷, 儲(chǔ)海榮, 賈宏光，等. 捷聯(lián)式光學(xué)圖像導(dǎo)引頭視線角速率估計(jì)[J]. 光學(xué)學(xué)報(bào), 2014, 34(6): 1-7.

[7]林喆, 姚郁, 馬克茂. 捷聯(lián)成像尋的器ACDKF慣性視線重構(gòu)[J]. 紅外與激光工程, 2008, 37(3)：400-405.

[8]韓子鵬等. 彈箭外彈道學(xué)[M]. 北京: 北京理工大學(xué)出版社, 2008: 142.

[9]劉義, 趙晶, 馮德軍，等.高精度慣導(dǎo)速度信息輔助的彈目相對(duì)運(yùn)動(dòng)模型構(gòu)建方法[J]. 電子學(xué)報(bào), 2011, 39(9): 2207-2211.

[10]Icnkaran Arasaratnam, Simon Haykin. Cubature kalman filters[J]. IEEE Transactions on Automatic Control，2009, 54(6): 1254-1269.