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

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

關鍵詞：局部地圖； 同時定位和地圖創建； 粒子濾波算法； 貝葉斯規則； 柵格地圖

中圖分類號：TP24文獻標志碼：A

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

0引言

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

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

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

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

1標準的粒子濾波SLAM算法

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

標準的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)為對應Xr(k)的權值；d0，…，k表示由時刻0到k時刻的數據。樣本集通過離散化狀態來近似表示機器人實際位姿的信度。給定不同時間段0，1，…，k的一系列測量值Zk={Z(0)，…，Z(k)}估計狀態Xr(k)，這里Xr(k)是移動機器人的位姿Xr(k)=(xr(k)，yr(k)，θr(k))。系統描述如下：

1．2標準的PF－SLAM算法

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



2改進的PF－SLAM算法

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

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

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

3實驗結果

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

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

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

由表1的仿真統計數據可以看出，改進的PF－SLAM算法比標準的PF－SLAM的定位和地圖創建的精度高。

4結束語

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

參考文獻：

［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－hua．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．

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