多策略鯨魚算法優化粒子濾波的SLAM精度研究

蔡 艷,楊光永,黃訓愛,徐天奇

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

0 引言

瞬時定位與地圖構建(simultaneous localization and mapping,SLAM)是實現移動機器人自主工作的關鍵技術之一。目前,SLAM的求解方法可大致分為基于卡爾曼濾波器的方法、基于粒子濾波器的方法、基于圖優化的方法[1]。基于蒙特卡洛方法的粒子濾波算法(particle filter,PF)不受非高斯噪聲的影響,是常用于解決SLAM問題的方法之一。

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

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

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

1 多策略鯨魚算法優化的粒子重組PF

1.1 標準鯨魚算法

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

1) 包圍狩獵

(1)

(2)

A=2ar1-a

(3)

C=2r2

(4)

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

2) 螺旋包圍

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

(5)

(6)

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

3) 隨機搜索

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

(7)

(8)

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

1.2 自適應調整策略

為避免WOA僅依賴經驗值設置迭代次數造成優化不足或過度優化的問題,引入種群密度監測階段實現自適應調整迭代次數,實時監測最優個體附近的種群密度,當密度達到閾值時,停止迭代。

(9)

式中：Nb表示最優個體周圍的鯨魚數;rb表示密度半徑;find操作表示查找密度半徑內的鯨魚數量。

ρb=Nb/N

(10)

式中：ρb表示最優個體周圍的種群密度;N表示粒子總數。

1.3 Levy飛行策略

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

(11)

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

(12)

(13)

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

1.4 粒子重組重采樣

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

1) 計算有效粒子數Neff以及粒子權重,并將權重按降序排列;

(14)

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

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

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

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

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

(15)

步驟3IWOA優化,更新粒子集,如表1所示;

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

步驟5計算粒子權重;

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

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

(16)

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

IWOA算法：

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

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

3.Whilet

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

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

6.Fori=1 toNdo

7.根據式(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)執行Levy飛行策略

13.End If

14.Else

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

16.End If

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

18.Ifρ≥pmax

19.停止迭代

20.End if

21.End For

22.t=t+1

19.End While

20.返回最優解Xb、優化后的種群Xi

1.6 IWOA-PF算法時間復雜度分析

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

2 仿真實驗

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

2.1 密度半徑設定

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

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

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

2.2 粒子濾波估計精度測試

為驗證本文算法有較好的估計性能,將WOA-PF、IWOA-PF算法與傳統PF算法進行對比實驗,并進行誤差對比分析。基于單變量動態變化濾波模型[17],對粒子數為50、80和100時,進行3種算法的仿真實驗,并給出濾波精度的對比和分析。本文選取的狀態方程和觀測方程如下：

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

(17)

(18)

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

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

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

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

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

2.3 粒子分布多樣性測試

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

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

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

2.4 自適應調整迭代次數分析

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

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

從表2可以看出,隨著粒子數目的增加,IWOA-PF算法的平均迭代次數會相應減少,這是因為,粒子數的增加會提高粒子濾波的估計精度,達到期望優化程度的迭代次數會相應減少,由此可見,本文算法可根據當前優化程度自適應調整迭代次數,避免過度優化或優化不足導致的估計精度降低問題,且可自適應減少迭代次數,降低計算復雜度。

2.5 定位與建圖實驗及分析

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

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

(19)

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

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

(20)

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

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

表3 仿真實驗機器人參數設置

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

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

圖5 3種算法預測結果示意圖

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

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

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

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

圖8 路標位置估計誤差

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

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

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

3 結論

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

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