本發(fā)明涉及智能機(jī)器人運(yùn)動(dòng)規(guī)劃領(lǐng)域內(nèi)的一種路徑規(guī)劃算法，具體是一種針對(duì)地形復(fù)雜的地圖下Visual Graph算法的改進(jìn)，使其不僅可以應(yīng)用在凸多邊形障礙物的地圖下，也可以運(yùn)用在復(fù)雜的地圖下。

背景技術(shù)：

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

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

Visual Graph也是常用的路徑規(guī)劃算法，但是其應(yīng)用對(duì)地圖有嚴(yán)格的限制，要求地圖的障礙物要是凸多邊形，并且各個(gè)障礙物的頂點(diǎn)坐標(biāo)已知，隨后連接各個(gè)障礙物的頂點(diǎn)，并以這些連線為載體，生成路徑。但是在實(shí)際應(yīng)用場(chǎng)景中，地形是十分復(fù)雜的，地圖中的障礙物形狀是不規(guī)則的，而且其頂點(diǎn)坐標(biāo)也是未知的，因此要直接利用Visual Graph算法是不可行的?，F(xiàn)行的解決該問(wèn)題的方法為，首先對(duì)地圖進(jìn)行預(yù)處理，使用形態(tài)學(xué)方法，如分水嶺算法、特征匹配算法等，可以提取地圖中的特征點(diǎn)或者障礙物的邊界，獲得障礙物的大致形狀，并以直線連接各個(gè)特征點(diǎn)，將障礙物轉(zhuǎn)化為一個(gè)凸多邊形，再應(yīng)用Visual Graph算法。這種方法的計(jì)算量不小，涉及的額外算法多，效率很難估算，而且在其將障礙物轉(zhuǎn)化到凸多邊形時(shí)，會(huì)對(duì)障礙物的邊緣進(jìn)行一定程度的膨脹或腐蝕，可能造成地圖中原本不是障礙物的區(qū)域成為障礙物，原本是障礙物的區(qū)域成為可通過(guò)，可能使得最終得到的路徑非法。

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

本發(fā)明的技術(shù)解決問(wèn)題：克服了原始Visual Graph對(duì)地圖的硬性要求，提出一種在復(fù)雜地圖下改進(jìn)Visual Graph的方法，使其不再僅限于凸多邊形障礙物的地圖，在復(fù)雜的SLAM地圖下也可以運(yùn)行，并且無(wú)需對(duì)地圖做預(yù)處理，節(jié)點(diǎn)的密度參數(shù)也很容易調(diào)整，以生成精確的路徑。

本發(fā)明的技術(shù)解決方案：一種在復(fù)雜地圖下改進(jìn)Visual Graph的方法，其特征在于：能夠使Visual Graph不僅僅應(yīng)用于凸多邊形障礙物的地圖，對(duì)一般的SLAM地圖也可以適用，其步驟如下：

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

步驟2：針對(duì)轉(zhuǎn)化后的地圖，給定起點(diǎn)和終點(diǎn)，將起點(diǎn)和終點(diǎn)儲(chǔ)存在一個(gè)順序表中，并從起點(diǎn)和終點(diǎn)開(kāi)始發(fā)射光線，開(kāi)始建立光路圖，此過(guò)程稱(chēng)為初始化過(guò)程；

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

步驟4：從保留的節(jié)點(diǎn)開(kāi)始，重新發(fā)射光線，并重復(fù)步驟3的過(guò)程，此過(guò)程稱(chēng)為重復(fù)光線發(fā)射過(guò)程；

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

步驟6：在建好的拓?fù)涞貓D上應(yīng)用A*尋路算法，即可獲得一條連接起點(diǎn)和終點(diǎn)，以光路為載體的路徑，此過(guò)程稱(chēng)為尋路過(guò)程。

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

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

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

(1)對(duì)于某一點(diǎn)，從0度，即水平向右方向開(kāi)始每隔一個(gè)固定的步長(zhǎng)發(fā)射光線，直到360度為止；

(2)對(duì)于每個(gè)方向，光線逐像素向前推進(jìn)，每推進(jìn)一個(gè)像素，將會(huì)檢測(cè)該點(diǎn)是否為可通過(guò)點(diǎn)，若可通過(guò)則繼續(xù)推進(jìn)，若不可通過(guò)則表示碰到障礙物，記錄該點(diǎn)，進(jìn)入步驟3的判斷過(guò)程；

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

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

設(shè)置兩個(gè)順序表，一個(gè)順序表allnode儲(chǔ)存所有的節(jié)點(diǎn)，即已經(jīng)發(fā)射過(guò)光線與尚未發(fā)射光線的所有節(jié)點(diǎn)，另一個(gè)順序表open儲(chǔ)存尚未發(fā)射光線的節(jié)點(diǎn)。初始化時(shí)，將起點(diǎn)和終點(diǎn)都存入兩個(gè)順序表中。

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

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

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

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

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

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

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

(1)本發(fā)明沿用原始算法的光線直線傳播思路，但突破了原始算法只能應(yīng)用于凸多邊形障礙物地圖的局限。在復(fù)雜地圖中，以光線和障礙物表面的碰撞點(diǎn)作為節(jié)點(diǎn)，即類(lèi)似于凸多邊形的頂點(diǎn)，并從該節(jié)點(diǎn)發(fā)射新的光線來(lái)生成更多的節(jié)點(diǎn)，遍布整張地圖的障礙物表面，并以光路為載體形成路徑，對(duì)地圖本身沒(méi)有任何要求；

(2)本發(fā)明無(wú)需對(duì)地圖進(jìn)行預(yù)處理，計(jì)算量極小。與現(xiàn)有的改進(jìn)Visual Graph的方法相比，現(xiàn)有技術(shù)需要額外設(shè)計(jì)算法來(lái)對(duì)地圖進(jìn)行預(yù)處理，以生成一張凸多邊形地圖來(lái)適應(yīng)原始的Visual Graph算法，增加了非常巨大的計(jì)算量。而本發(fā)明對(duì)地圖沒(méi)有任何要求，即無(wú)需另外設(shè)計(jì)算法對(duì)地圖進(jìn)行預(yù)處理來(lái)生成凸多邊形障礙物地圖，直接對(duì)原地圖進(jìn)行操作，省去大量計(jì)算量，提高了運(yùn)行效率；

(3)本發(fā)明的各項(xiàng)閾值參數(shù)都十分容易調(diào)整，與現(xiàn)有的技術(shù)方法相比，現(xiàn)有技術(shù)需要對(duì)障礙物表面進(jìn)行膨脹或腐蝕，使障礙物的形狀變得規(guī)則均勻，以此適應(yīng)原始的Visual Graph算法，所得的路徑常常出現(xiàn)穿越障礙物的非法情況。而本發(fā)明通過(guò)調(diào)整閾值參數(shù)可以得到一條更精確、合法的路徑，無(wú)需對(duì)障礙物的邊緣腐蝕或膨脹，不會(huì)導(dǎo)致地圖中的障礙物點(diǎn)被腐蝕為可通過(guò)點(diǎn)，可通過(guò)點(diǎn)被膨脹為障礙物點(diǎn)，所得的路徑精確而且合法。

附圖說(shuō)明

圖1為原始Visual Graph算法效果；

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

圖3為本發(fā)明效果圖。

具體實(shí)施方式

本發(fā)明一般應(yīng)用于slam算法建立的地圖中，即是實(shí)際地形，較為復(fù)雜。

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

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

步驟2：給定起點(diǎn)和終點(diǎn)，若已經(jīng)由SLAM等定位算法定位機(jī)器人所在的位置，則起點(diǎn)為機(jī)器人當(dāng)前所在點(diǎn)，終點(diǎn)則是目標(biāo)點(diǎn)。首先將起點(diǎn)和終點(diǎn)置入一個(gè)名為open的順序表中，作為待發(fā)射光線的節(jié)點(diǎn)，同時(shí)將它們存入一張名為allnode的順序表，allnode是用來(lái)儲(chǔ)存所有節(jié)點(diǎn)的順序表。

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

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

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

步驟6：當(dāng)某一光線360度光線發(fā)射結(jié)束后均無(wú)法生成新的節(jié)點(diǎn)，則從這個(gè)時(shí)候開(kāi)始，open表的內(nèi)容將會(huì)減少，當(dāng)open表中所有的節(jié)點(diǎn)都無(wú)法創(chuàng)建新的節(jié)點(diǎn)時(shí)，open表將為空，此時(shí)表明建圖完成，已獲得一張拓?fù)涞貓D，其頂點(diǎn)為光線發(fā)射點(diǎn)，其邊為節(jié)點(diǎn)與節(jié)點(diǎn)之間的光線連接；

步驟7：在所得的拓?fù)涞貓D上應(yīng)用A*尋路算法，A*的啟發(fā)式代價(jià)設(shè)置為節(jié)點(diǎn)之間光線的距離，從起點(diǎn)開(kāi)始，依照A*的啟發(fā)式規(guī)則，到終點(diǎn)為止，即可獲得連接起點(diǎn)和終點(diǎn)的一條以光路為載體的路徑。

如圖1，是未改進(jìn)前的Visual Graph路徑規(guī)劃算法，其應(yīng)用的地圖只能是如圖所示的，障礙物均為凸多邊形的地圖，而且每個(gè)障礙物的頂點(diǎn)都需要已知，將起點(diǎn)和終點(diǎn)并障礙物的頂點(diǎn)互相之間建立連接，則可以以連接為載體構(gòu)成一條從起點(diǎn)到終點(diǎn)的連通路徑。路徑的優(yōu)點(diǎn)在于直線度好，拐彎次數(shù)少，因而機(jī)器人運(yùn)行時(shí)的速度損失小，但由于其只能運(yùn)用在簡(jiǎn)單的凸多邊形障礙物地圖上，故很難應(yīng)用在實(shí)際問(wèn)題中。

如圖3，是本發(fā)明改進(jìn)以后的Visual Graph方法效果，圖中的圓點(diǎn)表示光線發(fā)射節(jié)點(diǎn)，細(xì)線表示光路，而粗實(shí)線表示最終獲得的路徑。本發(fā)明繼承了原有方法的優(yōu)點(diǎn)，即拐彎次數(shù)較少，機(jī)器人的速度損失小，并且本發(fā)明克服了原有算法致命的缺點(diǎn)，使其可以應(yīng)用在復(fù)雜地圖中，而且無(wú)需對(duì)地圖進(jìn)行預(yù)處理操作，使其在實(shí)際問(wèn)題中的應(yīng)用前景大大的提高。