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

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

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

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

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

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

1 解耦算法

捷聯(lián)導(dǎo)引頭測量的體視線角中包括了目標(biāo)相對于慣性空間的視線角和彈體姿態(tài)角，而導(dǎo)彈制導(dǎo)所需慣性視線角速率需要從體視線角信號中去除彈體運動信息。可以通過不同坐標(biāo)系的相對旋轉(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)系相對于發(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 視線角速率估計

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

2.1 狀態(tài)方程

(8)

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

(9)

(10)

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

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

(11)

2.2 量測方程

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

則量測方程為

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個σ點及其權(quán)重分別如下：

(14)

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

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

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

(15)

3) 其估值及方差

(16)

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

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

圖2 UKF濾波流程

3 仿真與分析

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

(17)

(18)

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

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

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

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

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

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

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

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

4 結(jié)論

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

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