周寧亞 黃友銳 韓 濤

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

0 引 言

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

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

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

1 基本SLAM問題

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

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

(1)

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

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

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

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

2.1 傳統(tǒng)FastSLAM算法

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

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

(2)

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

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

(3)

FastSLAM算法步驟如下：

(1) 采樣新位姿，擴(kuò)展對機(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個粒子的權(quán)重。當(dāng)Neff小于設(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ù)主要是捕獵和照顧幼獅。它們首先探尋獵物的蹤跡，再通過協(xié)作來進(jìn)行圍捕。在追捕獵物時先進(jìn)行大范圍的搜索，當(dāng)靠近獵物時，縮小包圍圈，然后圍捕獵物。

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

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

2.2.1獅群初始化

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

(6)

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

2.2.2獅王守護(hù)

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

(7)

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

2.2.3母獅捕獵

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

(8)

(9)

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

2.2.4幼獅跟隨

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

(10)

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

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

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

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

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

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

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

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

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

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

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

算法步驟如下：

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

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

① 對新粒子集中的所有粒子計(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)行更新。

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

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

3.1 仿真模型

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

(11)

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

(12)

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

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

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

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

仿真模擬器環(huán)境界面如圖2所示，界面尺寸為200 m×160 m。仿真環(huán)境采用黑色星星為路標(biāo)點(diǎn)，數(shù)量設(shè)為35個，黑色空心圓為位姿點(diǎn)，數(shù)量設(shè)為17個，黑色細(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)硬盤，Windows 7系統(tǒng)。軟件開發(fā)環(huán)境為MATLAB 2010a。仿真程序中設(shè)置獅群的群體規(guī)模，向量空間維數(shù)D=2，最大迭代次數(shù)T=100，根據(jù)文獻(xiàn)[10]并經(jīng)過多次仿真實(shí)驗(yàn)發(fā)現(xiàn)比例因子β=2時收斂效果最好。

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

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

續(xù)表1

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

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

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

對比圖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兩種算法的誤差對比仿真圖。由圖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é)果對比圖

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

(13)

考慮到實(shí)驗(yàn)具有偶然性，對此本文分別對兩種算法在圖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)行時間上也明顯低于傳統(tǒng)算法，提高了FastSLAM的實(shí)時性。

4 結(jié) 語

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