基于獅群優化的FastSLAM算法

周寧亞 黃友銳 韓 濤

(安徽理工大學電氣與信息工程學院 安徽 淮南 232001)

0 引 言

同時定位與建圖(SLAM)是機器人在未知環境下進行自主導航等任務的基本問題[1]，FastSLAM是一種成功求解SLAM問題的算法。FastSLAM中使用的RaoBlackwellized Particle Filter(RBPF)[2]算法與其他算法相比，具有線性時間復雜度和多假設數據關聯兩大優點。然而，FastSLAM在精度方面會隨時間發生退化[3-5]，當機器人估計姿態的粒子集失去多樣性時，就會發生這種退化。FastSLAM中粒子多樣性的喪失主要有兩個原因：(1) 由于目標分布與建議分布不匹配導致樣本貧化，產生不可靠粒子，導致對機器人姿態估計不準確。(2) 在FastSLAM重采樣過程中，不可靠的粒子被丟棄，只有權重大的粒子存活。但是，如果準確估計機器人姿態的粒子權重較低，則會被誤丟棄，那么被丟棄的粒子中存儲的機器人信息將無法恢復。換句話說，對粒子權重的錯誤計算導致了粒子損耗問題。

近年來，許多算法被提出來克服粒子耗盡問題。Kwak等[6]對FastSLAM中使用的幾種粒子濾波器進行了分析，提出了一種針對損耗問題的補償技術。Kim等[7]提出了一種Unscented FastSLAM算法，該算法利用無跡變換進行粒子濾波、特征初始化和特征估計。Cugliari等[8]利用無跡變換提高了粒子濾波和特征估計器的精度，并提出了一種自適應選擇性重采樣技術。到目前為止，這些技術已經顯示出比傳統的FastSLAM更好的性能，但其對緩解粒子退化問題效果并不佳，只是試圖保持多樣性來改進FastSLAM算法。

為了提高FastSLAM的性能，本文引入了群體智能算法——獅群優化算法。獅群算法[9-10]比遺傳算法、粒子群算法和萬有引力算法等具有更好的全局收斂性，收斂速度更快，精度更高，能更好地獲得全局最優解。首先通過獅群優化算法對FastSLAM中預測粒子進行更新，調整粒子的建議分布。其次對粒子群進行分工合作擴大搜索范圍，增加粒子多樣性。最后通過“適者生存”的競爭法則促使粒子更快地朝著真實的機器人位姿狀態逼近，減緩粒子退化。

1 基本SLAM問題

從概率的角度來看，SLAM問題涉及對機器人路徑后驗值的估計以及對地圖的估計。

p(xt,m|zt,ut,nt)

(1)

式中：xt={x1,x2,…,xt}為機器人的路徑，xt表示t時刻機器人的位姿；m為地圖，由一系列環境特征組成，每個特征用mn表示，靜止特征的總數設為N，為了計算方便，通常認為它們的坐標和數量是已知的且坐標是唯一確定的，相互是獨立的；zt={z1,z2,…,zt}、ut={u1,u2,…,ut}分別為t時刻之前的機器人的觀測量和控制量。

將SLAM問題畫成一個動態貝葉斯網絡[11]可以更方便地理解這一點。如圖1所示，機器人在t時刻機器人的位姿記為xt，該位姿是機器人位姿xt-1和機器人執行的控制量ut的概率函數，傳感器在時間t的觀測值，記作zt，由位姿xt和觀測到的路標mn決定。機器人在t-1時刻和t+1時刻觀察到的路標均是mi，在t時刻觀察到的路標是mj，從xt-1到xt為機器人的完整路徑。

圖1 SLAM動態貝葉斯網絡

2 算法設計

2.1 傳統FastSLAM算法

FastSLAM算法是由Montemerlo等[14]首先提出來的，利用一組隨機地從分布中抽取出的粒子(樣本)來表示機器人的位姿。Montemerlo等利用貝葉斯理論將式(1)的條件聯合概率分布分解為條件概率：

p(xt,m|zt,ut,nt)=p(xt|zt,ut,nt)·

(2)

式中：nt={n1,n2,…,nt}為相關性變量，每個nt指定t時刻觀測到的路標的標識。這一因子分解說明SLAM問題可以分解為N+1個遞歸估計量的乘積：機器人路徑上的一個估計量，路標位置上的N個獨立估計量，且每個獨立估計量都以路徑估計為條件(每個路標是相互獨立的)。等式右邊前一項被稱為路徑估計，FastSLAM算法利用粒子濾波器來實現此估計，粒子濾波器中的每個粒子都代表一條機器人可能的路徑，利用觀測信息計算每個粒子的權重大小來評價每條路徑的好壞；后一項是基于該路徑估計下的路標估計，采用N個擴展卡爾曼濾波器分別估計N個路標。

所以，在FastSLAM中每個粒子k可以表示為：

(3)

FastSLAM算法步驟如下：

(1) 采樣新位姿，擴展對機器人路徑的后驗估計：

(4)

(2) 利用擴展卡爾曼濾波器更新觀察到路標估計。

(3) 計算重要性權重：

(4) 根據權重判斷是否進行重要性重采樣：

(5)

式中：Neff為有效粒子數；ω[k]表示第k個粒子的權重。當Neff小于設置的閾值時進行重采樣，否則不進行。

2.2 獅群算法

獅群算法[10]是一種新型群體智能算法，其根據自然界獅子真實的生存狀態將獅群按照一定比例分為雄獅、母獅和幼獅三類。

獅王是獅群中最強壯和兇猛的公獅，是在“弱肉強食，勝者為王”的自然界生存法則產生的首領，有守護領土和保護幼獅以及分配食物的職責。

母獅在獅群中的任務主要是捕獵和照顧幼獅。它們首先探尋獵物的蹤跡，再通過協作來進行圍捕。在追捕獵物時先進行大范圍的搜索，當靠近獵物時，縮小包圍圈，然后圍捕獵物。

幼獅在獅群中屬于跟隨獅，主要有三種活動，一是饑餓時向獅王附近位置移動尋找食物；二是食飽后跟隨母獅學習捕獵；三是長大后將被驅趕出獅群進行磨練，在經過磨練后可以對獅王發起挑戰。

獅群優化算法的主要思想如下:從待尋優空間中的某一初始位置開始，其中具有最佳適應度值的就是獅王，然后選取一定比例的捕獵獅，捕獵獅相互配合捕獵，一旦發現比當前獅王占有的獵物更優質的獵物，該獵物的位置會被獅王擁有。幼獅跟隨母獅學習打獵或在獅王附近進食，成年后會被驅趕出獅群，為了生存，被驅趕的獅子會努力朝記憶中的最佳位置靠近。獅群內部分工合作，不斷重復搜尋，最終得出目標函數最優值。

2.2.1獅群初始化

設獅群中的獅子數量為Q，維度空間為D，其中成年獅子的數量為qLeader：

(6)

式中：β為成年獅所占比例因子，為(0,1)內的一個隨機數。令待尋優的閾值向量為yi=(yi1,yi2,…,yiD),1≤i≤Q。捕獵過程中不同類型的獅子都會按照自己的方式來移動自身的位置。

2.2.2獅王守護

從待尋優空間的初始化位置開始，計算適應度值，其中具有最佳適應度值的為獅王，記為Llion。獅王在最佳食物處小范圍移動，以確保自己的特權，按照式(7)更新自己的位置:

(7)

獅王只負責照顧幼獅和保護領土和給幼獅分配食物，直到進入下一次迭代，被更為強壯和兇猛的成年獅替代。

2.2.3母獅捕獵

確定獅王后，選取一定比例的捕獵獅，開始時目標搜索空間只有一頭公獅，所以母獅的數量為nLeader-1。母獅在捕食過程中需要與另一頭母獅合作，按式(8)更新自己的位置：

(8)

(9)

式中：αf為母獅移動范圍擾動因子，這是為了加強局部開發能力，讓母獅在捕獵過程中先在較大的范圍內勘探食物，確定大致范圍后，勘探范圍慢慢縮小，后期保持活動范圍趨于零的微小值；step為獅子的最大步長；T為種群最大迭代次數；t為當前迭代次數。

2.2.4幼獅跟隨

幼獅是跟隨獅，主要圍繞獅王和母獅進行活動，按式(10)計算和調整自己的位置。

(10)

獅群優化步驟如下：

(1) 初始化相關參數。

(2) 根據式(6)計算三類獅子數量，并確定各個獅子此刻的位置，占據最優位置的為獅王。

(3) 根據式(7)更新獅王位置，并計算適應度值。

(4) 通過式(8)更新母獅的位置。

(5) 產生(0,1)內的均勻隨機數q，通過式(10)更新幼獅的位置。

(6) 計算適應度值，更新自身歷史最優位置和獅群歷史最優位置，判斷算法是否滿足終止條件，若滿足跳轉到步驟8，否則跳轉步驟7。

(7) 間隔一定的迭代次數(約10次)對獅群進行重新排序，重新確定三類獅子的位置，跳轉到步驟3。

(8) 輸出獅王的位置，即所求問題的最優解，算法結束。

2.3 基于獅群優化的FastSALM算法

針對傳統FastSLAM出現的粒子退化和多樣性缺失問題，本文引入獅群這一新型群智能算法優化FastSLAM。獅群內部分工合作，擴大搜索范圍，減緩粒子多樣性缺失，加快收斂，通過不斷重復搜尋最優值的方法，來優化調整機器人的建議分布情況，促使粒子集朝最優的真實位置移動，同時改善FastSLAM算法中出現的粒子退化和多樣性缺失問題。

算法步驟如下：

(1) 根據式(4)進行采樣，獲取新粒子集。

(2) 對新粒子集進行獅群優化，調整粒子分布情況：

① 對新粒子集中的所有粒子計算適應度函數，確定獅群構成。

② 位置更新，獅王根據式(7)進行位置更新；母獅根據式(8)進行位置更新；幼獅根據式(10)進行位置更新。

③ 更新適應度函數，進而更新自身歷史最優位置。

④ 判斷是否達到迭代次數，若達到輸出最優解，否則返回步驟②。

(3) 根據式(5)計算粒子權值。

(4) 重要性重采樣：計算Neff判斷是否進行重采樣。

(5) 更新地圖：利用擴展卡爾曼進行更新。

對傳統FastSLAM算法引入獅群優化，調整粒子的建議分布，使得粒子集更快地朝著真實的機器人位姿狀態逼近，為下一刻采樣提供更好的輸入值。

3 仿真實驗

3.1 仿真模型

(1) 運動模型。本文仿真運動模型為：

(11)

(2) 觀測模型。觀測模型zt使用傳感器與某一路標的距離值和角度值表示，在t時刻，機器人的觀測模型為：

(12)

式中：rt和φt分別表示機器人在t時刻離觀測到的路標的距離和角度值；(xi,yi)表示觀測到的第i個路標的坐標。

(3) 噪聲模型。移動機器人在運動過程中，會受到機器人輪子打滑、外界干擾以及傳感器等誤差的影響，這種影響會給實際測量值帶來很多不確定性，為了簡單起見，我們通常用高斯白噪聲近似表示噪聲模型。

3.2 實驗結果與分析

為驗證本文算法的正確性和有效性，對本文提出的LSO-FastSLAM算法進行仿真實驗，并與基礎FastSLAM算法進行對比，仿真平臺是在MATLAB R2012a版本下，使用悉尼大學發布的模擬器[13]進行測試的。

仿真模擬器環境界面如圖2所示，界面尺寸為200 m×160 m。仿真環境采用黑色星星為路標點，數量設為35個，黑色空心圓為位姿點，數量設為17個，黑色細曲線為機器人預設路徑軌跡，仿真模擬機器人在環境中運行的軌跡。

圖2 MATLAB模擬器仿真界面

實驗環境配置為：Intel酷睿i3，4 GB內存，300 GB固態硬盤，Windows 7系統。軟件開發環境為MATLAB 2010a。仿真程序中設置獅群的群體規模，向量空間維數D=2，最大迭代次數T=100，根據文獻[10]并經過多次仿真實驗發現比例因子β=2時收斂效果最好。

移動機器人的相關參數如表1所示。

表1 機器人仿真參數設置

續表1

對機器人相關參數設置后，機器人從坐標為(0,0)位置開始沿著位姿點逆時針運動行走一周。分別對兩種算法進行仿真實驗，實驗結果如圖3和圖4所示。

圖3 傳統FastSLAM算法估計結果

圖4 基于獅群優化的FastSLAM估計結果

對比圖3、圖4可以看出：傳統FastSLAM算法隨著累積誤差的影響，后面機器人的路徑嚴重偏離了真實路徑，路標估計也出現較大誤差；加入獅群優化的FastSLAM算法估計的路徑與真實路徑基本一致，估計的路標位置也基本一致，精度得到大幅提高。

圖5為傳統FastSLAM和LSO-FastSLAM兩種算法的誤差對比仿真圖。由圖5(a)、(b)可以看出，LSO-FastSLAM算法的位姿估計誤差均明顯低于傳統FastSLAM算法，且誤差幅度變化不大；由圖5(c)可以看出，LSO-FastSLAM算法的路標估計誤差也明顯低于傳統FastSLAM算法。

(a) x軸方向上的位姿估計誤差

(b) y軸方向上的位姿估計誤差

(c) 路標位置估計誤差圖5 算法仿真結果對比圖

為了進一步說明本文算法的優越性能，引入均方根誤差RMSE[14]，本文算法性能用機器人路徑和路標位置的均方根誤差(RMSE)進行評價，計算公式如下：

(13)

考慮到實驗具有偶然性，對此本文分別對兩種算法在圖1環境下運行30次仿真實驗，然后分別計算機器人位姿估計和路標估計的RMSE值，如表2所示。

表2 算法性能比較

可以看出，本文提出的LSO-FastSLAM算法的RMSE不僅在路標和路徑上優于傳統FastSLAM算法，而且在運行時間上也明顯低于傳統算法，提高了FastSLAM的實時性。

4 結 語

本文提出了一種基于獅群優化的FastSLAM算法，該算法創新性地將獅群優化思想引入到傳統FastSLAM算法中，通過獅群優化算法內部分工合作，擴大搜索范圍，增加粒子多樣性，利用其優越的全局收斂性，不斷迭代搜尋最優值，促使粒子更快地朝著真實的機器人位姿狀態逼近，減緩粒子退化。仿真實驗驗證了在等同條件下，本文算法的可行性和優越性。