救援機(jī)器人搜索目標(biāo)路徑環(huán)境快速建模仿真

包 琳，高 巍

(大連海洋大學(xué)，遼寧 大連 116023)

1 引言

災(zāi)害發(fā)生時(shí)，事故現(xiàn)場(chǎng)的危險(xiǎn)指數(shù)、復(fù)雜指數(shù)都是極高的，救援的緊迫性給救援工作帶來(lái)了嚴(yán)峻的考驗(yàn)。災(zāi)害發(fā)生后的黃金救援時(shí)間為72小時(shí)，而這段時(shí)間事故現(xiàn)場(chǎng)因?yàn)橛泻怏w較多、高溫缺氧以及二次災(zāi)害的影響，使得救援工作難度大大增加，如果救援人員此時(shí)貿(mào)然進(jìn)入事故現(xiàn)場(chǎng)進(jìn)行救援，對(duì)自身的生命安全也會(huì)構(gòu)成極大的威脅。救援機(jī)器人以其體積小、靈活性高等諸多優(yōu)點(diǎn)在事故救援中被廣泛應(yīng)用，使緊迫的救援工作效率得以提高，并且有效防止了二次災(zāi)害事故對(duì)人員的傷害。

通過(guò)對(duì)事故現(xiàn)場(chǎng)障礙物距離[1]的感知，為機(jī)器人的導(dǎo)航、避礙等功能提供依據(jù)，這就需要對(duì)事故發(fā)生后的非結(jié)構(gòu)化環(huán)境[2]進(jìn)行環(huán)境建模，即在未知環(huán)境中準(zhǔn)確判斷障礙物位置信息，實(shí)現(xiàn)安全無(wú)礙的行走。對(duì)于救援機(jī)器人來(lái)說(shuō)，環(huán)境建模的方法有很多，大多都是復(fù)雜且繁瑣的，通過(guò)將實(shí)際的救援現(xiàn)場(chǎng)環(huán)境轉(zhuǎn)換為圖的表達(dá)形式，進(jìn)而可以抽象的對(duì)救援現(xiàn)場(chǎng)的骨架進(jìn)行描述和分析。對(duì)于非結(jié)構(gòu)化環(huán)境建模的描述方法，主要有可視圖法、自由空間法等對(duì)救援環(huán)境建模描述。

文獻(xiàn)[3]提出一種可視圖法，對(duì)于規(guī)劃的機(jī)器人必須能等效地看成一個(gè)可活動(dòng)的點(diǎn)，保證起始點(diǎn)、目標(biāo)點(diǎn)以及障礙物的各個(gè)頂點(diǎn)可以連接在一起，同時(shí)還要保證連接的線不會(huì)觸碰到障礙物，這就構(gòu)成了一張無(wú)向圖，即可視圖。該方法在計(jì)算最短路徑方面較為簡(jiǎn)便，但同時(shí)也伴隨著一個(gè)很明顯的缺點(diǎn)：當(dāng)環(huán)境空間改變了，如障礙物個(gè)數(shù)的改變、障礙物位置信息發(fā)生變化等，則需要再重新構(gòu)建一張新的圖，缺乏靈活性，并且在路徑規(guī)劃方面誤差較大，無(wú)法實(shí)現(xiàn)機(jī)器人在未知環(huán)境中安全無(wú)碰的行駛。

文獻(xiàn)[4]提出基于自由空間法的救援機(jī)器人搜索目標(biāo)路徑方法，將環(huán)境空間中的障礙物信息通過(guò)二維平面內(nèi)的多種凸多邊形表現(xiàn)出來(lái)，在二維平面內(nèi)實(shí)現(xiàn)連通圖，完成對(duì)環(huán)境空間的建模。優(yōu)勢(shì)很明顯，無(wú)論環(huán)境空間怎樣變化，也不需要重新構(gòu)圖，具有極大的靈活性。但是缺點(diǎn)也很明顯：障礙物的密集度越大程序越復(fù)雜，計(jì)算起來(lái)非常麻煩，所以自由空間法只能做到粗略的估計(jì)路程情況，無(wú)法做到精確計(jì)算。

本文綜合考慮救援現(xiàn)場(chǎng)非結(jié)構(gòu)化環(huán)境的特點(diǎn)，提出了在未知的事故環(huán)境中構(gòu)建柵格地圖，采用拓?fù)浞椒▽?duì)地圖進(jìn)行處理，將救援機(jī)器人搜索到的障礙物信息點(diǎn)自動(dòng)生成柵格Voronoi圖，得到準(zhǔn)確的障礙物位置信息，對(duì)機(jī)器人的行駛路徑進(jìn)行分析，準(zhǔn)確判斷出障礙物位置，減少路徑規(guī)劃誤差，為機(jī)器人提供有效的路徑分析，保證機(jī)器人安全無(wú)碰行駛。

2 柵格地圖算法占有率計(jì)算

由于柵格地圖簡(jiǎn)單易懂，能直觀的表達(dá)環(huán)境空間信息和信息數(shù)據(jù)易存儲(chǔ)等特點(diǎn)，被廣泛應(yīng)用于機(jī)器人搜索目標(biāo)路徑導(dǎo)航、安全無(wú)礙行駛、位姿估計(jì)等方面[5]，尤其是處理激光雷達(dá)采集到的數(shù)據(jù)方面，更起著不可或缺的作用。柵格地圖，顧名思義，是把整個(gè)全局環(huán)境空間劃分為若干個(gè)大小相等的小柵格，并對(duì)每個(gè)小柵格的占有概率賦予概率值。當(dāng)機(jī)器人的位姿發(fā)生變化時(shí)，已經(jīng)保存在系統(tǒng)中的局部環(huán)境地圖也會(huì)隨之發(fā)生變化。雖然環(huán)境空間過(guò)大或者對(duì)機(jī)器人位姿準(zhǔn)確度要求過(guò)高，柵格地圖自身在運(yùn)算時(shí)占用了大量的存儲(chǔ)空間和計(jì)算時(shí)間，但是對(duì)于救援現(xiàn)場(chǎng)這種非結(jié)構(gòu)化環(huán)境來(lái)說(shuō)，采用柵格地圖算法環(huán)境建模也是可取的[6]。

柵格地圖創(chuàng)建算法采用貝葉斯濾波器計(jì)算每一個(gè)小柵格占用的后驗(yàn)概率，若機(jī)器人所在柵格的坐標(biāo)為(x，y)，mx，y為該柵格的占有概率，每個(gè)柵格有空閑或占用兩種狀態(tài)。假設(shè)事件x擁有的概率p(x)的概率定義為p(x)/[1-p(x)]，在已知位姿s′、觀測(cè)點(diǎn)z′的前提下，利用二元貝葉斯濾波器可得靜態(tài)地圖的工作原理公式如下：

(1)

加法運(yùn)算計(jì)算起來(lái)比乘法運(yùn)算更簡(jiǎn)單便捷，還可以避免乘法運(yùn)算的不穩(wěn)定性，所以通常會(huì)采用對(duì)數(shù)的形式

(2)

占有柵格地圖只有接收到傳感器傳來(lái)允許更新柵格單元狀態(tài)時(shí)才可以對(duì)數(shù)據(jù)進(jìn)行更新，否則不更新。占有柵格地圖在創(chuàng)建時(shí)要有2個(gè)概率密度：p(mx，y|zt，st)和p(mx，y)。p(mx，y)表示占有柵格的后驗(yàn)概率，p(mx，y|zt，st)表示逆向傳感器模型在采集障礙物數(shù)據(jù)信息zt時(shí)，指定位姿st在柵格元素mx，y的占有概率。表1為地圖靜態(tài)時(shí)逆向傳感器采集到的占有概率，表2為地圖在動(dòng)態(tài)狀態(tài)下逆向傳感器采集到的占有概率，為獲取救援現(xiàn)場(chǎng)環(huán)境信息提供依據(jù)。

表1 靜態(tài)地圖逆向傳感器

表2 動(dòng)態(tài)地圖逆向傳感器

3 柵格地圖下目標(biāo)路徑環(huán)境快速建模

3.1 救援現(xiàn)場(chǎng)環(huán)境信息獲取

現(xiàn)場(chǎng)環(huán)境信息采集設(shè)備選用URG-04LX機(jī)器人專用暗光條件激光雷達(dá)，它可采集240°內(nèi)69個(gè)方向上的不同障礙物的距離數(shù)據(jù)，篩選出有效信息。通過(guò)對(duì)比雷達(dá)所探測(cè)到的最大距離信息與機(jī)器人可通過(guò)的最窄寬度，可知角度分辨率為3.64°。如圖1所示，以機(jī)器人為中心，可獲得由不同方向上障礙物的位置信息構(gòu)成的扇形平面。

圖1 救援機(jī)器人建立柵格地圖

救援機(jī)器人在全局現(xiàn)場(chǎng)中的位置坐標(biāo)為(x，y)，航向角為γ，可以得到當(dāng)前某一方向上障礙物的位置坐標(biāo)為(Xo，Yo)，在局部環(huán)境空間中可用式(2)來(lái)表示，其中，φ=3.64°n，表示救援機(jī)器人的行走方向與第n個(gè)方向之間所形成的夾角。

3.2 障礙物地圖拓?fù)浞治?/h3>

便于救援機(jī)器人更好的了解救援現(xiàn)場(chǎng)環(huán)境，還需要對(duì)機(jī)器人下達(dá)指令，分析在當(dāng)前環(huán)境中基于全局搜索目標(biāo)的可行駛路徑[7]。

1)在對(duì)可形式路徑分析之前首先需要對(duì)地圖上的障礙物信息值進(jìn)行膨脹運(yùn)算。這樣做的優(yōu)勢(shì)在于一方面可以使機(jī)器人在遇到障礙物時(shí)減速運(yùn)行，產(chǎn)生一段緩沖距離，避免機(jī)器人碰撞到障礙物；另一方面使離散的障礙物信息隨著膨脹的過(guò)程形成連接在一起的一體障礙物，減少離散數(shù)據(jù)[8]在柵格地圖上對(duì)障礙物的表達(dá)準(zhǔn)確率誤差。

2)然后需要對(duì)膨脹后的柵格地圖上的障礙物圖塊進(jìn)行分割運(yùn)算并賦值。依據(jù)在柵格地圖上搜索到的障礙物圖塊的先后順序?qū)φ系K物賦值，賦值為m，m∈N。

3)最后對(duì)分割好并賦值的地圖采用基于柵格主動(dòng)生長(zhǎng)Voronoi圖做可行駛路徑規(guī)劃。連接各鄰點(diǎn)之間的直線構(gòu)成一個(gè)多邊形，得到傳統(tǒng)Voronoi圖，然后與沒(méi)有連接在一起的柵格單元相結(jié)合，障礙物柵格主動(dòng)生長(zhǎng)，最后得到柵格地圖的Voronoi邊。只要救援機(jī)器人在Voronoi邊上行走，那么與障礙物發(fā)生碰撞的幾率幾乎為0。

柵格主動(dòng)生長(zhǎng)是將救援機(jī)器人搜索到的柵格地圖上的所有障礙物信息點(diǎn)看成原始的核，在向周圍柵格等速擴(kuò)大的同時(shí)，占領(lǐng)空白柵格，稱為胚，在遇到非自由柵格時(shí)停止運(yùn)行。這時(shí)柵格地圖中不存在空白柵格，已全部被占領(lǐng)，自動(dòng)生成柵格Voronoi圖，生長(zhǎng)原理如圖2所示。

圖2 柵格Voronoi圖原理

通過(guò)以上對(duì)障礙物地圖拓?fù)浞治?，可知基于主?dòng)生長(zhǎng)的Voronoi圖生成過(guò)程如下：

步驟1：建立空集胚元素表D、核元素表S。

步驟2：添加核元素，將搜索到的所有值為0的鄰接?xùn)鸥袢糠湃氡鞸中。

步驟3：檢索障礙柵格，將所有值為0的鄰接?xùn)鸥袢糠湃氡鞤中，即為胚。

步驟4：將表D中所有胚的鄰接?xùn)鸥裰等吭O(shè)置為原始胚值，并將表D置入表S中。

步驟5：全面檢查新生長(zhǎng)的鄰接?xùn)鸥裰?，將在生長(zhǎng)過(guò)程中發(fā)生改變的鄰接?xùn)鸥裰捣湃氡鞤中，同時(shí)更新表D中的胚值，將原胚元素刪除。

步驟6：如果表S中存在空位柵格，重復(fù)步驟3操作，如果沒(méi)有空位柵格，即可輸出。

步驟7：分析全部鄰接?xùn)鸥裰担@取需要的Voronoi邊和節(jié)點(diǎn)。

3.3 建立地圖模型

對(duì)于事故現(xiàn)場(chǎng)這種規(guī)模比較大的環(huán)境來(lái)說(shuō)，在使用柵格地圖進(jìn)行環(huán)境建模時(shí)，應(yīng)充分控制好柵格地圖規(guī)模不易較大。在機(jī)器人進(jìn)行救援工作之前，還要分析自身位置與事故現(xiàn)場(chǎng)中障礙物位置，那么就需要對(duì)柵格地圖做預(yù)處理、明確障礙物位置信息、提取出可行駛路徑。采用拓?fù)浞椒▽?duì)地圖進(jìn)行預(yù)處理，對(duì)全局環(huán)境信息[9]做更全面的描述，減少離散數(shù)據(jù)，保證整個(gè)環(huán)境空間的連續(xù)性。

構(gòu)建柵格地圖首先需要構(gòu)建度量信息。每個(gè)柵格單元的大小，即柵格地圖的分辨率定義為g×g(g>0)，g代表事故現(xiàn)場(chǎng)環(huán)境與柵格地圖單元所對(duì)應(yīng)的度量值。局部范圍內(nèi)柵格地圖的大小可定義為w×l，w，l∈N，其中，w表示柵格地圖橫坐標(biāo)最大值，l表示柵格地圖縱坐標(biāo)最大值。已知救援機(jī)器人所在柵格坐標(biāo)為(x，y)，其中00，z∈N。任意柵格(p1，p2)的距離定義為4-鄰接?xùn)鸥窬嚯x，運(yùn)算過(guò)程如式(4)所示。這種柵格距離在柵格圖形上的表示方法都是非負(fù)整數(shù)，在計(jì)算過(guò)程中更加容易表達(dá)，完全不同于歐幾里德柵格距離表示方法。

d(p1，p2)=|x1-x2|+|y1-y2|

(3)

其中，p1坐標(biāo)為(x1，y1)，p2坐標(biāo)為(x2，y2)。定義柵格地圖上2個(gè)柵格(p1，p2)之間的拓?fù)潢P(guān)系，有相鄰和相離兩種。

相鄰的柵格拓?fù)潢P(guān)系從度量的角度可定義為

[(x1=x2)∧(y1=y2)]∨[(x1≠x2)∧(y1≠y2)]

(4)

相離的柵格拓?fù)潢P(guān)系可定義為

(x1≠x2)∧(y1≠y2)

(5)

通過(guò)定義柵格之間的拓?fù)潢P(guān)系，即可對(duì)膨脹、腐蝕兩種柵格拓?fù)溥\(yùn)算作出定義。

膨脹運(yùn)算：設(shè)定柵格p膨脹后的值為z，經(jīng)過(guò)膨脹運(yùn)算后可使該柵格單元的4-鄰接?xùn)鸥裰等刻鎿Q為z，即

{p(x-1，y)，p(x+1，y)，p(x，y+1)，p(x，y-1)}=z

(6)

腐蝕運(yùn)算：與膨脹運(yùn)算恰恰相反，運(yùn)算過(guò)程為膨脹運(yùn)算的反過(guò)程，最終結(jié)果可使該柵格的4-鄰接?xùn)鸥裰等坎坏扔趜，即

{p(x-1，y)，p(x+1，y)，p(x，y+1)，p(x，y-1)}=0

(7)

(8)

隨著機(jī)器人在工作區(qū)域內(nèi)行走獲取環(huán)境信息的同時(shí)，柵格地圖也會(huì)隨之不斷擴(kuò)展，這樣下去勢(shì)必會(huì)出現(xiàn)計(jì)算資源不足的情況。這就需要人為干預(yù)[10]柵格地圖的規(guī)模。設(shè)定一個(gè)矩形范圍，只允許機(jī)器人在此范圍內(nèi)行走，超出部分將不會(huì)被記錄。具體設(shè)定范圍大小應(yīng)根據(jù)實(shí)際救援現(xiàn)場(chǎng)區(qū)域的寬窄而定。

4 仿真研究

為了驗(yàn)證所提方法的可行性，進(jìn)行仿真測(cè)試。實(shí)驗(yàn)環(huán)境為6m×9m的環(huán)境空間，如圖3所示。機(jī)器人采用頻率2.4GHz的CPU及Windows2000系統(tǒng)。機(jī)器人在實(shí)驗(yàn)環(huán)境空間中行走，通過(guò)分析障礙物位置信息，可得出一條在當(dāng)前環(huán)境中安全無(wú)礙的可行駛路徑。實(shí)驗(yàn)中柵格拓?fù)涞貓D模型用60×90的柵格元素來(lái)表現(xiàn)，分辨率設(shè)為0.1m。

對(duì)實(shí)驗(yàn)環(huán)境空間進(jìn)行膨脹運(yùn)算，可得出當(dāng)前實(shí)驗(yàn)環(huán)境的柵格地圖，如圖3(a)所示。圖3(b)表示經(jīng)過(guò)分割運(yùn)算后，用不同的灰度值表示不同的障礙物位置信息；如圖3(c)表示障礙物柵格Voronoi圖。將圖3(c)進(jìn)行障礙物位置信息提取，可以得到機(jī)器人行走過(guò)的環(huán)境空間內(nèi)的可行駛路徑網(wǎng)絡(luò)，如圖3(d)所示。

圖3 實(shí)驗(yàn)環(huán)境建模與分析

在此實(shí)驗(yàn)測(cè)試中，環(huán)境地圖的更新及Voronoi圖分析時(shí)間較迅速，能夠通過(guò)提取障礙物位置信息得到障礙物柵格Voronoi圖，由此規(guī)劃?rùn)C(jī)器人行走過(guò)的環(huán)境空間內(nèi)的可行駛路徑準(zhǔn)確避開了障礙物位置，有效提高了救援機(jī)器人搜索目標(biāo)路徑環(huán)境規(guī)劃的準(zhǔn)確率及效率。

5 結(jié)論

本文提出了一種救援機(jī)器人在搜索目標(biāo)過(guò)程中路徑環(huán)境快速建模方法。通過(guò)分析柵格拓?fù)涞貓D上的Voronoi方法，對(duì)事故現(xiàn)場(chǎng)這種非結(jié)構(gòu)化環(huán)境進(jìn)行環(huán)境建模，可以得出救援機(jī)器人在環(huán)境空間內(nèi)安全無(wú)礙的可行駛路徑網(wǎng)絡(luò)。通過(guò)仿真測(cè)試得到結(jié)論如下：

1)所提方法能夠迅速得出環(huán)境地圖的更新及Voronoi圖分析結(jié)果，有效提升了救援機(jī)器人搜索目標(biāo)路徑效率。

2)通過(guò)障礙物柵格Voronoi圖得到的機(jī)器人可行駛路徑準(zhǔn)確避開了障礙物位置，救援機(jī)器人搜索目標(biāo)路徑環(huán)境規(guī)劃準(zhǔn)確率較高。

本文所提環(huán)境建模方法對(duì)救援機(jī)器人在救援現(xiàn)場(chǎng)工作提供了精確的地圖環(huán)境模型，并具有極高的適應(yīng)能力和較好的路徑規(guī)劃效率。但是本文方法在建立模型地圖規(guī)模方面還存在一定的局限性，以后將擴(kuò)大地圖規(guī)模作為主要的研究重點(diǎn)。