一種模擬激光雷達(dá)的自主移動機(jī)器人虛擬墻方法

馬小陸，袁書生，王 兵，吳紫恒

（1. 安徽工業(yè)大學(xué) 電氣與信息工程學(xué)院，安徽 馬鞍山 243000；2. 特種重載機(jī)器人安徽省重點實驗室，安徽 馬鞍山 243000）

0 引言

基于機(jī)器人操作系統(tǒng)（robot operating system，ROS）自主移動機(jī)器人已經(jīng)被廣泛地應(yīng)用到工業(yè)、農(nóng)業(yè)、國防、航天和服務(wù)等行業(yè)中[1]。自主移動機(jī)器人首先需要構(gòu)建其移動的環(huán)境地圖，然后由自身傳感器（如激光雷達(dá)）數(shù)據(jù)、初始位置和航跡推演算法來得出機(jī)器人的位姿并進(jìn)行導(dǎo)航。為提高自主移動機(jī)器人的安全性，實際使用中的自主移動機(jī)器人必須解決如下的關(guān)鍵問題：一些特定區(qū)域，如樓梯、陡坡等，要求自主移動機(jī)器人不能經(jīng)過這些區(qū)域。即要求移動機(jī)器人在路徑規(guī)劃時，將這些區(qū)域屏蔽，常用的方法是通過設(shè)定虛擬墻來阻止移動機(jī)器人進(jìn)入。

當(dāng)前，設(shè)置虛擬墻的方法主要是引導(dǎo)式虛擬墻，引導(dǎo)式虛擬墻的一般方法是在需要屏蔽的區(qū)域入口處設(shè)置“燈塔”來阻止機(jī)器人對該區(qū)域的訪問，這里的“燈塔”指的是在該區(qū)域設(shè)置某種傳感器，如磁條傳感器。這種方法常用在掃地機(jī)器人中，當(dāng)掃地機(jī)器人在接近磁條區(qū)域時，機(jī)器人會監(jiān)測到磁條，依據(jù)磁條位置信息，控制算法會對機(jī)器人進(jìn)行降速并重新規(guī)劃局部路徑來繞過該區(qū)域。此外用的比較多的還有紅外、視覺等傳感器，文獻(xiàn)[2-3]中的掃地機(jī)器人使用紅外傳感器設(shè)置虛擬墻；文獻(xiàn)[4-5]通過相機(jī)提取虛擬邊界來設(shè)置虛擬墻；文獻(xiàn)[6]對屏蔽區(qū)域照射紅色激光；文獻(xiàn)[7]對屏蔽區(qū)域用白色布基膠帶進(jìn)行環(huán)繞，然后通過視覺傳感器來識別虛擬墻；文獻(xiàn)[8]使用深度相機(jī)對要屏蔽的空間位置設(shè)置虛擬墻，用以監(jiān)測行人是否處于安全區(qū)域。

這些引導(dǎo)式虛擬墻在實際應(yīng)用中取得了較好的效果，但在實際應(yīng)用中，存在如下問題：屏蔽的區(qū)域處要安裝傳感器（有些區(qū)域不適合安裝）；因傳感器存在感知范圍，則傳感器的安裝位置的選取非常重要，特別是一些不規(guī)則的屏蔽區(qū)域，基于引導(dǎo)式虛擬墻的方法難以實現(xiàn)。

針對上述問題，提出1 種基于模擬激光雷達(dá)的ROS 自主移動機(jī)器人虛擬墻方法。該方法在ROS環(huán)境中，對需要設(shè)置虛擬墻的區(qū)域使用激光雷達(dá)數(shù)據(jù)模擬該區(qū)域的屏蔽墻；同時在ROS 的導(dǎo)航架構(gòu)中作為路徑規(guī)劃的輸入模塊，使得自主移動機(jī)器人在全局路徑規(guī)劃時不會進(jìn)入該區(qū)域。

1 基于ROS 的自主移動機(jī)器人的環(huán)境地圖

有關(guān)機(jī)器人地圖的表現(xiàn)形式，國內(nèi)外學(xué)者提出了多種表示方法，大致可以分為3 類：1）幾何信息表示法；2）拓?fù)鋱D表示法；3）柵格圖表示法[9]?；赗OS 的自主移動機(jī)器人的環(huán)境地圖一般常采用占據(jù)柵格地圖(occupancy grid map，OGM)表示法。

在通常的尺度地圖中，對于1 個點存在2 種情況：有障礙物（即occupied 狀態(tài)，用1 表示）和無障礙物（即free 狀態(tài)，用0 表示）。在占據(jù)柵格地圖中，對于1 個點s，使用p（s=1）來表示free狀態(tài)的概率，用p（s=0）來表示occupied 的概率，2 者之和為1，因1 個點使用2 個值的表達(dá)較麻煩，可以使用這個2 個概率的比值Odd 表示為

Odd 值越大，表示空概率越高。

對于1 個點，新的測量值為z=｛0，1｝，測量值也在這個點上有概率計算，由條件概率公式，比值公式更新為

在新的測量事件條件下，空的概率、占據(jù)概率根據(jù)貝葉斯公式可得：

將式（3）和式（4）帶入式（2），同時2 邊取對數(shù)得到

若使用log Odd(s)來表示位置s 的狀態(tài)S 的話，則地圖中的更新規(guī)則可為

式中：S+表示的是當(dāng)前時刻位置s 的狀態(tài)；S-表示之前一時刻位置s 的狀態(tài)。

在使用激光雷達(dá)傳感器構(gòu)建地圖時，激光雷達(dá)傳感器實時獲取測量數(shù)據(jù)（觀測值），然后依據(jù)式（8）對占據(jù)柵格地圖中的點的占用概率進(jìn)行更新。

在初始狀態(tài)下，1 個點的初始狀態(tài)為

網(wǎng)格占用概率更新示例如圖1 所示。

圖1 網(wǎng)格占用概率更新示例

圖1 中，機(jī)器人底盤上射出1 條激光束，被擋住的網(wǎng)格標(biāo)為深色，并且中心有黑點，表示激光束打在該網(wǎng)格的障礙物上，光束被擋住即表示該部分被物體占據(jù)，假設(shè)傳感器測量模型值：占用為0.9，空閑為-0.7。由式（9）可知，柵格初始值S 為0。從t0到t1時刻，圖1(a)所示的激光束在穿過5 個柵格后被第6 個柵格擋住，在觀測值z=1 的情況下，S=0+0.9=0.9，于是被擋住柵格占據(jù)率更新為0.9，在z=0 的情況下，S=0-0.7= -0.7，順利通過的那5 個柵格占據(jù)率更新為-0.7。如圖1(b)所示，從t1到t2時刻，再次根據(jù)激光雷達(dá)掃描的數(shù)據(jù)來更新，圖1(b)中又有3 個新的被激光束通過的網(wǎng)格占據(jù)率更新為-0.7，1 個新的擋住激光束的網(wǎng)格更新為0.9，而有3 個上次通過激光的網(wǎng)格在這次通過激光束之后，其占據(jù)率S=-0.7-0.7=-1.4。就是通過這樣的方式來對網(wǎng)格的占據(jù)率進(jìn)行更新，通常在這里會對占據(jù)率的數(shù)值設(shè)置2 個閾值，上限閾值和下限閾值，假設(shè)這里設(shè)置的下限閾值為-2，在t2時刻有3 個點占據(jù)率為-1.4，當(dāng)下1 次測量時，激光仍然通過該網(wǎng)格，那么該網(wǎng)格占據(jù)率數(shù)值就S=-1.4-0.7=-2.1，由于設(shè)置了下限閾值，該網(wǎng)格占據(jù)率為-2.0，同理上限也是這樣。

依據(jù)以上原理，可以得到地圖上已經(jīng)探測到的各個部分是否被占據(jù)，即該區(qū)域是否有障礙物，從而得出地圖中的各種信息，包括障礙物的位置以及輪廓等。

在移動機(jī)器人導(dǎo)航時，首先要載入環(huán)境地圖，環(huán)境地圖中包含整個環(huán)境的信息，主要是障礙物的分布等，它是以1 張圖片和配置文件形式保存的。地圖配置文件中主要有的參數(shù)有地圖名、分辨率、初始位置、完全占據(jù)閾值、完全空白閾值等。

而移動機(jī)器人實際使用的環(huán)境地圖是以灰度值形式表現(xiàn)的，因此還要進(jìn)行“占用概率-灰度值”之間的轉(zhuǎn)換。轉(zhuǎn)換方法如圖2 所示，當(dāng)灰度值大于等于完全占據(jù)閾值時（黑色部分），表示該部分是有障礙物；當(dāng)灰度值小于等于空白閾值時（白色部分），表示該部分無障礙物；而當(dāng)閾值介于2 者之間時（灰色部分），表示該區(qū)域為未知區(qū)域。但在移動機(jī)器人行走過程中，該部分的占用率會更新，若發(fā)現(xiàn)該區(qū)域并沒障礙物時，機(jī)器人可以移動到該區(qū)域，同時會更新地圖。

圖2 基于ROS 的移動機(jī)器人環(huán)境地圖

2 基于ROS 的自主移動機(jī)器人的代價地圖

第1 節(jié)介紹的環(huán)境地圖在基于ROS 的移動機(jī)器人路徑規(guī)劃中被稱為代價地圖，也被稱為單源代價地圖，早期的ROS 工程中，普遍采用這種代價地圖。但因存儲空間有限，地圖的更新存在問題。文獻(xiàn)[10]提出了分層代價地圖，如圖3 所示，從下到上依次為靜態(tài)地圖層、障礙物層、膨脹層以及主控地圖層。

圖3 分層代價地圖

ROS 中分層代價的地圖層主要分為3 層，即新功能層、人機(jī)交互層和標(biāo)準(zhǔn)地圖層。

新功能層是在分層代價地圖中添加了更加復(fù)雜的某些功能，比如聲吶層的分層代價地圖就增加了如何處理聲吶數(shù)據(jù)的選項；人機(jī)交互層使得人與機(jī)器人之間交互更加有效，比如過道層，在不同國家，行人有靠不同側(cè)行走的習(xí)慣，文獻(xiàn)[11]在人機(jī)交互層中就使用了類似的模型，使得機(jī)器人在走道里偏向于右側(cè)移動；標(biāo)準(zhǔn)地圖層是分層代價地圖中最為基礎(chǔ)的圖層，前面介紹的新功能層和人機(jī)交互層在應(yīng)用中不一定有，但標(biāo)準(zhǔn)地圖層一定會存在，標(biāo)準(zhǔn)地圖層主要分為靜態(tài)地圖層、障礙層、體素層和膨脹層。靜態(tài)地圖層是接收第1 節(jié)中的環(huán)境地圖數(shù)據(jù)，返回1 個覆蓋整個地圖的邊框，該地圖層是靜態(tài)的，因而其邊緣的尺寸在后續(xù)的迭代中是不會增大的；障礙層從高精度的傳感器（激光雷達(dá)或RGB-D 等）獲取數(shù)據(jù)，并將其放置于網(wǎng)格中，傳感器的數(shù)據(jù)如第1 節(jié)所述的對占據(jù)柵格進(jìn)行更新，在每個更新周期將新的傳感器數(shù)據(jù)放入圖層的代價地圖中，且邊緣會自適應(yīng)擴(kuò)展；體素層和障礙層的功能相同，但體素層是以3 維的形式跟蹤傳感器數(shù)據(jù)，文獻(xiàn)[12]介紹的3 維體素網(wǎng)格方法，能夠更加智能地檢測障礙物，可以觀測障礙物的多個高度信息；膨脹層，實際上是障礙層或者靜態(tài)層外側(cè)加入1 段的緩沖區(qū)域，如圖4所示。

圖4 膨脹層示意圖

圖4 中，黑色線條的為靜態(tài)圖層中的墻壁和障礙物，圍繞其外層的淺灰色1 圈為膨脹層。

3 模擬激光雷達(dá)虛擬墻原理

本文提出的激光雷達(dá)模擬虛擬墻的原理，是通過在ROS 環(huán)境中模擬發(fā)送激光雷達(dá)數(shù)據(jù)，以這個激光數(shù)據(jù)來模擬激光傳感器打在墻壁上的狀態(tài)，這樣在障礙層和膨脹層就會加入障礙物的數(shù)據(jù)。

對于ROS 的路徑規(guī)劃來說，首先會有個全局路徑規(guī)劃，這主要是依據(jù)靜態(tài)地圖層得出的路線，然后機(jī)器人還有1 個局部路徑規(guī)劃，局部路徑規(guī)劃會通過激光雷達(dá)等傳感器的數(shù)據(jù)實時動態(tài)規(guī)劃出1 條擬合向全局路徑的路徑，這樣移動機(jī)器人就能從起點導(dǎo)航到目標(biāo)點，具體步驟如下：

ROS 的Map_Server 啟動時，會將靜態(tài)地圖層和膨脹層加入，主要是整個環(huán)境的邊框、已經(jīng)探測好的墻壁和一些靜態(tài)障礙物；

當(dāng)全局路徑規(guī)劃器接受了1 個目標(biāo)點坐標(biāo)后，根據(jù)靜態(tài)地圖層和膨脹層以及當(dāng)前傳感器所探測的范圍內(nèi)的障礙層和膨脹層（傳感器有個測量范 圍）信息就會規(guī)劃出1 條從當(dāng)前位置到目標(biāo)點位置的全局路徑；

這條全局路徑接著會交給局部路徑規(guī)劃器，局部路徑規(guī)劃器通過對規(guī)劃好的全局路徑進(jìn)行分段裁剪，然后再依據(jù)傳感器檢測到的動態(tài)障礙物進(jìn)行動態(tài)規(guī)劃，將規(guī)劃的結(jié)果交給底層執(zhí)行部分來進(jìn)行控制，經(jīng)過一段一段的控制，最終機(jī)器人會從起點走到目標(biāo)點。

而在有虛擬墻條件下，基于ROS 的路徑規(guī)劃架構(gòu)如圖5 所示。

圖5 基于ROS 的路徑規(guī)劃架構(gòu)

圖5 中深色模塊為模擬的虛擬墻，可見它也是作為整個路徑規(guī)劃的輸入部分。 ROS 的Map_Server 啟動后，就將激光模擬虛擬墻初始化，并且設(shè)置成不清理的狀態(tài)，這樣虛擬墻就會一直存在于該處。不像實際激光雷達(dá)探測到的動態(tài)障礙物，當(dāng)機(jī)器人的傳感器探測范圍已不在該區(qū)域時，該障礙物就在地圖上去掉。這樣的虛擬墻就如同是靜態(tài)障礙物，在全局路徑規(guī)劃器規(guī)劃出的路徑時就已經(jīng)繞開虛擬墻屏蔽的區(qū)域。

而對于其他方法設(shè)置的虛擬墻，全局路徑規(guī)劃時不會考慮虛擬墻區(qū)域，它只會影響局部路徑規(guī)劃。因此這些方法的缺點是：只有當(dāng)虛擬墻在局部路徑規(guī)劃器有效范圍內(nèi)時，才會重新規(guī)劃局部路徑繞開虛擬墻區(qū)域，因此規(guī)劃出來的路徑速度會更緩慢，且由于突然規(guī)劃出繞開虛擬墻區(qū)域，規(guī)劃出的路徑相對來說比較的曲折。

本文提出的模擬的虛擬墻在全局路徑規(guī)劃時就考慮到該區(qū)域，因此規(guī)劃出的路徑會更加平滑。

4 實驗與結(jié)果分析

4.1 實驗平臺

實驗平臺為實際使用的移動機(jī)器人底盤，其系統(tǒng)架構(gòu)如圖6 所示，圖6 中箭頭表示數(shù)據(jù)的流向，實線連接表示物理上存在連接。

圖6 移動機(jī)器人底盤系統(tǒng)架構(gòu)

實驗平臺主要分為3 個部分，即底盤運動控制部分、中間決策部分、監(jiān)測部分。底盤運動控制部分，主要采用STM32 單片機(jī)來處理來自決策部分的控制命令，主要有機(jī)器人速度的控制，編碼器、慣性測量單元（Inertial Measurement Unit， IMU）等各種傳感器數(shù)據(jù)的采集以及上傳，底盤運動控制部分和中間層通過RS-232 接口來進(jìn)行數(shù)據(jù)交換。中間決策部分是1 臺搭載了Ubuntu16.04 的x86 工控機(jī)。在該系統(tǒng)上已搭建好ROS 環(huán)境，主要負(fù)責(zé)機(jī)器人的實時定位、路徑規(guī)劃等運算量比較大的部分。上層監(jiān)測部分使用的是1 臺已經(jīng)搭建好ROS環(huán)境的筆記本電腦，該電腦通過無線網(wǎng)絡(luò)接入整個系統(tǒng)，這樣可通過遠(yuǎn)程的方式對決策部分下發(fā)命令，同時可以實時監(jiān)控導(dǎo)航的效果。移動機(jī)器人的底盤的實物如圖7 和圖8 所示。

圖7 機(jī)器人底盤實物外部

4.2 實驗過程

實驗的環(huán)境為所在的實驗室，其掃描得出的環(huán)境地圖如圖9 所示。為了更好地驗證本文設(shè)置虛擬墻的方法的有效性，實驗分為3 個部分：

圖8 底盤實物圖內(nèi)部

圖9 實驗環(huán)境地圖

1）不設(shè)置虛擬墻，查看從起點到終點的規(guī)劃出來的路徑以及導(dǎo)航的結(jié)果；

2）將虛擬墻設(shè)置在1）中規(guī)劃出的路徑上，查看此時規(guī)劃的路徑以及導(dǎo)航結(jié)果，同時增加虛擬墻的長度到封鎖整個無障礙區(qū)，查看此時的結(jié)果；

3）設(shè)置復(fù)雜區(qū)域（此處設(shè)置的是個方形區(qū)域），查看復(fù)雜區(qū)域的虛擬墻效果。

圖10(a)為第1 部分的實驗結(jié)果；圖10(b)、圖10(c)及圖10(d)為第2 部分的實驗結(jié)果；圖11為第3 部分實驗結(jié)果。

圖10 中，起點為橢圓形圈起來的黑色圓點，目標(biāo)點為橢圓形圈起來的帶有方向的箭頭（表示位置和方向），圖10 中設(shè)置的虛擬墻是以矩形框圈起來部分，全局路徑為圖10 中標(biāo)注的從起點到目標(biāo)點的線條，局部路徑為圖10 中標(biāo)出的從起點引出來的小段線條。

對比圖10(a）、圖10(b)可見，圖10(b)設(shè)置的虛擬墻，移動機(jī)器人不是在到達(dá)附近才繞開的，在全局路徑開始就已經(jīng)繞開了設(shè)置的虛擬墻區(qū)域，因而要比其他類的虛擬墻由局部路徑規(guī)劃才繞開 虛擬墻得到的路徑要平滑，達(dá)到了預(yù)期的效果。

圖10 第1 部分和第2 部分實驗結(jié)果圖

圖10(c)和圖10(d)中，將虛擬墻進(jìn)行延伸，使用虛擬墻把整個實驗室完全隔開，在虛擬墻的另一側(cè)的多個地方下發(fā)目標(biāo)點。這里只保留2 個點的結(jié)果，圖10(c)和圖10(d)顯示的為目標(biāo)點1 和目標(biāo)點2 下發(fā)之后的結(jié)果。會發(fā)現(xiàn)并沒有出現(xiàn)規(guī)劃的路徑，這是因為整個虛擬墻將實驗室分成2 個獨立的部分，無法找到能過去的地方，所以并沒有出現(xiàn)路徑，此時的機(jī)器人行為則表現(xiàn)為原地轉(zhuǎn)圈，轉(zhuǎn)圈用來嘗試清理障礙物后，最后還是發(fā)現(xiàn)尋 找路徑失敗，則放棄此次的導(dǎo)航。

在第3 部分實驗中，主要針對復(fù)雜區(qū)域的虛擬墻，要屏蔽的區(qū)域為方形區(qū)域，如圖11 所示，圖11 中的虛擬墻隔離的方形區(qū)域已用橢圓形圈出來。

由圖 11(a)可知，全局路徑和局部路徑因要繞開設(shè)置的虛擬墻區(qū)域，其規(guī)劃的路徑明顯彎向虛擬墻圍成方形區(qū)域的另一側(cè)。圖 11(b)將機(jī)器人置于虛擬墻圈的區(qū)域內(nèi)，在區(qū)域外下發(fā)導(dǎo)航點；圖11(c)將機(jī)器人置于虛擬墻圍成的區(qū)域外，導(dǎo) 航點放在隔離區(qū)內(nèi)。發(fā)現(xiàn)這2 次都沒有出現(xiàn)規(guī)劃路徑，并且機(jī)器人的行為也是原地轉(zhuǎn)圈一段時間后機(jī)器人停止運動，在監(jiān)測端出現(xiàn)如圖11(d)中的放棄導(dǎo)航結(jié)果的提示。

圖11 第3 部分實驗結(jié)果圖

5 結(jié)束語

本文提出了1 種基于模擬激光雷達(dá)的ROS 自主移動機(jī)器人虛擬墻方法，該方法在ROS 環(huán)境中，對需要設(shè)置虛擬墻的區(qū)域使用模擬的激光雷達(dá)數(shù)據(jù)模擬該區(qū)域的屏蔽墻，且作為路徑規(guī)劃的輸入模塊。并進(jìn)行了實驗驗證，通過在無虛擬墻時規(guī)劃的路徑處設(shè)置虛擬墻，查看此時路徑規(guī)劃的結(jié)果及導(dǎo)航效果，以及在此基礎(chǔ)上對虛擬墻進(jìn)行延伸至分割地圖，查看此時虛擬墻的效果。此外，還通過虛擬墻來隔離整個區(qū)域，在此基礎(chǔ)上，對整個區(qū)域，內(nèi)部隔離、外部隔離的效果都分別進(jìn)行了實驗驗證。實驗結(jié)果表明，該方法能夠很好地實現(xiàn)虛擬墻的功能。該方法不用在屏蔽區(qū)域安裝傳感器，成本低；同時該方法不受屏蔽區(qū)域的形狀的影響，具有設(shè)置靈活的優(yōu)點。