張鐸，宋建梅，趙良玉,*，焦天峰，丁國強(qiáng)

1.北京理工大學(xué) 宇航學(xué)院，北京 100081 2.中國兵器工業(yè)導(dǎo)航與控制技術(shù)研究所，北京 100089 3.鄭州輕工業(yè)大學(xué) 電氣信息工程學(xué)院，鄭州 450002

捷聯(lián)導(dǎo)引頭因性能可靠、結(jié)構(gòu)簡單、成本低廉等優(yōu)勢，在空空導(dǎo)彈、反坦克導(dǎo)彈、制導(dǎo)炮彈、制導(dǎo)火箭彈等戰(zhàn)術(shù)武器領(lǐng)域具有廣闊而明確的應(yīng)用需求。但是，由于捷聯(lián)導(dǎo)引頭只能敏感到目標(biāo)與彈體軸線方向的角度關(guān)系，無法直接獲取諸多戰(zhàn)術(shù)武器制導(dǎo)所需的視線角速率信息。為了解決捷聯(lián)導(dǎo)引頭的視線角速率提取問題，國內(nèi)外專家學(xué)者開展了廣泛而深入的研究，并取得了豐富的研究成果。

姚郁和章國江[1]在將視線角與彈體姿態(tài)進(jìn)行解耦的基礎(chǔ)上，實(shí)現(xiàn)了捷聯(lián)成像導(dǎo)引頭視線角速率的提取。文獻(xiàn)[2]建立了以視線高低角、視線高低角速率、視線方位角和視線方位角速率為狀態(tài)變量的視線角速率估計(jì)模型以及全捷聯(lián)導(dǎo)引頭解耦模型，提出了基于擴(kuò)展卡爾曼濾波(Extended Kalman Filter，EKF)的視線角速率提取算法。文獻(xiàn)[3]提出了基于無跡卡爾曼濾波(Unscented Kalman Filter，UKF)的視線角速率提取算法，得到了更高的估計(jì)精度。文獻(xiàn)[4]對狀態(tài)變量進(jìn)行擴(kuò)充，增加了導(dǎo)彈與目標(biāo)相對距離及其距離變化率兩個(gè)狀態(tài)變量，并利用五階容積卡爾曼濾波(Cubature Kalman Filter，CKF)算法提取視線角速率，得到了較高的估計(jì)精度，但是彈目相對距離與彈目相對距離變化率這兩個(gè)狀態(tài)變量在只有角度觀測情況下存在觀測性弱的問題，對狀態(tài)的估計(jì)精度和算法的穩(wěn)定性會造成不利影響。文獻(xiàn)[5]中提出了基于機(jī)載平臺的修正球面坐標(biāo)系目標(biāo)跟蹤模型，有效地改善了觀測性弱的問題。文獻(xiàn)[6]提出了修正極坐標(biāo)系下的雷達(dá)與電子支援措施航跡對準(zhǔn)關(guān)聯(lián)算法，實(shí)現(xiàn)了雷達(dá)與目標(biāo)之間的穩(wěn)定追蹤。上述研究均假設(shè)狀態(tài)噪聲與觀測噪聲為高斯白噪聲且它們之間相互獨(dú)立，這與真實(shí)場景下狀態(tài)噪聲和觀測噪聲的統(tǒng)計(jì)特性并不完全相符。

針對非高斯噪聲條件下的視線角速率提取問題，文獻(xiàn)[7-9]均采用粒子濾波(Particle Filter，PF)來提高視線角速率的估計(jì)精度，但是粒子濾波隨著迭代次數(shù)的增加，容易出現(xiàn)由于權(quán)重過度集中而導(dǎo)致的粒子退化現(xiàn)象。文獻(xiàn)[10]提出了應(yīng)用于紅外圖像導(dǎo)引頭的視線角速率提取分層采樣粒子濾波算法，有效解決了粒子退化現(xiàn)象，但是增加了系統(tǒng)計(jì)算負(fù)擔(dān)。文獻(xiàn)[11]提出了基于自適應(yīng)預(yù)測濾波的視線角速率提取算法，利用不斷變化的殘差對噪聲方差進(jìn)行在線調(diào)整，提高了對非高斯噪聲的濾波性能。由于集員濾波算法不需要已知噪聲的分布特性，故在非高斯噪聲環(huán)境下的視線角速率提取方面展現(xiàn)出良好的應(yīng)用前景。集員濾波算法最初由Schweppe[12]于1968年提出。Maksarov[13]、Kurzhanski[14]和Chernousko[15]等進(jìn)一步發(fā)展了針對狀態(tài)和參數(shù)估計(jì)的橢球計(jì)算方法。Scholte和Campbell[16]利用區(qū)間估計(jì)方法建立了非線性系統(tǒng)的擴(kuò)展集員濾波(Extended Set Membership Filter，ESMF)算法。文獻(xiàn)[17]拓展了集員濾波在故障檢測領(lǐng)域中的應(yīng)用，利用次最小容積法則實(shí)現(xiàn)狀態(tài)橢球與觀測橢球的更新計(jì)算，提出了基于擴(kuò)展橢球集員濾波的故障檢測與隔離算法。文獻(xiàn)[18]提出了一種基于MIT(Massachusetts Institute of Technology)規(guī)則的自適應(yīng)擴(kuò)展集員濾波算法，其利用MIT優(yōu)化規(guī)則對狀態(tài)噪聲橢球進(jìn)行自適應(yīng)選取，實(shí)現(xiàn)了更快的健康度恢復(fù)速率。文獻(xiàn)[19]提出了基于純方位目標(biāo)跟蹤的外定界橢球集員估計(jì)算法，該算法采用最小化后驗(yàn)估計(jì)誤差的李雅普諾夫函數(shù)上界的策略來獲得觀測更新橢球，能夠更加合理地利用觀測信息，并降低了運(yùn)算量。

然而，以上這些考慮非高斯噪聲的濾波算法均建立在狀態(tài)噪聲與觀測噪聲相互獨(dú)立的前提下，沒有考慮狀態(tài)噪聲與觀測噪聲的相關(guān)性對估計(jì)結(jié)果的影響。由于導(dǎo)彈在飛行過程中存在大量不確定的干擾因素[20-21]，例如機(jī)械振動(dòng)以及彈體彈性形變等。這些客觀因素使得狀態(tài)噪聲與觀測噪聲相互獨(dú)立的假設(shè)難以保證[22]。為此，本文圍繞考慮相關(guān)噪聲解耦的視線角速率提取算法開展研究，推導(dǎo)了基于修正球面坐標(biāo)系的視線角速率提取模型，建立了視線角速率提取模型的相關(guān)噪聲解耦方法以及解耦后模型的線性化表達(dá)式，得到了相關(guān)噪聲解耦的擴(kuò)展橢球集員濾波算法，并以某型搭載捷聯(lián)導(dǎo)引頭的制導(dǎo)火箭彈為例進(jìn)行了仿真驗(yàn)證。

1 修正球面坐標(biāo)系下的視線角速率提取模型

圖1 彈目相對位置關(guān)系

根據(jù)球面坐標(biāo)系下的彈目相對位置關(guān)系可以得到彈目相對運(yùn)動(dòng)方程為[2]

(1)

(2)

(3)

(4)

(5)

(6)

(7)

由彈目相對運(yùn)動(dòng)模型式(1)可知

(8)

將式(8)代入式(7)并化簡得到

(9)

(10)

(11)

式中：Δt為采樣時(shí)間間隔；wk-1為包含離散化誤差的狀態(tài)噪聲項(xiàng)。

由于目標(biāo)在視線坐標(biāo)系和體視線坐標(biāo)系中的坐標(biāo)均為(r,0,0)，則經(jīng)過坐標(biāo)轉(zhuǎn)換得到目標(biāo)在彈體坐標(biāo)系和地面坐標(biāo)系內(nèi)的投影分別為

(12)

(13)

(14)

聯(lián)立式(12)～式(14)并整理得到視線角解耦模型為[3]

(15)

(16)

式中：γ、?和ψ分別為導(dǎo)彈的滾轉(zhuǎn)角、俯仰角和偏航角。選取捷聯(lián)導(dǎo)引頭測得的體視線高低角qα和體視線方位角qβ為觀測量，可以得到觀測方程為

(17)

2 考慮相關(guān)噪聲解耦的擴(kuò)展橢球集員濾波算法

在第1節(jié)所建立視線角速率提取模型與視線角解耦模型的基礎(chǔ)上，針對狀態(tài)與觀測模型中存在非高斯相關(guān)噪聲的情況，根據(jù)文獻(xiàn)[23]中對高斯噪聲假設(shè)條件下相關(guān)噪聲的處理方法，利用集員濾波中橢球形狀矩陣與Kalman濾波中的噪聲協(xié)方差矩陣作用相似的特性，用非高斯噪聲的橢球形狀矩陣代替相關(guān)噪聲解耦框架中的噪聲協(xié)方差矩陣，對觀測模型中的相關(guān)噪聲進(jìn)行解耦，在此基礎(chǔ)上運(yùn)用擴(kuò)展橢球集員濾波算法，實(shí)現(xiàn)對非高斯相關(guān)噪聲的濾波計(jì)算。將第1節(jié)所建立的狀態(tài)模型式(10)和觀測模型式(17)簡寫為

(18)

wk-1∈Ω(0,Qk-1),vk∈Ω(0,Rk)

(19)

式中：Qk-1為狀態(tài)噪聲橢球的形狀矩陣；Rk為觀測噪聲橢球形狀矩陣，可以將每一時(shí)刻的狀態(tài)分布情況按照橢球集合定義為

Ω(a,P)={x∈Rn|(x-a)TP-1(x-a)≤1}

(20)

i=1,2,…，n

(21)

對式(18)中的觀測方程做變換[22]：

(22)

式中：G為待求矩陣，對式(22)中包含狀態(tài)變量的項(xiàng)和噪聲項(xiàng)分別進(jìn)行合并，得到

h*(xk)=h(xk)+G(xk-f(xk-1))

(23)

(24)

則觀測噪聲協(xié)方差矩陣，即觀測噪聲橢球形狀矩陣可以轉(zhuǎn)換為

(25)

Sk-Qk-1GT=0

(26)

由式(26)得到待求矩陣G的表達(dá)式為

(27)

將式(27)代入式(25)得到解相關(guān)性的觀測噪聲橢球形狀矩陣為

(28)

同理，將式(27)代入式(23)得到解相關(guān)性的觀測函數(shù)為

(29)

由此得到相關(guān)噪聲解耦的視線角速率提取算法狀態(tài)方程與觀測方程為

(30)

對相關(guān)噪聲解耦之后的狀態(tài)模型與觀測模型開展擴(kuò)展橢球集員濾波算法的推導(dǎo)。對式(30)中的狀態(tài)方程進(jìn)行線性化變換，對k-1步的狀態(tài)變量xk-1通過一階泰勒級數(shù)來逼近第k步的狀態(tài)變量預(yù)測值xk：

(31)

(32)

式中：Hf1,Hf2,…,Hfn為狀態(tài)函數(shù)關(guān)于每個(gè)狀態(tài)變量的海森矩陣(Hessian Matrix)。將狀態(tài)方程的拉格朗日余項(xiàng)作為其誤差的一部分來考慮，式(31)可以簡化為

(33)

(34)

同理可以得到解相關(guān)性觀測方程的線性化表達(dá)式為

(35)

(36)

式中：Hh*1,Hh*2,…,Hh*n為解相關(guān)性觀測函數(shù)關(guān)于每個(gè)狀態(tài)變量的海森矩陣。同理，把解相關(guān)性觀測方程的拉格朗日余項(xiàng)作為誤差的一部分來考慮，式(35)可以簡化為

(37)

(38)

(39)

利用橢球集員濾波算法計(jì)算時(shí)間更新的狀態(tài)橢球邊界為

(40)

(41)

觀測更新為

(42)

(43)

(44)

其中：

(45)

(46)

F和H分別為狀態(tài)方程的系數(shù)矩陣和解相關(guān)性的觀測方程系數(shù)矩陣；βk和ρk分別為時(shí)間更新橢球和觀測更新橢球的調(diào)節(jié)尺度因子；δk為算法健康度因子，當(dāng)狀態(tài)橢球或觀測噪聲橢球無效時(shí)δk≥1，其表達(dá)式為

(47)

尺度因子參數(shù)的求解涉及到兩個(gè)橢球直和運(yùn)算得到的外包橢球最優(yōu)化問題，對βQk-1、βRk和βk-1的計(jì)算選取外包橢球的最小跡計(jì)算方法，該方法求解形式簡單，且與外包橢球最小化體積的優(yōu)化準(zhǔn)則相比，該方法魯棒性更強(qiáng)，計(jì)算量更小。根據(jù)橢球最小跡法則有

β=argmin tr(P)

(48)

從而可以根據(jù)

(49)

獲得最優(yōu)的尺度因子參數(shù)，其中：P1和P2為任意兩個(gè)橢球形狀矩陣。

(50)

(51)

考慮觀測更新的橢球形狀矩陣計(jì)算公式為

(52)

在迭代計(jì)算過程中，觀測更新的橢球形狀矩陣的形式往往較為復(fù)雜，導(dǎo)致Pk計(jì)算復(fù)雜度較高，無論采用最小化橢球體積法還是橢球最小跡法，尺度因子參數(shù)ρk的優(yōu)化計(jì)算都很困難，甚至無法獲得解析解。采用最小化性能指標(biāo)δk的上界形式來計(jì)算，可以大大降低計(jì)算成本，根據(jù)：

ρk=argmin sup(δk)

(53)

得到尺度因子ρk的次優(yōu)計(jì)算公式為

(54)

3 案例研究

本文選用裝載捷聯(lián)導(dǎo)引頭的某型遠(yuǎn)程制導(dǎo)火箭彈的末制導(dǎo)仿真對算法進(jìn)行驗(yàn)證，利用彈道仿真獲得的火箭彈加速度與姿態(tài)數(shù)據(jù)實(shí)時(shí)更新狀態(tài)模型與觀測模型，根據(jù)比例導(dǎo)引律將視線角速率的提取結(jié)果實(shí)時(shí)進(jìn)行導(dǎo)引，控制火箭彈精確命中目標(biāo)。設(shè)置仿真初始時(shí)刻參數(shù)如表1 所示。

表1 初始仿真狀態(tài)

選取球面坐標(biāo)系下狀態(tài)模型的初始值為

x0=[-0.917 5,-0.009 5,0,0,18 890,-611]T

修正球面坐標(biāo)系下狀態(tài)模型的初始值為

xMSC0=[-0.917 5,-0.009 5,0,0,5.293 8×

10-5,-0.032 3]T

對比兩種模型初始值可以看出，修正球面坐標(biāo)系模型在經(jīng)過規(guī)范化處理以后，實(shí)現(xiàn)了狀態(tài)變量從有量綱到無量綱的轉(zhuǎn)換，便于后續(xù)的迭代計(jì)算。為了驗(yàn)證修正球面坐標(biāo)系下視線角速率提取模型的性能，對修正球面坐標(biāo)系下的視線角速率提取模型與傳統(tǒng)球面坐標(biāo)系下的視線角速率提取模型的估計(jì)精度進(jìn)行對比分析。假設(shè)狀態(tài)噪聲與觀測噪聲均為高斯白噪聲，且相互獨(dú)立，其中觀測噪聲的均方根誤差為0.035 rad。在初始條件相同的情況下，采用EKF算法分別進(jìn)行仿真的結(jié)果如圖2和圖3所示。

圖2 球面坐標(biāo)系模型與修正球面坐標(biāo)系模型視線高低角與視線高低角速率估計(jì)曲線

圖3 球面坐標(biāo)系模型與修正球面坐標(biāo)系模型視線方位角與視線方位角速率估計(jì)曲線

在修正球面坐標(biāo)系模型的基礎(chǔ)上，假設(shè)狀態(tài)噪聲與觀測噪聲為相互獨(dú)立且滿足均勻分布的非高斯噪聲，其中觀測噪聲形狀矩陣根據(jù)式(19)定義為分別采用ESMF算法和PF算法進(jìn)行視線角速率提取，得到的仿真結(jié)果如圖4和圖5所示。

圖4 ESMF與PF算法視線高低角及其角速率估計(jì)曲線

圖5 ESMF與PF算法視線方位角及其角速率估計(jì)曲線

可以看出，在非高斯噪聲條件下，PF算法得到的視線角及視線角速率的估計(jì)結(jié)果在后半段出現(xiàn)了嚴(yán)重發(fā)散，導(dǎo)致估計(jì)結(jié)果與真實(shí)狀態(tài)之間的誤差迅速擴(kuò)大，無法提供可用的制導(dǎo)信息。反觀ESMF算法，由于不需要已知觀測噪聲和狀態(tài)噪聲的分布情況，僅要求噪聲有界，表現(xiàn)出更好的魯棒性。此外，ESMF算法將線性化誤差項(xiàng)與模型本身的噪聲項(xiàng)通過直和計(jì)算引入到虛擬噪聲橢球中，充分考慮了線性化誤差對模型估計(jì)結(jié)果的影響，故該算法得到的視線角與視線角速率估計(jì)精度高于PF算法，最大誤差僅為0.01 rad/s，能夠滿足高精度比例導(dǎo)引制導(dǎo)的需要，是一種更符合工程應(yīng)用且精度較高的濾波方案。

針對噪聲相關(guān)且非高斯的情況，假設(shè)狀態(tài)噪聲與觀測噪聲滿足均勻分布，視線高低角與體視線高低角、視線方位角與體視線方位角存在相關(guān)性，令狀態(tài)噪聲與觀測噪聲的相關(guān)性矩陣為開展相關(guān)噪聲解耦的ESMF算法仿真，與相關(guān)噪聲未解耦ESMF算法的仿真結(jié)果如圖6和圖7所示?？梢钥闯?，未進(jìn)行相關(guān)噪聲解耦的ESMF算法得到的估計(jì)結(jié)果已經(jīng)不能很好地跟蹤狀態(tài)真實(shí)值，且估計(jì)結(jié)果在末端出現(xiàn)不同程度的發(fā)散，其中，視線方位角速率的末段估計(jì)誤差達(dá)到了0.05 rad/s以上。而采用相關(guān)噪聲解耦算法得到的視線方位角速率與視線高低角速率的估計(jì)精度較高，在末段誤差最大僅為0.005 rad/s，能夠更好地跟蹤狀態(tài)真實(shí)值，實(shí)現(xiàn)了在非高斯相關(guān)噪聲條件下對捷聯(lián)導(dǎo)引頭視線角速率的高精度估計(jì)。

圖6 相關(guān)噪聲解耦與未解耦狀態(tài)下視線高低角及其角速率估計(jì)曲線

圖7 相關(guān)噪聲解耦與未解耦狀態(tài)下視線方位角及其角速率估計(jì)曲線

4 結(jié) 論

1)通過對傳統(tǒng)球面坐標(biāo)系下的彈目距離及其變化率進(jìn)行規(guī)范化處理，建立了修正球面坐標(biāo)系下的視線角速率提取模型，經(jīng)過仿真驗(yàn)證，該模型具有更高的估計(jì)精度以及更好的數(shù)值穩(wěn)定性。

2)提出利用相關(guān)噪聲解耦的ESMF算法解決捷聯(lián)導(dǎo)引頭的視線角速率提取問題，經(jīng)過仿真驗(yàn)證，在模型中存在均勻噪聲的條件下，其視線角速率的估計(jì)誤差不大于0.005 rad/s，視線角的估計(jì)誤差不大于0.01 rad，說明所提出的算法在典型非高斯相關(guān)噪聲情況下具有良好的濾波精度，對于其他非高斯噪聲條件下的濾波效果尚需進(jìn)一步研究。