基于盒粒子濾波的多擴(kuò)展目標(biāo)伯努利濾波算法

趙小龍

(安徽工業(yè)經(jīng)濟(jì)職業(yè)技術(shù)學(xué)院計(jì)算機(jī)與藝術(shù)學(xué)院，安徽 合肥 230051)

0 引言

多目標(biāo)跟蹤是指通過對(duì)傳感器得到的量測(cè)數(shù)據(jù)進(jìn)行處理，以實(shí)現(xiàn)對(duì)目標(biāo)個(gè)數(shù)和狀態(tài)的持續(xù)估計(jì)。作為一項(xiàng)關(guān)鍵技術(shù)，在諸如機(jī)器人、監(jiān)控、自動(dòng)駕駛、自動(dòng)化和傳感器網(wǎng)絡(luò)中都有著廣泛的應(yīng)用。

傳統(tǒng)的目標(biāo)跟蹤均采用點(diǎn)目標(biāo)模型，并且在大量的實(shí)際應(yīng)用場景中，這種假設(shè)也是合理的。然而由于傳感器技術(shù)在近年來的快速發(fā)展，目標(biāo)占據(jù)著越來越多的分辨單元，如在移動(dòng)機(jī)器人和自動(dòng)駕駛中，點(diǎn)目標(biāo)的假設(shè)便難以適用。對(duì)此，我們將占據(jù)多個(gè)傳感器分辨單元的目標(biāo)稱為擴(kuò)展目標(biāo)[1-2]，這類跟蹤問題便稱為擴(kuò)展目標(biāo)跟蹤(EOT，Extended Object Tracking)。擴(kuò)展目標(biāo)具有不同空間分布的多個(gè)量測(cè)源，也稱作反射點(diǎn)，這些點(diǎn)會(huì)產(chǎn)生多個(gè)測(cè)量值。如何更好地對(duì)目標(biāo)進(jìn)行位置估計(jì)，需要建立當(dāng)前目標(biāo)和其測(cè)量值得關(guān)聯(lián)，當(dāng)目標(biāo)數(shù)較多時(shí)，這便成了一個(gè)高度復(fù)雜的問題。首先在量測(cè)建模方面，Gilholm等人[1]提出了非齊次泊松點(diǎn)過程(PPP)，將目標(biāo)數(shù)目建模為服從泊松分布模型，多個(gè)量測(cè)點(diǎn)分布在目標(biāo)周圍，這種模型具有數(shù)學(xué)上的方便，可以避免顯式求解。在隨機(jī)集理論(RFS)和PPP模型的基礎(chǔ)上，Mahler在貝葉斯濾波的框架下推導(dǎo)并提出了擴(kuò)展目標(biāo)概率假設(shè)密度濾波器(Extended-Target PHD，ET-PHD)[2]。Karl等人在此基礎(chǔ)上，將運(yùn)動(dòng)轉(zhuǎn)移模型假設(shè)是線性的，同時(shí)假設(shè)觀測(cè)噪聲服從高斯分布，從而可以得到算法的閉合解，便可在已有的卡爾曼濾波理論框架下求得濾波器的快速迭代方程，進(jìn)一步發(fā)展了概率假設(shè)密度濾波器在擴(kuò)展目標(biāo)跟蹤領(lǐng)域的應(yīng)用[3]。

隨著RFS理論在EOT問題上的引入，相關(guān)研究成果便層出不窮[4-6]?；赗FS的PHD方法被認(rèn)為是實(shí)現(xiàn)EOT問題的一種有效技術(shù)手段[7-9]，然而現(xiàn)有方法大多是基于線性高斯模型的，而實(shí)際中大部分情況都是非線性非高斯的。雖然將粒子濾波方法引入，提出的SMCPHD濾波器能夠很好地處理非線性問題，但是計(jì)算量也是算法應(yīng)用的一大瓶頸。近年來，有研究人員將區(qū)間分析技術(shù)和粒子濾波進(jìn)行結(jié)合，提出了一種被稱為盒粒子濾波(Box Particle Filter，BPF)的算法。與傳統(tǒng)的粒子濾波采用誤差統(tǒng)計(jì)模型不同，盒粒子濾波采用盒粒子和誤差界限模型對(duì)量測(cè)點(diǎn)進(jìn)行建模表示，經(jīng)過這樣處理后可以大幅降低所需要的粒子數(shù)，對(duì)粒子濾波固有的運(yùn)算復(fù)雜度較高的問題具有較好的改善作用。近幾年，很多研究者已經(jīng)將區(qū)間分析技術(shù)用于跟蹤問題并取得了很好的效果[10-11]，先后提出基于盒粒子的單目標(biāo)Bernoulli算法[12-14]、基于盒粒子的單擴(kuò)展目標(biāo)Bernoulli算法[15-17]以及基于盒粒子的多目標(biāo)PHD濾波算法[18]等。上述方法雖采用了區(qū)間分析理論，但是依然處理的是點(diǎn)量測(cè)數(shù)據(jù)而實(shí)際復(fù)雜場景下我們得到的數(shù)據(jù)一般都包含有較大的測(cè)量誤差，采用區(qū)間量測(cè)進(jìn)行表示可以更好地容納這種不確定性。

對(duì)此，本文提出了一種基于BPF的多擴(kuò)展目標(biāo)伯努利濾波算法(ET-BP-MeMBer)。在區(qū)間量測(cè)的假設(shè)前提下，改進(jìn)了偽似然函數(shù)，并針對(duì)存在的區(qū)間量測(cè)集劃分問題，將相互距離度量用于近鄰傳播聚類算法，提出了基于MD-AP聚類的區(qū)間量測(cè)集劃分方法。

1 盒粒子濾波

考慮如下非線性系統(tǒng)模型：

xk+1=f(xk,ωk)

(1)

zk=h(xk,vk)

(2)

式中，xk為擴(kuò)展目標(biāo)的狀態(tài)，ωk表示過程噪聲，vk表示量測(cè)噪聲，f(·)為非線性狀態(tài)轉(zhuǎn)移函數(shù)，h(·)為量測(cè)方程，zk為擴(kuò)展目標(biāo)的觀測(cè)值。

對(duì)于如上的非線性跟蹤系統(tǒng)，廣泛采用粒子濾波算法進(jìn)行求解。但是粒子濾波算法并不是十全十美的，粒子退化、計(jì)算量大等問題仍然制約著該算法的發(fā)展。近年來一種被稱為盒粒子濾波的方法得到了廣泛地研究。

盒粒子濾波算法采用占據(jù)一定空間且面積可控的矩形區(qū)域來代替?zhèn)鹘y(tǒng)的點(diǎn)粒子去描述目標(biāo)的狀態(tài)分布。基本的算法過程如圖1所示，簡述如下：

2)狀態(tài)預(yù)測(cè)：k+1刻目標(biāo)的預(yù)測(cè)概率密度函數(shù)為：

(3)

p(zk+1|xk+1)=U[zk+1](g(xk+1))

(4)

(5)

(6)

(7)

5)重采樣：為了保持盒粒子多樣性，必須進(jìn)行重采樣，由于處理的是帶誤差的數(shù)據(jù)，傳統(tǒng)的適用于粒子濾波的重采樣算法無法適用，本文采用隨機(jī)子劃分進(jìn)行處理。

由于觀測(cè)不確定性以及未知的各類偏差導(dǎo)致量測(cè)數(shù)據(jù)常常表現(xiàn)為區(qū)間的形式，如a=[a-,a+]。在這種條件下，傳統(tǒng)的算法難以適用，便迫切需要尋求合適的濾波算法。

2 本文算法

本章首先對(duì)擴(kuò)展目標(biāo)伯努利濾波算法的大致過程進(jìn)行簡要介紹，接著詳細(xì)敘述本文所提算法的主要過程。

2.1 擴(kuò)展目標(biāo)MeMBer濾波

在線性高斯的條件下，文獻(xiàn)[13]對(duì)擴(kuò)展目標(biāo)MeMBer濾波算法進(jìn)行了高斯混合實(shí)現(xiàn)。首先對(duì)傳統(tǒng)ET-MeMBer濾波算法的跟蹤過程簡述如下：

1)預(yù)測(cè)

(8)

式(8)中，

(9)

(10)

(11)

式(11)中，

(12)

(13)

(14)

(15)

2.2 基于盒粒子濾波的擴(kuò)展目標(biāo)MeMBer算法

上述2.1節(jié)給出了標(biāo)準(zhǔn)的ET-MeMBer濾波跟蹤算法，但是在實(shí)際應(yīng)用中，各種復(fù)雜因素常常導(dǎo)致產(chǎn)生不確定性量測(cè)，上述算法便無法處理。除此之外，在利用粒子濾波實(shí)施該算法時(shí)，為保證濾波精度，需要大量的粒子來覆蓋目標(biāo)的后驗(yàn)概率密度，增加了算法的運(yùn)算復(fù)雜度。針對(duì)上述問題，本文假設(shè)量測(cè)為區(qū)間的形式，將盒粒子濾波方法代替粒子濾波方法，提出了ET-BP-MeMBer濾波算法，具體算法步驟如下：

步驟1 預(yù)測(cè)

設(shè)k-1時(shí)刻的多擴(kuò)展目標(biāo)PDF可表示為：

(16)

(17)

則預(yù)測(cè)PDF為：

8月28日，湖北省發(fā)改委已正式同意“東風(fēng)汽車集團(tuán)股份有限公司在武漢市建設(shè)乘用車擴(kuò)建項(xiàng)目”，據(jù)了解，該項(xiàng)目將用于提升東風(fēng)日產(chǎn)的在華產(chǎn)能，但是具體運(yùn)營模式仍處在內(nèi)部探討階段。新工廠位于武漢經(jīng)濟(jì)技術(shù)開發(fā)區(qū)鳳凰工業(yè)園，分兩期建設(shè)，年產(chǎn)能30萬輛，能夠生產(chǎn)包括乘用車（含傳統(tǒng)燃油車和純電動(dòng)車型）及其他類乘用車（含傳統(tǒng)燃油車和混合動(dòng)力車型），建設(shè)期為2019年1月至2022年12月。該項(xiàng)目總投資為98.51億元，其中東風(fēng)公司持有66.86%的股份。

(18)

步驟2 更新

假設(shè)k時(shí)刻預(yù)測(cè)為：

(19)

(20)

則更新的后驗(yàn)密度為：

(21)

其中，

(22)

(23)

(24)

(25)

(26)

(27)

(28)

(29)

(30)

步驟3 約束傳播

[x]=[x]×[xz]，[y]=[y]×[yz]

步驟4 重采樣

為了保證粒子的多樣性，以及區(qū)間粒子的特殊性，本文利用隨機(jī)子劃分的方法進(jìn)行重采樣。

步驟5 修剪與合并

隨著迭代更新和目標(biāo)數(shù)的增加，伯努利項(xiàng)數(shù)目將會(huì)越來越多，為了提高計(jì)算效率，同時(shí)避免虛假軌跡的干擾，需要對(duì)伯努利項(xiàng)進(jìn)行刪減處理，一方面避免了干擾，另一方面也降低了計(jì)算量。

步驟6 狀態(tài)提取

僅提取概率大于0.5的伯努利分量，再利用傳統(tǒng)的Kmeans算法進(jìn)行聚類，從而獲得目標(biāo)的狀態(tài)估計(jì)，即：

(31)

2.3 基于MD-AP的區(qū)間量測(cè)劃分

AP算法是一種被廣泛使用的聚類方法[19]。在區(qū)間量測(cè)的環(huán)境下，所有的數(shù)和矢量均以區(qū)間的形式給出，傳統(tǒng)的度量方法難以適用，為此，采用一種新的度量方法稱之為相互度量。

設(shè)a=[a-，a+]、b=[b-，b+]為兩個(gè)區(qū)間量測(cè)，則將a相對(duì)于b的相互距離：

(32)

而b相對(duì)于a的相互距離：

(33)

式(33)中，(a∧b)=Max(a+,b+)-Min(a-,b-)。

由于a相對(duì)于b的相互距離并非總等于b相對(duì)于a的相互距離，為了適用于AP算法，本節(jié)定義a和b的相似性度量為：

(34)

設(shè)A=[a1,a2,…,an]、B=[b1,b2,…,bn]為兩個(gè)區(qū)間矢量，則A相對(duì)于B的區(qū)間相似度為：

(35)

接著直接采用前文提過的AP算法，算法中要求的相似度矩陣采用相互距離來計(jì)算，這樣便得到了適用于區(qū)間量測(cè)的劃分算法，如下所示：

步驟1 采用相互距離構(gòu)建所有量測(cè)點(diǎn)間的相似度矩陣，將代表矩陣和適選矩陣的元素都初始化為0：a(0)(i,k)=r(0)(i,k)=0；

步驟2 對(duì)建立的兩個(gè)矩陣進(jìn)行交替更新，當(dāng)滿足收斂條件時(shí)停止迭代，并通過對(duì)各個(gè)量測(cè)點(diǎn)進(jìn)行計(jì)算尋找各自的中心點(diǎn)從而實(shí)現(xiàn)量測(cè)集劃分；

至此，給出了基于MD-AP的區(qū)間量測(cè)劃分主要步驟。與傳統(tǒng)點(diǎn)量測(cè)集劃分相比，區(qū)間量測(cè)集劃分主要在于對(duì)區(qū)間量測(cè)點(diǎn)相似度的定義以及準(zhǔn)則函數(shù)的確定上有所不同。

3 算法分析及仿真實(shí)驗(yàn)

表1 目標(biāo)初始狀態(tài)和運(yùn)動(dòng)信息Tab.1 Target initial states and moving information

其狀態(tài)轉(zhuǎn)移密度為：

πk|k+1(x|x′)=N(x;Fx′,Q)

(36)

式(36)中，

Q=diag(0.01×I9，Qk)，Qk=diag(Q1,Q1)，

其中，I9表示9維的單位對(duì)角陣，T表示采樣周期。

圓形目標(biāo)半徑R=20 m，則目標(biāo)狀態(tài)可表示為：

(37)

量測(cè)方程h(x)定義為：

(38)

圖1給出了多擴(kuò)展目標(biāo)一次MC仿真的態(tài)勢(shì)圖，其中圓形表示目標(biāo)，方塊表示雜波和量測(cè)。圖2給出了跟蹤過程中目標(biāo)數(shù)目的估計(jì)比較?？梢钥闯?，本文算法在對(duì)目標(biāo)進(jìn)行數(shù)據(jù)估計(jì)時(shí)波動(dòng)更小，估計(jì)更穩(wěn)定。

圖1 態(tài)勢(shì)圖Fig.1 The situation of targets

圖2 目標(biāo)個(gè)數(shù)估計(jì)圖Fig.2 The estimated number of targets

為了對(duì)兩種算法的跟蹤性能表現(xiàn)情況進(jìn)行充分比較，對(duì)算法進(jìn)行了多次MCMC仿真實(shí)驗(yàn)，如圖3-圖6所示。

圖3 平均OSPA距離與雜波的關(guān)系Fig.3 The relationship between the mean OSPA distance and mean number of clutters

圖4 兩種算法OSPA距離與粒子數(shù)關(guān)系Fig.4 The relationship between OSPA distance and number of particles

圖3表示算法平均OSPA距離隨雜波的變化情況。從圖中可以很明顯地看出雜波對(duì)算法的性能有較大的影響，本文算法稍優(yōu)于原始算法。但是值得注意的是，兩種算法均不能達(dá)到對(duì)雜波的良好抑制作用。

圖4對(duì)兩種算法在不同粒子數(shù)下的平均OSPA進(jìn)行了比較試驗(yàn)，其中圖4(a)為本文所提算法，N表示設(shè)置的粒子數(shù)目。隨著粒子數(shù)的增加，兩種算法濾波的OSPA值都有逐步減少的趨勢(shì)。但是可以看出，在近似的性能表現(xiàn)要求下，本文算法所需要的粒子數(shù)目遠(yuǎn)遠(yuǎn)小于基于點(diǎn)粒子濾波的方法。

圖5 ET-P-MeMber算法平均運(yùn)行時(shí)間Fig.5 The average operation time of ET-P-MeMBer

圖6 ET-BP-MeMber算法平均運(yùn)行時(shí)間Fig.6 Theaverage operation time of ET-BP-MeMBer

圖5對(duì)算法的運(yùn)行時(shí)間進(jìn)行對(duì)比，主要通過在相同新生目標(biāo)數(shù)的前提下，各自產(chǎn)生不同新生粒子數(shù)的情況。為了捕獲新生目標(biāo)，ET-P-MeMBer算法需要產(chǎn)生大量的新生粒子，使得運(yùn)算量有所增加，而ET-BP-MeMBer算法的主要時(shí)間消耗在盒粒子濾波的區(qū)間運(yùn)算上。綜合來看，本文提出的ET-BP-MeMBer算法由于在運(yùn)算效率和精度上有著良好的折中，使得可以更好地應(yīng)用于實(shí)際場景中。

4 結(jié)論

針對(duì)不確定量測(cè)下的對(duì)擴(kuò)展目標(biāo)跟蹤問題，本文提出了基于MD-AP的ET-BP-MeMBer算法。當(dāng)量測(cè)采用區(qū)間表示的時(shí)候，傳統(tǒng)的處理手段難以利用，本文將盒粒子濾波算法用于濾波求解過程，在建模了量測(cè)誤差的前提下，較大地提升了算法的運(yùn)算效率，同時(shí)本文提出了基于相互距離的AP算法用于處理區(qū)間量測(cè)集劃分問題。實(shí)驗(yàn)結(jié)果表明本文算法有著較好的跟蹤精度和計(jì)算效率。