多策略鯨魚算法優(yōu)化粒子濾波的SLAM精度研究

蔡 艷,楊光永,黃訓(xùn)愛,徐天奇

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

0 引言

瞬時定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)是實現(xiàn)移動機器人自主工作的關(guān)鍵技術(shù)之一。目前,SLAM的求解方法可大致分為基于卡爾曼濾波器的方法、基于粒子濾波器的方法、基于圖優(yōu)化的方法[1]?；诿商乜宸椒ǖ牧Ｗ訛V波算法(particle filter,PF)不受非高斯噪聲的影響,是常用于解決SLAM問題的方法之一。

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

受這些方法的啟發(fā),本文引入鯨魚算法改善粒子濾波器的性能。鯨魚優(yōu)化算法(whale optimization algorithm,WOA)是2016年由Mirjalili等[12]提出的一種新的群體智能優(yōu)化算法,該算法操作簡單,設(shè)置的參數(shù)少及跳出局部最優(yōu)的能力強,目前已被應(yīng)用于三維路徑規(guī)劃[13]、制造工藝[14]、變壓器故障診斷方法[15]、云上資源調(diào)度[16]等問題的求解之中。為避免僅依靠經(jīng)驗值設(shè)置迭代次數(shù),本文引入自適應(yīng)調(diào)整迭代次數(shù)策略,為增強其全局探索能力引入Levy飛行策略。

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

1 多策略鯨魚算法優(yōu)化的粒子重組PF

1.1 標準鯨魚算法

鯨魚優(yōu)化算法(whale optimization algorithm,WOA)是參照座頭鯨的氣泡網(wǎng)捕食機制提出的一種新型啟發(fā)式優(yōu)化算法,該算法包括以下3種行為方式。

1) 包圍狩獵

(1)

(2)

A=2ar1-a

(3)

C=2r2

(4)

式中：r1、r2為[0,1]之間的隨機數(shù);a從2到0線性下降,此階段|A|≤1。

2) 螺旋包圍

鯨魚圍捕獵物時,選擇螺旋包圍的概率是P,選擇包圍狩獵或隨機搜索的概率是1-P,螺旋包圍公式如下：

(5)

(6)

式中：l為(-1,1)內(nèi)的隨機數(shù);β為定義螺線形狀的常數(shù)參數(shù)。

3) 隨機搜索

鯨魚種群會根據(jù)彼此的位置進行隨機搜索,借此找到更好的獵物,這樣可以加強算法的勘察能力,從而進行全局搜索。隨機搜索公式如下：

(7)

(8)

式中：Xrand為隨機選擇的鯨魚位置向量。此階段,A>1。

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

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

(9)

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

ρb=Nb/N

(10)

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

1.3 Levy飛行策略

當(dāng)種群進行隨機搜索時,引入Levy飛行更新個體位置,通過步長變化擴大搜索空間,以提高算法全局搜索能力。當(dāng)種群密度大于設(shè)置的隨機搜索閾值時,采用Levy飛行策略更新個體位置,公式如下：

(11)

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

(12)

(13)

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

1.4 粒子重組重采樣

為解決粒子濾波算法在重采樣階段去除小權(quán)值粒子導(dǎo)致估計性能降低的問題,在重采樣階段設(shè)計如下粒子篩選策略：

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

(14)

1.5 基于多策略鯨魚算法的粒子重組粒子濾波

針對傳統(tǒng)粒子濾波算法存在的問題,本文引入WOA算法對其進行優(yōu)化以提高粒子集質(zhì)量;為避免優(yōu)化不足或過度優(yōu)化,同時減少時間復(fù)雜度,采用自適應(yīng)調(diào)整迭代次數(shù)策略;在隨機搜索階段,引入Levy飛行策略,擴大搜索空間自適應(yīng)調(diào)整粒子集分布;在重采樣階段,采用粒子重組策略增加粒子多樣性。IWOA-PF算法步驟如下：

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

步驟2從proposal分布中采樣粒子集,t時刻的采樣粒子集Xt如下：

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

(15)

步驟3IWOA優(yōu)化,更新粒子集,如表1所示;

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

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

步驟6粒子重組重采樣,根據(jù)式(14)重組粒子集;

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

(16)

多策略鯨魚算法偽代碼如下：

IWOA算法：

1.初始化鯨魚數(shù)量N、最大迭代次數(shù)T、個體維度d

2.初始化種群：Xi(i=1,2,…,N)

3.Whilet

4.檢查是否有鯨魚超出了搜索空間并進行修改

5.計算每條鯨魚的適應(yīng)度值,找到位置最佳的鯨魚Xb

6.Fori=1 toNdo

7.根據(jù)式(3)和式(4)更新a,A,C,l

8.Ifp<0.5 then

9.If |A|≤1 then

10.使用式(1)—式(4)進行收縮包圍策略

11.Else if |A|>1&&ρ≥pth

12.使用式(11)—式(13)執(zhí)行Levy飛行策略

13.End If

14.Else

15.使用式(5)和式(6)執(zhí)行螺旋包圍策略

16.End If

17.使用式(9)和式(10)計算最優(yōu)個體的粒子密度

18.Ifρ≥pmax

19.停止迭代

20.End if

21.End For

22.t=t+1

19.End While

20.返回最優(yōu)解Xb、優(yōu)化后的種群Xi

1.6 IWOA-PF算法時間復(fù)雜度分析

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

2 仿真實驗

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

2.1 密度半徑設(shè)定

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

表1 不同時刻不同密度半徑下的粒子密度占比

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

2.2 粒子濾波估計精度測試

為驗證本文算法有較好的估計性能,將WOA-PF、IWOA-PF算法與傳統(tǒng)PF算法進行對比實驗,并進行誤差對比分析?；趩巫兞縿討B(tài)變化濾波模型[17],對粒子數(shù)為50、80和100時,進行3種算法的仿真實驗,并給出濾波精度的對比和分析。本文選取的狀態(tài)方程和觀測方程如下：

8cos[1.2(t-1)]+w(t)

(17)

(18)

式中：x(t)、y(t)分別為t時刻系統(tǒng)的狀態(tài)量和觀測量;w(t)、v(t)為標準分布的高斯噪聲,系統(tǒng)初始狀態(tài)x為0;采樣時間步長為50。粒子數(shù)N為50、80、100時的狀態(tài)估計結(jié)果和誤差如圖1—圖3所示。

圖1 N=50時,3種算法對比實驗

圖2 N=80時,3種算法對比實驗

圖3 N=100時,3種算法對比實驗

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

2.3 粒子分布多樣性測試

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

圖4 不同時刻粒子分布多樣性實驗結(jié)果示意圖

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

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

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

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

從表2可以看出,隨著粒子數(shù)目的增加,IWOA-PF算法的平均迭代次數(shù)會相應(yīng)減少,這是因為,粒子數(shù)的增加會提高粒子濾波的估計精度,達到期望優(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 定位與建圖實驗及分析

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

1) 運動模型,本文仿真運動模型如下：

(19)

式中：[xtytθt]T表示t時刻機器人預(yù)測位姿;ΔT為機器人里程計采樣時間間隔。

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

(20)

式中：dt與βt分別為t時刻機器人觀測到的路標距離和角度。

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

表3 仿真實驗機器人參數(shù)設(shè)置

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

可以看出,隨著時間推移傳統(tǒng)FastSLAM誤差累積,機器人路徑誤差增大,路標估計也出現(xiàn)較大誤差;WOA-FastSLAM與IWOA-FastSLAM的機器人路徑與真實路徑基本一致,且路標估計的誤差明顯減少;IWOA-FastSLAM相比WOA-FastSLAM,其路徑估計更穩(wěn)定且誤差更小,同時,環(huán)境路標估計位置與實際路標位置基本一致。其原因在于：WOA-FastSLAM運用鯨魚算法的氣泡網(wǎng)捕食策略這一思想優(yōu)化傳統(tǒng)算法,能有效提高路徑與路標估計性能,但僅依靠經(jīng)驗值會存在過度優(yōu)化,導(dǎo)致精度下降或優(yōu)化不夠,從而出現(xiàn)誤差較大的問題;IWOA-FastSLAM采用改進的WOA算法調(diào)整預(yù)測粒子集,避免了過度優(yōu)化或優(yōu)化不足的問題且減少時間復(fù)雜度,同時提升了WOA的全局搜索能力,能實時糾正機器人路徑估計,降低路標估計誤差。

圖5 3種算法預(yù)測結(jié)果示意圖

為進一步驗證本文算法應(yīng)用到實際場合中的有效性,在該仿真環(huán)境中,分別對傳統(tǒng)FastSLAM、WOA-FastSLAM以及IWOA-FastSLAM 3種算法,進行位姿預(yù)測誤差對比與路標位置預(yù)測誤差對比,如圖6—圖8所示。

圖6、圖7為3種算法的位姿預(yù)測誤差曲線。可以看出,IWOA-FastSLAM的位姿預(yù)測誤差穩(wěn)定且最小;從圖8可以看出,IWOA-FastSLAM算法的路標位置預(yù)測誤差相比WOA-FastSLAM算法更小。

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

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

圖8 路標位置估計誤差

考慮到實驗結(jié)果的偶然性,為進一步證明本文提出的算法性能的優(yōu)越性,引入均方根誤差(RMSE)對機器人路徑與路標位置估計進行評價[19],將3種算法分別進行20次仿真實驗并取平均值,采用RMSE作為衡量指標,表達式如下：

表4 3種算法x軸、y軸、路標位置均方根誤差對比

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

3 結(jié)論

1) 提出一種基于多策略WOA優(yōu)化的粒子重組粒子濾波算法,引入混合策略的WOA更新預(yù)測粒子集,在隨機搜索階段,通過Levy飛行策略擴大搜索空間,自適應(yīng)調(diào)整粒子集分布,當(dāng)粒子密集度過高時,自適應(yīng)調(diào)整優(yōu)化程度,使預(yù)測粒子集更快向移動機器人真實位置聚集,提高粒子集質(zhì)量;在重采樣階段,考慮大權(quán)值粒子和部分小權(quán)值粒子的共同作用重組粒子集。

2) 通過仿真驗證了IWOA-PF較好的估計性能,將該算法應(yīng)用到SLAM中,結(jié)果表明,當(dāng)粒子數(shù)為20時,IWOA-FastSLAM相比傳統(tǒng)FastSLAM,在x軸位姿估計精度提高了83.8%,在y軸位姿估計精度提高了77.6%,路標估計精度提高了81.4%。在同等條件下,該算法定位及地圖構(gòu)建精度更高,綜合性能更強,為智能移動設(shè)備的自動化作業(yè)提供了一種精確定位與建圖的方法,可為工業(yè)機器人智能化研究提供參考。