1、第40卷第1期2010年1月 東南大學學報(自然科學版)JOURNALOFSOUTHEASTUNIVERSITY(NaturalScienceEdition) Vo.l40No.1Jan.2010doi:10.3969/.jissn.1001-0505.2010.01.022基于粒子濾波的移動機器人SLAM算法涂剛毅 金世俊 祝雪芬 宋愛國(東南大學儀器科學與工程學院,南京210096)摘要:針對FastSLAM算法對傳感器精度要求較高,不適用于方向性差的超聲傳感器問題,提出了一種基于超聲概率柵格地圖環境特征點提取匹配的移動機器人粒子濾波同時定位與地圖創建(SLAM)算法.該算法可分解為機器人

2、位姿估計和環境路標估計2個部分.基于蒙特卡羅定位原理利用粒子濾波算法對機器人運動軌跡進行估計;在建立全局超聲概率柵格地圖的基礎上,利用概率柵格地圖環境特征提取算法對環境路標坐標進行估計.實驗證明,該算法較好地解決了超聲測距傳感器由于散射角大帶來的特征點估計不準的問題,對環境路標和機器人軌跡的估計都比較準確.并對移動機器人累計誤差進行了有效的補償,減少了由于累積誤差造成的移動機器人軌跡扭曲失真.關鍵詞:移動機器人;同步定位與地圖創建;粒子濾波器;柵格地圖中圖分類號:TP242 文獻標志碼:B 文章編號:1001-0505(2010)01 0117 06ParticlefilterSLAMmeth

3、odformobilerobotTuGangyi JinShijun ZhuXuefen SongAiguo(SchoolofInstrumentScienceandEngineering,SoutheastUniversity,Nanjing210096,China)Abstract:FocusingonthelimitedapplicationofFastSLAMalgorithmduetoitshighaccuracyrequir mentforsensorandunfitnessforultrasonicsensorwithpoordirectivity,asimultaneouslo

4、calizationandmapping(SLAM)algorithmutilizingparticlefilterisprovided,whichisbasedonultrasonicprobabilitygridmapfeaturepointsextractionandmatching.Thisalgorithmconsistsofrobotposeesti mationandenvironmentlandmarkestimation.ParticlefiltersareappliedtoestimatetherobottrajectoryaccordingtoMonteCarlometh

5、ods.Basedonglobalultrasonicprobabilisticgridmap,theenvironmentlandmarkpositionisobservedbyenvironmentfeatureextractionalgorithm.Theeffectivenessoftheproposedalgorithmisvalidatedbyexperimentalresul.tTheestimationaccuracyoffeaturepointposi tionaswellasenvironmentlandmarkareimproved.Thecumulativeerroro

6、ftherobotiscompensatedeffectivelyandthemobilerobottrajectorydistortionisreduced.Keywords:mobilerobo;tsimultaneouslocalizationandmapping;particlefilter;gridmap移動機器人的定位和地圖創建是機器人領域的研究熱點.在目前的研究中,針對已知環境的自主定位研究已有一些實用的解決方法.但在未知環境下機器人位置不確定時,如何將定位和地圖創建結合,利用自身的定位來創建地圖,同時利用地圖進行自主定位和導航,即移動機器人的同時定位與地圖創建(simultan

7、eouslocalizationandmapping,SLAM)問題還有待進一步研究.SLAM被認為是移動機器人實現真正全自主的核心問題.粒子濾波(particlefilter,PF)算法是基于蒙特卡羅和貝葉斯估計的濾波方法,也稱為順序蒙特卡洛方法(sequentialMonteCarlomethods,SMC),基本思想是采用一組從概率密度上隨機抽取的加權樣本來近似后驗概率密度,可用于對非線性運動模收稿日期:2009 06 17. 作者簡介:涂剛毅(1981 ),男,博士生;金世俊(聯系人),男,博士,副教授,jinsj263.ne.t基金項目:教育部高等學校科技創新工程重大項目培育資金項目

8、(708045)、江蘇省 六大高峰人才!資助項目(06 D 031)、東南大學優秀青年教師基金資助項目(4022001004).引文格式:涂剛毅,金世俊,祝雪芬,等.基于粒子濾波的移動機器人SLAM算法J.東南大學學報:自然科學版,2010,40(1):117 122.10.jiss0505.2010.p(xuk,xk-1)p(xk-1zk-1,uk-1)dxk-1(1)型進行估計.考慮到機器人位姿和環境特征的獨立性,Montemerlo等提出了一種基于Rao Black wellized粒子濾波(Rao Blackwellizedparticlefil ter,RBPF)的FastSLAM算

9、法1 2k式中,為歸一化常量;p(zkxk)為傳感器測量值概率分布,即機器人觀測模型;p(xkuk,xk-1)表示移動機器人在施加運動控制量uk后,位姿更新為xk的概率,稱為機器人運動模型.SLAM問題被分解成機器人運動軌跡的粒子濾波估計和路標位置的擴展卡爾曼濾波(extendedKalmanfilter,EKF)估計2個過程.在采用EKF對路標進行估計時,假定高斯分布對于在探測過程中出現的異常點敏感,錯誤的匹配將產生嚴重的后果.FastSLAM算法對傳感器精度要求較高,適用于方向性較好的激光測距傳感器.超聲測距傳感器由于散射角大,測量精度低,觀測不可靠,不適用于該算法.針對這一問題,本文將高

10、斯超聲模型建圖算法32 粒子濾波SLAM算法2 1 粒子濾波SLAM算法理論基礎在SLAM中機器人狀態向量xk應同時包含機器人的位姿信息sk和環境信息m,即xk=sk,mT式中,sk=(xk,yk,!k)表示移動機器人在全局坐標系下的位置坐標xk,yk和角度值;m=m1,(,mN表示N個環境路標坐標集.基于PF的SLAM方法的基本思想是:根據所有時刻環境感知信息z=z1,(,zk和機器人位姿更新信息u=u1,(,uk,估計機器人運動軌跡s=s1,(,sk的后驗概率密度p(skkkk與粒子濾波算法4相結合,提出了一種基于超聲概率柵格地圖環境特征點提取匹配的移動機器人粒子濾波SLAM算法.利用粒子

11、濾波算法對機器人運動軌跡進行估計,利用概率柵格地圖對路標坐標進行估計.算法基本框架可概括為環境感知、特征匹配、狀態更新、地圖更新4個過程.環境感知.通過超聲測距傳感器獲取環境信息,建立全局超聲概率柵格地圖.#特征匹配.將當前測量值與超聲概率柵格地圖中環境路標進行匹配.狀態更新.結合PF算法原理,對機器人粒子集的粒子位姿及權值進行更新,計算機器人粒子集概率中心位姿,作為機器人估計位姿.%地圖更新.結合蒙特卡羅定位原理,根據機器人估計位姿和超聲測量值對概率柵格地圖和環境路標信息進行更新.z,u,n)、kkk環境地圖和機器人軌跡的聯合后驗概率密度pkkkk1(s,mz,u,n).式(1)可等效表示為

12、p(s,mz,u,n)= p(s p(skkkkkkz,u,n)p(mNkkks,z,u,n)=ikkkkkz,u,n)kkk)p(mi=1s,z,u,n)(2)kkkk式中,n表示在k時刻所觀測到的環境特征點信息,n=n1,n2,(,nk.如式(2)所示,SLAM問題可被分解為機器人位姿估計和基于機器人位姿估計的環境路標估計2個過程.2 2 算法設計結合PF算法基本原理算法如下.7k1 蒙特卡羅定位蒙特卡羅定位5 6,基于PF的SLAM(MonteCarlolocalization,MCL)的理論基礎是貝葉斯濾波(Bayesianfilter,BF)理論.其核心思想是通過遞歸方式以目前所接收

13、到的所有數據為條件,對狀態變量xk進行估計.移動機器人數據可以分為2類:環境感知信息zk= k, k,其中, k,k分別為超聲傳感器測距和方位角信息;機器人位姿更新信息uk= sk, !k,其中, sk為里程計增量信息, !k為角速度陀螺儀角度積分變化量.將zk作為系統的觀測向量,后驗概率密度p(xkzk,uk)可表示為如下貝葉斯概率模型形式p(7TT(i)(i)輸入:機器人初始坐標s0和粒子數總數Num.輸出:機器人運動軌跡及環境地圖.根據初始坐標,隨機生成由Num個粒子s0、粒子權值w0組成的粒子集s0=s0,w0i=1,(,Num.#預測機器人位姿(i)k-18 9(i)(i).根據k-

14、1時刻粒子集(i)sk-1中的每個粒子s及運動模型p(sk計算k時刻粒子sk.根據觀測模型p(zkk.(i)(i)(i)uk,sk-1),(i)(i)sk)計算粒子sk對應:p(的特征點坐標,并將其與環境路標集中的某一路標第1期(i)涂剛毅,等:基于粒子濾波的移動機器人SLAM算法k(xL-xk/)+(yL-y/k)=y-ykL-!kxL-x119%對粒子權值wk進行更新,即wk(i)=wk-1p(zkNum(i)sk)(i)(3)(9)歸一化處理為w10(i)k=w(i)k(i)k+wi=1(i)k(4)式中,(xL,yL)為特征點坐標;(xk/,yk/)為傳感器中心坐標.理想情況下,基于粒

15、子sk的觀測模型p(zksk)與移動機器人觀測模型相同.若傳感器中心(x/)與粒子坐標(xk,yk)的偏差為L,與k,y/k粒子觀測模型p(zk計算公式為z(i)L,k(i)(i)(i)(i)(i)(i),根據粒子權值w樣服粒子退化現象.環境地圖更新對路標mi進行更新.2 3 粒子位姿估計2對粒子集sk進行重采,剔除權值小的粒子,復制權值大的粒子,克.利用p(mis,z,u,n)kkkksk)對應的環境特征點坐標(i)(i)以sk來表示粒子集合,粒子集中的每一個粒子s.Sk表示機器人可能的位姿,即Sk=s,(,si=1,(,Numsk(i)(1)k(i)k(i)k=xL,ky(i)L,(i)(

16、i=x/+ + kkkcos(!ky/+ + kksin(!(i)(i)(i)k(i)(i)k(i)k(i)k(i)=(5)(6)(7)xk+Lcos!+ kcos(!+ kkky(i)(i)k=(xk,yk,!)ksk=i=+1skwNum(i)(i)k(i)(i)(i)+Lsin!+ ksin(!+ k)(10)式中,zL,k為與粒子sk對應的特征點坐標.2 5 特征點坐標估計及匹配超聲傳感器由于存在散射角大、方向性差的問題,其測距信息存在較大的不確定性,需要根據一定的模型對其進行處理.常見的模型有中線模型和平2,11均分布弧線模型.本文中采用了高斯分布模型.該模型認為障礙物可能存在于發散

17、角圓弧上的任一點,以發射角中間線距離R為中心點,呈正態分布;同時在半徑R的圓弧空間內沒有障礙物存在.根據高斯分布模型及貝葉斯公式,可建立概率柵格地圖.環境中的特征點被映射為柵格地圖中的大概率區域.環境特征點坐標mi計算式如下:mi=Cxy.#式中,i表示粒子編號.隨著移動機器人運動,粒子集sk的位姿應根據位姿更新信息uk進行同步更新.在理想狀態下,粒子sk位姿更新模型p(sk(i)(i)uk,sk-1)應和移動機(i)器人運動模型相同.但實際情況由于考慮到里程計、角速度陀螺儀積分存在誤差,所以在粒子位姿更新時加入里程計噪聲分量N sk和角度噪聲分量N sk.同時,為了避免由于粒子退化以及加入重

18、采樣算法后出現的粒子分布過度收斂等問題而導致的定位精度降低,在進行位姿更新時加入位姿隨機高斯發散增量Nx,Ny,N!.粒子位姿更新模型p(sk(i)+(P(OCxy)PCxy)uk,sk-1)如下式所示:xk-1(i)(i)( sk+N sk)cosk!k+N !kNxN(8)Cxy.#P(OCxy)(11)sk(i)=(i)yk-1+( sk+N sk)sin!k+Ny式中,#為特征點可能所在區域;PCxy為柵格Cxy中心坐標;P(OCxy)為柵格被占據的概率.機器人在感知新的觀測值zk=( k, k)后,將其與已知環境特征點集m中的特征點進行匹配.在環境路標相距較遠時,可依據最近鄰居(ne

19、arestneighbor,NN)數據關聯準則,即選取距離觀測值最近的環境路標作為其匹配點.環境路標間距離相距越遠,誤匹配率越低,即f(sk,m)=mm.mind(mi,zL,k)i!k-(i)2 4 環境特征點坐標估計機器人運動及觀測模型如圖1所示,可表述為(i)(i)(12)(i)式中,d(mi,zL,k)表示與粒子sk對應的特征點坐標zL,k與環境路標mi之間距離;函數f(sk,m)返回值為粒子sk與環境特征點集m中最近的特征點距離.設Th為最大匹配誤差閾值,若f(sk,m)圖 (i)(i)(i)(i)(i)h,.1202 6 粒子權值更新東南大學學報(自然科學版) 第40卷式移動機器人

20、,在室外自然環境中完成了SLAM(i)若zL,k與mi匹配成功,可對粒子權值wk進行更新,依據式(2)可計算得到mi對應的觀測值( ).當前時刻測量值zk= k, kk, k.令超聲測距誤差 k= k- k,超聲測量角度誤差 k= k- k. k主要誤差源是機器人角度誤差 !k,可認為 k0 !k. !k被認為符合高斯分布,則 ,!).測距誤差也被認為符合高斯分kN(M!布, , ).選取誤差項 kN(M k, k作為測量增量對粒子權值進行更新,則p(zk如下1222(i)(i)(i)(i)T(i)算法實驗.移動機器人選取真實環境中的樹作為絕對路標,在約8m18m的區域做連續繞圈實驗.與結構化

21、的室內環境相比,室外環境更具有一般性,因而對本文提出的SLAM算法的檢驗提出了更高的要求. 探索者!機器人配備有超聲測距傳感器、角速度陀螺儀、電子羅盤、里程計等傳感器,其特點是采用了單超聲傳感器掃描的設計方案.通過將一個超聲傳感器加裝在舵機上,利用舵機旋轉,帶動超聲傳感器在前方1802方向上以11 252間隔進行掃描,彌補了超聲傳感器數量不足的缺陷,可替代超聲環設計.雖然此設計方案在系統實時性方面不及傳統方案,但在系統復雜度、成本、質量和體積方面具有顯著優勢,適合室外環境工作.圖2為實驗環境及超聲概率柵格地圖,通過對比,采用基于高斯超聲模型算法2sk)計算式(13)(i):(i)p(zksk)

22、=GM!,2( k)GM,2( k)!式中,函數G為高斯概率計算公式.將式(13)代入式(3)則可根據當前測量值zk對粒子集權值進行更新.建立的概率柵3 實驗結果實驗載體采用了自主研制的 探索者!號四輪格地圖較為準確地反映了環境特征.圖2 實驗環境及超聲概率柵格地圖選取真實環境中的樹作為絕對路標,概率柵格地圖中與其對應的環境路標概率中心坐標如表1所示.表1 環境路標坐標 cm路標123456概率中心(-117.5,44.7)(11.3,219.5)(88,96)(163.2,288.4)(174,-70.6)(268.6,122.3)實際中心(-115,48)(13,225)(91,101)(

23、165,293)(180,-73)(271,118)誤差間距4.145.755.834.936.464.92考慮到測量誤差、柵格細化不夠等原因,可認為環境路標概率中心也與實際坐標近似一致.圖3為部分時刻移動機器人粒子集分布變化圖.圖中, 橢圓!表示路標或粒子分布的3不確.圖3 粒子分布圖在未探測到有效聲納數據時,粒子位姿根據移動機器人軌跡進行運動更新.由于存在位姿隨機高斯發,第1期涂剛毅,等:基于粒子濾波的移動機器人SLAM算法121利用獲得的有效超聲測距數據對粒子權值進行更新、重采樣后,權值小的粒子被剔除,權值大的粒子被復制,粒子集合收斂,分布方差減小,如圖4所示.圖5(a)中,由于移動機器

24、人在運動過程中里程計、角速度陀螺儀等傳感器累計誤差在不斷累積,軌跡偏差越來越大,特征點分布呈發散趨勢.圖5(b)為利用PF SLAM算法處理后獲得的機器人粒子集概率中心軌跡圖.由于粒子分布更新時存在隨機增量,概率中心軌跡存在一定的擾動.通過不斷將當前測量值與環境路標進行匹配,可以對累計誤差進行有效修正.(a)粒子X坐標分布修正曲線如圖6所示,由圖可見.移動機器人累計誤差得到了有效的補償,角度等誤差發散的趨勢得到了抑制.(b)粒子Y坐標分布(a)X坐標(c)粒子角度分布圖4 方差變化曲線(b)Y坐標移動機器人順時針連續行駛6圈,原始實驗數據與處理后的實驗結果分別如圖5(a)和(b)所示.(c)角

25、度圖6 修正曲線4 結語實驗證明,本文提出的基于超聲概率柵格地圖環境特征點提取匹配的移動機器人PF SLAM算(a)移動機器人原始軌跡法,對移動機器人累計誤差進行了有效的補償,解決了超聲測距傳感器由于散射角大帶來的特征點估計不準的問題,對環境路標和機器人軌跡的估計都比較準確.今后的研究將主要集中在繼續提高SLAM算法效率和精度方面.參考文獻(References)1MontemerloM,ThrunS.SimultaneouslocalizationandmappingwithunknowndataassociationusingFastSLAM(b)移動機器人處理后軌跡C3ProcIEEEI

26、ntConfRoboticsandAutomation.ii,:.5122東南大學學報(自然科學版) 第40卷rativedistributedperceptionJ.Robot,2008,30(3):210 216.(inChinese)7ArulampalamS,MaskellS,GordonN,eta.lAtutorialonparticlefiltersforon linenon linear/non GaussianBayesiantrackingJ.IEEETransactionsonSignalProcessing,2002,50(2):174 188.8LiuXian en,M

27、aBojun,QiNingning,eta.lAsonardatabasedparticlefilteringlocalizationmethodformobilerobotC/Proceedingsofthe7thWorldCongressonIntelligentControlandAutomation.Chongqing,China:IEEEPress,2008:3920 3924.9LiuGuocheng,WangYongj.iMulti sensormovingtargettrackingusingparticlefilterC/Proceedingsofthe2007IEEEInt

28、ernationalConferenceonRoboticsandBiomimetics.673.10GiorgioG,CyrillS,WolframB.ImprovinggridbasedSLAMwithrao blackwellizedparticlefiltersbyadaptiveproposalsandselectiveresamplingC/ProcIEEEIntConfRoboticsandAutomation.Barce lona,Spain:IEEEPress,2005:2443 2448.11BankD,KampkeT.High resolutionultrasonicenvironmentimagingJ.IEEETransactionsonRobot ics,2007,23(2):370 381.12IoannisRekleitis.AparticlefiltertutorialformobilerobotlocalizationTR CIM 04 02R.Montrea,lQuebec,Canada:CentreforIntelligentMachines,McGillUniversity,2004.San