基于IMM-RDCKF的機(jī)動(dòng)目標(biāo)跟蹤算法

(中國電子科技集團(tuán)公司第二十研究所， 陜西西安 710068)

0 引言

目標(biāo)跟蹤是利用傳感器獲得關(guān)于目標(biāo)的不精確的觀測信息，對(duì)目標(biāo)狀態(tài)持續(xù)進(jìn)行準(zhǔn)確的估計(jì)和預(yù)測目標(biāo)的真實(shí)信息。目標(biāo)運(yùn)動(dòng)的不確定性、目標(biāo)機(jī)動(dòng)能力日益增強(qiáng)都使得準(zhǔn)確估計(jì)目標(biāo)的運(yùn)動(dòng)狀態(tài)日益困難。在這種情況下，交互多模型混合估計(jì)方法被認(rèn)為是一種最有效的混合估計(jì)方案。

交互多模型算法(IMM)是由Blom和Bar-Shalom提出的一種軟切換算法，應(yīng)用很廣泛。近年來IMM研究的主要內(nèi)容第一層是變結(jié)構(gòu)交互多模及模型集自適應(yīng)、轉(zhuǎn)移概率自適應(yīng)，而第二層是目標(biāo)運(yùn)動(dòng)模型、非線性濾波器及其參數(shù)自適應(yīng)。雖然IMM的機(jī)理從理論上仍然無法論證清晰，但交互多模型混合估計(jì)方法仍然成為機(jī)動(dòng)目標(biāo)跟蹤領(lǐng)域主流的混合估計(jì)方案。

Markov轉(zhuǎn)移概率是IMM算法的重要參數(shù)之一，影響子模型之間的交互與模型切換速度，一般是先驗(yàn)給定的。這必然會(huì)與目標(biāo)的實(shí)際運(yùn)動(dòng)狀態(tài)不匹配，引起濾波跟蹤精度下降。因此，轉(zhuǎn)移概率的實(shí)時(shí)自適應(yīng)一直是國內(nèi)外學(xué)者探討的重要內(nèi)容[1-4]。

在目標(biāo)跟蹤系統(tǒng)中，解決非線性濾波最常用的方法是EKF方法及其相關(guān)改進(jìn)算法。CKF算法是近年來新出現(xiàn)的一種非線性濾波算法，該算法利用了三階球面-徑向容積積分準(zhǔn)則進(jìn)行了嚴(yán)密的數(shù)學(xué)推導(dǎo)，在理論上對(duì)該算法具有嚴(yán)格的保證[5-6]，估計(jì)精度和數(shù)值穩(wěn)定性都比較高。在對(duì)CKF算法深入研究的基礎(chǔ)上，有學(xué)者提出了降維CKF算法[6-7]，并將降維CKF成功應(yīng)用于工程實(shí)踐[7-8]。

本文針對(duì)機(jī)動(dòng)目標(biāo)跟蹤，基于降維CKF，線性簡化CKF[9]，采用時(shí)變Markov轉(zhuǎn)移概率IMM算法，設(shè)計(jì)了交互式多模型降維容積卡爾曼濾波算法( IMM-RDCKF)，提高了算法的魯棒性和估計(jì)精度。仿真表明，計(jì)算量約為IMM-CKF的一半，僅比IMM-EKF增加約30%，目標(biāo)跟蹤精度提升，便于工程應(yīng)用。尤其是勻速運(yùn)動(dòng)速度估計(jì)精度提升約27%。這對(duì)于預(yù)警探測、火力控制、指揮控制等軍事應(yīng)用領(lǐng)域具有非常重要的意義。

1 問題描述

常規(guī)氣動(dòng)目標(biāo)的機(jī)動(dòng)可以假設(shè)為目標(biāo)在不同時(shí)間段依據(jù)不同的運(yùn)動(dòng)模型，因而機(jī)動(dòng)目標(biāo)的運(yùn)動(dòng)模型可以假設(shè)為具有加性高斯噪聲的混合系統(tǒng)，是典型的非線性系統(tǒng)，其數(shù)學(xué)描述如下：

(1)

式中：Xk是k時(shí)刻系統(tǒng)狀態(tài)變量；Zk是k時(shí)刻的系統(tǒng)量測變量；Fk是系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣；Wk是過程噪聲Wk～N(Wk；0,Qk)；hk(Xk)是非線性測量函數(shù)；Vk是觀測噪聲為Vk～N(Vk；0,Rk)。Wk與Vk相互統(tǒng)計(jì)獨(dú)立。

本文中，系統(tǒng)狀態(tài)向量三維空間中的9維向量：

雷達(dá)量測點(diǎn)跡在以雷達(dá)天線原點(diǎn)為中心的三維球坐標(biāo)系下獲得的距離r，方位角α，俯仰角β，量測向量為

(2)

式中，ωr，ωα，ωβ分別為距離、方位和俯仰角的量測噪聲。

考慮目標(biāo)加速及轉(zhuǎn)彎影響，目標(biāo)運(yùn)動(dòng)模型選擇三維協(xié)同轉(zhuǎn)彎模型(3DCSCT)、擴(kuò)維勻速直線運(yùn)動(dòng)模型(CV)，3DCSCT相應(yīng)的過程噪聲Q(ω)參見文獻(xiàn)[10]。

(3)

(4)

2 CKF算法

2.1 CKF算法

CKF是基于高斯假設(shè)的貝葉斯濾波估計(jì)的基本框架，通過容積準(zhǔn)則能將非線性濾波問題轉(zhuǎn)化成為高斯概率密度函數(shù)加權(quán)的多維幾何體的容積計(jì)算。

設(shè)離散非線性系統(tǒng)為

(5)

式中：f(·)和h(·)為已知任意函數(shù)，系統(tǒng)噪聲為Wk～N(Wk；0,Qk)；觀測噪聲為Vk～N(Vk；0,Rk)。

根據(jù)三階球面-徑向容積積分準(zhǔn)則，CKF算法[5]的實(shí)現(xiàn)步驟為：

計(jì)算狀態(tài)預(yù)測：

xi,k|k-1=f(xi,k-1)

(6)

(7)

計(jì)算量測預(yù)測：

zi,k|k-1=h(xi,k|k-1)

(9)

(10)

計(jì)算協(xié)方差和互協(xié)方差：

(11)

(12)

狀態(tài)更新：

(13)

(14)

(15)

2.2 線性簡化CKF算法

在實(shí)際的工程應(yīng)用中，狀態(tài)方程、量測方程并非都是非線性的。例如導(dǎo)航領(lǐng)域中，量測方程是線性的；目標(biāo)跟蹤應(yīng)用領(lǐng)域中，其狀態(tài)方程是線性的。此時(shí)，針對(duì)線性的模型方程，常規(guī)線性Kalman濾波算法理論上是最小均方準(zhǔn)則最優(yōu)的，因此可以直接用線性Kalman濾波進(jìn)行運(yùn)算[9]。

針對(duì)式(1)描述的目標(biāo)跟蹤應(yīng)用場景，一步預(yù)測步驟可以簡化為

(16)

(17)

即為KF算法的一步預(yù)測過程，其濾波精度與式(6)～式(8)相當(dāng)。

2.3 降維CKF算法

常規(guī)CKF算法采樣點(diǎn)均是系統(tǒng)狀態(tài)向量維數(shù)的2倍。對(duì)于導(dǎo)航、目標(biāo)跟蹤等特殊非線性模型，可以發(fā)現(xiàn)，影響系統(tǒng)非線性的只是其狀態(tài)向量的部分元素，引起線性Kalman濾波算法無法使用，而只能使用非線性濾波算法。例如針對(duì)式(1)的EKF算法中，其量測矩陣H中，只有1，4，7列針對(duì)位置的偏導(dǎo)數(shù)不為零，而2，3，5，6，8，9列針對(duì)速度、加速度的偏導(dǎo)數(shù)均為零。因此在增益計(jì)算中：

Kk=Pk|k-1HT(HPk|k-1HT+R)-1

實(shí)際只有Pk|k-1的1，4，7行、列中的位置協(xié)方差對(duì)增益有貢獻(xiàn)。據(jù)此，可以對(duì)EKF算法作一定程度的精簡。

對(duì)于目標(biāo)跟蹤應(yīng)用系統(tǒng)采用CKF濾波過程中，同樣存在類似情況。其系統(tǒng)由式(1)描述，狀態(tài)方程是線性的，量測方程的非線性僅與直角坐標(biāo)下目標(biāo)的位置x,y,z有關(guān)。對(duì)線性變量和非線性變量進(jìn)行區(qū)別處理，減少采樣的容積點(diǎn)，有利于降低計(jì)算復(fù)雜度，減少計(jì)算量[6-9]。由于降維CKF依然采用三階球面-徑向容積積分準(zhǔn)則，理論估計(jì)精度為三階多項(xiàng)式精度，因此濾波精度與常規(guī)CKF相當(dāng)，針對(duì)目標(biāo)跟蹤的CKF算法量測更新過程可以進(jìn)一步簡化。

首先，對(duì)系統(tǒng)狀態(tài)向量調(diào)整順序，變?yōu)?/p>

對(duì)協(xié)方差Pk同步進(jìn)行調(diào)整，把位置x,y,z有關(guān)的矩陣元素調(diào)整到前t行前t列，即矩陣的左上角。針對(duì)目標(biāo)跟蹤應(yīng)用場景，t=3。

zi,k|k-1=h(ζi,k|k-1)

(18)

(19)

計(jì)算協(xié)方差和互協(xié)方差：

(20)

(21)

至此，式(13)～式(21)構(gòu)成完整的線性簡化降維CKF(RDCKF)算法，將此算法與交互多模型算法結(jié)合，即可構(gòu)成IMM-RDCKF算法。

2.4 模型轉(zhuǎn)移概率實(shí)時(shí)修正

IMM算法的輸出是各個(gè)子模型輸出結(jié)果以模型概率加權(quán)作為最終的濾波估計(jì)，各子模型依據(jù)馬爾科夫鏈以一定的轉(zhuǎn)移概率進(jìn)行切換。因此，模型概率表征了子模型對(duì)目標(biāo)運(yùn)動(dòng)的匹配度。本文采用了文獻(xiàn)[2]的方法，實(shí)時(shí)修正模型概率。

對(duì)于IMM算法的r個(gè)子模型，k時(shí)刻子模型j的概率μj(k)，Markov矩陣中子模型i到子模型j的轉(zhuǎn)移概率為pij(k)。則

pij(k)′=κj(k)pij(k-1),i,j=1,…,r

(22)

式中，κj(k)=e(μj(k)-μj(k-1))。

進(jìn)一步歸一化處理：

(23)

該算法通過子模型后驗(yàn)概率自適應(yīng)地遞推估計(jì)模型轉(zhuǎn)移概率，提高匹配模型的概率，抑制非匹配模型的概率，因而可以提高濾波過程中的模型切換速度，從而提高跟蹤精度和收斂速度。

2.5 協(xié)方差矩陣對(duì)稱性和半正定性保障

在仿真實(shí)現(xiàn)過程中，發(fā)現(xiàn)由于數(shù)值計(jì)算誤差的累積，經(jīng)常出現(xiàn)協(xié)方差矩陣非正定導(dǎo)致Cholesky分解無法進(jìn)行，遞推中斷。因此，對(duì)代碼實(shí)現(xiàn)進(jìn)行了優(yōu)化：

(24)

采用式(24)替換式(15)，消除了誤差累積，極大地提高了算法的魯棒性。

同時(shí)，EKF算法在遞推過程中，同樣存在數(shù)值計(jì)算誤差、一階泰勒展開截?cái)嗾`差的累積導(dǎo)致協(xié)方差矩陣不可逆。因此EKF算法僅僅由于運(yùn)算簡單而廣泛使用，但同時(shí)要忍受其精度較差、魯棒性差的缺點(diǎn)。

平方根算法的優(yōu)點(diǎn)在于遞推中直接傳遞協(xié)方差矩陣的平方根而避免了Cholesky分解、奇異值分解等各種平方根分解步驟，因而魯棒性較好。但是對(duì)于各種改進(jìn)算法，如過程噪聲自適應(yīng)、量測噪聲動(dòng)態(tài)估計(jì)等，增加了Q，R的平方根分解，算法步驟過于復(fù)雜，計(jì)算效率不高。此時(shí)，本文算法的一系列改進(jìn)既能保證精度，又能保證魯棒性，還能進(jìn)一步疊加過程噪聲自適應(yīng)、量測噪聲動(dòng)態(tài)估計(jì)等其他改進(jìn)方法，其工程價(jià)值較高。

3 算法實(shí)現(xiàn)與仿真

3.1 仿真場景

目標(biāo)在三維空間的初始狀態(tài)為[120 000； -426；0；2 000；0；0；2 000；0；0]，采用周期T為1 s，雷達(dá)量測誤差距離標(biāo)準(zhǔn)差100 m，方位、俯仰角度標(biāo)準(zhǔn)差1°，初始協(xié)方差為P0=diag([100，10，1，100，10，1，100，10，1])，過程噪聲方差陣為Q=diag([80，10，10，80，10，10，80，10，10])。仿真時(shí)長200 s，目標(biāo)發(fā)生機(jī)動(dòng)時(shí)刻及加速度大小如表1所示，其余時(shí)間作勻速運(yùn)動(dòng)，進(jìn)行100次蒙特卡羅仿真。本文的算法在仿真時(shí)采用擴(kuò)維CV，3DCSCT模型，作為對(duì)比，常規(guī)三模型IMM子模型選擇CV，CS，3DCSCT。仿真平臺(tái)是Intel Core i5-3470，主頻3.2 GHz CPU，4 G內(nèi)存的PC機(jī)，軟件環(huán)境是Matlab 2013a。

算法性能評(píng)價(jià)指標(biāo)為均方根誤差：

(25)

IMM-RDCKF初始轉(zhuǎn)移概率矩陣：

三模型IMM轉(zhuǎn)移概率矩陣為

表1 目標(biāo)機(jī)動(dòng)運(yùn)動(dòng)情況表

3.2 仿真結(jié)果分析

仿真結(jié)果表明，本文算法和IMM-CKF算法都可以對(duì)目標(biāo)運(yùn)動(dòng)進(jìn)行有效跟蹤。經(jīng)過分析表明，IMM-RDCKF與常規(guī)IMM-EKF、IMM-CKF算法相比較，優(yōu)點(diǎn)在于以下幾點(diǎn)：

1) 跟蹤精度提高。本文算法通過實(shí)時(shí)修正模型轉(zhuǎn)移概率，使與目標(biāo)運(yùn)動(dòng)匹配的模型概率增大，加快模型切換速度，最終提高了精度，如圖1所示。從表2可以看出，在全航路，距離精度提高8.2%，速度精度提高12.9%；尤其是在勻速段，速度精度提升26.9%。另一方面，在圖2中，細(xì)實(shí)線表示IMM3-EKF算法，由于IMM3中存在模型競爭，實(shí)際濾波效果比本文算法還差。

2) 算法效率提升。采用線性簡化狀態(tài)方程、對(duì)量測方程降維處理。與IMM2-EKF相比，時(shí)間增加了35%；與IMM3-EKF相比，時(shí)間減少了21%；與IMM2-CKF相比，時(shí)間減少了53%。

3) 算法穩(wěn)定性提高。本文算法在實(shí)現(xiàn)時(shí)，優(yōu)化了實(shí)現(xiàn)步驟，避免了CKF算法實(shí)現(xiàn)時(shí)數(shù)值計(jì)算誤差積累造成的平方根分解錯(cuò)誤，因而不必采用平方根算法即可保證算法的穩(wěn)定性，同時(shí)也比EKF穩(wěn)定性強(qiáng)。

表2 算法性能對(duì)比

4 結(jié)束語

本文提出的時(shí)變轉(zhuǎn)移概率IMM-RDCKF算法，充分利用了量測方程中只有部分狀態(tài)變量是非線性的特點(diǎn)，只對(duì)非線性狀態(tài)變量采樣，不僅減小了計(jì)算量，而且繼承了CKF算法濾波精度高的優(yōu)點(diǎn)，有效克服了常規(guī)IMM-CKF算法計(jì)算量大的缺點(diǎn)；同時(shí)通過優(yōu)化代碼，消除了計(jì)算過程的數(shù)值計(jì)算誤差積累，保障協(xié)方差矩陣在濾波過程中始終保持對(duì)稱和正定，提高了算法穩(wěn)定性；通過實(shí)時(shí)修正模型轉(zhuǎn)移概率，使與目標(biāo)運(yùn)動(dòng)匹配的模型概率增大，加快模型切換速度，最終提高了精度。因此，本文提出的時(shí)變轉(zhuǎn)移概率IMM-RDCKF算法在計(jì)算效率、跟蹤精度、魯棒性方面都比常規(guī)交互多模算法強(qiáng)，是一種工程應(yīng)用價(jià)值比較高的機(jī)動(dòng)目標(biāo)跟蹤算法。