本發(fā)明涉及智能機(jī)器人運(yùn)動(dòng)規(guī)劃領(lǐng)域內(nèi)的一種提取道路中線的方法，具體是一種應(yīng)用visualgraph算法思路來提取道路的中線，最終輸出道路中線的坐標(biāo)點(diǎn)以及坐標(biāo)點(diǎn)之間的連接關(guān)系。

背景技術(shù)：

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

一種優(yōu)秀的路徑規(guī)劃算法，必須保證機(jī)器人運(yùn)行過程中能夠有效地避開障礙物，快速準(zhǔn)確地到達(dá)目的地。而目前許多的路徑規(guī)劃算法都有一種行為特點(diǎn)，那就是“貼邊行走”。如經(jīng)典的規(guī)劃算法a*，它所生成的路徑往往是緊貼障礙物的邊緣的，這樣的話，一旦機(jī)器人的傳感器出現(xiàn)了一些問題，精度不夠高的話，機(jī)器人很可能就會(huì)與障礙物發(fā)生碰撞導(dǎo)致不可預(yù)料的后果。為了使機(jī)器人獲得的路徑能夠更加安全可靠，更加有效地避開障礙物，一般需要機(jī)器人能夠運(yùn)動(dòng)在道路的中線，這樣，即使機(jī)器人出現(xiàn)一些小的故障，偏離了原來的軌道，機(jī)器人也能夠不碰撞障礙物，行走安全合法的道路。

現(xiàn)有的道路中線提取辦法有voronoi圖構(gòu)造法，此方法是以數(shù)學(xué)圖形voronoi圖為理論基礎(chǔ)，通過構(gòu)造廣義voronoi圖來進(jìn)行道路中線的提取。但一般意義上的voronoi圖并不能表現(xiàn)出道路中線的信息，必須構(gòu)造廣義的voronoi圖，而廣義voronoi圖的構(gòu)造比較困難，一般采用擴(kuò)展障礙物邊界的方法，直到障礙物邊界在某處相遇，則記該處為道路中線。這就首先需要障礙物邊界提取技術(shù)的支持，擴(kuò)展障礙物的算法復(fù)雜度也相對(duì)較大，導(dǎo)致該方法不易實(shí)現(xiàn)且計(jì)算量過大。

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

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

本發(fā)明的技術(shù)解決問題：克服現(xiàn)有技術(shù)的不足，提供一種應(yīng)用visualgraph算法思路來提取道路中線的方法，最終可以輸出道路中線的節(jié)點(diǎn)坐標(biāo)以及節(jié)點(diǎn)之間的連接關(guān)系，并在地圖中繪出道路的中線，以供給機(jī)器人一條安全合適的路徑進(jìn)行行走。

本發(fā)明的技術(shù)解決方案：應(yīng)用visualgraph算法提取道路中線的方法，其特點(diǎn)在于：應(yīng)用visualgraph算法思路來提取道路中線，最終輸出道路中線的坐標(biāo)點(diǎn)以及其連接關(guān)系，其步驟如下：

步驟1：對(duì)地圖進(jìn)行轉(zhuǎn)化，將灰度圖或者slam所構(gòu)建的地圖二值化處理，得到轉(zhuǎn)化后的地圖，存儲(chǔ)為一個(gè)二維數(shù)組。此過程稱為地圖二值化過程；

步驟2：針對(duì)轉(zhuǎn)化后的二維數(shù)組，在可通過的區(qū)域開始遍歷所有像素點(diǎn)，并從每個(gè)可通行的像素點(diǎn)發(fā)射光線，每次發(fā)射兩條射線，起點(diǎn)為所選點(diǎn)，兩條射線成180°反向發(fā)射，此過程稱為光線發(fā)射過程；

步驟3：當(dāng)兩條射線碰觸障礙物時(shí)，記錄該碰撞點(diǎn)的坐標(biāo)，并計(jì)算碰撞點(diǎn)到光線發(fā)射起點(diǎn)的距離，若兩條射線碰撞點(diǎn)到起點(diǎn)的距離相等，則該點(diǎn)即為道路中點(diǎn)，該過程稱為判定過程；

步驟4：若兩條射線的碰撞點(diǎn)到起點(diǎn)的距離不相等，則將射線順時(shí)針旋轉(zhuǎn)30°再次以發(fā)射射線，重復(fù)步驟3的判定過程，該過程稱為重復(fù)發(fā)射光線過程；

步驟5：若某像素點(diǎn)發(fā)射射線已轉(zhuǎn)過180°而判定過程總不成立，則該點(diǎn)即不是道路中點(diǎn)，開始對(duì)下一個(gè)像素點(diǎn)進(jìn)行步驟2、3、4的過程；

步驟6：當(dāng)所有可通過的像素點(diǎn)均遍歷完成后，則道路中線的坐標(biāo)點(diǎn)即都已生成，道路中線經(jīng)過的點(diǎn)提取過程結(jié)束。

步驟7：為將道路中點(diǎn)連接行成道路中線，即對(duì)每一個(gè)上述過程獲得的點(diǎn)，將其與鄰近像素為1的所有點(diǎn)建立連接，即可獲得道路中點(diǎn)之間的連接關(guān)系，即得道路中線，該過程稱為連接過程。

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

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

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

(1)對(duì)于某一點(diǎn)，從0°和180°，即水平方向開始向兩邊發(fā)射射線，重復(fù)發(fā)射射線時(shí)，即將射線順時(shí)針旋轉(zhuǎn)30°，直到180°為止；

(2)對(duì)于每個(gè)方向，光線逐像素向前推進(jìn)，每推進(jìn)一個(gè)像素，將會(huì)檢測該點(diǎn)是否為可通過點(diǎn)，若可通過則繼續(xù)推進(jìn)，若不可通過則表示碰到障礙物，記錄此時(shí)碰到障礙物的坐標(biāo)點(diǎn)，當(dāng)兩條射線都碰到障礙物時(shí)，進(jìn)入步驟3的判定過程；

所述步驟2中，遍歷像素點(diǎn)的方法為：

從地圖的左上角開始直到右下角結(jié)束，水平掃描遍歷，若某點(diǎn)為不可通過，則直接跳過；若某點(diǎn)為可通過，則進(jìn)行光線發(fā)射過程；

所述步驟3中，判定過程的方法為：

當(dāng)遍歷過程中從某一像素點(diǎn)發(fā)出的兩向射線都碰到障礙物時(shí)，記錄兩個(gè)碰撞點(diǎn)，分別計(jì)算其與發(fā)射點(diǎn)的距離，若兩距離相等，則保留該像素點(diǎn)作為道路中點(diǎn)，結(jié)束該點(diǎn)的光線發(fā)射過程，否則按照步驟4，將光線順時(shí)針旋轉(zhuǎn)一個(gè)角度再次發(fā)射光線。若光線已經(jīng)旋轉(zhuǎn)過180°，則該點(diǎn)不是道路中點(diǎn)，繼續(xù)判斷下一個(gè)像素點(diǎn)。

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

將兩條射線都順時(shí)針旋轉(zhuǎn)30°，它們之間仍成180°反向發(fā)射，直到旋轉(zhuǎn)過180°為止。

所述步驟7中，中點(diǎn)連接的方法為：

將經(jīng)上述過程所獲得的中點(diǎn)都存儲(chǔ)為一個(gè)順序表，對(duì)順序表中的每一個(gè)元素，都與表中其他元素進(jìn)行比較，若與之距離為1像素，則建立兩點(diǎn)之間的連接；當(dāng)某一點(diǎn)與所有點(diǎn)比較完成之后，則從順序表中刪去該點(diǎn)。將連接關(guān)系和中點(diǎn)存儲(chǔ)為另一張順序表，以供最后輸出。

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

(1)本發(fā)明的思路是全新的，應(yīng)用了路徑規(guī)劃算法visualgraph算法的運(yùn)行思路，采用光線沿直線傳播的基本思路，既簡單容易理解，并且十分有效。在道路的中線上，它到道路兩邊的距離是相等的，采用光沿直線傳播的策略，用光線來丈量到道路兩邊的距離，若光線的長度相等，則表示該點(diǎn)到道路兩邊的距離相等，則是道路中線上的點(diǎn)；

(2)本發(fā)明采用多方向發(fā)射光線的策略，因?yàn)樵谠S多情況下，機(jī)器人運(yùn)行的道路邊線并非規(guī)則的直線，可能存在多處的波折和不平整，此時(shí)可能丟失原本是道路中線的點(diǎn)，也可能出現(xiàn)原本不是道路中線的點(diǎn)。在這里采用多方向發(fā)射光線的策略，將兩條夾角180°的射線以一定步長不斷旋轉(zhuǎn)，直到旋轉(zhuǎn)180°為止，如此多次判定該點(diǎn)的特征，可以更準(zhǔn)確的得出道路中線的坐標(biāo)點(diǎn)；

(3)本發(fā)明的連接方法是采用鄰近像素點(diǎn)連接的方法，這樣，在道路中點(diǎn)產(chǎn)生的時(shí)候，可能生成一些冗余的、不是道路中線的點(diǎn)，這些點(diǎn)將會(huì)被孤立起來，無法連接入道路中線中去，這樣也就可以剔除冗余點(diǎn)，使產(chǎn)生的道路中線合理、正確。

附圖說明

圖1為直道地圖提取效果；

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

圖3為slam復(fù)雜地圖的提取效果。

具體實(shí)施方式

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

步驟1：用slam算法或其他建圖算法獲取所要生成道路中線的地圖，以.pgm格式的圖片傳入，調(diào)用opencv庫函數(shù)imread將其二值化，得到opencv的矩陣變量mat，再將其轉(zhuǎn)化成一個(gè)二維數(shù)組，數(shù)組的每一個(gè)元素代表一個(gè)像素點(diǎn)，其值為0或1,0表示可以通過，1表示存在障礙物，二值化過程完成；

步驟2：從地圖左上角開始遍歷整張地圖。若某一個(gè)像素點(diǎn)是不可通過點(diǎn)，則直接跳過，若某一像素點(diǎn)為可通過點(diǎn)，則從該點(diǎn)開始發(fā)射光線。從該點(diǎn)出發(fā)，首先水平方向發(fā)射兩條互相成180°的射線，射線逐像素前進(jìn)，每前進(jìn)一個(gè)像素就進(jìn)行一次檢測，檢測當(dāng)前點(diǎn)是否為不可通過點(diǎn)，若是，則停止光線的推進(jìn)；

步驟3：在步驟2中，發(fā)射的兩條射線到達(dá)不可通過點(diǎn)之后，存儲(chǔ)碰撞點(diǎn)的坐標(biāo)，并計(jì)算碰撞點(diǎn)和光線發(fā)射點(diǎn)的距離，若兩個(gè)碰撞點(diǎn)到發(fā)射點(diǎn)的距離相等，則該點(diǎn)即為道路中線上的點(diǎn)，則該點(diǎn)的光線發(fā)射過程立即結(jié)束，將該點(diǎn)儲(chǔ)存為道路中線點(diǎn)，并進(jìn)行下一個(gè)像素點(diǎn)的光線發(fā)射過程；

步驟4：在步驟3中，若兩條射線到達(dá)不可通過點(diǎn)之后，求得的到光線發(fā)射點(diǎn)的距離不相等，則將兩條射線的發(fā)射方向順時(shí)針旋轉(zhuǎn)一個(gè)步長，(此處是30°，可以調(diào)整步長來實(shí)現(xiàn)精度的調(diào)整)，再次重復(fù)步驟2的光線發(fā)射過程和步驟3的判定過程。；

步驟5：若射線的已經(jīng)旋轉(zhuǎn)過180°都無法滿足步驟3中的判定條件的話，則該點(diǎn)即不是道路中線上的點(diǎn)，應(yīng)予以舍棄，跳過該點(diǎn)進(jìn)行下一個(gè)像素點(diǎn)的光線發(fā)射過程和判定過程；

步驟6：當(dāng)?shù)貓D遍歷完成，右下角的像素點(diǎn)都已經(jīng)經(jīng)過了光線發(fā)射和判定過程，則產(chǎn)生道路中線的過程結(jié)束，獲得了一個(gè)順序表，其中儲(chǔ)存的道路中線點(diǎn)的坐標(biāo)；

步驟7：處理道路中線坐標(biāo)順序表，每次取出順序表頭的一個(gè)元素，將其和順序表中所有的元素進(jìn)行比較，若與某一元素的距離在一個(gè)像素，則建立這兩點(diǎn)之間的連接，當(dāng)該元素與順序表中所有的元素都已經(jīng)進(jìn)行比較，則從順序表中刪去該元素，將連接關(guān)系和點(diǎn)坐標(biāo)儲(chǔ)存在一個(gè)結(jié)構(gòu)體中，并將結(jié)構(gòu)體置入一個(gè)新的順序表中，以供最后輸出。對(duì)中線坐標(biāo)順序表中的每一個(gè)元素都重復(fù)上述操作，最終完成中線的連接。

如圖1，是本發(fā)明在直道地圖中的效果。圖中黑色部分表示不可通過區(qū)域，白色部分表示可以通過區(qū)域，即道路?，F(xiàn)要求提取白色區(qū)域道路部分的中線?？梢钥闯霰景l(fā)明在這種橫平豎直的道路中提取道路中線的效果還是非常好的。

如圖3所示，是本發(fā)明在復(fù)雜的slam地圖上應(yīng)用的效果，圖中深色部分表示不可通過區(qū)域，白色部分表示可以通過區(qū)域。在slam生成的地圖中，可以看出有許多噪點(diǎn)，這會(huì)相當(dāng)大程度影響道路中線的提取，可以看出本發(fā)明在這種情況下也能夠比較正確地提取出道路的中線，在一般的機(jī)器人應(yīng)用中已經(jīng)可以完成需求。