武風(fēng)波，劉 瑤，朱代先，王明博

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

引言

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

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

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

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

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

1 粒子濾波算法

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

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

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

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

2）更新過(guò)程 從t?1時(shí) 刻到達(dá)t時(shí)刻，得到一個(gè)新的觀測(cè)變量zt，修正上式的預(yù)測(cè)值。更新方程為

歸一化常數(shù)為

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

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

則輸出狀態(tài)為

2 鯨群算法

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

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

2.1 包圍獵物

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

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

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

2.2 螺旋搜尋

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

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

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

若p<0.5，則：

若p≥0.5，則：

2.3 隨機(jī)搜尋

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

式中：Xrand是鯨群中隨機(jī)選擇一個(gè)鯨魚的位置向量。

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

從上述2種算法的運(yùn)行機(jī)制上看，可以考慮采用鯨群算法來(lái)優(yōu)化粒子重要性采樣過(guò)程，通過(guò)鯨群算法的強(qiáng)尋優(yōu)能力，使粒子集合集中分布在高似然區(qū)域，緩解粒子退化問(wèn)題，并且每個(gè)鯨魚受最優(yōu)鯨魚位置的吸引力大小不同，這將使粒子多樣性得到提升?；诖耍疚奶岢龅腎WS-PF算法設(shè)計(jì)如下。

3.1 將粒子權(quán)值作為鯨魚個(gè)體位置的評(píng)判標(biāo)準(zhǔn)

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

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

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

若rand2<0.5，則：

若rand2≥0.5，則：

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

3.3 位置更新公式修正

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.5 算法計(jì)算復(fù)雜度分析

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

4 仿真實(shí)驗(yàn)及結(jié)果分析

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

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

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

4.1 算法收斂性分析

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

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

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

4.2 粒子分布測(cè)試

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

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

圖4 粒子分布Fig.4 Particle distribution

4.3 濾波精度測(cè)試

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

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

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

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

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

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

表2 3種算法運(yùn)行時(shí)間對(duì)比Table 2 Comparison of operation time of three algorithms

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

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

5 結(jié)論

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