基于蟻群優(yōu)化的邊緣化粒子濾波器跟蹤算法

曹義親，肖金勝，黃曉生

(1. 華東交通大學(xué) 軟件學(xué)院，江西 南昌 330013; 2. 華東交通大學(xué) 信息工程學(xué)院, 江西 南昌 330013)

機(jī)動(dòng)目標(biāo)跟蹤在軍事和民用領(lǐng)域均有著廣泛的應(yīng)用[1-3]。由于在系統(tǒng)動(dòng)態(tài)模型為線性且噪聲是高斯分布的條件下,傳統(tǒng)的Kalman濾波可獲得最優(yōu)解[4]，因此，基于傳統(tǒng)Kalman濾波器進(jìn)行目標(biāo)跟蹤是最為常用的機(jī)動(dòng)目標(biāo)跟蹤方法。然而在實(shí)際工程條件中，系統(tǒng)動(dòng)態(tài)模型常常為非線性、非高斯的，傳統(tǒng)的Kalman濾波難以很好地解決這類(lèi)問(wèn)題[5-6]。由Gordon等[7]完成的粒子濾波采用隨機(jī)采樣的方法獲取粒子，然后用大量粒子來(lái)近似、逼近狀態(tài)后驗(yàn)概率密度分布的方法很好地解決了這個(gè)問(wèn)題，它在動(dòng)態(tài)模型為非線性、非高斯的條件下對(duì)機(jī)動(dòng)目標(biāo)跟蹤的性能要遠(yuǎn)好于Kalman濾波[8]。該算法最大的問(wèn)題是時(shí)間復(fù)雜度高，并存在重要的采樣粒子退化以及重采樣粒子貧乏等問(wèn)題[9]。對(duì)此，許多學(xué)者采用改進(jìn)的或者新型的粒子濾波算法。比如，由Thomas Schoh等[10]提出的邊緣粒子濾波算法(marginalized particle filter MPF) 采用Kalman濾波處理速度和加速度等線性狀態(tài)，用粒子濾波器處理位置等非線性狀態(tài)，這種方法綜合了Kalman濾波與粒子濾波的優(yōu)點(diǎn)，近年來(lái)逐漸受到了越來(lái)越多的關(guān)注。然而用Kalman濾波處理速度和加速度等線性狀態(tài)時(shí)由于目標(biāo)的量測(cè)方程一般不包含任何線性變量信息，只包含非線性變量信息，因此在量測(cè)方程中無(wú)法得到Kalman濾波的量測(cè)更新。另外，用粒子濾波器處理位置等非線性狀態(tài)時(shí)仍然存在計(jì)算復(fù)雜、重要的采樣粒子退化以及重采樣粒子貧乏的現(xiàn)象。

為此，本文提出用狀態(tài)的預(yù)測(cè)值作為Kalman濾波器的量測(cè)更新，從而獲得在線性系統(tǒng)條件下Kalman的最優(yōu)濾波，用蟻群優(yōu)化群(ant colony optimization particle filter,AOPF)算法來(lái)改進(jìn)傳統(tǒng)粒子濾波的重采樣過(guò)程，以增加較小的計(jì)算復(fù)雜度為代價(jià)獲得更高的計(jì)算精度。

1 相關(guān)理論基礎(chǔ)

1.1 蟻群算法

(1)

其中:allowedk={0.1…(n-1)}，為螞蟻k下一步允許選擇的路徑；τij(t)為在t時(shí)刻的信息素軌跡強(qiáng)度；ηij為能見(jiàn)度因素，反映了由路徑i到路徑j(luò)的啟發(fā)程度，該量在螞蟻系統(tǒng)的運(yùn)行中不變；α和β為2個(gè)參數(shù)，分別反映螞蟻在運(yùn)動(dòng)多次中所積累的信息和啟發(fā)信息在螞蟻選擇路徑中的相對(duì)重要性。

經(jīng)過(guò)n個(gè)時(shí)刻，螞蟻完成一次循環(huán)，各條路徑上信息素量根據(jù)下式調(diào)整:

τij(t+1)=ρτij(t+1)+Δτij(t,t+1)

(2)

(3)

1.2 MPF算法

MPF也稱(chēng)為Rao-Blackwellised粒子濾波[12]，其主要思想是用Kalman濾波協(xié)助粒子濾波來(lái)完成濾波，即把狀態(tài)變量中的線性部分和非線性部分分離，然后用Kalman濾波處理線性部分，用粒子濾波器處理非線性部分。由于實(shí)際生活中的機(jī)動(dòng)目標(biāo)的運(yùn)動(dòng)大多數(shù)是非線性的，為此本文采用非線性模型(見(jiàn)式(4))來(lái)對(duì)問(wèn)題進(jìn)行描述。

(4)

其中yt是t時(shí)刻的量測(cè)；xt是系統(tǒng)變量；wt是過(guò)程噪聲；et是閃爍噪聲;f、h是2個(gè)非線性函數(shù)。

(5)

(6)

(7)

2 基于蟻群算法的改進(jìn)邊緣化粒子濾波

2.1 算法思想

MPF是應(yīng)用粒子濾波處理狀態(tài)方程和量測(cè)方程中位置等非線性狀態(tài)，用Kalman濾波處理狀態(tài)方程和量測(cè)方程中速度和加速度分量等線性狀態(tài)。狀態(tài)方程中包含線性狀態(tài)分量和非線性狀態(tài)，但是目標(biāo)的量測(cè)方程一般不包含任何線性變量信息，只包含非線性變量信息，在量測(cè)方程中無(wú)法得到Kalman濾波的量測(cè)更新。為此改進(jìn)的MPF，用狀態(tài)的預(yù)測(cè)值作為Kalman濾波的量測(cè)更新，處理線性狀態(tài)(速度和加速度)以便得到更好的線性狀態(tài)估計(jì)。標(biāo)準(zhǔn)粒子濾波算法采用重采樣方法來(lái)防止粒子貧化。具體方法是淘汰權(quán)值較小的粒子，復(fù)制權(quán)值較大的粒子。經(jīng)過(guò)多次迭代后，將造成粒子缺乏多樣性，若通過(guò)蟻群優(yōu)化的方式，使權(quán)值較小的粒子向粒子的最優(yōu)位置移動(dòng)，而不是直接復(fù)制權(quán)值較大粒子，同時(shí)保持權(quán)值較大的粒子位置不變，將使粒子具有較好的分布，保持粒子的多樣性。

新的改進(jìn)的MPF粒子濾波目標(biāo)跟蹤算法通過(guò)用狀態(tài)的預(yù)測(cè)值作為Kalman濾波器的量測(cè)更新，處理線性狀態(tài)，用蟻群優(yōu)化粒子的思想改進(jìn)粒子濾波器，處理非線性狀態(tài)。

2.2 算法步驟

算法的具體步驟描述為：

Step1:初始化。

Step2:重要性采樣。

fori=1,2,…,N,

(8)

歸一化

(9)

Step3：蟻群優(yōu)化粒子群算法濾波的量測(cè)更新(重采樣)

為了重采樣過(guò)程中使每個(gè)粒子都可以找到較高質(zhì)量的解，并使粒子具有較好的后驗(yàn)概率分布，定義粒子轉(zhuǎn)移概率pij(t)來(lái)表示t時(shí)刻粒子i向粒子j轉(zhuǎn)移的概率，有

(10)

(11)

Step4：蟻群優(yōu)化粒子濾波的時(shí)間更新和擴(kuò)展Kalman濾波時(shí)間更新。

① Kalman濾波的量測(cè)更新:

(12)

pt|t=pt|t-1

(13)

② 蟻群優(yōu)化粒子濾波的時(shí)間更新:

fori=1,2,…,N預(yù)測(cè)新的粒子

(14)

③ 根據(jù)式(15)—式(18)進(jìn)行Kalman濾波的時(shí)間更新：

(15)

(16)

(17)

(18)

當(dāng)前，我國(guó)最常使用的仍然是傳統(tǒng)項(xiàng)目管理，傳統(tǒng)的項(xiàng)目管理包括CM管理模式、三角形管理模式、設(shè)計(jì)建造模式和BOT管理模式。傳統(tǒng)的項(xiàng)目管理模式主要側(cè)重點(diǎn)是在項(xiàng)目工期、造價(jià)、質(zhì)量，沒(méi)有包涵集成、范圍、風(fēng)險(xiǎn)管理，因此，傳統(tǒng)項(xiàng)目管理的各個(gè)門(mén)類(lèi)相互之間沒(méi)有聯(lián)系，并且管理模式不系統(tǒng)和缺乏針對(duì)性，所以導(dǎo)致傳統(tǒng)項(xiàng)目管理效用不高[1]。但是，在傳統(tǒng)企業(yè)的運(yùn)營(yíng)中，通過(guò)對(duì)企業(yè)運(yùn)營(yíng)的項(xiàng)目管理可以提高傳統(tǒng)企業(yè)的工作效率，并且提高企業(yè)對(duì)市場(chǎng)環(huán)境的應(yīng)對(duì)。傳統(tǒng)項(xiàng)目管理過(guò)去企業(yè)普遍采用的管理模式，其特點(diǎn)是在經(jīng)濟(jì)不樂(lè)觀的情況下，會(huì)給企業(yè)帶來(lái)額外收益。傳統(tǒng)的項(xiàng)目管理模式對(duì)一些簡(jiǎn)單的項(xiàng)目有著一定效果，其理論研究也只能停留在較低層面。

Step5:t=t+1;重復(fù)步驟Step2。

3 仿真與分析

3.1 仿真背景

目前對(duì)于機(jī)動(dòng)目標(biāo)建模的方法主要有兩類(lèi)：一類(lèi)是基于直線運(yùn)動(dòng)的機(jī)動(dòng)模型，它假設(shè)目標(biāo)作直線運(yùn)動(dòng)，主要模型包含勻加速模型、當(dāng)前統(tǒng)計(jì)模型以及急動(dòng)模型等；二類(lèi)是假設(shè)目標(biāo)作圓周運(yùn)動(dòng)，主要包括圓周模型、弧線模型和協(xié)同轉(zhuǎn)彎模型等。當(dāng)前統(tǒng)計(jì)模型和協(xié)同轉(zhuǎn)彎模型能夠較真實(shí)地反映目標(biāo)的機(jī)動(dòng)范圍和機(jī)動(dòng)強(qiáng)度的變化，故文章采用協(xié)同轉(zhuǎn)彎模型進(jìn)行仿真與分析。

3.2 仿真模型

假設(shè)目標(biāo)跟蹤模型為

xt+1=Axt+Bwt

(19)

yt=h(xt)+et

(20)

實(shí)際測(cè)量得到的是目標(biāo)距離r、方位α和俯仰角e,其與x、y、z之間的關(guān)系分別為：

(21)

三維目標(biāo)狀態(tài)變量為

wt服從N(0,Qt),et可以任意分布。狀態(tài)變量包含3個(gè)方向位置，即非線性信息，6個(gè)弱非線性(線性)信息，即各個(gè)方向的速度和加速度。式(17)是非線性的Kalman濾波,跟蹤估計(jì)誤差大，所以線性部分：

為了比較算法的性能，采用位置均方根RMSE作為衡量標(biāo)準(zhǔn)，有

(22)

假設(shè)三維目標(biāo)的運(yùn)動(dòng)軌跡:以初始位置(1.25×105m、1.0×105m、4 000 m)和初始速度為(-100 m/s，-100 m/s,0 m/s)做勻速直線運(yùn)動(dòng)25 s后，以角速度4.77 rad/s作勻速圓周運(yùn)動(dòng)20 s；再勻速直線運(yùn)動(dòng)15 s后，以角速度5.06 rad/s作勻速圓周運(yùn)動(dòng)25 s；再勻速直線運(yùn)動(dòng)15 s后，以角速度6 rad/s作勻速圓周運(yùn)動(dòng)25 s，最后作勻速直線運(yùn)動(dòng)25 s。

3.3 仿真結(jié)果與分析

本文采用2種方法跟蹤該機(jī)動(dòng)目標(biāo):粒子數(shù)為600的蟻群優(yōu)化粒子濾波以及粒子數(shù)為400的改進(jìn)MPF,進(jìn)行100次仿真實(shí)驗(yàn)。圖1—圖3分別是2種跟蹤算法在x,y,z方向的誤差，圖4是2種算法跟蹤位置的誤差。從圖1—圖4可以看出，兩種方法都可以對(duì)機(jī)動(dòng)目標(biāo)跟蹤，但改進(jìn)的MPF的跟蹤精度要比蟻群優(yōu)化粒子濾波高得多。

圖1 x方向的誤差

圖2 y方向的誤差

圖3 z方向的誤差

圖4 位置誤差

由表1可知，通過(guò)增加粒子數(shù)可以提高蟻群優(yōu)化粒子濾波器的跟蹤精度，但是是以增加計(jì)算復(fù)雜度為代價(jià)，而MPF以較少的粒子就獲得較好的跟蹤效果。這說(shuō)明改進(jìn)后的MPF在提高目標(biāo)狀態(tài)估計(jì)精度的同時(shí)，降低了粒子濾波的計(jì)算復(fù)雜度，克服了粒子濾波退化現(xiàn)象。

表1 2種跟蹤算法100次仿真時(shí)間/s

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

一般的MPF算法中Kalman濾波器通常無(wú)法從量測(cè)方程中獲得線性狀態(tài)估計(jì),標(biāo)準(zhǔn)粒子濾波估計(jì)非線性狀態(tài)存在著計(jì)算量大和粒子退化的問(wèn)題。對(duì)此，本文采用狀態(tài)的預(yù)測(cè)值作為Kalman粒子濾波的量測(cè)更新，用來(lái)處理線性狀態(tài)變量；用蟻群優(yōu)化粒子群方法改進(jìn)標(biāo)準(zhǔn)粒子濾波的重采樣過(guò)程，用來(lái)處理非線性狀態(tài)變量。仿真結(jié)果表明:改進(jìn)后的MPF在提高目標(biāo)狀態(tài)估計(jì)精度的同時(shí)，降低了粒子濾波的計(jì)算復(fù)雜度，克服了粒子濾波退化現(xiàn)象。

[1] 王立紅,郭鑫.艦炮武器機(jī)動(dòng)目標(biāo)跟蹤技術(shù)和攻擊技術(shù)分析[J].艦船電子工程,2013,33(2):35-37.

[2] 方彥軍,周婷婷,譚磊.基于交互式多模型算法的無(wú)人車(chē)輛跟蹤預(yù)測(cè)技術(shù)[J].武漢大學(xué)學(xué)報(bào),2013,46(4):540-544.

[3] 孫寧,王壽峰,白俊奇,等.實(shí)時(shí)紅外目標(biāo)跟蹤方法[J].電光與控制,2012,19(10)：25-29.

[4] 王敏.雷達(dá)目標(biāo)跟蹤的轉(zhuǎn)換坐標(biāo)卡爾曼濾波算法[J].科技創(chuàng)新導(dǎo)報(bào),2013(1):15-16.

[5] 辛英,于靜.自適應(yīng)卡爾曼濾波算法改進(jìn)與仿真[J].中國(guó)科技信息,2011(22):59-60.

[6] 崔波,張家樹(shù)，楊宇.基于卡爾曼濾波的非線性目標(biāo)跟蹤算法[J] .計(jì)算機(jī)仿真,2013,30(4):317-321.

[7] Gordon N,Salmond D. Novel approach to non-linear and non-gaussian Bayesian state estimation[J]. Proceedings of Institute Electric Engineering,1993,140(2):107-113.

[8] 沈振.粒子濾波算法研究及其在機(jī)動(dòng)目標(biāo)跟蹤中的應(yīng)用[D]. 成都：電子科技大學(xué)，2012.

[9] 胡士強(qiáng),敬忠良.粒子濾波算法綜述[J].控制與決策，2005,20(4):361-365.

[10] Thomas Schoh,Fredrik Gustafaason,per-Johan Nordlund. Marginalized particle Filters for Mixed Linear/Nolinear State-space Models[J].IEEE Trans on Signal Processing,2005,53(7):2279-2289.

[11] 曹義親,鐘濤,黃曉生.一種改進(jìn)的基于蟻群優(yōu)化的粒子濾波算法[J].計(jì)算機(jī)應(yīng)用研究，2013,30(8)：2402-2404.

[12] 朱志宇.基于改進(jìn)邊緣化粒子濾波器的機(jī)動(dòng)目標(biāo)跟蹤[J].武漢理工大學(xué)學(xué)報(bào)，2008,30(6)：118-121.