基于骨架關(guān)鍵點(diǎn)重規(guī)劃的Voronoi圖法路徑規(guī)劃

朱建陽(yáng) 張旭陽(yáng) 蔣 林 李 峻 雷 斌

(1.武漢科技大學(xué)冶金裝備及其控制教育部重點(diǎn)實(shí)驗(yàn)室, 武漢 430081；2.武漢科技大學(xué)機(jī)器人與智能系統(tǒng)研究院, 武漢 430081；3.武漢科技大學(xué)機(jī)械傳動(dòng)與制造工程湖北省重點(diǎn)實(shí)驗(yàn)室, 武漢 430081)

0 引言

根據(jù)對(duì)環(huán)境信息掌握的程度，機(jī)器人路徑規(guī)劃可分為基于環(huán)境先驗(yàn)信息完全已知的全局路徑規(guī)劃和基于傳感器信息的局部路徑規(guī)劃[1]。全局路徑規(guī)劃大多都是基于柵格法實(shí)現(xiàn)的，例如主流算法中的A*算法和Dijkstra算法，但是在柵格法中，柵格粒度越小，柵格圖中障礙物位置會(huì)越精確，但需要的存儲(chǔ)空間會(huì)越大，算法的搜索范圍將會(huì)指數(shù)級(jí)擴(kuò)大；柵格粒度越大，最終搜索出的路徑就越不精確。確定柵格尺寸是柵格法的重要問(wèn)題[2]。

在LOZANO-PEREZ[3]提出了路徑規(guī)劃中構(gòu)型空間的概念后，構(gòu)型空間法逐漸成為了路徑規(guī)劃的一種重要方案，構(gòu)型空間法的出現(xiàn)，使路徑規(guī)劃方案有了新的選擇，不再完全依賴受限于柵格尺寸的柵格法方案。Voronoi圖法就是一種典型的構(gòu)型空間法，Voronoi圖法就是連接機(jī)器人的起始點(diǎn)和終點(diǎn)到已經(jīng)生成的Voronoi圖上，由此生成整體路徑[4]。每次規(guī)劃時(shí)只需要通過(guò)圖搜索法找到起點(diǎn)和終點(diǎn)到Voronoi圖的最短路徑，中間部分路徑依據(jù)Voronoi圖生成。這種路徑生成方式與柵格法中對(duì)整個(gè)柵格圖進(jìn)行搜索計(jì)算量減小了很多，實(shí)時(shí)性較好，生成的路徑比較安全，遠(yuǎn)離障礙物。近年來(lái)一些學(xué)者針對(duì)Voronoi圖法進(jìn)行了改進(jìn)，MAGID等[5]提出了一種利用全局信息構(gòu)建Voronoi圖，然后通過(guò)局部地區(qū)的參數(shù)調(diào)整進(jìn)行優(yōu)化的一種移動(dòng)機(jī)器人路徑規(guī)劃算法，雖然生成的路徑較為簡(jiǎn)潔，但是路徑彎曲，存在較大轉(zhuǎn)角，對(duì)機(jī)器人導(dǎo)航效率有較大的影響。蔣林等[6]提出一種改進(jìn)骨架提取的Voronoi路徑規(guī)劃算法，該算法首先對(duì)地圖進(jìn)行處理，去除地圖中的的噪點(diǎn)，解決了Voronoi地圖臃腫的問(wèn)題，但該算法同樣存在生成路徑曲折，路徑長(zhǎng)度冗余，機(jī)器人導(dǎo)航過(guò)程中實(shí)時(shí)性差、轉(zhuǎn)折較多、耗時(shí)較長(zhǎng)等問(wèn)題。

以上研究都針對(duì)Voronoi的缺點(diǎn)進(jìn)行了優(yōu)化，但是優(yōu)化后的結(jié)果都存在相同的問(wèn)題，即Voronoi地圖路徑彎曲冗余，依據(jù)Voronoi地圖進(jìn)行規(guī)劃時(shí)實(shí)時(shí)性差，規(guī)劃出的路徑較長(zhǎng)，導(dǎo)致機(jī)器人導(dǎo)航過(guò)程中的時(shí)間成本較高。本文針對(duì)該問(wèn)題，在當(dāng)前研究基礎(chǔ)上提出在生成的彎曲的Voronoi圖中尋找出關(guān)鍵點(diǎn)，確定關(guān)鍵點(diǎn)之間的連接關(guān)系，然后進(jìn)行重規(guī)劃，精簡(jiǎn)全局Voronoi地圖，并對(duì)依據(jù)重規(guī)劃的Voronoi地圖生成的路徑進(jìn)行降梯度采樣，平滑路徑，以實(shí)現(xiàn)在路徑規(guī)劃過(guò)程中快速搜索出優(yōu)質(zhì)路徑。

1 柵格地圖預(yù)處理

機(jī)器人通過(guò)建圖算法構(gòu)建環(huán)境柵格地圖，地圖以3色灰度圖的形式呈現(xiàn)出室內(nèi)環(huán)境輪廓。如圖1a所示，地圖上共有黑色、白色、灰色3種顏色，其中黑色為不可行區(qū)域，白色為可行區(qū)域，灰色為未知區(qū)域[7]。由于構(gòu)建的柵格地圖存在毛邊，并且邊界可能出現(xiàn)未完全封閉的情況，所以需要對(duì)地圖進(jìn)行預(yù)處理。

1.1 地圖二值化

柵格地圖為包含黑、白、灰3種顏色的灰度圖，如圖1a所示，其中黑色像素值為0，白色像素值為255，灰色像素值為205，首先設(shè)置閾值為200，依次對(duì)地圖中的每個(gè)像素值進(jìn)行二值化處理。二值化處理后的圖像中數(shù)據(jù)量大為減少，并且能夠更加清晰地凸顯出地圖中環(huán)境邊界的輪廓[8]，如圖1b所示。

1.2 地圖的膨脹腐蝕

在得到二值圖后需要對(duì)圖像進(jìn)行腐蝕和膨脹[9]，所謂腐蝕和膨脹就是對(duì)像素值都為255的白色區(qū)域進(jìn)行處理。如圖2a所示，假設(shè)白色區(qū)域?yàn)锳，定義一個(gè)結(jié)構(gòu)元素B，依次遍歷A中的像素，在遍歷時(shí)將B的中心與A的像素點(diǎn)重合[10]，求出此時(shí)結(jié)構(gòu)元素B覆蓋區(qū)域最小的像素值，并將該值賦予此刻結(jié)構(gòu)元素B所覆蓋的所有像素，如果結(jié)構(gòu)元素B覆蓋區(qū)域中有黑色像素值0，則此刻結(jié)構(gòu)元素B覆蓋區(qū)域的像素值將都變?yōu)?，即該覆蓋區(qū)域中的白色會(huì)變?yōu)楹谏?，?dāng)遍歷完成后，白色區(qū)域會(huì)縮小一圈，因此該過(guò)程被稱為腐蝕，整個(gè)計(jì)算過(guò)程中不會(huì)改變?cè)紨?shù)據(jù)，輸出的新圖像矩陣為新生成的單獨(dú)矩陣[11]。如圖2b所示，若在遍歷時(shí)求出此時(shí)結(jié)構(gòu)元素B覆蓋區(qū)域最大的像素值，并將該值賦予結(jié)構(gòu)元素B覆蓋的所有像素，如果結(jié)構(gòu)元素B覆蓋區(qū)域中有黑色像素值0，則此刻結(jié)構(gòu)元素B覆蓋區(qū)域的像素值將變?yōu)?55，則該覆蓋區(qū)域中的黑色會(huì)變?yōu)榘咨?dāng)遍歷完成后，白色區(qū)域會(huì)擴(kuò)大一圈，因此該過(guò)程被稱為膨脹。

圖2 腐蝕膨脹原理圖Fig.2 Principle of corrosion expansion

在圖像操作中，腐蝕一般用于去除孤立點(diǎn)，膨脹一般用于連接相對(duì)靠近的空隙，將腐蝕和膨脹配合在一起使用時(shí)，先腐蝕后膨脹被稱為開運(yùn)算，先膨脹后腐蝕被稱為閉運(yùn)算[12]。本文采用開運(yùn)算，開運(yùn)算可以在原始位置和形狀幾乎不變的情況下，去除毛刺和噪點(diǎn)。圖3a為預(yù)處理前的二值圖，為了更加清晰地展示地圖特點(diǎn)，選取兩處具有代表性的區(qū)域進(jìn)行放大。如放大區(qū)域所示，預(yù)處理前的地圖普遍存在毛邊和噪點(diǎn)。在進(jìn)行開運(yùn)算時(shí)，二值地圖中白色區(qū)域會(huì)先縮小，去除孤立的點(diǎn)，再擴(kuò)大，連接相對(duì)靠近的孤立點(diǎn)。整個(gè)開運(yùn)算過(guò)程，可以消除地圖上的噪點(diǎn)[13]，連接相距很近卻未連續(xù)的障礙物邊界，平滑地圖上的毛邊和毛刺，整體區(qū)域尺寸和位置幾乎不變[14]，預(yù)處理結(jié)果如圖3b所示。地圖預(yù)處理可以防止后續(xù)提取骨架時(shí)出現(xiàn)骨架溢出和生成不必要的骨架分支等問(wèn)題。

圖3 預(yù)處理結(jié)果Fig.3 Pretreatment result

2 骨架提取

依次遍歷圖像上的像素點(diǎn)，如圖4所示，設(shè)此刻遍歷到的像素點(diǎn)為P1，其周圍的像素點(diǎn)依次標(biāo)記為P2～P9，設(shè)黑色像素點(diǎn)的P值為0，白色像素點(diǎn)的P值為1。由于圖像最外圍的一圈像素不存在8鄰域，所以不對(duì)其進(jìn)行處理。

圖4 8鄰域示意圖Fig.4 Eight neighborhood diagram

將點(diǎn)P1的8鄰域P2～P9中，P值從0變?yōu)?的出現(xiàn)次數(shù)設(shè)置為M，每個(gè)像素的8鄰域P值和為N(Px)，將滿足表1中條件的P1點(diǎn)像素值設(shè)置為0。

表1 像素值條件Tab.1 Pixel value condition

通過(guò)此算法對(duì)預(yù)處理后的地圖進(jìn)行多次迭代處理，可以得到如圖5a所示的骨架[15]，為了表示骨架和原地圖的相對(duì)關(guān)系，將骨架以黑色加載到原地圖上得到如圖5b所示的骨架全局圖，圖5c為未對(duì)地圖進(jìn)行預(yù)處理得到的骨架全局圖，通過(guò)對(duì)比可以發(fā)現(xiàn)，未經(jīng)過(guò)預(yù)處理的地圖由于具有噪點(diǎn)，并且地圖邊界存在空隙，導(dǎo)致生成的骨架雜亂，甚至溢出邊界，無(wú)法用于后續(xù)導(dǎo)航，經(jīng)過(guò)預(yù)處理的地圖提取出的骨架清晰簡(jiǎn)潔，可以為后續(xù)導(dǎo)航提供更優(yōu)質(zhì)的路線選擇。

圖5 骨架提取結(jié)果Fig.5 Skeleton extraction result

3 骨架關(guān)鍵點(diǎn)重規(guī)劃

3.1 骨架關(guān)鍵點(diǎn)的提取與分類

在生成的骨架圖基礎(chǔ)上，首先提取出骨架中關(guān)鍵點(diǎn)，將關(guān)鍵點(diǎn)分為端點(diǎn)和分支點(diǎn)兩類，端點(diǎn)即為骨架分支的末端點(diǎn)，分支點(diǎn)為骨架的交叉點(diǎn)。為了將骨架上的關(guān)鍵點(diǎn)都找出來(lái)，對(duì)骨架進(jìn)行遍歷。如圖5a所示，由于生成的骨架為像素值255的白色，非骨架部分為像素值0的黑色，在尋找關(guān)鍵點(diǎn)時(shí)，主要關(guān)注像素值為255的骨架部分[16]。遍歷地圖中所有骨架點(diǎn)，設(shè)每個(gè)點(diǎn)的像素值為S，通過(guò)分析其如圖6所示的8鄰域的像素值的和確定其是否為關(guān)鍵點(diǎn)，白點(diǎn)S設(shè)為255，黑點(diǎn)S設(shè)為0。

圖6 關(guān)鍵點(diǎn)8鄰域示意圖Fig.6 Schematic of eight neighborhoods of key points

(1)端點(diǎn)確定

骨架端點(diǎn)的8鄰域中只有1個(gè)像素值為255的白點(diǎn)，因此將滿足S1+S2+S3+S4+S5+S6+S7+S8=255的骨架點(diǎn)定義為端點(diǎn)。

(2)分支點(diǎn)確定

骨架分支點(diǎn)是3個(gè)分支的交點(diǎn)，即分支點(diǎn)8鄰域中會(huì)有3個(gè)像素值為255的白點(diǎn)，因此將8鄰域的和滿足表2中條件的骨架點(diǎn)定義為分支點(diǎn)。

表2 分支點(diǎn)條件Tab.2 Branch point condition

采用上述方法遍歷骨架上所有的點(diǎn)，并將骨架和搜索出的關(guān)鍵點(diǎn)以黑色加載到地圖上，并將關(guān)鍵點(diǎn)加粗顯示，如圖7所示，骨架上的所有關(guān)鍵點(diǎn)基本上都被搜索出來(lái)。

圖7 骨架關(guān)鍵點(diǎn)Fig.7 Key points of skeleton

3.2 關(guān)鍵點(diǎn)重規(guī)劃

在確定關(guān)鍵點(diǎn)后需要進(jìn)行骨架重規(guī)劃，骨架重規(guī)劃首先需要確定各個(gè)點(diǎn)之間的連接關(guān)系，連接關(guān)系共分為2種，一種是端點(diǎn)和分支點(diǎn)的連接關(guān)系，另一種是分支點(diǎn)之間的連接關(guān)系。

首先，確定端點(diǎn)和分支點(diǎn)的連接關(guān)系，遍歷所有端點(diǎn)，以某個(gè)端點(diǎn)為起點(diǎn)，將所有分支點(diǎn)都設(shè)為終點(diǎn)，依次在原始骨架上搜索從該端點(diǎn)到分支點(diǎn)之間的路徑。如果搜索出的路徑中有且只有設(shè)為終點(diǎn)的那一個(gè)分支點(diǎn)，則該端點(diǎn)與分支點(diǎn)具有直接連接關(guān)系，并將該關(guān)系存儲(chǔ)起來(lái)。

然后，確定分支點(diǎn)與分支點(diǎn)之間的連接關(guān)系，遍歷所有的分支點(diǎn)，將遍歷到的分支點(diǎn)設(shè)為起點(diǎn)，其他分支點(diǎn)設(shè)為終點(diǎn)，依次在原有骨架上搜索該起點(diǎn)到每個(gè)終點(diǎn)的路徑。如果搜索到的路徑中，有且只有設(shè)為終點(diǎn)的那一個(gè)分支點(diǎn)，則該端點(diǎn)與分支點(diǎn)具有直接連接關(guān)系，并將該關(guān)系存儲(chǔ)起來(lái)。

在明確關(guān)鍵點(diǎn)之間的關(guān)系后，將關(guān)鍵點(diǎn)連接起來(lái)，連接圖像上的2個(gè)點(diǎn)，就是將2個(gè)像素點(diǎn)連接起來(lái)，處理的基本單位是像素[17]。

兩點(diǎn)連接一條直線，因?yàn)橹本€的一階導(dǎo)數(shù)是連續(xù)的，兩點(diǎn)X和Y方向上的間距ΔX和ΔY具有一定的比例關(guān)系，Xi+1=Xi+ΔXε，Yi+1=Yi+ΔYε，ε=1/max(|ΔX|,|ΔY|)，采用這個(gè)原理[18]，將連線分為以下幾個(gè)步驟：

(1)確定2個(gè)點(diǎn)的坐標(biāo)。

(2)分別計(jì)算X和Y方向上的間距ΔX和ΔY。

(3)確定單位步進(jìn)，取K=max(ΔX,ΔY)，如果ΔX≥ΔY,則將ΔX方向設(shè)為單位步進(jìn)，ΔX方向每步進(jìn)一個(gè)單位，ΔY方向以ΔY/K的步長(zhǎng)向前步進(jìn)一個(gè)單位；否則相反[19]。

(4)設(shè)置該點(diǎn)的像素值。

(5)設(shè)置循環(huán)值為0，循環(huán)次數(shù)為K，設(shè)置2個(gè)新的變量X和Y；開始進(jìn)入以下循環(huán)計(jì)算：①X向前步進(jìn)一個(gè)單位，Y向前步進(jìn)一個(gè)單位。②設(shè)置此時(shí)(X，Y)的像素值。

按照以上步驟完成連線后，得到如圖8所示的初次連接結(jié)果圖，圖中的骨架經(jīng)過(guò)重新連接后路徑變得簡(jiǎn)潔筆直，但是出現(xiàn)了骨架穿越障礙物的情況，因此在連接前需要加入判斷條件并添加中間點(diǎn)。

圖8 初次連接效果Fig.8 Initial connection effect

3.3 中間點(diǎn)添加

直接將關(guān)鍵點(diǎn)重連后，存在連接線穿越障礙物的問(wèn)題，因此在連接關(guān)鍵點(diǎn)時(shí)需要先進(jìn)行判斷。如果直接連接穿越了障礙物，則需要添加中間點(diǎn)。如圖9a所示，點(diǎn)A和點(diǎn)B為具有直接連接關(guān)系的中間點(diǎn)，兩點(diǎn)之間直線距離為L(zhǎng)AB，首先將關(guān)鍵點(diǎn)A、B間的原始連接線4等分，取出中間的3個(gè)點(diǎn)M1、M2、M3，分別計(jì)算出3個(gè)點(diǎn)到直線AB的距離L1、L2、L3，將距離大于0.25LAB的點(diǎn)篩選出來(lái)作為連接的中間點(diǎn)，如果重新連接的路線依然穿越障礙物[20]，將原始連接線再次以等分的方式多取出2個(gè)點(diǎn)，并使距離閾值自動(dòng)增加10%，再次進(jìn)行篩選來(lái)尋找中間點(diǎn)，因?yàn)樵悸肪€并未穿越障礙物，并且離障礙物較遠(yuǎn)，所以最終可以搜索出使連接線不穿越障礙物的中間點(diǎn)。最終連接的效果如圖9b所示，重規(guī)劃后的路徑筆直，整體Voronoi路徑數(shù)據(jù)量減少。

圖9 添加中間點(diǎn)示意圖Fig.9 Secondary connection effect

3.4 路徑平滑

機(jī)器人在導(dǎo)航時(shí)，會(huì)先根據(jù)起點(diǎn)和目標(biāo)點(diǎn)在生成的Voronoi全局路徑上尋找出相對(duì)應(yīng)的路徑，但是由于Voronoi圖的天然缺點(diǎn)，即使采用基于關(guān)鍵點(diǎn)重規(guī)劃后的Voronoi圖生成的路徑依然存在鋸齒狀和轉(zhuǎn)折角度較大等問(wèn)題[21]。

為了解決該方法中存在的問(wèn)題，在生成路徑時(shí)融入改進(jìn)的降梯度采樣方法，優(yōu)化生成的不平滑路徑[22]，優(yōu)化過(guò)程為：

(1)生成的初始路徑實(shí)質(zhì)上就是連續(xù)的點(diǎn)，將點(diǎn)依次設(shè)置為D1(X1,Y1)、D2(X2,Y2)、D3(X3,Y3)、…、Dn(Xn,Yn)。

(2)將平滑后路徑上的點(diǎn)依次設(shè)置為C1(X1,Y1)、C2(X2,Y2)、C3(X3,Y3)、…、Cn(Xn,Yn)。

(3)定義平滑函數(shù)Tsmooth=m(Di-Ci)+k(Ci-Ci+1)，m和k分別代表函數(shù)中前后2項(xiàng)的權(quán)重，第1項(xiàng)表示平滑后的點(diǎn)與平滑前的點(diǎn)之間的距離，第2項(xiàng)表示相鄰的平滑點(diǎn)之間的距離，m較k越大，平滑后的點(diǎn)越靠近原始點(diǎn)，m較k越小，相鄰平滑點(diǎn)互相之間越靠近，得到的路徑就越平滑，在經(jīng)過(guò)多次反饋分析后，最終選取m為0.5，k為0.4。整個(gè)平滑的過(guò)程就是將Tsmooth縮小到閾值范圍，所以采用梯度下降法不斷迭代更新Ci，從而不斷縮小Tsmooth。循環(huán)迭代表達(dá)式為Ci=Ci+m(Di-Ci)+k(Ci-1-2Ci+Ci+1)，通過(guò)不斷迭代這個(gè)表達(dá)式，使Tsmooth不斷縮小，直到Tsmooth小于閾值0.000 1。

平滑前后效果如圖10所示，通過(guò)對(duì)比，經(jīng)過(guò)平滑后，路徑中的鋸齒狀部分被消除掉[23]，路徑轉(zhuǎn)折點(diǎn)更少，轉(zhuǎn)折角度[24]更小，路徑更短。

圖10 路徑平滑對(duì)比Fig.10 Path smoothing contrast

4 仿真實(shí)驗(yàn)

先通過(guò)仿真實(shí)驗(yàn)來(lái)證明本文算法的可行性。仿真實(shí)驗(yàn)在Ubuntu 16.04的系統(tǒng)中基于ROS機(jī)器人操作系統(tǒng)進(jìn)行，主要使用的是Gazebo仿真平臺(tái)和Rviz圖形化工具[25]，其中Gazebo用于搭建三維環(huán)境模型，包括機(jī)器人本體和周圍環(huán)境，Rviz用來(lái)實(shí)時(shí)顯示機(jī)器人運(yùn)動(dòng)狀態(tài)和導(dǎo)航路徑。本仿真實(shí)驗(yàn)中機(jī)器人本體模型為輪式差動(dòng)機(jī)器人，底盤為圓柱形，機(jī)器人正上方搭載一臺(tái)二維激光雷達(dá)。

為了驗(yàn)證本文算法的環(huán)境適應(yīng)性和可行性，搭建了2種復(fù)雜環(huán)境，如圖11所示。仿真實(shí)驗(yàn)首先建立2種環(huán)境的柵格地圖，將該地圖作為先驗(yàn)信息輸入算法，使用本文算法生成相應(yīng)的Voronoi圖用于機(jī)器人導(dǎo)航。

圖11 三維環(huán)境模型Fig.11 Three dimensional environment models

4.1 柵格地圖預(yù)處理

圖11a為家庭三維環(huán)境模型，內(nèi)部結(jié)構(gòu)仿照家庭環(huán)境通過(guò)墻體劃分為不同區(qū)域；圖11b為多障礙物三維環(huán)境，環(huán)境中無(wú)規(guī)則放置的矩形塊代表障礙物[26]。采用建圖算法分別構(gòu)建環(huán)境二維柵格地圖，使用本文算法對(duì)柵格地圖進(jìn)行預(yù)處理，如圖12、13所示，預(yù)處理后的地圖與原地圖相比噪點(diǎn)更少，邊界清晰且完全封閉，可以防止在提取骨架時(shí)出現(xiàn)冗余骨架和骨架溢出等問(wèn)題。

圖12 家庭環(huán)境地圖預(yù)處理效果Fig.12 Pre-processing renderings of home environment maps

圖13 多障礙物環(huán)境地圖預(yù)處理效果Fig.13 Pre-processing rendering of multi-obstacle environment map

圖14 家庭環(huán)境骨架對(duì)比Fig.14 Family environment skeleton contrast

圖15 多障礙物環(huán)境骨架對(duì)比Fig.15 Skeleton comparison of multi-obstacle environment

4.2 骨架提取

柵格地圖預(yù)處理后進(jìn)行骨架提取，如圖14a、15a所示，原始Voronoi算法[2]由于自身的特點(diǎn)，并且未對(duì)柵格地圖進(jìn)行預(yù)處理，柵格地圖存在噪點(diǎn)，部分邊界呈鋸齒狀，導(dǎo)致生成的Voronoi全局路徑有很多非必要的分支；如圖14b、15b所示，原改進(jìn)骨架算法[5]通過(guò)改進(jìn)骨架提取方式精簡(jiǎn)并優(yōu)化了生成的全局路徑，但生成的路徑轉(zhuǎn)折多，轉(zhuǎn)折角度大，長(zhǎng)度冗余；如圖14c、15c所示，本文算法生成的全局路徑筆直，轉(zhuǎn)折極少，轉(zhuǎn)折角度較小。通過(guò)比較Voronoi全局路徑包含的像素點(diǎn)的數(shù)量來(lái)比較3種算法分別生成的全局路徑數(shù)據(jù)量。如表3所示，采用本文算法生成的全局路徑數(shù)據(jù)量相對(duì)原Voronoi算法生成的全局路徑數(shù)據(jù)量平均降低了82.69%，采用本文算法生成的全局路徑數(shù)據(jù)量相對(duì)原改進(jìn)骨架算法生成的全局路徑數(shù)據(jù)量平均降低了11.81%，因此機(jī)器人依據(jù)本文算法生成的Voronoi圖可以更加快速地規(guī)劃出路徑，機(jī)器人導(dǎo)航時(shí)具有更好的實(shí)時(shí)性。

表3 全局路徑數(shù)據(jù)量對(duì)比Tab.3 Global path comparison

4.3 路徑平滑實(shí)驗(yàn)對(duì)比

機(jī)器人在導(dǎo)航過(guò)程中會(huì)依據(jù)Voronoi圖搜索出相應(yīng)的路徑，當(dāng)機(jī)器人的起始點(diǎn)或目標(biāo)點(diǎn)不在Voronoi全局路徑上時(shí)，此時(shí)就通過(guò)圖搜索算法連接起點(diǎn)和終點(diǎn)到骨架圖上，再基于Voronoi全局路徑搜索出起點(diǎn)和終點(diǎn)間的最短路徑，得到最終的導(dǎo)航路徑[27]。機(jī)器人在規(guī)劃出起始點(diǎn)和終點(diǎn)之間的路徑后，就可以開始向目標(biāo)點(diǎn)移動(dòng)，但是由于Voronoi全局路徑并不平滑，導(dǎo)致搜索出的路徑轉(zhuǎn)折次數(shù)多，轉(zhuǎn)折角度大，部分路徑段呈鋸齒狀，機(jī)器人按照鋸齒狀路徑行走時(shí)會(huì)依據(jù)鋸齒多次轉(zhuǎn)向，導(dǎo)致機(jī)器人運(yùn)動(dòng)緩慢，很大程度上降低了機(jī)器人的效率和穩(wěn)定性[28]。本文針對(duì)該問(wèn)題提出使用降梯度采樣法對(duì)生成的路徑進(jìn)行平滑處理，平滑前后結(jié)果如圖16、17所示，為了更加鮮明地對(duì)比效果，將圖上的局部地區(qū)放大2倍，經(jīng)過(guò)本文算法平滑后鋸齒狀的問(wèn)題得到解決，路徑的轉(zhuǎn)折次數(shù)減少，轉(zhuǎn)角變小，轉(zhuǎn)角弧線的長(zhǎng)度也變短，路徑總長(zhǎng)也相應(yīng)得到縮短[29]，因此經(jīng)過(guò)本文算法處理的路徑質(zhì)量得到了很大提升。

圖16 家庭環(huán)境路徑對(duì)比Fig.16 Family environment path comparison

圖17 多障礙物環(huán)境路徑對(duì)比Fig.17 Multi-obstacle environment path comparison

4.4 本文算法與原算法生成的路徑比較

如圖18～21所示，在2種環(huán)境中分別選取6組相同的起點(diǎn)和終點(diǎn)，采用2種算法規(guī)劃出相應(yīng)的路徑，通過(guò)對(duì)比原算法[5]與本文算法生成的實(shí)際路徑，可知本文算法生成的導(dǎo)航路徑更短，規(guī)劃速度更快，整體路徑更平直，轉(zhuǎn)折少且轉(zhuǎn)折角度小。如表4、5所示，將2種算法規(guī)劃的路徑長(zhǎng)度、規(guī)劃路徑的時(shí)間，以及規(guī)劃路徑中轉(zhuǎn)折次數(shù)進(jìn)行對(duì)比，其中規(guī)劃路徑長(zhǎng)度用路徑中包含的像素?cái)?shù)表示。由表4、5可知，在2種環(huán)境下共規(guī)劃的12組路徑中，本文算法相對(duì)于原算法規(guī)劃出的路徑長(zhǎng)度平均減少了11.43%，規(guī)劃路徑的時(shí)間平均減少了15.65%，轉(zhuǎn)折次數(shù)平均減少了51.13%。由此證明本文算法相對(duì)于原算法在規(guī)劃路徑質(zhì)量和規(guī)劃路徑實(shí)時(shí)性上有明顯優(yōu)勢(shì)。

圖18 基于原改進(jìn)骨架算法家庭環(huán)境導(dǎo)航路徑Fig.18 Home environment navigation path based on improved skeleton algorithm

表4 室內(nèi)環(huán)境下規(guī)劃路徑參數(shù)Tab.4 Planning path parameters in an indoor environment

圖19 基于本文算法家庭環(huán)境導(dǎo)航路徑Fig.19 Home environment navigation path based on proposed algorithm

圖20 基于原改進(jìn)骨架算法多障礙物環(huán)境導(dǎo)航路徑Fig.20 Multi-obstacle environment navigation path based on improved skeleton algorithm

圖21 基于本文算法多障礙物環(huán)境導(dǎo)航路徑Fig.21 Multi-obstacle environment navigation path based on proposed algorithm

5 實(shí)際實(shí)驗(yàn)

通過(guò)多次仿真實(shí)驗(yàn)基本證明了本文算法的可行性和穩(wěn)定性后，在實(shí)際環(huán)境中對(duì)本文算法進(jìn)行驗(yàn)證，實(shí)際環(huán)境中使用的機(jī)器人為本實(shí)驗(yàn)室自主搭建的輪式差分機(jī)器人。如圖22所示,該機(jī)器人由機(jī)械部分、傳感器部分、控制器部分組成；機(jī)械部分主要包括連接件和玻璃纖維加工板；傳感器部分主要由SICKlms111型激光雷達(dá)、光電編碼器、KinectV2相機(jī)構(gòu)成；控制器由Intel-NUC和驅(qū)動(dòng)控制器構(gòu)成。

圖22 實(shí)驗(yàn)平臺(tái)Fig.22 Experiment platform1.供電電池 2.Intel-NUC 3.顯示器 4.激光雷達(dá) 5.光電編碼器

在實(shí)驗(yàn)室搭建如圖23a所示實(shí)驗(yàn)場(chǎng)景，該場(chǎng)景通過(guò)圍墻將實(shí)驗(yàn)室劃分為不同尺寸和形狀的區(qū)域，并在墻角部分放置一定數(shù)量的柜子。機(jī)器人在該環(huán)境中使用Gmapping建圖算法構(gòu)建如圖23b所示柵格地圖，采用本文算法對(duì)地圖進(jìn)行預(yù)處理并生成Voronoi圖。

表5 多障礙物環(huán)境下規(guī)劃路徑參數(shù)Tab.5 Planning path parameters in a multi-obstacle environment

圖23 實(shí)際實(shí)驗(yàn)環(huán)境Fig.23 Actual experimental environment

通過(guò)2種算法生成的Voronoi圖如圖24所示，圖24a為原改進(jìn)骨架算法[5]生成的Voronoi圖，整體路徑清晰簡(jiǎn)潔，但路徑彎曲冗長(zhǎng)，轉(zhuǎn)折次數(shù)多，圖24b為本文算法生成的Voronoi圖，路徑筆直簡(jiǎn)潔，數(shù)據(jù)存儲(chǔ)量也在一定程度上有所降低。

圖24 實(shí)際實(shí)驗(yàn)骨架Fig.24 Actual experimental skeleton diagram

在實(shí)際實(shí)驗(yàn)中，機(jī)器人利用原改進(jìn)骨架算法和本文骨架算法生成的Voronoi圖分別進(jìn)行導(dǎo)航，在相同起點(diǎn)和終點(diǎn)的條件下，規(guī)劃路徑如圖25、26所示。圖25a、26a和圖25c、26c為基于原改進(jìn)骨架算法[5]和本文算法生成的Voronoi圖規(guī)劃出的導(dǎo)航路徑1和路徑2，圖25b、26b和圖25d、26d為使用Odometry記錄的實(shí)際導(dǎo)航路徑。對(duì)比機(jī)器人采用原改進(jìn)骨架算法[5]和本文算法走出的實(shí)際路徑，依據(jù)原算法進(jìn)行導(dǎo)航時(shí)，機(jī)器人實(shí)際運(yùn)行路徑長(zhǎng)且存在很多大角度轉(zhuǎn)向，原算法由于生成的Voronoi圖彎曲，導(dǎo)致機(jī)器人依據(jù)原算法規(guī)劃出的路徑導(dǎo)航時(shí)轉(zhuǎn)折次數(shù)多且轉(zhuǎn)折角度大，大角度轉(zhuǎn)向時(shí)，機(jī)器人會(huì)減速，極大地降低了機(jī)器人的導(dǎo)航效率，總體運(yùn)動(dòng)路徑不合理；依據(jù)本文算法進(jìn)行導(dǎo)航時(shí)由于最終生成的Voronoi圖筆直，不僅在原算法的基礎(chǔ)上縮短了路徑，筆直的路徑相對(duì)于原算法的彎曲路徑消除了不必要的轉(zhuǎn)向，提高了機(jī)器人的導(dǎo)航效率，整體運(yùn)動(dòng)路徑也更加合理。

圖25 原改進(jìn)骨架算法實(shí)際導(dǎo)航路徑Fig.25 Actual navigation path of original improved skeleton algorithm

圖27展示了機(jī)器人采用本文算法在實(shí)際環(huán)境中的運(yùn)行情況。機(jī)器人基于本算法運(yùn)動(dòng)時(shí)能夠避開障礙物并快速到達(dá)目標(biāo)點(diǎn)。

圖26 本文算法實(shí)際導(dǎo)航路徑Fig.26 Actual navigation path of proposed algorithm

圖27 機(jī)器人實(shí)際運(yùn)行情況Fig.27 Actual operation of robot

6 結(jié)束語(yǔ)

針對(duì)現(xiàn)有的Voronoi算法生成的Voronoi地圖路徑彎曲冗余，依據(jù)Voronoi地圖規(guī)劃路徑時(shí)實(shí)時(shí)性較差，機(jī)器人導(dǎo)航時(shí)轉(zhuǎn)折次數(shù)多，角度大，時(shí)間成本高等問(wèn)題，采用提取骨架中關(guān)鍵點(diǎn)，并基于關(guān)鍵點(diǎn)重規(guī)劃的方法,解決Voronoi地圖路徑彎曲冗余的問(wèn)題，得到的骨架相對(duì)于原改進(jìn)骨架算法的數(shù)據(jù)量更加精簡(jiǎn)，機(jī)器人依據(jù)本算法生成的數(shù)據(jù)量精簡(jiǎn)后的Voronoi地圖規(guī)劃路徑時(shí)更加快速，規(guī)劃出的路徑更短、質(zhì)量更高，并且對(duì)規(guī)劃出的路徑進(jìn)行降梯度采樣平滑處理，尤其是對(duì)轉(zhuǎn)角路徑進(jìn)行降梯度采樣，既平滑了轉(zhuǎn)角處路徑，又縮短了轉(zhuǎn)角路徑長(zhǎng)度，機(jī)器人在運(yùn)動(dòng)過(guò)程中轉(zhuǎn)向次數(shù)減少，從而提高了機(jī)器人的導(dǎo)航效率。