一種改進的粒子濾波ＳＬＡＭ算法

摘要：提出一種改進的粒子濾波SLAM（simultaneous localization and map building）同時定位和地圖創(chuàng)建實現(xiàn)方法。改進方法讓機器人大約行進10步完成基于局部已創(chuàng)建地圖下的粒子濾波定位后，再利用激光傳感器探測環(huán)境并更新創(chuàng)建的地圖；同時在利用粒子濾波定位時，使粒子只分布在由航位推算法得出的機器人位姿附近，從而可有效地減少粒子的數(shù)量。實驗結(jié)果表明，與標(biāo)準(zhǔn)的粒子濾波SLAM 算法比較，改進算法提高了機器人SLAM過程中定位和地圖創(chuàng)建的精度和實時性，并為移動機器人在室外未知環(huán)境同時定位和地圖創(chuàng)建提供了新方法。

關(guān)鍵詞：局部地圖； 同時定位和地圖創(chuàng)建； 粒子濾波算法； 貝葉斯規(guī)則； 柵格地圖

中圖分類號：TP24文獻標(biāo)志碼：A

文章編號：1001－3695(2008)06－1698－03

0引言

自移動機器人誕生以來，對定位問題的研究就與地圖創(chuàng)建問題密切關(guān)聯(lián)。已知環(huán)境地圖的定位問題和已知位姿的地圖創(chuàng)建問題已經(jīng)被廣泛研究，提出了多種有效的解決途徑。然而在很多環(huán)境中機器人不能利用GPS等絕對定位技術(shù)進行定位，而且事先獲取機器人工作環(huán)境的地圖很困難，甚至是不可能的。這時機器人需要在自身位置和姿態(tài)不確定的條件下，在完全未知環(huán)境中創(chuàng)建地圖，同時利用地圖進行自主定位和導(dǎo)航。這就是移動機器人的同時定位與地圖創(chuàng)建問題(simultaneous localization and map building， SLAM)。

對SLAM問題的研究最早是Smith等人[1，2]使用擴展卡爾曼濾波器(extended Kalman filter， EKF)來進行估計。二十年來，EKF方法成為SLAM問題研究的主要方法，并在不同環(huán)境下提出了基于EKF的SLAM算法[3]。但隨著SLAM問題研究的深入，人們發(fā)現(xiàn)EKF方法的瓶頸在于其計算復(fù)雜性。對于其最優(yōu)濾波， 無論對濾波方程如何整理和計算， 其計算復(fù)雜度都至少與地圖中特征個數(shù)的平方成正比， 難以滿足構(gòu)建大規(guī)模地圖和實時性的要求。

粒子濾波又稱為序貫蒙特卡羅方法 (sequential Monte Carlo) [4，5]、自舉濾波 (bootstrap filter) [6]或者凝聚算法 (condensation algorithm)[7]，它是Markov定位的一種實現(xiàn)方法。該方法不需要直接求解概率密度函數(shù)，而是通過一系列的隨機樣本點表示位姿信度。粒子濾波的優(yōu)點在于它能夠表示任意概率密度，因此能用于全局定位和解決機器人“綁架”問題。同時粒子濾波方法作為一種基于貝葉斯估計的非線性濾波算法， 在處理非高斯非線性時變系統(tǒng)的參數(shù)估計和狀態(tài)濾波問題方面的獨到優(yōu)勢，使粒子最終匯集到真實的后驗概率點[8]。粒子濾波方法首先在機器人單一定位問題中得到成功的應(yīng)用[9]，接著被應(yīng)用于機器人定位過程并發(fā)地圖的創(chuàng)建[10，11]。而基于粒子濾波算法的快速SLAM方法的出現(xiàn)[12，13]，使粒子濾波算法成為移動機器人同時定位和地圖構(gòu)建的最重要、最有效的方法。

上述所有的基于粒子濾波的SLAM算法稱之為標(biāo)準(zhǔn)的粒子濾波SLAM算法，簡稱為PF－SLAM (particle filtering－SLAM)都是采用定位和地圖創(chuàng)建交替更迭、全局自回歸的方法，并且每次將粒子分布在機器人整個探索范圍內(nèi)。用不確定的機器人位姿來創(chuàng)建的環(huán)境地圖是不準(zhǔn)確的，同樣用不確定的環(huán)境地圖來估計的機器人位姿也是不準(zhǔn)確的。理論上，在滿足約束條件下，標(biāo)準(zhǔn)的基于粒子濾波的SLAM算法經(jīng)過無窮次迭代優(yōu)化運算達到收斂[14]，即得到確定性的機器人位姿和確定的環(huán)境地圖。但實際應(yīng)用中，機器人位姿的不確定和環(huán)境地圖的不確定給準(zhǔn)確的機器人定位和精確的環(huán)境地圖創(chuàng)建帶來相互影響，并且隨著機器人的運動，一方的誤差導(dǎo)致另一方更大的誤差，使得機器人完成定位和地圖創(chuàng)建的精度降低、時間延長，有時甚至難以滿足弱的約束條件而導(dǎo)致系統(tǒng)發(fā)散。本文改變標(biāo)準(zhǔn)的基于粒子濾波的SLAM算法中全局最優(yōu)回歸方法，采用讓機器人大約行進10步完成基于局部已創(chuàng)建地圖下的粒子濾波定位后，再利用激光傳感器探測環(huán)境并更新創(chuàng)建的地圖。

1標(biāo)準(zhǔn)的粒子濾波SLAM算法

1．1粒子濾波算法(PF)基本原理

標(biāo)準(zhǔn)的PF－SLAM算法的基本思想是使用m個隨機樣本S(k)={si(k)｜(Xir(k)，wi(k));i=1，…，m}，表示機器人在k時刻的位姿信度bel(Xr(k))=p(Xr(k)｜d0，…，k)。這里Xr(k)表示在時刻k機器人位姿的估計值；w(k)為對應(yīng)Xr(k)的權(quán)值；d0，…，k表示由時刻0到k時刻的數(shù)據(jù)。樣本集通過離散化狀態(tài)來近似表示機器人實際位姿的信度。給定不同時間段0，1，…，k的一系列測量值Zk={Z(0)，…，Z(k)}估計狀態(tài)Xr(k)，這里Xr(k)是移動機器人的位姿Xr(k)=(xr(k)，yr(k)，θr(k))。系統(tǒng)描述如下：

1．2標(biāo)準(zhǔn)的PF－SLAM算法

標(biāo)準(zhǔn)的PF－SLAM算法流程如圖1所示。該算法采用定位和地圖創(chuàng)建交替更迭、全局自回歸的方法，即用不確定的機器人位姿來創(chuàng)建環(huán)境地圖，同時又用創(chuàng)建的不確定環(huán)境地圖來估計機器人新的位姿。隨著迭代次數(shù)的增加，機器人位姿和環(huán)境地圖的確定性（即準(zhǔn)確度）都相應(yīng)提高并相互影響。理論上，在滿足約束條件下，標(biāo)準(zhǔn)的PF－SLAM算法經(jīng)過無窮次迭代優(yōu)化運算達到全局收斂，即最終得到確定性的機器人位姿和確定性的環(huán)境地圖。



2改進的PF－SLAM算法

雖然從理論上在滿足約束條件下，標(biāo)準(zhǔn)的PF－SLAM 算法經(jīng)過無窮次迭代優(yōu)化運算達到全局收斂，即最終得到確定性的機器人位姿和確定的環(huán)境地圖。但實際應(yīng)用中，由不確定機器人位姿創(chuàng)建的環(huán)境地圖是不準(zhǔn)確的，同樣由不確定環(huán)境地圖估計的機器人位姿也是不準(zhǔn)確的；并且隨著機器人的運動，一方的誤差導(dǎo)致另一方更大的誤差，使得機器人完成定位和地圖創(chuàng)建的精度降低、時間延長，有時甚至難以滿足弱的約束條件而導(dǎo)致系統(tǒng)發(fā)散。同時，標(biāo)準(zhǔn)的PF－SLAM 算法在機器人整個探索的環(huán)境內(nèi)均勻分布粒子，收斂速度降低；所需的粒子數(shù)量也增多，加大了運算量，影響了SLAM的實時性。

為解決標(biāo)準(zhǔn)的PF－SLAM算法在實際使用中存在的缺陷，提出一種改進的PF－SLAM算法。改進的PF－SLAM算法針對不確定的機器人位姿和不確定的環(huán)境地圖相互影響、相互制約的情況，采取在局部范圍內(nèi)，先使兩個因素中的其中一個因素盡可能地利用現(xiàn)有信息得到理想的結(jié)果，然后用此理想的結(jié)果來尋求另一因素的局部理想結(jié)果；再將尋求的另一因素的局部理想結(jié)果轉(zhuǎn)換到全局環(huán)境下的結(jié)果，將該全局環(huán)境下的結(jié)果作為尋求新的局部范圍內(nèi)理想結(jié)果的已知信息。如此進行直到最終得到全局確定性的機器人位姿和確定的環(huán)境地圖。同時，為解決整個探索環(huán)境內(nèi)分布粒子而影響SLAM的實時性，在利用粒子濾波定位時，使粒子只分布在由航位推算法得出的機器人位姿附近，從而可有效地減少粒子的數(shù)量。

實際中，一般采用機器人的運動模型和測量模型來完成機器人在未知環(huán)境中的SLAM問題。運動模型的數(shù)據(jù)來源于里程計、陀螺儀等，測量模型的數(shù)據(jù)來源于測距傳感器等。機器人在未知環(huán)境中運動，由于地形、機器人的物理結(jié)構(gòu)使得SLAM過程中里程計數(shù)據(jù)誤差較大，并且隨著行駛距離的增加，其累積誤差會無限增大，運動模型的數(shù)據(jù)誤差成為影響機器人定位和地圖構(gòu)建精度的主要因素。基于以上原因，改進的PF－SLAM算法采用先尋求機器人在局部范圍內(nèi)理想的位姿，然后用此局部范圍內(nèi)理想的位姿來創(chuàng)建局部范圍內(nèi)的環(huán)境地圖。具體方法是：將一定量數(shù)目的粒子均勻分布在以里程計推算出的機器人位姿為圓心的一定半徑范圍的圓內(nèi)(圓的半徑大小為具體系統(tǒng)的經(jīng)驗值)。機器人行進10步后完成定位，然后利用激光測距儀重新掃描環(huán)境，進行地圖更新。改進的PF－SLAM算法的步驟如下所示：

3實驗結(jié)果

探測環(huán)境為平坦的室內(nèi)靜態(tài)環(huán)境（即環(huán)境中的物體是靜止的）。環(huán)境地圖如圖2所示，大小為1 000 cm×1 000 cm，圖中曲線是機器人運動軌跡，起點位姿xs(0，0，00)，實際終點位姿xT(360，710，91.50)。

運動模型的數(shù)據(jù)利用里程計采集，測量模型的數(shù)據(jù)采集的是旋轉(zhuǎn)激光測距儀，假定激光測距傳感器的測距掃描間隔為0．5°。環(huán)境地圖表達采用基于貝葉斯原理的柵格地圖[15]。

由標(biāo)準(zhǔn)的PF－SLAM算法創(chuàng)建的環(huán)境地圖如圖3所示，得到機器人最終位姿。由圖3可以看出，隨著機器人運動步數(shù)的增加，定位誤差逐漸增大，只能提供一個粗糙、簡單的運動模型。圖4是由改進的PF－SLAM算法創(chuàng)建的環(huán)境地圖，得到的機器人最終位姿xT(369，702，92.60)，位置誤差為10 cm，角度誤差為1°，較為精確地完成了SLAM任務(wù)。圖4中，創(chuàng)建的地圖最大距離誤差為20 cm，該誤差是由于機器人激光傳感器水平掃描角度誤差造成的。

由表1的仿真統(tǒng)計數(shù)據(jù)可以看出，改進的PF－SLAM算法比標(biāo)準(zhǔn)的PF－SLAM的定位和地圖創(chuàng)建的精度高。

4結(jié)束語

實驗結(jié)果表明，采用改進的PF－SLAM算法提高了機器人在未知環(huán)境中SLAM的定位準(zhǔn)確度和地圖創(chuàng)建的精度和實時性。移動機器人在室外未知的大環(huán)境同時定位和地圖創(chuàng)建是目前國內(nèi)外研究的熱點和難點問題，改進的算法為今后在該領(lǐng)域的研究提供新的思路。

參考文獻：

［1］SMITH R，SELF M，CHEESEMAN P．A stochastic map for uncertain spatial relationships[C]//Proc of the 4th International Symposium on Robotics Research.Cambridge:MIT Press，1987: 467－474．

[2]SMITH R，SELF M，CHEESEMAN P．Estimating uncertain spatial relationships in robotics[M]//Autonomous Robot Vehicles.New York:Springer－Verlag ，1990:167－193．

[3]WILLIAMS S B，DISSANAYAKE G，DURRANT－WHYTE H F.Field deployment of the simultaneous localization and mapping algorithm[C]//Proc ofIFAC World Congress on Automatic Control.Barcelona，Spain: IFAC Press，2002: 861－866.

[4]FOX D， BURGARD W，DELLAERT F，et al.Monte Carlo localization:efficient position estimation for mobile robots[C]//Proc of the 16th National Conference on Artificial Intelligence.1999: 1322－1328.

[5]LENSER S， VELOSO M.Sensor resetting localization for poorly modelled mobile robots [C]//Proc of IEEE International Conference on Robotics and Automation.San Francisco: IEEE Press， 2000:1225－1232.

[6]GORDON N．A hybrid bootstrap filter for target tracking in clutter[J].IEEE Trans on Aerospace and Electronic Systems，1997，33 (1):353－358．

[7]JENSFELT P，WIJK O，AUSTIN D，et al．Feature based condensation for mobile robot localization[C]//Proc ofIEEE International Confe－rence on Robotics and Automation．San Francisco: IEEE Press， 2000: 2531－2537．

[8]ADAMS M，ZHANG Sen， XIE Li－h(huán)ua．Particle filter based outdoor robot localization using natural features extended from laser scanners[C]//Proc of IEEE International Conference on Robotics and Automation．New Orleans: IEEE Press， 2004:1493－1498．

[9]FOX D，BURGARD W，DELLAERT F，et al.Monte Carlo localization for mobile robots[C]//Proc ofIEEE International Conference on Robotics and Automation．Detroit: IEEE Press，1999:1322－1328.

[10]MURPHY K．Bayesian map learning in dynamic environments[C]//Proc of Advances in Nearal Information Processing System. Marriott:MIT Press，1999: 1015－1021．

[11]DOUCET A， FREITAS N，MURPHY K， et al．Rao－Blackwellized particle filtering for dynamic Bayesian networks[C]//Proc of the 16th Conference on Uncertainty in Artificial Intelligence.[S.l.]:Springer－Verlag， 2001: 499－515．

[12]MONTEMERLO M，THRUN S，KOLLER D，et al．FastSLAM: a factored solution to the simultaneous localization and mapping problem[C]//Proc of the 18th National Conference on Artificial Intelligence． Menlo Park:Americorn Association for Artificial Intelligence，2002: 593－598．

[13]MONTEMERLO M， THRUN S．Simultaneous localization and mapping with unknown data association using FastSLAM[C]//Proc ofIEEE International Conference on Robotics and Automation. Taipei: IEEE Press， 2003: 1985－1991．

[14]DOUCET A．Convergence of sequential Monte Carlo methods，CUED/F－INFENG/TR381[R]．Cambridge:University of Cambridge， 2000．

[15]LEE D，CHUNG W， KIM M．Autonomous map building and smart localization of the service robot PSR[C]//Proc of IEEE International Conference on Intelligent Robots and Systems．2003:454－459．

注：本文中所涉及到的圖表、注解、公式等內(nèi)容請以PDF格式閱讀原文