興趣區(qū)域地圖提取與ROS的多機(jī)器人SLAM地圖合成研究

李 森，范平清

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

0 引言

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

針對(duì)多機(jī)器人同步定位與建圖（SLAM）地圖融合問(wèn)題的研究，一般可以分為兩個(gè)方面：

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

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

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

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

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

圖1 整體框架Fig.1 Overall framework

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

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

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

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

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

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

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

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

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

式中，為地圖；為時(shí)刻的測(cè)量值；為機(jī)器人位姿定義的路徑。

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

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

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

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

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

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

可將上式表示為：

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

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

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

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

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

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

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

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

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

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

計(jì)算地圖的柵格個(gè)數(shù)。

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

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

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

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

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

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

3 仿真實(shí)驗(yàn)

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

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

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

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

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

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

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

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

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

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

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

圖11 CPU 占用率對(duì)比圖Fig.11 CPU occupancy comparison chart

由此可見(jiàn)，將地圖融合進(jìn)行集中式處理可以降低單個(gè)機(jī)器人所需要處理的信息量，降低機(jī)器人運(yùn)行的CPU 占用率。

4 結(jié)束語(yǔ)

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