基于粒子群優(yōu)化算法的機(jī)器人FastSLAM研究

程立英, 金新瑋, 肖 輝, 高宣爽, 張志美, 張浩華

(沈陽(yáng)師范大學(xué) 物理科學(xué)與技術(shù)學(xué)院, 沈陽(yáng) 110034)

0 引 言

機(jī)器人一直備受廣大愛(ài)好者以及相關(guān)學(xué)者的關(guān)注,并隨者計(jì)算機(jī)、網(wǎng)絡(luò)等相關(guān)技術(shù)的疾速發(fā)展,機(jī)器人領(lǐng)域亦隨之進(jìn)展迅速,其中移動(dòng)機(jī)器人實(shí)現(xiàn)自主性的關(guān)鍵技術(shù)之一----同時(shí)定位與建模(SLAM)技術(shù)成為機(jī)器人領(lǐng)域的研究熱點(diǎn)[1-4]。SLAM問(wèn)題是指當(dāng)機(jī)器人在位置環(huán)境中自主導(dǎo)航時(shí),增量式地創(chuàng)建該環(huán)境的連續(xù)地圖,同時(shí)確定移動(dòng)機(jī)器人在地圖中的位置。對(duì)于機(jī)器人來(lái)說(shuō),根據(jù)其傳感器獲取的信息來(lái)建立周邊的環(huán)境模型并對(duì)自身位姿估計(jì)是較為困難的一件事。早期的以擴(kuò)展卡爾曼濾波(EKF)方法為基礎(chǔ)的方法已成為處理SLAM問(wèn)題的基本方法之一,但基于EKF的方法要求假定系統(tǒng)的輸入噪聲和觀測(cè)噪聲必須服從高斯分布,這需要環(huán)境特征必須是唯一確定的。當(dāng)無(wú)法滿足這一點(diǎn)時(shí),就要采取其他方法例如粒子濾波器來(lái)實(shí)現(xiàn)SLAM。粒子濾波器也是一種蒙特卡洛方法,基本思想是利用新的觀測(cè)數(shù)據(jù)來(lái)更新機(jī)器人的位置分布[5-7]。很多學(xué)者結(jié)合優(yōu)化算法對(duì)FastSLAM算法進(jìn)行優(yōu)化,每個(gè)粒子發(fā)現(xiàn)的最好位置和整個(gè)群體中所有粒子發(fā)現(xiàn)的最好位置,進(jìn)而使粒子群向最優(yōu)的方向運(yùn)動(dòng),FastSLAM算法與傳統(tǒng)的SLAM算法相比較而言,其算法的計(jì)算量有著較大的降低,但其粒子退化問(wèn)題仍然得不到解決[8-13]。本文結(jié)合粒子群優(yōu)化算法對(duì)FastSLAM算法進(jìn)行改進(jìn)優(yōu)化,改進(jìn)算法將粒子群優(yōu)化的思想代入到粒子濾波中,粒子濾波算法不斷更新粒子的位置和權(quán)重值,以此逼近系統(tǒng)的真實(shí)后驗(yàn)概率分布,進(jìn)而使機(jī)器人逼近實(shí)際的位置。主要是在不需要增加粒子數(shù)量的前提下,在預(yù)估時(shí)考慮個(gè)體粒子和群體粒子共同的影響,進(jìn)而使機(jī)器人更接近真實(shí)系統(tǒng)狀態(tài)分布。實(shí)驗(yàn)證明,該方法預(yù)測(cè)的路徑與實(shí)際路徑之間的誤差相對(duì)于傳統(tǒng)的FastSLAM算法減小很多。

1 傳統(tǒng)的FastSLAM算法

1.1 傳統(tǒng)FastSLAM算法的原理

SLAM問(wèn)題中的輸入信息為從0時(shí)刻到k時(shí)刻的所有觀測(cè)信息z0:k及運(yùn)動(dòng)控制信息u1:k,SLAM的目的為依據(jù)其輸入信息來(lái)估計(jì)機(jī)器人的運(yùn)動(dòng)的路徑x0:k及地圖mk,其中x0:k為從0到k時(shí)刻所有時(shí)刻機(jī)器人的位姿,即機(jī)器人的運(yùn)動(dòng)路徑。SLAM的概率描述如公式(1)所示。

P(x0:k,mk|z0:k,u1:k)

(1)

對(duì)式(1)的Rao-Blackwellised分解如公式(2)所示。

(2)

mk,n為k時(shí)刻地圖中第n個(gè)路標(biāo)的位置,n=1,2,…,N;

p(x0:k|z0:k,u1:k)為機(jī)器人路徑估計(jì);

可以得出,FastSLAM算法中每個(gè)粒子sk均對(duì)應(yīng)了一個(gè)機(jī)器人的運(yùn)動(dòng)軌跡和地圖。其過(guò)程為,先由初始到k時(shí)刻機(jī)器人的測(cè)量值u1:k和觀測(cè)信息z0:k估計(jì)機(jī)器人的路徑軌跡x0:k的后驗(yàn)概率分布p(x0:k|z0:k,u1:k),然后由x0:k和z0:k計(jì)算地圖的后驗(yàn)概率p(mk|x0:k,z0:k,u1:k),從而得到系統(tǒng)狀態(tài)的概率分布。在FastSLAM算法中,機(jī)器人下一刻的位姿是從機(jī)器人的運(yùn)動(dòng)模型中采樣獲取:sk～p(mk|x0:k,u1:k),即用粒子集sk表示機(jī)器人下一時(shí)刻的可能位姿。

1.2 傳統(tǒng)FastSLAM算法的實(shí)現(xiàn)

圖1 傳統(tǒng)的FastSLAM算法流程圖Fig.1 The process oftraditional FastSLAM algorithm

傳統(tǒng)的FastSLAM算法由遞推的4步構(gòu)成:機(jī)器人位置預(yù)測(cè)、更新地圖特征、計(jì)算粒子權(quán)重及歸一化、重采樣。實(shí)現(xiàn)步驟如圖1所示。

首先進(jìn)行初始化操作:k=0時(shí),粒子集初始化為Sk=?;

1) 對(duì)每個(gè)粒子,利用運(yùn)動(dòng)模型p(Xr(k)|u(k),Xr(k-1))對(duì)機(jī)器人位姿進(jìn)行運(yùn)動(dòng)預(yù)測(cè);

2) 利用EKF濾波器及傳感器信息來(lái)更新路標(biāo)位置;

3) 對(duì)每個(gè)粒子,利用假定的運(yùn)動(dòng)軌跡的概率分布和估計(jì)的概率分布之間的差別計(jì)算其權(quán)重;

4) 通過(guò)計(jì)算得到的權(quán)重進(jìn)行加權(quán)采樣,得無(wú)權(quán)重的新粒子集。

不斷重復(fù)此過(guò)程,直到粒子的概率分布不斷地逼近真實(shí)的概率分布。

1.3 傳統(tǒng)FastSLAM算法的不足及相關(guān)優(yōu)化

傳統(tǒng)的FastSLAM算法本質(zhì)是系統(tǒng)狀態(tài)估計(jì)通過(guò)rao-blackwellised分解成機(jī)器人路徑軌跡的估計(jì)和地圖估計(jì)兩部分,其使用帶有權(quán)重的粒子表示機(jī)器人運(yùn)動(dòng)的狀態(tài)和地圖特征,但無(wú)法根據(jù)最新的觀測(cè)數(shù)據(jù)對(duì)機(jī)器人預(yù)測(cè)位置進(jìn)行及時(shí)更新。FastSLAM算法中機(jī)器人運(yùn)動(dòng)軌跡的概率分布由大量帶權(quán)重的粒子表示,其權(quán)重的大小體現(xiàn)著該粒子表示的機(jī)器人在此位置出現(xiàn)概率的高低。隨著序貫性采樣的不斷進(jìn)行,粒子間的權(quán)重值的差異則是逐步地增加,僅有少數(shù)的粒子保持較大的權(quán)重值而絕大多數(shù)的粒子的權(quán)重值將接近0,從而產(chǎn)生粒子退化的問(wèn)題。優(yōu)化重要性函數(shù)可以較少粒子退化的影響。重采樣通過(guò)復(fù)制權(quán)值比較高的粒子,淘汰權(quán)值比較低的粒子,使得粒子的分布逐步逼近真實(shí)的概率分布,較好地抑制粒子的退化問(wèn)題。但重采樣過(guò)程能使粒子快速地聚集到狀態(tài)空間的某些區(qū)域,其可能引發(fā)粒子過(guò)早聚集而錯(cuò)過(guò)機(jī)器人的真實(shí)位姿。粒子分布的貧乏同樣會(huì)影響FastSLAM算法逼近粒子真實(shí)的概率分布,因?yàn)镕astSLAM算法通過(guò)重采樣引導(dǎo)粒子向高概率軌跡聚集,但同時(shí)有可能陷入局部極值,從而喪失對(duì)粒子軌跡的最優(yōu)估計(jì)。

因此,引進(jìn)粒子群優(yōu)化算法對(duì)FastSLAM算法進(jìn)行優(yōu)化[14],在預(yù)估過(guò)程中,每個(gè)粒子綜合考慮到個(gè)體粒子和群體粒子共同的影響,不斷優(yōu)化更新粒子的位置和權(quán)重值,使得FastSLAM算法對(duì)粒子位姿預(yù)測(cè)時(shí),能夠?qū)崟r(shí)對(duì)粒子的最新位置進(jìn)行預(yù)測(cè),進(jìn)而更精準(zhǔn)地構(gòu)建地圖。

2 粒子群優(yōu)化的FastSLAM算法

2.1 算法改進(jìn)原理

在FastSLAM算法的基礎(chǔ)上,利用粒子群優(yōu)化方法對(duì)預(yù)估粒子進(jìn)行更新,調(diào)整了粒子的提議分布,使得位置預(yù)測(cè)過(guò)程實(shí)時(shí)考慮了最新路標(biāo)的觀測(cè)信息,預(yù)測(cè)采樣粒子集中于機(jī)器人的真實(shí)位置附近。在該方法中,主要為改進(jìn)機(jī)器人的位置預(yù)測(cè)過(guò)程,使得其在位置預(yù)測(cè)這一過(guò)程中,能實(shí)時(shí)考慮最新的路標(biāo)的觀測(cè)信息,同時(shí)在粒子預(yù)估時(shí)考慮到個(gè)體粒子和群體粒子共同的影響,從而獲得更接近真實(shí)系統(tǒng)狀態(tài)分布的預(yù)測(cè),而不需要增加粒子數(shù)量。主要從以下2個(gè)方面對(duì)算法進(jìn)行改進(jìn):

1) 首先,利用粒子群算法中的速度與位置方程對(duì)機(jī)器人的位置預(yù)測(cè)進(jìn)行更新,其更新的方程如公式(3)和(4)所示。

其中,drand1和drand2表示對(duì)角線矩陣,其對(duì)角線上的元素為正的標(biāo)準(zhǔn)正態(tài)分布的隨機(jī)數(shù);c1和c2表示學(xué)習(xí)因子;

xpbest和xgbest分別表示移動(dòng)機(jī)器人位姿預(yù)測(cè)值的額局部最優(yōu)解和全局最優(yōu)解。

2) 其次,引入適應(yīng)度函數(shù)判斷粒子群游湖啊算法對(duì)機(jī)器人位姿的優(yōu)化程度,適應(yīng)度函數(shù)定義如公式(5)所示。

(5)

(6)

2.2 改進(jìn)的算法實(shí)現(xiàn)

基于粒子群優(yōu)化的移動(dòng)機(jī)器人的FastSLAM方法為一個(gè)不斷迭代的過(guò)程,包含預(yù)測(cè)、粒子群優(yōu)化、權(quán)重計(jì)算和重采樣步驟:

1) 預(yù)測(cè):由提議分布對(duì)當(dāng)前粒子集進(jìn)行預(yù)測(cè)采樣,獲得下時(shí)刻的粒子集合,sk～p(sk|sk-1,uk);

2) 粒子群優(yōu)化:

③ 通過(guò)式(5)和式(6)計(jì)算路標(biāo)觀測(cè)的預(yù)測(cè)值以及適應(yīng)度函數(shù)值ffitness;

3) 權(quán)重計(jì)算:通過(guò)式(7)計(jì)算粒子的權(quán)重,可以提高求取方向位置速度的精度。

(7)

通過(guò)粒子群優(yōu)化,粒子集在計(jì)算權(quán)重前就更加接近機(jī)器人的真實(shí)位置,進(jìn)而讓權(quán)重計(jì)算更能體現(xiàn)粒子的分布情況,重采樣過(guò)程更加有效,加速了粒子集的收斂,為下一時(shí)刻機(jī)器人的位置預(yù)測(cè)提供了一個(gè)更好的初始值。

3 實(shí)驗(yàn)及分析

本文將采用MATLAB軟件進(jìn)行仿真實(shí)驗(yàn)[15]。建立的地圖模型如圖2所示,其中“*”表示路標(biāo),線條則為移動(dòng)機(jī)器人的控制輸入量,即移動(dòng)機(jī)器人的預(yù)設(shè)路徑軌跡。對(duì)于機(jī)器人來(lái)說(shuō),地圖中路標(biāo)和路徑均是未知的,圖2中34個(gè)路標(biāo),連接為路徑的17個(gè)點(diǎn)均為隨機(jī)生成。圖中所有的路標(biāo)估計(jì)值均以機(jī)器人的出發(fā)點(diǎn)為參照。采用最近鄰方法進(jìn)行數(shù)據(jù)關(guān)聯(lián),分別基于傳統(tǒng)的FastSLAM算法和改進(jìn)后基于粒子群優(yōu)化的FastSLAM算法進(jìn)行仿真,按照控制曲線進(jìn)行運(yùn)動(dòng),同時(shí)對(duì)周圍的路標(biāo)進(jìn)行探測(cè)的結(jié)果分別如圖3和圖4所示,其中“○”為機(jī)器人估計(jì)的路標(biāo)點(diǎn),黑色的線為機(jī)器人實(shí)際的運(yùn)動(dòng)路徑。

圖2 地圖模型Fig.2 The map model

圖3 傳統(tǒng)的FastSLAM算法的運(yùn)動(dòng)軌跡Fig.3 The result of traditional FastSLAM algorithm

圖4 改進(jìn)的FastSLAM算法的運(yùn)動(dòng)軌跡Fig.4 The result of improved FastSLAM algorithm

圖5 改進(jìn)算法與傳統(tǒng)算法的誤差比較Fig.5 Errors of improved and traditional algorithms

4 結(jié) 論

本文將粒子群優(yōu)化的思想結(jié)合到FastSLAM算法中,提出了一種基于粒子群優(yōu)化的FastSLAM方法,實(shí)驗(yàn)表明,該方法既可以發(fā)揮粒子濾波適用于任意非線性的系統(tǒng)的優(yōu)點(diǎn),在不需要增加粒子數(shù)量的情況下,逼近系統(tǒng)的真實(shí)后驗(yàn)概率分布,減小了生成路標(biāo)與實(shí)際路標(biāo)的誤差,進(jìn)而使機(jī)器人更接近真實(shí)系統(tǒng)狀態(tài)分布。