多傳感器箱粒子PHD濾波多目標(biāo)跟蹤算法

蔡如華，楊 標(biāo)，吳孫勇,2

（1.桂林電子科技大學(xué) 數(shù)學(xué)與計(jì)算科學(xué)學(xué)院，廣西 桂林 541004；2.廣西密碼學(xué)與信息安全重點(diǎn)實(shí)驗(yàn)室，廣西 桂林 541004）

0 引言

多目標(biāo)跟蹤（multi-target tracking,MTT）算法的有效性和實(shí)時(shí)性一直以來是國內(nèi)外學(xué)者關(guān)注的熱點(diǎn)。針對多目標(biāo)跟蹤問題，Mahler 提出了隨機(jī)有限集[1]（random finite set,RFS）理論，將多目標(biāo)狀態(tài)集和多目標(biāo)觀測集建模為隨機(jī)有限集的形式，有效地解決了基于多假設(shè)跟蹤[2]和聯(lián)合概率數(shù)據(jù)關(guān)聯(lián)[3]在多目標(biāo)跟蹤過程中產(chǎn)生的組合爆炸問題，并提出了基于隨機(jī)有限集的概率假設(shè)密度[4]（probability hypothesis density filter,PHD）濾波器，PHD濾波器能夠?qū)δ繕?biāo)狀態(tài)和數(shù)目進(jìn)行有效的估計(jì)。

傳統(tǒng)的粒子濾波器是以點(diǎn)粒子的形式來模擬目標(biāo)狀態(tài)。在進(jìn)行蒙特卡洛實(shí)驗(yàn)時(shí)，需要大量的點(diǎn)粒子來對目標(biāo)進(jìn)行捕獲和持續(xù)跟蹤，所以會產(chǎn)生較大的計(jì)算量，為減輕計(jì)算量，文獻(xiàn)[5]提出了基于區(qū)間分析[6-7]的箱粒子濾波算法，之后在文獻(xiàn)[8]中結(jié)合PHD濾波器實(shí)現(xiàn)了箱粒子PHD濾波器，從而減輕了計(jì)算量。2015年國內(nèi)學(xué)者宋驪平[9]將箱粒子PHD濾波用于擴(kuò)展目標(biāo)跟蹤，之后在文獻(xiàn)[10]中使用箱粒子PHD濾波對群目標(biāo)進(jìn)行了有效的跟蹤。文獻(xiàn)[11]提出了未知雜波狀態(tài)下基于箱粒子濾波的PHD算法。

但對于低可觀測目標(biāo)的跟蹤，單傳感器PHD濾波器已不能滿足需求，而多傳感器技術(shù)的應(yīng)用則能夠很好地解決這一問題。早在2009年，Mahler[12-13]便給出了多傳感器的實(shí)現(xiàn)。文獻(xiàn)[14-15]分別給出了多傳感器PHD濾波的序列蒙特卡洛實(shí)現(xiàn)和高斯實(shí)現(xiàn)，文獻(xiàn)[16]利用先驗(yàn)信息對下一時(shí)刻可能存在目標(biāo)的區(qū)域進(jìn)行壓縮，并利用多個(gè)被動式傳感器同時(shí)對可能存在目標(biāo)的區(qū)域進(jìn)行觀測，以多個(gè)被動式傳感器的觀測角度相交區(qū)域以及傳感器位置計(jì)算得到目標(biāo)位置區(qū)間，作為目標(biāo)區(qū)間量測，根據(jù)區(qū)間量測值產(chǎn)生箱粒子，再利用箱粒子PHD濾波對目標(biāo)進(jìn)行跟蹤，旨在去除虛假量測降低運(yùn)算量，提升計(jì)算效率。

本文則是針對低可觀測目標(biāo)難以跟蹤以及點(diǎn)粒子濾波產(chǎn)生較大計(jì)算量的問題，利用多傳感器技術(shù)在低可觀測目標(biāo)檢測上的優(yōu)勢以及箱粒子濾波能夠提升計(jì)算效率的優(yōu)勢，提出MS-BOX-PHD算法。并通過仿真實(shí)驗(yàn)驗(yàn)證了MS-BOX-PHD算法對多目標(biāo)狀態(tài)以及數(shù)目估計(jì)的穩(wěn)定有效性，以及MS-BOX-PHD算法相對于區(qū)間量測下多傳感器標(biāo)準(zhǔn)PHD 粒子濾波（multi-sensor standard probability hypothesis density particle filter with interval measurement,IM-PHD-PF）算法在計(jì)算效率上的優(yōu)勢。

1 系統(tǒng)模型

在多目標(biāo)跟蹤系統(tǒng)中，會有多個(gè)目標(biāo)隨機(jī)的出現(xiàn)和消失在觀測區(qū)域中。全文單目標(biāo)狀態(tài)用小寫字母x表示，其中(x,y)包含目標(biāo)位置信息，包含目標(biāo)速度信息；多目標(biāo)狀態(tài)用大寫字母X表示；單目標(biāo)量測用小寫字母z，z= [θ;r]，其中θ表示角度，r表示距離；多目標(biāo)量測集用大寫字母Z表示。

1.1 多目標(biāo)狀態(tài)轉(zhuǎn)移模型

考慮系統(tǒng)：

式中：k為時(shí)間指標(biāo)；f為非線性的狀態(tài)轉(zhuǎn)移函數(shù)；wk表示k時(shí)刻的過程噪聲。對于k時(shí)刻多目標(biāo)狀態(tài)可以描述為隨機(jī)有限集：

式中：N表示k時(shí)刻的目標(biāo)個(gè)數(shù)。xk,i表示k時(shí)刻的第i個(gè)目標(biāo)，可能為新生目標(biāo)，也可能為k－1時(shí)刻存活并轉(zhuǎn)移后的目標(biāo)。

1.2 多目標(biāo)多傳感器量測模型

對于k時(shí)刻的目標(biāo)xk,i，其可能被傳感器檢測到并產(chǎn)生量測值zk,i，也可能未被傳感器檢測到，沒有產(chǎn)生量測值，同樣在k時(shí)刻傳感器也可能產(chǎn)生虛假量測。

由第s∈{1,2,…,S}個(gè)傳感器產(chǎn)生的量測集合Zks可表示為：

本文采用非線性量測模型，假設(shè)第s個(gè)傳感器位置為(dxs,dys)，則量測方程為：

2 收縮算法

收縮算法以區(qū)間分析[6-7]為基礎(chǔ)，通過區(qū)間收縮，保證更新之后的箱粒子擁有一個(gè)合適的區(qū)間大小，并利用收縮前后的箱粒子區(qū)間大小來求解似然函數(shù)，從而對箱粒子的權(quán)重進(jìn)行更新。

假設(shè)原箱粒子為[x]，區(qū)間量測值記為[z]，h為量測方程，若則[x′]＝[x]，但是似然度否則收縮后的箱粒子滿足：

收縮之后得到的箱粒子[x′]的似然度可以表示為：

式中：|[?](i)|表示箱粒子[?]的第i維的長度。

3 MS-BOX-PHD濾波

假設(shè)在k－1時(shí)刻的后驗(yàn)PHD由一組服從均勻分布的箱粒子集描述為：

式中：[xk－1,i]為箱粒子支撐集；Lk－1為k－1時(shí)刻箱粒子的個(gè)數(shù)。

3.1 MS-BOX-PHD濾波預(yù)測

若γk(x)表示新生目標(biāo)貢獻(xiàn)強(qiáng)度，βSur,k(xk)表示k－1時(shí)刻到k時(shí)刻存活下來的目標(biāo)貢獻(xiàn)強(qiáng)度，表示k－1時(shí)刻目標(biāo)x空間分布概率密度，表示目標(biāo)x由k－1時(shí)刻到k時(shí)刻的轉(zhuǎn)移密度，則k時(shí)刻預(yù)測PHD可以由k時(shí)刻的預(yù)測箱粒子參數(shù)集表示為：

新生箱粒子根據(jù)k－1時(shí)刻量測產(chǎn)生[5]，在量測上服從均勻分布，為新生箱粒子集合，新生目標(biāo)貢獻(xiàn)強(qiáng)度γk(x)可由其表示為：

取[wk－1]為k－1 時(shí)刻的過程噪聲區(qū)間，[fk－1]為k－1 時(shí)刻的狀態(tài)轉(zhuǎn)移函數(shù)的包含函數(shù)；pSur([?])表示箱粒子[?]的存活概率，則可由狀態(tài)轉(zhuǎn)移方程得到： 區(qū)1

3.2 MS-BOX-PHD濾波更新

3.2.1 量測融合

量測融合分兩步進(jìn)行：

Step 1：輸入k時(shí)刻S個(gè)傳感器產(chǎn)生的量測集將S個(gè)傳感器產(chǎn)生的區(qū)間量測轉(zhuǎn)換到同一坐標(biāo)系下，步驟如下：

輸入k時(shí)刻區(qū)間量測和傳感器坐標(biāo)：

對S個(gè)傳感器循環(huán)：

對k時(shí)刻第s個(gè)傳感器產(chǎn)生所有量測循環(huán)：

Step 2：對S個(gè)傳感器產(chǎn)生的量測集進(jìn)行融合。若各傳感器區(qū)間量測間有交集，則保留相交的區(qū)間量測的交集以及其他互不相交的區(qū)間量測值作為當(dāng)前時(shí)刻區(qū)間量測集。

以兩個(gè)傳感器為例，對k時(shí)刻S個(gè)傳感器產(chǎn)生的區(qū)間量測進(jìn)行分析，共分為3種情況（如圖1(a)、1(b)、1(c)）。

圖1 傳感器量測分析Fig.1 Sensor measurements analysis

假設(shè)k時(shí)刻傳感器1 產(chǎn)生量測集經(jīng)過坐標(biāo)轉(zhuǎn)換后記為傳感器2 產(chǎn)生量測集經(jīng)過坐標(biāo)轉(zhuǎn)換后為記為

顯然因?yàn)榱繙y融合同時(shí)考慮到多個(gè)傳感器的區(qū)間量測，包含目標(biāo)真實(shí)信息的可能性也會更大。若其中一個(gè)傳感器無法對目標(biāo)進(jìn)行有效檢測時(shí)，其他傳感器也能夠?qū)ζ錂z測并產(chǎn)生量測。若多個(gè)傳感器同時(shí)檢測到目標(biāo)，則通過情形3可知，通過區(qū)間量測相交運(yùn)算則能夠?qū)δ繕?biāo)量測進(jìn)行壓縮，保證較小的量測區(qū)間即可包含目標(biāo)信息。

通過量測融合后得到的k時(shí)刻量測融合集Zk仍然是一個(gè)隨機(jī)有限集，包括真實(shí)目標(biāo)產(chǎn)生量測和雜波。將其看作是由一個(gè)虛擬傳感器產(chǎn)生的量測集，若第s個(gè)傳感器的雜波空間分布率和雜波空間密度分別用λs和pdfs表示，則虛擬傳感器的量測集中雜波概率假設(shè)密度為：

若第s個(gè)傳感器的檢測概率表示為pDs，則虛擬傳感器的檢測概率為：

3.2.2 濾波更新

輸入k時(shí)刻S個(gè)傳感器融合后的區(qū)間量測集Zk,對預(yù)測得到的箱粒子集進(jìn)行更新。

4 數(shù)值實(shí)驗(yàn)

實(shí)驗(yàn)計(jì)算機(jī)系統(tǒng)為Windows 10 專業(yè)版，64位操作系統(tǒng)。處理器為Intel(R) Core(TM) i5-6500 CPU @3.20 GHz，運(yùn)行內(nèi)存為8.00 GB。

實(shí)驗(yàn)中設(shè)置所有目標(biāo)在觀測區(qū)域中服從勻速直線（constant velocity,CV）運(yùn)動模型：

式中：過程噪聲wk為服從協(xié)方差為Q的高斯分布：

本文綜合考慮位置估計(jì)誤差和勢估計(jì)誤差，使用最優(yōu)子模式分配距離[17]（optimal subpattern assignment,OSPA）來對MS-BOX-PHD濾波器的跟蹤性能進(jìn)行評估。對于任意的兩個(gè)多目標(biāo)集合X和Y，若X中包含 |m X=個(gè)狀態(tài)，Y中包含 |n Y=個(gè)狀態(tài)，則X和Y之間的OSPA 距離定義為：

1）當(dāng)m≤n時(shí)：

2）當(dāng)m≥n時(shí)：

3）當(dāng)m＝n＝0時(shí)：

式中：c為目標(biāo)個(gè)數(shù)誤差懲罰參數(shù)；p為階數(shù)，用來懲罰目標(biāo)位置估計(jì)誤差。本文實(shí)驗(yàn)中c＝100，p＝1。

4.1 場景1

設(shè)置傳感器1位于坐標(biāo)原點(diǎn)[0,0]的觀測角度范圍為θ1∈[0,π/2]，距離范圍為r1∈[0 m,1500 m]，雜波率為λ1＝2；傳感器2位于[400,0]，觀測角度范圍為θ2∈[π/2,π]，距離范圍為r2∈[0 m,1500 m]，雜波率為λ2＝2；傳感器3位于[700,600]，觀測角度范圍為θ2∈[－π/2,－π]，距離范圍為r3∈[0 m,1500 m]，雜波率為λ3＝2。場景1 中共有10個(gè)目標(biāo)隨機(jī)出現(xiàn)和消失在所有傳感器的觀測區(qū)域內(nèi)。試驗(yàn)中所有目標(biāo)存活并進(jìn)行狀態(tài)轉(zhuǎn)移的概率為pSur＝0.98。

圖2為單次蒙特卡洛實(shí)驗(yàn)下，傳感器檢測概率為0.65時(shí)的目標(biāo)真實(shí)位置以及傳感器產(chǎn)生的區(qū)間量測。表1為10個(gè)目標(biāo)的信息。包括目標(biāo)的初始位置信息、速度信息、出生時(shí)刻和死亡時(shí)刻。

圖2 目標(biāo)真實(shí)位置和區(qū)間量測Fig.2 True targets location and interval measurements

表1 目標(biāo)信息Table1 Information of targets

圖3為單次蒙特卡洛實(shí)驗(yàn)下，MS-BOX-PHD濾波器在傳感器檢測概率都為0.65時(shí)對目標(biāo)的跟蹤效果圖，由圖3可知所提MS-BOX-PHD濾波器在檢測概率較低時(shí)也能夠有效地對多目標(biāo)狀態(tài)進(jìn)行跟蹤。

圖4為100次蒙特卡洛實(shí)驗(yàn)下，MS-BOX-PHD濾波器和Single-BOX-PHD濾波器在不同檢測概率下對目標(biāo)的勢估計(jì)圖，由圖 4可以看出 Single- BOX-PHD濾波在檢測概率逐漸減小時(shí)，對目標(biāo)個(gè)數(shù)的估計(jì)也隨之減少。相對于Single-BOX-PHD濾波器，本文所提MS-BOX-PHD濾波器雖然對目標(biāo)的個(gè)數(shù)估計(jì)也會受到檢測概率的影響，但受影響幅度明顯較小，在檢測概率為0.95、0.85、0.75、0.65時(shí)都能有效地對多目標(biāo)的個(gè)數(shù)進(jìn)行估計(jì)。

圖3 在x軸和y軸目標(biāo)狀態(tài)估計(jì)Fig.3 Targets state estimation in x-axis and y-axis

圖5為100次蒙特卡洛實(shí)驗(yàn)下，MS-BOX-PHD濾波和Single-BOX-PHD濾波在不同檢測概率下對目標(biāo)狀態(tài)估計(jì)的OSPA 距離。

圖5(b)為位置OSPA 距離，由圖5(b)可以看出對于位置估計(jì) MS-BOX-PHD濾波器與 Single- BOX-PHD濾波器相差不大，Single-BOX-PHD濾波器位置OSPA 距離普遍稍小是因?yàn)殡S著檢測概率的減小，目標(biāo)個(gè)數(shù)的估計(jì)會越來越少，當(dāng)少估計(jì)目標(biāo)個(gè)數(shù)時(shí)候，OSPA 距離只會統(tǒng)計(jì)估計(jì)到的目標(biāo)在位置上的誤差，所以隨著檢測概率的減小，在位置上的OSPA距離卻越來越小。對于MS-BOX-PHD濾波器，能夠在檢測概率較小時(shí)對目標(biāo)個(gè)數(shù)進(jìn)行正確的估計(jì)，但在位置估計(jì)上會統(tǒng)計(jì)所有估計(jì)到的目標(biāo)的位置OSPA距離，所以普遍稍大。且檢測概率越大，對位置的估計(jì)會越精確；圖5(c)為勢估計(jì)OSPA 距離，由圖5(c)可以看出在對目標(biāo)個(gè)數(shù)估計(jì)上Single-BOX-PHD濾波器明顯會受到檢測概率的影響，檢測概率越低，對目標(biāo)個(gè)數(shù)的估計(jì)誤差也越大；圖5(a)為綜合考慮目標(biāo)位置估計(jì)和個(gè)數(shù)估計(jì)的OSPA 距離，由圖5(a)可以看出MS-BOX-PHD濾波器對多目標(biāo)估計(jì)的性能會受到檢測概率的影響，但所受影響相對于Single-BOX-PHD濾波器受檢測概率的影響要小得多，且只有在目標(biāo)新生或者死亡時(shí)才會出現(xiàn)較大的估計(jì)誤差，如第1、13、15、24、28、41、45時(shí)刻都有目標(biāo)新生。說明MS-BOX-PHD濾波器在檢測概率較低時(shí)，能夠有效地估計(jì)多目標(biāo)的狀態(tài)和個(gè)數(shù)。

圖4 不同檢測概率下勢估計(jì)（100MC）Fig.4 Cardinality estimation under different detection probabilities (100MC)

4.2 場景2

設(shè)置傳感器1和傳感器2的觀測角度范圍都為θ∈[0 rad,π rad]，距離觀測范圍都為r∈[37.5 m,1000 m]，雜波率為λ1＝λ2＝2；場景2 中共有2個(gè)目標(biāo)，兩個(gè)目標(biāo)的航跡都在傳感器1和傳感器2的觀測區(qū)域內(nèi)。目標(biāo)1的初始狀態(tài)為[－800 m;16 m/s;100 m;7 m/s]，存活時(shí)間為1 s→100 s，目標(biāo)2的初始狀態(tài)為[800 m;－16 m/s;100 m;7 m/s]，存活時(shí)間為30 s→90 s。試驗(yàn)中所有目標(biāo)存活并進(jìn)行狀態(tài)轉(zhuǎn)移的概率為pSur＝0.98，每個(gè)目標(biāo)被傳感器檢測到的概率都是pD＝0.65。

圖6為100次蒙特卡洛實(shí)驗(yàn)下，MS-BOX-PHD濾波器和粒子數(shù)分別為3000個(gè)、4000個(gè)、4500個(gè)、5000個(gè)、8000個(gè)時(shí)，IM-PHD-PF 濾波器對目標(biāo)勢估計(jì)對比圖。

由圖6可以看出IM-PHD-PF 濾波器隨著粒子數(shù)的增多，對勢的估計(jì)也會越來越準(zhǔn)確。但在粒子數(shù)為3000個(gè)、4000個(gè)、4500個(gè)、5000個(gè)時(shí)都沒有箱粒子數(shù)為8個(gè)時(shí)MS-BOX-PHD濾波器對目標(biāo)勢估計(jì)準(zhǔn)確，而在IM-PHD-PF 濾波器粒子數(shù)提高到8000個(gè)時(shí)，IM-PHD-PF 濾波器對目標(biāo)的估計(jì)個(gè)數(shù)已經(jīng)接近于真實(shí)目標(biāo)個(gè)數(shù)。

圖7為100次蒙特卡洛實(shí)驗(yàn)下，MS-BOX-PHD濾波器和粒子數(shù)分別為3000個(gè)、4000個(gè)、4500個(gè)、5000個(gè)、8000個(gè)時(shí)，IM-PHD-PF 濾波器對多目標(biāo)狀態(tài)估計(jì)的OSPA 距離。

圖5 不同檢測概率下OSPA 距離估計(jì)（100MC）Fig.5 OSPA distance estimation under different detection probabilities (100MC)

圖6 MS-BOX-PHD濾波器和不同粒子數(shù)下IM-PHD-PF 濾波器對目標(biāo)勢估計(jì)（100MC）Fig.6 Cardinality estimation by utilizing MS-BOX-PHD filter and IM-PHD-PF filter with different number of particles,respectively (100MC)

圖7 MS-BOX-PHD濾波器和不同粒子數(shù)下IM-PHD-PF 濾波器對目標(biāo)狀態(tài)估計(jì)的OSPA 距離（100MC）Fig.7 OSPA distance estimation by utilizing MS-BOX-PHD filter and IM-PHD-PF filter with different number of particles,respectively (100MC)

其中圖7(c)為勢估計(jì)OSPA 距離，圖7(b)為位置估計(jì)OSPA 距離，圖7(b)中，IM-PHD-PF 濾波器在位置估計(jì)OSPA 距離要比MS-BOX-PHD濾波器位置估計(jì)OSPA 距離小，因?yàn)橄淞Ｗ邮且粋€(gè)區(qū)間，最終是使用箱粒子中心位置來對目標(biāo)位置進(jìn)行估計(jì)的，所以偏差要大于點(diǎn)粒子下IM-PHD-PF 濾波器對目標(biāo)位置的估計(jì)；圖7(c)則說明，粒子數(shù)越少IM-PHD-PF濾波器在對目標(biāo)進(jìn)行跟蹤時(shí)，越容易丟失目標(biāo)；圖7(a)綜合考慮目標(biāo)的位置估計(jì)誤差和勢估計(jì)誤差，圖7(a)表明MS-BOX-PHD濾波器僅使用8個(gè)箱粒子對多目標(biāo)狀態(tài)估計(jì)的性能，要比IM-PHD-PF 濾波器使用3000、4000、4500、5000個(gè)粒子的情況下要穩(wěn)定得多。

圖7(a)中MS-BOX-PHD濾波器的OSPA 距離均值為28.4835，運(yùn)行時(shí)間為14.97 s。

表2為100次蒙特卡洛實(shí)驗(yàn)下IM-PHD-PF 濾波器使用不同粒子數(shù)下的運(yùn)行時(shí)間以及OSPA 距離均值，由表2可以看出隨著粒子數(shù)目的增多，區(qū)間量測下多傳感器標(biāo)準(zhǔn)PHD 粒子濾波器對多目標(biāo)狀態(tài)估計(jì)性能越來越好，但運(yùn)行時(shí)間花費(fèi)也越來越大。

綜合表2以及圖6和圖7可知，在IM-PHD-PF濾波器使用4500個(gè)粒子時(shí)運(yùn)行時(shí)間為24.37 s，OSPA 距離均值為30.14；MS-BOX-PHD濾波器使用8個(gè)箱粒子，運(yùn)行時(shí)間為14.97 s，OSPA距離均值為28.4835。說明IM-PHD-PF濾波器和MS-BOX-PHD濾波器在對多目標(biāo)狀態(tài)估計(jì)性能相近時(shí)候，MS-BOX-PHD濾波器要比IM-PHD-PF 濾波器在計(jì)算效率上提升了38.57%。

表2 IM-PHD-PF 濾波器運(yùn)行時(shí)間（100MC）Table2 Time cost for IM-PHD-PF filter

5 結(jié)論

本文利用多傳感器技術(shù)，結(jié)合BOX PF 以及PHD濾波器給出了MS-BOX-PHD算法的實(shí)現(xiàn)。并通過數(shù)值實(shí)驗(yàn)，驗(yàn)證了對于低可觀測目標(biāo)MS-BOX-PHD算法的優(yōu)越跟蹤性能，并且驗(yàn)證了與區(qū)間量測下多傳感器標(biāo)準(zhǔn)PHD 粒子濾波器相比較，當(dāng)跟蹤性能接近時(shí)，MS-BOX-PHD濾波器在計(jì)算效率上提升了38.57%.