基于模擬退火粒子群優化的粒子濾波算法

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

1.引言

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

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

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

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

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

2.基于模擬退火粒子群優化的粒子濾波算法

2.1 高斯粒子群優化粒子濾波

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

首先，將最新的觀測值引入采樣過程，并定義適應度函數為：

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

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

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

通過移動粒子群向最優粒子ppbest靠近，粒子群優化算法實質是驅動所有的粒子向高似然概率區域運動。當粒子群的最優值符合某閾值ε時，說明粒子群已經分布在真實狀態附近，那么粒子群將停止優化。此時再對粒子集利用最新觀測值通過下式進行權重更新并進行歸一化處理：

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

在重采樣之后，真實狀態附近的粒子權重值將會大。

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

2.2 模擬退火粒子群優化粒子濾波

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

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

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

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

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

2.3 模擬退火粒子群優化粒子濾波偽代碼

Step1：取得量測值。

其中Zk為最新量測值，為預測量測值。

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

Step3：重要性權值計算。

據最優值并利用下式來更新每個粒子的速度與位置，使得粒子不斷地向真實狀態靠近：

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

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

通過移動粒子群向最優粒子ppbest靠近，粒子群優化算法實質是驅動所有的粒子向高似然概率區域運動。當粒子群的最優值符合某閾值ε時，說明粒子群已經分布在真實狀態附近，那么粒子群將停止優化。此時再對粒子集利用最新觀測值通過下式進行權重更新并進行歸一化處理：

Step5：輸出。

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

3.實驗仿真及分析

狀態估計仿真模型選取如下：

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

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

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

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

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

4.結論

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

[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]張瑞華，雷敏.粒子濾波技術的發展現狀綜述[J].噪聲與振動控制，2010，4(2)：1-4.

[4]陳志敏，薄煜明，吳盤龍等.基于自適應粒子群優化的新型粒子濾波在目標跟蹤中的應用[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]方正，佟國峰，徐心和.粒子群優化粒子濾波方法[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]高鷹，葉勝利.基于模擬退火的粒子群優化算法[J].計算機工程與應用，2004，40(1)：47-50.

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