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

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

0 引 言

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

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

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

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

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

(1)

對式(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均對應(yīng)了一個(gè)機(jī)器人的運(yùn)動軌跡和地圖。其過程為,先由初始到k時(shí)刻機(jī)器人的測量值u1:k和觀測信息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)動模型中采樣獲取: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ù)測、更新地圖特征、計(jì)算粒子權(quán)重及歸一化、重采樣。實(shí)現(xiàn)步驟如圖1所示。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

(5)

(6)

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

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

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

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

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

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

(7)

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

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

本文將采用MATLAB軟件進(jìn)行仿真實(shí)驗(yàn)[15]。建立的地圖模型如圖2所示,其中“*”表示路標(biāo),線條則為移動機(jī)器人的控制輸入量,即移動機(jī)器人的預(yù)設(shè)路徑軌跡。對于機(jī)器人來說,地圖中路標(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)動,同時(shí)對周圍的路標(biāo)進(jìn)行探測的結(jié)果分別如圖3和圖4所示,其中“○”為機(jī)器人估計(jì)的路標(biāo)點(diǎn),黑色的線為機(jī)器人實(shí)際的運(yùn)動路徑。

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

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

圖4 改進(jìn)的FastSLAM算法的運(yùn)動軌跡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)分布。