王依人， 鄧國(guó)慶， 劉 勇， 張 文，2

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

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

王依人1,2， 鄧國(guó)慶1， 劉 勇1， 張 文1，2

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

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

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

0 引 言

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

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

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

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

1.1 移動(dòng)機(jī)器人硬件平臺(tái)與傳感器系統(tǒng)

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

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

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

(1)

圖2 RPLIDAR A1工作原理

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

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

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

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

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

(2)

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

(3)

(4)

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

式中 右邊第一部分采用粒子濾波估計(jì)機(jī)器人運(yùn)動(dòng)路徑；第二部分采用擴(kuò)展Kalman濾波估計(jì)路標(biāo)位置。

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

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

圖4 觀測(cè)方程和運(yùn)動(dòng)方程的似然函數(shù)分布

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

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

(6)

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

(7)

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

(8)

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

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

(9)

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

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

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

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

(Nh+Nl)

(10)

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

(11)

如果第i個(gè)粒子被選擇的概率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 改進(jìn)的RBPF-SLAM算法流程

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

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

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

3 實(shí)驗(yàn)分析

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

圖5 機(jī)器人平臺(tái)

1)基本建圖

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

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

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

2)算法對(duì)比試驗(yàn)

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

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

圖7 柵格地圖

4 結(jié) 論

開展了室內(nèi)移動(dòng)機(jī)器人同時(shí)定位和環(huán)境建圖方法研究，基于激光雷達(dá)進(jìn)行RBPF-SLAM方法改進(jìn)，根據(jù)高精度激光雷達(dá)數(shù)據(jù)，修正基于里程計(jì)讀數(shù)的建議分布函數(shù)，減少了濾波過程所需的粒子數(shù)目；同時(shí)引入了自適應(yīng)重采樣機(jī)制，緩解了由于重采樣帶來的粒子消耗問題。為驗(yàn)證改進(jìn)算法性能，進(jìn)行建圖和與傳統(tǒng)方法對(duì)比實(shí)驗(yàn)，結(jié)果表明：改進(jìn)后的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] 歷茂海，洪炳榮，羅榮華.用改進(jìn)的Rao-blackwellized粒子濾波器實(shí)現(xiàn)移動(dòng)機(jī)器人同時(shí)定位和地圖創(chuàng)建[J].吉林大學(xué)學(xué)報(bào)，2007,37(2):402-406.

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

[12] 張建偉，張立偉，胡 俊,等.開源機(jī)器人操作系統(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的同時(shí)定位與地圖構(gòu)建[J].華中科技大學(xué)學(xué)報(bào):自然科學(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-)，女，碩士研究生,研究方向?yàn)橛?jì)算機(jī)視覺。