基于激光雷達傳感器的RBPF-SLAM系統優化設計

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

(1.中國科學院 合肥物質科學研究院 應用技術研究所，安徽 合肥 230031; 2.中國科學技術大學，安徽 合肥 230026)

基于激光雷達傳感器的RBPF-SLAM系統優化設計

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

(1.中國科學院 合肥物質科學研究院 應用技術研究所，安徽 合肥 230031; 2.中國科學技術大學，安徽 合肥 230026)

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

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

0 引 言

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

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

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

1 室內移動機器人系統

1.1 移動機器人硬件平臺與傳感器系統

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

圖1 移動機器人系統結構

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

(1)

圖2 RPLIDAR A1工作原理

1.2 機器人操作系統與軟件實現

在Ubuntu14.04操作系統下，基于機器人操作系統(ROS)環境構建軟件系統。算法軟件實現系統，通過節點獲取傳感器數據，并將數據傳輸到定位與地圖構建模塊中，通過地圖節點進行定位和建圖，并借助可視化軟件.rivz，實時顯示機器人移動過程中構建的環境地圖，其軟件系統如圖3所示。

圖3 SLAM系統軟件實現框架

2 基于RBPF-SLAM方法優化

2.1RBPF-SLAM系統原理

(2)

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

(3)

(4)

傳統的RBPF-SLAM算法的基本思想是[8,9]：根據觀測信息z0∶k={zk|k=1,…,k},控制輸入信息u1∶k={uk|k=1,…,k},估計機器人的運動軌跡的后驗概率分布p(x0∶k|z0∶k,u1∶k,n1∶k)，然后推算機器人系統狀態的后驗概率分布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濾波估計路標位置。

2.2 RBPF-SLAM系統優化

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

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

2.2.1 建議分布函數優化

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

(6)

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

(7)

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

(8)

2.2.2 自適應重采樣

濾波過程中，引入有效粒子數Neff[11,12]估計當前粒子集對目標函數的近似程度，有效粒子數Neff的大小反映粒子集權值的方差大小，表示為

(9)

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

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

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

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

(Nh+Nl)

(10)

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

(11)

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

(12)

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

2.3 改進的RBPF-SLAM算法流程

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

5)更新粒子的重要性權值。

6)根據設定的粒子重采樣條件判斷，是否執行重采樣計算。

3 實驗分析

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

圖5 機器人平臺

1)基本建圖

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

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

圖6 搭建地圖及構建的柵格地圖

2)算法對比試驗

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

構建的辦公區域柵格地圖如圖7所示，其中圖7(a)和7(b)分別為改進算法構建的部分走廊、辦公區域的柵格地圖，圖7(c)和7(d)為傳統RBPF-SLAM方法構建的柵格地圖。從試驗結果可知：傳統RBPF-SLAM方法，構建的走廊及辦公區域柵格地圖出現失真。而改進的RBPF-SLAM方法在障礙物較多、結構化特征不明顯的復雜環境中時，也能夠對運動中的機器人位姿進行準確定位，構建出準確的柵格地圖，同時地圖精度明顯高于傳統方法。

圖7 柵格地圖

4 結 論

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

[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粒子濾波器實現移動機器人同時定位和地圖創建[J].吉林大學學報，2007,37(2):402-406.

[7] 祝繼華,鄭南寧,袁澤劍,等.基于ICP算法和粒子濾波的未知環境地圖創建[J].自動化學報,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] 沈林成，徐 昕，朱華勇,等.移動機器人自主控制技術[M].北京：科學出版社，2011:248-269.

[12] 張建偉，張立偉，胡 俊,等.開源機器人操作系統ROS[M].北京：科學出版社，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] 羅 元,蘇 琴,張 毅,等.基于優化RBPF的同時定位與地圖構建[J].華中科技大學學報:自然科學版,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-)，女，碩士研究生,研究方向為計算機視覺。