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

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

1.引言

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

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

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

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的重要性采樣進行優(yōu)化將最新的觀測值引入重要性采樣分布中，使得重要性采樣分布向后驗概率較高的區(qū)域運動，提高了狀態(tài)預(yù)估的精度，然而該方法仍然存在重采樣過程帶來的粒子缺乏多樣性問題。

本文提出了基于模擬退火粒子群優(yōu)化的粒子濾波算法(SAPSO-PF：Simulated Annealing Particle Swarm Optimized Particle Filter)，該方法基于一個高斯分布來不斷更新粒子的速度，同時采用隨機概率擾動的方式作為基本粒子群算法的全局極值更新條件，從而增加全局最優(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ù)很難確定，因此本文采用一種改進的粒子群優(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ū)域運動。當(dāng)粒子群的最優(yōu)值符合某閾值ε時，說明粒子群已經(jīng)分布在真實狀態(tài)附近，那么粒子群將停止優(yōu)化。此時再對粒子集利用最新觀測值通過下式進行權(quán)重更新并進行歸一化處理：

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

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

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

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

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

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

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

式中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)值。

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

式中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ū)域運動。當(dāng)粒子群的最優(yōu)值符合某閾值ε時，說明粒子群已經(jīng)分布在真實狀態(tài)附近，那么粒子群將停止優(yōu)化。此時再對粒子集利用最新觀測值通過下式進行權(quán)重更新并進行歸一化處理：

Step5：輸出。

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

3.實驗仿真及分析

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

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

表1 100次獨立實驗后不同非線性濾波算法產(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次獨立運行實驗后，可以看到SAPSO-PF算法的誤差和均值均為最小，其總體誤差小于其它算法。具體狀態(tài)估計曲線如圖1和圖2所示，同樣可以看出SAPSO-PF算法得出的估計結(jié)果優(yōu)于GPSO-PF算法。

4.結(jié)論

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

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