兩種基于激光雷達(dá)的SLAM算法最優(yōu)參數(shù)分析*

高文研， 平雪良， 貝旭穎， 陳 威

(江南大學(xué) 機(jī)械工程學(xué)院 江蘇省食品先進(jìn)制造裝備技術(shù)重點(diǎn)實(shí)驗(yàn)室,江蘇 無錫 214122)

0 引 言

準(zhǔn)確的先驗(yàn)地圖對(duì)移動(dòng)機(jī)器人導(dǎo)航定位和路徑規(guī)劃具有重要意義[1]。激光雷達(dá)具有精度高，誤差累積小等優(yōu)點(diǎn)，在眾多傳感器中脫穎而出，越來越多地用于移動(dòng)機(jī)器人地圖構(gòu)建定位[2]。開源機(jī)器人操作系統(tǒng)(robot operating system，ROS)平臺(tái)，由于開放的理念及代碼的復(fù)用性，極大減少了工作量，目前已在世界范圍得到了大量應(yīng)用[3]。

目前，ROS平臺(tái)下主要有2種基于激光雷達(dá)的地圖構(gòu)建方法：Gmapping和Hector同步定位與地圖構(gòu)建(SLAM)。前者主要采用Rao-Blackwellized粒子濾波(Rao-Blackwellized particle filtering，RBPF)方法，借助里程計(jì)信息實(shí)現(xiàn)位姿估計(jì)；后者采用Gauss-Newton迭代法，通過激光數(shù)據(jù)匹配進(jìn)行位姿估計(jì)[4]。其他設(shè)備如Kinect，采集的數(shù)據(jù)大多轉(zhuǎn)化為激光數(shù)據(jù)的格式進(jìn)行處理[5]。

目前,相關(guān)研究多偏于理論，缺少對(duì)不同參數(shù)配置下各算法優(yōu)劣的對(duì)比。地圖構(gòu)建的效果和算法息息相關(guān)，但參數(shù)的選擇對(duì)最終地圖的生成也起著至關(guān)重要的作用。本文旨在通過不同參數(shù)配置條件下實(shí)現(xiàn)地圖構(gòu)建，對(duì)比二者構(gòu)圖效果，并指明參數(shù)最優(yōu)配置的選擇方法，實(shí)現(xiàn)算法的最優(yōu)性能。

1 硬件與算法

1.1 激光雷達(dá)

采用的RPLidar A2激光雷達(dá)具有4 000次/s的高速激光測距采樣能力，可以在二維平面8 m半徑范圍內(nèi)進(jìn)行 360°全方位的激光測距掃描，并產(chǎn)生所在空間的平面點(diǎn)云地圖信息。測距范圍為0.15～8 m，分辨率小于0.5 mm，最大掃描頻率可達(dá)10 Hz，具有很高的精度。

激光傳感器數(shù)據(jù)在ROS分布式架構(gòu)下封裝為sensor_msgs/LaserScan.msg數(shù)據(jù)格式，由/scan話題完成激光數(shù)據(jù)的發(fā)布工作。節(jié)點(diǎn)訂閱該話題即可獲得激光數(shù)據(jù)。

1.2 Gmapping算法

Gmapping算法基于RBPF方法，用于解決基于柵格的SLAM問題。需要里程計(jì)信息作為輸入，并以激光雷達(dá)的數(shù)據(jù)作為觀測值。R-B粒子濾波器的SLAM可以理解為將后驗(yàn)概率分解成機(jī)器人路徑和特征地圖兩部分[6]，即

p(x1∶t,m|z1∶t,u1∶t)=p(x1∶t|z1∶t,u1∶t)p(m|z1∶t,u1∶t)=

p(x1∶t|z1∶t,u1∶t-1)p(m|x1∶t,z1∶t)

(1)

式中x為機(jī)器人軌跡；m為由z觀測得到的特征地圖；u為里程計(jì)測量值。

RBPF算法運(yùn)用粒子濾波器來估計(jì)后驗(yàn)概率，每個(gè)粒子代表一種潛在的軌跡，地圖的生成高度依賴軌跡信息。但若傳感器觀測模型的誤差遠(yuǎn)小于里程計(jì)誤差，觀測模型的似然函數(shù)概率和先驗(yàn)概率分布重疊區(qū)間很小，且只有小部分處于觀測似然區(qū)間的粒子在更新后權(quán)值會(huì)增大。這部分粒子的權(quán)重與其他粒子相差較大，重采樣后極可能丟失重要粒子。

Gmapping算法的提出正是基于上述問題。雖然里程計(jì)具有較高的可信度，但仍然存在著噪聲大、不精確等問題，Gmapping算法將里程計(jì)和最新觀測信息同時(shí)作為提議分布，并使采樣盡可能分布在觀測模型的似然函數(shù)可行區(qū)域中。采樣提議分布如下[7]

(2)

與僅采用運(yùn)動(dòng)模型作提議分布的方法相比，提議分布融合了里程計(jì)信息和觀測模型，更易取得似然函數(shù)的峰值。上述改進(jìn)可以減少粒子損耗的風(fēng)險(xiǎn)，能夠?qū)崿F(xiàn)更精確的地圖構(gòu)建。

1.3 Hector SLAM

Hector SLAM屬于一種2D的SLAM方法。與其他柵格地圖構(gòu)建算法[9,11,12]相比，Hector SLAM不需要里程計(jì)信息，其2D位姿估計(jì)只基于輸入的激光數(shù)據(jù)與先驗(yàn)地圖進(jìn)行匹配。激光雷達(dá)的高更新頻率和高精度，為其快速構(gòu)圖與位姿估計(jì)提供了硬件保障。

Hector SLAM采用的激光匹配算法基于Gauss-Newton迭代公式[13]。通過尋找移動(dòng)機(jī)器人到先驗(yàn)地圖的最佳剛體轉(zhuǎn)換ξ=[pxpyψ]T，使激光數(shù)據(jù)與地圖達(dá)到了最優(yōu)匹配。即求最小的ξ*[4]

(3)

式中M(Si(ξ))為地圖在Si(ξ)處的值，如果再給定一個(gè)位姿估計(jì)的初始值ξ，則Δξ的求取轉(zhuǎn)化為通過迭代求最優(yōu)化問題

(4)

用一階泰勒展開式將M(Si(ξ+Δξ))進(jìn)行展開，并對(duì)Δξ求偏導(dǎo)數(shù)，得

Δξ=

(5)

式中H為黑塞矩陣。

盡管Hector SLAM算法本身不提供閉環(huán)檢測，但適用于多種場景，并構(gòu)建出完整的環(huán)境地圖，只需要很低的計(jì)算量，算法的實(shí)時(shí)性大幅提高。事實(shí)上，該算法已經(jīng)成功應(yīng)用于自主移動(dòng)機(jī)器人、交通工具、手持設(shè)備及無人機(jī)領(lǐng)域，證明了該算法的實(shí)用性。

2 實(shí)驗(yàn)與結(jié)果

2.1 實(shí)驗(yàn)平臺(tái)及架構(gòu)

實(shí)驗(yàn)平臺(tái)為自主研發(fā)的JNPF—2移動(dòng)機(jī)器人，其控制系統(tǒng)架構(gòu)及實(shí)物如圖1、圖2所示。嵌入式Linux平臺(tái)通過WiFi與計(jì)算機(jī)進(jìn)行通信；計(jì)算機(jī)端運(yùn)算能力強(qiáng)，用于數(shù)據(jù)量較大的運(yùn)算；嵌入式平臺(tái)由于運(yùn)算能力有限，只運(yùn)行基本的功能包；STM32提供豐富的接口用于下位機(jī)的信息采集與驅(qū)動(dòng)控制。

圖1 移動(dòng)機(jī)器人控制系統(tǒng)架構(gòu)

圖2 移動(dòng)機(jī)器人實(shí)物

2.2 實(shí)驗(yàn)環(huán)境與變量

主要基于室內(nèi)實(shí)驗(yàn)室環(huán)境，室內(nèi)擺放了若干桌椅，清除不必要的雜物。根據(jù)Wiki官方資料，影響Gmapping與Hector SLAM構(gòu)圖效果的因素有很多，其中起重要作用的參數(shù)分別為：

Gmapping:particles、linearUpdate和angularUpdate。

Hector SLAM:map_update_distance_thresh和map_update_angle_thresh。

Gmapping構(gòu)圖時(shí)，首先對(duì)particles進(jìn)行控制，選取合適的粒子數(shù)；然后保持粒子數(shù)目最優(yōu)，對(duì)linearUpdate和angularUpdate進(jìn)行測試。Hector SLAM構(gòu)圖時(shí)，首先確定map_update_distance_thresh和map_update_angle_thresh影響較大的范圍，再在這個(gè)范圍內(nèi)取值進(jìn)行測試。

2.3 構(gòu)圖效果分析

理論上，粒子數(shù)目越多，構(gòu)建出的地圖就越準(zhǔn)確。實(shí)驗(yàn)結(jié)果如圖3，通過比較發(fā)現(xiàn)，在粒子數(shù)目P由30增加到150的過程中，生成的地圖精度不斷變高，具體體現(xiàn)在邊角特征明顯，構(gòu)建出的邊特征鋸齒明顯變少；但當(dāng)粒子數(shù)目增大到180以上時(shí)，地圖效果較粒子數(shù)目為150時(shí)差，這是由于計(jì)算負(fù)載陡增，實(shí)時(shí)性降低導(dǎo)致。

圖3 改變粒子數(shù)對(duì) Gmapping 影響

在最優(yōu)粒子數(shù)目下，LinearUpdate和AngularUpdate參數(shù)對(duì)構(gòu)圖效果影響如圖4。

圖4 最優(yōu)粒子數(shù)目下Linear和Angular update對(duì)Gmapping影響

易知，AngularUpdate參數(shù)為0.2且LinearUpdate參數(shù)為0.1時(shí)地圖構(gòu)建效果最好。通過對(duì)比各列，隨著LinearUpdate參數(shù)的增大，邊特征的鋸齒狀效果越來越明顯；通過對(duì)比各行，隨AngularUpdate增大，邊特征越來越不明顯。整體來看，不同參數(shù)的選取，對(duì)Gmapping最終的構(gòu)圖效果影響有限。

angle_thresh和distance_thresh參數(shù)對(duì)地圖構(gòu)建效果影響如圖5。對(duì)此范圍之外的參數(shù)值，在地圖構(gòu)建啟動(dòng)時(shí)會(huì)有地圖重疊現(xiàn)象，如圖5中箭頭標(biāo)識(shí)處，已經(jīng)進(jìn)行剔除。angle_thresh在小于0.4的范圍內(nèi)影響作用有限。隨著angle_thresh的增大，構(gòu)建出的地圖邊特征趨于明顯；在angle_thresh不變的情況下，更小的distance_thresh值可以使地圖更為精確，較大的distance_thresh會(huì)使地圖起始位置產(chǎn)生重疊。

圖5 Hector SLAM 構(gòu)圖效果

3 結(jié) 論

使用RPLidar A2進(jìn)行SLAM實(shí)驗(yàn)，對(duì)比了Gmapping和Hector SLAM的構(gòu)圖效果，并且分析了各參數(shù)對(duì)最終地圖生成的影響，指明了參數(shù)最優(yōu)配置的方向。由于不同型號(hào)的激光雷達(dá)的最優(yōu)參數(shù)數(shù)值有所差異，通過采用本文分析方法可大幅節(jié)約確定最優(yōu)參數(shù)的時(shí)間，且最優(yōu)參數(shù)的確立對(duì)提高算法的性能具有重要意義。本文同時(shí)指出，Hector SLAM適用于對(duì)地圖要求較高的場合，Gmapping易用性更好。

參考文獻(xiàn):

[1] 吳明軒,神文文,姜忠民,等.室內(nèi)自主移動(dòng)機(jī)器人系統(tǒng)設(shè)計(jì)[J].機(jī)械科學(xué)與技術(shù),2013,32(6):44-48.

[2] 毛彬彬.基于激光雷達(dá)的移動(dòng)機(jī)器人同時(shí)定位與地圖創(chuàng)建的研究[J].機(jī)械設(shè)計(jì)與制造,2009(2):197-199.

[3] 高 雅,李曉娟,關(guān) 永,等.運(yùn)用定理證明器ACL2驗(yàn)證機(jī)器人操作系統(tǒng)ROS節(jié)點(diǎn)間通信[J].小型微型計(jì)算機(jī)系統(tǒng),2014,35(9):2126-2130.

[4] Kamarudin K,Mamduh S M,Shakaff A Y,et al.Performance analysis of the Microsoft Kinect sensor for 2D simultaneous localization and mapping(SLAM)techniques[J].Sensors,2014,14(12):23365-87.

[5] Kamarudin K,Mamduh S M,Shakaff A Y M,et al.Method to convert Kinect’s 3D depth data to a 2D map for indoor SLAM[C]∥IEEE International Colloquium on Signal Processing and ITS Applications,IEEE,2013:247-251.

[6] Murphy K,Russell S.Rao-Blackwellised particle filtering for dynamic Bayesian networks[C]∥Conference on Uncertainty in Artificial Intelligence,Morgan Kaufmann Publishers Inc, 2013:400.

[7] 厲茂海,洪炳熔,羅榮華.用改進(jìn)的Rao-Blackwellized粒子濾波器實(shí)現(xiàn)移動(dòng)機(jī)器人同時(shí)定位和地圖創(chuàng)建[J].吉林大學(xué)學(xué)報(bào)：工學(xué)版,2007,37(2):401-406.

[8] Kohlbrecher S,Von Stryk O,Meyer J,et al.A flexible and scalable SLAM system with full 3D motion estimation[C]∥IEEE International Symposium on Safety,Security,and Rescue Robotics,IEEE,2011:155-160.

[9] Kamarudin K,Mamduh S M,Shakaff A Y,et al.Performance analysis of the Microsoft Kinect sensor for 2D simultaneous localization and mapping(SLAM)techniques[J].Sensors,2014,14(12):23365-23387.