武風波，劉 瑤，朱代先，王明博

（西安科技大學 通信與信息工程學院，陜西 西安 710600）

引言

基于卡爾曼濾波理論的濾波算法運用環(huán)境有限，在非線性、非高斯系統(tǒng)中其性能會大大降低。基于蒙特卡羅和貝葉斯濾波方法的粒子濾波（particle filter，PF）算法不受線性化誤差和高斯噪聲假定的限制[1]，適用于由狀態(tài)空間模型表示的任何非線性或非高斯系統(tǒng)。它已廣泛應用于目標跟蹤、機器人定位建圖、數(shù)據(jù)分析、圖像處理、衛(wèi)星導航動態(tài)定位等研究領(lǐng)域。

在標準的粒子濾波算法迭代后期，除了少數(shù)粒子，大多數(shù)粒子的權(quán)重值都小到可以忽略不計，這些小權(quán)重粒子濾波性能差，這就是粒子權(quán)值退化現(xiàn)象。如果繼續(xù)迭代，不僅會造成計算資源的浪費，還會使濾波精度降低，因此引入重采樣技術(shù)，可以改善粒子退化現(xiàn)象。然而有效粒子數(shù)目減少，會帶來粒子貧化問題，雖然通過大量的粒子可以取得較好的濾波效果，但是算法的復雜度和計算量也會隨之增加。為此，國內(nèi)外學者進行了大量研究，文獻[2]使用廣義回歸神經(jīng)網(wǎng)絡(luò)調(diào)整粒子樣本，使其更接近后驗概率密度，提高濾波精度。文獻[3]將無跡卡爾曼濾波和權(quán)值優(yōu)化方法相結(jié)合，以無跡卡爾曼濾波作為重要性密度函數(shù)對粒子狀態(tài)進行優(yōu)化，指導粒子采樣，并引入影響因子優(yōu)化粒子權(quán)值。文獻[4]提出了基于誤差橢圓重采樣的粒子濾波算法，通過建立具有不同置信水平的誤差橢圓，根據(jù)粒子的幾何位置將粒子分層，實現(xiàn)基于粒子位置的誤差重采樣。這些算法都在一定程度上提高了濾波性能，但運行過程中仍然可能會舍棄掉部分有效粒子，均未從本質(zhì)上完全消除粒子貧化的問題。

群智能優(yōu)化理論為粒子濾波算法提供了一個新的改進思路，經(jīng)典的群智能算法具有穩(wěn)健性、強搜索性、強尋優(yōu)性等特點，目前國內(nèi)外已有許多學者將群智能優(yōu)化理論成功應用到粒子濾波研究中。如基于引力場的粒子濾波[5]在粒子重采樣中引入引力場優(yōu)化思想，將采樣后的粒子作為宇宙中的灰塵，將全局最優(yōu)值作為中心灰塵，通過中心灰塵的吸引與排斥作用來優(yōu)化采樣粒子的分布情況。除此之外，還有基于果蠅算法的粒子濾波方法[6]、基于蝙蝠算法的粒子濾波方法[7]和基于螢火蟲算法的粒子濾波方法[8]等。截至目前，基于群智能優(yōu)化理論改進粒子濾波的算法仍存在不足，在算法計算后期粒子的尋優(yōu)能力下降，尋優(yōu)結(jié)果出現(xiàn)偏差，算法迭代過程中容易陷入早熟或欠收斂狀態(tài)，導致濾波性能降低。因此，如何快速準確地尋找全局最優(yōu)解是目前研究中面臨的難點問題[9]。

鯨群算法[10]（whale swarm algorithm，WSA）是由Mirjalili等人在2016年提出的一種新型、基于種群的仿生智能優(yōu)化算法。該算法思想源于對海洋中座頭鯨生物搜尋、包圍、狩獵等行為的效仿，算法實現(xiàn)過程簡單，收斂速度快，具有較強的魯棒性。文獻[11]已證明，鯨群算法的尋優(yōu)搜索能力優(yōu)于蝙蝠算法、蜻蜓算法。鯨群算法自提出后多被用于系統(tǒng)調(diào)度、圖像檢索、特征選擇等領(lǐng)域[12-14]，目前還沒有將鯨群算法與粒子濾波結(jié)合的相關(guān)研究。

本文將鯨群算法引入粒子濾波中，提出一種改進的鯨群算法優(yōu)化粒子濾波（improved whale swarm algorithm optimized particle filter，IWS-PF）。該算法中，考慮到粒子濾波的運行機制和鯨群算法的特點，利用鯨魚群的螺旋運動尋優(yōu)方式更新粒子位置，并引入最優(yōu)鄰域擾動策略和自適應權(quán)重因子策略，動態(tài)調(diào)整鯨群的全局最優(yōu)位置，平衡算法的局部與全局搜索能力。改進的鯨群優(yōu)化粒子濾波算法實現(xiàn)了對重要性采樣過程的優(yōu)化，使粒子的分布更加接近真實的后驗概率密度，改善了粒子權(quán)值退化現(xiàn)象，從根本上解決了粒子貧化問題，提高了粒子濾波的精度。

1 粒子濾波算法

粒子濾波算法的基本思想來源于蒙特卡羅方法（Monte Carlo methods），它適用于所有可被狀態(tài)空間模型描述的系統(tǒng)。粒子濾波采用狀態(tài)空間的隨機樣本近似概率密度函數(shù)，使用樣本均值避免復雜的積分計算[15]。給定非線性動態(tài)方程為

式中：xt代 表系統(tǒng)在t時 刻的狀態(tài)變量；f(·)代表狀態(tài)轉(zhuǎn)移方程；wt代表系統(tǒng)的過程噪聲。zt代表系統(tǒng)在t時刻的觀測值；h(·)代 表觀測方程；vt代表系統(tǒng)觀測噪聲；ut代 表系統(tǒng)在t時刻的輸入量。

粒子濾波的核心是求解邊緣后驗概率p(xtjz1:t)，其實現(xiàn)可分為2個過程：預測過程和更新過程。

1）預測過程 給定t?1時刻的數(shù)據(jù)觀測值，預測t時刻狀態(tài)變量xt出現(xiàn)的概率。預測方程為

2）更新過程 從t?1時 刻到達t時刻，得到一個新的觀測變量zt，修正上式的預測值。更新方程為

歸一化常數(shù)為

粒子權(quán)值更新公式表示為

對權(quán)值進行歸一化，即：

則輸出狀態(tài)為

2 鯨群算法

鯨群算法是一種模擬海洋中座頭鯨捕食行為的群智能優(yōu)化算法。座頭鯨的捕食行為如圖1所示。通過氣泡網(wǎng)進行攻擊，不斷螺旋向上靠近目標，逐漸收縮包圍圈，最終到達目標魚群的位置。該算法主要包含3個階段：包圍獵物、螺旋搜尋、隨機搜尋。

圖1 座頭鯨捕食行為Fig.1 Feedation behavior of humpback whale

2.1 包圍獵物

在搜尋獵物過程中，WSA令當前最優(yōu)解或接近最優(yōu)候選解為獵物目標，然后其他鯨魚向當前最接近獵物的鯨魚方向移動，并逐漸縮小整個鯨魚群的包圍圈，實現(xiàn)對獵物的包圍。在此過程中，鯨魚位置更新公式為

式中：t為當前迭代次數(shù)；D為最優(yōu)鯨魚與其他鯨魚個體的距離向量；X?(t)為當前最優(yōu)的鯨魚位置；X(t)為當前鯨魚個體位置的坐標向量；系數(shù)向量A和C的計算公式為

式中：r1和r2是 0～1之間的隨機數(shù)；a是收斂因子，從2～0線性遞減；tmax是最大迭代次數(shù)。

2.2 螺旋搜尋

座頭鯨狩獵時以螺旋式游向獵物，構(gòu)建鯨魚群狩獵行為的數(shù)學分析模型為

式中：D′為 鯨魚與獵物之間的距離；b是決定螺旋形狀的系數(shù)；l是?1～1的隨機數(shù)。

WSA選擇概率p作為閾值，決定鯨魚位置更新的方式，其中包含包圍獵物和狩獵行為兩種機制[16]。其數(shù)學模型描述為

若p<0.5，則：

若p≥0.5，則：

2.3 隨機搜尋

鯨魚捕食時，1）假定A<1時，采用螺旋包圍狩獵行為；2）假定A≥1時，采用隨機搜尋方式。隨機搜尋過程的數(shù)學模型為

式中：Xrand是鯨群中隨機選擇一個鯨魚的位置向量。

3 鯨群算法優(yōu)化的粒子濾波

從上述2種算法的運行機制上看，可以考慮采用鯨群算法來優(yōu)化粒子重要性采樣過程，通過鯨群算法的強尋優(yōu)能力，使粒子集合集中分布在高似然區(qū)域，緩解粒子退化問題，并且每個鯨魚受最優(yōu)鯨魚位置的吸引力大小不同，這將使粒子多樣性得到提升。基于此，本文提出的IWS-PF算法設(shè)計如下。

3.1 將粒子權(quán)值作為鯨魚個體位置的評判標準

在粒子濾波算法中，越接近真實值的粒子權(quán)值越大。在鯨群算法中，越接近目標獵物的鯨魚適應度越大，所處位置越優(yōu)。因此，將粒子權(quán)值作為鯨魚個體位置的評判標準，這樣就建立了鯨群算法與粒子濾波算法兩者之間的聯(lián)系，即每個粒子的權(quán)重值大小表征鯨魚個體所處位置的狀態(tài)，最佳位置所處的的鯨魚相當于集合中權(quán)重值最高粒子的狀態(tài)，鯨魚個體通過迭代不斷向最優(yōu)位置鯨魚靠近，此過程可以類比為粒子不斷逼近真實系統(tǒng)狀態(tài)的后驗概率分布。因此，可以考慮通過鯨群算法優(yōu)化重要性采樣后粒子分布。

3.2 全局最優(yōu)值調(diào)整策略設(shè)計

當更新鯨魚位置時，通常會針對當前的最佳鯨魚進行迭代。在整個迭代過程中，只有出現(xiàn)優(yōu)于當前時刻的最佳位置時，才會對原來的最佳位置進行更新。這樣的搜索策略會使總的更新次數(shù)較少，導致群體的多樣性降低，從而影響算法的搜索效率。為此，考慮引入最優(yōu)鄰域隨機擾動策略，增加對附近空間的隨機搜索，尋找一個更優(yōu)的全局值，以提升整個算法的收斂速度和全局搜索能力。最優(yōu)鄰域隨機擾動公式[17]為

若rand2<0.5，則：

若rand2≥0.5，則：

式中： rand1、 rand2 是 [0,1]之 間的隨機數(shù)；是新生成的鄰域位置。隨機數(shù) rand2與鯨群算法中概率p的作用類似，它以相同的概率50%作為閾值來更新新生成的位置。當 rand2<0.5時，新生成的鄰域位置為（20）式所示；當 r and2≥0.5時，新生成的鄰域位置仍為當前最優(yōu)鯨魚的位置。若新生成的位置比原位置的適應度好，則更新原位置，否則，原位置保持不變。數(shù)學模型如下：

3.3 位置更新公式修正

為實現(xiàn)鯨群算法與粒子濾波的有效結(jié)合，本文采用螺旋搜尋方式優(yōu)化采樣粒子，實現(xiàn)對重要性采樣后的粒子分布優(yōu)化。權(quán)重因子能夠有效平衡算法的局部尋優(yōu)能力和全局搜索能力，本文考慮對鯨魚位置的更新過程引入自適應權(quán)重因子wt，動態(tài)地平衡算法的局部與全局搜索能力。構(gòu)造的自適應因子表達式為

加入自適應權(quán)重后，鯨魚位置更新公式（16）和（17）修正為

結(jié)合 sin 函 數(shù)在 ( 0,π/2)區(qū)間的變化特性可知，在算法前期wt會得到較大值，此時，鯨魚受當前種群中最優(yōu)解的吸引力較弱，這樣可提升算法在前期的全局搜索能力。隨著迭代次數(shù)t的 增加，wt的值會越來越小，使當前種群的最優(yōu)解的影響力逐漸提升，對其他鯨魚的吸引力越來越強，其他鯨魚逐漸向當前最優(yōu)位置移動。這樣加強了算法的局部開發(fā)能力，提升了算法求解精度和收斂速度。

3.4 算法實現(xiàn)步驟

綜上所述，IWS-PF算法實現(xiàn)流程圖如圖2所示。

圖2 算法流程圖Fig.2 Flow chart of algorithm

1）t=0 時 刻，在狀態(tài)空間采樣N個初始粒子i=1，2···Ng。

2）根據(jù)（1）式計算t+1時 刻粒子狀態(tài)值xt和觀測值zt， 將每個粒子的狀態(tài)值作為每個鯨魚的位置X(i)(t)。

3）使用（6）式計算每個粒子權(quán)重值，更新當前時刻的全局最優(yōu)值X?(t)。

4）對X?(t)進行最優(yōu)鄰域隨機擾動更新，生成均勻分布的隨機數(shù)r and1、 r and2 ，若 r and2<0.5，則執(zhí)行（20）式。否則，執(zhí)行（21）式。

6）生成一個獨立的隨機數(shù) rand(p)， 通過p的取值大小執(zhí)行（24）式，更新粒子位置。

7）判斷是否達到最大迭代次數(shù)，若是達到，則停止迭代；否則，轉(zhuǎn)到步驟3。

8）更新粒子權(quán)值，并使用（7）式歸一化權(quán)值。

9）根據(jù)（8）式輸出每個粒子的狀態(tài)值。

3.5 算法計算復雜度分析

在IWS-PF算法中，設(shè)粒子數(shù)量為N，最大迭代次數(shù)為M。從圖2算法流程圖可以看出，加入自適應權(quán)重的位置更新修正公式并未增加循環(huán)次數(shù)，理論上算法時間復雜度為O(N?M)，加入最優(yōu)鄰域隨機擾動將要生成一個新位置，意味著算法增加了一次遍歷種群的循環(huán)，算法復雜度為O(2?N?M)，也就是說與PF相比，IWS-PF整體上增加了O(N?M)的計算復雜度。因此，IWS-PF的運行時間也會略高于PF，這也與下文的仿真測試結(jié)果相符。

4 仿真實驗及結(jié)果分析

實驗的硬件環(huán)境為臺式電腦（IntelCorei5處理器，4GB內(nèi)存），實驗環(huán)境為MATLAB 2018b。粒子濾波仿真中采用的狀態(tài)方程和觀測方程為

此模型是一個典型的非線性、非高斯系統(tǒng)模型，w(t)、v(t)為零均值高斯噪聲。采用均方根誤差公式作為算法性能評價的依據(jù)，其定義為

實驗中，設(shè)R=1，Q=10，初始狀態(tài)0.1，采樣步長50。因為WSA與引力場算法具有很多共通之處，兩者都是根據(jù)群體中某個個體得到的信息求解最優(yōu)解。引力場算法是通過中心灰塵的吸引力與排斥力作用于其他灰塵使其靠近或遠離。WSA是通過當前種群中的最優(yōu)鯨魚對其他鯨魚的吸引力大小來決定靠近或遠離。本文提出的IWSPF算法與引力場優(yōu)化的粒子濾波算法[5]（gravitation field algorithm particle filter，GFA-PF）都是基于標準PF做出的改進算法，所以在濾波精度、運算時間等方面對這3種算法進行了對比分析，比較了算法的性能。

4.1 算法收斂性分析

為驗證IWS-PF算法與PF算法的收斂性，其他參數(shù)設(shè)置相同，在粒子數(shù)目不同的條件下對2種算法進行RMSE對比，算法收斂性如圖3所示。

圖3 PF，IWS-PF算法收斂性Fig.3 Convergence of PF and IWS-PF algorithms

由圖3可知，本文提出的IWS-PF算法隨著粒子數(shù)目的增多，算法的濾波精度逐漸提升，并且當粒子數(shù)目達到80后，算法的RMSE值慢慢趨于穩(wěn)定狀態(tài)。這表明當粒子數(shù)目達到一定條件后IWSPF與傳統(tǒng)PF算法都具有誤差趨于平穩(wěn)的特征，也就是說，本文提出的IWS-PF算法也具有相似的收斂性。

4.2 粒子分布測試

為測試IWS-PF粒子多樣性，設(shè)置粒子總數(shù)為100，標準PF與IWS-PF算法進行50次迭代后的粒子分布如圖4所示。

由圖4可知，標準PF只用大權(quán)值粒子進行狀態(tài)估計，導致粒子分布狀態(tài)單一且聚集性強，不利于整體狀態(tài)估計。IWS-PF大部分粒子整體分布在高似然區(qū)域，少部分粒子散落分布在低似然區(qū)域，粒子多樣性豐富。通過包圍以及螺旋搜尋兩種不同的方式優(yōu)化，使得粒子分布更加合理。測試結(jié)果表明，IWS-PF有效改善了粒子退化問題，增加了粒子多樣性。

圖4 粒子分布Fig.4 Particle distribution

4.3 濾波精度測試

圖5～圖7分別是PF、GFA-PF和IWS-PF在粒子數(shù)為20、50、100時的狀態(tài)估計及對應的誤差統(tǒng)計對比結(jié)果圖。

圖5 狀態(tài)估計及誤差統(tǒng)計對比（N=20，Q=10，R=1）Fig.5 Statistical comparison of state estimation and error（N = 20，Q = 10，R = 1）

圖6 狀態(tài)估計及誤差統(tǒng)計對比（N=50，Q=10，R=1）Fig.6 Statistical comparison of state estimation and error（N = 50，Q = 10，R = 1）

由圖5～圖7可知，在粒子數(shù)目相同的條件下，相較于標準PF、GFA-PF，IWS-PF的預測曲線與真實狀態(tài)曲線重合度最高，說明IWS-PF的估計絕對值誤差最小，這是由于運用了鯨群算法的尋優(yōu)機制，使粒子分布更加接近真實值；引入的最優(yōu)鄰域隨機擾動策略和自適應權(quán)重，實現(xiàn)了算法前期全局尋優(yōu)，后期局部精準尋優(yōu)，從而提高了粒子濾波的精度。

圖7 狀態(tài)估計及誤差統(tǒng)計對比（N=100，Q=10，R=1）Fig.7 Statistical comparison of state estimation and error（N =100，Q = 10，R = 1）

實驗中取不同粒子數(shù)目N=20、50、100，對3種算法分別進行20次獨立仿真實驗，取其均方根誤差的平均值進行對比，結(jié)果如表1所示，3種算法運行時間如表2所示。

表2 3種算法運行時間對比Table 2 Comparison of operation time of three algorithms

由表1實驗結(jié)果可以看出，隨著粒子數(shù)目的不斷增加，3種算法的的誤差在逐漸減小，即粒子數(shù)目越多，濾波效果也越好。在粒子數(shù)目相同的條件下，IWS-PF的均方誤差值始終低于PF、GFAPF，并且IWS-PF在粒子數(shù)為N=20的時候，就能夠達到標準PF算法N=100時的濾波精度，所以IWSPF算法的狀態(tài)估計效果更好，并且IWS-PF的運算時間略高于PF，但與GFA-PF算法的運行時間幾乎一致，說明算法在保證濾波精度的同時，也能夠保持較好的實時性。

表1 3種算法均方根誤差對比Table 1 Comparison of root mean square error of three algorithms

5 結(jié)論

本文提出了一種鯨群優(yōu)化的粒子濾波算法，利用鯨群算法的螺旋尋優(yōu)機制優(yōu)化了粒子濾波的重要性采樣過程，使粒子分布更加合理，并引入了最優(yōu)鄰域隨機擾動策略，增強了算法的強尋優(yōu)能力，并在鯨魚位置更新過程中加入自適應權(quán)重因子，動態(tài)地平衡算法的局部與全局搜索能力，提高了粒子濾波求解精度。實驗結(jié)果表明，提出的IWSPF相較于標準PF、GFA-PF具有更高的粒子濾波估計精度和更小的均方誤差，并且在運行后期依然能夠保持良好的粒子多樣性，避免了粒子貧化現(xiàn)象。