劃分交互式箱粒子概率假設(shè)密度濾波法

王 海，楊小軍

(長(zhǎng)安大學(xué) 信息工程學(xué)院，陜西 西安 710064)

0 引 言

多目標(biāo)跟蹤是民用自動(dòng)駕駛、軍用雷達(dá)探測(cè)等方面的關(guān)鍵技術(shù)，其利用傳感器接收到的測(cè)量信息，通過(guò)預(yù)測(cè)更新依次確定多個(gè)目標(biāo)的數(shù)量和狀態(tài)的過(guò)程。傳統(tǒng)的多目標(biāo)跟蹤基于“小目標(biāo)”假設(shè)[1]來(lái)處理復(fù)雜問(wèn)題，常用方法有：聯(lián)合概率數(shù)據(jù)關(guān)聯(lián)算法(joint probabilistic data association，JPDA)[2]、多假設(shè)跟蹤算法(multiple hypotheses tracking，MHT)[3]、概率多重假設(shè)跟蹤(probabilistic multiple hypothesis tracking，PMHT)[4]、隨機(jī)有限集(random finite set，RFS)[5]等。RFS將目標(biāo)和量測(cè)建立為隨機(jī)集集合來(lái)處理問(wèn)題，文獻(xiàn)[6]提出概率假設(shè)密度(probability hypothesis density，PHD)濾波器，降低了多目標(biāo)貝葉斯濾波器的組合計(jì)算困難問(wèn)題。進(jìn)一步提出了PHD濾波器的混合高斯(Gaussian mixture，GM)[7]實(shí)現(xiàn)，序貫蒙特卡羅(sequence Monte Carlo，SMC)方法[8-9]等。

在PHD濾波中，采用粒子對(duì)后驗(yàn)狀態(tài)進(jìn)行擬合，可以解決非線性、非高斯條件下的多目標(biāo)跟蹤問(wèn)題，但是為了達(dá)到良好的擬合效果，往往需要大量的粒子，這增加了計(jì)算量和時(shí)間。文獻(xiàn)[10-11]中，A. Gning等利用區(qū)間包含粒子濾波，提出箱粒子濾波算法，箱粒子可以有效減少粒子數(shù)，提高運(yùn)算效率。文獻(xiàn)[12]提出了箱粒子與PHD結(jié)合的(Box-PHD)方法，驗(yàn)證了箱粒子在計(jì)算效率上的優(yōu)勢(shì)。進(jìn)一步，文獻(xiàn)[13]運(yùn)用箱粒子處理機(jī)動(dòng)目標(biāo)跟蹤問(wèn)題，文獻(xiàn)[14]利用箱粒子劃分技術(shù)提升多目標(biāo)狀態(tài)的估計(jì)性能。文獻(xiàn)[15]將交互式多模[16-17]的思想與箱粒子結(jié)合提出交互多模型箱粒子濾波(interacting multiple model box particle filter，IMM-BPF)來(lái)處理多機(jī)動(dòng)目標(biāo)。對(duì)于單傳感器的多機(jī)動(dòng)目標(biāo)跟蹤問(wèn)題，該文在上述研究的基礎(chǔ)上，提出了劃分交互式概率假設(shè)密度濾波PIMM-Box-PHD算法，通過(guò)仿真實(shí)驗(yàn)與IMM-Box-PHD算法進(jìn)行比較，驗(yàn)證了該濾波算法在跟蹤橢圓形多機(jī)動(dòng)目標(biāo)的良好性能。

1 系統(tǒng)模型

根據(jù)RFS理論，在多目標(biāo)系統(tǒng)中，將所有測(cè)量時(shí)刻，所有目標(biāo)的狀態(tài)和量測(cè)信息分別表示為兩個(gè)集合。由于狀態(tài)和量測(cè)信息的變化，可以用隨機(jī)有限子集來(lái)表示多目標(biāo)狀態(tài)集Xk和量測(cè)集Zk。

Xk={xk,1,xk,2,…,xk,Nk}∈F(χ)

(1)

Nk表示k時(shí)刻目標(biāo)數(shù)目，集合元素xk,i(i=1～Nk)表示單個(gè)目標(biāo)i在單目標(biāo)狀態(tài)空間F(χ)中的狀態(tài)矢量。

Zk={zk,1,zk,2,…,zk,Mk}∈F(ψ)

(2)

Mk表示k時(shí)刻量測(cè)數(shù)目，集合元素zk,j(j=1～Mk)表示量測(cè)j在單目標(biāo)量測(cè)空間F(ψ)中的量測(cè)矢量，當(dāng)然在量測(cè)信息中包含雜波量測(cè)。該文采用如下系統(tǒng)：

xk=fk-1(x)+uk-1

(3)

zk=hk(x)+vk

(4)

式(3)、式(4)分別表示k時(shí)刻的狀態(tài)方程和量測(cè)方程(線性或非線性)，過(guò)程噪聲uk-1和量測(cè)噪聲vk均為加性噪聲。在經(jīng)典的貝葉斯框架下，將濾波過(guò)程分為預(yù)測(cè)和更新兩個(gè)部分，預(yù)測(cè)是根據(jù)馬爾可夫轉(zhuǎn)移矩陣，通過(guò)預(yù)測(cè)對(duì)象狀態(tài)來(lái)評(píng)估后驗(yàn)密度，而更新是利用當(dāng)前時(shí)刻的量測(cè)信息修正上一時(shí)刻的預(yù)測(cè)結(jié)果。

為了減少多目標(biāo)貝葉斯遞推公式的計(jì)算復(fù)雜度，Mahler提出了一種次最優(yōu)的解決方案─PHD濾波器。PHD濾波器通過(guò)傳播后驗(yàn)強(qiáng)度函數(shù)完成目標(biāo)數(shù)的估計(jì)，然后再根據(jù)預(yù)測(cè)目標(biāo)數(shù)加權(quán)融合估計(jì)出每個(gè)目標(biāo)的狀態(tài)，將雜波和不確定關(guān)聯(lián)中估計(jì)多個(gè)目標(biāo)的問(wèn)題被放到貝葉斯濾波框架中。PHD過(guò)濾器由兩個(gè)步驟組成：預(yù)測(cè)和更新，根據(jù)文獻(xiàn)[12]：

k時(shí)刻多目標(biāo)后驗(yàn)PHD預(yù)測(cè)：

其中，b(xk)表示新生目標(biāo)強(qiáng)度，b(xk|xk-1)表示與前一狀態(tài)有關(guān)的衍生概率，s(xk-1)表示前一時(shí)刻到下一時(shí)刻目標(biāo)存活概率，f(xk|xk-1)表示目標(biāo)轉(zhuǎn)移概率。

k時(shí)刻多目標(biāo)后驗(yàn)PHD更新：

pk|k(xk)=[1-pD(xk)]pk|k-1(xk)+

(6)

其中，pD表示檢測(cè)概率，g(z|xk)表示測(cè)量似然，λ表示雜波均值，c(z)表示雜波分布先驗(yàn)概率。上式分為兩部分，第一部分為未檢測(cè)到目標(biāo)對(duì)更新PHD的影響，后一部分為檢測(cè)到的目標(biāo)對(duì)其更新的貢獻(xiàn)。

2 箱粒子劃分

箱粒子濾波(Box-PF)在文獻(xiàn)[10]中被首次提出，是粒子濾波與區(qū)間分析相結(jié)合的一種濾波方法。在二維空間中利用區(qū)間形式畫出一定大小范圍的“箱”來(lái)表示點(diǎn)粒子，用箱粒子代替?zhèn)鹘y(tǒng)的點(diǎn)粒子來(lái)進(jìn)行濾波的預(yù)測(cè)和更新，稱為箱粒子濾波。相較于傳統(tǒng)的粒子濾波需要大量粒子來(lái)擬合后驗(yàn)概率密度以得到良好的跟蹤效果，使用箱粒子可以有效提高計(jì)算效率，降低運(yùn)算成本。由于區(qū)間測(cè)度的不確定性，在區(qū)間量測(cè)噪聲大雜波率高等復(fù)雜場(chǎng)景中，傳感器獲得的量測(cè)信息包含很多虛假量測(cè)，根據(jù)前一時(shí)刻量測(cè)信息產(chǎn)生的新生箱粒子偏大，用偏大且包含過(guò)多虛假量測(cè)信息的箱粒子去修正預(yù)測(cè)信息，會(huì)導(dǎo)致估計(jì)目標(biāo)狀態(tài)有偏的情況[18-19]。

針對(duì)該問(wèn)題，為了提高估計(jì)精度，提出箱粒子劃分的思想：該方法將預(yù)測(cè)后得到的持續(xù)箱粒子和新生箱粒子，在速度維和位置維進(jìn)行箱的劃分，劃分之后的箱粒子變小，然后利用劃分后的小箱粒子集進(jìn)行量測(cè)更新，通過(guò)預(yù)測(cè)步的劃分，去除冗余信息，找到包含更多量測(cè)信息的小箱粒子，就能在更新步更好地利用區(qū)間量測(cè)信息，達(dá)到提高目標(biāo)估計(jì)精度的效果。

3 劃分交互式概率假設(shè)密度濾波算法

本節(jié)將箱劃分技術(shù)和IMM算法與BOX-PHD結(jié)合，提出處理多機(jī)動(dòng)目標(biāo)的PIMM-BOX-PHD算法。

使用IMM算法，首先需要確定目標(biāo)的運(yùn)動(dòng)模型。理論上，建立的運(yùn)動(dòng)模型需要覆蓋目標(biāo)的所有運(yùn)動(dòng)模式，對(duì)于各模型，產(chǎn)生一組箱粒子，這組箱粒子經(jīng)過(guò)輸入交互、箱粒子濾波后進(jìn)行重采樣，最后進(jìn)行輸出交互。如此不斷循環(huán)遞推傳播更新這些箱粒子以完成對(duì)目標(biāo)狀態(tài)的估計(jì)。

首先，設(shè)馬爾可夫模型轉(zhuǎn)移概率矩陣為：π={pij,i,j=1,2,…,Nr}，各模型初始概率為：U={μi,k,i=1,2,…,Nr}，Nr為模型數(shù)。該算法具體流程如下：

3.1 初始化箱粒子產(chǎn)生

3.2 交互式預(yù)測(cè)

計(jì)算k-1時(shí)刻模型i到j(luò)的混合概率:

(7)

得到模型j的交互箱粒子輸入：

(8)

目標(biāo)狀態(tài)預(yù)測(cè)如下：

(9)

根據(jù)式(5)中的存活概率進(jìn)行權(quán)值更新：

(10)

3.3 劃分箱粒子

在交互式預(yù)測(cè)之后，對(duì)預(yù)測(cè)的箱粒子進(jìn)行劃分，可表示為：

3.4 更 新

(11)

其中：

(12)

λk|k-1([zm])=λc([zm])+

(13)

其中，[hcp]表示量測(cè)函數(shù)的包含函數(shù)，pD表示檢測(cè)概率，[gk|k(·)]表示箱粒子量測(cè)似然，λk表示雜波均值數(shù)，c(·)表示雜波分布先驗(yàn)概率密度。因?yàn)榱繙y(cè)是區(qū)間的形式，所以似然函數(shù)就不能采用傳統(tǒng)的目標(biāo)集中式融合的方法，此處的似然函數(shù)通過(guò)量測(cè)箱粒子和預(yù)測(cè)箱粒子的重疊面積比，再采用收縮算法得到。

3.5 約束箱粒子

3.6 重采樣

3.7 模型概率更新

模型似然：

(14)

更新后的模型概率為：

(15)

3.8 目標(biāo)狀態(tài)估計(jì)

(16)

最后將更新后的概率模型和提取的目標(biāo)狀態(tài)混合輸出。

4 仿真實(shí)驗(yàn)

4.1 仿真儀器

仿真場(chǎng)景中使用軟、硬件設(shè)施如下：

操作系統(tǒng)：Win10 64位操作系統(tǒng)；運(yùn)行軟件：MATLAB R2012a；計(jì)算機(jī)品牌：Lenovo；處理器：Intel(R) Core(TM) i5-4258U CPU @ 2.40 GHz 2.10 GHz；RAM：8.00 GB。

4.2 仿真場(chǎng)景設(shè)置

假設(shè)一個(gè)雷達(dá)扇面觀測(cè)場(chǎng)景，傳感器雷達(dá)位于坐標(biāo)[0,0]處，多個(gè)橢圓機(jī)動(dòng)目標(biāo)在[-200 m,200 m]×[0 m,200 m]的二維觀測(cè)區(qū)域內(nèi)運(yùn)動(dòng)，運(yùn)動(dòng)模型由一個(gè)勻速直線運(yùn)動(dòng)模型、一個(gè)協(xié)同右轉(zhuǎn)運(yùn)動(dòng)模型和一個(gè)協(xié)同左轉(zhuǎn)運(yùn)動(dòng)模型構(gòu)成。

勻速直線(CV)運(yùn)動(dòng)模型：

協(xié)同轉(zhuǎn)彎(CT)運(yùn)動(dòng)模型：

噪聲標(biāo)準(zhǔn)差為σv=0.02 m/s，三個(gè)模型之間的模型轉(zhuǎn)換概率矩陣為:

考慮到傳感器存在系統(tǒng)誤差，會(huì)造成一定的觀測(cè)偏差，把區(qū)間量測(cè)定義為：

在監(jiān)控區(qū)域內(nèi)檢測(cè)時(shí)長(zhǎng)為100 s，目標(biāo)存活概率ps=0.99，目標(biāo)檢測(cè)概率pD=0.98，均勻雜波的泊松平均率λ=5，每個(gè)時(shí)刻產(chǎn)生新生箱粒子1個(gè)，存活箱粒子數(shù)8個(gè)。箱粒子擴(kuò)展范圍為10 m。

4.3 實(shí)驗(yàn)結(jié)果分析

假設(shè)雷達(dá)扇面中有三個(gè)機(jī)動(dòng)目標(biāo)，目標(biāo)的初始狀態(tài)及出生死亡時(shí)刻如表1所示。

表1 目標(biāo)初始狀態(tài)和時(shí)間

在雷達(dá)扇面檢測(cè)區(qū)域內(nèi)，目標(biāo)1在5 s～20 s做勻速直線運(yùn)動(dòng)，21 s～65 s協(xié)同右轉(zhuǎn)運(yùn)動(dòng)，66 s～85 s協(xié)同左轉(zhuǎn)運(yùn)動(dòng)；目標(biāo)2在15 s～35 s做勻速直線運(yùn)動(dòng)，36 s～65 s協(xié)同左轉(zhuǎn)運(yùn)動(dòng)，66 s～95 s協(xié)同右轉(zhuǎn)運(yùn)動(dòng)；目標(biāo)3在20 s～35 s做勻速直線運(yùn)動(dòng)，36 s～66 s協(xié)同左轉(zhuǎn)運(yùn)動(dòng)，67 s～86 s協(xié)同右轉(zhuǎn)運(yùn)動(dòng)。

圖1所示為三個(gè)橢圓形機(jī)動(dòng)目標(biāo)的運(yùn)動(dòng)狀態(tài)軌跡及其在單次蒙特卡洛實(shí)驗(yàn)下的目標(biāo)區(qū)間量測(cè)圖。

圖1 區(qū)間量測(cè)分布

該文采用最優(yōu)子模型分配距離[22](optimal subpattern assignment，OSPA)對(duì)算法的估計(jì)精度進(jìn)行評(píng)估。c為目標(biāo)數(shù)目估計(jì)懲罰參數(shù)，c值越大，在OSPA距離中目標(biāo)數(shù)目估計(jì)誤差所占比重越大，p為階數(shù)，也稱目標(biāo)位置懲罰參數(shù)，p值越大，在OSPA距離中對(duì)目標(biāo)位置估計(jì)誤差越敏感，位置估計(jì)誤差所占比重越大。本實(shí)驗(yàn)中p=1，c=30。圖2所示為PIMM-Box-PHD算法單次蒙特卡洛實(shí)驗(yàn)效果跟蹤圖，拆分為x和y兩個(gè)坐標(biāo)，可以看出該方法能有效地對(duì)多機(jī)動(dòng)目標(biāo)的狀態(tài)和運(yùn)動(dòng)軌跡進(jìn)行估計(jì)。

圖2 目標(biāo)狀態(tài)估計(jì)跟蹤效果

表2 三種算法單次運(yùn)行時(shí)間比較

從表2可看出，BOX-PHD方法平均運(yùn)行時(shí)間最短，在加入IMM模型匹配之后運(yùn)行時(shí)間變?yōu)樵瓉?lái)的6倍，同時(shí)可以看出在加入箱粒子劃分技術(shù)之后運(yùn)行時(shí)間比未劃分前增加1/6，由于PIMM-Box-PHD算法采用劃分技術(shù)使得箱粒子個(gè)數(shù)增多，計(jì)算效率略有下降。

進(jìn)行100次蒙特卡洛實(shí)驗(yàn)，圖3和圖4為IMM-Box-PHD與PIMM-Box-PHD兩種算法對(duì)多目標(biāo)狀態(tài)估計(jì)的OSPA距離對(duì)比圖和OSPA位置分量對(duì)比圖。

圖3 OSPA距離估計(jì)

圖4 OSPA位置估計(jì)

圖3所示為綜合考慮目標(biāo)個(gè)數(shù)估計(jì)和目標(biāo)位置估計(jì)的OSPA距離，從圖中可以直觀看出PIMM-Box-PHD算法在所有采樣時(shí)刻，其OPSA距離都明顯小于IMM-Box-PHD，驗(yàn)證了PIMM-Box-PHD在目標(biāo)估計(jì)方面的優(yōu)越性能。進(jìn)一步，拆分出對(duì)多目標(biāo)的位置估計(jì)分量，圖4所示為OSPA位置分量，可以看出在前80 s采樣時(shí)刻，PIMM-Box-PHD算法的OSPA距離明顯小于IMM-Box-PHD算法，前者對(duì)位置的估計(jì)更準(zhǔn)確更穩(wěn)定。仿真結(jié)果驗(yàn)證了將箱粒子劃分為更小的箱粒子集，有助于目標(biāo)的精確定位。

圖5 多目標(biāo)的勢(shì)估計(jì)

圖5所示為多機(jī)動(dòng)目標(biāo)的勢(shì)估計(jì)，可以進(jìn)一步看出，兩種算法的勢(shì)估計(jì)在采樣時(shí)間內(nèi)與實(shí)際目標(biāo)數(shù)進(jìn)行比較，有新目標(biāo)出生和原目標(biāo)死亡時(shí)，對(duì)目標(biāo)的勢(shì)估計(jì)存在一定的延時(shí)和誤差，不過(guò)很快修正，能夠準(zhǔn)確檢測(cè)到目標(biāo)個(gè)數(shù)的變化，兩種方法都能對(duì)目標(biāo)的個(gè)數(shù)進(jìn)行穩(wěn)定的估計(jì)，在目標(biāo)數(shù)目增加或減少時(shí)能夠迅速進(jìn)行調(diào)整，記錄目標(biāo)變化情況。PIMM-Box-PHD算法略有優(yōu)勢(shì)，但效果不明顯。然后使用均值將兩種算法的OSPA距離進(jìn)行量化比較，如表3所示。

表3 100次MC平均OSPA比較

通過(guò)表3可以看出在距離和位置估計(jì)方面，PIMM-Box-PHD算法的平均OSPA明顯小于IMM-Box-PHD，前者性能更優(yōu)，說(shuō)明劃分后的箱粒子能更好地利用量測(cè)信息，更準(zhǔn)確地估計(jì)目標(biāo)狀態(tài)和位置。在多目標(biāo)個(gè)數(shù)估計(jì)方面，PIMM-Box-PHD算法的性能并不明顯，但其平均OSPA略小，說(shuō)明箱劃分技術(shù)對(duì)目標(biāo)個(gè)數(shù)的估計(jì)精度略有提升。

5 結(jié)束語(yǔ)

將箱劃分技術(shù)與IMM算法加入到BOX-PHD濾波器中，利用交互式實(shí)現(xiàn)多個(gè)模型對(duì)目標(biāo)狀態(tài)進(jìn)行聯(lián)合描述，實(shí)現(xiàn)對(duì)橢圓多機(jī)動(dòng)目標(biāo)跟蹤。通過(guò)仿真實(shí)驗(yàn)對(duì)比分析，證明所提PIMM-Box-PHD算法在略微犧牲計(jì)算效率的情況下，能有效提高目標(biāo)的跟蹤性能，對(duì)目標(biāo)的定位更加精確，改進(jìn)目標(biāo)跟蹤過(guò)程中狀態(tài)中箱粒子冗余和估計(jì)有偏的情況，對(duì)最終多機(jī)動(dòng)目標(biāo)的狀態(tài)估計(jì)起到了很好的修正作用。