基于獅群優(yōu)化的FastSLAM算法

周寧亞 黃友銳 韓 濤

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

0 引 言

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

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

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

1 基本SLAM問(wèn)題

從概率的角度來(lái)看，SLAM問(wèn)題涉及對(duì)機(jī)器人路徑后驗(yàn)值的估計(jì)以及對(duì)地圖的估計(jì)。

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

(1)

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

將SLAM問(wèn)題畫(huà)成一個(gè)動(dòng)態(tài)貝葉斯網(wǎng)絡(luò)[11]可以更方便地理解這一點(diǎn)。如圖1所示，機(jī)器人在t時(shí)刻機(jī)器人的位姿記為xt，該位姿是機(jī)器人位姿xt-1和機(jī)器人執(zhí)行的控制量ut的概率函數(shù)，傳感器在時(shí)間t的觀測(cè)值，記作zt，由位姿xt和觀測(cè)到的路標(biāo)mn決定。機(jī)器人在t-1時(shí)刻和t+1時(shí)刻觀察到的路標(biāo)均是mi，在t時(shí)刻觀察到的路標(biāo)是mj，從xt-1到xt為機(jī)器人的完整路徑。

圖1 SLAM動(dòng)態(tài)貝葉斯網(wǎng)絡(luò)

2 算法設(shè)計(jì)

2.1 傳統(tǒng)FastSLAM算法

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

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

(2)

式中：nt={n1,n2,…,nt}為相關(guān)性變量，每個(gè)nt指定t時(shí)刻觀測(cè)到的路標(biāo)的標(biāo)識(shí)。這一因子分解說(shuō)明SLAM問(wèn)題可以分解為N+1個(gè)遞歸估計(jì)量的乘積：機(jī)器人路徑上的一個(gè)估計(jì)量，路標(biāo)位置上的N個(gè)獨(dú)立估計(jì)量，且每個(gè)獨(dú)立估計(jì)量都以路徑估計(jì)為條件(每個(gè)路標(biāo)是相互獨(dú)立的)。等式右邊前一項(xiàng)被稱為路徑估計(jì)，F(xiàn)astSLAM算法利用粒子濾波器來(lái)實(shí)現(xiàn)此估計(jì)，粒子濾波器中的每個(gè)粒子都代表一條機(jī)器人可能的路徑，利用觀測(cè)信息計(jì)算每個(gè)粒子的權(quán)重大小來(lái)評(píng)價(jià)每條路徑的好壞；后一項(xiàng)是基于該路徑估計(jì)下的路標(biāo)估計(jì)，采用N個(gè)擴(kuò)展卡爾曼濾波器分別估計(jì)N個(gè)路標(biāo)。

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

(3)

FastSLAM算法步驟如下：

(1) 采樣新位姿，擴(kuò)展對(duì)機(jī)器人路徑的后驗(yàn)估計(jì)：

(4)

(2) 利用擴(kuò)展卡爾曼濾波器更新觀察到路標(biāo)估計(jì)。

(3) 計(jì)算重要性權(quán)重：

(4) 根據(jù)權(quán)重判斷是否進(jìn)行重要性重采樣：

(5)

式中：Neff為有效粒子數(shù)；ω[k]表示第k個(gè)粒子的權(quán)重。當(dāng)Neff小于設(shè)置的閾值時(shí)進(jìn)行重采樣，否則不進(jìn)行。

2.2 獅群算法

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

獅王是獅群中最強(qiáng)壯和兇猛的公獅，是在“弱肉強(qiáng)食，勝者為王”的自然界生存法則產(chǎn)生的首領(lǐng)，有守護(hù)領(lǐng)土和保護(hù)幼獅以及分配食物的職責(zé)。

母獅在獅群中的任務(wù)主要是捕獵和照顧幼獅。它們首先探尋獵物的蹤跡，再通過(guò)協(xié)作來(lái)進(jìn)行圍捕。在追捕獵物時(shí)先進(jìn)行大范圍的搜索，當(dāng)靠近獵物時(shí)，縮小包圍圈，然后圍捕獵物。

幼獅在獅群中屬于跟隨獅，主要有三種活動(dòng)，一是饑餓時(shí)向獅王附近位置移動(dòng)尋找食物；二是食飽后跟隨母獅學(xué)習(xí)捕獵；三是長(zhǎng)大后將被驅(qū)趕出獅群進(jìn)行磨練，在經(jīng)過(guò)磨練后可以對(duì)獅王發(fā)起挑戰(zhàn)。

獅群優(yōu)化算法的主要思想如下:從待尋優(yōu)空間中的某一初始位置開(kāi)始，其中具有最佳適應(yīng)度值的就是獅王，然后選取一定比例的捕獵獅，捕獵獅相互配合捕獵，一旦發(fā)現(xiàn)比當(dāng)前獅王占有的獵物更優(yōu)質(zhì)的獵物，該獵物的位置會(huì)被獅王擁有。幼獅跟隨母獅學(xué)習(xí)打獵或在獅王附近進(jìn)食，成年后會(huì)被驅(qū)趕出獅群，為了生存，被驅(qū)趕的獅子會(huì)努力朝記憶中的最佳位置靠近。獅群內(nèi)部分工合作，不斷重復(fù)搜尋，最終得出目標(biāo)函數(shù)最優(yōu)值。

2.2.1獅群初始化

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

(6)

式中：β為成年獅所占比例因子，為(0,1)內(nèi)的一個(gè)隨機(jī)數(shù)。令待尋優(yōu)的閾值向量為yi=(yi1,yi2,…,yiD),1≤i≤Q。捕獵過(guò)程中不同類型的獅子都會(huì)按照自己的方式來(lái)移動(dòng)自身的位置。

2.2.2獅王守護(hù)

從待尋優(yōu)空間的初始化位置開(kāi)始，計(jì)算適應(yīng)度值，其中具有最佳適應(yīng)度值的為獅王，記為L(zhǎng)lion。獅王在最佳食物處小范圍移動(dòng)，以確保自己的特權(quán)，按照式(7)更新自己的位置:

(7)

獅王只負(fù)責(zé)照顧幼獅和保護(hù)領(lǐng)土和給幼獅分配食物，直到進(jìn)入下一次迭代，被更為強(qiáng)壯和兇猛的成年獅替代。

2.2.3母獅捕獵

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

(8)

(9)

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

2.2.4幼獅跟隨

幼獅是跟隨獅，主要圍繞獅王和母獅進(jìn)行活動(dòng)，按式(10)計(jì)算和調(diào)整自己的位置。

(10)

獅群優(yōu)化步驟如下：

(1) 初始化相關(guān)參數(shù)。

(2) 根據(jù)式(6)計(jì)算三類獅子數(shù)量，并確定各個(gè)獅子此刻的位置，占據(jù)最優(yōu)位置的為獅王。

(3) 根據(jù)式(7)更新獅王位置，并計(jì)算適應(yīng)度值。

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

(5) 產(chǎn)生(0,1)內(nèi)的均勻隨機(jī)數(shù)q，通過(guò)式(10)更新幼獅的位置。

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

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

(8) 輸出獅王的位置，即所求問(wèn)題的最優(yōu)解，算法結(jié)束。

2.3 基于獅群優(yōu)化的FastSALM算法

針對(duì)傳統(tǒng)FastSLAM出現(xiàn)的粒子退化和多樣性缺失問(wèn)題，本文引入獅群這一新型群智能算法優(yōu)化FastSLAM。獅群內(nèi)部分工合作，擴(kuò)大搜索范圍，減緩粒子多樣性缺失，加快收斂，通過(guò)不斷重復(fù)搜尋最優(yōu)值的方法，來(lái)優(yōu)化調(diào)整機(jī)器人的建議分布情況，促使粒子集朝最優(yōu)的真實(shí)位置移動(dòng)，同時(shí)改善FastSLAM算法中出現(xiàn)的粒子退化和多樣性缺失問(wèn)題。

算法步驟如下：

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

(2) 對(duì)新粒子集進(jìn)行獅群優(yōu)化，調(diào)整粒子分布情況：

① 對(duì)新粒子集中的所有粒子計(jì)算適應(yīng)度函數(shù)，確定獅群構(gòu)成。

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

③ 更新適應(yīng)度函數(shù)，進(jìn)而更新自身歷史最優(yōu)位置。

④ 判斷是否達(dá)到迭代次數(shù)，若達(dá)到輸出最優(yōu)解，否則返回步驟②。

(3) 根據(jù)式(5)計(jì)算粒子權(quán)值。

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

(5) 更新地圖：利用擴(kuò)展卡爾曼進(jìn)行更新。

對(duì)傳統(tǒng)FastSLAM算法引入獅群優(yōu)化，調(diào)整粒子的建議分布，使得粒子集更快地朝著真實(shí)的機(jī)器人位姿狀態(tài)逼近，為下一刻采樣提供更好的輸入值。

3 仿真實(shí)驗(yàn)

3.1 仿真模型

(1) 運(yùn)動(dòng)模型。本文仿真運(yùn)動(dòng)模型為：

(11)

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

(12)

式中：rt和φt分別表示機(jī)器人在t時(shí)刻離觀測(cè)到的路標(biāo)的距離和角度值；(xi,yi)表示觀測(cè)到的第i個(gè)路標(biāo)的坐標(biāo)。

(3) 噪聲模型。移動(dòng)機(jī)器人在運(yùn)動(dòng)過(guò)程中，會(huì)受到機(jī)器人輪子打滑、外界干擾以及傳感器等誤差的影響，這種影響會(huì)給實(shí)際測(cè)量值帶來(lái)很多不確定性，為了簡(jiǎn)單起見(jiàn)，我們通常用高斯白噪聲近似表示噪聲模型。

3.2 實(shí)驗(yàn)結(jié)果與分析

為驗(yàn)證本文算法的正確性和有效性，對(duì)本文提出的LSO-FastSLAM算法進(jìn)行仿真實(shí)驗(yàn)，并與基礎(chǔ)FastSLAM算法進(jìn)行對(duì)比，仿真平臺(tái)是在MATLAB R2012a版本下，使用悉尼大學(xué)發(fā)布的模擬器[13]進(jìn)行測(cè)試的。

仿真模擬器環(huán)境界面如圖2所示，界面尺寸為200 m×160 m。仿真環(huán)境采用黑色星星為路標(biāo)點(diǎn)，數(shù)量設(shè)為35個(gè)，黑色空心圓為位姿點(diǎn)，數(shù)量設(shè)為17個(gè)，黑色細(xì)曲線為機(jī)器人預(yù)設(shè)路徑軌跡，仿真模擬機(jī)器人在環(huán)境中運(yùn)行的軌跡。

圖2 MATLAB模擬器仿真界面

實(shí)驗(yàn)環(huán)境配置為：Intel酷睿i3，4 GB內(nèi)存，300 GB固態(tài)硬盤(pán)，Windows 7系統(tǒng)。軟件開(kāi)發(fā)環(huán)境為MATLAB 2010a。仿真程序中設(shè)置獅群的群體規(guī)模，向量空間維數(shù)D=2，最大迭代次數(shù)T=100，根據(jù)文獻(xiàn)[10]并經(jīng)過(guò)多次仿真實(shí)驗(yàn)發(fā)現(xiàn)比例因子β=2時(shí)收斂效果最好。

移動(dòng)機(jī)器人的相關(guān)參數(shù)如表1所示。

表1 機(jī)器人仿真參數(shù)設(shè)置

續(xù)表1

對(duì)機(jī)器人相關(guān)參數(shù)設(shè)置后，機(jī)器人從坐標(biāo)為(0,0)位置開(kāi)始沿著位姿點(diǎn)逆時(shí)針運(yùn)動(dòng)行走一周。分別對(duì)兩種算法進(jìn)行仿真實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如圖3和圖4所示。

圖3 傳統(tǒng)FastSLAM算法估計(jì)結(jié)果

圖4 基于獅群優(yōu)化的FastSLAM估計(jì)結(jié)果

對(duì)比圖3、圖4可以看出：傳統(tǒng)FastSLAM算法隨著累積誤差的影響，后面機(jī)器人的路徑嚴(yán)重偏離了真實(shí)路徑，路標(biāo)估計(jì)也出現(xiàn)較大誤差；加入獅群優(yōu)化的FastSLAM算法估計(jì)的路徑與真實(shí)路徑基本一致，估計(jì)的路標(biāo)位置也基本一致，精度得到大幅提高。

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

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

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

(c) 路標(biāo)位置估計(jì)誤差圖5 算法仿真結(jié)果對(duì)比圖

為了進(jìn)一步說(shuō)明本文算法的優(yōu)越性能，引入均方根誤差RMSE[14]，本文算法性能用機(jī)器人路徑和路標(biāo)位置的均方根誤差(RMSE)進(jìn)行評(píng)價(jià)，計(jì)算公式如下：

(13)

考慮到實(shí)驗(yàn)具有偶然性，對(duì)此本文分別對(duì)兩種算法在圖1環(huán)境下運(yùn)行30次仿真實(shí)驗(yàn)，然后分別計(jì)算機(jī)器人位姿估計(jì)和路標(biāo)估計(jì)的RMSE值，如表2所示。

表2 算法性能比較

可以看出，本文提出的LSO-FastSLAM算法的RMSE不僅在路標(biāo)和路徑上優(yōu)于傳統(tǒng)FastSLAM算法，而且在運(yùn)行時(shí)間上也明顯低于傳統(tǒng)算法，提高了FastSLAM的實(shí)時(shí)性。

4 結(jié) 語(yǔ)

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