基于改進PRM算法的機器人路徑規劃

封 澳,楊錦宇,謝玉陽,孫延康,王璇之,肖 建*

(1.南京郵電大學 集成電路科學與工程學院,江蘇 南京 210023;2.金陵中學 中美班,江蘇 南京 210041;3.南京郵電大學 電子與光學工程學院、柔性電子(未來技術)學院,江蘇 南京 210023)

0 引 言

機器人路徑規劃是機器人領域中十分關鍵的技術之一。現有的路徑規劃算法主要分為基于圖搜索的算法[1-2]、基于采樣的算法[3-4]、基于演化的算法[5]以及基于學習的算法[6-7]。其中,基于采樣的算法主要包括快速探索隨機樹(Rapidly-exploring Random Tree,RRT)算法[8]和概率路線圖(Probabilistic Roadmap,PRM)算法[9-10]。PRM算法由于其魯棒性強、可擴展性好以及算法簡單易實現等優點,被廣泛應用在機器人導航、機械臂路徑規劃等領域。

近年來,研究人員對傳統PRM算法進行了大量的改進研究。邱添等人通過將地圖柵格化劃分,根據每個柵格的威脅等級采用不同的采樣策略,提高了在狹窄通道內的采樣效率[11]。鄒善席等人將落在障礙物中的節點重新采樣,在保證采樣點數目不變的條件下提高了節點的利用率[12];李瓊瓊等人提出了一種以空間主軸線為參照軸的偽隨機采樣策略,優化了采樣點的生成方式,增強了采樣點的導向性[13];薛陽等人使用局部敏感哈希算法構建無向路線圖,加快了無向路線圖的構建速度[14];程謙等人優化了采樣點的連接方式,減少了無向路線圖中的不合理路徑[15];寧新杰等人對規劃出的路徑使用道格拉斯-普克算法進行峰值提取,去除了路徑的冗余節點[16]。針對傳統PRM算法采樣點分布不均勻、路線圖構建效率低以及路徑冗余不平滑等缺點,該文引進Sobol序列優化采樣策略,使采樣點全局均勻分布;通過對節點進行分類并施加連接約束,優化路線圖的構建和搜索效率;采用節點平移算法調整節點位置,使路徑滿足實際空間中最優路徑;最后,使用貝塞爾曲線平滑路徑,使其滿足實際機器人的運動約束。

1 傳統PRM算法

1.1 PRM算法定義

傳統PRM算法的基本原理是利用隨機采樣策略生成一組采樣點,去除落在障礙物中的采樣點后,將相距滿足連通距離的兩個采樣點相互連接建立無向路線圖,通過圖搜索算法在該圖上尋找連通起始點到目標點的一條最短路徑。傳統PRM算法一般需要經過兩個主要階段:采樣階段和查詢階段。

采樣階段:PRM算法從狀態空間C中隨機采樣N個點Xi,i=1,2,…,N,去除落在障礙物中的采樣點后得到一組有效的采樣點集合V;然后遍歷集合中的采樣點進行連通性分析,當d(Xi,Xj)≤R時,點Xi,Xj之間建立一條無向邊,其中d(Xi,Xj)表示采樣點Xi和Xj之間的距離,R是PRM算法的連通半徑;最終PRM算法會得到一個路線圖G(V,E),其中V={X1,X2,…,XM}表示有效采樣點的集合且M≤N,E={Xi,Xj|Xi,Xj∈V}表示兩個有效采樣點之間邊的集合。

查詢階段:將起始點qinit和目標點qgoal接入到路線圖G(V,E)中,使用Dijkstar等圖搜索算法在路線圖G(V,E)中尋找連通起始點qinit和目標點qgoal之間的一條最短路徑。其中,Dijkstra算法是典型基于圖的最短路算法,用于計算一個節點到其他節點的最短路徑[2]。

1.2 傳統PRM算法存在的問題

PRM算法可以在復雜環境中進行路徑規劃,還可以處理多機器人或在多維空間中進行路徑規劃,因此在路徑規劃領域被廣泛應用。但是傳統的PRM算法通常具有以下問題:

(1)受采樣點質量影響:傳統PRM算法中,采樣點位置通常由隨機采樣策略生成,這種采樣策略完全隨機且難以確保每個空間區域均有采樣點,導致算法每次規劃出的路徑長度和路徑質量均有所不同;并且由于不是全局均勻采樣,當采樣點數量較少時,會導致搜索空間不足,難以規劃出有效路徑;當采樣點數量過多時,又會導致效率降低和計算復雜度增加,嚴重影響PRM算法的運行效率。

(2)路線圖構圖效率低下:在路線圖構圖階段,對于任意有效的采樣點,PRM算法均會與其周圍一定距離內可相互連通的點進行連接,并作為路線圖的一個邊;而很多邊對路徑搜索并沒有任何幫助,不僅占用系統計算資源,還嚴重影響路線圖構圖以及路徑搜索的效率。

(3)路徑冗余且不平滑:PRM算法規劃出的路徑由多個采樣點直線連接組合而成,即使在路線圖中搜索到最短路徑,但在實際空間中并不一定是最優路徑;并且路徑上存在許多拐點,還需要對路徑進行平滑處理以提升路徑的行駛效果。

2 改進PRM算法

針對傳統PRM算法存在的問題,該文分別從改進采樣策略,改進路線圖構圖方式以及采用路徑優化與平滑算法三方面進行改進。

2.1 改進采樣策略

為了解決PRM算法采樣點分布不均勻、采樣質量低等問題,該文引進二維Sobol序列改進采樣策略。

Sobol序列是一種基于二進制分割的低差異序列。其對于每個維度,首先將單位區間[0,1]等分為2k個子區間,并編號為0,1,…,2k-1。然后將每個子區間編號轉化為k位二進制數。對于第i行二進制序列bi,1,bi,2,…,bi,k,通過將其轉換為di=bi,1×2k-1+bi,2×2k-2+…+bi,k×20,計算出每行的權重系數ci=di/2k。對于第j個維度上的序列x1,j,x2,j,…,xn,j,根據權重系數可以計算出:

x1,j=cj,1
x2,j=cj,1⊕cj,2
x3,j=cj,1⊕cj,2⊕cj,3
…
xn,j=cj,1⊕cj,2⊕…⊕cj,n

(1)

其中,⊕表示按位異或運算符。由于Sobol序列中的各個維度之間可能存在相關性,為了提高序列的隨機性,通常還需要對序列進行去相關處理并對該維度進行重排,從而保證生成隨機數的質量。

使用隨機采樣策略和Sobol采樣策略分別在空白地圖中采樣相同次數(N=16),采樣結果如圖1(a)和圖1(b)所示。相比于隨機采樣策略,Sobol采樣策略的采樣點分布更加均勻,可以用較少的點覆蓋更大的空間。將地圖以橫向、縱向和棋盤格等分的方式劃分成多個子區域,劃分結果分別如圖1(c)、圖1(d)、圖1(e)所示。Sobol采樣策略可以確保每個地圖子區域中均有采樣點,極大程度上解決了隨機采樣策略因采樣點數量較少導致的搜索空間不足的問題。

圖1 隨機采樣策略和Sobol采樣策略對比

2.2 改進路線圖

為了降低路線圖的大小,提高路線圖的構建、搜索以及算法的運行效率,該文對采樣點分配鄰域屬性并施加連接約束。首先,將與起始點滿足一定范圍且具備連通條件的采樣點稱為第一鄰域點,與第一鄰域內的點滿足一定范圍且具備連通條件的采樣點稱為第二鄰域點,依次類推,直至所有采樣點均被分配一個鄰域屬性。規定起始點只與第一鄰域內的點進行連接,其他采樣點只能與其相鄰的鄰域點進行連接,同一鄰域內的點不進行相互連接。在同一地圖上采樣相同次數(N=40),傳統路線圖和改進路線圖構圖結果如圖2所示。

從圖2可知,在相同采樣點數量且不改變路線圖連通性的前提下,改進路線圖的邊數明顯低于傳統路線圖。經過測試,在相同采樣點數量下,改進路線圖的平均邊數、平均構建耗時以及平均路徑搜索耗時分別比傳統路線圖減少67.89%～73.09%,79.99%～80.73%和24.72%～33.84%。隨著采樣點數量的不斷增加,兩種路線圖的平均邊數、構建時間以及路徑搜索耗時上的差距將越來越明顯。

2.3 路徑優化與平滑策略

針對PRM算法規劃出的路徑在實際空間中并非最優路徑且存在較多拐點的情況,該文提出一種基于節點平移和貝塞爾曲線的路徑優化與平滑策略。

首先,使用Dijkstra算法在路線圖中搜索出連接起始點qinit和目標點qgoal的最短路徑,路徑上的點集用F={qinit,q1,q2,…,qn,qgoal}表示。對于q1到qn中的任意節點qm,將節點qm沿著qm和qm+1之間的連線平移,若節點qm與qm+1之間的連線緊切障礙物安全半徑,則節點qm停止移動并將當前位置作為節點qm的最新位置更新點集F;若節點qm移動到qm+1,則刪除qm節點,使節點qm-1與qm+1直接相連并更新點集F。遍歷節點q1到qn,直至所有節點位置均更新完畢。

圖3顯示了路徑優化的示意圖,圖3(b)中路徑優化步驟如下:

圖3 路徑優化示意圖

經過優化后的路徑長度和質量均有所提升,但仍是由數個節點直線相連而成,存在著不少拐點,因此該文采用貝塞爾曲線平滑路徑,使其符合實際機器人的運動約束。該文選用二階貝塞爾曲線對優化后的路徑進行平滑。路徑平滑示意圖如圖3(c)所示。在具體實施過程中,以固定間隔對優化后的路徑進行離散化,選取路徑節點和距其最近的兩個離散點作為貝塞爾曲線的離散控制點,經過公式2對離散點進行插值擬合,實現對路徑拐點的平滑處理。

P(t)=(1-t)2P0+2P1(1-t)t+P2t2,t∈[0,1]

(2)

圖3(d)展示了原始路徑與優化路徑的對比結果,經過節點平移和貝塞爾曲線優化后的路徑更符合實際機器人的運動約束,且路徑長度比原始路徑減少11.71%。

2.4 改進PRM算法的步驟

改進PRM算法的步驟如下:

步驟1 PRM算法參數初始化:機器人起點、目標點、采樣點數量N、連通半徑R和路徑離散間隔L。

步驟2 二維Sobol策略生成N個采樣點,按照采樣點生成順序依次去除落入障礙物中的無效采樣點,剩余有效采樣點作為路線圖的節點。

步驟3 對有效采樣點分配鄰域屬性,對相鄰鄰域內的采樣點進行距離和連通性分析,符合連通條件則進行連接作為路線圖的一個邊。

步驟4 使用Dijkstra算法在路線圖中搜索連通起始點和目標點的最短路徑。

步驟5 對搜索出的最短路徑使用節點平移優化算法,將最短路徑上的節點進行平移,找到符合實際空間中最優路徑。

步驟6 按照固定間隔對路徑進行離散化,選擇優化后的節點和距其最近的兩個離散點進行二階貝塞爾曲線插值,實現路徑的平滑處理。

改進PRM算法流程如圖4所示。

圖4 改進PRM算法流程

3 改進PRM算法的仿真與結果分析

為了驗證改進PRM算法的有效性和可行性,對傳統PRM算法、文中改進PRM算法和其他改進的PRM算法在Python環境中進行了仿真實驗,所用計算機參數為Intel(R) Core(TM) i7-7700HQ CPU @ 2.80 GHz,內存16 GB,64位Window10操作系統。改進PRM算法初始化參數分別設置為:連通半徑R=100,路徑離散間隔L=10。

3.1 傳統PRM算法比較結果分析

在320×240的狀態空間中,隨機設置機器人的起始點和目標點,傳統PRM算法和改進PRM算法分別在狀態空間中以不同采樣數量進行采樣,采樣點和路徑對比結果如圖5和表1所示。

表1 不同采樣點數量下的算法對比結果

圖5 不同采樣點數量下算法規劃結果對比

從圖5可以看出,當采樣點較少時(N=30),傳統PRM采樣點分布不均勻,部分地區存在采樣點密集或稀疏的情況,且規劃出的路徑存在很多曲折和拐點;隨著采樣點數量的增加(N=90),傳統PRM算法的采樣點分布逐漸改善且規劃出的路徑曲折程度和路徑長度也有所優化。在不同采樣點數量的情況下,改進PRM算法的采樣點分布均勻,不存在采樣點稀疏或密集的情況,且隨著采樣點數量的增加,改進PRM算法規劃的路徑長度及位置幾乎不發生變化,說明改進PRM算法的采樣點和規劃的路徑質量更為穩定。

從表1可知,在采樣點數量分別為30,60和90時,改進PRM算法的耗時比傳統PRM算法的耗時分別減少53.8%,72.7%和74.3%;且當采樣點數量較少時,算法規劃的路徑長度差別明顯,隨著采樣點數量的增加,算法規劃的路徑長度差別逐漸減少;說明改進PRM算法在運行效率上明顯優于傳統PRM算法,且在采樣點數量較低時也能規劃出合理路徑。

3.2 改進PRM算法泛化性分析

為了驗證改進PRM算法的泛化性,該文在不同測試的地圖上以(30,30)為起點、(row-30,col-30)為目標點進行測試,其中(row,col)為測試地圖的大小。在每個測試地圖上分別運行100次,路徑長度和運行時間取多次運行的均值,路徑規劃成功次數占總運行次數的比例為成功率。測試地圖由圖7可知。

由表2可知,在三個不同的測試地圖中,當采樣點數量較少時(N=30),兩種算法的運行成功率隨著地圖面積的增加而逐漸減低,改進PRM算法在路徑長度和運行時間上優于傳統PRM算法;當采樣點數量增加到90時,兩種算法的運行成功率均明顯提高,改進PRM算法在提升率上快于傳統PRM算法;隨著采樣點數量的增加,改進PRM算法的路徑長度幾乎不變,傳統PRM算法的路徑長度減少,兩種算法之間的路徑差距降低;雖然運行時間均有所增加,但改進PRM算法在運行時間上明顯優于傳統PRM算法。

表2 不同地圖下算法對比結果

3.3 其他改進PRM算法比較結果分析

為了進一步驗證改進PRM算法的性能,與其他改進PRM算法在測試地圖中以成功率、平均路徑長度和平均運行時間為評價指標進行測試。其他改進PRM算法介紹如下:

改進PRM算法1:寧新杰等人提出的改進PRM算法[16],該算法在傳統PRM算法中加入邊集優化方法對采樣點進行約束,避免距離較近的采樣點相互連接,減少路線圖構圖的大小;并對規劃出的路徑使用道格拉斯-普克算法進行峰值點提取,去除冗余節點。

改進PRM算法2:李瓊瓊等人提出的修正PRM算法[13],該算法設計了一種以空間主軸線為參照軸的偽隨機采樣策略,采用鄰層連接策略并使用雙向遞增方法進行碰撞檢測,優化碰撞檢測調用次數,最后對規劃出的路徑使用四階貝塞爾曲線進行平滑處理。

將傳統PRM算法和三種改進PRM算法在測試地圖中分別以不同采樣點總數測試100次,比較其成功次數,測試結果如圖6所示。

(a)測試地圖1 (b)測試地圖2 (c)測試地圖3

從圖6可以看出,由于改進PRM算法1繼續沿用了傳統PRM的隨機采樣策略,因此,該算法在三種測試地圖上的成功率與傳統PRM算法接近;改進PRM算法2優化了采樣點的生成方式,但這種采樣策略只是在空間主軸線附近均勻采樣,并非全局均勻采樣,因此,該算法在大多數測試情況中的運行成功率比其他測試算法的低;該文改進的PRM算法采用二維Sobol序列改進采樣策略,使采樣點均勻分布在狀態空間中,在運行成功率上優于其他測試算法。

將不同改進PRM算法以相同采樣點數目(N=90)分別在測試地圖1,2和3中測試100次,平均路徑長度和平均運行時間對比結果如表3所示。為了更直觀地展示不同改進PRM算法之間的對比結果,其中一次規劃路徑的結果如圖7所示。

表3 不同改進PRM算法在測試地圖下的對比結果

從表3和圖7可以看出,改進PRM算法1使用道格拉斯-普克算法去除冗余節點,優化后的路徑長度與文中改進的PRM算法的路徑長度接近,但其并未對路徑進行平滑處理,不符合實際機器人的運動約束;改進PRM算法2使用四階貝塞爾曲線對路徑進行平滑處理,但其并未改變路徑節點的位置,導致規劃出的路徑長度并非最優;文中改進的PRM算法使用節點平移算法優化節點位置,并使用二階貝塞爾曲線對路徑拐點進行平滑處理,在保證路徑平滑的同時維持了較短的路徑長度。

傳統PRM算法最耗時的部分為構建無向路線圖,改進PRM算法1在構建無向路線圖時加入了邊集優化方法,避免距離較近的采樣點相互連接,減少了構圖的大小,但其優化性能有限,在三種優化算法中耗時最長;改進PRM算法2在構建無向路線圖時采用鄰層連接,并使用雙向遞增方法進行碰撞檢測,進一步縮短了算法的運行時間;文中改進的PRM算法對所有采樣點進行鄰域分類,并規定相鄰鄰域采樣點相互連接,相同鄰域采樣點不進行連接,減少了與路徑規劃無關的邊被加入到無向路線圖中的概率,算法的運行時間在三種優化算法中最短。

通過上述實驗可以得到以下結論:

(1)采用二維Sobol序列優化采樣策略,用較少的采樣點覆蓋更多區域,保證全局均勻采樣,重復運行時算法規劃出的路徑長度波動更小,更為穩定;

(2)對全局采樣點進行鄰域分類并施加連接約束,在保證路線圖連通性的前提下減少了構圖及路徑搜索的時間,提高了算法的運行效率;

(3)使用節點平移優化算法優化路徑節點位置,并使用貝塞爾曲線平滑路徑拐點,降低了規劃路徑受采樣點質量的影響,保證了較短的路徑長度,有效提升規劃路徑的質量;

(4)在測試地圖上進行的大量實驗結果表明,相比于傳統PRM算法和其他改進PRM算法,文中改進的PRM算法在路徑長度、運行時間以及成功率上擁有更好的表現。

4 結束語

針對傳統PRM算法采樣點分布不均勻、路線圖構圖效率低以及路徑冗余不平滑等缺點,分別采用二維Sobol序列改進采樣策略,優化采樣點分布;通過對節點進行鄰域分類并施加連接約束,提高路線圖的構圖和搜索效率;最后采用節點平移優化算法去除冗余路徑并使用貝塞爾曲線平滑路徑拐點,提高PRM算法規劃路徑的質量。在不同大小和布局的測試地圖中進行的大量仿真實驗表明,相比于傳統PRM算法和其他改進PRM算法,文中改進的PRM算法在路徑長度、運行時間以及成功率上具有明顯優勢。