網(wǎng)絡(luò)出版地址：http://www.cnki.net/kcms/detail/23.1538.tp.20150507.1135.001.html

改進(jìn)RBPF的移動機(jī)器人同步定位與地圖構(gòu)建

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

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

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

關(guān)鍵詞：移動機(jī)器人；Rao-Blackwellized粒子濾波器；同步定位與地圖構(gòu)建(SLAM)；Gmapping算法；自適應(yīng)重采樣技術(shù)

DOI:10.3969/j.issn.1673-4785.201404024

中圖分類號：TP242.6 文獻(xiàn)標(biāo)志碼：A

收稿日期：2014-04-15. 網(wǎng)絡(luò)出版日期：2015-05-07.

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

作者簡介：

中文引用格式：羅元，余佳航，汪龍峰，等. 改進(jìn)RBPF的移動機(jī)器人同步定位與地圖構(gòu)建[J]. 智能系統(tǒng)學(xué)報, 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.

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

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

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

1RBPF與地圖構(gòu)建

1.1Rao-Blackwellized變換

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

(1)

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

1.2使用RBPF進(jìn)行地圖構(gòu)建

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

1.2.1粒子濾波部分

(2)

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

(3)

(4)

(5)

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

(6)

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

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

(7)

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

(8)

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

1.2.2卡爾曼濾波部分

式中：

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

2改進(jìn)RBPF算法

1)標(biāo)準(zhǔn)RBPF的卡爾曼更新步聚：

for i=1toNdo

endfor

2)改進(jìn)RBPF的卡爾曼更新步聚：

fori=2toNdo

else

end if

end for

此外，建議分布采用文獻(xiàn)[7]中的

圖1　改進(jìn)RBPF算法流程 Fig. 1　The flow chart of improved RBPF algorithm

3實驗結(jié)果及分析

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

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

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

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

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

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

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

4結(jié)束語

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

參考文獻(xiàn):

[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]張文玲, 朱明清, 陳宗海. 基于強(qiáng)跟蹤UKF的自適應(yīng) SLAM 算法[J]. 機(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]張建偉, 張立新, 胡穎, 等. 開源機(jī)器人操作系統(tǒng)—ROS[M]. 北京: 科學(xué)出版社, 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年生，教授，博士，主要研究方向為機(jī)器視覺、數(shù)字圖像處理。獲得國家發(fā)明專利6項，重慶市科技進(jìn)步獎1項。發(fā)表學(xué)術(shù)論文50余篇，被SCI、EI檢索20余篇。

余佳航，男，1990年生，碩士研究生，主要研究方向為移動機(jī)器人導(dǎo)航。

汪龍峰，男，1989年生，碩士研究生，主要研究方向為移動機(jī)器人導(dǎo)航。