多傳感器箱粒子PHD濾波多目標跟蹤算法

蔡如華，楊 標，吳孫勇,2

（1.桂林電子科技大學 數學與計算科學學院，廣西 桂林 541004；2.廣西密碼學與信息安全重點實驗室，廣西 桂林 541004）

0 引言

多目標跟蹤（multi-target tracking,MTT）算法的有效性和實時性一直以來是國內外學者關注的熱點。針對多目標跟蹤問題，Mahler 提出了隨機有限集[1]（random finite set,RFS）理論，將多目標狀態集和多目標觀測集建模為隨機有限集的形式，有效地解決了基于多假設跟蹤[2]和聯合概率數據關聯[3]在多目標跟蹤過程中產生的組合爆炸問題，并提出了基于隨機有限集的概率假設密度[4]（probability hypothesis density filter,PHD）濾波器，PHD濾波器能夠對目標狀態和數目進行有效的估計。

傳統的粒子濾波器是以點粒子的形式來模擬目標狀態。在進行蒙特卡洛實驗時，需要大量的點粒子來對目標進行捕獲和持續跟蹤，所以會產生較大的計算量，為減輕計算量，文獻[5]提出了基于區間分析[6-7]的箱粒子濾波算法，之后在文獻[8]中結合PHD濾波器實現了箱粒子PHD濾波器，從而減輕了計算量。2015年國內學者宋驪平[9]將箱粒子PHD濾波用于擴展目標跟蹤，之后在文獻[10]中使用箱粒子PHD濾波對群目標進行了有效的跟蹤。文獻[11]提出了未知雜波狀態下基于箱粒子濾波的PHD算法。

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

本文則是針對低可觀測目標難以跟蹤以及點粒子濾波產生較大計算量的問題，利用多傳感器技術在低可觀測目標檢測上的優勢以及箱粒子濾波能夠提升計算效率的優勢，提出MS-BOX-PHD算法。并通過仿真實驗驗證了MS-BOX-PHD算法對多目標狀態以及數目估計的穩定有效性，以及MS-BOX-PHD算法相對于區間量測下多傳感器標準PHD 粒子濾波（multi-sensor standard probability hypothesis density particle filter with interval measurement,IM-PHD-PF）算法在計算效率上的優勢。

1 系統模型

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

1.1 多目標狀態轉移模型

考慮系統：

式中：k為時間指標；f為非線性的狀態轉移函數；wk表示k時刻的過程噪聲。對于k時刻多目標狀態可以描述為隨機有限集：

式中：N表示k時刻的目標個數。xk,i表示k時刻的第i個目標，可能為新生目標，也可能為k－1時刻存活并轉移后的目標。

1.2 多目標多傳感器量測模型

對于k時刻的目標xk,i，其可能被傳感器檢測到并產生量測值zk,i，也可能未被傳感器檢測到，沒有產生量測值，同樣在k時刻傳感器也可能產生虛假量測。

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

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

2 收縮算法

收縮算法以區間分析[6-7]為基礎，通過區間收縮，保證更新之后的箱粒子擁有一個合適的區間大小，并利用收縮前后的箱粒子區間大小來求解似然函數，從而對箱粒子的權重進行更新。

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

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

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

3 MS-BOX-PHD濾波

假設在k－1時刻的后驗PHD由一組服從均勻分布的箱粒子集描述為：

式中：[xk－1,i]為箱粒子支撐集；Lk－1為k－1時刻箱粒子的個數。

3.1 MS-BOX-PHD濾波預測

若γk(x)表示新生目標貢獻強度，βSur,k(xk)表示k－1時刻到k時刻存活下來的目標貢獻強度，表示k－1時刻目標x空間分布概率密度，表示目標x由k－1時刻到k時刻的轉移密度，則k時刻預測PHD可以由k時刻的預測箱粒子參數集表示為：

新生箱粒子根據k－1時刻量測產生[5]，在量測上服從均勻分布，為新生箱粒子集合，新生目標貢獻強度γk(x)可由其表示為：

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

3.2 MS-BOX-PHD濾波更新

3.2.1 量測融合

量測融合分兩步進行：

Step 1：輸入k時刻S個傳感器產生的量測集將S個傳感器產生的區間量測轉換到同一坐標系下，步驟如下：

輸入k時刻區間量測和傳感器坐標：

對S個傳感器循環：

對k時刻第s個傳感器產生所有量測循環：

Step 2：對S個傳感器產生的量測集進行融合。若各傳感器區間量測間有交集，則保留相交的區間量測的交集以及其他互不相交的區間量測值作為當前時刻區間量測集。

以兩個傳感器為例，對k時刻S個傳感器產生的區間量測進行分析，共分為3種情況（如圖1(a)、1(b)、1(c)）。

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

假設k時刻傳感器1 產生量測集經過坐標轉換后記為傳感器2 產生量測集經過坐標轉換后為記為

顯然因為量測融合同時考慮到多個傳感器的區間量測，包含目標真實信息的可能性也會更大。若其中一個傳感器無法對目標進行有效檢測時，其他傳感器也能夠對其檢測并產生量測。若多個傳感器同時檢測到目標，則通過情形3可知，通過區間量測相交運算則能夠對目標量測進行壓縮，保證較小的量測區間即可包含目標信息。

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

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

3.2.2 濾波更新

輸入k時刻S個傳感器融合后的區間量測集Zk,對預測得到的箱粒子集進行更新。

4 數值實驗

實驗計算機系統為Windows 10 專業版，64位操作系統。處理器為Intel(R) Core(TM) i5-6500 CPU @3.20 GHz，運行內存為8.00 GB。

實驗中設置所有目標在觀測區域中服從勻速直線（constant velocity,CV）運動模型：

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

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

1）當m≤n時：

2）當m≥n時：

3）當m＝n＝0時：

式中：c為目標個數誤差懲罰參數；p為階數，用來懲罰目標位置估計誤差。本文實驗中c＝100，p＝1。

4.1 場景1

設置傳感器1位于坐標原點[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個目標隨機出現和消失在所有傳感器的觀測區域內。試驗中所有目標存活并進行狀態轉移的概率為pSur＝0.98。

圖2為單次蒙特卡洛實驗下，傳感器檢測概率為0.65時的目標真實位置以及傳感器產生的區間量測。表1為10個目標的信息。包括目標的初始位置信息、速度信息、出生時刻和死亡時刻。

圖2 目標真實位置和區間量測Fig.2 True targets location and interval measurements

表1 目標信息Table1 Information of targets

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

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

圖3 在x軸和y軸目標狀態估計Fig.3 Targets state estimation in x-axis and y-axis

圖5為100次蒙特卡洛實驗下，MS-BOX-PHD濾波和Single-BOX-PHD濾波在不同檢測概率下對目標狀態估計的OSPA 距離。

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

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

4.2 場景2

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

圖6為100次蒙特卡洛實驗下，MS-BOX-PHD濾波器和粒子數分別為3000個、4000個、4500個、5000個、8000個時，IM-PHD-PF 濾波器對目標勢估計對比圖。

由圖6可以看出IM-PHD-PF 濾波器隨著粒子數的增多，對勢的估計也會越來越準確。但在粒子數為3000個、4000個、4500個、5000個時都沒有箱粒子數為8個時MS-BOX-PHD濾波器對目標勢估計準確，而在IM-PHD-PF 濾波器粒子數提高到8000個時，IM-PHD-PF 濾波器對目標的估計個數已經接近于真實目標個數。

圖7為100次蒙特卡洛實驗下，MS-BOX-PHD濾波器和粒子數分別為3000個、4000個、4500個、5000個、8000個時，IM-PHD-PF 濾波器對多目標狀態估計的OSPA 距離。

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

圖6 MS-BOX-PHD濾波器和不同粒子數下IM-PHD-PF 濾波器對目標勢估計（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濾波器和不同粒子數下IM-PHD-PF 濾波器對目標狀態估計的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)為勢估計OSPA 距離，圖7(b)為位置估計OSPA 距離，圖7(b)中，IM-PHD-PF 濾波器在位置估計OSPA 距離要比MS-BOX-PHD濾波器位置估計OSPA 距離小，因為箱粒子是一個區間，最終是使用箱粒子中心位置來對目標位置進行估計的，所以偏差要大于點粒子下IM-PHD-PF 濾波器對目標位置的估計；圖7(c)則說明，粒子數越少IM-PHD-PF濾波器在對目標進行跟蹤時，越容易丟失目標；圖7(a)綜合考慮目標的位置估計誤差和勢估計誤差，圖7(a)表明MS-BOX-PHD濾波器僅使用8個箱粒子對多目標狀態估計的性能，要比IM-PHD-PF 濾波器使用3000、4000、4500、5000個粒子的情況下要穩定得多。

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

表2為100次蒙特卡洛實驗下IM-PHD-PF 濾波器使用不同粒子數下的運行時間以及OSPA 距離均值，由表2可以看出隨著粒子數目的增多，區間量測下多傳感器標準PHD 粒子濾波器對多目標狀態估計性能越來越好，但運行時間花費也越來越大。

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

表2 IM-PHD-PF 濾波器運行時間（100MC）Table2 Time cost for IM-PHD-PF filter

5 結論

本文利用多傳感器技術，結合BOX PF 以及PHD濾波器給出了MS-BOX-PHD算法的實現。并通過數值實驗，驗證了對于低可觀測目標MS-BOX-PHD算法的優越跟蹤性能，并且驗證了與區間量測下多傳感器標準PHD 粒子濾波器相比較，當跟蹤性能接近時，MS-BOX-PHD濾波器在計算效率上提升了38.57%.