宮婉婷，孫作雷，潘 盼

(上海海事大學(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)格地圖)通過改變內(nèi)部掃描配準(zhǔn)算法，實(shí)現(xiàn)錄制數(shù)據(jù)(原始2D掃描數(shù)據(jù))經(jīng)過激光配準(zhǔn)算法后生成相對運(yùn)動文件作為tf，用以提高地圖生成和激光掃描配準(zhǔn)的精度。實(shí)驗(yàn)基于機(jī)器人操作系統(tǒng)(ROS)，開源的GMapping軟件包進(jìn)行同時(shí)定位和地圖構(gòu)建(SLAM)，通過RVIZ(用于ROS的3D可視化工具)控制機(jī)器人運(yùn)動。該方案可應(yī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)航等,建立地圖是移動機(jī)器人的重要任務(wù)。學(xué)術(shù)范圍中，移動機(jī)器人構(gòu)圖是同時(shí)定位與地圖構(gòu)建(SLAM)的重要組成部分。隨著工業(yè)、商業(yè)、醫(yī)療以及軍事和民用領(lǐng)域?qū)θ斯ぶ悄芘c自動化的日益增長的技術(shù)需求，機(jī)器人導(dǎo)航相關(guān)的理論與應(yīng)用已成為研究重點(diǎn)。獲取機(jī)器人導(dǎo)航中里程數(shù)據(jù)對構(gòu)圖是至關(guān)重要的，可由編碼器獲取，或基于CMOS的圖像處理方法和使用多傳感數(shù)據(jù)融合來獲得更精確的里程數(shù)據(jù)方法，還可以用單個(gè)激光掃描測距儀使用GMapping包來進(jìn)行構(gòu)圖和定位。本文中，使用激光掃描儀與光學(xué)增量編碼器進(jìn)行預(yù)映射。由于激光掃描儀可以提供詳盡的數(shù)據(jù)，因此可以進(jìn)行直接數(shù)據(jù)處理。 此外激光數(shù)據(jù)的精度高于超聲波和光學(xué)相機(jī)[1]。市場上已經(jīng)有許多廣泛應(yīng)用于科研領(lǐng)域的2D激光儀，在論著中被廣泛提及。

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

1 SLAM算法

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

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

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

2 GMapping算法

2.1 GMapping算法

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

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

(1)

RBPF的基本原理是首先設(shè)置假設(shè)狀態(tài)，每個(gè)粒子保持通過激光掃描器獲得的測量狀態(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)中的概率比例方程選擇粒子，將觀測值與其自己的地圖進(jìn)行比較，并選擇最可能的粒子，其中每個(gè)粒子濾波器攜帶地圖特征。這種方法為SLAM的主要定位問題提供了無成本的解決方案。GMapping算法避免從里程表或其他控制數(shù)據(jù)定義姿態(tài)進(jìn)行驗(yàn)證控制數(shù)據(jù)。

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

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

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

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

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

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

使用掃描匹配將當(dāng)前掃描的地圖與構(gòu)造的地圖配準(zhǔn)。掃描匹配用于對準(zhǔn)當(dāng)前掃描zt與給定的初始狀態(tài)xt0[6]和地圖pt-1[7]。 從初始狀態(tài)xt開始，地圖pt-1應(yīng)用掃描匹配算法。僅通過算法對xt狀態(tài)的有限區(qū)域進(jìn)行對準(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è)階段，使用歸一化器來計(jì)算權(quán)重:

(5)

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

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

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

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

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

3.1 ICP算法

目前有開放許多配準(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ī)器人相對閉合解估計(jì)算法，求出相對運(yùn)動估計(jì)RTnn。

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

3.2 Libpointmatcher

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

為了弱化這些因素對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庫為配準(zhǔn)算法提供了理想的解決方案[11]。

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

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

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

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

當(dāng)以預(yù)定路線手動控制運(yùn)行機(jī)器人時(shí)獲取實(shí)驗(yàn)數(shù)據(jù)是有效的，例如識別傳感器、輪子或者其他拓展等誤差。手動導(dǎo)航是使用基于Android的應(yīng)用程序，通過藍(lán)牙與Arduino控制板通信。 藍(lán)牙模塊(HC-06)用于實(shí)現(xiàn)Android應(yīng)用和控制板之間的通信。 Android手機(jī)是用來進(jìn)行控制的。用于構(gòu)圖過程的控制方法不同于基于藍(lán)牙模塊的手動控制方法。 構(gòu)圖過程中在ROS中使用rqt控制臺[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)話題訂閱

啟動hukuyo_node節(jié)點(diǎn)保持機(jī)器人與計(jì)算機(jī)的連接，通過base_laser與base_link之間靜態(tài)tf變換來安裝和確定激光測距儀的位置[13]；發(fā)布并廣播odom變換,使用編寫的odom_tf，將里程計(jì)信息發(fā)布并廣播，發(fā)布/pose_topic獲得的里程計(jì)位姿?？刂菩≤囘\(yùn)動，啟動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)程操作以記錄和探測它的幾個(gè)ROS bag，然后啟動GMapping以從不同參數(shù)的記錄bag來分析它們對性能的影響，可以找到有關(guān)在記錄bag時(shí)使用的步驟快速參考[14]。

ROS bag數(shù)據(jù)包經(jīng)過轉(zhuǎn)換后成為過程數(shù)據(jù)。過程數(shù)據(jù)都是.csv格式，第一行為標(biāo)題，這種格式可被許多語言和軟件(包括Python、MATLAB)輕松解析。點(diǎn)云在局部坐標(biāo)和全局坐標(biāo)中可用。所提供的真實(shí)地面姿態(tài)直接給出原點(diǎn)到給定掃描基礎(chǔ)的變換[10]。場景環(huán)境為地面實(shí)況信息，過程數(shù)據(jù)使用Libpointmatcher進(jìn)行ICP匹配，求出read cloud 與reference cloud相對變換關(guān)系，最終輸出為.vtk格式的點(diǎn)云匹配關(guān)系。將過匹配的對應(yīng)關(guān)系作為GMapping的訂閱tf話題進(jìn)行廣播，實(shí)現(xiàn)掃描數(shù)據(jù)的相對變換。將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平臺上根據(jù)遙感操作桿方式錄制的bag轉(zhuǎn)換點(diǎn)集經(jīng)過本文的方法進(jìn)行GMapping，建立的圖形如圖4所示。

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

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

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

5 結(jié)論

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

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

GMapping算法處理包括固定和非固定地標(biāo)。 在構(gòu)圖過程中，檢測到的非靜態(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平臺上的地圖顯示以及環(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)線激光路面車轍檢測技術(shù)研究[D].長安大學(xué),2013.

[3] 呂強(qiáng),林輝燦,張洋,等.基于視覺的多機(jī)器人協(xié)作 SLAM 問題[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)境中的移動機(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-)，男，博士，副教授，主要研究方向：移動機(jī)器人導(dǎo)航，機(jī)器學(xué)習(xí)算法等。

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