基于多策略人工蜂鳥優(yōu)化PF的SLAM研究*

蔡 艷,楊光永,樊康生,徐天奇

(云南民族大學(xué)電氣信息工程學(xué)院,昆明 650000)

0 引言

瞬時定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)在移動機(jī)器人自主工作領(lǐng)域扮演重要角色。目前,SLAM的求解方法可大致分為基于卡爾曼濾波器的方法、基于粒子濾波器的方法、基于圖優(yōu)化的方法[1]。基于蒙特卡洛方法的粒子濾波算法(particle filter,PF)不受系統(tǒng)非線性和非高斯噪聲的限制,是常用于SLAM工程領(lǐng)域的方法之一。

由于粒子濾波算法基于蒙特卡洛方法,因此要達(dá)到所需估計精度需要大量的粒子,而粒子數(shù)量越多,算法時間復(fù)雜度越高;此外,重采樣易導(dǎo)致樣本多樣性下降,出現(xiàn)粒子退化的問題。而進(jìn)化問題和粒子濾波器本質(zhì)上都是通過評價、選擇和更新的迭代過程獲得最優(yōu)解[4],因此很多研究者采用進(jìn)化方法來解決粒子濾波器存在的問題。張翔等[5]引入遺傳算法改善標(biāo)準(zhǔn)粒子濾波中的粒子退化與粒子衰退問題。劉海濤等[6]在基于遺傳算法的智能粒子濾波基礎(chǔ)上,提出對低權(quán)值粒子的改進(jìn)的智能粒子濾波(IIPF)處理策略,提高了濾波精度。韓錕等[7]提出一種基于果蠅優(yōu)化思想的粒子濾波算法,將遺傳算法中的交叉、變異操作自適應(yīng)地應(yīng)用到果蠅優(yōu)化算法尋優(yōu)過程當(dāng)中,有效提高了估計精度。陳志敏等[8]提出了一種基于蝙蝠算法的新型粒子濾波算法。該算法用粒子表征蝙蝠個體,模擬蝙蝠群體搜索獵物的過程,進(jìn)而提髙粒子整體的質(zhì)量和分布的合理性。李冀等[9]結(jié)合融入圍獵策略的哈里斯鷹優(yōu)化算法設(shè)計一種群智能優(yōu)化粒子濾波方法(EHHOPF),有效提升了系統(tǒng)狀態(tài)估計精度及濾波穩(wěn)定性等。上述方法均提高了粒子濾波算法的性能,但大多都是通過經(jīng)驗(yàn)值控制智能算法迭代次數(shù),易造成優(yōu)化不足或過度優(yōu)化而引起估計精度降低的問題。

本文為進(jìn)一步提高PF算法的性能,引入多策略的人工蜂鳥算法(artificial hummingbird algorithm,AHA)[10]尋優(yōu)能力強(qiáng)的特性更新預(yù)測粒子集,提高粒子集質(zhì)量;當(dāng)粒子密集度過高時,自適應(yīng)調(diào)整優(yōu)化程度;根據(jù)改進(jìn)的人工蜂鳥算法獲取的最優(yōu)解調(diào)整預(yù)測粒子集,使粒子集在權(quán)重計算前就更接近期望值,以此提高路徑和路標(biāo)的估計精度;在重采樣階段通過重組粒子集來增加粒子多樣性。

1 多策略人工蜂鳥算法優(yōu)化的粒子重組PF

1.1 標(biāo)準(zhǔn)人工蜂鳥算法

人工蜂鳥算法模擬了自然界中蜂鳥的特殊飛行技能和智能覓食策略,該算法包括3種覓食行為方式。

(1)引導(dǎo)覓食。在覓食過程中,AHA算法建模了3種飛行技能,通過引入方向切換向量控制D維空間中的一個或多個方向是否為可用,其中軸向飛行定義如下:

(1)

對角飛行定義為:

(2)

全向飛行定義為:

D(i)=1i=1,…,d

(3)

式中:D(i)表示飛行技能,randi([1,d])表示生成一個1～d的隨機(jī)整數(shù),randperm(k)表示創(chuàng)建一個1～k的隨機(jī)整數(shù)排列,r1是(0,1]的隨機(jī)數(shù)。憑借這些飛行技能,蜂鳥可以訪問目標(biāo)食物源,從而獲得候選食物源。候選食物源位置更新為:

vi(t+1)=xi,tar(t)+a·D·(xi(t)-xi,tar(t))

(4)

式中:vi(t+1)為第t+1次迭代第i個候選食物源位置,xi(t)為第t次迭代第i個食物源位置,xi,tar(t)為第i只蜂鳥計劃訪問的目標(biāo)食物源位置,a為服從標(biāo)準(zhǔn)正態(tài)分布的引導(dǎo)因子。第i個食物源的位置更新如下:

(5)

式中:f(·)表示適應(yīng)度值。

(2)區(qū)域性覓食。蜂鳥在訪問了目標(biāo)食物源后,可能會移動到鄰近區(qū)域?qū)ふ倚碌氖澄镌?而不是訪問其他現(xiàn)有的食物源。鄰近區(qū)域候選食物源位置更新為:

vi(t+1)=xi(t)+b·D·xt(t)

(6)

式中:b為服從標(biāo)準(zhǔn)正態(tài)分布的引導(dǎo)因子。

(3)遷徙覓食。當(dāng)蜂鳥經(jīng)常訪問的地區(qū)缺乏食物時,通常會遷移到較遠(yuǎn)的食物源覓食。位于花蜜補(bǔ)充率最差食物源的蜂鳥將遷徙到搜索空間中隨機(jī)產(chǎn)生的新食物源處,為:

xwor(t+1)=Low+r·(Up-Low)

(7)

式中:xwor為種群中花蜜補(bǔ)充率最差的食物來源。

1.2 中垂線收斂策略

中垂線收斂[12]策略二維示意圖如圖1所示,圖中best表示矩形區(qū)域內(nèi)的最優(yōu)解,本文用bs與bt分別表示區(qū)域內(nèi)當(dāng)前最優(yōu)和次優(yōu)的值,若best到bs的距離小于best到bt的距離,則best點(diǎn)必位于bs與bt的中垂線靠bs側(cè)區(qū)域(即圖1灰色區(qū)域)。此時,會重新在灰色區(qū)域找到bt點(diǎn),再按照上述規(guī)則再次判斷最優(yōu)點(diǎn)的區(qū)域,直至找到最優(yōu)點(diǎn)。中垂線收斂策略利用該思想,以點(diǎn)的適應(yīng)度代替“距離”。

圖1 中垂線算法原理圖

設(shè)目標(biāo)種群當(dāng)前迭代過程中適應(yīng)度最優(yōu)的個體為bs。適應(yīng)度次優(yōu)的個體為bt,定義:

(8)

若某一點(diǎn)xi位于中垂線靠bt側(cè)區(qū)域,則xi的第j維需滿足下列條件:

(9)

式中:

(10)

若判定該粒子位于中垂線靠bt側(cè)區(qū)域,則更新xi,為:

(11)

(12)

式中:D為維度。

1.3 自適應(yīng)調(diào)整策略

為避免AHA僅依賴經(jīng)驗(yàn)值設(shè)置迭代次數(shù)造成優(yōu)化不足或過度優(yōu)化的問題,引入種群密度監(jiān)測階段實(shí)現(xiàn)自適應(yīng)調(diào)整迭代次數(shù),實(shí)時監(jiān)測最優(yōu)個體附近的種群密度,當(dāng)密度達(dá)到閾值時,停止迭代,相應(yīng)的表達(dá)式為:

(13)

式中:Nb表示最優(yōu)個體周圍的蜂鳥數(shù),rb表示密度半徑,find操作表示查找密度半徑內(nèi)的個體數(shù)量。

(14)

式中:ρb表示最優(yōu)個體周圍的種群密度,N表示粒子總數(shù)。

1.4 Levy飛行策略

當(dāng)種群進(jìn)行區(qū)域性覓食時,引入Levy飛行更新個體位置,通過步長變化擴(kuò)大搜索空間,以提高算法全局搜索能力。當(dāng)種群密度大于設(shè)置的區(qū)域搜索閾值時采用Levy飛行策略更新個體位置,公式為:

vi(t+1)=xi(t)+b·D·xt(t)+α⊕Levy(s,β)

(15)

式中:Levy(·)為Levy分布函數(shù),α為步長控制因子,β為概率系數(shù),s為Levy飛行步長,公式為:

(16)

(17)

式中:Γ(·)為伽馬函數(shù)。

1.5 粒子重組重采樣

針對粒子濾波算法在重采樣階段去除小權(quán)值粒子而降低估計性能的問題,在重采樣階段設(shè)計如下粒子篩選策略:

(1)計算有效粒子數(shù)Neff以及粒子權(quán)重,并將權(quán)重按降序排列;

(18)

1.6 基于多策略人工蜂鳥的粒子重組粒子濾波

針對傳統(tǒng)粒子濾波算法精度較低以及粒子貧乏的問題,本文引入AHA算法對其進(jìn)行優(yōu)化以提高粒子集質(zhì)量;為避免優(yōu)化不足或過度優(yōu)化且減少時間復(fù)雜度,采用自適應(yīng)調(diào)整迭代次數(shù)策略;為提高算法收斂速度在引導(dǎo)覓食階段引入中垂線收斂策略;在區(qū)域性覓食階段引入Levy飛行擴(kuò)大搜索空間自適應(yīng)調(diào)整粒子集分布,多樣化粒子;在重采樣階段采用粒子重組策略增加粒子多樣性。IAHA-PF算法步驟為:

步驟1:初始化粒子集,每個粒子初始權(quán)重為w0=1/M;

步驟2:從proposal分布中采樣粒子集,t時刻的采樣粒子集Xt為:

Xt～p(xt|xt-1,ut)

(19)

步驟3:IAHA優(yōu)化,更新粒子集;

步驟4:根據(jù)IAHA的輸出更新粒子集分布;

步驟5:計算粒子權(quán)重;

步驟6:根據(jù)式(18)重組粒子集;

步驟7:迭代更新預(yù)測值,根據(jù)式(20)輸出預(yù)測值。

(20)

多策略人工蜂鳥算法步驟如表1所示。

表1 多策略人工蜂鳥算法

1.7 IAHA-PF算法時間復(fù)雜度分析

假設(shè)粒子數(shù)為N,最大迭代次數(shù)為M,跟蹤時長為T,則粒子集采樣時間復(fù)雜度為O(TN),IAHA算法時間復(fù)雜度為O(TMN),重采樣階段時間復(fù)雜度為O(TN+TNN),所以總的時間復(fù)雜度為O(TN+TMN+TNN),由于本文算法采用自適應(yīng)調(diào)整迭代次數(shù)策略,因此實(shí)際迭代次數(shù)是小于最大迭代次數(shù)M的,并且隨著粒子數(shù)量的增加,實(shí)際迭代次數(shù)會逐漸減少,此外,實(shí)際迭代次數(shù)一般是小于N的,這與仿真實(shí)驗(yàn)自適應(yīng)調(diào)整迭代次數(shù)部分相符,忽略低階項(xiàng),記K=max(M,N),最后IAHA-PF算法的時間復(fù)雜度為O(TKN),與PF的時間復(fù)雜度同量級,說明相比于標(biāo)準(zhǔn)PF算法,本文的改進(jìn)算法總體時間復(fù)雜度略高,但增加幅度不大。

2 仿真實(shí)驗(yàn)

為驗(yàn)證本文算法具有較好的濾波性能以及應(yīng)用在SLAM中有較好的定位與建圖性能,進(jìn)行仿真對比實(shí)驗(yàn)。

2.1 密度半徑設(shè)定

引入智能算法進(jìn)行優(yōu)化時,一般設(shè)置最大迭代次數(shù),而智能算法迭代過程中具有隨機(jī)性,僅通過最大迭代次數(shù)可能會造成過度優(yōu)化或優(yōu)化不足問題。本文引入自適應(yīng)調(diào)整策略,設(shè)置粒子密度監(jiān)測階段,自適應(yīng)調(diào)整迭代次數(shù)。其中粒子密度的衡量標(biāo)準(zhǔn)取決于密度半徑的設(shè)定,表2統(tǒng)計了當(dāng)粒子數(shù)為100時,不同時刻不同密度半徑下的粒子密度占比值。

表2 不同時刻不同密度半徑下的粒子密度占比 (%)

從表2可知,當(dāng)密度半徑為0.001時,粒子密度總體隨著迭代次數(shù)的增加而增大,但局部存在波動且隨著迭代次數(shù)增加粒子密度增長較緩慢,達(dá)到期望優(yōu)化程度時間復(fù)雜度較高;當(dāng)密度半徑為0.005與0.05時,隨著迭代次數(shù)的增加粒子密度的增長速度較快,但后者粒子密度在迭代次數(shù)為20左右就達(dá)到90%以上,迭代次數(shù)過少會導(dǎo)致精度下降問題。當(dāng)密度半徑為0.005時,隨著迭代次數(shù)的增加,粒子密度增長趨勢相對平緩,能較好反映粒子分布情況,因此,本文選取密度半徑為0.005。

2.2 粒子濾波估計精度分析

為驗(yàn)證本文算法有較好的估計性能,將BA-PF[8]、AHA-PF、IAHA-PF算法與傳統(tǒng)PF算法進(jìn)行對比實(shí)驗(yàn),并進(jìn)行誤差對比分析。基于單變量動態(tài)變化濾波模型[15]進(jìn)行粒子數(shù)為50、100和150時4種算法的仿真實(shí)驗(yàn),并給出濾波精度的對比和分析。本文選取的狀態(tài)方程和觀測方程為:

(21)

(22)

式中:x(t)、y(t)分別為t時刻系統(tǒng)的狀態(tài)量和觀測量,w(t)、v(t)為標(biāo)準(zhǔn)分布的高斯噪聲,系統(tǒng)初始狀態(tài)x為0,采樣時間步長為50,粒子數(shù)N為50、100、150時的狀態(tài)估計結(jié)果和誤差對比結(jié)果如圖2～圖4所示。

(a) 狀態(tài)估計值 (b) 估計誤差

(a) 狀態(tài)估計值 (b) 估計誤差

(a) 狀態(tài)估計值 (b) 估計誤差

可以看到,隨著粒子數(shù)目的增加,4種算法的估計精度均有提升,在同等情況下,BA-PF、AHA-PF與IAHA-PF的整體估計效果都比PF好,其中,IAHA-PF的估計誤差最小。其原因在于:AHA-PF運(yùn)用智能優(yōu)化這一思想優(yōu)化傳統(tǒng)算法,能有效提高狀態(tài)估計性能,但僅依靠經(jīng)驗(yàn)值會存在過度優(yōu)化導(dǎo)致精度下降或優(yōu)化不夠?qū)е抡`差較大的問題;IAHA-PF采用自適應(yīng)調(diào)整策略的AHA算法調(diào)整預(yù)測粒子集,避免了過度優(yōu)化或優(yōu)化不足的問題且減少時間復(fù)雜度,并且引入中垂線收斂策略提高算法收斂速度,此外,引入Levy飛行策略以提升全局搜索能力,能實(shí)時糾正估計誤差。

2.3 粒子分布多樣性測試

為驗(yàn)證IAHA-PF算法進(jìn)行狀態(tài)估計時粒子分布的多樣性,進(jìn)行仿真實(shí)驗(yàn)對比PF與IAHA-PF算法的粒子分布情況。粒子總數(shù)為100,分別在t=10、t=25和t=45時刻測試了粒子分布情況,如圖5所示。

(a) t=10時刻粒子分布對比圖 (b) t=25時刻粒子分布對比圖

可以看出,在不同采樣時刻,粒子濾波算法在重采樣階段去除小權(quán)值粒子而保留大權(quán)值粒子,引起粒子多樣性降低的現(xiàn)象,可以看到,幾乎大部分粒子都集中在一個區(qū)域,易導(dǎo)致濾波精度降低的問題。相比之下,IAHA-PF算法中大部分粒子都集中在期望狀態(tài)附近,同時有少部分粒子分布較分散,使得探索區(qū)域更廣,增加了粒子多樣性,有利于提高整體濾波精度。

2.4 自適應(yīng)調(diào)整迭代次數(shù)分析

為進(jìn)一步驗(yàn)證本文算法自適應(yīng)調(diào)整迭代次數(shù)的準(zhǔn)確性,分別在粒子數(shù)為50、100和150的情況下進(jìn)行20次仿真實(shí)驗(yàn),計算該算法的平均迭代次數(shù),最后計算20次實(shí)驗(yàn)的平均迭代次數(shù),結(jié)果如表3所示。

表3 不同粒子數(shù)的IAHA-PF平均迭代次數(shù)

可以看出,隨著粒子數(shù)目的增加,IAHA-PF算法的平均迭代次數(shù)會相應(yīng)減少,這是因?yàn)?粒子數(shù)的增加會提高粒子濾波的估計精度,達(dá)到期望優(yōu)化程度的迭代次數(shù)會相應(yīng)減少,由此可見,本文算法可根據(jù)當(dāng)前優(yōu)化程度自適應(yīng)調(diào)整迭代次數(shù),避免過度優(yōu)化或優(yōu)化不足導(dǎo)致的估計精度降低問題,且可自適應(yīng)減少迭代次數(shù),降低計算復(fù)雜度。

2.5 定位與建圖實(shí)驗(yàn)及分析

進(jìn)行仿真實(shí)驗(yàn)驗(yàn)證本文算法在SLAM中的估計性能,由于基于粒子濾波的FastSLAM因定位精度較高、魯棒性較好等特性已發(fā)展成SLAM技術(shù)的主流算法[15],所以將傳統(tǒng)FastSLAM與BA-FastSLAM、AHA-FastSLAM、IAHA-FastSLAM算法進(jìn)行仿真對比實(shí)驗(yàn),其中機(jī)器人的運(yùn)動模型與觀測模型如下:

(1)運(yùn)動模型,本文移動機(jī)器人運(yùn)動模型為:

(23)

式中:[xtytθt]T為t時刻機(jī)器人預(yù)測位姿,ΔT為機(jī)器人里程計采樣時間間隔。

(2)觀測模型,描述了機(jī)器人觀測值與當(dāng)前位姿估計和環(huán)境路標(biāo)之間的關(guān)系。

(24)

式中:dt和βt分別為t時刻機(jī)器人觀測到的路標(biāo)距離和角度。

根據(jù)Bailey開發(fā)的SLAM算法通用模擬器[1]設(shè)計了尺寸為100 m×82 m,有42個路標(biāo)的環(huán)境地圖及機(jī)器人運(yùn)行參照路徑,模擬機(jī)器人在真實(shí)場景中進(jìn)行瞬時定位與建圖,仿真環(huán)境如圖6所示,其中星號代表路標(biāo),細(xì)實(shí)線折線代表機(jī)器人的控制輸入量,即規(guī)定路徑,圓圈為機(jī)器人位姿點(diǎn),共有14個。仿真環(huán)境的移動機(jī)器人相關(guān)參數(shù)設(shè)置如表4所示。

表4 仿真實(shí)驗(yàn)機(jī)器人參數(shù)設(shè)置

(a) 傳統(tǒng)FastSLAM算法預(yù)測結(jié)果 (b) BA-FastSLAM算法預(yù)測結(jié)果

設(shè)置機(jī)器人相關(guān)參數(shù)后,機(jī)器人從坐標(biāo)原點(diǎn)出發(fā)沿預(yù)設(shè)路徑逆時針運(yùn)動一圈,通過運(yùn)動模型與觀測信息實(shí)現(xiàn)實(shí)時定位與建圖。仿真實(shí)驗(yàn)結(jié)果對比如圖6所示。

可以看出,傳統(tǒng)FastSLAM隨著誤差的累積,機(jī)器人路徑隨著時間推移誤差增大,路標(biāo)估計也出現(xiàn)較大誤差;相比之下BA-FastSLAM的路徑與路標(biāo)估計誤差都有所減小,但存在估計不穩(wěn)定現(xiàn)象,AHA-FastSLAM與IAHA-FastSLAM的機(jī)器人路徑與真實(shí)路徑基本一致,且路標(biāo)估計的誤差明顯減少;相比BA-FastSLAM與AHA-FastSLAM,IAHA-FastSLAM路徑估計更穩(wěn)定且誤差更小,同時環(huán)境路標(biāo)估計位置與實(shí)際路標(biāo)位置基本一致。其原因在于:AHA-FastSLAM運(yùn)用人工蜂鳥算法的智能覓食策略這一思想優(yōu)化傳統(tǒng)算法,能有效提高路徑與路標(biāo)估計性能,但僅依靠經(jīng)驗(yàn)值會存在過度優(yōu)化導(dǎo)致精度下降或優(yōu)化不夠?qū)е抡`差較大的問題;IAHA-FastSLAM采用改進(jìn)的AHA算法調(diào)整預(yù)測粒子集,避免了過度優(yōu)化或優(yōu)化不足的問題且減少時間復(fù)雜度,同時提升了AHA的收斂速度及全局搜索能力,能實(shí)時糾正機(jī)器人路徑估計,降低路標(biāo)估計誤差。

為進(jìn)一步驗(yàn)證本文算法應(yīng)用到實(shí)際場合中的有效性,在該仿真環(huán)境中,分別對傳統(tǒng)FastSLAM、BA-FastSLAM、AHA-FastSLAM、IAHA-FastSLAM4種算法進(jìn)行位姿預(yù)測誤差對比與路標(biāo)位置預(yù)測誤差對比,如圖7～圖9所示。

圖7 x軸方向位姿估計誤差

圖9 路標(biāo)位置估計誤差

圖7、圖8為4種算法的位姿預(yù)測誤差比較圖。可以看出,IAHA-FastSLAM的位姿預(yù)測誤差穩(wěn)定且最小;從圖9可以看出,IAHA-FastSLAM算法的路標(biāo)位置預(yù)測誤差相比BA-FastSLAM與AHA-FastSLAM算法更小。

考慮到實(shí)驗(yàn)結(jié)果的偶然性,為進(jìn)一步證明本文提出的算法性能的優(yōu)越性,引入均方根誤差(RMSE)對機(jī)器人路徑與路標(biāo)位置估計進(jìn)行評價[17],將4種算法分別進(jìn)行30次仿真實(shí)驗(yàn)并取平均值,采用RMSE作為衡量指標(biāo),表達(dá)式為:

(25)

表5 4種算法x軸、y軸、路標(biāo)位置均方根誤差對比 (m)

從表5可知,當(dāng)粒子數(shù)相同時BA-FastSLAM、AHA-FastSLAM和IAHA-FastSLAM算法的估計誤差要明顯小于FastSLAM,其中IAHA-FastSLAM的路徑估計與路標(biāo)估計誤差最小,且IAHA-FastSLAM采用自適應(yīng)調(diào)整迭代次數(shù)策略,時間復(fù)雜度更小。此外,IAHA-FastSLAM算法使用20個粒子的估計精度比FastSLAM算法使用100個粒子的估計精度更高,這是因?yàn)?本文引入混合策略的AHA更新預(yù)測粒子集,使預(yù)測粒子集在權(quán)重計算前就更靠近移動機(jī)器人真實(shí)位置,整體提高了粒子集質(zhì)量,此外,在重采樣階段采用粒子重組策略提高了粒子多樣性,有利于提高估計精度。因此,IAHA-FastSLAM算法不僅在粒子數(shù)相同時具有較高精度,還能減少粒子使用數(shù)量,以此減少計算量。

3 結(jié)論

本文提出一種基于多策略AHA優(yōu)化的粒子重組粒子濾波算法,引入混合策略的AHA更新預(yù)測粒子集,在引導(dǎo)覓食階段引入中垂線收斂策略提高算法收斂速度,區(qū)域性覓食階段通過Levy飛行策略擴(kuò)大搜索空間自適應(yīng)調(diào)整粒子集分布,當(dāng)粒子密集度過高時,自適應(yīng)調(diào)整優(yōu)化程度,使預(yù)測粒子集更快向移動機(jī)器人真實(shí)位置聚集,提高粒子集質(zhì)量;在重采樣階段,考慮大權(quán)值粒子和部分小權(quán)值粒子的共同作用重組粒子集。通過仿真實(shí)驗(yàn)驗(yàn)證了IAHA-PF較好的估計性能,將該算法應(yīng)用到SLAM中,仿真實(shí)驗(yàn)結(jié)果表明,在同等條件下,該算法定位及地圖構(gòu)建精度更高,綜合性能更強(qiáng)。在后續(xù)工作中,將進(jìn)一步合理減少算法時間復(fù)雜度,提高算法實(shí)時性,綜合提升SLAM的估計性能。