魯棒邊緣粒子濾波及在目標(biāo)跟蹤中應(yīng)用

王宗原，周衛(wèi)東

(1.哈爾濱工程大學(xué) 數(shù)學(xué)科學(xué)學(xué)院，哈爾濱 150001; 2.哈爾濱工程大學(xué) 智能科學(xué)與工程學(xué)院，哈爾濱 150001)

0 引言

濾波是基于從初始時(shí)間到當(dāng)前時(shí)間的噪聲測量對當(dāng)前狀態(tài)進(jìn)行估計(jì)的一種方法。已經(jīng)被廣泛應(yīng)用于許多領(lǐng)域，如目標(biāo)跟蹤、導(dǎo)航、機(jī)器人控制和信號(hào)處理[1-4]等。粒子濾波是利用加權(quán)粒子求和方法進(jìn)行非線性狀態(tài)空間模型狀態(tài)估計(jì)的高效方法。但是高維狀態(tài)如果直接用粒子濾波求解會(huì)產(chǎn)生維數(shù)災(zāi)難，導(dǎo)致粒子濾波崩潰而無法應(yīng)用[3]。為了提高粒子濾波廣泛的實(shí)際應(yīng)用性，目前普遍關(guān)注的是從系統(tǒng)模型結(jié)構(gòu)出發(fā)探究其是否具有條件線性子結(jié)構(gòu)，從而通過狀態(tài)降維方法進(jìn)行狀態(tài)估計(jì)。通過查閱已有文獻(xiàn)可知，這一類模型在導(dǎo)航和目標(biāo)跟蹤領(lǐng)域廣泛存在。目前估計(jì)最有效方法是利用Rao-Blackwellised思想設(shè)計(jì)的粒子濾波(RBPF)，或稱為邊緣粒子濾波[5]，它將粒子濾波與線性最優(yōu)的卡爾曼濾波巧妙融合完成整體狀態(tài)估計(jì)。

邊緣粒子濾波由于其更優(yōu)異的濾波設(shè)計(jì)方案，一經(jīng)出現(xiàn)在導(dǎo)航和目標(biāo)跟蹤及動(dòng)態(tài)貝葉斯網(wǎng)絡(luò)中就取得成功應(yīng)用，而且為適應(yīng)不同系統(tǒng)模型及量測出現(xiàn)的各種新問題，近幾年來研究人員對RBPF及其不同改進(jìn)形式進(jìn)行了廣泛研究，例如，文獻(xiàn)[6]中提出用遞推期望最大化方法開發(fā)具有跳躍馬爾科夫鏈和系統(tǒng)結(jié)構(gòu)分解的Rao-Blackwellised粒子濾波算法；同時(shí)文獻(xiàn)[7]利用Rao-Blackwellised準(zhǔn)則結(jié)合RBPF平滑解決混合線性狀態(tài)空間模型具有量測失序的RBPF算法性能下降問題，通過標(biāo)準(zhǔn)常加速模型的仿真結(jié)果，表明所提算法精度具有超過RBPF精度10%的性能；再有文獻(xiàn)[8]考慮了混合線性模型線性狀態(tài)方程噪聲之間及狀態(tài)方程噪聲與量測噪聲相依賴的情況，利用格萊姆-施密特解耦方法實(shí)現(xiàn)全部噪聲協(xié)方差矩陣的對角化形式，并在常速目標(biāo)跟蹤模型仿真中取得優(yōu)于標(biāo)準(zhǔn)RBPF精度的結(jié)果；進(jìn)一步文獻(xiàn)[9]將RBPF框架擴(kuò)展到系統(tǒng)全部為非線性方程的模型中得到分散粒子濾波(DPF)，不同于卡爾曼濾波粒子濾波混合求解過程，DPF通過兩次權(quán)值及兩次重采樣計(jì)算得到兩次粒子混合估計(jì)結(jié)果。由以上的文獻(xiàn)描述可見RBPF算法適用各種新出現(xiàn)系統(tǒng)問題的改進(jìn)形式已被廣泛探究，并且得到優(yōu)于標(biāo)準(zhǔn)RBPF的結(jié)果。但據(jù)作者所知，RBPF量測噪聲參數(shù)未知的改進(jìn)形式還沒有被涉及。

雖然模型參數(shù)未知的貝葉斯濾波已通過狀態(tài)擴(kuò)維法[10]或交互式多模型(IMM)法[11]進(jìn)行了研究，但它們對噪聲參數(shù)未知情形還無法解決。近期文獻(xiàn)[12-13]針對噪聲異常情況通過變分推斷理論給出魯棒高斯近似濾波算法，并在目標(biāo)跟蹤模擬中得到成功應(yīng)用，但標(biāo)準(zhǔn)無跡卡爾曼濾波(UKF)或擴(kuò)展卡爾曼濾波(EKF)由于樣本點(diǎn)有限或線性化誤差原因?qū)τ谙嗤瑢?dǎo)航或目標(biāo)跟蹤問題精度低于邊緣粒子濾波或其變形的DPF[14-15]。另外，水下運(yùn)載器地形輔助導(dǎo)航[16]、重力梯度輔助導(dǎo)航等組合導(dǎo)航估計(jì)方法也均采用RBPF[17]，這是因?yàn)檫@些基于數(shù)據(jù)庫圖的地球物理場導(dǎo)航只能通過粒子表示載體全部可能位置，而且隨著粒子數(shù)增加，導(dǎo)航精度還可能繼續(xù)提高。

RBPF的另一個(gè)重要應(yīng)用是在目標(biāo)運(yùn)動(dòng)分析(TMA) 領(lǐng)域，也即目標(biāo)跟蹤，這屬于通過方位測量對運(yùn)動(dòng)目標(biāo)狀態(tài)進(jìn)行估計(jì)的范疇[18]。由單運(yùn)動(dòng)觀測器收集量測形式已在無源傳感領(lǐng)域(如聲納或雷達(dá))具有廣泛應(yīng)用[19]，過去幾年成為研究熱點(diǎn)?，F(xiàn)在關(guān)注的是用遞推貝葉斯方法得到狀態(tài)后驗(yàn)概率密度函數(shù)(PDF)。TMA的特點(diǎn)通過觀測平臺(tái)的運(yùn)動(dòng)對目標(biāo)距離進(jìn)行觀測。下面將討論貝葉斯TMA的平面跟蹤問題，它將從一維方位量測集獲得四維狀態(tài)后驗(yàn)PDFs，狀態(tài)包含目標(biāo)位置和速度。但高機(jī)動(dòng)目標(biāo)跟蹤通常會(huì)產(chǎn)生量測噪聲異常問題[12-13]，影響量測噪聲因素有接觸實(shí)際角度、目標(biāo)的距離和相對速度、信噪比、接收陣列形狀、傳播信道等等，這些因素很少能準(zhǔn)確知道并且隨時(shí)間推移而變化，因此工程實(shí)際中精確知道量測方差幾乎是不可能的，這就造成 TMA量測噪聲方差時(shí)變未知且包含異常值的特性。

注意到現(xiàn)有邊緣粒子濾波及其各種改進(jìn)形式都沒對量測噪聲時(shí)變的目標(biāo)跟蹤問題給出有效解決方案，這或許是由于如何在RBPF引入噪聲參數(shù)實(shí)時(shí)估計(jì)的方法還沒有被探究。為了設(shè)計(jì)具有量測方差自適應(yīng)估計(jì)能力的混合系統(tǒng)模型的魯棒邊緣粒子濾波算法，本文將探究使用Rao-Blackwellised原理及變分推斷公式對混合系統(tǒng)量測噪聲異常問題進(jìn)行狀態(tài)估計(jì)，并在典型TMA的平面跟蹤模型中進(jìn)行仿真驗(yàn)證。

1 混合模型及邊緣粒子濾波

1.1 目標(biāo)跟蹤模型

Xk+1=FkXk-Uk+1,k+Gkwk

(1)

純角度觀測模型為:

zk=atan2(xk,yk)+vk

(2)

atan2(·)表示四象限反正切函數(shù)，此處得到k時(shí)刻目標(biāo)真實(shí)角度。觀測噪聲vk～N(0,Rk)，Rk可能是時(shí)變且未知的。

1.2 混合模型及濾波

對式(1)、(2)組成目標(biāo)跟蹤系統(tǒng)模型進(jìn)行一般化引申，得到與組合導(dǎo)航及定位相同的如下線性非線性狀態(tài)混合模型表達(dá)式：

(3)

先忽略量測噪聲變化的情況，RBPF的狀態(tài)估計(jì)推導(dǎo)如下，聯(lián)合狀態(tài)的后驗(yàn)可分解為：

(4)

依據(jù)chapman-Kolmogorov公式，并根據(jù)高斯分布引理[12]：

N(x|Φμ+d,ΦPΦT+∑)

(5)

可見前一時(shí)刻線性狀態(tài)后驗(yàn)對非線性狀態(tài)預(yù)測是附加噪聲，由于高斯過程噪聲，非線性狀態(tài)預(yù)測是如下高斯分布形式：

(6)

(7)

(8)

其中參數(shù)為:

(9)

(10)

(11)

(12)

2 量測噪聲魯棒RBPF推導(dǎo)

下面考慮量測噪聲方差未知的情形。采用將量測噪聲方差視為先驗(yàn)分布已知隨機(jī)變量，并把它應(yīng)用到遞推模型中，假定狀態(tài)與量測噪聲方差相互獨(dú)立有：

p(xk,Rk|xk-1,Rk-1)=p(xk|xk-1)p(Rk|Rk-1)

(13)

其中:p(xk|xk-1)狀態(tài)遞推式，p(Rk|Rk-1)方差遞推式。利用貝葉斯濾波公式和Rao-Blackwellised法則，可用兩種方式同時(shí)求解狀態(tài)和方差后驗(yàn)，第一種是如下分解:

(14)

(15)

變分推斷得到的后驗(yàn)近似分布公式:

q(Θi)∝exp{Eq(Θj≠i)[lnp(Θ|Z)]}

(16)

其中:Eq(Θj≠i)[lnp(Θ|Z)]表示條件聯(lián)合PDF對異于Θi的其它參數(shù)求期望[2]。下面給出適用于所提混合模型的具體推導(dǎo)過程，利用貝葉斯公式，非線性狀態(tài)及量測噪聲參數(shù)條件聯(lián)合分布表示為：

(17)

(18)

G(·|α,β)表示超參數(shù)為α,β的伽馬分布，伽馬分布與高斯分布一樣都屬于具有封閉性質(zhì)共軛指數(shù)族。式(17)右端第一個(gè)表達(dá)式為高斯分布。第二個(gè)表達(dá)式噪聲參數(shù)即尺度矩陣、隱變量及自由度的聯(lián)合分布p(Λk,uk,vk)，設(shè)為伽馬分布連乘積，又根據(jù)概率乘法公式及式(18)的第二個(gè)式子，有如下表達(dá)形式：

其中：上角標(biāo)jj表示對角尺度矩陣第jj個(gè)分量,jj∈{1,2,…,m}。注意此時(shí)狀態(tài)粒子權(quán)值計(jì)算式:

(19)

注意權(quán)值計(jì)算時(shí)右面量測似然是用估計(jì)的尺度矩陣與自由度的t分布表示。由于參數(shù)粒子依賴狀態(tài)粒子，因此這也是參數(shù)粒子權(quán)值表達(dá)式。

為了表示噪聲參數(shù)的變化情況，噪聲參數(shù)預(yù)測引入波動(dòng)參數(shù)ρ∈(0,1)，噪聲超參數(shù)預(yù)測[2.20]表達(dá)式為:

(20)

變分推斷得尺度矩陣、隱變量及自由度參數(shù)變分后驗(yàn)如下。

在(16)中令Θi=Λk，計(jì)算尺度矩陣變分后驗(yàn)PDF對數(shù)為：

(21)

可見尺度矩陣更新的超參數(shù)中包含隱變量的期望E[uk]。

在(16)中,令Θi=uk，計(jì)算隱變量uk變分后驗(yàn)PDFq(uk)的對數(shù)為：

(22)

自由度變分后驗(yàn)計(jì)算與線性模型自由度變分后驗(yàn)計(jì)算相同，具體過程及結(jié)果參見文獻(xiàn)[2]。上式超參數(shù)遞推可見超參數(shù)更新過程會(huì)出現(xiàn)參數(shù)耦合情況，通過使用設(shè)置初始超參數(shù)值的固定點(diǎn)迭代方法[2]可以解決這個(gè)問題。通過概率分布的期望公式，量測噪聲分布的參數(shù)期望計(jì)算為：

(23)

所提算法一步運(yùn)行過程總結(jié)如下：

魯棒邊緣粒子濾波算法:

2)對每一個(gè)αk|k-1：

(1)從式(6)的建議分布提取非線性狀態(tài)預(yù)測αk|k-1，用(20)預(yù)測超參數(shù)αk|k-1，βk|k-1，

(24)

如果迭代沒終止，令k=k+1, 從步驟1開始重復(fù)上述過程。

3 仿真分析比較

(25)

1)Rk=Rk-1+εk，εk表示量測噪聲方差遞推式的過程噪聲被建模為高斯噪聲，即：

εk～N(0,ζ2)

(26)

ζ2=0.012，單位deg2。

2) 異常時(shí)變:

(27)

實(shí)驗(yàn)通過一次獨(dú)立實(shí)驗(yàn)真實(shí)位置和估計(jì)位置比較，及20次蒙特卡洛仿真均方根誤差來衡量不同算法的性能。均方根誤差(RMSE) 定義式為：

(28)

對于所給量測噪聲方差設(shè)置，此時(shí)標(biāo)準(zhǔn)RBPF算法運(yùn)行經(jīng)常有失效情況發(fā)生，具體為RBPF算法在迭代幾個(gè)時(shí)間步后算法失效，原因是粒子濾波運(yùn)行崩潰，從而導(dǎo)致整個(gè)濾波失效。而所提量測魯棒邊緣粒子濾波算法則始終在每次蒙特卡洛仿真時(shí)正常穩(wěn)健運(yùn)行。

下面將RBPF能正常運(yùn)行情況下x-y面軌跡圖及東向與北向RMSE圖進(jìn)行比較。圖1～2為第一種噪聲情況下與標(biāo)準(zhǔn)RBPF算法比較的RMSE圖及軌跡圖。

圖1 噪聲1下所提算法和RBPF算法估計(jì)軌跡RMSE比較圖

圖2 噪聲1下真實(shí)軌跡與RBPF算法和所提算法軌跡比較

表1 噪聲1下所提算法和RBPF算法目標(biāo)跟蹤性能比較

從位置的具體數(shù)值上可以看到兩種算法對問題描述的目標(biāo)跟蹤問題都有較高的估計(jì)精度，即使所選擇的粒子數(shù)僅為100。而所提算法在X、Y方向的RMSE精度上要略高于所比較的RBPF，說明在此種量測噪聲方差緩慢變化的情況下，所提具有方差自適應(yīng)魯棒邊緣濾波算法更具有狀態(tài)估計(jì)優(yōu)勢。

圖3～4為第二種噪聲情況下兩種算法的東向與北向RMSE及軌跡比較圖。從圖3～4可見，在x軸方向所提算法狀態(tài)估計(jì)精度略高于RBPF算法的精度；對于y軸方向精度可見除個(gè)別時(shí)間點(diǎn)外，所提算法的狀態(tài)估計(jì)精度對已有RBPF算法有較大提高；相比于RBPF位置估計(jì)，所提算法位置估計(jì)更靠近真實(shí)位置。具體數(shù)值如表2所示。

圖4 噪聲2下真實(shí)軌跡與RBPF算法和所提算法估計(jì)軌跡比較

表2 噪聲2下所提算法和RBPF在目標(biāo)跟蹤性能比較

第二種噪聲情況下，即使RBPF算法能夠正常運(yùn)行，所提算法的精度更高，相比于第一種噪聲結(jié)果優(yōu)勢更加明顯，說明第二種噪聲情況下所提算法更具有適用性。

當(dāng)然，如果噪聲方差是恒定不變且事先精確已知的情況下，已有RBPF算法精度略高于所提算法，因?yàn)樗崴惴ǖ姆讲钍怯盟惴▽?shí)時(shí)估計(jì)的方差計(jì)算，而RBPF算法是用精確已知的方差，這也解釋了為什么第二種噪聲情況某小段時(shí)間內(nèi)RBPF精度高于所提算法。

另外雖然所提算法在CV模型中仿真驗(yàn)證，可以驗(yàn)證對常加速度CA的目標(biāo)跟蹤問題也同樣具有優(yōu)異性能。另外所提算法單次仿真時(shí)間近似為RBPF算法單次運(yùn)行時(shí)間的2.5倍。

4 結(jié)束語

由于邊緣粒子濾波在導(dǎo)航和目標(biāo)跟蹤中的不可替代性，本文對現(xiàn)有邊緣粒子濾波無法解決量測噪聲方差未知的問題進(jìn)行研究，通過將學(xué)生t分布加入混合模型，利用Rao-Blackwellised思想分別實(shí)現(xiàn)狀態(tài)條件降維及參數(shù)與狀態(tài)的同時(shí)估計(jì)，由此開發(fā)出一種魯棒邊緣粒子濾波。文中推導(dǎo)了噪聲參數(shù)及含有噪聲參數(shù)狀態(tài)的遞推計(jì)算公式，并把所提算法在典型目標(biāo)跟蹤模型中及設(shè)置的兩種不同量測噪聲下進(jìn)行仿真分析，結(jié)果表明這兩種情況下都具有更優(yōu)越的估計(jì)性能。