網絡出版地址：http://www.cnki.net/kcms/detail/23.1538.tp.20150507.1135.001.html

改進RBPF的移動機器人同步定位與地圖構建

羅元，余佳航，汪龍峰，王運凱

(重慶郵電大學 國家信息無障礙工程研發(fā)中心，重慶 400065)

摘要：傳統Rao-Blackwellized粒子濾波器(RBPF)在移動機器人同步定位與地圖構建(SLAM)研究中，存在算法復雜度過高、占用內存空間過多導致實時性不理想的問題，因此提出一種改進算法。在某一特定狀態(tài)的一組粒子集中，粒子的統計特性是一致的，改進算法從中選取一個代表粒子，進行卡爾曼更新步驟，并在同一粒子集中重復使用。同時結合Gmapping算法的建議分布和自適應重采樣技術。實際Pioneer III移動機器人在機器人操作系統(ROS)平臺上進行的實驗表明，該方法在保證柵格地圖精度的同時能提高系統的實時性，降低復雜度，提高運算速度。

關鍵詞：移動機器人；Rao-Blackwellized粒子濾波器；同步定位與地圖構建(SLAM)；Gmapping算法；自適應重采樣技術

DOI:10.3969/j.issn.1673-4785.201404024

中圖分類號：TP242.6 文獻標志碼：A

收稿日期：2014-04-15. 網絡出版日期：2015-05-07.

基金項目：國家自然科學基金資助項目(51075420)；重慶市教委科學技術研究項目(KJ120519).

作者簡介：

中文引用格式：羅元，余佳航，汪龍峰，等. 改進RBPF的移動機器人同步定位與地圖構建[J]. 智能系統學報, 2015, 10(3): 460-464.

英文引用格式：LUO Yuan, YU Jiahang, WANG Longfeng, et al. Simultaneous localization and mapping of an improved RBPF based mobile robot [J]. CAAI Transactions on Intelligent Systems, 2015, 10(3):460-464.

Simultaneous localization and mapping of

an improved RBPF based mobile robot

LUO Yuan, YU Jiahang, WANG Longfeng, WANG Yunkai

(National Information Accessibility Engineering Research & Development Center, Chongqing University of Posts and Telecommunications, Chongqing 400065, China)

Abstract：As in the research of simultaneous localization and mapping (SLAM) of mobile robot applying traditional Rao-Blackwellized particle filter, the computational complexity is too high and memory space usage is too large, which causes poor real-time performance, an improved approach is proposed. Among a group of particles gathering in a particular state, the statistical properties of particles are identical. By applying the Kalman updating step to one representative particle in the group of particles, and using it repeatedly in the same group, the complexity is reduced and arithmetic speed is improved. Combining the proposed distribution and adaptive resampling methods from the Gmapping algorithm, the results of actual experiment carried out with Pioneer III robot and ROS platform illustrate that the real-time performance of the proposal could be enhanced while ensuring the quality of grid map.

Keywords：mobile robot; Rao-Blackwellized particle filter; simultaneous localization and mapping (SLAM); Gmapping algorithm; adaptive resampling methods

通信作者：余佳航. E-mail: tracy_the_1@126.com.

同步定位與地圖構建(simultaneous localization and mapping, SLAM)最先是由Smith等提出來的[1]，指移動機器人從一個未知的位置出發(fā)，在運動過程中利用傳感器對環(huán)境的觀測遞增地建立地圖,同時根據已建立的地圖同步確定自己的位置。十多年來，SLAM問題仍然是機器人研究領域的熱點[2]。

目前SLAM問題的解決方法以概率估計為主，主要分為基于卡爾曼濾波(KF)的算法和基于粒子濾波(PF)的算法。其中，基于EKF的SLAM算法[3]的研究最早開始于1986年，雖然計算速度快，但此類算法存在難以解決的數據關聯問題，且計算量過大、精度不高。UKF不需要進行雅可比矩陣計算、計算量小。而在實際應用時，系統噪聲相關信息的不確定性都會影響UKF濾波的精度，并且主要參數和濾波增益不能在線調整，缺乏自適應能力[4]。近年來，基于粒子濾波的方法正逐漸成為研究SLAM的熱點。相對于KF及其衍生算法，PF的優(yōu)點在于能有效處理非線性非高斯分布的狀態(tài)估計問題。Doucet等[5]提出有效解決SLAM問題的Rao-Blackwellized粒子濾波器(RBPF)方法。每一個粒子代表機器人一種可能的運動軌跡，同時每一個粒子都具有自己的全局地圖，它們和該粒子的軌跡相對應。因此該算法能夠較好地近似移動機器人位姿和環(huán)境地圖的聯合概率密度。但當粒子數目增多、環(huán)境地圖的尺寸增大時，算法會占用大量的內存空間，而且重采樣過程中，全局地圖進行拷貝需要占用大量的內存空間[6]。因此，降低計算復雜度以及減少構建精確地圖所需的粒子數成為此類方法需要重點解決的問題。Griseti等[7]通過改進建議分布和引入自適應重采樣機制提高該算法的執(zhí)行效率。

2010年Willow Garage公司發(fā)布了開源機器人操作系統(robot operating system, ROS),由于其具有點對點設計、不依賴編程語言、開源等優(yōu)點, 很快在機器人領域展開學習和使用ROS的熱潮[8]。在ROS中，Griseti等的算法被制作成一個名為Gmapping的SLAM功能包[9]，使用激光測距儀可以建立精度較高的二維柵格地圖，但其實時性仍有待提高。本文在此基礎上提出了一種降低復雜度的改進算法，在RBPF的卡爾曼更新步驟中，從一組粒子里選取一個代表粒子進行更新，避免計算所有粒子。通過在配有激光測距儀的Pioneer III機器人上的實驗來驗證該算法的有效性。

1RBPF與地圖構建

1.1Rao-Blackwellized變換

在SLAM問題中，Rao-Blackwellized變換可理解為將聯合后驗概率分解為對路徑的后驗概率和在給定路徑的情況下對地圖的后驗概率[10]。具體分解如式(1)：

(1)

式中：x為機器人運動軌跡，m為地圖，z為觀測信息，u為里程計信息。計算p(x1:t|z1:t,u1:t-1)可以使用粒子濾波算法，每一個粒子代表機器人可能的軌跡。對于p(m|x1:t,z1:t)，在求得p(x1:t|z1:t,u1:t-1)之后可以使用卡爾曼濾波算法進行分析性計算。

1.2使用RBPF進行地圖構建

如1.1小節(jié)所述，RBPF可以分為粒子濾波和卡爾曼濾波兩部分。

1.2.1粒子濾波部分

(2)

式中：π(x1:t|z1:t,u1:t-1)稱作重要性函數或者建議分布。歸一化后的重要性權值為

(3)

(4)

(5)

由此，地圖的后驗概率可估算為

(6)

可以使用一組N個卡爾曼濾波器進行分析性計算，每個粒子使用一個卡爾曼濾波器[12]。

1)蒙特卡羅步驟：RBPF算法中的蒙特卡羅步驟包括從先驗分布中采樣：

(7)

2)序貫重要性采樣步驟：包括計算式(5)中的p(x1:t|z1:t,u1:t-1)。如果把先驗分布當作重要性函數，例如π(x1:t|z1:t,u1:t-1)=P(xt|xt-1)，從而可以遞歸計算：

(8)

3)重采樣步驟：舍棄低權值粒子，保留高權值粒子。

1.2.2卡爾曼濾波部分

式中：

通過在均值和方差更新步驟使用卡爾曼濾波器，RBPF算法的估算精度能得到較大提高。然后，RBPF算法需要對每一個粒子使用卡爾曼濾波器，當有大量粒子時計算復雜度會急劇增加。

2改進RBPF算法

1)標準RBPF的卡爾曼更新步聚：

for i=1toNdo

endfor

2)改進RBPF的卡爾曼更新步聚：

fori=2toNdo

else

end if

end for

此外，建議分布采用文獻[7]中的

圖1　改進RBPF算法流程 Fig. 1　The flow chart of improved RBPF algorithm

3實驗結果及分析

實驗平臺由裝載URG-04LX激光測距儀的Pioneer III-DX機器人、配有Intel雙核、CPU 2.19 GHz、內存1.96 GB的筆記本電腦組成，筆記本電腦安裝Ubuntu 12.04操作系統與Groovy版本ROS，由筆記本鍵盤控制機器人的運動，如圖2所示。實驗環(huán)境為重慶郵電大學信息無障礙工程研發(fā)中心一樓，如圖3所示。

圖2　實驗硬件平臺 Fig. 2　Experiment hardware platform

圖3　實驗環(huán)境 Fig. 3　Experiment environment

在同步定位與地圖構建中，總時間消耗與機器人的運動速度和環(huán)境大小有關，具體算法的時間消耗難以確定。但可以通過控制機器人的運動速度來衡量算法的實時性：如果算法復雜度過大，計算所需時間因此較長，較快的運動速度將導致地圖的精度下降，需放慢運動速度以保證地圖精度；如果算法復雜度較小，計算所需時間因此較小，較快的運動速度下也能保證地圖精度。2種算法均需120個粒子，每種算法每種速度作為1組進行10次實驗，共進行60次實驗。每組第1次實驗中使用機器人實地運行，得出地圖同時將觀測數據(里程計數據與激光數據)記錄在bag文件中，其余9次實驗使用該bag文件中的數據進行仿真，得出相應地圖。bag文件為ROS特有的文件格式，可以將各類數據信息存儲在一個文件內，在調試算法過程中使用廣泛。使用bag文件進行算法調試可以保證每組實驗的數據一致，減小誤差。實驗結果顯示每組均出現共同特征，選取代表性結果進行對比。圖4和5為不同運動速度下Gmapping算法與改進算法所繪地圖。

圖4　3種不同速度下Gmapping算法所繪地圖 Fig. 4　Maps built with Gmapping in three different velocities

圖4　3種不同速度下改進算法所繪地圖 Fig. 5　 Maps builts with improved algorithm in three different velocities

實驗表明，2種算法隨著機器人運動速度的增加，均出現柵格誤差增大、地圖質量下降的情況。在0.2 m/s低速運動時，2種方法均能創(chuàng)建穩(wěn)定的地圖；以0.4 m/s運動時，Gmapping算法開始出現誤差，地圖出現不確定性，改進算法沒有出現誤差；以0.8 m/s運動時，Gmapping所繪地圖誤差增多，改進算法也出現誤差，但整體效果明顯強于前者。此外，改進算法在不同速度下所建地圖更為穩(wěn)定，邊緣更加清晰。

4結束語

本文提出了一種改進RBPF算法用于移動機器人的同步定位與地圖構建，通過卡爾曼濾波更新某一特定狀態(tài)一組粒子中的代表粒子，在必要時更新其均值和方差，并在同一粒子群中重復使用，以降低復雜度提高系統實時性。在近年來國外使用越來越廣泛的機器人操作系統平臺下進行實驗，實驗結果驗證了其有效性。在后續(xù)工作中，將致力于量化降低的復雜度以及所提高的實時性。

參考文獻:

[1]SMITH R, SELF M, CHEESEMAN P. Estimating uncertain spatial relationships in robotics[M]//COX I J, WILFONG G T. Autonomous Robot Vehicles. New York: Springer, 1990: 167-193.

[2]DISSANAYAKE G, HUANG S, WANG Z, et al. A review of recent developments in simultaneous localization and mapping[C]//6th International Conference on Industrial and Information Systems (ICIIS). Kandy, Sri Lanka, 2011: 477-482.

[3]SMITH R C, CHEESEMAN P. On the representation and estimation of spatial uncertainty[J]. The International Journal of Robotics Research, 1986, 5(4): 56-68.

[4]張文玲, 朱明清, 陳宗海. 基于強跟蹤UKF的自適應 SLAM 算法[J]. 機器人, 2010, 32(2): 190-195.

ZHANG Wenling, ZHU Mingqing, CHEN Zonghai. An adaptive SLAM algorithm based on strong tracking UKF[J]. Robot, 2010, 32(2): 190-195.

[5]DOUCET A, De FREITAS N, MURPHY K, et al. Rao-Blackwellised particle filtering for dynamic Bayesian networks[C]//Proceedings of the Sixteenth Conference on Uncertainty in Artificial Intelligence. San Francisco, USA, 2000: 176-183.

[6]QU Liping, WANG Hongjian. An overview of robot SLAM problem[C]//International Conference on Consumer Electronics, Communications and Networks (CECNet). Xianning, China, 2011: 1953-1956.

[7]GRISETTI G, STACHNISS C, BURGARD W. Improved techniques for grid mapping with Rao-Blackwellized particle filters[J]. Robotics, 2007, 23(1): 34-46.

[8]張建偉, 張立新, 胡穎, 等. 開源機器人操作系統—ROS[M]. 北京: 科學出版社, 2012: 1-6.

[9]GERKEY B. Gmapping[EB/OL]. [2010-08-05]. http://wiki.ros.org/slam_gmapping.

[10]GRISETTI G, STACHNISS C, BURGARD W. Improving grid-based slam with Rao-Blackwellized particle filters by adaptive proposals and selective resampling[C]//Proceedings of the 2005 IEEE International Conference on Robotics and Automation. Barcelona, Spain, 2005: 2432-2437.

[11]DOUCET A, De FREITAS N, GORDON N. An introduction to sequential Monte Carlo methods[M]//DOUCET A, DE FREITAS N, GORDON N. Sequential Monte Carlo Methods in Practice. New York: Springer, 2001: 3-14.

[12]De FREITAS N. Rao-Blackwellised particle filtering for fault diagnosis[C]//2002 IEEE Aerospace Conference Proceedings. Big Sky, USA, 2002, 4: 1767-1772.

羅元，女，1972年生，教授，博士，主要研究方向為機器視覺、數字圖像處理。獲得國家發(fā)明專利6項，重慶市科技進步獎1項。發(fā)表學術論文50余篇，被SCI、EI檢索20余篇。

余佳航，男，1990年生，碩士研究生，主要研究方向為移動機器人導航。

汪龍峰，男，1989年生，碩士研究生，主要研究方向為移動機器人導航。