改進概率路標圖算法

寧新杰，崔 煒，徐照翔，李興廣，陳鵬宇

(長春理工大學 電子信息工程學院，吉林 長春 130022)

0 引 言

目前，常用的路徑規劃算法有遺傳算法[1]、人工勢場法[2]、隨機路標圖(PRM)算法[3]等算法。當環境中的障礙物較為復雜，或在高維空間進行規劃時，將導致規劃算法計算量大大增加，算法效率大大降低。基于隨機采樣的PRM算法可以有效解決高維空間或多障礙物的復雜環境中的路徑規劃問題，因此，PRM算法廣泛應用于在機械臂路徑規劃[4]、建筑物疏散路徑規劃、分支管路自動布局[5]等領域。

為了提高PRM算法中路線圖的連通性，Mali等提出應用多重樣本的節點和路線圖的有效性[6]。Janso L等將隨機采樣序列替換為某種確定性采樣序列，用以實現PRM算法搜索到路徑的漸進最優[7]。楊岱川等對帶有實際地圖障礙物的多個目標點進行先后排序和路徑搜尋，較好地解決了目前存在的移動處理器響應速度慢和不能一次規劃多個目標點路徑的問題[8]。李敏等提出一種定量計算自由空間障礙物數量稠密度的方法來解決窄通道問題[9]。SANTIAGO等將遺傳算法與PRM算法進行結合，用來辨別PRM算法產生的隨機點，但該混合算法規劃的航跡收斂速度慢，只適用于特定場景[10]。然而，大部分學者的學者未將改進方法的計算量考慮在內，造成時間利用率降低，延長規劃到可行路徑的時間，本文使用邊集優化方法，通過對隨機點施加約束，對傳統PRM算法進行改進，對搜索到的路徑進行峰值節點提取，形成一條更利于機器人行走的路徑，并通過仿真實驗驗證改進PRM算法的可行性。

1 傳統PRM算法

PRM算法全稱是概率路標圖算法(probabilistic roadmap)，是一種基于圖論的搜索算法，包含學習階段和查詢階段兩個階段。

1.1 學習階段

在學習階段，路線圖的數據結構是依賴概率的方式為給定場景構建的，路線圖是一個無向圖R=(N,E)，N代表在自由空間Cfree上所生成隨機點集合，E代表連接任意兩個節點的可行路徑的集合。首先使用足夠的隨機點來提供一個相當均勻的自由空間Cfree覆蓋，這些隨機點構成點集N，然后從當前的N中選擇多個節點ni(i=1,2,3,……)構成鄰近集Nc。使用局部規劃器計算ni與ni+1路徑，如果路徑與障礙物碰撞，則舍去，反之，則將路徑(邊)加入到E中。學習階段構建路線圖R供查詢階段使用。

1.2 查詢階段

在查詢階段，路線圖被用來解決輸入場景中的單個路徑規劃問題。嘗試將源點s目標點g分別與N中的節點si,gi進行連接，如果連接成功，在路線圖R=(N,E)中從E中搜索連接si,gi的邊。通過重復計算符合要求的局部路徑，并將它們連接起來，最終，利用搜索算法在起點、路線圖R=(N,E)，PRM算法的算法效率取決于查詢階段的搜索速度，也就是說查詢階段的工作量決定算法的效率。然而，通過傳統學習階段構建的路線圖較為復雜，使用尋路算法進行搜索會耗費大量時間，且生成的路徑含有較多冗余節點。

2 PRM算法的局限性及相應的改進方法

2.1 PRM算法的局限性

傳統路線圖R(N,E)中的邊集E是通過每一個隨機點與其余一定范圍內隨機點連接后，經過可行性邊檢測后，除去與障礙物相交的邊，最終得到的邊集放入E中。然而，通過這種方式得到的邊集，在查詢階段使用搜索算法搜索時耗費的時間較多，從而增加了PRM算法的運行時間[11]。某隨機點n周圍較小范圍內可能存在多個隨機點hi，將n與hi連接后構成邊集E后，在一定程度上，增大了后續查詢階段尋路算法的計算量，延長了尋路所用時間[12]。由于這些隨機點與該隨機點的連接構成邊集E的一部分，因此，在查詢出的路徑中可能在較短距離內發生轉折，這會導致機器人在行駛過程中頻繁的轉向，不利于機器人的能耗節省。

2.2 邊集優化方法

針對于某隨機點周圍較近范圍內存在較多隨機點會增加計算量且搜索出來的路徑中很短距離發生轉折的問題，采用對隨機點進行約束的方法，對于存在于自由空間Cfree的采樣點，以其位置p為圓心，半徑為r1,r2分別繪制一個圓形區域，僅將圓心與圓環內隨機點進行連接，并對連接的邊進行障礙物檢測，判斷各邊是否與障礙物相交，如果相交，則去掉相交的邊，如果不發生碰撞，則將其加入到該點的邊集中。具體如圖1所示，白色區域代表自由空間Cfree，黑色的小圓代表隨機點(將隨機點作為圓心點后進行適當放大)，黑色的橢圓形區域代表障礙物，在500*500的障礙物地圖中選取某隨機點B(XB,YB)，并以其為圓心，以r1,r2為半徑畫圓，且僅將該圓心點與圓環內的隨機點Xi進行連接，進行障礙物檢測，去除碰撞邊后加入到邊集E中。該隨機點B(XB,YB)的邊集應滿足以下條件：①點B與點Xi的歐氏距離小于半徑r1，且大于r2(r1大于r2)；②點B，點Xi是位于自由空間Cfree中的點；③根據所需的機器人行走步長設計r1與r2的大小，r1與r2的差值應不小于30(500*500地圖上)；④當約束外徑一定時，隨著約束內經的增大，算法的時間復雜度逐漸減小。當約束內經一定時，隨著約束外徑的增大，算法的時間復雜度逐漸增大。

圖1 某圓心點的邊集E

2.3 峰值節點提取

傳統方法PRM算法查詢出的路徑容易在一定范圍內發生多次轉折，此外，邊集優化方法也會產生一些冗余節點，這會導致路徑節點增多，路徑長度增加，機器人行駛時間延長的問題。針對這些問題，文中采用道格拉斯-普克抽稀算法[13]對查詢階段搜索到的路徑提取關鍵節點進行二次優化，是用來對冗余節點進行壓縮后提取必要節點。提取關鍵節點的步驟如下：連接起點、終點得到一條準線，之后計算path中除起始點和終點外，剩余結點到準線的歐式距離，選取歐式距離最大的峰值結點，比較其與閾值φ的大小，如果其小于閾值φ，則舍去起點和終點之間的所有點，如果最大距離大于φ，則保留峰值節點，并將原線分割成兩部分，連接起點和峰值節點及峰值節點和終點，并對起點和峰值節點及峰值節點和終點之間的節點重復上述方法。抽稀結果中的節點數目隨著選取閾值φ的增大而減小，使用時根據所需精度來選取合適的閾值，來獲得最好的結果。

以圖2中路徑將道格拉斯-普克算法總結為以下幾步：

(1)如圖2(a)所示，使用虛線連接起點A(x1,y1)和終點H(x2,y2)，形成一條基準線AH，將式(1)和式(2)進行聯立求出a和b的值(a、b值為設一次函數方程時參數，a、b為常數)使用式(3)找到距基準線最大距離Dmax的峰值節點C(x0,y0)。

(2)如圖2(a)所示，過點C向基準線做垂線交AH于點M，使用式(3)求出CM長度為d，如果φ大于d，則舍去B,C,D,E,F等點。如果φ小于d，則用虛線連接AC，HC。

(3)對AC，HC這兩條新的基準線重復步驟(1)，步驟(2)的操作。

(4)如圖2(c)將路徑起點、每次找到的峰值節點和終點連接起來，最終路徑為ACEH

圖2 道格拉斯-普克算法提取峰值節點

ax1+y1+b=0

(1)

ax2+y2+b=0

(2)

(3)

改進后的PRM算法，在學習階段構建線圖時，選擇合適的約束半徑r1=120，r2=50，僅將圓心處的隨機點與半徑為r1和半徑為r2的兩個圓形成的圓環內的隨機點連接起來如圖1所示，然后通過障礙物檢測將無碰撞邊加入到E中，通過這種方式建立好路線圖后通過A*算法在路線圖中搜索到一條連接起點和目標點的無碰撞路徑，使用道格拉斯普克算法對無碰撞路徑進行峰值節點提取操作，形成一條更優路徑，易于滿足機器人行駛需求。

改進后的PRM算法的優點：①在學習階段使用約束圓環，會大幅度的簡化查詢階段搜索集合E中邊的數量，構造的路線圖R較傳統方法構建簡單，一定程度上降低后續查詢階段搜索算法搜索到路徑所用時間；②增加了查詢階段判斷搜索是否會成功，決定PRM搜索是否成功在于起點和目標點能否在一個路線圖中。增加這個判斷是為了減少規劃失敗時浪費在查詢階段的時間；③使用道格拉斯-普克算法對PRM算法查詢階段得到的無碰撞路徑進行峰值節點提取，有效去除冗余節點。

改進后的PRM算法流程如圖3所示。

圖3 改進后的PRM算法流程

改進后的PRM算法的偽代碼：

Input:

n:the number of nodes to put in the roadmap

r1,r2: the radius of two circles

Output:

a roadmap R=(N,E)

(1)N←?,E←?

(2)while |N| < n do

(3)loop

(4)c← a randomly chosen free configuration

(5)if c is collision then

(6)b = RandomNode (c,r)

V←V ∪ { b }

(7)else V← V ∪ { c }

(8)end if

(9)end while

(10)for all c ∈ V do

(11)Nc← a set of candidate neighbors of c chosen

from N

for all v ∈ Nc， in order of increasing D (c,v ) do

(c,v) then

(13) if D(c,n)r2, E←E∪{(c,v) } %%設置約束

(14) update Rs connected components %%更新

(15) end if

(16) end for

(17)end for

(18)R = (N,E)

(19)path=AStaP(E) %%搜索路徑

(20) path=ARrecurisionF(pointsTab,path,Thres-hold) %%設定閾值，加入起點，目標點后進行峰值節點提取

(21)return path

3 仿真驗證

仿真實驗在電腦處理器為Intel(R) Core(TM) i5-5200U，安裝內存為8 GB，64位操作系統Windows 8.1上的matlab2016b中進行。通過對比了傳統的PRM算法和改進后的PRM算法在簡單地圖與復雜地圖下的時間特性和對應的路徑節點個數來驗證改進方法的可行性和有效性。

在500*500的障礙物地圖中，設定機器人起點為(10,10)，目標點為(490,490)，黑色橢圓圖形代表障礙物，線條代表可行性邊，白色空間代表自由空間Cfree，原點(0,0)在圖像左上角，豎直向下為x軸正方向，水平向右為y軸正方向。隨機選取100個隨機點構成點集N，加入起點和終點，并將其與路線圖在約束條件下連接起來，圖4(a)中約束半徑為210，圖4(b)中采用邊集優化方法設定約束外徑為r1=210，約束內經r2=140。

圖4 簡單地圖中傳統PRM算法和 改進PRM算法路線對比

當隨機點為100個或200個時，設定簡單地圖中約束內徑為140，約束外徑為210，復雜地圖圖中約束內經為80，約束外徑為120。由圖4可知，在障礙物已知環境中，使用邊集約束方法對隨機點的邊集施加約束，較傳統PRM算法得到的路線圖，改進后的PRM算法得到的路線圖更加簡單，使得PRM算法在查詢階段用時更少。實驗數據見表1，傳統PRM算法和改進后的PRM算法在不同隨機點數情況下在簡單地圖和復雜地圖上的運行時間。實驗結果表明，當隨機點個數為100時，在簡單地圖和復雜地圖中，改進后的PRM算法運行時間較傳統PRM算法分別減少約10.64%、6.75%。當隨機點個數為200時，在簡單地圖和復雜地圖中，改進后的PRM算法運行時間較傳統PRM算法分別減少約9.68%、7.17%。改進后的PRM算法能在已知障礙物的環境中更加快速規劃出一條無碰撞路徑，減少規劃所用時間，對機器人的實時性有所提高。

為驗證相同約束外徑，不同約束內經時，改進PRM算法的時間特性。首先，設定相同的約束外徑r1=150，同時設定約束內經r2從20至120逐漸變化，使用該邊集約束方法對PRM算法進行改進后運行在簡單地圖上的運行時間數據見表2，運行時間數據是通過運行5次程序取中位數的方法得到的，從表2中數據可以看出，隨著約束內徑的不斷增大，算法的運行時間在不斷減少。這說明邊集約束方法是切實可行的，對減少算法的運行時間是有效的。改進后 的PRM算法較傳統的PRM算法運行時間大大降低，在保證一定路徑搜索成功率的情況下，選擇合適內外約束半徑，表1中改進算法的時間特性還可以繼續提升。

表1 成功規劃到路徑時間對比

表2 簡單地圖相同約束外徑，不同約束內徑運行時間對比(隨機點為100個)

選取同一組隨機點，且隨機點數目為100個的簡單地圖上，圖5(a)表示在傳統的PRM學習階段構建的路線搜索出來的無碰撞路徑，圖5(b)表示使用邊集優化方法對PRM學習階段構建的路線進行簡化后，使用峰值節點提取對路徑進行再優化后查詢出一條無碰撞路徑如圖5(b)所示。從簡單地圖 中可以看出，采用傳統方法查詢出的路徑中，有的節點之間在較短距離內發生多次轉折，如果機器人沿著該路徑進行前進會發生頻繁轉向，不利于機器人的控制，采用改進方法的PRM算法查詢出的路徑中節點數目較少，機器人行駛時轉向次數較少，更利于機器人的控制及能量節省。

圖5 簡單地圖傳統PRM算法和改進PRM算法路徑對比

選取同一組隨機點，且隨機點數目為100個的復雜地圖上，圖6(a)表示在傳統的PRM學習階段構建的路線搜索出來的無碰撞路徑，圖6(b)表示使用邊集優化方法對PRM學習階段構建的路線進行簡化后，先查詢出一條無碰撞路徑，再使用峰值節點提取對路徑進行再優化。從復雜圖中可以看出，搜索到的無碰撞路徑，改進方法較傳統方法發生轉折次數更少，且節點間在較短距離內不會發生轉折，路線更加平滑，充分驗證了峰值節點提取的必要性和有效性。

圖6 復雜地圖傳統PRM算法和改進PRM算法路徑對比

通過將邊集約束方法應用到傳統PRM算法成功查詢出一條無碰撞路徑后，設置合適的閾值φ，將生成的路徑進行峰值節點提取，經過在簡單地圖和復雜地圖仿真實驗后，通過圖5(a)、圖5(b)、圖6(a)、圖6(b)得到的可行路徑數據可以得出表3數據。在簡單地圖中，傳統PRM算法初始路徑的節點個數為7個，設置閾值為38后，改進PRM算法初始路徑節點個數為4個，優化節點個數為3個。在復雜地圖中，傳統PRM算法初始路徑的節點個數為17個，設置閾值為25后，改進PRM算法初始路徑節點個數為10個，優化節點個數為7個。無論是簡單地圖還是復雜地圖，較傳統PRM算法查詢出的無碰撞路徑中節點數目，改進PRM算法后得出的路徑節點更少，路徑更加平滑，更利于機器人的行走。

表3 改進PRM初始路徑與優化路徑實驗數據對比

4 結束語

針對于傳統PRM算法查詢路徑所需時間較長及生成的初始路徑中節點較多等問題，本文通過對點集N中的隨機點采用內外徑約束的邊集優化方法，以減少生成的邊集E的大小，從而減少查詢階段的計算量，最終將峰值節點提取方法應用于路徑的再優化中，可以有效減少路徑中的節點數目。仿真結果表明，相比于傳統PRM算法，改進后的PRM算法具有計算量小，實時性好，路徑中節點數目少等優點，對機器人使用PRM進行路徑規劃的后續開發有一定參考意義。