武風(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)題,提高了粒子濾波的精度。
粒子濾波算法的基本思想來(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)為
鯨群算法是一種模擬海洋中座頭鯨捕食行為的群智能優(yōu)化算法。座頭鯨的捕食行為如圖1所示。通過(guò)氣泡網(wǎng)進(jìn)行攻擊,不斷螺旋向上靠近目標(biāo),逐漸收縮包圍圈,最終到達(dá)目標(biāo)魚群的位置。該算法主要包含3個(gè)階段:包圍獵物、螺旋搜尋、隨機(jī)搜尋。
圖1 座頭鯨捕食行為Fig.1 Feedation behavior of humpback whale
在搜尋獵物過(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ù)。
座頭鯨狩獵時(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,則:
鯨魚捕食時(shí),1)假定A<1時(shí),采用螺旋包圍狩獵行為;2)假定A≥1時(shí),采用隨機(jī)搜尋方式。隨機(jī)搜尋過(guò)程的數(shù)學(xué)模型為
式中:Xrand是鯨群中隨機(jī)選擇一個(gè)鯨魚的位置向量。
從上述2種算法的運(yùn)行機(jī)制上看,可以考慮采用鯨群算法來(lái)優(yōu)化粒子重要性采樣過(guò)程,通過(guò)鯨群算法的強(qiáng)尋優(yōu)能力,使粒子集合集中分布在高似然區(qū)域,緩解粒子退化問(wèn)題,并且每個(gè)鯨魚受最優(yōu)鯨魚位置的吸引力大小不同,這將使粒子多樣性得到提升?;诖耍疚奶岢龅腎WS-PF算法設(shè)計(jì)如下。
在粒子濾波算法中,越接近真實(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)化重要性采樣后粒子分布。
當(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é)模型如下:
為實(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ā)能力,提升了算法求解精度和收斂速度。
綜上所述,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)值。
在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é)果相符。
實(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ì)比分析,比較了算法的性能。
為驗(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算法也具有相似的收斂性。
為測(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
圖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
本文提出了一種鯨群優(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)象。