沈永福, 王希彬

(1海軍軍訓(xùn)器材研究所,北京　102301; 2海軍航空工程學(xué)院控制工程系,山東 煙臺(tái)　264001)

?

一種改進(jìn)的無人機(jī)FastSLAM1.0算法

沈永福1, 王希彬2

(1海軍軍訓(xùn)器材研究所,北京102301; 2海軍航空工程學(xué)院控制工程系,山東 煙臺(tái)264001)

摘要：無人機(jī)FastSLAM1.0算法粒子濾波的預(yù)測過程沒有考慮地標(biāo)的觀測值,并不能利用新的測量數(shù)據(jù)更新無人機(jī)的位姿,使得粒子集偏離無人機(jī)真實(shí)位姿,需要大量粒子改進(jìn)粒子濾波的精度,增加了算法的計(jì)算復(fù)雜性。為此,提出采用模擬退火算法更新粒子的預(yù)測值,調(diào)整提議分布,使得粒子集中在無人機(jī)真實(shí)位姿的周圍,并利用分層重采樣解決粒子退化問題。在無人機(jī)環(huán)境下對(duì)該算法進(jìn)行仿真研究,結(jié)果表明其估計(jì)精度明顯優(yōu)于FastSLAM2.0算法,驗(yàn)證了算法的有效性和可行性。

關(guān)鍵詞：快速同時(shí)定位與地圖創(chuàng)建; 無人機(jī); 粒子濾波; 模擬退火; 分層重采樣

1引言

FastSLAM是由Montemerlo[1-3]最早提出的,采用粒子濾波的方法,把SLAM分解為定位部分和條件地圖部分,很好地克服了EKFSLAM的缺陷,能夠應(yīng)用在大規(guī)模的室外環(huán)境。但是FastS粒子濾波中大多數(shù)粒子經(jīng)多次迭代后,只有很小的權(quán)值,產(chǎn)生“粒子退化”現(xiàn)象。重采樣可以抑制“粒子退化”,但重采樣選擇較大權(quán)值的粒子使得粒子的多樣性缺失,帶來“粒子貧乏”現(xiàn)象,使得估計(jì)精度降低。針對(duì)存在的這些問題,目前許多學(xué)者提出了改進(jìn)的FastSLAM的粒子濾波算法。Yin[4]利用遺傳算法的選擇、交叉和變異算子進(jìn)行粒子優(yōu)化;Li[5]把SLAM問題轉(zhuǎn)化為基于免疫算法的多目標(biāo)優(yōu)化問題;Chen[6]提出基于粒子群優(yōu)化的方法解決移動(dòng)機(jī)器人的SLAM問題。

FastSLAM1.0算法粒子濾波的預(yù)測過程沒有考慮地標(biāo)的觀測值,并不能利用新的測量數(shù)據(jù)更新無人機(jī)的位姿,當(dāng)無人機(jī)運(yùn)動(dòng)模型噪聲大于觀測模型噪聲時(shí),粒子集分布在較低測量兼容性的范圍內(nèi),偏離無人機(jī)的真實(shí)位姿,導(dǎo)致SLAM算法的失敗。為解決此問題,傳統(tǒng)的方法就是增加粒子數(shù),但由此又增加了算法的計(jì)算復(fù)雜度。

Metropolis[7]于1953年首先提出模擬退火算法,但是并沒有引起很大的反響,直到1983年Kirkpatrick[8]才真正創(chuàng)建了現(xiàn)代的模擬退火算法,應(yīng)用于組合優(yōu)化問題,可以克服初值的依賴性,避免局部極值。因此,考慮引入模擬退火過程解決FastSLAM1.0中粒子濾波需要大量粒子的問題,使粒子分布在無人機(jī)的真實(shí)軌跡附近,同時(shí)使得粒子數(shù)最小。

2FastSLAM1.0算法

絕大多數(shù)SLAM算法的目標(biāo)就是計(jì)算后驗(yàn)值:

為計(jì)算此后驗(yàn)值,以條件概率分布的形式,利用具有獨(dú)立高斯噪聲的非線性函數(shù),重新建立運(yùn)動(dòng)和測量模型:

FastSLAM是基于后驗(yàn)值分解的思想提出的:

表明已知無人機(jī)的路徑,地標(biāo)的位置可以獨(dú)立估計(jì)出來。而實(shí)際上無人機(jī)的路徑是未知的,但這種獨(dú)立性使得后驗(yàn)值的分解成為可能:首先估計(jì)無人機(jī)的路徑;其次以無人機(jī)路徑為條件估計(jì)N個(gè)地標(biāo)的位置。

與FastSLAM1.0算法相比,FastSLAM2.0算法主要發(fā)生了兩個(gè)變化:首先,從新的提議分布中采樣;其次,粒子權(quán)值的更新反映了提議分布的變化。顯然,FastSLAM2.0要明顯優(yōu)于FastSLAM1.0算法。

3無人機(jī)FastSLAM算法的數(shù)學(xué)模型

SLAM算法從本質(zhì)上說是一個(gè)估計(jì)問題,給定無人機(jī)的運(yùn)動(dòng)方程,利用機(jī)載傳感器(慣導(dǎo)與攝像機(jī))的測量值,估計(jì)無人機(jī)和地標(biāo)的狀態(tài)。

將慣導(dǎo)方程作為無人機(jī)的運(yùn)動(dòng)方程:xv(k)=fv(xv(k-1),u(k))+w(k)

慣導(dǎo)位置、速度和姿態(tài)方程分別為:

其中,xv(k)為無人機(jī)的狀態(tài)向量,包含無人機(jī)的位置Pn(k)、速度vn(k)和姿態(tài)Ψn(k),將慣導(dǎo)陀螺輸出ωb(k)和加速度計(jì)輸出fb(k)作為控制量u(k),w(k)為零均值白噪聲,方差為Q(k)。

無人機(jī)的觀測方程為:

其中,mi(k)為靜止的第i個(gè)地標(biāo)的狀態(tài),vi為零均值白噪聲,方差為R。

4基于模擬退火算法的粒子優(yōu)化

4.1模擬退火算法

模擬退火的主要思想來源于熱力學(xué)的退火過程。物理退火過程包括加溫、等溫和冷卻過程。1953年,Metropolis等提出了一種重要性重采樣法,即以概率來接受新狀態(tài)。具體而言,在溫度T,由當(dāng)前狀態(tài)i產(chǎn)生新狀態(tài)j,兩者的能量分別為Ei和Ej,若Ei>Ej,則接受新狀態(tài)為當(dāng)前狀態(tài);否則以一定的概率pr=exp[-(Ej-Ei)/kBT]來接受新狀態(tài),其中kB為Boltzmann常數(shù)。當(dāng)這種過程多次重復(fù),即經(jīng)過大量遷移后,系統(tǒng)將趨于能量較低的平衡態(tài),各狀態(tài)的概率分布將趨于一定的正則分布。這種接受新狀態(tài)的方法被稱為Metropolis準(zhǔn)則,它能夠大大減少采樣的計(jì)算量

模擬退火算法(SA)就是一個(gè)將退火過程中系統(tǒng)熵值類比為目標(biāo)函數(shù)值F,來模擬這個(gè)退火系統(tǒng)的算法。在SA中,優(yōu)化問題中的一個(gè)解xi及其目標(biāo)函數(shù)c(xi)分別可以看成物理退火中的物體的一個(gè)狀態(tài)和能量函數(shù),而最優(yōu)解x*就是最低能量的狀態(tài)。而設(shè)定一個(gè)初始高溫、基于Metropolis準(zhǔn)則的搜索和控制溫度參數(shù)t的下降分別相當(dāng)于物理退火的加溫等溫和冷卻過程的狀態(tài)。Kirkpatrick等人利用金屬的退火過程模擬組合優(yōu)化問題,將Metropolis準(zhǔn)則作為搜索策略,以避免陷入局部最優(yōu),并最終趨于問題的全局最優(yōu)解。

4.2基于模擬退火的FastSLAM

FastSLAM1.0中粒子濾波算法通過連續(xù)更新粒子的位置和權(quán)值,近似真實(shí)的后驗(yàn)概率分布。將模擬退火引入粒子濾波的主要思想就是考慮測量值帶來的影響,在不增加粒子數(shù)的情形下,使預(yù)測值近似真實(shí)系統(tǒng)的狀態(tài)分布。

(1)

(2)

4.3算法步驟

基于模擬退火的無人機(jī)FastSLAM是一個(gè)連續(xù)迭代過程,主要包含預(yù)測、模擬退火優(yōu)化、權(quán)值計(jì)算和分層重采樣四個(gè)步驟。

(1) 預(yù)測

根據(jù)提議分布從當(dāng)前粒子集中采樣預(yù)測值,獲得下一時(shí)刻的粒子st～p(st|st-1,ut)

(2) 模擬退火優(yōu)化過程

其中,r是0～1之間的隨機(jī)數(shù);g是鄰域調(diào)整因子。

(b) 利用(1)、(2)式計(jì)算地標(biāo)預(yù)測觀測值與目標(biāo)函數(shù)

(3) 權(quán)值計(jì)算

(4) 分層重采樣

利用模擬退火算法,在權(quán)值計(jì)算之前,粒子集更加接近于無人機(jī)的真實(shí)置,權(quán)值計(jì)算使得粒子的分布和重采樣過程更加有效。因此,粒子集加速收斂,并在下一時(shí)刻,賦予無人機(jī)位姿一個(gè)更好的初值。

5仿真試驗(yàn)與分析

在仿真過程中,無人機(jī)作半徑為20m的圓周運(yùn)動(dòng),并從無人機(jī)的周圍隨機(jī)選擇12個(gè)特征作為地標(biāo)。無人機(jī)的真實(shí)初始位置作為濾波的初值:

系統(tǒng)噪聲與測量噪聲為:

模擬退火算法中,初始溫度t0=1°,循環(huán)迭代次數(shù)L=300,溫度下降速率η=0.8。

圖1～4給出了基于模擬退火的FastSLAM1.0(簡寫為SAFastSLAM1.0)與FastSLAM2.0估計(jì)的誤差曲線的對(duì)比,其中紅色實(shí)線表示基于模擬退火的FastSLAM1.0的估計(jì)誤差曲線,藍(lán)色虛線表示FastSLAM2.0的估計(jì)誤差曲線。從仿真結(jié)果看,與FastSLAM2.0相比,不論是無人機(jī)的位置、速度和姿態(tài)誤差,還是地標(biāo)的位置誤差,基于模擬退火的FastSLAM1.0獲得了更加精確的結(jié)果。

圖1　 SAFastSLAM1.0與FastSLAM2.0估計(jì)的 無人機(jī)的位置誤差比較Fig.1　 The comparison of estimated UAV position error between SAFastSLAM1.0 and FastSLAM2.0

圖2　 SAFastSLAM1.0與FastSLAM2.0估計(jì)的 無人機(jī)的速度誤差比較Fig.2　 The comparison of estimated UAV velocity error between SAFastSLAM1.0 and FastSLAM2.0

6結(jié)論

本文將模擬退火引入無人機(jī)FastSLAM1.0的粒子濾波,通過調(diào)整粒子的位置分布,使預(yù)測采樣粒子集中在無人機(jī)的真實(shí)位姿周圍,有效提高SLAM的精度,減少粒子數(shù)并降低計(jì)算時(shí)間復(fù)雜度。仿真試驗(yàn)顯示,與FastSLAM2.0相比,提出的算法可以獲得更加準(zhǔn)確的無人機(jī)位姿和地標(biāo)位置的估計(jì)值,表明該算法是有效可行的。

圖3　 SAFastSLAM1.0與FastSLAM2.0估計(jì)的 無人機(jī)的姿態(tài)誤差比較Fig.3　 The comparison of estimated UAV attitude error between SAFastSLAM1.0 and FastSLAM2.0

圖4　 SAFastSLAM1.0與FastSLAM2.0估計(jì)的 地標(biāo)的位置誤差比較Fig.4　 The comparison of estimated landmark position error between SAFastSLAM1.0 and FastSLAM2.0

參考文獻(xiàn):

[1]Michal Montemerlo.FastSLAM:A factored solution to the simultaneous localization and mapping problem with unknown data association[D].Carnegie Mellon University Pittsbwghers,2003.

[2]M Montemerlo,S Thrun,D Koller,and B Wegbreit.FastSLAM:A Factored Solution to the Simultaneous Localization and Mapping Problem[C]∥Proceedings of the National Conference on Artificial Intelligence.Edmonton:2002,AAAI,593-598.

[3]Montemerlo M,Thrun S.Simultaneous localization and mapping with unknown data association using FastSLAM[C]∥Proceedings of the IEEE International Conference on Robotics and Automation,Karlsruhe Germany,2003,1985-1991.

[4]Yin B,Wei Z O,Zhuang X D.Robust mobile robot localization using an evolutionary particle filter[M]∥Lecture Notes in Computer Science(vo1．3801),Berlin,Germany:Springer-Verlag,2005:279-284.

[5]李枚毅.結(jié)合免疫機(jī)制的并發(fā)定位與建圖多目標(biāo)進(jìn)化算法[J].湘潭大學(xué)自然科學(xué)學(xué)報(bào),2007,29(2):111-117.

LI Meiyi.Multi-objective evolutionary algorithms for SLAM with immunity[J].Natural Science Journal of Xiangtan University,2007,29(2):111-117.

[6]陳白帆,蔡自興,袁成.基于粒子群優(yōu)化的移動(dòng)機(jī)器人SLAM方法[J].機(jī)器人,2009,31(6):513-517.

CHEN Baifan,Cai Zixing,Yuan Cheng.Mobile robot SLAM method based on particle swarm optimization[J].Robot,2009,31(6):513-517.

[7]Metroplis N,Rosenbluth A,Rosenbluth M et al.Equation of state calculations by fast computing machines[J].Journal of Cherimal Physics,1953,21(6):1087-1092.

[8]Kirkpatrick S,Gelatt Jr C D,Vecchi M P.Optimized by simulated annealing[J].Science,1983,220(4598):671-680.

沈永福男(1974-),浙江人,碩士,工程師,主要研究方向?yàn)閷?dǎo)航制導(dǎo)與控制。

王希彬(1985-),山東人,博士,講師,主要研究方向?yàn)楹娇兆詣?dòng)化。

AnImprovedFastSLAM1.0AlgorithmforUAVSHEN Yongfu1,WANGXibin2

(1.NavalMilitaryTrainingResearchInstitute,Bejing,102301,China;

2.DepartmentofControlEngineering,NavalAeronauticalandAstronauticalUniversity,YantaiShandon,264001,China)

Abstract：Landmark observations are not considered to update the predicted UAV pose in the prediction process of particle filtering for fast simultaneous localization and mapping (FastSLAM) of uninhabited aerial vehicle (UAV),so the particle set deviates from the true UAV pose.A great deal of particles are needed to improve the prediction which increases the computation complexity.Simulated annealing (SA) method is proposed to update the particle’s prediction and adjust the proposal distribution,so the particles are concentrated around the true pose.Stratified resampling is used to solve the degeneracy of particles.At last,the proposed algorithm is simulated in the environment of UAV.The results show that the estimation accuracy of the proposed algorithm is superior to the FastSLAM 2.0 algorithm and validate the availability and feasibility of this method.

Key words：fast simultaneous localization and mapping (FastSLAM); uninhabited aerial vehicle (UAV); particle filtering; simulated annealing (SA); stratified resampling

中圖分類號(hào)：V 24

文獻(xiàn)標(biāo)識(shí)碼：A