基于模擬退火粒子群優(yōu)化的粒子濾波算法

江蘇科技大學(xué)電子信息學(xué)院 劉 浩 楊官校 吳 將

1.引言

近年來，粒子濾波方法在國內(nèi)外備受關(guān)注，與傳統(tǒng)濾波方法相比，該方法具有簡單易行，適用于非線性及非高斯噪聲環(huán)境等優(yōu)點(diǎn)，因而被廣泛應(yīng)用于視覺跟蹤、機(jī)器人定位、航空導(dǎo)航、圖象處理、故障檢測、工業(yè)控制等諸多領(lǐng)域。[1-4]

粒子濾波(PF)的基本思想是先在狀態(tài)空間中根據(jù)先驗分布產(chǎn)生一組隨機(jī)樣本集合，這些樣本被稱為粒子，然后在觀測的基礎(chǔ)上，通過調(diào)節(jié)每一樣本所對應(yīng)權(quán)值的大小和樣本的位置，來獲得服從實際分布的樣本，并以這些樣本的均值作為系統(tǒng)狀態(tài)估計值。

粒子濾波方法采用帶有權(quán)重值的粒子集來近似表示后驗概率分布，因此，理論上該方法可以表示任意形式的概率分布，然而，因為常規(guī)粒子濾波方法采用了次優(yōu)的重要性函數(shù)，所以常規(guī)粒子濾波方法存在粒子貧乏和計算效率等缺點(diǎn)，為解決以上問題，國內(nèi)外不少學(xué)者提出了改進(jìn)方法。

Rudolph等[5]將UKF方法引入粒子濾波中，提出了無味粒子濾波算法(UPF)，其核心思想是利用UKF得到比常規(guī)PF更好的重要性函數(shù)，該方法將最新的觀測值引入預(yù)測過程中，因此提高了常規(guī)粒子濾波的性能，但計算量也大大增加了；Clapp等[6]將模擬退火思想引入粒子濾波中，提出了模擬退火粒子濾波(SA-PF)，該算法引入退火重要性采樣和中間分布的概念，改善了出現(xiàn)先驗尾部觀測值時的算法性能；方正等[7]提出了粒子群優(yōu)化粒子濾波方法（PSO-PF），將粒子群優(yōu)化算法引入粒子濾波方法中，利用粒子群(PSO)算法對PF的重要性采樣進(jìn)行優(yōu)化將最新的觀測值引入重要性采樣分布中，使得重要性采樣分布向后驗概率較高的區(qū)域運(yùn)動，提高了狀態(tài)預(yù)估的精度，然而該方法仍然存在重采樣過程帶來的粒子缺乏多樣性問題。

本文提出了基于模擬退火粒子群優(yōu)化的粒子濾波算法(SAPSO-PF：Simulated Annealing Particle Swarm Optimized Particle Filter)，該方法基于一個高斯分布來不斷更新粒子的速度，同時采用隨機(jī)概率擾動的方式作為基本粒子群算法的全局極值更新條件，從而增加全局最優(yōu)區(qū)域的搜索能力，避免了粒子過早的“趨同性”，使得粒子濾波的性能得到很大的提高。

2.基于模擬退火粒子群優(yōu)化的粒子濾波算法

2.1 高斯粒子群優(yōu)化粒子濾波

常規(guī)的粒子濾波采用了次優(yōu)的重要性函數(shù)，因此，粒子的重要性采樣過程是次優(yōu)的。為了優(yōu)化粒子濾波的采樣過程，本文將粒子群優(yōu)化算法融入粒子濾波中。

首先，將最新的觀測值引入采樣過程，并定義適應(yīng)度函數(shù)為：

其中：Rk是觀測噪聲方差，yNew是最新的觀測值，yPred是預(yù)測觀測值。粒子群優(yōu)化算法通過計算適應(yīng)度值將所有的粒子向最優(yōu)粒子移動。但有時經(jīng)典的粒子群優(yōu)化算法的最大速度等參數(shù)很難確定，因此本文采用一種改進(jìn)的粒子群優(yōu)化算法，即高斯粒子群優(yōu)化算法(GPSO-PF：Gaussian Particle swarm Optimized Particle Filter)。

該方法基于一個高斯分布來不斷更新粒子的速度，其收斂性好于經(jīng)典的粒子群優(yōu)化算法[7-8]。

如果粒子集都分布在真實狀態(tài)附近，那么粒子群中每個粒子的適應(yīng)度都很高。反之，如果粒子群中每個粒子的個體最優(yōu)值以及粒子群的全局最優(yōu)值都很低，則說明粒子沒有分布在真實狀態(tài)附近。此時粒子集利用粒子群優(yōu)化算法，不斷根據(jù)最優(yōu)值并利用下式來更新每個粒子的速度與位置，使得粒子不斷地向真實狀態(tài)靠近：

通過移動粒子群向最優(yōu)粒子ppbest靠近，粒子群優(yōu)化算法實質(zhì)是驅(qū)動所有的粒子向高似然概率區(qū)域運(yùn)動。當(dāng)粒子群的最優(yōu)值符合某閾值ε時，說明粒子群已經(jīng)分布在真實狀態(tài)附近，那么粒子群將停止優(yōu)化。此時再對粒子集利用最新觀測值通過下式進(jìn)行權(quán)重更新并進(jìn)行歸一化處理：

為了解決粒子濾波的退化問題，需要選擇和復(fù)制權(quán)重值較大的粒子，即對粒子集進(jìn)行重采樣：

在重采樣之后，真實狀態(tài)附近的粒子權(quán)重值將會大。

通過以上的優(yōu)化過程，使得粒子集在權(quán)重值更新前更加趨向于高似然區(qū)域，從而解決了粒子貧乏問題。同時，優(yōu)化過程使得遠(yuǎn)離真實狀態(tài)的粒子趨向于真實狀態(tài)出現(xiàn)概率較大的區(qū)域，提高了每個粒子的作用效果。于是，粒子濾波需要大量粒子才能進(jìn)行精確狀態(tài)預(yù)估的問題也被削弱了，尤其當(dāng)初始狀態(tài)未知時。

2.2 模擬退火粒子群優(yōu)化粒子濾波

粒子群算法的本質(zhì)是利用本身信息、個體極值信息和全局極值三個信息，指導(dǎo)粒子下一步迭代位置。但是粒子群在追逐最優(yōu)粒子過程中，隨著它接近最優(yōu)粒子，其速度越來越小，因此粒子群表現(xiàn)出強(qiáng)烈的“趨同性”，容易陷入局部極小點(diǎn)。本文引進(jìn)模擬退火算法對其進(jìn)行相應(yīng)改進(jìn)研究，得到模擬退火粒子群粒子濾波算法[9](SAPSO-PF)。

在粒子群算法中，粒子通過更新其全局最優(yōu)和自身最優(yōu)的方法來搜索全局最優(yōu)解。但在其更新過程中，粒子只有遇到更好解的時候才會更新極值，這樣做縮小了粒子的搜索鄰域，使其很容易達(dá)到收斂，陷入局部最優(yōu)。本文引進(jìn)模擬退火算法對全局極值的更新條件做了改進(jìn)，基本思想如下：

當(dāng)新粒子的適應(yīng)值大于當(dāng)前全局極值時，系統(tǒng)一定接受該粒子；當(dāng)新粒子適應(yīng)度小于全局極值時，按式(5)計算出模擬退火概率p，當(dāng)p大于閾值r的時候，r為(0，1)間的隨機(jī)值，也接受該粒子，否則不接受。模擬退火概率計算如下：

式中Tt為第t次迭代的溫度，Tt=Kt×Tt-1，K為降溫系數(shù)；ΔD為當(dāng)前粒子失真變化，ΔD=1f（Ni）-1pg，f(Ni)為第i個粒子的當(dāng)前適應(yīng)值，pg為當(dāng)前全局最優(yōu)適應(yīng)值。

改進(jìn)算法的全局極值更新條件采用了隨機(jī)概率擾動接受的方式，既接收優(yōu)化解，也可以接受惡化解，增大全局搜索范圍。Tt很大時，使局部極值與全局極值之間的差值很大，接受概率比較大，可以接受的較差解比較多；Tt不是很高時，使差值小的狀態(tài)易于接受，即中溫時，易于接受與當(dāng)前狀態(tài)相比不是很差的解；Tt很小時，差值足夠小的狀態(tài)才易于接受，即低溫時，只能接受與當(dāng)前狀態(tài)相比差很少的解。當(dāng)經(jīng)過足夠長時間的溫度下降過程，固體達(dá)到最小能量狀態(tài)時，相應(yīng)也達(dá)到了全局最優(yōu)解。

2.3 模擬退火粒子群優(yōu)化粒子濾波偽代碼

Step1：取得量測值。

其中Zk為最新量測值，為預(yù)測量測值。

Step2：初始化，在k=0 時刻，從重要性函數(shù)采樣取N個粒子，抽樣出的粒子用表示。重要性密度函數(shù)取轉(zhuǎn)移先驗：

Step3：重要性權(quán)值計算。

據(jù)最優(yōu)值并利用下式來更新每個粒子的速度與位置，使得粒子不斷地向真實狀態(tài)靠近：

當(dāng)新粒子的適應(yīng)值大于當(dāng)前全局極值時，系統(tǒng)一定接受該粒子；當(dāng)新粒子適應(yīng)度小于全局極值時，按計算出模擬退火概率p，當(dāng)p大于閾值r的時候，r為(0，1)間的隨機(jī)值，也接受該粒子，否則不接受。模擬退火概率計算如下：

式中Tt為第t次迭代的溫度，，K為降溫系數(shù)；ΔD為當(dāng)前粒子失真變化，ΔD=1f（Ni）-1pg，f(Ni)為第i個粒子的當(dāng)前適應(yīng)值，pg為當(dāng)前全局最優(yōu)適應(yīng)值。

通過移動粒子群向最優(yōu)粒子ppbest靠近，粒子群優(yōu)化算法實質(zhì)是驅(qū)動所有的粒子向高似然概率區(qū)域運(yùn)動。當(dāng)粒子群的最優(yōu)值符合某閾值ε時，說明粒子群已經(jīng)分布在真實狀態(tài)附近，那么粒子群將停止優(yōu)化。此時再對粒子集利用最新觀測值通過下式進(jìn)行權(quán)重更新并進(jìn)行歸一化處理：

Step5：輸出。

Step6：判斷是否結(jié)束，若是則退出本算法，若否則返回step2。

3.實驗仿真及分析

狀態(tài)估計仿真模型選取如下：

其中vk服從伽馬分布?a(3,2)，表示系統(tǒng)噪聲，測量噪聲uk服從高斯分布N(0,0.0001)。實驗中使用的粒子數(shù)目為N=200個，觀測時間為T＝60，進(jìn)行100次獨(dú)立實驗。UT參數(shù)設(shè)置為α=1，β=0，κ=2。

表1 100次獨(dú)立實驗后不同非線性濾波算法產(chǎn)生的均方誤差的均值與方差Table1 100 independent experiments of different non-linear filtering algorithm mean square error

圖1 高斯粒子群粒子濾波狀態(tài)估計N＝100Fig 1 State estimation of Gaussian Particle swarm particle filter

圖2 模擬退火粒子群粒子濾波狀態(tài)估計N＝100Fig2 State Estimation of Simulated Annealing Particle Swarm Optimized Particle filter

粒子群優(yōu)化粒子濾波與其他非線性濾波算法估計性能比較表如表1所示。在幾種粒子濾波器中，PF對于非線性系統(tǒng)的適應(yīng)性是最差的。經(jīng)過100次獨(dú)立運(yùn)行實驗后，可以看到SAPSO-PF算法的誤差和均值均為最小，其總體誤差小于其它算法。具體狀態(tài)估計曲線如圖1和圖2所示，同樣可以看出SAPSO-PF算法得出的估計結(jié)果優(yōu)于GPSO-PF算法。

4.結(jié)論

在利用常規(guī)粒子濾波方法進(jìn)行系統(tǒng)狀態(tài)預(yù)估時，通常粒子集數(shù)目不能太大，否則系統(tǒng)的實時性很差。另一方面，如果粒子集的數(shù)目太小，則系統(tǒng)的魯棒性將會降低，容易受到粒子貧乏現(xiàn)象的影響。特別是在觀測量較準(zhǔn)確或似然概率位于先驗概率尾部的情況下，常規(guī)粒子濾波器的預(yù)估性能很差。本文通過分析常規(guī)粒子濾波方法存在問題的原因，將粒子群優(yōu)化算法同粒子濾波算法結(jié)合，得到高斯粒子群粒子濾波算法(GPSOPF)，使得采樣分布向后驗概率較高的區(qū)域運(yùn)動，從而減輕了粒子貧乏現(xiàn)象的產(chǎn)生，同時提高了狀態(tài)預(yù)估的精度，在此基礎(chǔ)上還討論了一種模擬退火粒子群粒子濾波算法(SAPSO-PF)，采用隨機(jī)概率擾動的方式作為基本粒子群算法的全局極值更新條件，從而增加全局最優(yōu)區(qū)域的搜索能力，在一定程度上提高了粒子濾波的有效樣本數(shù)。

[1]Bog dan K.Finding location using a particle f ilter and histogram matching[C].Proc of Artif icial Intelligence and Soft Computing.Poland：Springer，2004：786-791.

[2]Guthrun S.Particle filters in robotics[C].Proc of Uncertainty in AI.San Francisco：Morgan Hofmann Publishers，2002：511-518.

[3]張瑞華，雷敏.粒子濾波技術(shù)的發(fā)展現(xiàn)狀綜述[J].噪聲與振動控制，2010，4(2)：1-4.

[4]陳志敏，薄煜明，吳盤龍等.基于自適應(yīng)粒子群優(yōu)化的新型粒子濾波在目標(biāo)跟蹤中的應(yīng)用[J].2013，28(2)：193-200.

[5]Van M R，Doucet A.The unscented particle filter[R].Cambridge：Cambridge University，2000.

[6]Cl app T C.Statistical methods for the processing of communication data[D].Cambridge：University of Cambridge，2000.

[7]方正，佟國峰，徐心和.粒子群優(yōu)化粒子濾波方法[J].控制與決策，2007，22(3)：273-277.

[8]Rockling R A.Gaussian swarm：A novel particle swarm optimization algorithm[C].Proc of the IEEE Conf on Cybernetics and Intelligent Systems.Singapore，2004：372-376.

[9]高鷹，葉勝利.基于模擬退火的粒子群優(yōu)化算法[J].計算機(jī)工程與應(yīng)用，2004，40(1)：47-50.

[10]黃文娟，馬勤，王立群，楊淑瑩.一種粒子群優(yōu)化擴(kuò)展卡爾曼粒子濾波算法[J].天津理工大學(xué)學(xué)報，2009，25(5)：51-53.