面向非線性MTT的多模型泊松多伯努利混合濾波算法

陳嵩杰,李 波,張 露

(遼寧工業(yè)大學(xué) 電子與信息工程學(xué)院,遼寧 錦州 121001)

0 引 言

多目標(biāo)跟蹤[1](Multi Target Tracking,MTT)是一個(gè)重要的研究方向,適用于航空航天、空中交管和自動(dòng)駕駛等領(lǐng)域[2,3].考慮MTT共軛性所反映的目標(biāo)先驗(yàn)與后驗(yàn)分布概率密度具有相同形式的特點(diǎn)[4],基于多伯努利共軛先驗(yàn)理論的帶標(biāo)簽廣義標(biāo)簽多伯努利(Generalized Label Multi-Bernoulli,GLMB)濾波[5]和不帶標(biāo)簽的泊松多伯努利混合(Poisson Multi-Bernoulli Mixture Filter,PMBM)濾波[6]已有研究.其中,PMBM濾波可為目標(biāo)概率密度提供精確閉式解,計(jì)算代價(jià)和精度較GLMB濾波器更優(yōu).文獻(xiàn)[7]提出了一種多模型多伯努利濾波算法,給出了該算法在線性高斯模型的實(shí)現(xiàn);文獻(xiàn)[8]提出了一種多伯努利混合濾波的高斯實(shí)現(xiàn);文獻(xiàn)[9]提出了一種基于多模型泊松多伯努利混合濾波的高斯逆威沙特實(shí)現(xiàn),利用高斯逆威沙特分布估計(jì)目標(biāo)質(zhì)心,并引入強(qiáng)跟蹤濾波修正狀態(tài)模型協(xié)方差矩陣.

針對(duì)非線性模型的MTT問(wèn)題,現(xiàn)有文獻(xiàn)主要采用序貫蒙特卡洛(Sequential Monte Carlo,SMC)和高斯混合(Gaussian Mixture,GM)兩種方式實(shí)現(xiàn).其中,SMC實(shí)現(xiàn)[10,11]存在粒子退化問(wèn)題,濾波計(jì)算繁瑣,且跟蹤速度慢;GM實(shí)現(xiàn)[12]的計(jì)算復(fù)雜程度相對(duì)較低,適用于線性高斯模型的MTT.基于此,文獻(xiàn)[13]提出了一種非線性模型的擴(kuò)展卡爾曼濾波(Extended Kalman Filter,EKF)的PMBM算法,但EKF在強(qiáng)非線性系統(tǒng)中存在較低的魯棒性,其雅克比矩陣計(jì)算也較為困難;文獻(xiàn)[14]提出了一種非線性模型的(Cubature Kalman Filter-Multi-Bernoulli,CKF-MB)和(Square-rooted CKF-MB,SCKF-MB)濾波算法,但在低檢測(cè)概率時(shí),目標(biāo)勢(shì)估計(jì)存在一定偏差;文獻(xiàn)[15]提出了一種用于非線性模型δ-GLMB濾波的積分卡爾曼高斯混合實(shí)現(xiàn),但有效性不高.針對(duì)非線性高雜波環(huán)境的MTT,文獻(xiàn)[16]提出了一種均方根容積卡爾曼濾波與δ-GLMB結(jié)合的GM實(shí)現(xiàn),但仍存在平衡濾波運(yùn)算時(shí)間和跟蹤精度難以平衡的問(wèn)題.

針對(duì)MTT的非線性與低檢測(cè)概率問(wèn)題,本文在非線性模型下融合SCKF與高斯混合多模型泊松多伯努利混合濾波(GM Multiple Model PMBM,GM-MM-PMBM),提出一種基于SCKF的GM-MM-PMBM濾波算法.首先,推導(dǎo)出MM-PMBM預(yù)測(cè)和更新的GM實(shí)現(xiàn).然后,采用誤差協(xié)方差的平方根方式進(jìn)行遞推,應(yīng)用等權(quán)值容積點(diǎn)集計(jì)算多目標(biāo)密度函數(shù)的均值與協(xié)方差矩陣.最后,結(jié)合SCKF對(duì)GM-MM-PMBM濾波高斯項(xiàng)進(jìn)行預(yù)測(cè)和更新,采用最優(yōu)子模式[17,18](Optimal Sub-Pattern Assignment,OSPA)距離評(píng)價(jià)該算法的性能.

1 相關(guān)工作

1.1 隨機(jī)有限集

在隨機(jī)有限集(Random Finite Set,RFS)理論框架中[19],假設(shè)單目標(biāo)狀態(tài)x∈nx,多目標(biāo)狀態(tài)X∈F(nx),X為單目標(biāo)狀態(tài)向量的集合,F(nx)為nx空間中所有有限子集集合,目標(biāo)狀態(tài)可由量測(cè)集合Z∈F(nz)中的量測(cè)觀察.

假設(shè)k時(shí)刻的狀態(tài)集為X={x1,…,xn};測(cè)量集為Z={Z1,…,Zn}∪κ(z),κ(z)為雜波量測(cè)集合且滿足κ(z)=λc(z),λ為泊松率,c(z)為雜波空間概率分布函數(shù),Zi為目標(biāo)i產(chǎn)生的量測(cè)集合,κ(z)與Z1…Zn相互獨(dú)立.

1.2 PMBM共軛先驗(yàn)

PMBM濾波可由泊松和多伯努利混合兩部分表示[20].其中,泊松部分用于表示未檢目標(biāo);多伯努利混合部分則表示已檢目標(biāo)[21].假設(shè)X=Xu+∪Xd為目標(biāo)集合,Xu和Xd分別為未檢目標(biāo)和已檢目標(biāo),則共軛先驗(yàn)可定義為:

(1)

(2)

(3)

式中,wi,a為權(quán)重,+∪表示不相交并集;fp(·)為泊松部分概率密度函數(shù);μ(·)為泊松強(qiáng)度;fmbm(·)為多伯努利混合部分概率密度函數(shù);A為全局假設(shè)索引集合,a為所有全局假設(shè)索引,n為所有伯努利分量的數(shù)目.

于是,第a個(gè)全局假設(shè)中第i個(gè)伯努利量為:

(4)

式中,ri,a(·)和fi,a(·)分別為第a個(gè)全局假設(shè)中第i個(gè)的目標(biāo)存在概率和伯努利分量.

1.3 標(biāo)準(zhǔn)PMBM算法

1.3.1 預(yù)測(cè)過(guò)程

由式(1)～式(3)可知,PMBM濾波器的預(yù)測(cè)過(guò)程包含泊松部分和多伯努利混合部分.假設(shè)k-1時(shí)刻泊松強(qiáng)度為μk-1(xk-1),則k時(shí)刻泊松強(qiáng)度預(yù)測(cè)密度函數(shù)為:

μk|k-1(xk)=λb(xk)+〈μk-1(xk-1),pSf(xk|xk-1)〉

(5)

式中,λb(·)為新生強(qiáng)度,pS為目標(biāo)存活概率,f(xk|xk-1)為狀態(tài)轉(zhuǎn)移函數(shù),[·,·]表示函數(shù)的內(nèi)積.

同時(shí),多伯努利部分預(yù)測(cè)參數(shù)可表示為:

(6)

1.3.2 更新過(guò)程

更新過(guò)程包含3個(gè)部分:未檢目標(biāo)的泊松強(qiáng)度更新、首次檢測(cè)目標(biāo)的泊松強(qiáng)度更新、已檢目標(biāo)的伯努利更新[22].

未檢目標(biāo)的泊松強(qiáng)度更新為:

(7)

首次檢測(cè)目標(biāo)的伯努利更新為:

(8)

(9)

已檢目標(biāo)(漏檢目標(biāo)和量測(cè)目標(biāo))的伯努利更新為:

(10)

(11)

可以看出,泊松強(qiáng)度更新和伯努利更新過(guò)程是分別進(jìn)行的,首次檢測(cè)目標(biāo)的伯努利更新和已檢目標(biāo)的伯努利更新差異是前者中的伯努利分量含泊松分量.

2 多伯努利混合濾波非線性實(shí)現(xiàn)

由于標(biāo)準(zhǔn)PMBM算法在遞歸步驟中存在難以求解RFS積分的問(wèn)題[23],因此基于高斯線性模型推導(dǎo)出該算法的GM實(shí)現(xiàn),將積分形式傳遞的伯努利參數(shù)轉(zhuǎn)化為高斯項(xiàng)加權(quán)求和形式.

為表述簡(jiǎn)潔,下文中的各函數(shù)主要以變量ξ定義,且假設(shè)檢測(cè)概率pd(·)和存活概率pS(·)與模型相關(guān),滿足pd(xk,ξ)=pd(ξ),pS(xk,ξ)=pS(ξ).于是,PMBM算法的GM實(shí)現(xiàn)的預(yù)測(cè)和更新遞推如下.

2.1 GM-MM-PMBM算法

2.1.1 預(yù)測(cè)過(guò)程

基于高斯分布N(·),新生目標(biāo)泊松強(qiáng)度的GM形式可記為:

(12)

假設(shè)k-1時(shí)刻泊松強(qiáng)度的GM形式為:

(13)

則k時(shí)刻預(yù)測(cè)強(qiáng)度的GM形式為:

(14)

式中,Fk(ξ)為模型ξ的狀態(tài)轉(zhuǎn)移矩陣,Qk(ξ)為模型ξ的過(guò)程噪聲協(xié)方差矩陣,fk|k-1(ξ|ξ′)為模型ξ′～模型ξ的轉(zhuǎn)移概率.

假設(shè)k-1時(shí)刻伯努利密度函數(shù)為:

(15)

則預(yù)測(cè)過(guò)程伯努利分量的權(quán)重保持不變,其存在概率滿足:

(16)

于是,多伯努利部分預(yù)測(cè)密度函數(shù)為:

(17)

2.1.2 更新過(guò)程

未檢目標(biāo)的泊松強(qiáng)度更新:假設(shè)泊松預(yù)測(cè)強(qiáng)度GM形式由式(19)給出.

(18)

則更新后的后驗(yàn)強(qiáng)度為:

μk|k(xk,ξ)=(1-pd)μk|k-1(xk,ξ)

(19)

首次檢測(cè)到目標(biāo)的泊松強(qiáng)度更新:

(20)

(21)

式中,相關(guān)參數(shù)定義如下:

已檢目標(biāo)的伯努利更新:對(duì)于漏檢目標(biāo)定義下述公式.

(22)

(23)

(24)

對(duì)于量測(cè)目標(biāo)則有:

(25)

(26)

(27)

式中,相關(guān)參數(shù)定義如下:

2.2 基于SCKF的GM-MM-PMBM算法

考慮運(yùn)動(dòng)模型ξ,假設(shè)目標(biāo)狀態(tài)方程和量測(cè)方程分別為:

(28)

式中,xk和zk分別為狀態(tài)向量和量測(cè)向量;fk(·)和hk(·)分別為狀態(tài)函數(shù)和量測(cè)函數(shù);uk-1是均值為0、協(xié)方差矩陣為Qk-1的過(guò)程噪聲;vk是均值為0、協(xié)方差矩陣為Rk的量測(cè)噪聲.

假設(shè)k-1時(shí)刻誤差協(xié)方差的平方根滿足下三角陣Γk-1|k-1=Tria(A),Tria(·)為矩陣的三角化運(yùn)算,于是基于SCKF的非線性濾波過(guò)程如下:

2.2.1 預(yù)測(cè)過(guò)程

由Cholesky分解計(jì)算預(yù)測(cè)高斯項(xiàng)的協(xié)方差矩陣:

(29)

利用三階球面-徑向容積準(zhǔn)則生成容積點(diǎn)χq,k-1及權(quán)重wq=1/2n,q=1,2,…,2n,這里系統(tǒng)狀態(tài)維數(shù)設(shè)為2n.于是,計(jì)算非線性狀態(tài)函數(shù)的傳遞容積點(diǎn):

(30)

計(jì)算狀態(tài)預(yù)測(cè)均值:

(31)

計(jì)算預(yù)測(cè)誤差協(xié)方差矩陣的平方根:

(32)

2.2.2 更新過(guò)程

產(chǎn)生容積點(diǎn)χq,+并計(jì)算非線性量測(cè)函數(shù)傳遞容積點(diǎn):

(33)

計(jì)算量測(cè)預(yù)測(cè)均值:

(34)

計(jì)算新息協(xié)方差平方根:

(35)

計(jì)算互協(xié)方差矩陣:

(36)

計(jì)算卡爾曼增益:

(37)

計(jì)算狀態(tài)均值更新:

(38)

計(jì)算協(xié)方差矩陣的平方根更新:

(39)

3 仿真驗(yàn)證與結(jié)果分析

3.1 仿真情形及參數(shù)設(shè)置

目標(biāo)運(yùn)動(dòng)模型ξ設(shè)為勻速(Constant Velocity,CV)運(yùn)動(dòng)和勻速轉(zhuǎn)彎(Constant Turn,CT)運(yùn)動(dòng),其狀態(tài)轉(zhuǎn)移方程分別為:

式中,wk-1是均值為0、協(xié)方差矩陣為Qk-1的過(guò)程噪聲.

目標(biāo)非線性量測(cè)模型方程為:

觀測(cè)矩陣和量測(cè)噪聲協(xié)方差矩陣分別為:

式中,標(biāo)準(zhǔn)差σε=10,I2為二維單位方陣.此外,目標(biāo)存活概率為pS=0.99,由泊松分布對(duì)雜波建模.模型1定義為CV運(yùn)動(dòng),設(shè)過(guò)程噪聲為5m/s2;模型2定義為CT運(yùn)動(dòng),設(shè)過(guò)程噪聲為5 m/s2,逆時(shí)針轉(zhuǎn)動(dòng)角度為3°/s;模型3定義為CT運(yùn)動(dòng),設(shè)過(guò)程噪聲為5m/s2,順時(shí)針轉(zhuǎn)動(dòng)角度為3°/s.于是,定義CV和CT運(yùn)動(dòng)的線性狀態(tài)轉(zhuǎn)移矩陣如下:

假設(shè)目標(biāo)新生模型為泊松形式,新生目標(biāo)泊松強(qiáng)度GM為:

所有存活目標(biāo)在前20s進(jìn)行CV運(yùn)動(dòng),在第21～ 40s內(nèi)按逆時(shí)針?lè)较蛴蒀V運(yùn)動(dòng)轉(zhuǎn)換為CT運(yùn)動(dòng),在第60～80s內(nèi)按順時(shí)針?lè)较蜣D(zhuǎn)換為CT運(yùn)動(dòng),模型間轉(zhuǎn)移概率矩陣定義為:

圖1給出了檢測(cè)區(qū)域和3個(gè)目標(biāo)的非線性運(yùn)動(dòng)軌跡.

圖1 目標(biāo)真實(shí)軌跡Fig.1 Target real trajectory

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

為比較本文SCKF-GM-MM-PMBM算法與EKF-PMBM算法[13]、SCKF-GM-MM-MB算法[14]、SMC-MM-PMBM算法的跟蹤性能,本實(shí)驗(yàn)進(jìn)行100次SMC仿真.

3.2.1 固定雜波密度及高檢測(cè)概率情形下

假設(shè)檢測(cè)概率為pd=0.96,雜波密度為λC=0.6×10-3/m2,目標(biāo)勢(shì)估計(jì)及誤差估計(jì)仿真結(jié)果如圖2所示.

圖2 高檢測(cè)概率下勢(shì)估計(jì)Fig.2 Cardinality estimation under high detection probability

可以看出,隨著時(shí)間的變化,各算法都能較好完成勢(shì)估計(jì).EKF-PMBM算法、SCKF-GM-MM-MB算法和本文算法都能接近目標(biāo)真實(shí)數(shù)目,而經(jīng)典SMC-MM-PMBM算法在粒子數(shù)目較少時(shí),會(huì)出現(xiàn)低于目標(biāo)真實(shí)值的估計(jì)情形;并在第15s和第30s時(shí),SMC-MM-PMBM算法估計(jì)時(shí)延較長(zhǎng)且對(duì)目標(biāo)勢(shì)估計(jì)有明顯偏差,EKF-PMBM算法和SCKF-GM-MM-MB算法的目標(biāo)數(shù)目估計(jì)時(shí)延較低,而本文算法的估計(jì)時(shí)延和消耗較最低,能有效地估計(jì)目標(biāo)勢(shì)變化.

本文采用OSPA距離作為MTT性能的評(píng)價(jià)指標(biāo),該距離越大表示算法的總體精度越差,設(shè)定參誤差調(diào)節(jié)參數(shù)c為100,階次參數(shù)p為1[24].各濾波算法的OSPA距離對(duì)比如圖3所示.可以看出,各算法具有相似的OSPA誤差,在第5s和第18s時(shí),SCKF-GM-MM-MB算法給出的OSPA距離略優(yōu)于SCKF-GM-MM-PMBM算法.若目標(biāo)在第20s出現(xiàn),在第60s和第80s時(shí)目標(biāo)消失,本文算法優(yōu)于SCKF-GM-MM-MB算法,其原因?yàn)镻MBM算法中泊松部分保留了漏檢分量和剛消失的目標(biāo)分量,能及時(shí)檢測(cè)新生目標(biāo)并延遲對(duì)消亡目標(biāo)的響應(yīng).EKF-PMBM算法和SMC-MM-PMBM算法的OSPA距離相比于另兩種算法偏大,其原因?yàn)閺?qiáng)非線性下EKF需先進(jìn)行線性處理,忽略了高階項(xiàng),出現(xiàn)發(fā)散問(wèn)題,導(dǎo)致跟蹤目標(biāo)丟失;而SMC的采樣粒子較多易發(fā)生粒子退化現(xiàn)象,若當(dāng)采樣粒子數(shù)目較少時(shí),目標(biāo)跟蹤性能和運(yùn)算速度降低.

圖3 OSPA距離對(duì)比Fig.3 OSPA distance comparison

3.2.2 固定雜波密度及低檢測(cè)概率情形下

假設(shè)目標(biāo)檢測(cè)概率為pd=0.6,仿真結(jié)果如圖4所示.

圖4 低檢測(cè)概率下勢(shì)估計(jì)Fig.4 Cardinality estimation under low detection probability

對(duì)比高檢測(cè)概率環(huán)境,可以看出SCKF-GM-MM-PMBM算法的勢(shì)估計(jì)與SCKF-GM-MM-MB算法、EKF-PMBM算法和SMC-MM-PMBM算法有著明顯的差異,SCKF-GM-MM-PMBM算法具有更好的勢(shì)估計(jì)誤差.

各算法的OSPA距離對(duì)比如圖5所示.可以看出,當(dāng)?shù)?2s目標(biāo)消亡時(shí),本文算法的OSPA距離誤差較SCKF-GM-MM-MB算法大,其原因?yàn)檩^低的Pd使本文算法在更新過(guò)程中的泊松分量權(quán)重增大,無(wú)法及時(shí)刪除剛剛消失的目標(biāo)狀態(tài).

圖5 OSPA距離對(duì)比Fig.5 OSPA distance comparison

表1綜合比較了OSPA距離誤差和單步平均運(yùn)行時(shí)間.可以看出,本文算法在低檢測(cè)概率時(shí)下具有更好的跟蹤性能.

表1 不同檢測(cè)概率的OSPA距離和單步平均運(yùn)行時(shí)間Table 1 OSPA distance and single-step average running time under different detection probabilities

3.2.3 不同雜波密度情形下

為進(jìn)一步驗(yàn)證所提算法的跟蹤性能,給出了各算法在不同雜波密度λC=(0.6,1.2,1.8,2.4,3.0)×10-3/m2時(shí)的勢(shì)估計(jì)誤差與OSPA距離對(duì)比.

圖6和圖7分別為勢(shì)估計(jì)誤差對(duì)比圖與OSPA距離對(duì)比圖.可以看出,當(dāng)雜波密度增大時(shí),各算法的勢(shì)估計(jì)誤差與OSPA距離誤差增大,EKF-PMBM算法僅次于SCKF-GM-MM-MB算法,其原因?yàn)槿醴蔷€性環(huán)境下EKF-PMBM算法可優(yōu)先考慮,在強(qiáng)非線性情形下EKF算法與線性假設(shè)原則沖突,導(dǎo)致濾波發(fā)散并產(chǎn)生估計(jì)誤差,但本文算法在不同雜波密度情況下,其勢(shì)估計(jì)誤差和OSPA距離誤差都要優(yōu)于其他3種算法.

圖6 勢(shì)估計(jì)誤差對(duì)比Fig.6 Cardinality estimation and error comparison

圖7 OSPA距離對(duì)比Fig.7 OSPA distance comparison

表2對(duì)比了當(dāng)檢測(cè)概率pd=0.96時(shí)不同雜波密度環(huán)境下的OSPA距離與單步平均運(yùn)行時(shí)間.可以看出,當(dāng)同一檢測(cè)概率時(shí),各算法的OSPA距離和運(yùn)行時(shí)間都隨雜波密度的增加而增大.當(dāng)不同雜波時(shí),本文算法具有較高跟蹤精度,運(yùn)行時(shí)間低于其他算法.當(dāng)雜波密度較高時(shí),由于EKF-PMBM算法進(jìn)行雅克比矩陣計(jì)算,導(dǎo)致運(yùn)行時(shí)間增加;而SMC-MM-PMBM算法所需的采樣粒子數(shù)目不斷增加,導(dǎo)致運(yùn)行時(shí)間增加.

表2 不同雜波密度下的OSPA距離和單步平均運(yùn)行時(shí)間Table 2 OSPA distance and single-step average running time under different clutter densities

綜上,本文算法在不同檢測(cè)概率情形下能以較高的精度估計(jì)目標(biāo)的勢(shì)和運(yùn)動(dòng)狀態(tài).尤其是在低檢測(cè)概率情形下,其跟蹤精度和運(yùn)行時(shí)間優(yōu)于其他3種算法;在高檢測(cè)概率不同雜波密度情形下,各算法的OSPA距離與運(yùn)行時(shí)間具有一定差距,本文算法的OSPA距離優(yōu)于其他3種算法.

4 總 結(jié)

本文針對(duì)MTT存在的非線性與低檢測(cè)概率問(wèn)題,提出了MM-PMBM算法的GM實(shí)現(xiàn),在低檢測(cè)概率環(huán)境下為目標(biāo)概率密度提供精確閉式解,其跟蹤精度和運(yùn)行時(shí)間上相比MM-MB算法有所提高;在非線性系統(tǒng)模型下,融合SCKF濾波框架,避免了計(jì)算協(xié)方差矩陣的平方根,其數(shù)值穩(wěn)定性和濾波性能得以保證;在不同雜波密度情形下,該算法可準(zhǔn)確估計(jì)目標(biāo)的勢(shì)與運(yùn)動(dòng)狀態(tài),提高了MTT的跟蹤精度.由不同情形下的仿真實(shí)驗(yàn)結(jié)果可知,本文算法在非線性情形中OSPA距離誤差與運(yùn)行時(shí)間均最小.

針對(duì)MTT的運(yùn)行時(shí)間,接下來(lái)將深入研究本文算法用于解決機(jī)動(dòng)目標(biāo)、擴(kuò)展目標(biāo)和群目標(biāo)等比較復(fù)雜的問(wèn)題,進(jìn)一步節(jié)省運(yùn)行時(shí)間.