本發(fā)明涉及智能機(jī)器人運(yùn)動規(guī)劃領(lǐng)域內(nèi)的一種輔助操作算法，具體是提取二維slam地圖的障礙物邊界信息，以適應(yīng)部分路徑規(guī)劃算法的需求。

背景技術(shù)：

當(dāng)下能夠自主導(dǎo)航避障的機(jī)器人系統(tǒng)研究異?；馃?，而且也已經(jīng)有許多產(chǎn)品應(yīng)用于實踐。如餐館的服務(wù)機(jī)器人，就擁有障礙物識別，路徑規(guī)劃的能力；又如家庭掃地機(jī)器人，也擁有在開闊空間內(nèi)規(guī)劃自身運(yùn)動路徑的能力。在公共運(yùn)輸方面，無人機(jī)快遞投遞的應(yīng)用已經(jīng)開展的如火如荼，在快遞投遞上也必須用到路徑規(guī)劃，以使無人機(jī)能夠在避開障礙物的前提下以最短的路徑到達(dá)目的地并完成投遞。

路徑規(guī)劃即是指機(jī)器人決策如何從地圖的某一點運(yùn)動到另一點的能力。首先要求機(jī)器人能夠獲得當(dāng)前環(huán)境的地圖信息，并能定位當(dāng)前自身的位置，隨后才可以進(jìn)行路徑規(guī)劃，定位和建圖的算法目前最實用的就是slam算法。目前有許多路徑規(guī)劃算法，如rrt、prm等。

visualgraph也是常用的路徑規(guī)劃算法，其主要應(yīng)用了光線沿直線傳播這一思想，利用模擬光線發(fā)射，與障礙物相撞得到碰撞點，再以碰撞點為新的發(fā)射節(jié)點重新發(fā)射光線，重復(fù)此過程以建立一張以光路為載體的拓?fù)鋱D。

在部分的路徑規(guī)劃算法或者規(guī)劃問題中，常常需要機(jī)器人準(zhǔn)確獲得障礙物邊界的信息。如在1979年最初提出visualgraph算法時，要求障礙物都是凸多邊形，并以凸多邊形頂點之間的連線為載體建立拓?fù)鋱D。在一般的slam地圖上這是無法實現(xiàn)的，這就需要對地圖進(jìn)行預(yù)處理，將復(fù)雜的地形轉(zhuǎn)化成凸多邊形的形式。在這之前，首先就要進(jìn)行的就是障礙物邊界的提取，再對邊界進(jìn)行模糊操作，得到一系列的凸多邊形。這是障礙物邊界提取的一個應(yīng)用場景，又如在路徑規(guī)劃語義地圖的語義賦予和區(qū)域分割時，需要應(yīng)用一些形態(tài)學(xué)算法對地圖進(jìn)行處理，這些形態(tài)學(xué)的算法首先要得到障礙物的形狀，再進(jìn)一步進(jìn)行操作，而這首先就需要提取障礙物的邊界信息，以此獲得障礙物的形狀。

現(xiàn)有的常用于障礙物邊界提取的算法有微分算子法、分水嶺算法等，主要是直接對圖像進(jìn)行操作，計算圖像的灰度梯度，探尋其中灰度變化劇烈的區(qū)域，就視為障礙物邊界并予以提取。其方法是屬于圖像處理領(lǐng)域，許多都十分復(fù)雜，難以領(lǐng)會也難以實踐出來，在特定的問題上很難進(jìn)行相應(yīng)的優(yōu)化，提取的精度很難人為地操作。若要在路徑規(guī)劃問題中使用算法的結(jié)果還需要經(jīng)過頗多轉(zhuǎn)化，導(dǎo)致其執(zhí)行效率比較低。

技術(shù)實現(xiàn)要素：

本發(fā)明的技術(shù)解決問題：克服現(xiàn)有技術(shù)的不足，提供一種應(yīng)用visualgraph算法提取障礙物邊界的方法，應(yīng)用于二維的簡單地圖和slam地圖的障礙物邊界提取，不僅可以提取孤立障礙物的邊界，對于墻壁、通道等占地面積廣大的障礙物邊界也能實現(xiàn)很好的提取。

本發(fā)明的技術(shù)解決方案：一種應(yīng)用visualgraph算法提取障礙物邊界的方法，其特點在于：在二維的簡單地圖或slam地圖下應(yīng)用，可以有效提取障礙物的邊界信息，其步驟如下：

步驟1：對地圖進(jìn)行轉(zhuǎn)化，將灰度圖或者slam所構(gòu)建的地圖二值化處理，得到轉(zhuǎn)化后的地圖，此過程稱為地圖二值化過程；

步驟2：針對轉(zhuǎn)化后的地圖，在地圖上的可通行域隨機(jī)取一點儲存在順序表中，并從此開始發(fā)射光線，此過程稱為初始化過程；

步驟3：當(dāng)光線碰觸障礙物時，記錄該碰撞點的坐標(biāo)，并與順序表中已有的節(jié)點求取距離，若距離小于一個閾值則舍棄該點，否則保留，該過程稱為節(jié)點儲存判定過程；

步驟4：從保留的節(jié)點開始，重新發(fā)射光線，并重復(fù)步驟3的過程，此過程稱為重復(fù)光線發(fā)射過程；

步驟5：當(dāng)所有光線碰撞障礙物都無法生成新的節(jié)點時，判斷建圖結(jié)束；

步驟6：將所有的碰撞節(jié)點進(jìn)行連接，最終輸出障礙物的邊界信息，此過程稱為連接過程。

所述步驟1中，地圖的二值化的方法為：

調(diào)用opencv庫讀入所給的地圖文件，并將地圖矩陣轉(zhuǎn)化為一個二維整形數(shù)組，即表示二維平面空間，數(shù)組中的每一個元素代表地圖上相應(yīng)坐標(biāo)的一個像素點，若該點為障礙物點，則將數(shù)組值設(shè)為1，若該點為可通過點，則將數(shù)組值設(shè)為0。

所述步驟2中，發(fā)射光線的方法為：

(1)對于某一點，從0度，即水平向右方向開始每隔一個固定的步長發(fā)射光線，直到360度為止；

(2)對于每個方向，光線逐像素向前推進(jìn)，每推進(jìn)一個像素，將會檢測該點是否為可通過點，若可通過則繼續(xù)推進(jìn)，若不可通過則表示碰到障礙物，記錄此時碰到障礙物的坐標(biāo)點，進(jìn)入步驟3的判斷過程；

(3)若某一方向的光線距離過小，則表示該方向與障礙物十分貼近，是不合理的，因此若光線距離小于某一閾值，則該方向的光線將被舍棄。

所述步驟2中，儲存節(jié)點的方法為：

設(shè)置兩個順序表，一個順序表allnode儲存所有的節(jié)點，即已經(jīng)發(fā)射過光線與尚未發(fā)射光線的所有節(jié)點，另一個順序表open儲存尚未發(fā)射光線的節(jié)點；初始化時，隨機(jī)選取的點加入open表中。

所述步驟3中，節(jié)點儲存的判定方法為：

當(dāng)光線碰到障礙物時，將該碰撞點與allnode中的所有節(jié)點求取距離，若該距離小于某一閾值，則舍棄該點，否則將該點加入allnode和open表中。

所述步驟4中，重復(fù)光線發(fā)射的方法為：

取出open表頭的一個元素，重復(fù)步驟2所示的光線發(fā)射過程。

所述步驟5中，建圖結(jié)束的判斷方法為：

當(dāng)某一節(jié)點發(fā)射的光線與障礙物的碰撞點與已有點的距離均小于閾值時，則該節(jié)點不會生成新的節(jié)點，此后再從open表中取出一個點進(jìn)行光線發(fā)射，此時open表的元素個數(shù)將會減少，當(dāng)open表為空時，即節(jié)點生成過程結(jié)束。

所述步驟6中，連接過程的連接方法為：

從allnode順序表的表頭開始，每一個元素都與其后的所有元素求取距離，若其距離小于某一閾值，則將這兩個節(jié)點進(jìn)行連接。該閾值與上述建圖步驟3中的閾值有相關(guān)性，此處閾值應(yīng)略大于步驟3中的閾值，以達(dá)到較好的效果。順序表中所有元素處理完后，連接過程即結(jié)束。

本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點在于：

(1)本發(fā)明創(chuàng)新性地使用了visualgraph算法的主要思路來進(jìn)行障礙物邊界的提取，采用光線碰撞障礙物獲取碰撞點，并建立碰撞點之間的連接關(guān)系，最終得到障礙物邊界的連接關(guān)系。因此可以適應(yīng)不同復(fù)雜程度的地圖，不論是孤立的障礙物還是長直通道、墻壁，都可以實現(xiàn)較好的邊界提?。?/p>

(2)本發(fā)明是對地圖的一種預(yù)處理操作，是為后續(xù)的路徑規(guī)劃提供很好的支持，可以根據(jù)障礙物的邊界信息來選擇使用不同的路徑規(guī)劃算法，或者某些路徑規(guī)劃算法能夠使用障礙物的邊界信息進(jìn)行更好的路徑規(guī)劃；

(3)本發(fā)明的障礙物邊界提取精度是可調(diào)的，精細(xì)或者粗糙的提取，可以通過上述步驟中的閾值調(diào)整來進(jìn)行非常方便的調(diào)整。

附圖說明

圖1為visualgraph算法節(jié)點生成圖；

圖2為本發(fā)明方法的流程圖；

圖3為邊界連接圖。

具體實施方式

本發(fā)明一種應(yīng)用visualgraph算法提取障礙物邊界的方法，應(yīng)用visualgraph算法的主要思路，即光沿直線傳播，來進(jìn)行二維slam地圖障礙物邊界的提取。visualgraph是1979年提出的路徑規(guī)劃算法，模擬了光線沿直線傳播這一特性，本發(fā)明主要應(yīng)用該思路，從地圖的任意一點開始發(fā)射光線，光線碰到障礙物表面后停止，記錄該碰撞點并以該碰撞點為新的發(fā)射光線節(jié)點重新發(fā)射光線，最終覆蓋整個地圖，得到一系列障礙物邊界上的點，最終將其連接即可獲得障礙物的邊界信息，輸出節(jié)點坐標(biāo)和節(jié)點之間的連接圖。

如圖2所示，本發(fā)明具體實施步驟詳細(xì)說明如下：

步驟1：用slam算法或其他建圖算法獲取當(dāng)前所處環(huán)境的地圖信息，以.pgm格式的圖片傳入，調(diào)用opencv庫函數(shù)將其二值化，得到opencv的矩陣變量mat，再將其轉(zhuǎn)化成一個二維數(shù)組，其值為0或1,0表示可以通過，1表示存在障礙物，二值化過程完成；

步驟2：在地圖的可通行域，即值為0的區(qū)域，隨機(jī)選取一點，首先將將該點置入一個名為open的順序表中，作為待發(fā)射光線的節(jié)點，同時將它們存入一張名為allnode的順序表，allnode是用來儲存所有節(jié)點的順序表。

步驟3：當(dāng)open表非空時，從open表中取出一個點開始以起始發(fā)射角0度，即水平向右發(fā)射光線，在光線還沒有碰到障礙物時，光線逐像素向前推進(jìn)，每推進(jìn)一個像素點，都對下一個像素點進(jìn)行檢測，若可通過則繼續(xù)推進(jìn)，若不可通過則表示碰到障礙物，此時光線停止推進(jìn)，并記錄該碰撞點坐標(biāo)；

步驟4：對該碰撞點進(jìn)行檢測，若該碰撞點與光線發(fā)出點的距離小于某一閾值，則表示該方向與障礙物十分貼近，該方向的光線是不合理的，應(yīng)當(dāng)舍棄。若光線合理，則求取碰撞點與allnode表中所有元素的歐式距離，當(dāng)其距離小于某一閾值時，則該點舍棄，否則將該點作為新的預(yù)備發(fā)射節(jié)點，同時存入open表和allnode表的表尾，并將該節(jié)點與光線發(fā)射節(jié)點建立連接，連接的具體實現(xiàn)為兩個節(jié)點的結(jié)構(gòu)體中各自包含一個指針，令兩個對象所包含的指針互指，即實現(xiàn)節(jié)點連接，亦即拓?fù)鋱D中的邊；

步驟5：給發(fā)射角度一個步進(jìn)角度，重復(fù)光線發(fā)射的過程，并對光線與障礙物的碰撞點進(jìn)行步驟4的檢測。若角度到達(dá)360度則該點的光線發(fā)射結(jié)束，重復(fù)步驟3，重新從open表中取出一個點，重復(fù)進(jìn)行光線發(fā)射；

步驟6：當(dāng)某一光線360度光線發(fā)射結(jié)束后均無法生成新的節(jié)點，則從這個時候開始，open表的內(nèi)容將會減少，當(dāng)open表中所有的節(jié)點都無法創(chuàng)建新的節(jié)點時，open表將為空，此時表明節(jié)點生成完成，已獲得所有障礙物邊界碰撞點的信息，儲存在allnode順序表中；

步驟7：處理allnode中所有節(jié)點，從表頭開始，以此將其與其后的所有節(jié)點進(jìn)行比較，求取兩點之間的距離，若該距離小于某一閾值，則建立兩點之間的連接關(guān)系，可視化顯示為兩點之間用直線段進(jìn)行連接，調(diào)用opencv的line函數(shù)，繪制兩點之間的連線。當(dāng)所有的節(jié)點都經(jīng)過該操作后，就可以得到一個障礙物邊界圖，即如圖3所示。

如圖1，是經(jīng)過visualgraph算法在一張slam地圖上得到的節(jié)點以及其光路的示意圖。

如圖3，是節(jié)點生成之后連接的效果圖，可以看到障礙物邊界的提取效果還是比較好的。