馬小陸,袁書生,王 兵,吳紫恒
(1. 安徽工業(yè)大學(xué) 電氣與信息工程學(xué)院,安徽 馬鞍山 243000;2. 特種重載機(jī)器人安徽省重點(diǎn)實(shí)驗(yàn)室,安徽 馬鞍山 243000)
基于機(jī)器人操作系統(tǒng)(robot operating system,ROS)自主移動(dòng)機(jī)器人已經(jīng)被廣泛地應(yīng)用到工業(yè)、農(nóng)業(yè)、國防、航天和服務(wù)等行業(yè)中[1]。自主移動(dòng)機(jī)器人首先需要構(gòu)建其移動(dòng)的環(huán)境地圖,然后由自身傳感器(如激光雷達(dá))數(shù)據(jù)、初始位置和航跡推演算法來得出機(jī)器人的位姿并進(jìn)行導(dǎo)航。為提高自主移動(dòng)機(jī)器人的安全性,實(shí)際使用中的自主移動(dòng)機(jī)器人必須解決如下的關(guān)鍵問題:一些特定區(qū)域,如樓梯、陡坡等,要求自主移動(dòng)機(jī)器人不能經(jīng)過這些區(qū)域。即要求移動(dòng)機(jī)器人在路徑規(guī)劃時(shí),將這些區(qū)域屏蔽,常用的方法是通過設(shè)定虛擬墻來阻止移動(dòng)機(jī)器人進(jìn)入。
當(dāng)前,設(shè)置虛擬墻的方法主要是引導(dǎo)式虛擬墻,引導(dǎo)式虛擬墻的一般方法是在需要屏蔽的區(qū)域入口處設(shè)置“燈塔”來阻止機(jī)器人對(duì)該區(qū)域的訪問,這里的“燈塔”指的是在該區(qū)域設(shè)置某種傳感器,如磁條傳感器。這種方法常用在掃地機(jī)器人中,當(dāng)掃地機(jī)器人在接近磁條區(qū)域時(shí),機(jī)器人會(huì)監(jiān)測(cè)到磁條,依據(jù)磁條位置信息,控制算法會(huì)對(duì)機(jī)器人進(jìn)行降速并重新規(guī)劃局部路徑來繞過該區(qū)域。此外用的比較多的還有紅外、視覺等傳感器,文獻(xiàn)[2-3]中的掃地機(jī)器人使用紅外傳感器設(shè)置虛擬墻;文獻(xiàn)[4-5]通過相機(jī)提取虛擬邊界來設(shè)置虛擬墻;文獻(xiàn)[6]對(duì)屏蔽區(qū)域照射紅色激光;文獻(xiàn)[7]對(duì)屏蔽區(qū)域用白色布基膠帶進(jìn)行環(huán)繞,然后通過視覺傳感器來識(shí)別虛擬墻;文獻(xiàn)[8]使用深度相機(jī)對(duì)要屏蔽的空間位置設(shè)置虛擬墻,用以監(jiān)測(cè)行人是否處于安全區(qū)域。
這些引導(dǎo)式虛擬墻在實(shí)際應(yīng)用中取得了較好的效果,但在實(shí)際應(yīng)用中,存在如下問題:屏蔽的區(qū)域處要安裝傳感器(有些區(qū)域不適合安裝);因傳感器存在感知范圍,則傳感器的安裝位置的選取非常重要,特別是一些不規(guī)則的屏蔽區(qū)域,基于引導(dǎo)式虛擬墻的方法難以實(shí)現(xiàn)。
針對(duì)上述問題,提出1 種基于模擬激光雷達(dá)的ROS 自主移動(dòng)機(jī)器人虛擬墻方法。該方法在ROS環(huán)境中,對(duì)需要設(shè)置虛擬墻的區(qū)域使用激光雷達(dá)數(shù)據(jù)模擬該區(qū)域的屏蔽墻;同時(shí)在ROS 的導(dǎo)航架構(gòu)中作為路徑規(guī)劃的輸入模塊,使得自主移動(dòng)機(jī)器人在全局路徑規(guī)劃時(shí)不會(huì)進(jìn)入該區(qū)域。
有關(guān)機(jī)器人地圖的表現(xiàn)形式,國內(nèi)外學(xué)者提出了多種表示方法,大致可以分為3 類:1)幾何信息表示法;2)拓?fù)鋱D表示法;3)柵格圖表示法[9]。基于ROS 的自主移動(dòng)機(jī)器人的環(huán)境地圖一般常采用占據(jù)柵格地圖(occupancy grid map,OGM)表示法。
在通常的尺度地圖中,對(duì)于1 個(gè)點(diǎn)存在2 種情況:有障礙物(即occupied 狀態(tài),用1 表示)和無障礙物(即free 狀態(tài),用0 表示)。在占據(jù)柵格地圖中,對(duì)于1 個(gè)點(diǎn)s,使用p(s=1)來表示free狀態(tài)的概率,用p(s=0)來表示occupied 的概率,2 者之和為1,因1 個(gè)點(diǎn)使用2 個(gè)值的表達(dá)較麻煩,可以使用這個(gè)2 個(gè)概率的比值Odd 表示為

Odd 值越大,表示空概率越高。
對(duì)于1 個(gè)點(diǎn),新的測(cè)量值為z={0,1},測(cè)量值也在這個(gè)點(diǎn)上有概率計(jì)算,由條件概率公式,比值公式更新為

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

將式(3)和式(4)帶入式(2),同時(shí)2 邊取對(duì)數(shù)得到

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

式中:S+表示的是當(dāng)前時(shí)刻位置s 的狀態(tài);S-表示之前一時(shí)刻位置s 的狀態(tài)。
在使用激光雷達(dá)傳感器構(gòu)建地圖時(shí),激光雷達(dá)傳感器實(shí)時(shí)獲取測(cè)量數(shù)據(jù)(觀測(cè)值),然后依據(jù)式(8)對(duì)占據(jù)柵格地圖中的點(diǎn)的占用概率進(jìn)行更新。
在初始狀態(tài)下,1 個(gè)點(diǎn)的初始狀態(tài)為

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

圖1 網(wǎng)格占用概率更新示例
圖1 中,機(jī)器人底盤上射出1 條激光束,被擋住的網(wǎng)格標(biāo)為深色,并且中心有黑點(diǎn),表示激光束打在該網(wǎng)格的障礙物上,光束被擋住即表示該部分被物體占據(jù),假設(shè)傳感器測(cè)量模型值:占用為0.9,空閑為-0.7。由式(9)可知,柵格初始值S 為0。從t0到t1時(shí)刻,圖1(a)所示的激光束在穿過5 個(gè)柵格后被第6 個(gè)柵格擋住,在觀測(cè)值z(mì)=1 的情況下,S=0+0.9=0.9,于是被擋住柵格占據(jù)率更新為0.9,在z=0 的情況下,S=0-0.7= -0.7,順利通過的那5 個(gè)柵格占據(jù)率更新為-0.7。如圖1(b)所示,從t1到t2時(shí)刻,再次根據(jù)激光雷達(dá)掃描的數(shù)據(jù)來更新,圖1(b)中又有3 個(gè)新的被激光束通過的網(wǎng)格占據(jù)率更新為-0.7,1 個(gè)新的擋住激光束的網(wǎng)格更新為0.9,而有3 個(gè)上次通過激光的網(wǎng)格在這次通過激光束之后,其占據(jù)率S=-0.7-0.7=-1.4。就是通過這樣的方式來對(duì)網(wǎng)格的占據(jù)率進(jìn)行更新,通常在這里會(huì)對(duì)占據(jù)率的數(shù)值設(shè)置2 個(gè)閾值,上限閾值和下限閾值,假設(shè)這里設(shè)置的下限閾值為-2,在t2時(shí)刻有3 個(gè)點(diǎn)占據(jù)率為-1.4,當(dāng)下1 次測(cè)量時(shí),激光仍然通過該網(wǎng)格,那么該網(wǎng)格占據(jù)率數(shù)值就S=-1.4-0.7=-2.1,由于設(shè)置了下限閾值,該網(wǎng)格占據(jù)率為-2.0,同理上限也是這樣。
依據(jù)以上原理,可以得到地圖上已經(jīng)探測(cè)到的各個(gè)部分是否被占據(jù),即該區(qū)域是否有障礙物,從而得出地圖中的各種信息,包括障礙物的位置以及輪廓等。
在移動(dòng)機(jī)器人導(dǎo)航時(shí),首先要載入環(huán)境地圖,環(huán)境地圖中包含整個(gè)環(huán)境的信息,主要是障礙物的分布等,它是以1 張圖片和配置文件形式保存的。地圖配置文件中主要有的參數(shù)有地圖名、分辨率、初始位置、完全占據(jù)閾值、完全空白閾值等。
而移動(dòng)機(jī)器人實(shí)際使用的環(huán)境地圖是以灰度值形式表現(xiàn)的,因此還要進(jìn)行“占用概率-灰度值”之間的轉(zhuǎn)換。轉(zhuǎn)換方法如圖2 所示,當(dāng)灰度值大于等于完全占據(jù)閾值時(shí)(黑色部分),表示該部分是有障礙物;當(dāng)灰度值小于等于空白閾值時(shí)(白色部分),表示該部分無障礙物;而當(dāng)閾值介于2 者之間時(shí)(灰色部分),表示該區(qū)域?yàn)槲粗獏^(qū)域。但在移動(dòng)機(jī)器人行走過程中,該部分的占用率會(huì)更新,若發(fā)現(xiàn)該區(qū)域并沒障礙物時(shí),機(jī)器人可以移動(dòng)到該區(qū)域,同時(shí)會(huì)更新地圖。

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

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

圖4 膨脹層示意圖
圖4 中,黑色線條的為靜態(tài)圖層中的墻壁和障礙物,圍繞其外層的淺灰色1 圈為膨脹層。
本文提出的激光雷達(dá)模擬虛擬墻的原理,是通過在ROS 環(huán)境中模擬發(fā)送激光雷達(dá)數(shù)據(jù),以這個(gè)激光數(shù)據(jù)來模擬激光傳感器打在墻壁上的狀態(tài),這樣在障礙層和膨脹層就會(huì)加入障礙物的數(shù)據(jù)。
對(duì)于ROS 的路徑規(guī)劃來說,首先會(huì)有個(gè)全局路徑規(guī)劃,這主要是依據(jù)靜態(tài)地圖層得出的路線,然后機(jī)器人還有1 個(gè)局部路徑規(guī)劃,局部路徑規(guī)劃會(huì)通過激光雷達(dá)等傳感器的數(shù)據(jù)實(shí)時(shí)動(dòng)態(tài)規(guī)劃出1 條擬合向全局路徑的路徑,這樣移動(dòng)機(jī)器人就能從起點(diǎn)導(dǎo)航到目標(biāo)點(diǎn),具體步驟如下:
ROS 的Map_Server 啟動(dòng)時(shí),會(huì)將靜態(tài)地圖層和膨脹層加入,主要是整個(gè)環(huán)境的邊框、已經(jīng)探測(cè)好的墻壁和一些靜態(tài)障礙物;
當(dāng)全局路徑規(guī)劃器接受了1 個(gè)目標(biāo)點(diǎn)坐標(biāo)后,根據(jù)靜態(tài)地圖層和膨脹層以及當(dāng)前傳感器所探測(cè)的范圍內(nèi)的障礙層和膨脹層(傳感器有個(gè)測(cè)量范 圍)信息就會(huì)規(guī)劃出1 條從當(dāng)前位置到目標(biāo)點(diǎn)位置的全局路徑;
這條全局路徑接著會(huì)交給局部路徑規(guī)劃器,局部路徑規(guī)劃器通過對(duì)規(guī)劃好的全局路徑進(jìn)行分段裁剪,然后再依據(jù)傳感器檢測(cè)到的動(dòng)態(tài)障礙物進(jìn)行動(dòng)態(tài)規(guī)劃,將規(guī)劃的結(jié)果交給底層執(zhí)行部分來進(jìn)行控制,經(jīng)過一段一段的控制,最終機(jī)器人會(huì)從起點(diǎn)走到目標(biāo)點(diǎn)。
而在有虛擬墻條件下,基于ROS 的路徑規(guī)劃架構(gòu)如圖5 所示。

圖5 基于ROS 的路徑規(guī)劃架構(gòu)
圖5 中深色模塊為模擬的虛擬墻,可見它也是作為整個(gè)路徑規(guī)劃的輸入部分。 ROS 的Map_Server 啟動(dòng)后,就將激光模擬虛擬墻初始化,并且設(shè)置成不清理的狀態(tài),這樣虛擬墻就會(huì)一直存在于該處。不像實(shí)際激光雷達(dá)探測(cè)到的動(dòng)態(tài)障礙物,當(dāng)機(jī)器人的傳感器探測(cè)范圍已不在該區(qū)域時(shí),該障礙物就在地圖上去掉。這樣的虛擬墻就如同是靜態(tài)障礙物,在全局路徑規(guī)劃器規(guī)劃出的路徑時(shí)就已經(jīng)繞開虛擬墻屏蔽的區(qū)域。
而對(duì)于其他方法設(shè)置的虛擬墻,全局路徑規(guī)劃時(shí)不會(huì)考慮虛擬墻區(qū)域,它只會(huì)影響局部路徑規(guī)劃。因此這些方法的缺點(diǎn)是:只有當(dāng)虛擬墻在局部路徑規(guī)劃器有效范圍內(nèi)時(shí),才會(huì)重新規(guī)劃局部路徑繞開虛擬墻區(qū)域,因此規(guī)劃出來的路徑速度會(huì)更緩慢,且由于突然規(guī)劃出繞開虛擬墻區(qū)域,規(guī)劃出的路徑相對(duì)來說比較的曲折。
本文提出的模擬的虛擬墻在全局路徑規(guī)劃時(shí)就考慮到該區(qū)域,因此規(guī)劃出的路徑會(huì)更加平滑。
實(shí)驗(yàn)平臺(tái)為實(shí)際使用的移動(dòng)機(jī)器人底盤,其系統(tǒng)架構(gòu)如圖6 所示,圖6 中箭頭表示數(shù)據(jù)的流向,實(shí)線連接表示物理上存在連接。

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

圖7 機(jī)器人底盤實(shí)物外部
實(shí)驗(yàn)的環(huán)境為所在的實(shí)驗(yàn)室,其掃描得出的環(huán)境地圖如圖9 所示。為了更好地驗(yàn)證本文設(shè)置虛擬墻的方法的有效性,實(shí)驗(yàn)分為3 個(gè)部分:

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

圖9 實(shí)驗(yàn)環(huán)境地圖
1)不設(shè)置虛擬墻,查看從起點(diǎn)到終點(diǎn)的規(guī)劃出來的路徑以及導(dǎo)航的結(jié)果;
2)將虛擬墻設(shè)置在1)中規(guī)劃出的路徑上,查看此時(shí)規(guī)劃的路徑以及導(dǎo)航結(jié)果,同時(shí)增加虛擬墻的長(zhǎng)度到封鎖整個(gè)無障礙區(qū),查看此時(shí)的結(jié)果;
3)設(shè)置復(fù)雜區(qū)域(此處設(shè)置的是個(gè)方形區(qū)域),查看復(fù)雜區(qū)域的虛擬墻效果。
圖10(a)為第1 部分的實(shí)驗(yàn)結(jié)果;圖10(b)、圖10(c)及圖10(d)為第2 部分的實(shí)驗(yàn)結(jié)果;圖11為第3 部分實(shí)驗(yàn)結(jié)果。
圖10 中,起點(diǎn)為橢圓形圈起來的黑色圓點(diǎn),目標(biāo)點(diǎn)為橢圓形圈起來的帶有方向的箭頭(表示位置和方向),圖10 中設(shè)置的虛擬墻是以矩形框圈起來部分,全局路徑為圖10 中標(biāo)注的從起點(diǎn)到目標(biāo)點(diǎn)的線條,局部路徑為圖10 中標(biāo)出的從起點(diǎn)引出來的小段線條。
對(duì)比圖10(a)、圖10(b)可見,圖10(b)設(shè)置的虛擬墻,移動(dòng)機(jī)器人不是在到達(dá)附近才繞開的,在全局路徑開始就已經(jīng)繞開了設(shè)置的虛擬墻區(qū)域,因而要比其他類的虛擬墻由局部路徑規(guī)劃才繞開 虛擬墻得到的路徑要平滑,達(dá)到了預(yù)期的效果。

圖10 第1 部分和第2 部分實(shí)驗(yàn)結(jié)果圖
圖10(c)和圖10(d)中,將虛擬墻進(jìn)行延伸,使用虛擬墻把整個(gè)實(shí)驗(yàn)室完全隔開,在虛擬墻的另一側(cè)的多個(gè)地方下發(fā)目標(biāo)點(diǎn)。這里只保留2 個(gè)點(diǎn)的結(jié)果,圖10(c)和圖10(d)顯示的為目標(biāo)點(diǎn)1 和目標(biāo)點(diǎn)2 下發(fā)之后的結(jié)果。會(huì)發(fā)現(xiàn)并沒有出現(xiàn)規(guī)劃的路徑,這是因?yàn)檎麄€(gè)虛擬墻將實(shí)驗(yàn)室分成2 個(gè)獨(dú)立的部分,無法找到能過去的地方,所以并沒有出現(xiàn)路徑,此時(shí)的機(jī)器人行為則表現(xiàn)為原地轉(zhuǎn)圈,轉(zhuǎn)圈用來嘗試清理障礙物后,最后還是發(fā)現(xiàn)尋 找路徑失敗,則放棄此次的導(dǎo)航。
在第3 部分實(shí)驗(yàn)中,主要針對(duì)復(fù)雜區(qū)域的虛擬墻,要屏蔽的區(qū)域?yàn)榉叫螀^(qū)域,如圖11 所示,圖11 中的虛擬墻隔離的方形區(qū)域已用橢圓形圈出來。
由圖 11(a)可知,全局路徑和局部路徑因要繞開設(shè)置的虛擬墻區(qū)域,其規(guī)劃的路徑明顯彎向虛擬墻圍成方形區(qū)域的另一側(cè)。圖 11(b)將機(jī)器人置于虛擬墻圈的區(qū)域內(nèi),在區(qū)域外下發(fā)導(dǎo)航點(diǎn);圖11(c)將機(jī)器人置于虛擬墻圍成的區(qū)域外,導(dǎo) 航點(diǎn)放在隔離區(qū)內(nèi)。發(fā)現(xiàn)這2 次都沒有出現(xiàn)規(guī)劃路徑,并且機(jī)器人的行為也是原地轉(zhuǎn)圈一段時(shí)間后機(jī)器人停止運(yùn)動(dòng),在監(jiān)測(cè)端出現(xiàn)如圖11(d)中的放棄導(dǎo)航結(jié)果的提示。


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