基于骨架關鍵點重規劃的Voronoi圖法路徑規劃

朱建陽 張旭陽 蔣 林 李 峻 雷 斌

(1.武漢科技大學冶金裝備及其控制教育部重點實驗室, 武漢 430081；2.武漢科技大學機器人與智能系統研究院, 武漢 430081；3.武漢科技大學機械傳動與制造工程湖北省重點實驗室, 武漢 430081)

0 引言

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

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

以上研究都針對Voronoi的缺點進行了優化，但是優化后的結果都存在相同的問題，即Voronoi地圖路徑彎曲冗余，依據Voronoi地圖進行規劃時實時性差，規劃出的路徑較長，導致機器人導航過程中的時間成本較高。本文針對該問題，在當前研究基礎上提出在生成的彎曲的Voronoi圖中尋找出關鍵點，確定關鍵點之間的連接關系，然后進行重規劃，精簡全局Voronoi地圖，并對依據重規劃的Voronoi地圖生成的路徑進行降梯度采樣，平滑路徑，以實現在路徑規劃過程中快速搜索出優質路徑。

1 柵格地圖預處理

機器人通過建圖算法構建環境柵格地圖，地圖以3色灰度圖的形式呈現出室內環境輪廓。如圖1a所示，地圖上共有黑色、白色、灰色3種顏色，其中黑色為不可行區域，白色為可行區域，灰色為未知區域[7]。由于構建的柵格地圖存在毛邊，并且邊界可能出現未完全封閉的情況，所以需要對地圖進行預處理。

1.1 地圖二值化

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

1.2 地圖的膨脹腐蝕

在得到二值圖后需要對圖像進行腐蝕和膨脹[9]，所謂腐蝕和膨脹就是對像素值都為255的白色區域進行處理。如圖2a所示，假設白色區域為A，定義一個結構元素B，依次遍歷A中的像素，在遍歷時將B的中心與A的像素點重合[10]，求出此時結構元素B覆蓋區域最小的像素值，并將該值賦予此刻結構元素B所覆蓋的所有像素，如果結構元素B覆蓋區域中有黑色像素值0，則此刻結構元素B覆蓋區域的像素值將都變為0，即該覆蓋區域中的白色會變為黑色，當遍歷完成后，白色區域會縮小一圈，因此該過程被稱為腐蝕，整個計算過程中不會改變原始數據，輸出的新圖像矩陣為新生成的單獨矩陣[11]。如圖2b所示，若在遍歷時求出此時結構元素B覆蓋區域最大的像素值，并將該值賦予結構元素B覆蓋的所有像素，如果結構元素B覆蓋區域中有黑色像素值0，則此刻結構元素B覆蓋區域的像素值將變為255，則該覆蓋區域中的黑色會變為白色，當遍歷完成后，白色區域會擴大一圈，因此該過程被稱為膨脹。

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

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

圖3 預處理結果Fig.3 Pretreatment result

2 骨架提取

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

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

將點P1的8鄰域P2～P9中，P值從0變為1的出現次數設置為M，每個像素的8鄰域P值和為N(Px)，將滿足表1中條件的P1點像素值設置為0。

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

通過此算法對預處理后的地圖進行多次迭代處理，可以得到如圖5a所示的骨架[15]，為了表示骨架和原地圖的相對關系，將骨架以黑色加載到原地圖上得到如圖5b所示的骨架全局圖，圖5c為未對地圖進行預處理得到的骨架全局圖，通過對比可以發現，未經過預處理的地圖由于具有噪點，并且地圖邊界存在空隙，導致生成的骨架雜亂，甚至溢出邊界，無法用于后續導航，經過預處理的地圖提取出的骨架清晰簡潔，可以為后續導航提供更優質的路線選擇。

圖5 骨架提取結果Fig.5 Skeleton extraction result

3 骨架關鍵點重規劃

3.1 骨架關鍵點的提取與分類

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

圖6 關鍵點8鄰域示意圖Fig.6 Schematic of eight neighborhoods of key points

(1)端點確定

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

(2)分支點確定

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

表2 分支點條件Tab.2 Branch point condition

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

圖7 骨架關鍵點Fig.7 Key points of skeleton

3.2 關鍵點重規劃

在確定關鍵點后需要進行骨架重規劃，骨架重規劃首先需要確定各個點之間的連接關系，連接關系共分為2種，一種是端點和分支點的連接關系，另一種是分支點之間的連接關系。

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

然后，確定分支點與分支點之間的連接關系，遍歷所有的分支點，將遍歷到的分支點設為起點，其他分支點設為終點，依次在原有骨架上搜索該起點到每個終點的路徑。如果搜索到的路徑中，有且只有設為終點的那一個分支點，則該端點與分支點具有直接連接關系，并將該關系存儲起來。

在明確關鍵點之間的關系后，將關鍵點連接起來，連接圖像上的2個點，就是將2個像素點連接起來，處理的基本單位是像素[17]。

兩點連接一條直線，因為直線的一階導數是連續的，兩點X和Y方向上的間距ΔX和ΔY具有一定的比例關系，Xi+1=Xi+ΔXε，Yi+1=Yi+ΔYε，ε=1/max(|ΔX|,|ΔY|)，采用這個原理[18]，將連線分為以下幾個步驟：

(1)確定2個點的坐標。

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

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

(4)設置該點的像素值。

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

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

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

3.3 中間點添加

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

圖9 添加中間點示意圖Fig.9 Secondary connection effect

3.4 路徑平滑

機器人在導航時，會先根據起點和目標點在生成的Voronoi全局路徑上尋找出相對應的路徑，但是由于Voronoi圖的天然缺點，即使采用基于關鍵點重規劃后的Voronoi圖生成的路徑依然存在鋸齒狀和轉折角度較大等問題[21]。

為了解決該方法中存在的問題，在生成路徑時融入改進的降梯度采樣方法，優化生成的不平滑路徑[22]，優化過程為：

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

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

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

平滑前后效果如圖10所示，通過對比，經過平滑后，路徑中的鋸齒狀部分被消除掉[23]，路徑轉折點更少，轉折角度[24]更小，路徑更短。

圖10 路徑平滑對比Fig.10 Path smoothing contrast

4 仿真實驗

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

為了驗證本文算法的環境適應性和可行性，搭建了2種復雜環境，如圖11所示。仿真實驗首先建立2種環境的柵格地圖，將該地圖作為先驗信息輸入算法，使用本文算法生成相應的Voronoi圖用于機器人導航。

圖11 三維環境模型Fig.11 Three dimensional environment models

4.1 柵格地圖預處理

圖11a為家庭三維環境模型，內部結構仿照家庭環境通過墻體劃分為不同區域；圖11b為多障礙物三維環境，環境中無規則放置的矩形塊代表障礙物[26]。采用建圖算法分別構建環境二維柵格地圖，使用本文算法對柵格地圖進行預處理，如圖12、13所示，預處理后的地圖與原地圖相比噪點更少，邊界清晰且完全封閉，可以防止在提取骨架時出現冗余骨架和骨架溢出等問題。

圖12 家庭環境地圖預處理效果Fig.12 Pre-processing renderings of home environment maps

圖13 多障礙物環境地圖預處理效果Fig.13 Pre-processing rendering of multi-obstacle environment map

圖14 家庭環境骨架對比Fig.14 Family environment skeleton contrast

圖15 多障礙物環境骨架對比Fig.15 Skeleton comparison of multi-obstacle environment

4.2 骨架提取

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

表3 全局路徑數據量對比Tab.3 Global path comparison

4.3 路徑平滑實驗對比

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

圖16 家庭環境路徑對比Fig.16 Family environment path comparison

圖17 多障礙物環境路徑對比Fig.17 Multi-obstacle environment path comparison

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

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

圖18 基于原改進骨架算法家庭環境導航路徑Fig.18 Home environment navigation path based on improved skeleton algorithm

表4 室內環境下規劃路徑參數Tab.4 Planning path parameters in an indoor environment

圖19 基于本文算法家庭環境導航路徑Fig.19 Home environment navigation path based on proposed algorithm

圖20 基于原改進骨架算法多障礙物環境導航路徑Fig.20 Multi-obstacle environment navigation path based on improved skeleton algorithm

圖21 基于本文算法多障礙物環境導航路徑Fig.21 Multi-obstacle environment navigation path based on proposed algorithm

5 實際實驗

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

圖22 實驗平臺Fig.22 Experiment platform1.供電電池 2.Intel-NUC 3.顯示器 4.激光雷達 5.光電編碼器

在實驗室搭建如圖23a所示實驗場景，該場景通過圍墻將實驗室劃分為不同尺寸和形狀的區域，并在墻角部分放置一定數量的柜子。機器人在該環境中使用Gmapping建圖算法構建如圖23b所示柵格地圖，采用本文算法對地圖進行預處理并生成Voronoi圖。

表5 多障礙物環境下規劃路徑參數Tab.5 Planning path parameters in a multi-obstacle environment

圖23 實際實驗環境Fig.23 Actual experimental environment

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

圖24 實際實驗骨架Fig.24 Actual experimental skeleton diagram

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

圖25 原改進骨架算法實際導航路徑Fig.25 Actual navigation path of original improved skeleton algorithm

圖27展示了機器人采用本文算法在實際環境中的運行情況。機器人基于本算法運動時能夠避開障礙物并快速到達目標點。

圖26 本文算法實際導航路徑Fig.26 Actual navigation path of proposed algorithm

圖27 機器人實際運行情況Fig.27 Actual operation of robot

6 結束語

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