李 森，范平清

（上海工程技術(shù)大學(xué) 機械與汽車工程學(xué)院，上海 201620）

0 引言

隨著科技的發(fā)展，移動機器人越來越便宜，功能也越來越強大。如今，處理能力和功能的提高，允許更多的場合去使用。以往由一個機器人完成的任務(wù)現(xiàn)在可以分配給多個機器人配合完成，這樣不僅提高了工作效率，也可以提高系統(tǒng)的魯棒性。但是，在多機器人協(xié)調(diào)過程中，將多個機器人的局部地圖合成為一張完整的全局地圖，是一個關(guān)鍵性任務(wù)。

針對多機器人同步定位與建圖（SLAM）地圖融合問題的研究，一般可以分為兩個方面：

一是在圖像的基準(zhǔn)上對局部地圖進行合成。如，文獻［5］提出了基于霍夫變換的快速地圖融合方法，通過霍夫變換，并利用兩張地圖霍夫譜的相關(guān)性，找到最佳的旋轉(zhuǎn)和平移變換，進行融合。該方法可以將兩張有250 000個網(wǎng)格的地圖在500ms 內(nèi)合成。文獻［6］中提出利用SIFT 變換來檢測兩張地圖的不變特征，計算兩張地圖的變換。

另一方面是基于數(shù)學(xué)的度量和優(yōu)化。如，文獻［7］提出了一種啟發(fā)式的搜索方法，引導(dǎo)局部地圖的重復(fù)區(qū)域進行地圖融合。以上方法都是對已有的局部地圖進行融合，無法在機器人構(gòu)建局部地圖時，進行實時的地圖融合。而文獻［8］中提到，實時建圖與融合的方案更具應(yīng)用價值，并提出了一種利用云端對不同機器人的局部地圖進行融合的方法。

本文針對實時地圖融合問題，提出一種興趣區(qū)域地圖提取與ROS（Robot Operating System）的多機器人SLAM地圖融合方法。通過局部柵格地圖興趣區(qū)域提取，對局部柵格地圖進行優(yōu)化處理，得到有效區(qū)域的最小矩形柵格地圖，然后利用霍夫變換地圖融合方法并結(jié)合ROS，實現(xiàn)多機器人地圖的快速實時合并。

1 系統(tǒng)設(shè)計與框架

為了提高系統(tǒng)的魯棒性，在設(shè)計時，各個機器人之間采用分布式的控制，并且為了減少單個機器人處理過多的信息，采用集中處理的方式。系統(tǒng)框架基于ROS 進行搭建，整體框架如圖1 所示。具體實現(xiàn)步驟如下：

圖1 整體框架Fig.1 Overall framework

單個機器人通過Gmapping 算法構(gòu)建局部地圖，并將地圖數(shù)據(jù)傳送到地圖預(yù)處理節(jié)點。

由地圖預(yù)處理節(jié)點，通過興趣區(qū)域提取算法，對局部地圖進行處理，剪切為最小矩形，并將處理后的地圖數(shù)據(jù)發(fā)送到設(shè)置的信息共享節(jié)點。

信息共享節(jié)點通過ROS 提供的通信工具包multimaster_fike 設(shè)置為整個系統(tǒng)的共享節(jié)點，并由該節(jié)點將地圖數(shù)據(jù)發(fā)送給中央處理機器人。

中央處理機器人對接收到的數(shù)據(jù)進行處理，將局部地圖合成全局地圖。

2 興趣區(qū)域地圖提取

對于二維柵欄網(wǎng)格地圖融合，通常分兩個步驟：首先是地圖的對齊，然后將對齊的地圖合并。其中最主要的步驟即為對齊問題。文獻［5］中是基于霍夫變換求得兩張地圖的對齊關(guān)系。但該方法對圖像融合的準(zhǔn)確度依賴于霍夫變換的計算速度與準(zhǔn)確度，并且受到SLAM 算法生成圖像因素的影響。ROS 社區(qū)中常用的二維占用網(wǎng)格地圖（Occupancy Grid Map，OGM），如圖2 所示的一個608?480的二維柵格地圖。圖中白色部分是機器人可以移動的自由區(qū)域，黑色部分是機器人不能移動的占用區(qū)域，灰色部分是未被確認(rèn)的未知區(qū)域。激光雷達掃描區(qū)域以外的灰色區(qū)域，對地圖合成無用，會浪費計算的代價。因此，必須對地圖進行裁剪，將未知部分去除。

圖2 完整原始柵格地圖Fig.2 Complete original grid map

2.1 二維柵欄地圖構(gòu)建原理

為獲取興趣區(qū)域地圖，首先要求得單個占用柵格在地圖中的具體表示。每個柵格有2 種狀態(tài)：空閑（free）和被占據(jù)（occupied）。各柵格之間相互獨立，可以單獨估計每個格子被占據(jù)的概率。單個占用網(wǎng)格構(gòu)建的黃金概率，是給定的數(shù)據(jù)計算整個地圖的后驗概率：

式中，為地圖；為時刻的測量值；為機器人位姿定義的路徑。

通常柵欄網(wǎng)格地圖被劃分為有限多個柵欄單元。

其中，m為第個單元格。如果該單元格被占用其值為“1”，否則為“0”。

標(biāo)準(zhǔn)的占用柵格方法，將構(gòu)建地圖問題劃分為獨立問題，對所有的柵格單元m建立后驗概率，整個地圖的后驗概率表示為：

根據(jù)貝葉斯公式可得第個柵格m的占用概率為：

第個網(wǎng)格的未被占用概率為：

將式（4）與式（5）的比值取對數(shù)可得：

可將上式表示為：

其中，（m）表示柵格m占用的概率。

在實際編程的過程中，當(dāng)其以ROS 消息發(fā)布時，會被重新定義為占有度，是［0～100］之間的整數(shù)。越接近“0”就越接近移動自由的區(qū)域，而越接近“100”就越是不可移動的已占用的區(qū)域。此外，“-1”是定義為未知區(qū)域。地圖生成根據(jù)設(shè)置的閾值，當(dāng)小于閾值時被定義為空閑，在地圖上表示為白色，高于閾值時被定義為占用，地圖上表示為黑色，而未知區(qū)域則被定義為灰色（如圖2）。

2.2 興趣區(qū)域地圖提取

興趣區(qū)域地圖提取原理是根據(jù)柵欄地圖的數(shù)據(jù)值進行的。ROS 構(gòu)建的地圖信息可用圖3 方式表示。將地圖中邊界灰色未知區(qū)域的地圖剪切，可以通過將ROS 發(fā)布的信息矩陣中，每一行從上到下，以及從下到上的部分中只有-1的行去除。同理，也要將每列從邊界向內(nèi)的部分進行刪除，得到一個包含已知區(qū)域和部分未知區(qū)域的優(yōu)化地圖。

圖3 ROS 發(fā)布地圖的信息矩陣Fig.3 Information matrix of map released by ROS

柵欄地圖可以表示為一個矩陣，對矩陣中的數(shù)據(jù)進行編碼。編碼從每一行開始，編碼表示如圖4 所示。

圖4 ROS 發(fā)布地圖信息矩陣編碼Fig.4 ROS publishes map information matrix coding

根據(jù)以上定義，可以利用逐行遍歷得出所需矩形的邊界。已知柵格地圖，其行數(shù)表示為，列數(shù)表示為。需要獲取的值為：最左邊第一個非1 柵格所在的列數(shù)、最右邊第一個非1 柵格的列數(shù)、最上方第一個非1 柵格所在的行數(shù)、最下方第一個非1 柵格所在的行數(shù)。

算法實現(xiàn)步驟如下：

初始化地圖及各邊界值：1，1，1，1，0。

計算地圖的柵格個數(shù)。

遍歷地圖的各個柵格，判斷當(dāng)前柵格是否為已知區(qū)域，如果為已知區(qū)域，則計算更新各邊值。

反復(fù)執(zhí)行步驟3，直到當(dāng)前柵格編號大于為止。

根據(jù)各邊值，計算剪切后的地圖1 算法流程圖如5 所示：

圖5 算法流程圖Fig.5 Algorithm framework

圖6 是一個275?177的地圖，從中可以看出，經(jīng)過算法優(yōu)化過的地圖，已去除了對合成無影響的區(qū)域，僅保留所需區(qū)域的最小矩形局部地圖。

圖6 優(yōu)化后的局部地圖Fig.6 Optimized local map

3 仿真實驗

本次仿真實驗采用一臺筆記本電腦（R720-15IKBN）、一臺臺式機，以及一個水星（MW325R）路由器。在gazebo 中搭建仿真環(huán)境，并在兩臺計算機中分別使用搭載了激光雷達的husky 小車進行仿真。兩臺小車分別利用基于RBPF的Gmapping 算法，構(gòu)建自己的局部地圖。搭建的Gazebo 仿真環(huán)境如圖7 所示。

圖7 Gazebo 仿真環(huán)境Fig.7 Gazebo simulation environment

兩個機器人各自構(gòu)建的局部地圖，以及實時合成的全局地圖，可以通過ROS 提供的可視化工具RVIZ 來顯示。與前面的定義相同，灰色區(qū)域代表未知區(qū)域，白色區(qū)域表示空閑，黑色區(qū)域表示障礙物。假設(shè)：進行仿真的臺式機中的機器人為R1，筆記本中的仿真機器人為R2。在進行感興趣區(qū)域提取前的效果如圖8 所示。

圖8 未提取前的效果Fig.8 Effect before extraction

興趣區(qū)域提取后的合成效果，如圖9 所示。

圖9 提取后的效果Fig.9 Effect after extraction

從以上結(jié)果對比可以看出，在進行興趣區(qū)域提取前，當(dāng)局部地圖的重疊區(qū)域達到80%以上時，地圖可以基本合成，但是邊界區(qū)域并不能完全對齊，并且地圖的融合成功率較低。

在進行興趣區(qū)域提取后，當(dāng)局部地圖的重疊區(qū)域達到60%時，地圖便可以合成，并且邊界區(qū)域?qū)R比較完整。對地圖合成的成功率也有較大的提升。

為了驗證所提出的興趣區(qū)域地圖提取與ROS的多機器人SLAM地圖合成系統(tǒng)的高效性、實時性，以及圖像感興趣區(qū)域提取方法對合成效果的積極影響。對興趣區(qū)域地圖提取前和提取后實驗成功率對比結(jié)果如圖10 所示。

圖10 局部地圖處理前后對比圖Fig.10 Comparison before and after local map processing

在實驗中，對仿真電腦在進行地圖合成與不進行地圖合成時，記錄180 s 內(nèi)的CPU 占用率。系統(tǒng)CPU 利用率對比結(jié)果如圖11 所示。

圖11 CPU 占用率對比圖Fig.11 CPU occupancy comparison chart

由此可見，將地圖融合進行集中式處理可以降低單個機器人所需要處理的信息量，降低機器人運行的CPU 占用率。

4 結(jié)束語

針對單機器人在大環(huán)境中建圖效率不足的問題，本文提出一種實時多機器人地圖的方法，該方法利用快速融合地圖的霍夫變換方法，并對單機器人構(gòu)建的局部地圖進行興趣區(qū)域提取。實驗證明，該方法可以提升地圖融合的準(zhǔn)確性與融合效率。同時，該設(shè)計利用ROS的分布式節(jié)點設(shè)計，將地圖和節(jié)點放在單獨的中央處理機器上進行運行，極大的降低了單個機器人所需處理的任務(wù)量，使多機器人系統(tǒng)更為簡潔。