一種改進的無人機FastSLAM1.0算法

沈永福, 王希彬

(1海軍軍訓器材研究所,北京　102301; 2海軍航空工程學院控制工程系,山東 煙臺　264001)

?

一種改進的無人機FastSLAM1.0算法

沈永福1, 王希彬2

(1海軍軍訓器材研究所,北京102301; 2海軍航空工程學院控制工程系,山東 煙臺264001)

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

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

1引言

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

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

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

2FastSLAM1.0算法

絕大多數(shù)SLAM算法的目標就是計算后驗值:

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

FastSLAM是基于后驗值分解的思想提出的:

表明已知無人機的路徑,地標的位置可以獨立估計出來。而實際上無人機的路徑是未知的,但這種獨立性使得后驗值的分解成為可能:首先估計無人機的路徑;其次以無人機路徑為條件估計N個地標的位置。

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

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

SLAM算法從本質(zhì)上說是一個估計問題,給定無人機的運動方程,利用機載傳感器(慣導與攝像機)的測量值,估計無人機和地標的狀態(tài)。

將慣導方程作為無人機的運動方程:xv(k)=fv(xv(k-1),u(k))+w(k)

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

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

無人機的觀測方程為:

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

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

4.1模擬退火算法

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

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

4.2基于模擬退火的FastSLAM

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

(1)

(2)

4.3算法步驟

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

(1) 預測

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

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

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

(b) 利用(1)、(2)式計算地標預測觀測值與目標函數(shù)

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

(4) 分層重采樣

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

5仿真試驗與分析

在仿真過程中,無人機作半徑為20m的圓周運動,并從無人機的周圍隨機選擇12個特征作為地標。無人機的真實初始位置作為濾波的初值:

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

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

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

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

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

6結(jié)論

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

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

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

參考文獻:

[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é)合免疫機制的并發(fā)定位與建圖多目標進化算法[J].湘潭大學自然科學學報,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)化的移動機器人SLAM方法[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-),浙江人,碩士,工程師,主要研究方向為導航制導與控制。

王希彬(1985-),山東人,博士,講師,主要研究方向為航空自動化。

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

中圖分類號：V 24

文獻標識碼：A