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

沈永福, 王希彬

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

?

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

沈永福1, 王希彬2

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

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

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

1引言

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

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

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

2FastSLAM1.0算法

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

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

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

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

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

3無人機FastSLAM算法的數學模型

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

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

慣導位置、速度和姿態方程分別為:

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

無人機的觀測方程為:

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

4基于模擬退火算法的粒子優化

4.1模擬退火算法

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

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

4.2基于模擬退火的FastSLAM

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

(1)

(2)

4.3算法步驟

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

(1) 預測

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

(2) 模擬退火優化過程

其中,r是0～1之間的隨機數;g是鄰域調整因子。

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

(3) 權值計算

(4) 分層重采樣

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

5仿真試驗與分析

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

系統噪聲與測量噪聲為:

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

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

圖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結論

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

圖3　 SAFastSLAM1.0與FastSLAM2.0估計的 無人機的姿態誤差比較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]李枚毅.結合免疫機制的并發定位與建圖多目標進化算法[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]陳白帆,蔡自興,袁成.基于粒子群優化的移動機器人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