FastSLAM算法的仿生優化改進研究

張鎧翔，姜文剛

(江蘇科技大學 電子信息學院，江蘇 鎮江 212000)

0 引 言

同時定位與地圖創建(simultaneous localization and mapping，SLAM)是探索未知環境的關鍵因素之一，尤其是在機器人自主導航領域更是扮演著至關重要的角色。SLAM的過程就是移動機器人在未知環境中進行探索，在不斷移動的過程中通過自身攜帶的傳感器獲得的控制信息與觀測信息進行環境地圖的增量式創建，并在建造的地圖中確定自身的位置。其中的控制信息和觀測信息都會存在不確定的噪聲，所以可以看出SLAM的本質就是概率估計的問題[1]。

一般由概率方法來解決SLAM問題的經典算法分為基于擴展卡爾曼濾波SLAM(EKF-SLAM)算法和快速SLAM(FastSLAM)算法[2-4]。但在實際的應用環境中，移動機器人的運動模型與觀測模型一般是非線性的，EKF算法主要是通過一階泰勒展開式來近似非線性模型，所以避免不了引入線性化誤差[5]，這便導致該算法無法在大范圍的環境中運行。Murphy[5]通過對全狀態濾波器分解，提出了一種基于Rao—Blackwellise粒子濾波器(RBPF)的新方法來解決SLAM問題。隨后Montemerlo等[6]提出的FastSLAM算法，對解決SLAM問題起到了很大的推動作用。

FastSLAM主要的思想是將機器人的運動狀態與建立地圖分化成對機器人的運動估計以及基于位姿的路標估計兩個主要的過程。其中在機器人運動估計中使用的則是粒子濾波進行機器人位姿定位，地圖上的每個路標估計則是使用的EKF估計[1]。相比一些傳統的SLAM算法，FastSLAM算法的計算量大大降低，但在使用粒子濾波進行運動狀態估計的過程中很容易出現粒子退化這一類問題[7]，此類問題一直是眾多學者研究的熱點，也涌現了大量的較為優秀的改進策略。目前在算法改進策略上有一大部分的研究者通過融入仿生算法對粒子濾波進行優化，因其具有很強的尋優性能，能夠快速、智能地將粒子集中至高似然區域，有效地改善了樣本貧化和粒子多樣性喪失現象[8-9]，進而提高了濾波精度。像基于果蠅算法優化的粒子濾波算法[8]、基于蝙蝠算法的粒子濾波算法[9]等都是通過結合仿生算法進行粒子優化，其過程是初始化每個粒子最初時刻位置，并通過不斷迭代發現局部最優值以及全局最優值，進而不斷更新位置向粒子群中最優解的位置靠近。但這些結合仿生算法優化后的粒子一定程度上存在盲目搜索，導致出現尋優結果出現較多偏差、容易陷入早熟或收斂速度較慢的問題。

針對FastSLAM算法存在的缺陷，融合蝴蝶算法進行優化，主要的改進策略是將蝴蝶種群覓食的思想代入到FastSLAM的運動狀態估計中，傳統的蝴蝶算法中并沒有使用當前時刻的測量值，從而使得算法結果存在較大的偏差。針對此問題，該文引入機器人運動過程中最新時刻的觀測值和狀態值，另外傳統的蝴蝶算法在局部搜索飛行過程中，通過個體的香味進行互相吸引，但需要整個群體進行交互運算，這便產生較大計算量，導致較差的實時性。因此引入自適應香味半徑和自適應調整飛行步長因子[10]，同時在重采樣的過程中引入偏差修正指數加權平均(exponentially weighted average，EWA)算法[11]，分離出對估計精度影響較大的大權值粒子，對剩余部分的中小權值粒子進行分布采樣，以提高粒子篩選的準確性，自適應調整重采樣粒子數，增加粒子的多樣性，減緩粒子退化。

1 機器人傳統的FastSLAM算法

1.1 FastSLAM的基本原理

p(X1:k,m|Z1:k,u1:k,n1:k)=

(1)

其中，X1:k為機器人的路徑估計；m為路標位置的集合；Z1:k為機器人的觀測量；u1:k為機器人的控制輸入量；n1:k為數據關聯表，每個nk為k時刻觀測點到的路標點。

1.2 FastSLAM算法的實現

FastSLAM算法基本上由五步構成：機器人狀態預測、數據關聯、重要性采樣、地圖更新、重采樣。具體步驟如下：

(1)機器人的下一時刻的位置預測。基于每一時刻輸入的控制信息和機器人運動模型進行預測。

(2)數據關聯。將觀測信息和各個粒子t時刻的路標特征依次進行數據關聯。

(3)重要性采樣。重要性權值可以根據下面的公式遞歸計算求得。

(2)

粒子的歸一化權值為：

(3)

(4)地圖更新。采用EKF算法關聯觀測信息更新每個特征的估計。

(5)重采樣。設置閾值并計算有效粒子數Neff。若Neff小于閾值則進行重采樣。

重采樣過程能夠將分散狀態空間各處粒子較快地聚合到某些區域，但這樣可能存在的問題是粒子過早地收斂陷入局部最優解導致機器人運動狀態偏離真實狀態概率分布，從而使得粒子的估計狀態存在較大偏差。

因此，將改進后的蝴蝶算法融合到FastSLAM算法上可以讓其在處理重要性采樣的時候，能夠充分考慮粒子間的作用關系，得到更加準確的粒子均值與方差，這樣可以很好地保證整個系統在運行過程的效率，大大地提高算法性能，在重采樣過程中，對自適應重采樣進行改進，提高粒子多樣性，獲得更加準確的定位與地圖。

2 基于改進的蝴蝶算法優化的FastSLAM算法

2.1 蝴蝶算法

蝴蝶算法(butterfly algorithm，BA)[13]是一種基于蝴蝶覓食行為的優化算法。在收斂精度與速度上都要高于其他算法。其思想是在每個蝴蝶搜尋食物時，自身會發出具有一定濃度的香味，并可以通過香味的濃度吸引周圍的其他蝴蝶，其他蝴蝶則會朝著香味較多的個體進行移動[14]。影響蝴蝶的香味最主要的因素是感知形態、刺激強度以及冪指數。用方程表示為：

F=cIa

(4)

其中，F是香味濃度，c是感知形式，I是刺激強度，a是冪指數。

已知目標函數f(x)，蝴蝶算法的步驟如下：

(a)初始化蝴蝶種群，并通過目標函數f(xi)確定每只蝴蝶xi的刺激強度Ii。

(b)計算蝴蝶種群的個體適應度值，根據這個適應度值來確立在所定蝴蝶種群中處于最佳性能的那只蝴蝶。

(c)計算蝴蝶散發的香味?？紤]到環境因素的干擾，使用隨機數P作為搜索方式的更換標志，有利于后期的位置更新。

(d)全局搜索。低香味的蝴蝶個體向全局適應度最高的蝴蝶飛行，全局搜索的表達式為:

(5)

(e)局部搜索，蝴蝶個體進行隨機飛行。局部搜索的表達式為:

(6)

為了避免蝴蝶進入局部最優，Lévy飛行被引入到算法中。

Lévy～u=t-λ，1<λ≤3

(7)

Lévy飛行可以有效提高搜索效率。一般λ取(1,3]。

2.2 改進蝴蝶算法

改進的蝴蝶算法粒子濾波[14]中提出引入粒子濾波中當前時刻的觀測值，進一步地提高算法的濾波精度。該文在此基礎上進行優化，采用最新時刻觀測和狀態值作為反饋信息對當前目標預估結果進行修正，并重新定義蝴蝶算法的刺激強度公式表達：

(8)

(9)

2.2.1 自適應香味半徑

蝴蝶算法具有很強的全局搜索能力，然而一旦個體的數量不斷增大，計算復雜度也會相應增大。改進的蝴蝶算法粒子濾波[14]提出的自適應半徑，可以有效將蝴蝶個體間吸引的范圍限定在固定區域內，進而可以減少粒子數目的使用，將其引入到FastSLAM粒子濾波階段可以很好地降低系統的計算量。香味半徑的大小由香味濃度決定，其香味半徑與香味濃度成正比關系。對于兩只香味不同的蝴蝶，位置更新的條件則是任意兩個蝴蝶間距小于等于這兩個蝴蝶中香味濃度最大那個蝴蝶的香味半徑，否則不更新。該文定義的半徑的公式為：

r=η*Fi

(10)

其中，r為相對吸引半徑；Fi為種群個體的香味值；η作為調節半徑參數，一般在(0，1)之間取值。加入自適應半徑之后相鄰粒子間吸引更加快速，則引入參數后的算法，在準確率與運行效率上都有所提高。

2.2.2 自適應調整飛行步長因子

模擬蝴蝶算法的實際運行初期，每個預設的蝴蝶間距較大，較高的全局尋優能力能夠使蝴蝶粒子較晚地陷入局部極值。在算法不斷迭代后期，蝴蝶的個體可移動步長逐漸變小，較強的局部尋優能力有利于蝴蝶算法后期的收斂，不會因可移動間距過大而跳過全局最優解。因此固定的步長因子會降低濾波整體的性能，導致預測精度低和收斂速度慢[15]。

針對以上問題，該文設計一種非線性衰減函數，加入觀測噪聲的方差，保證模型對于實際目標函數有較好的近似，兼顧了收斂速度與穩定誤差，實現α值的自適應調整：

(11)

α(i)×(rand÷2)

(12)

2.3 自適應部分分布重采樣

FastSLAM中粒子濾波的重采樣一般采用的是自適應重采樣，其基本思想是：若采樣后的有效粒子數小于設定的閾值，則進行重采樣，否則不重采樣。此方法可以有效地緩解粒子退化、預估精度降低等問題。為了進一步提高粒子的多樣性，該文改進自適應重采樣方法，并通過基于偏差修正指數加權平均算法優化粒子篩選，選出大權值的粒子，并對中小權值的粒子進行分布重采樣，并設計如下策略：

計算粒子權重并按升序排列；

采用基于偏差修正指數加權平均算法得到粒子集的平均值分布曲線，其表達式如下：

(13)

該算法中的偏差修正項可以降低前期加權平均值的誤差，提供更加精確的平均值，進而更好區分出大權值，對中小權值進行采樣，有效緩解了粒子退化，提高了粒子的多樣性。

2.4 改進后的FastSLAM算法實現

結合上面引入的改進蝴蝶算法和自適應部分分布重采樣的算法對FastSLAM進行優化，優化具體步驟如下：

預測：由提議分布對當前粒子集合進行預測采樣，獲得下一時刻的粒子集合，sk～p(sk|sk-1,uk)。

仿生算法優化粒子集合：將粒子集融合改進后的蝴蝶算法，粒子集合根據自身的吸引半徑自適應地更新位置尋找到最優解，得到更加符合真實分布的后驗概率密度。

重要性采樣：計算出融合蝴蝶算法優化后粒子的后驗位置和姿態建議分布，從建議分布中獲得新的粒子集，并歸一化權重。

地圖更新：結合最新觀測值更新機器人位姿和路標位置。

重采樣：當有效粒子數小于設置閾值時，采用改進后的自適應部分分布重采樣。

改進后的FastSLAM中通過對粒子的仿生優化使得粒子集更加符合真實分布，以及收斂速度上也有所提高，因此可以在下一時刻得到更好的初始值。

3 實驗分析

該文利用MATLAB2012a仿真軟件分別對傳統FastSLAM、BA-FastSLAM與文中算法IBA-FastSLAM在稀疏路標環境下進行仿真實驗并比較，以驗證提出的算法的性能。

仿真中用到的運動模型與觀測模型如式(14)和式(15)所示。

(14)

(15)

其中，(xi,yi)為地圖中第i存在的路標點；(ri,θi)為相對于機器人的位置傳感器所檢測到的路標點的距離以及方向夾角；εt為觀測噪聲，其噪聲服從高斯分布。

仿真實驗的機器人參數配置如表1所示。

表1 FastSLAM仿真實驗運動參數配置

仿真實驗中采用50個粒子，并建立約為200 m*160 m的定位與建圖的模擬環境，此環境一共有36個路標點與18個航向點，在以上建立的環境下對FastSLAM、BA-FastSLAM、IBA-FastSLAM分別進行移動機器人定位與建圖的仿真實驗，得到的實驗結果如圖1～圖3所示。

圖1 FastSLAM

圖2 BA-FastSLAM

圖3 IBA-FastSLAM

由圖可知，利用仿生尋優思想后一定程度上優化了粒子位置與路標位置，但依然存在較大的偏差，而文中在蝴蝶算法基礎上進行改進的算法得到的估計路徑更加貼合實際路徑，路標的估計精度也相對提高。

(a)x方向

圖5 路標估計誤差比較

對三種算法的性能進行對比，如圖4、圖5所示。IBA-FastSLAM在位姿估計偏差以及路標估計偏差上有少許部分稍微大于BA-FastSLAM和FastSLAM，但整體上隨著時間推移，IBA-FastSLAM不管是位姿估計精度還是路標估計精度明顯要比BA-FastSLAM和FastSLAM這兩種算法高。

為了進一步驗證算法的穩定性，分別利用20，40，60粒子數對兩種算法進行仿真實驗，計算兩種算法在不同粒子數時，連續運行系統20次的位姿估計的均方根誤差的均值和方差以作為評價算法性能的標準，如圖6所示。

圖6 不同粒子數在位姿估計的均方根誤差的均值和方差

通過圖6所示，從整體上分析IBA-FastSLAM算法與原始的BA-FastSLAM在均方根誤差的均值和方差都要高于FastSLAM，這表明融合了蝴蝶算法后，粒子得到了優化，使得系統的精度以及穩定性都有明顯的提升，一定程度上表明了仿生算法的引入，可以很好地提高原始FastSLAM的性能。而文中提出的IBA-FastSLAM算法比原始的BA-FastSLAM算法在均方根的均值和方差上具有更高的精度與穩定性，表明了未改進的蝴蝶算法存在一定程度的缺陷，滿足不了系統的實際需求，進一步說明改進后的蝴蝶算法性能上遠遠要優于原始的蝴蝶算法。從不同粒子數的對比中可以看出，傳統的FastSLAM隨著粒子數的增加，誤差也隨之成非線性大幅度下降，可見，傳統FastSLAM對于粒子數的依賴較大，而BA-FastSLAM相對于傳統FastSLAM誤差都有所降低，但對于粒子數的依賴還是較大，IBA-FastSLAM均方根誤差的均值在不同粒子數之間變化，相對于BA-FastSLAM與FastSLAM都要較平穩，其主要原因是改進后的重采樣保證粒子多樣性。從這一點可以看出，提出的IBA-FastSLAM算法對于通過增加粒子數來緩解粒子退化，提高粒子多樣性以及濾波精度的需求相對降低，因此在選用粒子數量的時候可以適量減少采樣粒子數，來降低計算量。

為了對三種算法在綜合性能上進行比較，設定粒子數為50，分別對三種算法連續運行20次，得出三種算法在均方根的均值與方差以及運行時間對比，如表2所示。可以看出，BA-FastSLAM在預測精度與穩定性上分別比FastSLAM提高了17.10%和27.99%，表明融合蝴蝶算法后的FastSLAM在預測精度和穩定性上都有所提高，而運行時間卻多于FastSLAM算法7.711秒，相當于收斂速度降低了7.9%，可見原始的BA-FastSLAM算法存在一定計算量較大的問題。而提出的IBA-FastSLAM均方根的均值和方差分別是0.379 5和0.231，在預測精度和穩定性上分別比FastSLAM提高了40.6%和41.22%，同時比改進前的BA-FastSLAM提高了28.34%和6.27%，表明在香味公式中融入最新觀測值和狀態值，以及引入香味半徑后的蝴蝶算法提高了FastSLAM中粒子的濾波能力，減少了誤差。IBA-FastSLAM在運行時間上是95.721秒，與FastSLAM的運行時間相比，縮短了3.876秒，而相對于BA-FastSLAM縮短了11.587秒，相當于收斂速度提高了10.93%，主要原因是提出的自適應蝴蝶步長因子加快了粒子向最優位置飛行速率。因此可以結合圖6所得出的結論推出，IBA-FastSLAM算法可以在保持粒子數較小的情況下實現更加準確的預測，能從根本上改善粒子匱乏的現象以及粒子的多樣性，從而使系統更加具有實時性，一定程度提升了系統運行的效率，因此能夠使移動機器人在未知場景下進行精確定位與建圖，從而合理地去決策與規劃路徑，更好地滿足移動機器人的實際環境中的應用需求。

表2 三種算法的均方根的均值與方差以及運行時間對比

4 結束語

對FastSLAM的粒子濾波中的粒子集合融入了改進后的蝴蝶算法，對重要性采樣過程進行優化，在粒子位置更新的過程中加入自適應香味半徑和自適應蝴蝶飛行調制步長因子來控制蝴蝶種群尋優的搜索范圍，能夠有效保證算法的運行時間縮短和預測精度的提高，在重采樣過程中加入偏差修正指數加權算法劃分出穩定與不穩定的粒子集，并對劃分后放入不穩定的粒子部分進行分布重采樣，減少不穩定的粒子對重采樣階段的干擾，進而保證粒子的多樣性。通過仿真實驗結果表明，提出的IBA-FastSLAM算法可以在不需要增加過多粒子數量的情況下，既可以改善粒子的退化和多樣性，又可以提升位姿與路標預測精度與穩定性，以及算法的收斂速度，進而使機器人更實時地接近真實系統狀態分布。上述方法為移動機器人的自主導航提供了一種精確的定位與建圖的方法。