基于無跡卡爾曼濾波的捷聯(lián)導(dǎo)引頭視線角速率估計(jì)方法

楊 陽，蔡正誼，陳升澤，趙 帥

(中國運(yùn)載火箭技術(shù)研究院研究發(fā)展中心， 北京 100076)

隨著導(dǎo)引頭技術(shù)的發(fā)展，新型捷聯(lián)式導(dǎo)引頭及尋的技術(shù)應(yīng)運(yùn)而生，它取消了萬向支架等機(jī)械結(jié)構(gòu)，將導(dǎo)引頭直接與飛行器剛性捷聯(lián)。這項(xiàng)技術(shù)已成為各國重點(diǎn)發(fā)展的關(guān)鍵技術(shù)[1-2]。捷聯(lián)導(dǎo)引頭應(yīng)用于實(shí)際武器系統(tǒng)仍有需要解決的問題[3]。由于導(dǎo)引頭測(cè)量信號(hào)中耦合了彈體姿態(tài)信息，需要采用解耦算法；另外，導(dǎo)引頭測(cè)量信息中缺少視線角速率信息，并且所測(cè)量的視線角信息中含有更強(qiáng)的噪聲，無法直接提取，必須對(duì)視線角速率進(jìn)行準(zhǔn)確估計(jì)。

針對(duì)捷聯(lián)導(dǎo)引頭視線角速率的估計(jì)研究，國內(nèi)外學(xué)者已開展相關(guān)方面的研究。顏東[4]為了實(shí)現(xiàn)捷聯(lián)導(dǎo)引頭的導(dǎo)引律設(shè)計(jì)，引入了“瞬態(tài)導(dǎo)引法”，采用卡爾曼濾波方法對(duì)捷聯(lián)導(dǎo)引頭測(cè)量信息進(jìn)行處理，并利用龐德里亞金最小值原理推導(dǎo)出捷聯(lián)最優(yōu)制導(dǎo)律。但該算法需要已知飛行器的加速度等信息，且進(jìn)行了一些假設(shè)，工程實(shí)際應(yīng)用比較困難。楚德強(qiáng)[5]根據(jù)捷聯(lián)導(dǎo)引頭能測(cè)量視線角信息的特點(diǎn)，對(duì)經(jīng)典導(dǎo)引律進(jìn)行改造變形，設(shè)計(jì)了多種形式僅需要視線角信息的捷聯(lián)制導(dǎo)律，分析了各因素對(duì)制導(dǎo)精度的影響以及捷聯(lián)制導(dǎo)律的魯棒性，驗(yàn)證了捷聯(lián)導(dǎo)引律的性能和適應(yīng)性。R.D.Ehrich、Smita Sadhu、Emmert RI等[6-8]對(duì)有關(guān)捷聯(lián)導(dǎo)引頭視線角速率估計(jì)方法進(jìn)行了研究。姚郁等也對(duì)視線角速率提取的解耦問題進(jìn)行了推導(dǎo)；孫婷婷等[9-10]設(shè)計(jì)了一些濾波算法，有效推動(dòng)了視線角速率提取的研究。

本文以末制導(dǎo)武器為研究對(duì)象，根據(jù)彈體視線、彈體姿態(tài)與視線角速率的運(yùn)動(dòng)關(guān)系，提出了基于無跡卡爾曼濾波的捷聯(lián)導(dǎo)引頭視線角速率估計(jì)方法，克服了擴(kuò)展卡爾曼濾波算法精度偏低、計(jì)算量較大的缺點(diǎn)，具有很好的工程應(yīng)用價(jià)值。

1 解耦算法

捷聯(lián)導(dǎo)引頭測(cè)量的體視線角中包括了目標(biāo)相對(duì)于慣性空間的視線角和彈體姿態(tài)角，而導(dǎo)彈制導(dǎo)所需慣性視線角速率需要從體視線角信號(hào)中去除彈體運(yùn)動(dòng)信息?？梢酝ㄟ^不同坐標(biāo)系的相對(duì)旋轉(zhuǎn)關(guān)系推導(dǎo)視線角速率解耦算法[11]。

從圖1可以看出，發(fā)射系轉(zhuǎn)換到視線坐標(biāo)系的轉(zhuǎn)換有兩種方法。

圖1 坐標(biāo)轉(zhuǎn)換關(guān)系

方法1：由發(fā)射坐標(biāo)系經(jīng)彈體坐標(biāo)系、體視線坐標(biāo)系到視線坐標(biāo)坐標(biāo)系。

視線坐標(biāo)系相對(duì)于發(fā)射坐標(biāo)系的旋轉(zhuǎn)角速度為ω′，在發(fā)射坐標(biāo)系中表示為：

(1)

方法2：由視線坐標(biāo)系直接到發(fā)射坐標(biāo)系，可以求得ω′為：

(2)

(3)

考慮到目標(biāo)在體視線坐標(biāo)系及視線坐標(biāo)系下的位置，得到目標(biāo)在體坐標(biāo)系以及發(fā)射坐標(biāo)系中的坐標(biāo)分別為：

(4)

(5)

(6)

式中qα是在體視線下的高低角，qβ是在體視線角下的方位角。同理，可得：

(7)

2 視線角速率估計(jì)

式(3)和式(7)可以發(fā)現(xiàn)獲取視線角速率需要導(dǎo)引頭測(cè)量到的信息，即兩個(gè)視線角及其微分。若是量測(cè)系統(tǒng)并不包含噪聲，使用上述算法可以有效求解出系統(tǒng)所需的視線角速度，但捷聯(lián)導(dǎo)引頭只能測(cè)得相對(duì)于彈體的視線角信息，其中耦合有彈體姿態(tài)運(yùn)動(dòng)，具有強(qiáng)非線性，含有大量的噪聲干擾信息。因此，有必要對(duì)視線角及角速率的提取建立合適的濾波器，選取合適的濾波算法，以獲取更加精確的結(jié)果。實(shí)際的導(dǎo)航系統(tǒng)中，狀態(tài)方程和量測(cè)方程都是非線性的，采用傳統(tǒng)的擴(kuò)展卡爾曼濾波(Extended Kalman Filter,EKF)需要進(jìn)行泰勒級(jí)數(shù)展開的線性化處理，并將一階近似項(xiàng)作為原狀態(tài)方程和量測(cè)方程的近似表達(dá)式。這就在高斯隨機(jī)變量的實(shí)際后驗(yàn)均值和方差中引入誤差，導(dǎo)致仿真結(jié)果次優(yōu)，甚至引起發(fā)散。為了改善對(duì)非線性問題進(jìn)行濾波的效果，Julier等人提出了無跡卡爾曼濾波方法(Unscented Kalman Filter,UKF)對(duì)非線性問題進(jìn)行濾波估計(jì)。由于UKF可以有效克服EKF濾波精度偏低及需要計(jì)算雅克比矩陣的局限性，故其在組合導(dǎo)航系統(tǒng)、慣性導(dǎo)航初始對(duì)準(zhǔn)、機(jī)動(dòng)目標(biāo)跟蹤等各個(gè)領(lǐng)域已獲得廣泛的應(yīng)用[12-13]。該方法在處理狀態(tài)方程時(shí)首先進(jìn)行了UT(Unscented Transformation)變換，使用UT變換后的狀態(tài)變量進(jìn)行濾波估計(jì)，以減少估計(jì)誤差。

2.1 狀態(tài)方程

(8)

其中f1(·)由彈-目運(yùn)動(dòng)視線運(yùn)動(dòng)模型式(3)和式(7)得到，f2(·)由姿態(tài)運(yùn)動(dòng)模型式(9)得到。

(9)

(10)

式(8)離散化后可表示為

Xk+1=F(Xk,ωk)+Wk

(11)

2.2 量測(cè)方程

由于導(dǎo)引頭可直接測(cè)量體視線高低角和范圍角qα,qβ，陀螺可實(shí)施測(cè)量彈體運(yùn)動(dòng)的角速率ωx1,ωy1,ωz1，將其作為量測(cè)信息。

則量測(cè)方程為

Z(t)=h[X(t),ω(t),t]+n(t)

(12)

離散化后可表示為

Zk=Hk(Xk,ωk)+Vk

(13)

2.3 UKF濾波

1) 設(shè)狀態(tài)變量為n×1維，那么2n+1個(gè)σ點(diǎn)及其權(quán)重分別如下：

(14)

式中τ=α2(n+κ)-n是一個(gè)比例因子，κ是一個(gè)標(biāo)量，取κ=2，α選取0<α<1，對(duì)于高斯分布，取β=2。

2) 每個(gè)δ采樣點(diǎn)通過非線性函數(shù)傳播，得到

y=g(Xik),i=0,…,2n

(15)

3) 其估值及方差

(16)

通過式(12)-式(14)UT變換方法，可以更為準(zhǔn)確地求得隨機(jī)分布經(jīng)過非線性變換后的均值和方差，這樣結(jié)合卡爾曼濾波器的一般實(shí)現(xiàn)框架，將UT變換分別應(yīng)用于均值和方差的求解，并不斷遞推，便可得到UKF非線性濾波器。

UKF濾波過程如圖2所示，經(jīng)過式UKF濾波可得視線角和視線角速度的估計(jì)值。再基于視線角速度的估計(jì)值進(jìn)行制導(dǎo)控制律設(shè)計(jì)，從而實(shí)現(xiàn)捷聯(lián)導(dǎo)引控制。

圖2 UKF濾波流程

3 仿真與分析

以打擊移動(dòng)目標(biāo)的末制導(dǎo)武器系統(tǒng)作為研究對(duì)象，采用UKF濾波對(duì)視線角速率估計(jì)，并采用經(jīng)典比例導(dǎo)引，得到制導(dǎo)指令角速度為：

(17)

(18)

仿真時(shí)采用的初始參數(shù)如表1。

表1 初始參數(shù)(地心慣性坐標(biāo)系)

末制導(dǎo)武器與目標(biāo)初始距離為22 km，仿真時(shí)間為20 s，方位角仿真進(jìn)行到19.998 s時(shí)刻，末制導(dǎo)武器與目標(biāo)間的距離為0.475 0 m，可實(shí)現(xiàn)對(duì)目標(biāo)的成果打擊。仿真結(jié)果如圖3所示。

圖3 末制導(dǎo)武器與目標(biāo)運(yùn)動(dòng)軌跡

濾波估計(jì)視線角速率時(shí)，捷聯(lián)導(dǎo)引頭測(cè)量視線角估計(jì)誤差如圖4所示，視線角速率估計(jì)誤差如圖5所示。

圖4 捷聯(lián)導(dǎo)引頭視線角估計(jì)絕對(duì)誤差

圖5 捷聯(lián)導(dǎo)引頭視線角速率估計(jì)絕對(duì)誤差

由仿真可以看出視線角估計(jì)精度絕對(duì)誤差不大于0.05°，視線角速率估計(jì)精度絕對(duì)誤差不大于0.05 (°)/s，估計(jì)精度高、收斂速度快。

4 結(jié)論

1) 本文推導(dǎo)了模型視線角速率解耦算法，提出了基于無跡卡爾曼濾波的捷聯(lián)導(dǎo)引頭視線角速率估計(jì)方法，克服了擴(kuò)展卡爾曼濾波算法精度偏低及需要計(jì)算雅克比矩陣的局限性問題。

2) 仿真結(jié)果表明該方法具有較高的精度和動(dòng)態(tài)性能，滿足打擊機(jī)動(dòng)目標(biāo)的需求，具有工程應(yīng)用價(jià)值。