基于激光雷達傳感器的RBPF-SLAM系統(tǒng)優(yōu)化設(shè)計

王依人， 鄧國慶， 劉 勇， 張 文，2

(1.中國科學(xué)院 合肥物質(zhì)科學(xué)研究院 應(yīng)用技術(shù)研究所，安徽 合肥 230031; 2.中國科學(xué)技術(shù)大學(xué)，安徽 合肥 230026)

基于激光雷達傳感器的RBPF-SLAM系統(tǒng)優(yōu)化設(shè)計

王依人1,2， 鄧國慶1， 劉 勇1， 張 文1，2

(1.中國科學(xué)院 合肥物質(zhì)科學(xué)研究院 應(yīng)用技術(shù)研究所，安徽 合肥 230031; 2.中國科學(xué)技術(shù)大學(xué)，安徽 合肥 230026)

針對傳統(tǒng)Rao-Blackwellized粒子濾波(RBPF)方法的同步定位和建圖(SLAM)問題，提出了一種基于激光雷達的RBPF-SLAM系統(tǒng)優(yōu)化方法，利用高精度激光雷達數(shù)據(jù)，修正了基于里程計讀數(shù)的建議分布函數(shù)，減少了濾波過程所需的粒子數(shù)目；引入了自適應(yīng)重采樣機制，緩解由于重采樣帶來的粒子消耗問題。為驗證改進算法性能，在搭建的差速型移動機器人平臺上，進行了驗證試驗，結(jié)果表明：改進后的RBPF-SLAM方法，能夠?qū)崟r構(gòu)建柵格地圖，在建圖效率和精度上均有明顯的提升。

激光雷達傳感器; 同時定位與建圖(SLAM); 機器人; Rao-Blackwellized粒子濾波(RBPF)

0 引 言

室內(nèi)自主移動機器人具有廣泛的應(yīng)用前景，其實現(xiàn)自主移動需同時具備環(huán)境建圖、自主定位、路徑規(guī)劃、自主避障、運動控制等能力，而實現(xiàn)同步定位和環(huán)境建圖(simultaneous localization and mapping,SLAM)是核心問題，也是當(dāng)前機器人領(lǐng)域的研究熱點。

機器人SLAM，概念最早由Smith R,Self M,Cheeseman P提出[1]，指機器人在未知環(huán)境中，借助傳感器感知環(huán)境，以構(gòu)建環(huán)境地圖，同時確定自身在環(huán)境中所處位置的方法。Hammersley J M等人提出了基于粒子濾波的SLAM方法[2]，利用狀態(tài)空間中一組帶權(quán)值的粒子，擬合機器人系統(tǒng)狀態(tài)的后驗概率分布，但由于計算量大，該方法發(fā)展緩慢。20世紀初，Murphy K等人提出了基于Rao-Blackwellized粒子濾波(Rao-Blackwellized particle filtering,RBPF)方法[3]，Montemerlo M等人又將RBPF應(yīng)用于SLAM問題中[4，5]RBPF-SLAM，使得粒子濾波的SLAM方法計算量大大減少，促進了SLAM技術(shù)的發(fā)展。國內(nèi)，哈爾濱工業(yè)大學(xué)歷茂海、洪炳榮等人提出了自適應(yīng)的重采樣的RBPF-SLAM方法[6]；祝繼華等人，改進了粒子采樣過程中的建議分布函數(shù)及重采樣方法[7]，緩解了SLAM過程中的粒子消耗問題。

本文在RBPF-SLAM方法的基礎(chǔ)上，利用高精度的激光雷達傳感器數(shù)據(jù)，修正基于里程計讀數(shù)的建議分布函數(shù)，獲得了更為有效的粒子集合。同時，引入自適應(yīng)重采樣機制，有效緩解粒子濾波過程中的粒子消耗問題。為驗證算法的有效性，以Hands Free開源項目為模型，搭建了機器人平臺并實現(xiàn)本文算法，并開展了相關(guān)實驗。

1 室內(nèi)移動機器人系統(tǒng)

1.1 移動機器人硬件平臺與傳感器系統(tǒng)

以Hands Free機器人為模型，搭建了差速型移動機器人平臺，系統(tǒng)結(jié)構(gòu)如圖1所示。主要包括機械結(jié)構(gòu)部分、傳感器部分、控制部分組成，包含電機驅(qū)動器、控制器、傳感器、電池等模塊。系統(tǒng)中激光雷達、編碼器與嵌入式實時控制器連接，嵌入式控制設(shè)備與上位機連接。各模塊信息處理通過上位機實現(xiàn)，實時獲取傳感器數(shù)據(jù)并進行處理。

圖1 移動機器人系統(tǒng)結(jié)構(gòu)

RBPF-SLAM系統(tǒng)，主要使用激光雷達和里程計數(shù)據(jù)。其中，里程計通過編碼器記錄機器人軌跡。激光雷達屬于外部傳感器，測量數(shù)據(jù)包含被觀測環(huán)境的結(jié)構(gòu)化信息。文中選取激光雷達RPLIDAR A1，其原理如圖2所示，可實現(xiàn)360°，6 m范圍內(nèi)的激光測距掃描，掃描頻率最高可達10 Hz，分辨率達到1°。原始激光雷達數(shù)據(jù)用極坐標(biāo)(d,θ)表示， 轉(zhuǎn)換為笛卡爾坐標(biāo)

(1)

圖2 RPLIDAR A1工作原理

1.2 機器人操作系統(tǒng)與軟件實現(xiàn)

在Ubuntu14.04操作系統(tǒng)下，基于機器人操作系統(tǒng)(ROS)環(huán)境構(gòu)建軟件系統(tǒng)。算法軟件實現(xiàn)系統(tǒng)，通過節(jié)點獲取傳感器數(shù)據(jù)，并將數(shù)據(jù)傳輸?shù)蕉ㄎ慌c地圖構(gòu)建模塊中，通過地圖節(jié)點進行定位和建圖，并借助可視化軟件.rivz，實時顯示機器人移動過程中構(gòu)建的環(huán)境地圖，其軟件系統(tǒng)如圖3所示。

圖3 SLAM系統(tǒng)軟件實現(xiàn)框架

2 基于RBPF-SLAM方法優(yōu)化

2.1RBPF-SLAM系統(tǒng)原理

(2)

式中 函數(shù)π(·)為建議分布函數(shù)。RBPF-SLAM算法中，為簡化計算一般以基于里程計讀數(shù)的運動模型為建議分布函數(shù)，計算公式如下

(3)

(4)

傳統(tǒng)的RBPF-SLAM算法的基本思想是[8,9]：根據(jù)觀測信息z0∶k={zk|k=1,…,k},控制輸入信息u1∶k={uk|k=1,…,k},估計機器人的運動軌跡的后驗概率分布p(x0∶k|z0∶k,u1∶k,n1∶k)，然后推算機器人系統(tǒng)狀態(tài)的后驗概率分布p(x0∶k,m|z0∶k,u1∶k,n1∶k)，表示為

p(x0∶k,m|z0∶k,u1∶k,n1∶k)=p(x0∶k|z0∶k,u1∶k,n1∶k)

(5)

式中 右邊第一部分采用粒子濾波估計機器人運動路徑；第二部分采用擴展Kalman濾波估計路標(biāo)位置。

2.2 RBPF-SLAM系統(tǒng)優(yōu)化

機器人在實際運動過程中，機器人運動模型誤差相對于觀測模型誤差較大，似然概率與先驗概率分布之間重疊較小，如圖4所示。此時，根據(jù)基于運動模型的建議分布函數(shù)重采樣的RBPF-SLAM方法，存在粒子多樣性退化和重要粒子丟失的問題。針對上述情況，提出了利用高精度的激光雷達數(shù)據(jù)修正機器人運動模型，優(yōu)化建議分布函數(shù)。同時引入自適應(yīng)重采樣機制，緩解了由于重采樣產(chǎn)生的粒子消耗問題。

圖4 觀測方程和運動方程的似然函數(shù)分布

2.2.1 建議分布函數(shù)優(yōu)化

由于里程計易受外部環(huán)境干擾，將魯棒性較強的激光雷達數(shù)據(jù)引入到建議分布函數(shù)中，修正基于里程計讀數(shù)的機器人運動方程，獲得改進的建議分布函數(shù)。改進后的建議分布函數(shù)為

(6)

代入粒子權(quán)重計算公式(3)可得

(7)

將激光雷達數(shù)據(jù)加入建議分布函數(shù)中，主要根據(jù)激光雷達的前、后2幀掃描數(shù)據(jù)進行匹配，獲得機器人的運動參數(shù)，修正依靠里程計數(shù)據(jù)估計的機器人運動模型。本文通過迭代最近點(iterative closed point，ICP)算法[10]進行掃描匹配，表示為

(8)

2.2.2 自適應(yīng)重采樣

濾波過程中，引入有效粒子數(shù)Neff[11,12]估計當(dāng)前粒子集對目標(biāo)函數(shù)的近似程度，有效粒子數(shù)Neff的大小反映粒子集權(quán)值的方差大小，表示為

(9)

為保持粒子多樣性，減少粒子退化，引入了一種自適應(yīng)的重采樣方法，該方法的思想為：重采樣過程中，對集中具有較高權(quán)值和較低權(quán)值的粒子進行采樣，保留具有中等權(quán)值的粒子。具體步驟為：

1)根據(jù)文獻[13，14]中提出的相關(guān)方法，設(shè)置采樣粒子高權(quán)重閾值wh=2/N和低權(quán)重閾值wl=1/(2N)。根據(jù)粒子的重要性權(quán)值與設(shè)置的閾值大小關(guān)系，將樣本粒子分為高、中、低三部分。

2)計算重采樣過程中各樣本粒子被復(fù)制的次數(shù)ni。中等權(quán)重粒子保留，高權(quán)重粒子和低權(quán)重粒子按權(quán)重由高到低排序。計算每個粒子被選擇的概率為

p(i)={a+[rank(i)/(Nh+Nl-1)]·(b-a)}/

(Nh+Nl)

(10)

式中 rank(i)為粒子根據(jù)權(quán)重排序后的序號；a,b為系數(shù)，當(dāng)粒子數(shù)確定時，a=2-b,1≤b≤2，取值為

(11)

如果第i個粒子被選擇的概率p(i)*=1/(Nh+Nl)≥p(i)，則該粒子被復(fù)制，復(fù)制次數(shù)ni為

(12)

根據(jù)該算法執(zhí)行重采樣，如果粒子被復(fù)制的總次數(shù)∑n(i)<(Nh+Nl)，則權(quán)重由高到低的增加粒子被復(fù)制次數(shù)，最終使得∑ni=Nh+Nl；如果粒子被復(fù)制的總次數(shù)∑n(i)>(Nh+Nl)，則依次減少權(quán)重由低到高的粒子被復(fù)制次數(shù)，保證∑ni=Nh+Nl的總數(shù)不變。

2.3 改進的RBPF-SLAM算法流程

4)根據(jù)改進的建議分布，采樣獲取新的位姿粒子集。

5)更新粒子的重要性權(quán)值。

6)根據(jù)設(shè)定的粒子重采樣條件判斷，是否執(zhí)行重采樣計算。

3 實驗分析

為探究改進后的RBPF-SLAM方法性能，開展算法驗證實驗。本文自主搭建的機器人平臺如圖5所示，所述算法在計算機(處理器：IntelCore i5—4200U;RAM:4.00 GB)Ubuntu14.04操作系統(tǒng)環(huán)境下，基于ROS框架環(huán)境編程實現(xiàn)，其中激光雷達RPLIDAR A1安裝于機器人平臺頂部，里程計位于平臺內(nèi)部，分別開展基本建圖實驗和對比試驗。

圖5 機器人平臺

1)基本建圖

首先在圖6(a)所示的模擬環(huán)境中，開展建圖實驗。實驗構(gòu)建柵格地圖初始設(shè)置為200 m×200 m，算法中濾波定位部分，樣本粒子數(shù)目經(jīng)過多次試驗后設(shè)置為30，效率和建圖效果最佳。

實驗結(jié)果表明：機器人在地圖中移動，可以根據(jù)激光雷達和里程計數(shù)據(jù)，構(gòu)建滿足自主機器人導(dǎo)航的柵格地圖，圖6(b)所示。

圖6 搭建地圖及構(gòu)建的柵格地圖

2)算法對比試驗

實際情況下，機器人工作環(huán)境較搭建地圖復(fù)雜，環(huán)境特征更為豐富，且存在較多不確定因素干擾，例如移動的物體、行人等。為了驗證此類情況下改進算法的有效性，分別采用傳統(tǒng)RBPF-SLAM方法和本文改進算法進行定位和建圖試驗。

構(gòu)建的辦公區(qū)域柵格地圖如圖7所示，其中圖7(a)和7(b)分別為改進算法構(gòu)建的部分走廊、辦公區(qū)域的柵格地圖，圖7(c)和7(d)為傳統(tǒng)RBPF-SLAM方法構(gòu)建的柵格地圖。從試驗結(jié)果可知：傳統(tǒng)RBPF-SLAM方法，構(gòu)建的走廊及辦公區(qū)域柵格地圖出現(xiàn)失真。而改進的RBPF-SLAM方法在障礙物較多、結(jié)構(gòu)化特征不明顯的復(fù)雜環(huán)境中時，也能夠?qū)\動中的機器人位姿進行準(zhǔn)確定位，構(gòu)建出準(zhǔn)確的柵格地圖，同時地圖精度明顯高于傳統(tǒng)方法。

圖7 柵格地圖

4 結(jié) 論

開展了室內(nèi)移動機器人同時定位和環(huán)境建圖方法研究，基于激光雷達進行RBPF-SLAM方法改進，根據(jù)高精度激光雷達數(shù)據(jù)，修正基于里程計讀數(shù)的建議分布函數(shù)，減少了濾波過程所需的粒子數(shù)目；同時引入了自適應(yīng)重采樣機制，緩解了由于重采樣帶來的粒子消耗問題。為驗證改進算法性能，進行建圖和與傳統(tǒng)方法對比實驗，結(jié)果表明：改進后的RBPF-SLAM系統(tǒng)，能夠構(gòu)建更加準(zhǔn)確的環(huán)境地圖。

[1] Smith R,Self M,Cheeseman P.Estimating uncertain spatial relationships in robotics[C]∥Proceedings of 1987 IEEE Interna-tional Conference on Robotics and Automation,North-Holland:1987:850.

[2] Hammersley J M,Morton K W.Poorman's Monte Carlo[J].J of the Royal Statistical Society B,1954,16(1):23-38.

[3] Murphy K,Russell S.Rao-Blackwellized particle filtering for dynamic Bayesian networks,sequential MonteCarlo methods[M].Lorne,Australia:Springer-Verlag,2001:499-515.

[4] Montemerlo M,Thrun S,Koller D,et al.FastSLAM:A factored solutionto the simultaneous localization and mapping pro-blem[C]∥Proceedings of the AAAI National Conference on Artificial Intelligence,Menlo Park,CA,USA:AAA I,2002:593-598.

[5] Montemerlo M,Thrun S,Koller D,et al.FastSLAM 2.0:An improved particle filtering algorithm for simultaneous localization and mapping that provably converges[C]∥Proceedigns of the Sixteenth International Joint Conference on Artificial Intelligence(IJCAI),Menlo Park,CA,USA:AAAI,2003:1151-1156.

[6] 歷茂海，洪炳榮，羅榮華.用改進的Rao-blackwellized粒子濾波器實現(xiàn)移動機器人同時定位和地圖創(chuàng)建[J].吉林大學(xué)學(xué)報，2007,37(2):402-406.

[7] 祝繼華,鄭南寧,袁澤劍,等.基于ICP算法和粒子濾波的未知環(huán)境地圖創(chuàng)建[J].自動化學(xué)報,2009,35(8):1107-1113.

[8] Giorgio Grisetti,Cyrill Stachniss,Wolfram Burgard.Improving grid-based SLAM with Rao-Blackwellized particle filters by adaptive proposals and selective resampling[C]∥IEEE International Conference on robotics and Automation(ICRA),2005.

[9] Giorgio Grisetti,Cyrill Stachniss,Wolfram Burgard.Improved techniques for grid mapping with Rao-Blackwellized particle filters[J].IEEE Transactions on Robotics,2007,23:34-46.

[10] Besl P J,Mckay N D.Method for registration of 3D shapes[J].IEEE Transactions on Pattern Analysis & Machine Intelligence,1992,14(2):239-256.

[11] 沈林成，徐 昕，朱華勇,等.移動機器人自主控制技術(shù)[M].北京：科學(xué)出版社，2011:248-269.

[12] 張建偉，張立偉，胡 俊,等.開源機器人操作系統(tǒng)ROS[M].北京：科學(xué)出版社，2012.

[13] Bolic M,Djuric P M,Hong S.New resampling algorithms for particle filters[C]∥IEEE International Conference on Acoustics,Speech,and Signal Processing,HongKong: IEEE，2003:589-592.

[14] 羅 元,蘇 琴,張 毅,等.基于優(yōu)化RBPF的同時定位與地圖構(gòu)建[J].華中科技大學(xué)學(xué)報:自然科學(xué)版,2016,44(5):30-34.

Optimal design of RBPF-SLAM system based on LIDAR sensor

WANG Yi-ren1,2, DENG Guo-qing1, LIU Yong1, ZHANG Wen1,2

(1.Institute of Applied Technology,Hefei Institutes of Physical Science,Chinese Academy of Sciences, Hefei 230031,China; 2.University of Science and Technology of China,Hefei 230026,China)

Aiming at problem of simultaneous localization and mapping(SLAM)of traditional Rao-Blackwellized particle filtering(RBPF)method,an optimization method of RBPF-SLAM system based on laser radar(LIDAR)is proposed.This method uses high precision LIDAR data to correct the proposed distribution function based on the odometer readings and reduce number of particles required for filtering process.At the same time, the adaptive resampling mechanism is introduced to alleviate the problem of particle consumption due to resampling.In order to verify the performance of the improved algorithm,a method validation test is carried out on the platform of the differential mobile robot.The results show that the improved RBPF-SLAM method can build the gridding map in real time,and it can improve the efficiency and precision of mapping.

laser radar(LIDAR)sensor; simultaneous localization and mapping(SLAM); robot; Rao-Blackwellized particle filtering(RBPF)

10.13873/J.1000—9787(2017)09—0077—04

2017—07—13

TP 242.6

A

1000—9787(2017)09—0077—04

王依人(1990-)，女，碩士研究生,研究方向為計算機視覺。