關鍵節點和平滑處理的PRM路徑優化方法

魏念巍，姜媛媛，劉延彬，辛元芳，洪 炎

安徽理工大學 電氣與信息工程學院，安徽 淮南 232001

1 引言

路徑規劃是機器人研究領域的一項基礎并且關鍵的技術。路徑規劃技術迅速發展，研究人員提出諸如單元分解法、快速搜索隨機樹（Rapidly Exploring Random Tree，RRT）法[1]、人工勢場法[2]、概率路圖（Probabilistic Roadmap，PRM）[3]法等各種路徑規劃算法[4]。一些傳統的規劃算法需要對圖中的障礙物進行建模確定其位置，計算難度及規劃路徑變得復雜。基于采樣的PRM算法可以在含有障礙物的地圖中規避障礙物，并在自由空間內規劃路徑，降低了計算難度。基于采樣的規劃方法不僅應用在各種機器人的路徑規劃方面[5]，在建筑物疏散路徑規劃、分支管路自動布局[6]、計算機動畫、生物學等其他領域也被運用[7]。

為了提高PRM 算法中路線圖的連通性，Huang 和Gupta提出應用Delaunay三角剖分特性的臨近點選擇策略[8]。Mali提出使用多重樣本節點以及節點和地圖路線的有效性等特征提高PRM 的表現力[9]。Janson 等人提出使用某種確定性采樣序列替代隨機性采樣序列實現PRM路徑的漸進最優[10]。

采用基本PRM算法進行路徑規劃所形成的路徑含有的拐點過多，在需大幅轉向的位置路徑不夠平滑。機器人在路徑的拐點處需要調整姿態完成轉向，過多地調整姿態會使得整體的行駛時間加長，整體行駛性能下降[11]。初始路徑不夠平滑使得部分路徑不能遵循機器人的行駛規律，使得路徑不能確定可行。在此問題的基礎上提出使用D-P算法提取PRM初始路徑節點中的關鍵節點，以降低機器人調整姿態的次數，從而降低行駛時間。基本的D-P 算法對復雜曲線簡化時易產生自相交等錯誤，閾值的選取對最終簡化結果有較大影響，閾值過大會導致局部區域失真，閾值過小不能有效化簡曲線。劉波等人提出基于單調鏈與二分法的D-P 改進方法解決壓縮復雜曲線時的自相交問題[12]。Zhai 等人將D-P 算法與多路樹模型相結合用于簡化線性特征質量評估，無需設置 D-P 算法的閾值[13]。Zhao 和 Shi 考慮D-P算法閾值可變性，根據船舶尺寸和水域情況選擇閾值實現對船舶軌跡的壓縮[14]。路徑的平滑處理使用Clothoid曲線進行擬合。Clothoid曲線是一種曲率連續的平面參數曲線，可使擬合曲線連續、平滑[15]，最初在道路以及橋梁的設計中應用較多，后來被應用于機床加工[16]、無人機航行軌跡[17]等方面。

2 基本PRM算法

PRM 算法是一種基于圖搜索的方法，基本的PRM算法包括學習階段和查詢階段。學習階段主要是構建路徑網絡圖，首先在給定地圖的自由空間(Cfree)中隨機撒點，然后通過局部規劃器連接節點，形成路徑網絡圖。查詢階段是將給定的初始節點和目標節點與路徑圖相連接，并通過A*等搜索算法找到起始節點到目標節點的路徑。

學習階段構造路徑網絡圖R(N,E)的過程如下：

算法1 PRM

1.N←NULL

2.E←NULL

3.loop

4.c←a randomly chosen free configuration

5.N←N∪{c}

6.Nc←a set of candidate neighbors ofcchosen fromN

7.for alln∈Nc, in order of increasingD(c,n) do

8.if (c,n) is not same connected component ofEand Δ(c,n)≠NIL then

9.E←E∪(c,n)

10.updateR(N,E)

R(N,E)為無向圖，其中N為隨機點的點集，E為所有可能兩點連接的路徑集。首先對R(N,E)初始化，N、E初始狀態為空。然后在自由空間內隨機撒點，每個隨機點要確保機器人不與障礙物相碰撞。在點集N中選取ni作為ci的鄰域點構成鄰域點集Nc，鄰域點的距離要在一定范圍內，即Nc={n∈N|D(c,n)＜d}，d為設定的最大允許距離。使用局部規劃器連接采樣點c以及其鄰域點n，生成一條無碰撞路徑。如果連接線段與威脅體碰撞，則不能相連，即保持連接線段全部處于自由空間中。若經過碰撞檢測，鄰域點集Nc中存在節點n能與隨機采樣點c相連，將相應的連線作為圖的邊加入到對應的路線子圖中。若新采樣點與其鄰近點集不存在直線段相連，則把該采樣點作為一個新的子圖存儲。當采樣點可以同時與多個子圖連接時，被連接的子圖就通過新采樣點合并成一個子圖。隨著采樣點的增多，不同的子圖連接在一起，當所有子圖連接成一張路線圖并滿足采樣點密度等其他終止條件時，路徑網絡圖構建完成。圖1 為PRM 算法在學習階段構建的路徑網絡圖。

圖1 路徑網絡圖

在查詢階段將初始點s和目標點g與路徑網絡中的兩個節點ci、cj分別連接，然后在無向路徑網絡中尋找ci與cj之間的路徑，由此可構建s節點與g節點之間的路徑。在學習階段的關鍵在于如何尋找s到ci以及g到cj的路徑，可以采用學習階段的局部規劃器的方法來尋找無碰撞路徑。學習階段尋找到的一條機器人路徑如圖2所示。

3 路徑優化

PRM 算法在構建路徑網絡圖時采用隨機采樣策略，采樣點隨機分布在自由空間Cfree中，在查詢階段搜索路徑時依賴于采樣點的分布。若在構建網絡圖時選擇的采樣點過少，則隨機采樣策略不能保證采樣點覆蓋整個自由空間，最終導致規劃路徑失敗；若選取過多的采樣點，則會增加算法的計算量，延長路徑規劃時間，路徑節點個數會大量增加，最終形成的機器人路徑包含的拐點過多。雖然研究人員提出了在構建路徑網絡圖時的改進方法，如在自由空間內采樣時用均勻采樣代替隨機采樣以保證采樣點均勻分布在整個自由空間，或采用高斯采樣或橋測試等方法增強在障礙物周圍的連通性，提高計算效率。但為了保證路徑規劃成功，并不能選取過少的采樣點，故規劃路徑仍存在拐點過多的問題。由于機器人需在拐點處調整姿態轉向，這使得機器人整體行駛時間加長。由于規劃所得的路徑為在路徑網絡圖中兩節點間的最短路徑，路徑中存在的斜線多，一些拐角過陡。路徑的曲率突然改變，會影響到機器人的整體行駛性能和效率。機器人不能急轉彎，需要一定的角度才能完成轉向，初始路徑包含一些機器人無法遵循的急轉彎。針對這一問題，提出一種路徑優化方法，使用D-P算法以及Clothoid 曲線提取路徑關鍵節點并進行平滑處理。優化過程如圖3所示。

圖2 PRM路徑

圖3 路徑優化過程

在一給定障礙地圖中，首先使用PRM 算法進行路徑規劃，構建自由空間內的路徑網絡圖，然后通過查詢階段得到一條從起始點到目標點的初始機器人路徑。對于初始路徑中的路徑節點使用D-P 算法提取其中的關鍵節點，連接路徑關鍵節點形成的是含有較少拐點的優化路徑。使用Clothoid 曲線進行擬合可以得到最終的比較平滑的機器人路徑。

3.1 關鍵節點的提取

在PRM算法形成的初始路徑中，路徑節點過多，機器人在拐點處需要調整姿態轉彎，拐點過多使得路徑過長，機器人行駛時間加長。提取路徑節點中的關鍵節點進行路徑優化可以有效減少路徑的總長以及機器人行駛時間。

算法2 D-P算法

1.Function DP=D-P_original({N1,N2,…,Nn},φ)

2.DP=NULL

2.Fit a linel withN1andNn

4.D←{d1,d2,…,dn} //求各節點{N1,N2,…,Nn}到基準線l的距離{d1,d2,…,dn}

5.for alldi∈Ddo

6.dmax←max{d1,d2,…,dn} and pointNmaxcorresponding todmax

7.ifdmax≤φthen

8.DP←{DP,N1,Nn}

9.else

10.DP←{DP,D-P_max(N1,Nmax)} and DP←{DP,D-P_max(Nmax,Nn)}

11.end if

12.return DP

使用D-P算法提取路徑關鍵節點的基本步驟如下：首先確定一閾值φ，連接初始路徑節點的初始點和目標點形成一條基準線。然后計算初始點和目標點之間所有節點到基準線的距離，得到距離基準線最遠的節點。比較此距離與預先給定的閾值φ的大小。如果小于φ，則該基準線段作為新的路徑，該段路徑處理完畢。如果距離大于給定的閾值φ，把此節點納入關鍵節點集，該關鍵節點分別與初始點和目標點相連接形成兩條新的基準線，并對這兩段基準線重復上面步驟提取新的關鍵節點。最后可以得到關鍵節點集，依次連接關鍵節點，即可得到新的優化路徑。閾值的選取對路徑優化的影響很大，閾值過大雖然會有效減少拐點個數，但無法確保優化路徑可行性；閾值過小不能有效減少路徑中拐點的個數。故需根據地圖障礙物的分布情況以及初始路徑節點個數來確定D-P算法的閾值，在含有較多障礙物的復雜地圖中，選取較小的閾值有助于保證優化路徑成功可行；在含較少障礙物或障礙物分布簡單的地圖中，可選擇較大閾值提取路徑節點中關鍵節點。

以圖4 的路徑為例，初始路徑節點為A1～A8。設定一合適閾值，連接A1、A8，在A1和A8之間的路徑節點中距離線段A1A8最遠的節點為A3,A3與A1A8的距離為d，大于預先設定的閾值φ，把A3視為一個關鍵節點。分別連接A1、A3和A3、A8形成兩條新的基準線段A1A3和A3A8。由圖4（b）可以看出，在A1與A3之間與線段A1A3距離最遠的路徑節點為A2，在A3和A8之間距離線段A3A8最遠的路徑節點為A5，分別與閾值相比較，找出新的路徑關鍵節點。依次重復下去可以得到路徑關鍵節點集，連接路徑關鍵節點即可得到圖4（c）所示的新路徑B1—B2—B3—B4。

圖4 D-P算法提取路徑關鍵節點

3.2 路徑平滑處理

Clothoid曲線是一種參數平面螺旋曲線。該曲線的最大特點是：以弧長為參變量及曲率連續變化，并囊括直線與圓兩種特例。Clothoid曲線的曲率變化與曲線的弧長成正比，Clothoid曲線表達式如式（1）：

式中，(x0,y0)為初始點，θ0表示初始切線角，k0為初始曲率，c表示曲率銳度的參數，s表示曲線的弧長。Clothoid曲線的曲率k以及切線角可以表示為式（2）：

Clothoid曲線大多應用在根據已知的一系列離散坐標點，在特定的條件下生成曲率連續變化的曲線。若設D-P算法提取的關鍵節點是坐標為P(xi,yi)(i=1,2,…,k)的一組離散點，那么使用Clothoid 曲線進行擬合，就是在曲率連續的條件下求解上述k個離散點的各Clothoid曲線段。對一組離散點用Clothoid 曲線進行擬合形成的曲線如圖5所示。

對于其中第l段Clothoid 曲線來說，其兩端端點坐標為(xl,yl)、(xl+1,yl+1)，由式（1）可知兩點應滿足下列條件：

圖5 一組離散點和Clothoid曲線

其中，sl為第l個Clothoid曲線段的弧長，θ0l以及k0l分別表示(xl,yl)點處的切線角和曲率。為了保證在各個點處的曲率連續性和曲線平滑性，(xl+1,yl+1)作為第l段Clothoid曲線的終點以及第l+1 段Clothoid曲線的起點，此處各參數應當滿足：

式中，θ0l+1、k0l+1分別表示第l+1 段Clothoid曲線的初始切線角和初始曲率，分別與第l段Clothoid 曲線在(xl+1,yl+1)處的切線角θl以及曲率kl相等。即在兩段Clothoid曲線的連接處的切線角和曲率的大小不變。

4 仿真結果

使用Matlab進行仿真，在如圖6所示的含障礙物的250 m×250 m大小的地圖中，設定機器人的起始點坐標為（10，240），機器人的目標點坐標為（240，10）。選擇隨機點的個數為500，使用基本PRM方法進行路徑規劃得到的初始機器人路徑如圖6 所示。圖中的節點為機器人路徑的路徑節點，連線為初始路徑。

圖6 PRM路徑規劃的初始路徑

由圖6可以看出，經基本PRM方法進行路徑規劃得到的初始路徑共有14 個路徑節點，在此基礎上進行路徑優化，選擇D-P算法的閾值為4，提取路徑節點中的關鍵節點，并使用Clothoid 曲線對路徑關鍵節點進行擬合，得到最終較為平滑的新路徑如圖7所示。圖中點劃線路徑為初始路徑，星點為經D-P算法提取的路徑節點中的關鍵節點，虛線為關鍵節點組成的新路徑，實線路徑為經Clothoid曲線平滑處理過后的平滑優化路徑。

圖7 經優化后的平滑路徑

由圖7 可知，經D-P 算法提取路徑節點中的關鍵節點，路徑節點由原先的14 個縮減為5 個關鍵節點，有效減少了機器人路徑中拐點的個數，經Clothoid曲線平滑處理后的最終路徑與初始路徑相比更加平滑。

在圖8所示的較為復雜的250 m×250 m大小的障礙地圖中，選擇機器人的起始點坐標為（10，10），目標點坐標選為（10，120），由PRM 方法進行路徑規劃后得到的初始路徑如圖8所示。

圖8 較復雜地圖PRM路徑規劃的初始路徑

由圖8 可以看出，機器人路徑共有27 個路徑節點，經過圓形障礙物時需大幅轉向行駛。初始路徑在此處的拐角過陡，有突出棱角，機器人需在拐點處調整姿態來完成轉向，路徑不夠平滑，使得機器人的行駛時間加長。選擇D-P 算法的閾值為4，提取路徑節點中的關鍵節點，再經平滑處理后的最終路徑如圖9所示。

圖9 較復雜地圖經優化后的平滑路徑

由圖9可知，優化后的平滑路徑的路徑節點由原先的27 個減少為12 個，有效減少了機器人行駛過程中的拐點個數。在圓形障礙物處的機器人路徑變得更加平滑，無較突出的棱角。經過優化后的最終路徑整體比較平滑，能有效減少機器人的行駛時間，并提高機器人的整體行駛性能。

在圖10 的障礙地圖中，設機器人初始點坐標為（10，10），目標點坐標為（10，240），機器人行駛路徑較復雜。使用基本的PRM方法進行路徑規劃得到的初始路徑如圖10所示。

圖10 復雜路徑經PRM路徑規劃的初始路徑

初始路徑中的路徑節點為23 個，由于路徑較為復雜，初始路徑含有較多的尖銳拐角，有較多的急轉彎，這使得機器人在行駛過程中需要經常大幅度調整姿態形成轉向，機器人的整體行駛時間加長。選擇D-P算法的閾值為3，提取路徑關鍵節點，Clothoid曲線進行平滑處理得到的平滑路徑如圖11所示。

經優化后的最終路徑的路徑節點的個數由23個減為14 個。最終路徑與初始路徑相比，在較為尖銳的拐角處更加平滑，能夠有效減少機器人在拐角處調整姿態所需的時間。經優化處理后的最終路徑與初始路徑相比，拐點個數明顯減少，平滑性明顯提高，可以有效降低機器人的整體行駛時間。

表1 PRM初始路徑和優化平滑路徑實驗數據比較

圖11 復雜路徑經優化后的平滑路徑

由表1可以看出，相較于基本PRM規劃路徑形成的初始路徑，經優化后的最終平滑路徑的路徑節點個數以及路徑轉折次數明顯減少，相應地機器人的拐點個數也會降低。與簡單障礙地圖相比，較復雜的障礙地圖以及復雜路徑障礙地圖中初始PRM路徑節點以及路徑轉折次數較多，經優化處理后的平滑路徑可以有效降低機器人拐點的個數。

5 結束語

移動機器人路徑規劃使用基本PRM算法生成的初始路徑中拐點個數過多，采用D-P算法提取初始路徑節點中的關鍵節點，從而減少機器人行駛路徑拐點的個數。針對路徑部分拐角過陡和路徑不夠平滑的問題，使用Clothoid曲線平滑路徑。仿真結果表明，在復雜路徑或復雜障礙地圖情況下該優化方法都可有效減少路徑中拐點的個數并使路徑更加平滑。