基于自適應(yīng)粒子群優(yōu)化的粒子濾波跟蹤算法

林曉杰 索繼東

摘 要： 傳統(tǒng)粒子濾波算法中在重要性采樣部分存在采樣粒子位置不精確的問題，可用粒子群優(yōu)化算法優(yōu)化，但目前的標(biāo)準(zhǔn)粒子群優(yōu)化粒子濾波算法會(huì)出現(xiàn)粒子局部尋優(yōu)的情況。對(duì)此對(duì)算法中的慣性權(quán)重和學(xué)習(xí)因子同時(shí)采取自適應(yīng)調(diào)整的方法，平衡粒子的搜索能力以減少這種情況的出現(xiàn)，并且為了解決算法優(yōu)化后因粒子聚集而造成的多樣性缺失問題，對(duì)粒子進(jìn)行隨機(jī)變異以提高粒子多樣性。仿真結(jié)果表明，經(jīng)過改進(jìn)后的優(yōu)化算法可有效提高粒子濾波算法的準(zhǔn)確性，使跟蹤誤差減小。

關(guān)鍵詞： 粒子濾波跟蹤; 粒子群優(yōu)化; 自適應(yīng)調(diào)整; 搜索能力平衡; 隨機(jī)變異; 優(yōu)化算法

中圖分類號(hào)： TN911.1?34; TP391.9? ? ? ? ? ? ? ? ?文獻(xiàn)標(biāo)識(shí)碼： A? ? ? ? ? ? ? ? ? ?文章編號(hào)： 1004?373X（2020）17?0011?05

Abstract： The problem of inaccurate sampling particle position exists in the important sampling part of the traditional particle filter algorithm， which can be optimized by particle swarm optimization algorithm. However， the current standard particle swarm optimization particle filtering algorithm may lead to particle local optimization. In this paper， self?adaptive adjustment method is adopted to adjust the inertia weight and learning factor in the algorithm， so as to balance the searching ability of particles to reduce the occurrence of this situation. In order to solve the problem of diversity lack caused by particle aggregation after optimization of the algorithm， the particles are randomly mutated to improve the diversity of particles. The simulation results show that the improved optimization algorithm can effectively improve the accuracy of particle filtering algorithm and reduce the tracking error.

Keywords： particle filtering tracking; particle swarm optimization; adaptive adjustment; search capability balance; random variation; optimization algorithm

0? 引? 言

粒子濾波基于蒙特卡羅模擬方法實(shí)現(xiàn)遞推貝葉斯濾波估計(jì)[1]，對(duì)于系統(tǒng)的過程噪聲和量測(cè)噪聲沒有任何的限制，更適合于實(shí)際工程中的應(yīng)用[2]。但是其仍然存在問題，包括采樣粒子不準(zhǔn)確、多樣性缺失等問題[3]。

針對(duì)粒子濾波所采樣的粒子不準(zhǔn)確，造成整個(gè)粒子濾波算法正確性大幅度降低的情況，智能算法可以在一定程度上解決這個(gè)問題。已經(jīng)有學(xué)者將人工魚群算法[4]、粒子群優(yōu)化算法[5]、人工鳥群算法[6]等與粒子濾波算法相結(jié)合，對(duì)采樣粒子進(jìn)行位置上的調(diào)整，用于提高重要性采樣階段采樣粒子的準(zhǔn)確性。除此之外，還有學(xué)者用遺傳算法[7]對(duì)重采樣進(jìn)行改進(jìn)，防止粒子退化，增加粒子多樣性。其中粒子群優(yōu)化算法是通過計(jì)算粒子的適應(yīng)度值來對(duì)粒子位置和速度進(jìn)行調(diào)整，使粒子能夠移動(dòng)到高適應(yīng)度位置的地方以提高算法的準(zhǔn)確率[8]，但是這種算法存在著局部尋優(yōu)的情況，容易發(fā)生跟蹤丟失的情況。

針對(duì)智能算法中粒子群算法優(yōu)化粒子濾波后會(huì)出現(xiàn)的局部尋優(yōu)情況，有學(xué)者提出使用自適應(yīng)慣性權(quán)重的方法解決此類問題[9]。除了慣性權(quán)重外，學(xué)習(xí)因子也通常設(shè)定為常數(shù)[10]，不利于平衡隨時(shí)間變化的搜索能力，因此，在標(biāo)準(zhǔn)粒子群算法中，有學(xué)者對(duì)學(xué)習(xí)因子進(jìn)行自適應(yīng)變化來代替常量提高精度[11]。基于這些方法，本文將粒子群算法中對(duì)學(xué)習(xí)因子進(jìn)行自適應(yīng)這一措施加以調(diào)整后加入到優(yōu)化算法中，然后對(duì)自適應(yīng)權(quán)重的方法也進(jìn)行調(diào)整，通過同時(shí)對(duì)慣性權(quán)重和學(xué)習(xí)因子進(jìn)行自適應(yīng)調(diào)整來提高跟蹤精度。

除此之外，粒子群在調(diào)整位置后會(huì)出現(xiàn)過于集中造成多樣性缺失的情況，針對(duì)這種情況，本文在算法中加入粒子位置變異算法，用以改善粒子過于集中造成的多樣性缺失問題。

1? 基于粒子群優(yōu)化的粒子濾波

粒子濾波算法主要用于解決非線性非高斯情況下的目標(biāo)跟蹤，其利用一組隨機(jī)采樣樣本對(duì)后驗(yàn)概率密度進(jìn)行假設(shè)，用樣本均值代替復(fù)雜的積分運(yùn)算[12]。粒子濾波算法可能因?yàn)椴蓸恿Ｗ拥牟粶?zhǔn)確造成算法性能的低下，因此，采用粒子群優(yōu)化算法對(duì)采樣粒子的位置進(jìn)行調(diào)整以提高目標(biāo)跟蹤的準(zhǔn)確性。基本粒子群優(yōu)化粒子濾波算法的主要步驟如下：

1） 初始化粒子群。隨機(jī)抽取[N]個(gè)粒子。從先驗(yàn)概率密度函數(shù)中隨機(jī)抽取粒子組成采樣粒子集合[{Xn，n=1，2，…，N}]，并且隨機(jī)賦予這些粒子初始速度為[V1，V2，…，Vn]。

2） 對(duì)抽取的粒子利用粒子群優(yōu)化算法來優(yōu)化粒子位置。

評(píng)價(jià)粒子群，計(jì)算粒子的適應(yīng)度值[P（y）]，計(jì)算公式為：

式中：[R]為量測(cè)噪聲方差;[zn]為粒子的量測(cè)值;[znn-1]為量測(cè)預(yù)測(cè)值。

設(shè)第[i]個(gè)粒子的位置為[{xid，d=1，2，…，D}]，速度為[{vid，d=1，2，…，D}]。令[Pid]為粒子[i]個(gè)體最優(yōu)位置;[Pgd]為群體中所有粒子全局最優(yōu)位置[13]。將粒子的當(dāng)前適應(yīng)度值與此粒子的個(gè)體最優(yōu)解[Pid]的適應(yīng)度值進(jìn)行比較，若更優(yōu)，則令此粒子位置為個(gè)體當(dāng)前最優(yōu)位置。對(duì)于每個(gè)粒子每次計(jì)算得到的適應(yīng)度值，都與全局最優(yōu)解[Pgd]進(jìn)行比較，如果大于[Pgd]的適應(yīng)度值，則將所對(duì)應(yīng)的粒子位置調(diào)整為全局最優(yōu)位置。按式（2），式（3）更新每個(gè)粒子的速度和位置以產(chǎn)生新的種群。

在式（2），式（3）中：[1≤i≤n]，[1≤d≤D];[vid（t）]表示第[t]次循環(huán)粒子的飛行速度;[xid（t）]表示第[t]次循環(huán)粒子[i]所在的位置;[Pid]表示經(jīng)過[t]次循環(huán)后第[i]個(gè)粒子的個(gè)體最優(yōu)位置;[Pgd]表示經(jīng)過[t]次循環(huán)后粒子的群體最優(yōu)位置;[w（t）]為慣性權(quán)重;[c1]和[c2]為學(xué)習(xí)因子。不斷循環(huán)此步驟直到滿足迭代次數(shù)，此時(shí)更新后的位置為粒子的最終位置。

3） 權(quán)值計(jì)算

通過得到的最新量測(cè)信息對(duì)粒子的權(quán)值進(jìn)行計(jì)算，由貝葉斯準(zhǔn)則可以得到權(quán)值的更新公式：

權(quán)值計(jì)算更新后，需要對(duì)權(quán)值進(jìn)行歸一化計(jì)算，公式為：

4） 重采樣

通過計(jì)算算法中的有效粒子數(shù)[Neff]的個(gè)數(shù)，將其與所設(shè)定的有效粒子數(shù)閾值[Nth]進(jìn)行比較，判斷是否需要進(jìn)行重采樣。

若[Neff

5） 狀態(tài)輸出

2? 自適應(yīng)優(yōu)化策略

在粒子群優(yōu)化算法中，可以對(duì)粒子的參數(shù)進(jìn)行自適應(yīng)優(yōu)化，慣性權(quán)重和學(xué)習(xí)因子的自適應(yīng)就是其中的一種方法[14]。對(duì)兩者進(jìn)行自適應(yīng)調(diào)整可以減少由于局部尋優(yōu)造成的準(zhǔn)確性降低的問題。文獻(xiàn)[9]提出了自適應(yīng)權(quán)重調(diào)整的粒子群優(yōu)化粒子濾波算法，降低發(fā)生局部尋優(yōu)情況的概率。本文在此基礎(chǔ)上對(duì)粒子群優(yōu)化粒子濾波算法進(jìn)行慣性權(quán)重和學(xué)習(xí)因子兩方面的自適應(yīng)，并且使用變異算法以提高多樣性。

2.1? 慣性權(quán)重自適應(yīng)

在粒子群優(yōu)化算法中，通常慣性權(quán)重為一個(gè)常數(shù)。慣性權(quán)重較大，表示粒子有較好的全局搜索能力，較小則更有利于局部搜索。若慣性權(quán)重保持不變，粒子的搜索能力不變，會(huì)造成局部尋優(yōu)情況[14]。針對(duì)這種情況，可以對(duì)慣性權(quán)重根據(jù)適應(yīng)度值進(jìn)行自適應(yīng)調(diào)節(jié)。當(dāng)某一點(diǎn)的適應(yīng)度值小于適應(yīng)度均值時(shí)，可以將此點(diǎn)認(rèn)為是遠(yuǎn)離真實(shí)值的點(diǎn)或者陷入局部尋優(yōu)情況的點(diǎn)，對(duì)其慣性權(quán)重進(jìn)行自適應(yīng)調(diào)節(jié)，使其能夠向更優(yōu)方向移動(dòng)，具體優(yōu)化方法如下：

令[w（t）]為自適應(yīng)慣性權(quán)重，慣性權(quán)重的計(jì)算方式按式（8）方法進(jìn)行改進(jìn)：

式中：[fmin]為粒子適應(yīng)度最小值;[favg]為當(dāng)前所有粒子的適應(yīng)度均值;[F]為當(dāng)前適應(yīng)度值;[wmax]和[wmin]分別為設(shè)定的慣性權(quán)重最大值和最小值;iter表示粒子的最大迭代次數(shù);[t]表示當(dāng)前的迭代次數(shù)。若[Ffavg]，認(rèn)為粒子位置較好，文獻(xiàn)[9]中設(shè)定慣性權(quán)重為一個(gè)常數(shù)來表示。本文如式（8）所示，用改進(jìn)的線性變化來調(diào)整慣性權(quán)重[14]，讓慣性權(quán)重隨迭代次數(shù)減小，使其前期有較好的全局搜索能力，后期有較好的局部搜索能力。

計(jì)算得到慣性權(quán)重后，通過其對(duì)相應(yīng)的粒子進(jìn)行位置和速度的調(diào)整。每個(gè)粒子的速度和位置按照式（2）和式（3）進(jìn)行迭代，從而提高粒子位置的準(zhǔn)確性。

2.2? 學(xué)習(xí)因子自適應(yīng)

在標(biāo)準(zhǔn)粒子群算法中，學(xué)習(xí)因子[c1]和[c2]通常被設(shè)定為一個(gè)常數(shù)。其中，學(xué)習(xí)因子[c1]表示自我認(rèn)知，即對(duì)自身的調(diào)整;學(xué)習(xí)因子[c2]表示群體認(rèn)知，即對(duì)整個(gè)種群的信息交流。[c1]的大小決定了粒子局部范圍搜索的能力，[c2]的大小決定粒子收斂到最優(yōu)值的速度[14]，通過對(duì)[c1]和[c2]的調(diào)節(jié)能夠控制粒子向最優(yōu)位置移動(dòng)的能力。

在算法初期，提高粒子向全局最優(yōu)位置移動(dòng)的概率，避免陷入局部最優(yōu)位置。而到了粒子后期，則希望能夠加強(qiáng)粒子移向最優(yōu)位置的速度，獲得高質(zhì)量的粒子。

[c1]和[c2]設(shè)定為一常數(shù)。在優(yōu)化算法中，令學(xué)習(xí)因子[c1]隨著迭代次數(shù)的增加而不斷增加，相反，學(xué)習(xí)因子[c2]則隨著迭代次數(shù)減小。其變化公式為：

式中：iter表示粒子的最大迭代次數(shù);[t]表示當(dāng)前的迭代次數(shù)。通過這個(gè)自適應(yīng)算法，能夠動(dòng)態(tài)調(diào)整兩個(gè)學(xué)習(xí)因子的數(shù)值，提高了粒子向最優(yōu)位置移動(dòng)的能力。兩者的相互配合可以使粒子快速收斂到全局最優(yōu)位置，從而提高算法的收斂精度。

2.3? 變異操作

為了提高粒子的多樣性，引入了變異操作。設(shè)定一個(gè)隨機(jī)函數(shù)rand，再設(shè)定一個(gè)閾值，對(duì)此隨機(jī)函數(shù)進(jìn)行判斷，當(dāng)此隨機(jī)函數(shù)超過某一個(gè)閾值時(shí)，對(duì)粒子進(jìn)行變異操作，即隨機(jī)調(diào)整該粒子的位置，避免粒子過于集中，提高粒子的多樣性，位置變動(dòng)公式如下：

式中[Rmax]為粒子的最大追蹤范圍。

3? 改進(jìn)的自適應(yīng)粒子群優(yōu)化粒子濾波算法

根據(jù)上面的內(nèi)容，基于自適應(yīng)粒子群優(yōu)化的粒子濾波算法的步驟可總結(jié)如下：

1） 初始化。初始化各個(gè)參數(shù)，包括測(cè)量噪聲協(xié)方差、過程狀態(tài)協(xié)方差、模擬步長(zhǎng)、粒子數(shù)、進(jìn)化代數(shù)、慣性權(quán)重最大最小值等。

2） 重要性采樣，從重要性概率密度函數(shù)中采樣新的粒子[xkiNi=1]。

3） 進(jìn)行粒子群優(yōu)化。將所采樣粒子作為初代粒子對(duì)其進(jìn)行粒子群優(yōu)化。在第一代中，粒子的速度由隨機(jī)函數(shù)獲取，位置為粒子濾波算法中所提取的粒子的位置，得到此時(shí)粒子的速度以及位置。

利用式（1）計(jì)算粒子的適應(yīng)度，通過比較當(dāng)前的適應(yīng)度值和適應(yīng)度均值來判斷如何改變慣性權(quán)重，利用式（8）進(jìn)行改變，同時(shí)更新個(gè)體最優(yōu)解和全局最優(yōu)解。學(xué)習(xí)因子由式（9）和式（10）計(jì)算得到后，按式（2）和式（3）來更新每個(gè)粒子的速度和位置，根據(jù)式（11）對(duì)滿足要求的粒子進(jìn)行變異操作。

不斷循環(huán)此步驟直到滿足進(jìn)化代數(shù)達(dá)到最大迭代次數(shù)或者全局最優(yōu)解[Pgd]達(dá)到理想的改進(jìn)精度，此時(shí)粒子的位置為更新后的位置。

4） 權(quán)值計(jì)算，將經(jīng)過粒子群優(yōu)化算法調(diào)整后的粒子取出。利用式（4）計(jì)算其權(quán)重，并且歸一化權(quán)重。

5） 重采樣，利用式（6）計(jì)算粒子的有效粒子數(shù)，若有效粒子數(shù)達(dá)不到閾值要求，則需要對(duì)粒子進(jìn)行重采樣以得到新的粒子集，新的粒子集權(quán)重均為[1N]。若有效粒子數(shù)大于閾值，則進(jìn)行下一步。

6） 狀態(tài)輸出。

4? 仿真分析

針對(duì)上述部分所提到的方法與步驟進(jìn)行仿真分析。本文所進(jìn)行的仿真實(shí)驗(yàn)都是在Matlab R2016a的環(huán)境下進(jìn)行仿真。所選用的系統(tǒng)模型和狀態(tài)模型表示為：

式中：rand（）為0～1的隨機(jī)數(shù);[R]為量測(cè)噪聲方差;[Q]是過程噪聲方差。

為了驗(yàn)證算法的性能，將標(biāo)準(zhǔn)粒子濾波算法（PF）、文獻(xiàn)[9]調(diào)整慣性權(quán)重的粒子群優(yōu)化粒子濾波算法（PSO?PF）和本文改進(jìn)的自適應(yīng)粒子群優(yōu)化粒子濾波算法（APSO?PF）進(jìn)行比較。比較的標(biāo)準(zhǔn)為均方誤差（Mean?Square Error，MSE），其計(jì)算公式如下：

粒子數(shù)設(shè)定為50，迭代次數(shù)設(shè)置為100次，粒子的最大飛行速度設(shè)置為1 m/s，最大飛行距離設(shè)定為20 m。

PSO?PF算法中的慣性權(quán)重采用文獻(xiàn)[9]所使用的方法進(jìn)行變換，學(xué)習(xí)因子[c1]和[c2]均設(shè)定為2。

本文改進(jìn)的APSO?PF算法中，慣性權(quán)重最大值和最小值[wmax]和[wmin]分別設(shè)定為0.9和0.4。慣性權(quán)重按照前面所提到的式（8）進(jìn)行判斷和改變。學(xué)習(xí)因子[c1]和[c2]按照式（9）和式（10）進(jìn)行自適應(yīng)變化，并進(jìn)行變異操作。

在相同的情況下，對(duì)隨機(jī)抽取的同一組粒子，分別使用粒子濾波算法、調(diào)整權(quán)重的粒子群優(yōu)化粒子濾波算法和本文的改進(jìn)算法對(duì)目標(biāo)進(jìn)行跟蹤。圖1，圖2分別是量測(cè)噪聲方差[R=1]、過程噪聲方差[Q=1]和量測(cè)噪聲方差[R=1]、過程噪聲方差[Q=10]的情況下的跟蹤效果圖。

仿真圖給出了粒子的真實(shí)值和各種方法得到的粒子狀態(tài)值。通過仿真圖可以看出，標(biāo)準(zhǔn)粒子濾波算法所得到的預(yù)測(cè)目標(biāo)位置與真實(shí)目標(biāo)位置相差較大，調(diào)整慣性權(quán)重的粒子群優(yōu)化粒子濾波算法精度居中，而本文的改進(jìn)算法能進(jìn)一步提升目標(biāo)跟蹤的精度。在大部分情況下，改良算法都能較好地跟蹤目標(biāo)，其得到的目標(biāo)位置大多都在真實(shí)位置附近，改良后的算法對(duì)目標(biāo)的跟蹤更為準(zhǔn)確。

除此之外，對(duì)三種算法均方誤差的計(jì)算中也可以驗(yàn)證改良算法的性能更好。其誤差圖如圖3，圖4所示。

從圖3，圖4中可以看出：標(biāo)準(zhǔn)粒子濾波算法的誤差最大，調(diào)整慣性權(quán)重的粒子群優(yōu)化粒子濾波算法的誤差有明顯減小，而本文改進(jìn)的算法誤差最小。

計(jì)算三種算法的均方誤差，其結(jié)果如表1所示。

從仿真實(shí)驗(yàn)計(jì)算得到的平均值可以看出，本文改進(jìn)的自適應(yīng)粒子群優(yōu)化粒子濾波算法得到的均方誤差值最小。因此可以證實(shí)，對(duì)粒子濾波算法所提取的粒子利用改進(jìn)的自適應(yīng)粒子群優(yōu)化算法進(jìn)行位置調(diào)整，可以有效地提高算法的準(zhǔn)確性。

5? 結(jié)? 語

本文在粒子群優(yōu)化算法中引入自適應(yīng)慣性權(quán)重以及學(xué)習(xí)因子，以減少基于粒子群優(yōu)化的粒子濾波算法中出現(xiàn)局部尋優(yōu)情況的可能性，同時(shí)，對(duì)移動(dòng)后的粒子進(jìn)行隨機(jī)變異以提高多樣性。仿真結(jié)果表明，改進(jìn)后的基于自適應(yīng)慣性權(quán)重和學(xué)習(xí)因子的粒子群優(yōu)化粒子濾波算法能夠減小跟蹤誤差，提高濾波性能。

參考文獻(xiàn)

[1] GORDON N J， SALMOND D J， SMITH A F M. Novel approach to nonlinear/non?Gaussian Bayesian state estimation [J]. IEEE proceedings F： radar and signal processing， 1993， 140（2）： 107?113.

[2] 劉暢，楊鎖昌，汪連棟.粒子濾波理論在單目標(biāo)跟蹤中的應(yīng)用綜述[J].飛航導(dǎo)彈，2017（10）：67?71.

[3] 昝孟恩，周航，韓丹，等.粒子濾波目標(biāo)跟蹤算法綜述[J].計(jì)算機(jī)工程與應(yīng)用，2019，55（5）：8?17.

[4] LI Min， CHEN Zhimei， SHAO Xuejuan. Iterated unscented Kalman particle filter algorithm based on a variable?step adaptive artificial fish swarm optimization [C]// 2016 35th Chinese Control Conference （CCC）. Chengdu， China： IEEE， 2016： 7050?7053.

[5] 方正，佟國(guó)峰，徐心和.基于粒子群優(yōu)化的粒子濾波定位方法[J].控制理論與應(yīng)用，2008，25（3）：533?537.

[6] ZHANG Liang， BAO Qilian， FAN Wenxiu， et al. An improved particle filter based on bird swarm algorithm [C]// 2017 10th International Symposium on Computational Intelligence and Design （ISCID）. Hangzhou， China： IEEE， 2017： 198?203.

[7] SUN Chengyuan， HOU Jian， SUN Wei， et al. An adaptive intelligent particle filter for state estimation [C]// 2017 36th Chinese Control Conference （CCC）. Dalian， China： IEEE， 2017： 7009?7014.

[8] 張才千，葛磊，韓東.基于目標(biāo)跟蹤的粒子群粒子濾波算法研究[J].計(jì)算機(jī)仿真，2014，31（8）：392?396.

[9] 劉峰，韓艷麗，王鐸.自適應(yīng)權(quán)重粒子群優(yōu)化的粒子濾波算法[J].計(jì)算機(jī)仿真，2013，30（11）：330?333.

[10] KENNEDY J， EBERHART R， SHI Yuhui. Swarm intelligence [M]. San Francisco， USA： Morgan Kaufmann Publishers， 2001.

[11] 仝秋娟，趙豈，李萌.基于自適應(yīng)動(dòng)態(tài)改變的粒子群優(yōu)化算法[J].微電子學(xué)與計(jì)算機(jī)，2019，36（2）：6?10.

[12] ARULAMPALAM M S， MASKELL S， GORDON N， et al. A tutorial on particle filters for online nonlinear/non?Gaussian Bayesian tracking [J]. IEEE transactions on signal processing， 2002， 50（2）： 174?188.

[13] 高國(guó)棟，林明，許蘭.基于似然分布調(diào)整的粒子群優(yōu)化粒子濾波新方法[J].計(jì)算機(jī)應(yīng)用，2017，37（4）：980?985.

[14] 沈顯君.自適應(yīng)粒子群優(yōu)化算法及其應(yīng)用[M].北京：清華大學(xué)出版社，2015.