基于IGA算法的FastSLAM算法研究

劉坤坤

摘 要：為了使自主移動機器人在SLAM（同步定位和地圖創(chuàng)建）上更加準確，分析了粒子濾波器（Particle Filter，PF）的FastSlam 算法在粒子退化和粒子早熟兩方面的不足，提出了一種改進算法（IGA算法）。該算法通過替代原有的重采樣過程，改善了粒子多樣性，提高了預測精度。在粒子早熟方面采用模擬退火思想對遺傳算子進行改進，避免了遺傳算法中的遺傳算子易陷入局部最優(yōu)解產(chǎn)生“早熟”現(xiàn)象問題。仿真結果表明，IGA算法使粒子保持的多樣性更加持久，算法精度持續(xù)時間更長。

關鍵詞：SLAM；Fast SLAM；IGA遺傳算法粒子濾波；粒子退化；粒子早熟

DOI：10.11907/rjdk.172234

中圖分類號：TP312 文獻標識碼：A 文章編號：1672-7800（2017）009-0055-06

Abstract：In order to make the autonomous mobile robot more accurate in SLAM， an improved algorithm （IGA algorithm） is proposed based on the shortcomings of Particle Filter （PF） FastSlam algorithm in particle degradation and early maturing of particles. The algorithm is used to replace the original resampling process to improve the particle diversity and improve the prediction accuracy. The genetic algorithm is improved by using simulated annealing in the early maturing of particles， which avoids the genetic algorithm in the genetic algorithm which is easy to fall into the local optimal solution and produce "premature" phenomenon. The simulation results show that the IGA algorithm makes the diversity of particles more durable and the algorithm has longer duration.

Key Words：Synchronous location and map creation （SLAM）； genetic algorithm particle filter； particle degeneration； particle premature

0 引言

SLAM（Simultaneous Localization and Mapping） 指機器人在未知環(huán)境中通過自身傳感器數(shù)據(jù)同時進行地圖創(chuàng)建與定位導航任務。目前典型的 SLAM 算法大致分為兩類，一種是概率定位模型方法，另一種是非概率定位模型方法。目前基于擴展卡爾曼濾波器（ Extended Kalman FIlter， EKF）的 EKF_SLAM 算法和基于粒子濾波器（Particle Filter， PF）的 FastSlam 算法都屬于概率定位，非概率定位有數(shù)據(jù)融合、掃描匹配、SM-SLAM等。

移動機器人同時定位與地圖創(chuàng)建是實現(xiàn)未知環(huán)境下機器人自主導航的關鍵技術，結合對SLAM問題的描述以及對FastSLAM1.0和FastSLAM2.0算法模型的分析，可以看出粒子早熟和粒子耗盡是困擾基于粒子群的SLAM算法的一個重要問題。GA-FastSLAM2.0算法利用遺傳優(yōu)化代替FastSLAM2.0的重采樣過程，主流的重采樣算法有重要性采樣算法、殘差重采樣算法、優(yōu)化組合重采樣等等。優(yōu)化組合重采樣法主要思想是充分利用權值較小的粒子，在復制某個粒子點時，通過采樣點和被廢棄的采樣點進行適當?shù)木€性組合，從而得到新的采樣點。該算法生成的粒子沒有重復，保證了粒子群的多樣性。這種變化改善了“粒子耗盡”問題，提高了算法的精度。但是通過實驗發(fā)現(xiàn)，GA-FastSLAM2.0算法雖然在移動機器人運行初期對粒子群的多樣性保持得很好，但是到了算法運算后期，粒子退化問題仍然會出現(xiàn)，這對需要長時間運行的實驗過程來說是行不通的。故本文提出改進遺傳算法中變異和交叉過程，使粒子多樣性保持更長時間，從而使整個SLAM算法能長時間高精度運行。

1 基于改進遺傳算法的FastSLAM算法

1.1 粒子濾波器

粒子濾波算法源于蒙特卡洛思想，即用某一事件出現(xiàn)的頻率代替該事件的概率。首先根據(jù)已知的先驗概率求其狀態(tài)變量的建議分布，接著利用該建議分布的狀態(tài)空間生成一群粒子，并在之后的估計過程中不斷更新每個粒子的權重值，利用這些權值來指代狀態(tài)變量的概率分布情況。基本的粒子濾波算法過程如下：

（1）粒子初始化。從初始化分布P（x0）中選取N個粒子，定義為：{xi0，i=1，2，…，N}，每個粒子初始化權重為wi0=1/N。

（2）重要性采樣。假設xik為近似后驗概率分布，建議分布函數(shù)為P（x1：k|z1：k，u1：k）。

計算權值：wik=wik-1P（zk|xik）P（xik|xik-1）P（xik|xik-1，z1：k）

（1） 計算歸一化權值：ik=wik∑Nj=1wjk

（2） （3）分析粒子退化情況。粒子退化現(xiàn)象是粒子濾波中普遍存在的問題也是研究難點，這主要是由于算法在多次迭代后，只有少數(shù)粒子的權重值得分較高，其余大多數(shù)粒子權重值得分過低，導致算法在運算后期大量的運算消耗在對結果影響較小的粒子上，降低了算法的運算效率。endprint

本文定義有效粒子數(shù)比例參數(shù)為Neff，Neff越小表示粒子退化現(xiàn)象越嚴重。本文將Neff參數(shù)定義為：Neff=1∑Ni=1（ik）2×100%

（3） （4）重采樣。重采樣過程是緩解粒子退化問題的常用途徑，即當Neff小于某一給定閾值Nmin時，算法通過增加權重值較大的粒子和減少權重值較小的粒子來更新粒子群。

主流的重采樣算法有重要性采樣算法、殘差重采樣算法、優(yōu)化組合重采樣等。優(yōu)化組合重采樣法的主要思想是：充分利用權值較小的粒子，在復制某個粒子點時，通過采樣點和被廢棄的采樣點進行適當?shù)木€性組合，從而得到新的采樣點。該算法生成的粒子沒有重復，保證了粒子群的多樣性。

（5）輸出。根據(jù)重采樣后更新的粒子群，計算出后驗概率估計公式為：P（xk|z1：k）=∑Ni=1ikδ（xk-xik）

（4） 狀態(tài)估計公式為：k=∑Ni=1ikxik

（5） 方差估計公式為：Pk=∑Ni=1ik（xik-k）（xik-k）T

（6） （6）判斷移動機器人運動過程是否完成任務，如果完成則結束程序，否則，令k=k+1，返回步驟（2）。

1.2 基于改進遺傳算法的粒子優(yōu)化

對粒子群存在的粒子耗盡問題，常用方法是對預測的粒子群進行重采樣操作，該方法可有效降低粒子早熟現(xiàn)象的發(fā)生，但算法運行時間長，算法效率低。本文提出用一種改進遺傳算法（improved genetic algorithm， IGA）替代原有的重采樣過程，改善了粒子多樣性、提高了預測精度。遺傳算法是模仿自然界生物進化機制的隨機全局搜索和優(yōu)化方法，是一種高效、并行、全局搜索的方法，能在搜索過程中自動獲取和累積所需的數(shù)據(jù)，并自適應地控制搜索過程以求最佳解。但遺傳算法中的遺傳算子易陷入局部最優(yōu)解，產(chǎn)生“早熟”現(xiàn)象，為解決這一問題，本文采用模擬退火思想對遺傳算子進行改進。

1.2.1 適應度與多樣性計算

在FastSLAM算法中，本文利用粒子權重（wk）表示粒子的適應度大小，因為粒子的權重大小直接反映了路徑估計的優(yōu)劣，即粒子適應度函數(shù)為：f（xk）=wk

（7） 本文采用k時刻粒子群的平均海明距離和粒子群最優(yōu)平均海明距離的比值表示粒子群的多樣性。粒子群多樣性函數(shù)為：Vdiv=ko

（8） 其中，Hk表示k時刻粒子群的平均海明距離，Ho表示k時刻粒子群最優(yōu)平均海明距離。

根據(jù)公式（7）和公式（8）計算出所有粒子的適應度和整個粒子群的多樣性。

1.2.2 粒子評估

根據(jù)每個粒子的適應度大小從低到高進行排序，并平均分為十檔，第一檔設為0分，之后每檔評分比前一檔多一分，將得分最好的一半粒子復制到優(yōu)化粒子群中。

1.2.3 交叉與變異

交叉和變異操作的概率為Pc和Pm，其值直接影響算法的收斂性。Pc和Pm的取值由粒子群的多樣性決定。多樣性較好時，Pc取較大值，Pm取較小值，而多樣性較差時，Pc取較小值，Pm取較大值。本文引入模擬退火思想，自適應地給出概率Pc和Pm的值，函數(shù)如下：

定義交叉操作的概率Pc為：Pc=sin（1T×π2）+0.4

（9） 定義變異操作的概率Pm為：Pm=sin（T×π2）+0.4

（10） 其中，T為模擬退火思想中的溫度概念，在算法初期，粒子種群多、多樣性好，需要粒子間更多的交叉操作來提高Pc值，保持粒子群多樣性。而到了算法后期，粒子多樣性下降，需要更多的變異操作來提高Pm值，緩解粒子多樣性下降速度。

（1）交叉操作。在優(yōu)化粒子群中，根據(jù)得分產(chǎn)生粒子群{xi}i=1，…，Pc*N/2，并從排序得分低的粒子群中隨機生成粒子群{xj}j=1，…，Pc*N/2，新粒子的位姿和地圖估計定義為：xcross=0.5（xi+xj）

（11） 其中，xcross為交叉操作后的子代，xi與xj為xcross的父代，而xi為xcross的直系父代。

計算完所有xcross后，分別計算每個進行交叉操作的粒子直系父代與子代適應度。如果子代適應度低于直系父代適應度，則用子代代替直系父代。否則，粒子群以概率P接受子代。

（2）變異操作。在優(yōu)化粒子群中，根據(jù)得分產(chǎn)生粒子群{xi}i=1，…，Pm*N/2，新粒子的地圖估計不變，位姿估計為：xvariance=（1+n*Nrand）xi

（12） 其中，xvariance為變異操作后的子代，xi為xvaricance的直系父代，Nrand是均值為0、方差為1的隨機數(shù)，n為較小正數(shù)，表示變異程度。

計算完所有xvariance后，分別計算每個進行交叉操作的粒子直系父代與子代適應度，如果子代適應度低于直系父代適應度，則用子代代替直系父代。否則，粒子群以概率P接受子代。

1.2.4 獲取最終優(yōu)化粒子群

將退火交叉和變異過程得到的新粒子與粒子估計得到的一半優(yōu)化粒子組合成最終的粒子群，并將所有粒子的權重設為1/N。

2 FastSLAM算法流程

算法偽碼如下：Start

設定0、P0、Q0、 R0值；

（1）基于差動移動機器人運動模型，利用控制輸入計算出移動機器人在k+1時刻的位姿狀態(tài)，即計算出每個粒子在k+1時刻對機器人位姿的預測均值和方差。

（2）將k時刻每個粒子估計觀測值與獲取的k+1時刻觀測值進行數(shù)據(jù)關聯(lián)，這些數(shù)據(jù)關聯(lián)是相互獨立的。

（3）根據(jù)每個粒子獲得的觀測值，計算出粒子位姿估計的均值和方差。利用這些均值和方差構建一個高斯分布函數(shù)作為重要性概率密度函數(shù)。

（4）利用IGA算法步驟對移動機器人進行路徑估計，并更新粒子群。endprint

（5）用公式（11）對SLAM問題進行Rao-Blackwellised分解架構，環(huán)境地圖的估計被分解成獨立的路標進行估計，而對每個獨立的路標估計，EKF既可滿足精度要求，也可滿足運算效率要求，所以地圖路標的后驗概率分布P（miX1∶k+1，Z1∶k+1，U1∶k+1）采用EKF算法進行估計。地圖估計表示為：{μ′，P′，…，μM，PM}，其中，μi表示地圖中第i個路標的高斯均值，Pi表示地圖中第i個路標的方差。因為本文采用靜態(tài)仿真地圖作為研究對象，所以可直接用k時刻路標的估計均值和方差作為μi和Pi。如果路標被觀察到，則使用KEF算法對地圖進行更新，否則此時刻的均值和方差等于上一時刻的均值和方差。

（6）檢測是否達到停止條件，如果未完成，則返回步驟（2）繼續(xù)執(zhí)行。

End

IGA-FastSLAM算法流程如圖1所示。

3 實驗結果分析

本文通過移動機器人傳感器感知周邊環(huán)境，并依靠周邊環(huán)境預測位置信息。利用IGA算法改進粒子濾波器，有效抑制了粒子退化，提高了移動機器人運動精度。

3.1 系統(tǒng)結構

本文利用MATLAB環(huán)境開發(fā)移動機器人運動仿真模型，為了更好地比較各算法的優(yōu)劣，本文對實驗環(huán)境進行統(tǒng)一設定。設置移動機器人每個輪子的運動速度為3m/s，兩差動輪之間的距離為1m，機器人可以觀測到30m以內(nèi)的路標信息，系統(tǒng)采樣時間為0.025s，速度誤差為0.3m/s，仿真環(huán)境大小為150m×150m，此環(huán)境中航標信息17個，路標信息35個，如圖2所示。

設置粒子重采樣閾值Neff<75%時進行重采樣。綠色*表示路標信息，紅色o表示航標信息，綠色三角表示真實移動機器人位置，地圖中的紅色點以及紅色三角表示預測的路標位置以及預測的移動機器人位置?？梢钥闯?，由于移動機器人觀測距離的限制，部分位于中間的路標未被觀測到。

3.2 算法比較和分析

為了比較傳統(tǒng)遺傳算法與本文改進遺傳算法對粒子退化問題的抑制效果，設計將兩種改進算法運用到同一個粒子濾波過程中，比較兩種情況下有效粒子數(shù)比例的變化。如圖3所示，X軸表示算法運行的迭代次數(shù)，Y軸表示有效粒子數(shù)比例。從圖中可以看出，算法剛運行時，有效粒子數(shù)比例很高，兩種算法粒子群多樣性保持很好。隨著算法迭代次數(shù)的增加，有效粒子數(shù)比例不斷下降，從圖3可以清楚地看出，基于傳統(tǒng)遺傳算法改進的粒子濾波器有效粒子數(shù)比例下降速率比改進的粒子濾波器下降速率要快。傳統(tǒng)算法在迭代20次左右時，有效粒子已經(jīng)降低到10%，而改進遺傳算法在迭代30次左右時才降低到10%。這是由于在遺傳算法的變異和交叉過程中，兩種運算過程的比例由基于模擬退火思想的公式（9）和公式（10）自適應取值，比傳統(tǒng)遺傳算法人為給定比例值更具客觀性。所以，本文算法能更有效地抑制粒子濾波器中粒子退化現(xiàn)象的產(chǎn)生。運用到SLAM問題中，則有效提高了算法預測精度。為了比較IGA-FastSLAM與FastSLAM2.0算法在預測移動機器人自身位姿信息時的準確性，本文分別讓移動機器人利用兩種SLAM算法在實驗環(huán)境中運行10次，每次測試在仿真環(huán)境中運行兩圈，計算移動機器人位姿x軸、y軸和整體的平均估計偏差值，如圖4、圖5、圖6所示。紅色表示IGA-FastSLAM算法估計誤差值，綠色表示GA-FastSLAM2.0算法估計誤差值，藍色表示FastSLAM2.0算法估計誤差值。

對比各算法X軸偏差值可以清楚地看出，在9 000步以前，機器人在未知環(huán)境中行走一圈，F(xiàn)astSLAM2.0算法產(chǎn)生的最大估計誤差為21.57m，GA-FastSLAM2.0算法產(chǎn)生的最大估計誤差為12.59m，而本文算法產(chǎn)生最大估計誤差為8.08m，比FastSLAM2.0算法估計誤差降低了62.54%，比GA-FastSLAM2.0算法估計誤差降低了35.82%。9 000步以后，3個算法估計誤差趨于平穩(wěn)，這是由于機器人運行到了第2圈，并不是面對完全未知的環(huán)境，所以對環(huán)境有了一定的預知性，降低了估計時的不確定性。9 000步以后IGA-FastSLAM算法分別比Fast SLAM2.0算法和GA-FastSLAM2.0算法估計誤差平均降低了48.98%和47.92%。

對比各算法Y軸估計偏差值：在第1圈測試中，IGA-FastSLAM算法分別比FastSLAM2.0算法和GA-FastSLAM2.0算法最大估計誤差降低了75.06%和69.73%，而在第2圈測試中，IGA-FastSLAM算法分別比FastSLAM2.0算法和GA-FastSLAM2.0算法估計誤差平均降低了48.98%和47.92%。

對比各算法整體估計誤差值，如圖6可以清楚看出，IGA-FastSLAM算法在整個測試過程中保持著良好的估計精度。在第1圈測試中，本文算法比FastSLAM2.0算法估計誤差降低了72.21%，比GA-FastSLAM2.0算法估計誤差降低了46.10%。而在第2圈趨于穩(wěn)定的預測過程中，本文算法比FastSLAM2.0算法估計誤差降低了59.04%，比GA-FastSLAM2.0算法估計誤差降低了24.30%。

比較各種算法的實時性，如表1所示。從表中可以清楚地看出IGA-FastSLAM比FastSLAM2.0算法和GA-FastSLAM2.0算法運算時間分別增加了約36.93%和6.91%。這是由于本文算法利用改進遺傳算法代替FastSLAM2.0算法的重采樣過程，算法復雜度要高于其它算法，導致算法運算時間增加。但考慮到本文算法對誤差的優(yōu)化效果大于運算時間的消耗，所以本文算法更適合解決SLAM問題。

為了對比不同粒子數(shù)對IGA-FastSLAM算法的影響，本文分別對初始化為10、50、100和200個的粒子數(shù)進行實驗。如圖7、圖8、圖9所示，紅色、黃色、藍色、綠色線條分別表示100個、10個、50個、200個粒子的IGA-FastSLAM實驗結果。從實驗結果可以清楚看出，隨著粒子數(shù)的增加，算法估計誤差不斷降低，但是降低的趨勢下降。為了更方便地比較各個算法精度，只觀察測試第2圈時的數(shù)據(jù)，如表2所示。從表中可以清楚看出，隨著粒子數(shù)的增多，X軸、Y軸和整體誤差不斷減小，但是減小的幅度越來越小，帶來的負面影響是算法計算時間的增加。從圖10可以看出，50個粒子比100粒子運算時間增加了約8.83%，誤差縮減了47.41%，但200粒子比100粒子運算時間增加了約10.09%，而誤差僅僅降低了18.65%。從經(jīng)濟學角度考慮，為增加算法精度而一味提高粒子個數(shù)，從而導致運算時間的大幅度消耗，這種優(yōu)化策略并不可取。endprint

為了充分證明IGA-FastSLAM算法可以適應各種環(huán)境，本文改變原有仿真環(huán)境，將航標設為9個、路標設為14個，再次比較FastSLAM2.0、GA-FastSLAM2.0以及IGA-FastSLAM算法的估計誤差值，如圖11所示。降低環(huán)境復雜度是為了方便比較各種算法之間的表現(xiàn)效果。

圖12、13、14分別表示FastSLAM2.0、GA-FastSLAM 2.0以及IGA-FastSLAM算法在新建仿真環(huán)境中移動機

器人的估計誤差值。從圖中可以清楚地看出，F(xiàn)astSLAM2.0算法無論在X軸、Y軸估計誤差值比較，還是在整體估計誤差值比較中，都表現(xiàn)出算法估計誤差遠高于其它兩種算法。通過計算，可得出FastSLAM2.0算法估計誤差比GA-FastSLAM2.0算法高56.21%，比IGA-FastSLAM算法高57.38%。比較本文算法和GA-FastSLAM2.0算法可以看出，在仿真程序開始時，兩種算法精度相差不大，這是由于GA-FastSLAM2.0已經(jīng)就重采樣問題進行了改進，有效提高了粒子群濾波器的多樣性。但從整體誤差圖不難看出，在仿真程序運行后期，本文算法的誤差值明顯小于GA-FastSLAM2.0算法。在程序的前半段，兩種算法的平均整體估計均為0.48m，但在程序的后半段，IGA-FastSLAM算法整體估計誤差比GA-FastSLAM2.0算法降低了約43.25%。這是因為本文算法利用改進的遺傳算法改進了粒子濾波器，比僅利用遺傳算法進行改進，粒子保持的多樣性更加持久，算法精度持續(xù)時間也就更長。

4 結語

本文從傳統(tǒng)粒子群算法出現(xiàn)的粒子早熟和粒子耗盡問題出發(fā)，利用改進遺傳算法對粒子濾波器重采樣過程進行優(yōu)化，使粒子多樣性保持更加持久。實驗結果表明，合理設置粒子個數(shù)能在保持高精度的前提下，降低計算機消耗，有效提高算法的運算效率。

參考文獻：

[1] GUPTA S K， GARG S. Chapter four-multiobjective optimization using genetic algorithm[J]. Advances in Chemical Engineering， 2013（43）：205-245.

[2] LIU J S， CHEN R. Sequential monte carlo methods for dynamic systems[J]. Journal of the American Statistical Association， 1998，93（443）：1032-1044.

[3] 周武，趙春霞.一種基于遺傳算法的FastSLAM 2.0算法[J]. 機器人， 2009，31（1）：25-32.

[4] WANZHOU YE. Optimality of the median filtering operator [J]. Circuits， Systems， and Signal Processing， 2011，30（6）：1329-1340.

[5] SB WILLIAMS， G DISSANAYAKE， H DURRANT WHYTE. An efficient approach to the simultaneous localisation and mapping problem[C]. IEEE International Conference on Robotics and Automation， 2002：406-411.

[6] 董海巍，陳衛(wèi)東.基于稀疏化的快速擴展信息濾波SLAM算法[J].機器人， 2008，30（3）：193-200.

[7] THRUN S， LIU Y， KOLLER D， et al. Simultaneous localization and mapping with sparse extended information filters[J]. Int.j.robot.res， 2004，23（7-8）：693-716.

[8] MOREFIELD C L. Application of 0-1 integer programming to multitarget tracking problems[J]. Automatic Control IEEE Transactions on， 1977，22（3）：302-312.

[9] COOPER A J. A comparison of data association techniques for simultaneous localization and mapping[D]. Aerospace Engineering and Mechanics， University of Minnesota， 2005.

[10] MONTEMERLO M， THRUN S. Simultaneous localization and mapping with unknown data association using fastSLAM[C].Proc of IEEE International Conference on Robotics and Automation， Taibei， 2003：1985-1991.

[11] OHY A， NAGASHIMA Y， YUTA S. Exploring unknown environment and map construction using ultrasonic sensing of normal direction of walls[J]. Proceedings of the IEEE International Conferences on Robotics and Automation， 1994（1）：485-492.

[12] 鄒國輝，敬忠良，胡洪濤.基于優(yōu)化組合重采樣的粒子濾波算法[J].上海交通大學學報，2006，40（7）：1135-1139.

（責任編輯：杜能鋼）endprint