基于二維激光的機器人室內構圖

宮婉婷，孫作雷，潘 盼

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

基于二維激光的機器人室內構圖

宮婉婷，孫作雷，潘 盼

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

建立了2D激光掃描匹配數據的構圖方案，使用GMapping(一種高效的Rao-Blackwellized粒子濾波器，從激光數據學習網格地圖)通過改變內部掃描配準算法，實現錄制數據(原始2D掃描數據)經過激光配準算法后生成相對運動文件作為tf，用以提高地圖生成和激光掃描配準的精度。實驗基于機器人操作系統(ROS)，開源的GMapping軟件包進行同時定位和地圖構建(SLAM)，通過RVIZ(用于ROS的3D可視化工具)控制機器人運動。該方案可應用于移動機器人的導航、構圖與2D重建、同步定位與地圖構建(SLAM)等領域。

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 引言

機器人的應用方向主要是自身定位、地圖構建以及自主導航等,建立地圖是移動機器人的重要任務。學術范圍中，移動機器人構圖是同時定位與地圖構建(SLAM)的重要組成部分。隨著工業、商業、醫療以及軍事和民用領域對人工智能與自動化的日益增長的技術需求，機器人導航相關的理論與應用已成為研究重點。獲取機器人導航中里程數據對構圖是至關重要的，可由編碼器獲取，或基于CMOS的圖像處理方法和使用多傳感數據融合來獲得更精確的里程數據方法，還可以用單個激光掃描測距儀使用GMapping包來進行構圖和定位。本文中，使用激光掃描儀與光學增量編碼器進行預映射。由于激光掃描儀可以提供詳盡的數據，因此可以進行直接數據處理。 此外激光數據的精度高于超聲波和光學相機[1]。市場上已經有許多廣泛應用于科研領域的2D激光儀，在論著中被廣泛提及。

目前導航技術中，激光導航應用得最為廣泛。激光導航有以下幾點優勢：(1)激光導航應用廣泛，不易受環境干擾，具有很強的穩定性；(2)激光測距儀測量距離遠、測量范圍廣、測量精度高、測量準確性強；(3)激光導航成本相對較低，且掃描頻率高，可以獲得大量有效點集。因此本文使用激光測距儀Hokuyo進行數據采集。本文對傳感器數據應用匹配算法，進而優化GMapping構圖，實現SLAM。

1 SLAM算法

SLAM是同步定位與地圖構建。 在構圖過程中，機器人位置是已知的，因此相對于機器人位置檢測標記位置。在定位中，相對于標記位置檢測機器人的位置。 因此，很明顯，在環境構圖中，構圖和定位彼此耦合[2]。

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

圖1 SLAM算法的參數

2 GMapping算法

2.1 GMapping算法

本文中，使用基于GMapping算法和掃描匹配算法的網格地圖來進行地圖構建。為了從激光掃描數據中學習網格地圖，GMapping算法使用Rao-Blackwellized粒子濾波器[4](RBPF), 在RBPF中，地圖數據間的聯合后驗pt和使用激光掃描測量數據Z1:t=z1…zt估測機器人的狀態x1:t=x1…xt[5]。

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

(1)

RBPF的基本原理是首先設置假設狀態，每個粒子保持通過激光掃描器獲得的測量狀態，并與相應的粒子相關聯標記。最強的假設持續給定權重，并且重采樣[5]后將弱假設丟棄。粒子狀態可以被定義為機器人的潛在位姿，并且每個狀態表示后驗。

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

(2)

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

圖2中，xt指車輛和地標的狀態，zt代表測量值，pt代表獲得的地圖。可以看出在這個流程圖中沒有使用控制數據， 因此，表明GMapping算法沒有控制數據。

圖2 無控制數據的GMapping

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

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

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

(3)更新粒子點。基于測量值zt更新粒子點，地圖pt的粒子基于狀態量xt與測量值zt更新。

使用掃描匹配將當前掃描的地圖與構造的地圖配準。掃描匹配用于對準當前掃描zt與給定的初始狀態xt0[6]和地圖pt-1[7]。 從初始狀態xt開始，地圖pt-1應用掃描匹配算法。僅通過算法對xt狀態的有限區域進行對準。

(3)

掃描匹配選擇具有高假設的粒子狀態xt,根據式(3)的結果計算下一個狀態。

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

(4)

在這個階段，使用歸一化器來計算權重:

(5)

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

2.2使用SLAM- GMapping構建地圖

機器人操作系統(ROS)中SLAM-GMapping包是對SLAM問題開源實現的一個封裝，ROS包包含名為SLAM-GMapping的可執行節點，是SLAM 算法的實現，可以通過激光掃描數據和移動機器人姿勢創建2D占用網格圖[4]。

SLAM-GMapping節點訂閱激光數據scan(sensor_msgs/LaserScan)和傳輸數據( tf/tfMessage)，發布占用網格地圖作為輸出。可以通過運行map_sever保存構建的地圖，地圖被存儲在YAML文件和圖像文件中。YAML文件描述地圖元數據，并命名圖像文件對占用數據進行編碼。

3 點集配準算法

3.1 ICP算法

目前有開放許多配準解決方案，例如，迭代最近點算法(ICP)被認為是許多基于激光定位和構圖系統的主干。ICP算法流程如下[8]：

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

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

(6)

(7)

(3)外層循環i從1執行到N。

(8)

(5)求得參考激光束與新激光束之間的激光點關聯配置：

(9)

將Cnn，Lref，Lnew代入機器人相對閉合解估計算法，求出相對運動估計RTnn。

輸出：參考激光束與新激光束所有激光點間的關聯配置CICP[9]，以及對相對運動的估計RTICP。

3.2 Libpointmatcher

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

為了弱化這些因素對ICP算法性能的影響，本文中使用ETH的Libpointmatcher[10]進行實驗點集的匹配。Libpointmatcher是模塊化ICP鏈，其中libnabo1基于kd-tree優化實現最近鄰搜索，它是ICP最快的kd-tree之一。由于模塊化和快速性，Libpointmatcher庫為配準算法提供了理想的解決方案[11]。

4 實驗部分

4.1 ROS實驗平臺

該系統在通過WiFi(分布式系統)連接的兩個Linux機器上實現。具體地，運行Ubuntu的PC(Core-i5,2.53 GHz CPU，6.00 GB RAM)，基于ROS在RVIZ上的機器人遠程操作，進行地圖可視化。

然后，配備Hokuyo URG-04LX-UG01激光測距儀并由Raspberry Pi 2 B型控制的Hercules 4WD移動平臺構成低成本移動機器人。應用了激光傳感器Hokuyo，在ROS環境下獲取多個環境點集，使用rosbag來錄制數據。實際場景圖主要環境為辦公區域、走廊、工位隔斷之間的空隙以及花盆等靜態標志物。

4.2機器人控制過程

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

v=0.003 2p-0.404 5

(10)

式中v是機器人速度，p是PWM。上述關系假設是線性關系，雖然不是完全線性，由于電機的PWM控制，PWM中的一點變化可以忽略。

4.3 ROS節點話題訂閱

啟動hukuyo_node節點保持機器人與計算機的連接，通過base_laser與base_link之間靜態tf變換來安裝和確定激光測距儀的位置[13]；發布并廣播odom變換,使用編寫的odom_tf，將里程計信息發布并廣播，發布/pose_topic獲得的里程計位姿。控制小車運動，啟動GMapping;運行/map與/odom 之間的坐標變換，然后使用RVIZ進行地圖顯示，設置LaserScan的topic為/laser， Map的topic為/map用以顯示構造環境圖。

4.4優化方法

為了優化GMapping的性能，在建筑走廊構圖環境進行試驗，機器人遠程操作以記錄和探測它的幾個ROS bag，然后啟動GMapping以從不同參數的記錄bag來分析它們對性能的影響，可以找到有關在記錄bag時使用的步驟快速參考[14]。

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

4.5實驗結果

運用GMapping錄制bag包[9]進行環境地圖構建，直接錄制的地圖實況如圖3所示。將ROS平臺上根據遙感操作桿方式錄制的bag轉換點集經過本文的方法進行GMapping，建立的圖形如圖4所示。

圖3 錄制數據包構圖

可以看出相同環境下，相較于圖3，圖4的邊界線條更加清晰，且通過多個數據包驗證，說明優化后的GMapping對于障礙物輪廓匹配正確率更高。因為Libpointmatcher封裝的ICP算法使用多重濾波方法，使匹配對應關系更加準確，因此構圖效果更佳。

圖4 優化gmapping后的構圖

5 結論

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

AMCL)的硬編碼，ROS的RVIZ接口可以用來觀測構圖與導航過程。

GMapping算法處理包括固定和非固定地標。 在構圖過程中，檢測到的非靜態地標將從所創建的地圖中移除，只有靜止地標將被添加到狀態矢量，本文中使用的都是靜止地標。原始GMapping構圖將錄制數據包數據進行匹配優化，或者優化GMapping參數達到優化構圖的目的。將2D激光掃描數據準換成GMapping訂閱的tf話題，實現2D激光掃描數據在ROS平臺上的地圖顯示以及環境構建。該算法為低成本小型機器人導航構圖提供了方法，可以用于機器人同步定位與地圖構建的算法驗證工作。

[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] 趙康.基準點線激光路面車轍檢測技術研究[D].長安大學,2013.

[3] 呂強,林輝燦,張洋,等.基于視覺的多機器人協作 SLAM 問題[J]. 科技導報, 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] 孫作雷.大規模不規則環境中的移動機器人定位與地圖構圖[D].上海:上海海事大學，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].微型機與應用，2017,36(18)：55-58,62.

2017-02-27)

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

孫作雷(1982-)，男，博士，副教授，主要研究方向：移動機器人導航，機器學習算法等。

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