基于交互式多模型魯棒濾波的目標(biāo)機(jī)動(dòng)估計(jì)算法*

彭一洋，奚 勇，陳 輝，孫 遜，仲科偉

0 引 言

目標(biāo)加速度估計(jì)是機(jī)動(dòng)目標(biāo)跟蹤領(lǐng)域的研究熱點(diǎn)，對(duì)于提高導(dǎo)彈的制導(dǎo)精度和彈道品質(zhì)具有重要意義[1,2].目標(biāo)加速度估計(jì)的有效性和可靠性取決于數(shù)學(xué)模型的匹配度以及濾波算法的估計(jì)精度.交互式多模型算法(interacting multiple model，IMM)能夠較好的解決單一模型難以匹配復(fù)雜目標(biāo)運(yùn)動(dòng)問題.通過預(yù)先設(shè)定運(yùn)動(dòng)模型集，IMM算法面對(duì)復(fù)雜機(jī)動(dòng)目標(biāo)具有較強(qiáng)的魯棒性[3].

濾波算法是影響IMM算法估計(jì)精度的重要因素之一，目標(biāo)加速度估計(jì)是一個(gè)典型的非線性濾波問題.傳統(tǒng)的擴(kuò)展卡爾曼濾波[4](extended Kalman filter，EKF)在面對(duì)強(qiáng)非線性系統(tǒng)時(shí)會(huì)引入較大的截?cái)嗾`差，同時(shí)計(jì)算雅克比矩陣也會(huì)提高濾波器的設(shè)計(jì)難度.無跡卡爾曼濾波[5](unscented Kalman filter，UKF)、容積卡爾曼濾波[6](cubature Kalman filter，CKF)等基于確定性采樣的非線性濾波方法雖然不需要計(jì)算雅克比矩陣，避免了線性截?cái)嗾`差.但UKF在面對(duì)高維系統(tǒng)時(shí)容易出現(xiàn)采樣點(diǎn)權(quán)值為負(fù)的情況，CKF無法準(zhǔn)確計(jì)算某些簡(jiǎn)單多項(xiàng)式的權(quán)值積分.高階容積卡爾曼濾波[7](high-degree cubature Kalman filter，HCKF)是近幾年提出的一種確定性采樣濾波方法.相比于傳統(tǒng)CKF，HCKF將球面-徑向容積準(zhǔn)則的階次提高為五階，避免了傳統(tǒng)CKF無法計(jì)算某些簡(jiǎn)單多項(xiàng)式權(quán)值的缺點(diǎn)，濾波穩(wěn)定性更好，精度更高.文獻(xiàn)[8]將高階容積卡爾曼濾波應(yīng)用于極區(qū)慣性/重力梯度組合導(dǎo)航.文獻(xiàn)[9]針對(duì)傳統(tǒng)的容積卡爾曼濾波(CKF)估計(jì)精度有限的問題，提出了一種基于任意階容積規(guī)則的高階容積卡爾曼濾波方法并應(yīng)用于機(jī)動(dòng)目標(biāo)跟蹤問題.

以上濾波算法皆假設(shè)測(cè)量噪聲具有高斯特性.但在實(shí)際應(yīng)用中，由于目標(biāo)不同部位對(duì)雷達(dá)波的反射能力不同，雷達(dá)導(dǎo)引頭對(duì)同一目標(biāo)的測(cè)量位置容易出現(xiàn)隨機(jī)擺動(dòng)，使導(dǎo)引頭的測(cè)量噪聲呈現(xiàn)出閃爍噪聲的特性.在這種情況下，傳統(tǒng)非線性卡爾曼濾波算法的估計(jì)精度會(huì)下降甚至發(fā)散.Huber提出的Huber-Based濾波能夠在傳統(tǒng)的高斯濾波理論框架內(nèi)解決非高斯問題，對(duì)具有閃爍噪聲的系統(tǒng)具有漸進(jìn)最優(yōu)魯棒性[10].文獻(xiàn)[11]提出了一種基于Huber理論的魯棒廣義HCKF，該方法在魯棒性和估計(jì)精度上均具有優(yōu)越的性能.文獻(xiàn)[12]基于Huber理論提出了一種魯棒差分濾波算法，數(shù)字仿真結(jié)果表明該濾波算法對(duì)混合高斯噪聲的精度更高.

本文針對(duì)雷達(dá)導(dǎo)引頭測(cè)量噪聲為閃爍噪聲的問題，在HCKF的測(cè)量更新部分采用Huber濾波理論，提出高階容積魯棒濾波，結(jié)合交互式多模型算法框架，提出多模型魯棒濾波算法.數(shù)字仿真結(jié)果表明，采用所提算法設(shè)計(jì)的目標(biāo)機(jī)動(dòng)估計(jì)器具有較強(qiáng)的魯棒性，在面對(duì)閃爍噪聲時(shí)，具有更高的濾波精度.

1 高階容積魯棒濾波

針對(duì)測(cè)量噪聲為閃爍噪聲的非線性濾波問題，采用高階容積卡爾曼濾波進(jìn)行時(shí)間更新，Huber-Based濾波進(jìn)行測(cè)量更新，提出高階容積魯棒濾波方法.

將目標(biāo)機(jī)動(dòng)估計(jì)問題考慮為如下非線性離散系統(tǒng):

xk=f(xk-1)+wk-1

(1)

zk=h(xk)+vk

(2)

式中，f和h分別為系統(tǒng)的狀態(tài)方程和測(cè)量方程，x∈Rn是狀態(tài)矢量，z∈Rm是測(cè)量矢量，wk-1是系統(tǒng)噪聲矢量，vk是測(cè)量噪聲矢量.

1.1 狀態(tài)更新

高階容積卡爾曼濾波采用五階容積-徑向準(zhǔn)則計(jì)算高斯權(quán)值積分公式來確定容積點(diǎn)和權(quán)值，通過容積點(diǎn)在非線性狀態(tài)方程中的傳播進(jìn)行狀態(tài)更新.高階容積魯棒濾波的狀態(tài)更新方法如下.

(3)

式中，n為狀態(tài)向量的維數(shù)，點(diǎn)集ξi表達(dá)式為

(4)

(5)

(6)

(2)使用系統(tǒng)狀態(tài)方程傳播容積點(diǎn)為

(7)

(3)式(8)、式(9)為狀態(tài)一步預(yù)測(cè)及其誤差協(xié)方差矩陣

(8)

(9)

式(9)中，Qk-1為系統(tǒng)噪聲協(xié)方差矩陣，權(quán)值wi表達(dá)式

(10)

(4)使用狀態(tài)一步預(yù)測(cè)及其誤差協(xié)方差計(jì)算容積點(diǎn)預(yù)測(cè)

(11)

(5)使用系統(tǒng)測(cè)量方程傳播容積點(diǎn)

(12)

(6)計(jì)算測(cè)量一步預(yù)測(cè)

(13)

(7)計(jì)算協(xié)方差矩陣

(14)

(15)

1.2 測(cè)量更新

高階容積魯棒濾波采用Huber-Based濾波進(jìn)行測(cè)量更新的方法，該方法將測(cè)量更新轉(zhuǎn)化為線性回歸問題，利用廣義極大似然估計(jì)進(jìn)行求解.下面給出高階容積魯棒濾波的測(cè)量更新方法.

令xk為k時(shí)刻目標(biāo)狀態(tài)的真實(shí)值，δk為狀態(tài)估計(jì)誤差.則有：

(16)

線性化測(cè)量方程可得：

(17)

式中，測(cè)量矩陣Hk=[(Pk/k-1)-1Pxz].

對(duì)式進(jìn)行移項(xiàng)可得如下線性回歸問題：

(18)

yk=Mkxk+ξk

(19)

式中，各變量表達(dá)式如式(20)～(21)所示.

(20)

(21)

(22)

(23)

測(cè)量更新即為求解如下代價(jià)函數(shù)極小值：

(24)

式中，ζi為ζ的第i個(gè)元素，ζ為殘差.

ρ為目標(biāo)函數(shù)，Huber證明了當(dāng)ρ滿足式(25)時(shí)，Huber-Based濾波對(duì)噪聲為閃爍噪聲的系統(tǒng)具備漸進(jìn)最優(yōu)魯棒性.

(25)

式中，γ為可調(diào)因子.

使代價(jià)函數(shù)J最小的解應(yīng)滿足式(26).

(26)

定義權(quán)值函數(shù)ψ(ζi)和矩陣Ψ分別如式(27)～(28)所示

ψ(ζi)=ρ′(ζi)/ζi

(27)

Ψ(ζi)=diag{ψ(ζi)}

(28)

則式(26)可寫成如式(29)所示的矩陣形式.

(29)

使用加權(quán)迭代法得到式(29)的解如式(30)所示

(30)

迭代初值如式(31)所示

(31)

狀態(tài)估計(jì)誤差協(xié)方差矩陣的計(jì)算方法如式(32)所示

(32)

2 IMM-HCHF算法

IMM算法需要預(yù)先設(shè)定不同的目標(biāo)跟蹤模型組成目標(biāo)機(jī)動(dòng)模型集.選取慣性系下目標(biāo)的位置矢量、速度矢量、加速度矢量作為狀態(tài)量，本文所采用的目標(biāo)機(jī)動(dòng)運(yùn)動(dòng)模型的離散形式如2.1節(jié)所示.

2.1 目標(biāo)機(jī)動(dòng)模型

本文選取常加速度模型、Singer模型[13]、“當(dāng)前”統(tǒng)計(jì)模型[14]3種模型組成目標(biāo)機(jī)動(dòng)運(yùn)動(dòng)模型集，下面將給出一維條件下3種模型的離散形式，三維條件下可直接進(jìn)行矩陣擴(kuò)展.

(1)常加速度模型

常加速度模型認(rèn)為目標(biāo)進(jìn)行勻加速運(yùn)動(dòng)，同時(shí)考慮實(shí)際系統(tǒng)中的加速度攝動(dòng)，加入了過程噪聲.常加速度模型的優(yōu)點(diǎn)是形式簡(jiǎn)單，易于使用，缺點(diǎn)是假設(shè)條件過于簡(jiǎn)化，估計(jì)精度較低.

常加速度模型的離散形式狀態(tài)方程如式(33)所示

(33)

(34)

(35)

式中，T為采樣步長，q為加速度方差.

(2)Singer模型

Singer模型認(rèn)為目標(biāo)加速度服從零均值假設(shè)，即認(rèn)為目標(biāo)加速度在一定范圍內(nèi)為均勻分布.Singer模型面對(duì)大多數(shù)機(jī)動(dòng)形式均能保證較高的精度，但由于該模型基于零均值假設(shè)，當(dāng)目標(biāo)的機(jī)動(dòng)形式為非零均值時(shí)，模型的估計(jì)精度則會(huì)下降.

Singer模型的離散形式狀態(tài)方程如式(36)所示

(36)

(37)

(38)

(39)

(3)“當(dāng)前”統(tǒng)計(jì)模型

“當(dāng)前”統(tǒng)計(jì)模型認(rèn)為目標(biāo)加速度服從修正的瑞利分布，當(dāng)目標(biāo)長時(shí)間存在較大機(jī)動(dòng)加速度時(shí)，“當(dāng)前”統(tǒng)計(jì)模型的估計(jì)精度明顯高于Singer模型，但由于“當(dāng)前”統(tǒng)計(jì)模型服從的是非零均值假設(shè)，在跟蹤勻速目標(biāo)時(shí)，該模型往往不能有效反映目標(biāo)的機(jī)動(dòng)情況.“當(dāng)前”統(tǒng)計(jì)模型和Singer模型能夠在一定程度上實(shí)現(xiàn)互補(bǔ).

“當(dāng)前”統(tǒng)計(jì)模型的離散形式狀態(tài)方程如式(37)所示

(40)

(41)

式中，T為采樣步長，α為機(jī)動(dòng)頻率，σ2為目標(biāo)加速度方差，則“當(dāng)前”統(tǒng)計(jì)模型的目標(biāo)加速度方差σ2的表達(dá)式如式(42)所示

(42)

2.2 測(cè)量模型

(43)

(44)

qε=arcsin(Δy/r)

(45)

qβ=arctan2(-Δz,Δx)

(46)

(47)

(48)

式中，Δ表示目標(biāo)和導(dǎo)彈之間的位置或速度差值.

2.3 IMM-HCHF算法

IMM算法的基本思想是選取不同的目標(biāo)機(jī)動(dòng)模型組成模型集，用于匹配不同的目標(biāo)機(jī)動(dòng)形式.在每個(gè)濾波周期內(nèi)，各個(gè)模型對(duì)同一量測(cè)數(shù)據(jù)進(jìn)行并行濾波處理，模型與模型之間通過時(shí)齊馬爾科夫鏈進(jìn)行轉(zhuǎn)換，最后采用貝葉斯公式對(duì)各并行濾波器的估計(jì)結(jié)果進(jìn)行數(shù)據(jù)融合作為濾波輸出.

IMM算法中并行濾波器的估計(jì)精度將直接影響整個(gè)算法的精度，本文提出的IMM-HCHF算法將前文介紹的HCHF算法應(yīng)用于傳統(tǒng)IMM算法的并行濾波器中，其他流程不變.

假設(shè)模型集所含模型數(shù)目為n，則模型間的轉(zhuǎn)移概率矩陣為馬爾科夫矩陣Π.

(49)

IMM算法流程如下：

(1)輸入交互

(50)

(51)

交互后各并行濾波器的狀態(tài)估計(jì)和協(xié)方差矩陣輸入如式(52)和式(53)所示.

(52)

(53)

(2)并行濾波器濾波

(3)模型概率估計(jì)

依據(jù)第②步中各濾波器的估計(jì)結(jié)果對(duì)模型更新模型概率.

(54)

(55)

(4)模型估計(jì)混合

利用更新后的模型概率對(duì)并行濾波器的輸出進(jìn)行加權(quán)融合得到最終的狀態(tài)估計(jì)結(jié)果，狀態(tài)估計(jì)如式(56)所示，狀態(tài)估計(jì)協(xié)方差矩陣如式(57)所示

(56)

(57)

3 數(shù)字仿真與結(jié)果分析

3.1 仿真場(chǎng)景

本文選擇戰(zhàn)斗機(jī)為目標(biāo)，采用2.2節(jié)建立的雷達(dá)導(dǎo)引頭測(cè)量模型對(duì)目標(biāo)進(jìn)行跟蹤，認(rèn)為目標(biāo)在雷達(dá)導(dǎo)引頭開機(jī)后開始進(jìn)行機(jī)動(dòng)，分別在導(dǎo)引頭的噪聲為閃爍噪聲和高斯白噪聲時(shí)進(jìn)行仿真.

閃爍噪聲的概率密度函數(shù)和參數(shù)如式(58)所示.目標(biāo)初始狀態(tài)如表1所示，導(dǎo)引頭測(cè)量誤差如表2所示.

表1 目標(biāo)初始狀態(tài)Tab.1 Initial states of target

表2 導(dǎo)引頭測(cè)量誤差Tab.2 Noise characteristics of seeker

p(x)=(1-ε)p1+εp2

(58)

3.2 仿真結(jié)果與分析

在導(dǎo)引頭開機(jī)后目標(biāo)首先進(jìn)行轉(zhuǎn)彎機(jī)動(dòng)逃離，然后進(jìn)行加速機(jī)動(dòng)，最后進(jìn)行滾筒機(jī)動(dòng).目標(biāo)的飛行軌跡如圖1所示.進(jìn)行100次蒙特卡洛數(shù)字仿真.當(dāng)導(dǎo)引頭測(cè)量噪聲為閃爍噪聲時(shí)，仿真結(jié)果如圖2～圖10所示.

圖1 目標(biāo)飛行軌跡Fig.1 The trajectory of target

圖2 x向位置估計(jì)結(jié)果Fig.2 The estimate results of the target position in x direction

圖3 y向位置估計(jì)結(jié)果Fig.3 The estimate results of the target position in y direction

圖5 x向速度估計(jì)結(jié)果Fig.5 The estimate results of the target velocity in x direction

圖6 y向速度估計(jì)結(jié)果Fig.6 The estimate results of the target velocity in y direction

圖7 z向速度估計(jì)結(jié)果Fig.7 The estimate results of the target velocity in z direction

圖8 x向加速度估計(jì)結(jié)果Fig.8 The estimate results of the target acceleration in x direction

圖9 y向加速度估計(jì)結(jié)果Fig.9 The estimate results of the target acceleration in y direction

圖10 z向加速度估計(jì)結(jié)果Fig.10 The estimate results of the target acceleration in z direction

從以上仿真結(jié)果中可以看出，在測(cè)量信息帶有閃爍噪聲時(shí)，IMM-EKF和IMM-HCKF的精度基本一致，IMM-HCHF的目標(biāo)加速度估計(jì)精度明顯高于IMM-EKF和IMM-HCKF，位置估計(jì)精度提高了約52%，速度估計(jì)精度提高了約45%，加速度估計(jì)精度提高了約41%.

Y向的速度和加速度估計(jì)均方誤差在4.2 s～4.5 s突然增大，主要原因是在該時(shí)間段內(nèi)發(fā)生了加速度值的突變，由于系統(tǒng)的慣性，濾波器的輸出無法瞬間跟上目標(biāo)狀態(tài)的變化，導(dǎo)致誤差瞬間增大，經(jīng)過約0.4s的收斂后均方誤差回到了合理水平.Z向發(fā)生的加速度估計(jì)均方誤差突然增大現(xiàn)象的原因與Y向相同，但由于加速度幅值變化相對(duì)較小，對(duì)目標(biāo)速度變化的影響較小，因此速度估計(jì)的均方誤差并不明顯.

當(dāng)導(dǎo)引頭測(cè)量噪聲為高斯白噪聲時(shí)，加速度的均方誤差結(jié)果如圖11所示.

圖11 加速度估計(jì)結(jié)果Fig.11 The estimate results of the target acceleration

從仿真結(jié)果中可以看出，IMM-HCHF能夠在量測(cè)噪聲為高斯白噪聲和閃爍噪聲兩種典型狀態(tài)下均保持較高的精度，具有較強(qiáng)的魯棒性和適應(yīng)性，當(dāng)導(dǎo)引頭測(cè)量噪聲為高斯白噪聲時(shí)，IMM-HCHF的精度與IMM-EKF和IMM-HCKF的精度相當(dāng).具有較強(qiáng)的魯棒性和適應(yīng)性.

4 結(jié) 論

本文以導(dǎo)彈的雷達(dá)導(dǎo)引頭對(duì)復(fù)雜機(jī)動(dòng)目標(biāo)的軌跡跟蹤問題研究為背景，在IMM目標(biāo)狀態(tài)估計(jì)框架下，針對(duì)濾波過程中傳統(tǒng)卡爾曼濾波無法應(yīng)對(duì)閃爍噪聲的問題，提出了基于Huber算法的高階容積魯棒濾波算法.選取做復(fù)雜機(jī)動(dòng)的戰(zhàn)斗機(jī)為狀態(tài)估計(jì)對(duì)象，進(jìn)行了三維數(shù)字仿真驗(yàn)證.仿真結(jié)果表明，該方法可以有效解決測(cè)量噪聲為閃爍噪聲時(shí)的目標(biāo)加速度估計(jì)問題，估計(jì)精度高于IMM-EKF和IMM-HCKF，在面對(duì)高斯白噪聲時(shí)具有與傳統(tǒng)高斯濾波相當(dāng)?shù)臑V波精度，具有較強(qiáng)的魯棒性和適應(yīng)性.