宮婉婷，孫作雷，潘 盼

(上海海事大學(xué) 信息工程學(xué)院, 上海 201306)

基于二維激光的機(jī)器人室內(nèi)構(gòu)圖

宮婉婷，孫作雷，潘 盼

(上海海事大學(xué) 信息工程學(xué)院, 上海 201306)

建立了2D激光掃描匹配數(shù)據(jù)的構(gòu)圖方案，使用GMapping(一種高效的Rao-Blackwellized粒子濾波器，從激光數(shù)據(jù)學(xué)習(xí)網(wǎng)格地圖)通過(guò)改變內(nèi)部掃描配準(zhǔn)算法，實(shí)現(xiàn)錄制數(shù)據(jù)(原始2D掃描數(shù)據(jù))經(jīng)過(guò)激光配準(zhǔn)算法后生成相對(duì)運(yùn)動(dòng)文件作為tf，用以提高地圖生成和激光掃描配準(zhǔn)的精度。實(shí)驗(yàn)基于機(jī)器人操作系統(tǒng)(ROS)，開(kāi)源的GMapping軟件包進(jìn)行同時(shí)定位和地圖構(gòu)建(SLAM)，通過(guò)RVIZ(用于ROS的3D可視化工具)控制機(jī)器人運(yùn)動(dòng)。該方案可應(yīng)用于移動(dòng)機(jī)器人的導(dǎo)航、構(gòu)圖與2D重建、同步定位與地圖構(gòu)建(SLAM)等領(lǐng)域。

ROS；SLAM；激光匹配；SLAM-GMapping；tf

Abstract: This paper establishes the composition scheme of 2D laser scanning matching data. Using GMapping (an efficient Rao-Blackwellized particle filter, from the laser data to learn the grid map) by changing the internal scan registration algorithm, it achieves the recording data (raw 2D scan data) through the laser registration algorithm to generate a relative motion file as tf, to improve the accuracy of map generation and laser scanning registration. The experiment is based on the robot operating system (ROS), the open source GMapping package isused for simultaneous localization and mapping (SLAM), and robot movement is controlled through RVIZ (3D visualization tool for ROS). This scheme can be applied to the research of mobile robot navigation, composition and 2D reconstruction, simultaneous localization and mapping (SLAM).

Key words:ROS; SLAM; laser matching; SLAM-GMapping; tf

0 引言

機(jī)器人的應(yīng)用方向主要是自身定位、地圖構(gòu)建以及自主導(dǎo)航等,建立地圖是移動(dòng)機(jī)器人的重要任務(wù)。學(xué)術(shù)范圍中，移動(dòng)機(jī)器人構(gòu)圖是同時(shí)定位與地圖構(gòu)建(SLAM)的重要組成部分。隨著工業(yè)、商業(yè)、醫(yī)療以及軍事和民用領(lǐng)域?qū)θ斯ぶ悄芘c自動(dòng)化的日益增長(zhǎng)的技術(shù)需求，機(jī)器人導(dǎo)航相關(guān)的理論與應(yīng)用已成為研究重點(diǎn)。獲取機(jī)器人導(dǎo)航中里程數(shù)據(jù)對(duì)構(gòu)圖是至關(guān)重要的，可由編碼器獲取，或基于CMOS的圖像處理方法和使用多傳感數(shù)據(jù)融合來(lái)獲得更精確的里程數(shù)據(jù)方法，還可以用單個(gè)激光掃描測(cè)距儀使用GMapping包來(lái)進(jìn)行構(gòu)圖和定位。本文中，使用激光掃描儀與光學(xué)增量編碼器進(jìn)行預(yù)映射。由于激光掃描儀可以提供詳盡的數(shù)據(jù)，因此可以進(jìn)行直接數(shù)據(jù)處理。 此外激光數(shù)據(jù)的精度高于超聲波和光學(xué)相機(jī)[1]。市場(chǎng)上已經(jīng)有許多廣泛應(yīng)用于科研領(lǐng)域的2D激光儀，在論著中被廣泛提及。

目前導(dǎo)航技術(shù)中，激光導(dǎo)航應(yīng)用得最為廣泛。激光導(dǎo)航有以下幾點(diǎn)優(yōu)勢(shì)：(1)激光導(dǎo)航應(yīng)用廣泛，不易受環(huán)境干擾，具有很強(qiáng)的穩(wěn)定性；(2)激光測(cè)距儀測(cè)量距離遠(yuǎn)、測(cè)量范圍廣、測(cè)量精度高、測(cè)量準(zhǔn)確性強(qiáng)；(3)激光導(dǎo)航成本相對(duì)較低，且掃描頻率高，可以獲得大量有效點(diǎn)集。因此本文使用激光測(cè)距儀Hokuyo進(jìn)行數(shù)據(jù)采集。本文對(duì)傳感器數(shù)據(jù)應(yīng)用匹配算法，進(jìn)而優(yōu)化GMapping構(gòu)圖，實(shí)現(xiàn)SLAM。

1 SLAM算法

SLAM是同步定位與地圖構(gòu)建。 在構(gòu)圖過(guò)程中，機(jī)器人位置是已知的，因此相對(duì)于機(jī)器人位置檢測(cè)標(biāo)記位置。在定位中，相對(duì)于標(biāo)記位置檢測(cè)機(jī)器人的位置。 因此，很明顯，在環(huán)境構(gòu)圖中，構(gòu)圖和定位彼此耦合[2]。

在構(gòu)圖過(guò)程中機(jī)器人位姿Xv(k)已知，因此需要知道標(biāo)定a的位置，測(cè)量z(k)來(lái)構(gòu)建地圖p。SLAM算法是將定位與構(gòu)圖問(wèn)題[3]同時(shí)考慮的方法。如圖1所示已知機(jī)器人位姿為xv(k)和標(biāo)記位置為p(k)，需要知道測(cè)量值z(mì)(k)。

圖1 SLAM算法的參數(shù)

2 GMapping算法

2.1 GMapping算法

本文中，使用基于GMapping算法和掃描匹配算法的網(wǎng)格地圖來(lái)進(jìn)行地圖構(gòu)建。為了從激光掃描數(shù)據(jù)中學(xué)習(xí)網(wǎng)格地圖，GMapping算法使用Rao-Blackwellized粒子濾波器[4](RBPF), 在RBPF中，地圖數(shù)據(jù)間的聯(lián)合后驗(yàn)pt和使用激光掃描測(cè)量數(shù)據(jù)Z1:t=z1…zt估測(cè)機(jī)器人的狀態(tài)x1:t=x1…xt[5]。

p(x1:t,pz1:t,u1:t-1)

(1)

RBPF的基本原理是首先設(shè)置假設(shè)狀態(tài)，每個(gè)粒子保持通過(guò)激光掃描器獲得的測(cè)量狀態(tài)，并與相應(yīng)的粒子相關(guān)聯(lián)標(biāo)記。最強(qiáng)的假設(shè)持續(xù)給定權(quán)重，并且重采樣[5]后將弱假設(shè)丟棄。粒子狀態(tài)可以被定義為機(jī)器人的潛在位姿，并且每個(gè)狀態(tài)表示后驗(yàn)。

p(x1:t,pz1:t,u1:t-1)=p(x1:tz1:t,u0:t-1)×p(pz1:t,u1:t)

(2)

使用式(2)中的概率比例方程選擇粒子，將觀測(cè)值與其自己的地圖進(jìn)行比較，并選擇最可能的粒子，其中每個(gè)粒子濾波器攜帶地圖特征。這種方法為SLAM的主要定位問(wèn)題提供了無(wú)成本的解決方案。GMapping算法避免從里程表或其他控制數(shù)據(jù)定義姿態(tài)進(jìn)行驗(yàn)證控制數(shù)據(jù)。

圖2中，xt指車輛和地標(biāo)的狀態(tài)，zt代表測(cè)量值，pt代表獲得的地圖?？梢钥闯鲈谶@個(gè)流程圖中沒(méi)有使用控制數(shù)據(jù)， 因此，表明GMapping算法沒(méi)有控制數(shù)據(jù)。

圖2 無(wú)控制數(shù)據(jù)的GMapping

算法可以表示為以下幾步：

(1)初始狀態(tài)假設(shè)。假設(shè)機(jī)器人的姿勢(shì)從先前的姿勢(shì)獲得，即激光掃描器獲得的測(cè)量值z(mì)t-1。

(2)掃描匹配算法從初始狀態(tài)假設(shè)獲取地圖pt-1。

(3)更新粒子點(diǎn)?；跍y(cè)量值z(mì)t更新粒子點(diǎn)，地圖pt的粒子基于狀態(tài)量xt與測(cè)量值z(mì)t更新。

使用掃描匹配將當(dāng)前掃描的地圖與構(gòu)造的地圖配準(zhǔn)。掃描匹配用于對(duì)準(zhǔn)當(dāng)前掃描zt與給定的初始狀態(tài)xt0[6]和地圖pt-1[7]。 從初始狀態(tài)xt開(kāi)始，地圖pt-1應(yīng)用掃描匹配算法。僅通過(guò)算法對(duì)xt狀態(tài)的有限區(qū)域進(jìn)行對(duì)準(zhǔn)。

(3)

掃描匹配選擇具有高假設(shè)的粒子狀態(tài)xt,根據(jù)式(3)的結(jié)果計(jì)算下一個(gè)狀態(tài)。

p(ztpt-1,xj)→p(xjxt-1,ut-1)

(4)

在這個(gè)階段，使用歸一化器來(lái)計(jì)算權(quán)重:

(5)

最后根據(jù)狀態(tài)值xt與測(cè)量值z(mì)t更新地圖pt的粒子t[6]。

2.2使用SLAM- GMapping構(gòu)建地圖

機(jī)器人操作系統(tǒng)(ROS)中SLAM-GMapping包是對(duì)SLAM問(wèn)題開(kāi)源實(shí)現(xiàn)的一個(gè)封裝，ROS包包含名為SLAM-GMapping的可執(zhí)行節(jié)點(diǎn)，是SLAM 算法的實(shí)現(xiàn)，可以通過(guò)激光掃描數(shù)據(jù)和移動(dòng)機(jī)器人姿勢(shì)創(chuàng)建2D占用網(wǎng)格圖[4]。

SLAM-GMapping節(jié)點(diǎn)訂閱激光數(shù)據(jù)scan(sensor_msgs/LaserScan)和傳輸數(shù)據(jù)( tf/tfMessage)，發(fā)布占用網(wǎng)格地圖作為輸出?？梢酝ㄟ^(guò)運(yùn)行map_sever保存構(gòu)建的地圖，地圖被存儲(chǔ)在YAML文件和圖像文件中。YAML文件描述地圖元數(shù)據(jù)，并命名圖像文件對(duì)占用數(shù)據(jù)進(jìn)行編碼。

3 點(diǎn)集配準(zhǔn)算法

3.1 ICP算法

目前有開(kāi)放許多配準(zhǔn)解決方案，例如，迭代最近點(diǎn)算法(ICP)被認(rèn)為是許多基于激光定位和構(gòu)圖系統(tǒng)的主干。ICP算法流程如下[8]：

(1)判斷E←0，重復(fù)步驟(2)～(6)直到滿足終止條件：n=0或E=0。

(2)將Lnew投影到世界坐標(biāo)系w下。

(6)

(7)

(3)外層循環(huán)i從1執(zhí)行到N。

(8)

(5)求得參考激光束與新激光束之間的激光點(diǎn)關(guān)聯(lián)配置：

(9)

將Cnn，Lref，Lnew代入機(jī)器人相對(duì)閉合解估計(jì)算法，求出相對(duì)運(yùn)動(dòng)估計(jì)RTnn。

輸出：參考激光束與新激光束所有激光點(diǎn)間的關(guān)聯(lián)配置CICP[9]，以及對(duì)相對(duì)運(yùn)動(dòng)的估計(jì)RTICP。

3.2 Libpointmatcher

由于ICP算法的本質(zhì)是局部收斂近似算法，結(jié)果取決于初始姿態(tài)，需要提供初始假設(shè)，初始假設(shè)通常由慣性測(cè)量、里程計(jì)或啟發(fā)式運(yùn)動(dòng)模型提供，這些都存在精度有限和觀測(cè)中隨時(shí)間增加的不確定度問(wèn)題。另外激光掃描匹配的一個(gè)難點(diǎn)是大量的離群點(diǎn)，有大量的不能反映掃描特征的點(diǎn)。而且掃描的內(nèi)容也將對(duì)配準(zhǔn)產(chǎn)生很大的影響，室內(nèi)環(huán)境中通常是一些平面(例如：地面、墻體、天花板等)。

為了弱化這些因素對(duì)ICP算法性能的影響，本文中使用ETH的Libpointmatcher[10]進(jìn)行實(shí)驗(yàn)點(diǎn)集的匹配。Libpointmatcher是模塊化ICP鏈，其中l(wèi)ibnabo1基于kd-tree優(yōu)化實(shí)現(xiàn)最近鄰搜索，它是ICP最快的kd-tree之一。由于模塊化和快速性，Libpointmatcher庫(kù)為配準(zhǔn)算法提供了理想的解決方案[11]。

4 實(shí)驗(yàn)部分

4.1 ROS實(shí)驗(yàn)平臺(tái)

該系統(tǒng)在通過(guò)WiFi(分布式系統(tǒng))連接的兩個(gè)Linux機(jī)器上實(shí)現(xiàn)。具體地，運(yùn)行Ubuntu的PC(Core-i5,2.53 GHz CPU，6.00 GB RAM)，基于ROS在RVIZ上的機(jī)器人遠(yuǎn)程操作，進(jìn)行地圖可視化。

然后，配備Hokuyo URG-04LX-UG01激光測(cè)距儀并由Raspberry Pi 2 B型控制的Hercules 4WD移動(dòng)平臺(tái)構(gòu)成低成本移動(dòng)機(jī)器人。應(yīng)用了激光傳感器Hokuyo，在ROS環(huán)境下獲取多個(gè)環(huán)境點(diǎn)集，使用rosbag來(lái)錄制數(shù)據(jù)。實(shí)際場(chǎng)景圖主要環(huán)境為辦公區(qū)域、走廊、工位隔斷之間的空隙以及花盆等靜態(tài)標(biāo)志物。

4.2機(jī)器人控制過(guò)程

當(dāng)以預(yù)定路線手動(dòng)控制運(yùn)行機(jī)器人時(shí)獲取實(shí)驗(yàn)數(shù)據(jù)是有效的，例如識(shí)別傳感器、輪子或者其他拓展等誤差。手動(dòng)導(dǎo)航是使用基于Android的應(yīng)用程序，通過(guò)藍(lán)牙與Arduino控制板通信。 藍(lán)牙模塊(HC-06)用于實(shí)現(xiàn)Android應(yīng)用和控制板之間的通信。 Android手機(jī)是用來(lái)進(jìn)行控制的。用于構(gòu)圖過(guò)程的控制方法不同于基于藍(lán)牙模塊的手動(dòng)控制方法。 構(gòu)圖過(guò)程中在ROS中使用rqt控制臺(tái)[12]并給出了一個(gè)速度命令。 這個(gè)速度命令與機(jī)器人在導(dǎo)航時(shí)獲取的自主構(gòu)建環(huán)境中的命令相同。 為此獲得了電機(jī)速度與電機(jī)轉(zhuǎn)速的關(guān)系輸入PWM到電機(jī)：

v=0.003 2p-0.404 5

(10)

式中v是機(jī)器人速度，p是PWM。上述關(guān)系假設(shè)是線性關(guān)系，雖然不是完全線性，由于電機(jī)的PWM控制，PWM中的一點(diǎn)變化可以忽略。

4.3 ROS節(jié)點(diǎn)話題訂閱

啟動(dòng)hukuyo_node節(jié)點(diǎn)保持機(jī)器人與計(jì)算機(jī)的連接，通過(guò)base_laser與base_link之間靜態(tài)tf變換來(lái)安裝和確定激光測(cè)距儀的位置[13]；發(fā)布并廣播odom變換,使用編寫的odom_tf，將里程計(jì)信息發(fā)布并廣播，發(fā)布/pose_topic獲得的里程計(jì)位姿?？刂菩≤囘\(yùn)動(dòng)，啟動(dòng)GMapping;運(yùn)行/map與/odom 之間的坐標(biāo)變換，然后使用RVIZ進(jìn)行地圖顯示，設(shè)置LaserScan的topic為/laser， Map的topic為/map用以顯示構(gòu)造環(huán)境圖。

4.4優(yōu)化方法

為了優(yōu)化GMapping的性能，在建筑走廊構(gòu)圖環(huán)境進(jìn)行試驗(yàn)，機(jī)器人遠(yuǎn)程操作以記錄和探測(cè)它的幾個(gè)ROS bag，然后啟動(dòng)GMapping以從不同參數(shù)的記錄bag來(lái)分析它們對(duì)性能的影響，可以找到有關(guān)在記錄bag時(shí)使用的步驟快速參考[14]。

ROS bag數(shù)據(jù)包經(jīng)過(guò)轉(zhuǎn)換后成為過(guò)程數(shù)據(jù)。過(guò)程數(shù)據(jù)都是.csv格式，第一行為標(biāo)題，這種格式可被許多語(yǔ)言和軟件(包括Python、MATLAB)輕松解析。點(diǎn)云在局部坐標(biāo)和全局坐標(biāo)中可用。所提供的真實(shí)地面姿態(tài)直接給出原點(diǎn)到給定掃描基礎(chǔ)的變換[10]。場(chǎng)景環(huán)境為地面實(shí)況信息，過(guò)程數(shù)據(jù)使用Libpointmatcher進(jìn)行ICP匹配，求出read cloud 與reference cloud相對(duì)變換關(guān)系，最終輸出為.vtk格式的點(diǎn)云匹配關(guān)系。將過(guò)匹配的對(duì)應(yīng)關(guān)系作為GMapping的訂閱tf話題進(jìn)行廣播，實(shí)現(xiàn)掃描數(shù)據(jù)的相對(duì)變換。將tf話題與原始scan信息進(jìn)行節(jié)點(diǎn)訂閱與廣播，進(jìn)行構(gòu)圖。

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

運(yùn)用GMapping錄制bag包[9]進(jìn)行環(huán)境地圖構(gòu)建，直接錄制的地圖實(shí)況如圖3所示。將ROS平臺(tái)上根據(jù)遙感操作桿方式錄制的bag轉(zhuǎn)換點(diǎn)集經(jīng)過(guò)本文的方法進(jìn)行GMapping，建立的圖形如圖4所示。

圖3 錄制數(shù)據(jù)包構(gòu)圖

可以看出相同環(huán)境下，相較于圖3，圖4的邊界線條更加清晰，且通過(guò)多個(gè)數(shù)據(jù)包驗(yàn)證，說(shuō)明優(yōu)化后的GMapping對(duì)于障礙物輪廓匹配正確率更高。因?yàn)長(zhǎng)ibpointmatcher封裝的ICP算法使用多重濾波方法，使匹配對(duì)應(yīng)關(guān)系更加準(zhǔn)確，因此構(gòu)圖效果更佳。

圖4 優(yōu)化gmapping后的構(gòu)圖

5 結(jié)論

ROS有許多開(kāi)源庫(kù)和軟件包，支持傳感器融合和其他必要的框架。通過(guò)使用ROS，可以消除復(fù)雜算法(SLAM，

AMCL)的硬編碼，ROS的RVIZ接口可以用來(lái)觀測(cè)構(gòu)圖與導(dǎo)航過(guò)程。

GMapping算法處理包括固定和非固定地標(biāo)。 在構(gòu)圖過(guò)程中，檢測(cè)到的非靜態(tài)地標(biāo)將從所創(chuàng)建的地圖中移除，只有靜止地標(biāo)將被添加到狀態(tài)矢量，本文中使用的都是靜止地標(biāo)。原始GMapping構(gòu)圖將錄制數(shù)據(jù)包數(shù)據(jù)進(jìn)行匹配優(yōu)化，或者優(yōu)化GMapping參數(shù)達(dá)到優(yōu)化構(gòu)圖的目的。將2D激光掃描數(shù)據(jù)準(zhǔn)換成GMapping訂閱的tf話題，實(shí)現(xiàn)2D激光掃描數(shù)據(jù)在ROS平臺(tái)上的地圖顯示以及環(huán)境構(gòu)建。該算法為低成本小型機(jī)器人導(dǎo)航構(gòu)圖提供了方法，可以用于機(jī)器人同步定位與地圖構(gòu)建的算法驗(yàn)證工作。

[1] DISSANAYAKE M, NEWMAN P, CLARK S, et al. A solution to the simultaneous localization and map building(slam) problem[J]. IEEE Transactions on Robotics and Automation, 2001,17(4): 229-241.

[2] 趙康.基準(zhǔn)點(diǎn)線激光路面車轍檢測(cè)技術(shù)研究[D].長(zhǎng)安大學(xué),2013.

[3] 呂強(qiáng),林輝燦,張洋,等.基于視覺(jué)的多機(jī)器人協(xié)作 SLAM 問(wèn)題[J]. 科技導(dǎo)報(bào), 2015, 33(23): 110-115.

[4] GRISETTI G, STACHNISS C, BURGARD W. Improved techniques for grid mapping with rao-blackwellized particle filters[J].IEEE Transactions on Robotics, 2007, 23(1): 34-46.

[5] THRUN S, BURGARD W, FOX D. Probabilistic robotics(1st ed)[M]. The MIT Press, 2005.

[6] CENSI A, IOCCHI L, GRISETTI G. Scan matching in the hough domain[C]. Proceediags of the 2005 IEEE International Conference on Robotics and Automation,ICRA 2005, 2005,3(1): 2739-2744.

[7] LU F, MILIOS E. Globally consistent range scans alignment for environment mapping[J]. Journal of Autonomous Robots, 1997,4(4):333-349.

[8] 孫作雷.大規(guī)模不規(guī)則環(huán)境中的移動(dòng)機(jī)器人定位與地圖構(gòu)圖[D].上海:上海海事大學(xué)，2010.

[9] How to build a map using logged data[EB/OL]. [2017-02-27]http://wiki.ros.org.slam_gmapping/Tutorials/MappingFromLoggedData.

[10] RUSINKIEWICZ S, LEVOY M. Efficient variants of the ICP algorithm[C]. Proceedings of Third International Conference on 3-D Digital Imaging and Modeling, IEEE, 2001: 145-152.

[12] ROS TUTORIALS[EB/OL].[2017-02-27]http://wiki.ros.org/ROS/Tutorials.

[13] COLAS F, SIEGWART R. A review of point Cloud registration algorithms for mobile robotics[M]. Now Publisher Inc., 2005.

Indoor mapping of robot based on 2D laser

Gong Wanting, Sun Zuolei, Pan Pan

(College of Information Engineering, Shanghai Maritime University, Shanghai 201306, China)

TP242.3

A

10.19358/j.issn.1674- 7720.2017.18.017

宮婉婷，孫作雷，潘盼.基于二維激光的機(jī)器人室內(nèi)構(gòu)圖[J].微型機(jī)與應(yīng)用，2017,36(18)：55-58,62.

2017-02-27)

宮婉婷(1993-)，女，碩士研究生，主要研究方向：機(jī)器人自主導(dǎo)航。

孫作雷(1982-)，男，博士，副教授，主要研究方向：移動(dòng)機(jī)器人導(dǎo)航，機(jī)器學(xué)習(xí)算法等。

潘盼(1989-)，男，碩士研究生，主要研究方向：移動(dòng)機(jī)器人導(dǎo)航。