基于容積卡爾曼的粒子PHD多目標跟蹤算法

王海環(huán)，王 俊

（西安電子科技大學(xué)雷達信號處理國家重點實驗室，陜西西安710071）

基于容積卡爾曼的粒子PHD多目標跟蹤算法

王海環(huán)，王 俊

（西安電子科技大學(xué)雷達信號處理國家重點實驗室，陜西西安710071）

標準粒子概率假設(shè)密度（standard particle probability hypothesis density，SP-PHD）濾波在預(yù)測粒子狀態(tài)時沒有考慮最新的觀測信息，因而存在估計精度較低、粒子退化嚴重的問題，針對上述問題，提出基于容積卡爾曼的粒子概率假設(shè)密度（cubature Kalman particle probability hypothesis density，CP-PHD）濾波算法，該算法基于球面－徑向容積數(shù)值積分準則，利用容積卡爾曼濾波（cubature Kalman filter，CKF）產(chǎn)生建議密度函數(shù)，并對其進行采樣得到當前時刻的粒子狀態(tài)，從而使粒子分布更接近于真實的多目標后驗概率密度函數(shù)。同時，CP-PHD算法性能不受目標狀態(tài)維數(shù)影響，與無跡卡爾曼粒子概率假設(shè)密度（unscented Kalman particle probability hypothesis density，UP-PHD）濾波相比，具有更強適應(yīng)性和更好的跟蹤性能。實驗結(jié)果表明，CP-PHD算法的跟蹤精度優(yōu)于SP-PHD和UP-PHD。

多目標跟蹤；粒子概率假設(shè)密度濾波；容積卡爾曼濾波；建議密度函數(shù)

0 引 言

為避免傳統(tǒng)多目標跟蹤算法中的數(shù)據(jù)關(guān)聯(lián)，文獻［1］基于隨機有限集理論提出了概率假設(shè)密度（probability hypothesis density，PHD）濾波，該方法將目標的狀態(tài)和觀測量看成隨機有限集，將單目標的Bayes估計推廣到多目標跟蹤中，通過傳遞多目標全局后驗概率密度的一階矩得到各目標的狀態(tài)估計。由于PHD濾波算法不需要進行數(shù)據(jù)關(guān)聯(lián)，這不僅降低了多目標跟蹤中的計算量，同時也最大程度地保持了信息的完整性，因而受到越來越多的重視。

雖然PHD濾波算法只傳遞多目標全局后驗概率密度的一階矩，但在其遞推過程中仍需要求解高維積分，通常情況下很難得到閉合解。解決上述問題的方法一般有兩種：高斯混合概率假設(shè)密度［2］（Gaussian mixture PHD，GMPHD）和粒子概率假設(shè)密度［34］（particle PHD，P-PHD）。GMPHD是將目標的狀態(tài)分布表示成多個高斯分量的加權(quán)和的形式，在線性高斯的條件下得出PHDF的閉合解。P-PHD又稱為序貫蒙特卡羅概率假設(shè)密度（sequential Monte CarloPHD，SMC-PHD），是用一系列隨機樣本來近似多目標全局后驗概率密度的一階矩，具有粒子濾波的優(yōu)點，能適用于非線性非高斯環(huán)境。

標準的粒子概率假設(shè)密度（standard particle probability hypothesis density，SP-PHD）濾波算法在預(yù)測粒子狀態(tài)時，僅將目標狀態(tài)轉(zhuǎn)移方程作為建議密度函數(shù)，沒有利用觀測量，這會在目標運動模型不準的情況下，使大量粒子偏離真實目標狀態(tài)，更新這些粒子的權(quán)值需要消耗大量計算，但經(jīng)過若干步迭代后，這些粒子的權(quán)值會變得很小，對后驗概率卻幾乎沒有貢獻，這就是粒子退化問題。針對上述問題，很多學(xué)者采用不同的建議密度函數(shù)，提出多種改進的P-PHD濾波算法，例如基于輔助粒子濾波的概率假設(shè)密度（auxiliary particle PHD，AP-PHD）算法［5］、擴展卡爾曼粒子概率假設(shè)密度（extended Kalman particle PHD，EP-PHD）濾波［6］、無跡卡爾曼粒子概率假設(shè)密度（unscented Kalman particle PHD，UP-PHD）濾波［7］等，這些算法都是利用觀測值產(chǎn)生建議密度函數(shù)，從而達到修正粒子狀態(tài)的目的，在一定條件下比SP-PHD算法精度高。但是AP-PHD通過二次采樣產(chǎn)生建議密度函數(shù)，在增加算法計算量的同時，對算法的性能提升并不明顯；EP-PHD采用擴展卡爾曼濾波（extended Kalman filter，EKF）產(chǎn)生建議密度函數(shù)，EKF對泰勒展開式高階項的截斷導(dǎo)致算法在非線性程度比較高時誤差較大；UP-PHD采用無跡卡爾曼濾波［8］（unscented Kalman filter，UKF）產(chǎn)生建議密度函數(shù)，UKF算法理論基礎(chǔ)薄弱且當產(chǎn)生的sigma點的權(quán)值為負時算法不穩(wěn)定。

容積卡爾曼濾波［9］（cubature Kalman filter，CKF）是基于球面－徑向容積準則，采用一系列容積點近似目標后驗概率密度一種方法，屬于利用數(shù)值積分解決高維積分問題的范疇。該算法理論性強，在非線性條件下性能優(yōu)于EKF和UKF，實現(xiàn)中采用的容積點個數(shù)較UKF少，且算法穩(wěn)定性好。鑒于以上優(yōu)勢，本文提出一種基于容積卡爾曼的粒子概率假設(shè)密度（cubature Kalman particle PHD，CP-PHD）算法，該算法采用容積卡爾曼產(chǎn)生建議密度函數(shù)，并對其采樣得到粒子的預(yù)測狀態(tài)。仿真表明，本文提出的算法其跟蹤性能優(yōu)于SP-PHD濾波算法和UP-PHD濾波算法。

1 概率假設(shè)密度濾波

經(jīng)典的Bayes濾波理論適用于單目標跟蹤，其思想是利用目標狀態(tài)轉(zhuǎn)移方程和觀測方程，經(jīng)過時間更新和量測更新兩個步驟，實現(xiàn)對當前時刻目標狀態(tài)的預(yù)測和修正。在多目標跟蹤中，目標的狀態(tài)、個數(shù)及觀測量的數(shù)目都是時變的，為將經(jīng)典Bayes理論推廣到多目標跟蹤中，文獻［10］提出了隨機有限集（random finite sets，RFS）理論，在此基礎(chǔ)上建立多目標狀態(tài)和觀測模型。

狀態(tài)模型表示為

觀測模型表示為

式中，Xk＝｛x1，x2，…，xn（k）｝為k時刻目標的狀態(tài)集，其中xi（i＝1，2，…，n（k））為單目標狀態(tài)矢量；n（k）是k時刻目標的個數(shù)；Zk＝｛z1，z2，…，zm（k）｝為k時刻目標的量測集，zi（i＝1，2，…，m（k））為單個量測矢量；m（k）為k時刻觀測到的量測的個數(shù)；Ek｜k－1為存活目標狀態(tài)集；Bk｜k－1為衍生目標狀態(tài)集；Ψk為新生目標的狀態(tài)集；I（·）為目標量測集；Ck為雜波集。

基于以上模型，可推出多目標的Bayes估計遞推式如下：

（1）預(yù)測

（2）更新

式中，gk（·｜·）為多目標聯(lián)合似然函數(shù)；pk｜k（Xk｜Z1：k）為多目標聯(lián)合后驗概率密度；pk｜k－1（Xk｜Z1：k－1）為多目標聯(lián)合先驗概率密度；pk｜k－1（Xk｜Xk－1）為多目標狀態(tài)轉(zhuǎn)移概率密度函數(shù)。

為解決多目標Bayes估計中集合積分的問題，Mahler提出利用多目標全局后驗概率密度一階矩（稱為PHD或強度函數(shù)）代替其本身在Bayes遞推式中傳遞，這就是PHD濾波器。已知k－1時刻的多目標一階矩Dk－1（·），PHD濾波器的遞推方程如下［1］：

（1）預(yù)測

（2）更新

式中，ps，k（x）為目標存活概率；fk｜k－1（·｜·）為其單目標的狀態(tài)轉(zhuǎn)移密度；βk｜k－1（·｜·）為衍生目標的概率密度函數(shù)；γk為k時刻新生目標的強度函數(shù)；pd，k為k時刻目標檢測概率；ck為雜波概率密度；λk為雜波平均數(shù)；gk（·｜·）為單目標似然函數(shù)。

2 P-PHD濾波

盡管采用了全局后驗概率密度的一階矩近似，PHD的遞推方程中仍需要計算高維積分，通常情況下難以得到閉合解。為解決這一問題，文獻［3］提出采用粒子濾波實現(xiàn)PHD算法，這就是P-PHD濾波。P-PHD濾波是用一系列的加權(quán)粒子來表示全局后驗概率密度的一階矩在目標狀態(tài)空間中的分布，通過對這些粒子的狀態(tài)和權(quán)值進行繁衍和遞推，便可得到對全局后驗概率密度一階矩的遞推描述。

步驟1 預(yù)測

步驟2 更新

k時刻的量測集為Zk，用其對預(yù)測的粒子的權(quán)值進行更新得

式中

步驟3 重采樣

步驟4 目標狀態(tài)提取

利用k-means聚類方法得到目標狀態(tài)估計的具體流程如下：

（2）重復(fù)步驟3和步驟4直到每個聚類不再發(fā)生變化為止；

（3）計算其他粒子與聚類中心的距離，并將它們分配給與其相距最近的聚類中心所代表的聚類中；

（4）計算每個新聚類的聚類中心（該聚類中所有粒子狀態(tài)的均值）；

（5）將最終得到的聚類中心作為目標狀態(tài)估計輸出。

3 CP-PHD濾波

同PF相似，在P-PHD濾波中，建議密度函數(shù)的選取對算法的性能至關(guān)重要。SP-PHD算法直接將目標狀態(tài)轉(zhuǎn)移方程作為建議密度函數(shù)，沒有考慮當前觀測信息，在多次迭代后容易出現(xiàn)粒子退化?？紤]到狀態(tài)轉(zhuǎn)移方程和觀測方程都有可能是非線性的，通常采用二次采樣、EKF或UKF產(chǎn)生建議采樣函數(shù)，但二次采樣對算法性能提升不大，EKF在非線性程度較高時誤差較大，UKF的性能優(yōu)于二次采樣和EKF［11-12］，但在目標狀態(tài)維數(shù)較多時算法性能不穩(wěn)定。針對上述問題，本文提出利用CKF產(chǎn)生建議密度函數(shù)，該算法使用基于球面－徑向容積準則的數(shù)值積分方法直接計算粒子的均值和方差，較之EKF和UKF，其精度更高，算法更穩(wěn)定。

3.1 容積卡爾曼濾波

為得到非線性系統(tǒng)下Bayes濾波的遞推式，文獻［9］提出在高斯假設(shè)（系統(tǒng)狀態(tài)和噪聲都是高斯分布）的條件下，非線性濾波的問題可轉(zhuǎn)化成求解被積函數(shù)為非線性函數(shù)×高斯概率密度的積分的問題?？紤]最簡單的形式

式中，f（·）為為非線性函數(shù)；x∈Rn，n為x的維度；I為n×n階單位陣。

CKF通過以下兩步求解式（10）形式的積分：

步驟1 將式（10）由直角坐標系轉(zhuǎn)換到球面－徑向坐標系下。令x＝r y（yyT＝1），則xTx＝r2，r∈［0，∞），進而式（10）在球面－徑向坐標系下的形式為

式中，Un表示單位超球面；σ（·）表示積分微元。式（11）進一步化簡可寫為

由式（12）可看出，直角坐標系下的多維積分可分解為徑向一維積分和超球面多維積分的乘積。

步驟2 對式（12）通過高斯－厄米特準則和三次冪球面－徑向準則進行數(shù)值近似求解，可得到

可以看出，CKF與UKF結(jié)構(gòu)類似，但CKF所需的采樣點的權(quán)值相同，選擇較UKF簡單，且采樣點個數(shù)少于UKF。另外，UKF的采樣點和權(quán)值依賴于參數(shù)，而參數(shù)的選擇主要依靠經(jīng)驗，沒有明確的理論基礎(chǔ)，而CKF有嚴格的理論證明，且采樣點和權(quán)值不依賴于參數(shù)，較UKF有更強的適應(yīng)性。

3.2 CP-PHD濾波

CP-PHD濾波算法的框架與SP-PHD、UP-PHD相似，其主要區(qū)別為預(yù)測階段建議密度函數(shù)的產(chǎn)生方法上。設(shè)目標的狀態(tài)方程和觀測方程為

步驟1 預(yù)測

步驟1.1 對每一個粒子利用CKF產(chǎn)生建議密度函數(shù)

式中，n為目標狀態(tài)的維度。

a.時間更新

根據(jù)觀測方程傳遞容積點，更新粒子狀態(tài)，并計算其協(xié)方差矩陣

b.量測更新

計算容積點所對應(yīng)的觀測量及其均值為

步驟1.2 通過采樣獲得粒子預(yù)測狀態(tài)，并計算粒子權(quán)值。

式中，Jk＝，ρ為每個新生目標的采樣點數(shù)為新生目標的期望數(shù)。根據(jù)式（8）計算各個粒子的權(quán)值

步驟2 更新

該步驟與P-PHD濾波中的更新步驟相同，經(jīng)過該步驟后得到更新后的粒子權(quán)值。

步驟3 重采樣

步驟4 目標狀態(tài)提取

該步驟與P-PHD濾波中的目標狀態(tài)提取步驟相同。

3.3 性能分析

通過分析CP-PHD與UP-PHD的算法穩(wěn)定性來比較兩種算法的性能。CP-PHD與UP-PHD的主要不同點在于建議密度函數(shù)的產(chǎn)生方法不同：前者采用CKF，后者采用UKF。CKF和UKF都是利用數(shù)值積分來近似高維積分，定義穩(wěn)定因子為

式中，i為數(shù)值近似中的采樣點數(shù)。由文獻［9］可知，當I＞1時，多維積分的數(shù)值估計將引入較大誤差。在UKF中，w0＝κ／（n+κ），wi＝1／2（n+κ）（i＝1，2，…，2n），其中n+κ＝3。當目標狀態(tài)維數(shù)n小于等于3時，UKF和CKF的穩(wěn)定因子都等于1，兩者性能相當；當目標狀態(tài)維數(shù)n大于3時，UKF的穩(wěn)定因子大于1且隨著目標維數(shù)的增加而增大，從而導(dǎo)致積分估計精度變差，而CKF的穩(wěn)定因子與目標狀態(tài)維數(shù)無關(guān)，始終為1。通過以上分析可知，在高維目標狀態(tài)的條件下，CP-PHD的性能要優(yōu)于UP-PHD。

4 實驗仿真

本文在外輻射源雷達背景下，采用SP-PHD、UP-PHD、CP-PHD 3種方法對雜波環(huán)境下數(shù)目時變的多個目標進行跟蹤。外輻射源雷達本身沒有發(fā)射源，通過接收目標對第3方照射源的反射來實現(xiàn)目標探測。為提高定位和跟蹤精度，外輻射源雷達通常采用多站聯(lián)合定位，本文仿真采用3發(fā)單收（3個發(fā)射站，1個接收站）體制，如圖1所示。

圖1 3發(fā)單收外輻射源雷達示意圖

式中，q＝3 m／s2為過程噪聲標準差。觀測方程為

式中，νk為觀測噪聲；v1，k，v2，k為均值為零，標準差分別為100 m和1 m／s的高斯白噪聲。設(shè)目標存活概率ps＝0.98，檢測概率pd＝1，雜波在觀測區(qū)域內(nèi)均勻分布，雜波平均數(shù)λ＝10，每個存活目標采樣1 000個粒子，每個新生目標采樣500個粒子。

目標真實軌跡與3種方法的跟蹤軌跡如圖2所示，由圖2可直觀地看出，SP-PHD算法出現(xiàn)錯跟、漏跟的概率最大，這是由于SP-PHD在預(yù)測粒子狀態(tài)時沒有利用觀測信息，預(yù)測粒子狀態(tài)離真實值偏差較大，更新后較多粒子的權(quán)值很小，對得到目標當前狀態(tài)貢獻微弱導(dǎo)致的。UP-PHD算法和CP-PHD算法在預(yù)測粒子狀態(tài)時利用了觀測信息，因而其性能較SP-PHD算法有較大改善，其中CP-PHD的跟蹤精度又優(yōu)于UP-PHD算法，在3種算法中性能最優(yōu)。

圖2 跟蹤軌跡圖

文中采用最優(yōu)子模式分配（optimal subpattern assignment，OSPA）標準［13］計算多目標跟蹤中的誤差，OSPA距離是單個目標的平均誤差，由位置誤差和勢（目標個數(shù)）誤差兩部分組成，仿真中取距離誤差敏感參數(shù)p＝2，勢誤差敏感參數(shù)c＝500，圖3和圖4分別為100次蒙特卡羅實驗后，3種方法的目標數(shù)估計誤差對比圖和OSPA誤差對比圖。由圖3、圖4可以看出，SP-PHD的跟蹤精度最差，UPPHD和CP-PHD的性能與SP-PHD性能相比都有很大改善，而CP-PHD的跟蹤性能比UP-PHD更好，這符合第3.3節(jié)中對兩種算法性能的理論分析。

圖3 目標數(shù)目估計誤差對比圖

圖4 OSPA誤差對比圖

改變雜波平均數(shù)，使λ＝0.001，1，5，10，15，20，30，經(jīng)過100次蒙特卡羅實驗后，3種方法在不同雜波密度下的平均目標數(shù)估計誤差對比圖和平均OSPA誤差對比圖如圖5和圖6所示。由圖5和圖6可以看出，在相同雜波密度下，CP-PHD的算法性能要優(yōu)于其他兩種算法，同時隨著雜波密度的增大，3種算法性能在逐步下降，其中SP-PHD的性能下能最快，而CP-PHD對雜波環(huán)境的適應(yīng)性最強。

圖5 目標數(shù)目估計誤差圖

圖6 OSPA誤差對比圖

5 結(jié) 論

SP-PHD將目標狀態(tài)方程作為建議密度函數(shù)，沒有考慮當前觀測信息，因而加劇了粒子退化。UP-PHD算法利用UKF產(chǎn)生重要性函數(shù)，提高了P-PHD的算法性能。但UKF算法中的參數(shù)依賴經(jīng)驗調(diào)整，沒有明確理論依據(jù)，且算法的穩(wěn)定性不高。本文提出的CP-PHD算法，通過CKF產(chǎn)生建議密度函數(shù)，由于CKF比UKF所需的樣本點少，且算法性能不依賴于參數(shù)的設(shè)定，操作更簡單，因此CP-PHD算法與UP-PHD相比，算法計算量較小，且適用性更強。通過仿真對比可以看出，CP-PHD的算法性能要優(yōu)于UP-PHD算法。由于CKF中需要分解協(xié)方差矩陣，為避免這一操作，進一步增強算法的適用性和跟蹤精度，有學(xué)者提出平方根CKF［9］（square-rooted CKF，SCKF）算法和迭代的SCKF算法［1415］，將SCKF算法和迭代SCKF算法與P-PHD算法相結(jié)合，在保證跟蹤精度的同時提高算法穩(wěn)定性是下一步的工作重點。

［1］Mahler R.Multitarget Bayes filtering via first-order multitarget moments［J］.IEEE Trans.on Aerospace and Electronic Systems，2003，39（4）：1152-1178.

［2］Vo B N，Ma W K.The Gaussian mixture probability hypothesis density filter［J］.IEEE Trans.on Signal Processing，2006，54（11）：4091-4104.

［3］Vo B N，Singh S，Doucet A.Sequential Monte Carlo methods for multi-target filtering with random finite sets［J］.IEEE Trans.on Aerospace and Electronic Systems，2005，41（4）：1224-1245.

［4］Zajic T，Mahler R.A particle-systems implementation of the PHD multitarget tracking filter［C］∥Proc.of the SPIE-the International Society for Optical Engineering，2003：291-299.

［5］Whiteley N，Singh S，Godsill S.Auxiliary particle implementation of probability hypothesis density filter［J］.IEEE Trans.on Aerospace and Electronic Systems，2010，46（3）：1437-1454.

［6］Melzi M，Ouldali A，Messaoudi Z.Multiple target tracking using the extended Kalman particle probability hypothesis filter［C］∥Proc.of the 18th European Signal Processing Conference，2010：1821-1825.

［7］Melzi M，Ouldali A.Multiple target tracking and classification using the unscented probability hypothesis density filter［C］∥Proc.of the 7th International Workshop on Systems，Signal Processing and their Applications，2011：4-21.

［8］Julier S J，Uhlmann J K.Unscented filtering and nonlinear estimation［J］.Proc.of the IEEE，2004，92（3）：402-422.

［9］Arasaratnam I，Hykin S.Cubature Kalman filter［J］.IEEE Trans.on Automatic Control，2009，54（6）：1254-1269.

［10］Mahler R.Multitarget detection and acquisition：a unified approach［C］∥Proc.of the SPIE International Society for Optical Engineering，1999：218-229.

［11］Zuo J Y，Jia Y N.Adaptive iterated particle filter［J］.Electro-nics Letters，2013，49（12）：556-557.

［12］Pathuri B V，Unterrieder C，Huemer M.Battery internal state estimation：a comparative study of non-linear state estimation algorithms［C］∥Proc.of the IEEE Vehicle Power and Propulsion Conference（VPPC），2013：1-6.

［13］Schuhmacher D，Vo B T，Vo B N.A consistent metric for performance evaluation of multi-object filters［J］.IEEE Trans.on Signal Processing，2008，56（8）：3447-3457.

［14］Mu J，Cai Y L，Zhang J M.Cubature particle filter and its application［J］.Journal of Xi'an Jiaotong University，2011，45（8）：13-17.（穆靜，蔡遠利.容積粒子濾波算法及其應(yīng)用［J］.西安交通大學(xué)學(xué)報，2011，45（8）：13-17.）

［15］Wang H J，Jing Z R，Zheng W Q.Iterated cubature Kalman particle filter［J］.Journal of Southeast University，2013，43（S1）：85-88.（王華劍，景占榮，鄭文泉.迭代容積卡爾曼粒子濾波算法［J］.東南大學(xué)學(xué)報，2013，43（S1）：85-88.）

Multitarget tracking with the cubature Kalman particle probability hypothesis density filter

WANG Hai-huan，WANG Jun
（National Lab of Radar Signal Processing，Xidian University，Xi'an 710071，China）

A cubature Kalman particle probability hypothesis density（CP-PHD）filter is proposed to solve the problems of the low state estimation accuracy and the serious particles degradation in standard particle probability hypothesis density（SP-PHD）filter because of unused the most recent observation.CP-PHD uses cubature Kalman filter based on spherical-radial cubature rule to generate the proposal density function and obtains the present particles states by sampling from the proposal density function，so that particle distribution is closer to the real multi-target posterior probability density function.Otherwise，the performance of CP-PHD is not affected by the dimension of target state，so CP-PHD has stronger adaptive and better tracking performance than unscented Kalman particle probability hypothesis density（UP-PHD）filter.Simulation results show that the tracking accuracy of CP-PHD algorithm is superior to SP-PHD and UP-PHD.

multitarget tracking；particle probability hypothesis density（PHD）filter；cubature Kalman filter（CKF）；proposal density function

TN 953 文獻標志碼：A DOI：10.3969／j.issn.1001-506X.2015.09.02

王海環(huán)（1987-），女，博士研究生，主要研究方向為外輻射源雷達中的多目標跟蹤。

E-mail：haihuanwang＠126.com

王 ?。?969-），男，教授，博士，主要研究方向為無源多雙基地雷達探測系統(tǒng)技術(shù)、雷達信號處理和數(shù)據(jù)處理。

E-mail：wangjun＠xidian.edu.cn

1001-506X（2015）09-1960-07

2014-09-16；

2015-03-15；網(wǎng)絡(luò)優(yōu)先出版日期：2015-04-07。

網(wǎng)絡(luò)優(yōu)先出版地址：http：／／www.cnki.net／kcms／detail／11.2422.TN.20150407.1321.004.html

教育部創(chuàng)新團隊計劃（IRT0954）資助課題