基于修正PRM算法的智能車輛路徑規劃研究

李瓊瓊，徐溢琪，布升強,2，楊家富*，陳勇

(1.南京林業大學 機械電子工程學院，南京210037; 2.恒生電子股份電子有限公司，杭州 310053)

0 引言

近年來，隨著云計算、大數據等新興技術的發展和5G建設的全面啟動，智能車輛得到了快速發展，路徑規劃技術是智能車輛運動決策和定位導航的基礎[1-3]，最具有代表性的路徑規劃算法有基于地圖的路徑規劃算法、基于生物群體智能行為算法和基于采樣的路徑規劃算法[4-7]。基于采樣的路徑規劃算法有快速擴展隨機樹算法(Rapidly-Exploring Random Tree, RRT)[8]和概率地圖算法(Probabilistic Road Map，PRM)。PRM算法是一種隨機采樣算法[9]，通過構造規劃空間的路網拓撲關系地圖，將連續空間內的路徑規劃問題轉換為拓撲空間內的規劃問題[10-11]，但其在規劃路徑時存在著采樣點選取缺乏導向性、路標圖復用率低和算法搜索效率低等缺點[12]，國內外研究學者針對這些不足進行了大量的改進研究。Kurniawati等[13]于障礙物邊界采樣，評估最優可行區域，優化PRM算法隨機采樣的分散性;鄒善席等[14]在基本PRM算法基礎上引入增加節點的改進策略以優化陷入障礙物的采樣點，但是節點數量的增加使得計算成本較高，從而影響算法的實時性能;Ravankar等[15]采用分層混合PRM算法和人工勢場方法(Artifical Potential Field，APF)進行全局路徑規劃，使用一種節點分布分解的方法將路標圖劃分為高電位和低電位區域，提高了算法搜索路徑的效率;受到鄰相關矩陣的啟發，Esposito等[16]提出一種優化概率路線圖的處理算法，簡化處理自由空間內凸單元與節點數量所需的計算。

本研究針對PRM算法采樣點冗余、路標圖構建質量不穩定等缺點，基于均勻采樣設計一種偽隨機采樣方法，采用雙向增量式碰撞檢測機制，設置路點之間的距離連接閾值，提取規劃路徑的關鍵路點進行平滑處理，改善了概率地圖算法在規劃路徑時的不足。

1 修正PRM算法

1.1 PRM算法

PRM算法包括采樣和查詢階段。

采樣階段：PRM算法在規劃空間內隨機采樣，并通過局部規劃器判斷采樣點的合理性，重復采樣n次生成的有效路點構成集合V,遍歷V，連接所有路點之間的可行路徑擴展至整個規劃空間，形成路標圖G(V,E)，其中V={v1,v2,…,vn}表示路點集合，E={vi,vj|,vi,vj∈V}表示路點之間邊的集合。

算法查詢階段:將起始點qinit與目標點qgoal接入路標圖G(V,E)中，使用圖搜索算法在路標圖G(V,E)中尋找連接起始點qinit與目標點qgoal的無碰撞路徑。

1.2 偽隨機采樣

在PRM算法中，生成的采樣點數量隨著規劃空間的增大而增加，難以實現全局均勻分布，易造成采樣點冗余。由于最短路徑出現在起始點與目標點連線區域附近的概率較大，把該區域視作重點采樣區域，簡稱為空間主軸線區域[17-18],如圖1所示，s表示起點，g表示目標點。

圖1 空間主軸線

構建空間主軸信息。令起始點s坐標為(xs,ys)、目標點g坐標為(xg,yg)，空間主軸線的長度L與偏角θ為

L=||g-s||2

。

(1)

(2)

將長度為L的空間主軸線n等分，得到縱向采樣間距(Nd)為

(3)

參考隨機采樣方式，采樣點對稱分布在空間主軸線附近的扇形區域內，采樣點Pi,j(x,y)的計算公式如下

x=xs+rd×cos(θ+φj)

。

(4)

y=ys+rd×sin(θ+φj)

。

(5)

rd=i×Nd,i=(1,2,…,n)

。

(6)

式中：xs、xs為智能車輛的起始點；rd為采樣半徑，采樣半徑以起始點為圓心；φj∈[-φm,φm]為采樣點的偏向角度；φm為最大偏轉角，用于控制扇形采樣區域的角度，即橫向采樣的范圍。

由圖2(a)和圖2(b)可知，采樣點對稱分布在空間主軸線的兩側，采樣范圍受最大偏轉角φm的控制，隨著φm的增大，采樣點沿著空間主軸方向朝四周擴散。為了使采樣點分布更均勻，沿著空間主軸線調整橫向采樣范圍，采樣范圍調整增量為Δφ=φm/n，調整后采樣點分布情況如圖2(c)和2(d)所示。

圖2 基于空間主軸的采樣方式

依據均勻采樣的特點，統計自由空間內采樣點的個數p，定義橫向采樣層的有效采樣率為R

(7)

式中：N為當前采樣層的總采樣次數；R為有效采樣率，R越大，表示該采樣層的連通性越好，若R過小，表示該采樣層中大部分采樣點落入障礙空間。

若后續采樣層沿用相同的采樣間距，采樣點落入障礙空間的幾率將增大，為了提高采樣點的避障能力，引入隨機增量Δr調整采樣點的采樣間隔，以圖2(d)為基礎，調節隨機增量Δr的取值大小得到圖3，隨機增量Δr的取值越大，采樣點越趨近于隨機分布，Δr的取值越小，采樣點趨近均勻分布。

圖3 基于偽隨機的采樣方式

引入隨機增量后Δr后采樣半徑如公式(8)所示。

(8)

由圖4可知，空心圓點表示調整采樣間距前的采樣點，實心圓點表示調整采樣間距后的采樣點，紅色標記代表落入障礙空間的采樣點，黑色標記代表自由空間中的采樣點，調整采樣間距前，該采樣層的有效采樣率較低(R=0.3)，調整采樣間距提高了有效采樣率(R=0.8)，偽隨機采樣策略提高了采樣點的生成質量。

圖4 采樣點調整示意圖

1.3 雙向增量式碰撞檢測

在傳統的PRM算法中，碰撞檢測采取增量式檢測策略，規劃器按照固定的步長選取路點并檢測該點是否落入障礙空間，為了提高碰撞檢測執行效率，將增量式檢測方法與二分法結合，提出一種雙向增量式檢測策略。

由圖5(a)可知，首先判斷首末端路點的合理性，若路點屬于障礙空間，則結束檢測；若路點屬于自用空間，則沿著首末連接路點雙向逐步選取測試點，并判斷測試點的合理性；若所選的測試點屬于障礙空間，則停止檢測丟棄該路徑，如圖5(b)所示。通過碰撞檢測的路點相互連接，最終形成路標圖。

圖5 雙向增量式檢測策略示意圖

1.4 鄰層連接策略

若選取較多的處于同一采樣層的路點連接形成局部路徑不利于縮短全局路徑長度，考慮到縱向采樣層的分布特點，設置縱向采樣間距的連接閾值為LTH，使得路點由同一采樣層連接轉換為相鄰采樣層連接。

選取基于偽隨機采樣策略和碰撞檢測策略生成的路點(N=20)，得到種連接策略驅動下構建的路標圖，如圖6所示。圖6(a)為全連接策略生成的路標圖，紅色實線代表被篩選出的路徑，圖6(b)鄰層連接策略生成的路標圖。從耗時上分析，采用不同連接策略的構圖用時分別為0.906 s 和0.437 s，鄰層連接的連接方法使得構圖效率優化了48.2%。

圖6 路標圖對比情況

2 路徑平滑

經過算法搜索得到的路徑可能會存在“鋸齒”，需要經過平滑處理后才利于驅動底層控制器的實施，更為貼合智能車輛行駛的實際工況。貝塞爾曲線由于其簡單易設計、可直接生成光滑軌跡等優點被應用于航跡規劃中，n階貝塞爾曲線表達式為

(9)

式中：Pi為貝塞爾曲線的第n+1個控制點；bi,n(t)為Bernstein基函數，該函數的取值情況如公式(10)所示。

(10)

本文選用4階塞爾曲線對修正PRM算法的規劃路徑進行平滑處理，4階塞爾曲線表達式為

B(t)=(1-t)4P0+4P1(1-t)3t+6P2(1-t)2t2+

4P3(1-t)t3+P4t4，t∈[0,1]。

(11)

貝塞爾曲線在任意一點的曲率κ(t)為

(12)

假定規劃路徑path={Pn}由一系列離散點組成(n≥5)，將離散點作為貝塞爾曲線的控制點Pi，根據公式(12)可以得到貝塞爾曲線的曲率κ(P)

(13)

起始點處的貝塞爾曲線的曲率κ(0)為

(14)

在具體實施過程中，提取修正PRM算法搜索路徑的關鍵路點，對關鍵路點間的連線做離散化處理得到貝塞爾曲線的離散控制點Pi，經過公式(9)對離散點做插值擬合，實現路徑的平滑處理。

3 仿真試驗及分析

為驗證修正PRM算法的構圖效率和路徑規劃效率，以基本PRM算法為對比算法，搭建Matlab仿真試驗平臺和ROS小車試驗平臺對修正PRM算法的正確性進行驗證分析，計算機參數為：Windows10，硬盤512 GB，內存為8 G。

3.1 算法構圖效率對比

已知規劃空間如圖7和圖8所示。基本PRM算法和修正PRM算法在采樣階段保持采樣點總數N=m×n相同，其中，m、n分別代表算法的橫、縱向采樣點個數，重點關注規劃路徑長度和路標圖構建時間，重復多次試驗(記錄10次)結果取均值見表1。

表1 算法對比結果

以采樣點N=60為例，分析路標圖構建結果圖7(a)和圖8(a)，在PRM算法中采樣點分布較廣，存在著許多冗余采樣點，修正PRM算法構建的路標圖7(a)，采樣點的位置選擇具有一定的導向性，主要沿著空間主軸線分布，冗余采樣點較少。

圖7 基本PRM算法規劃結果(N=60)

圖8 修正PRM算法規劃結果(N=60)

表1可知，在采樣點數量不多的情況下(N=30,N=60)，算法規劃路徑的長度和路標圖的構建耗時差別不明顯，隨著采樣點數量增加(N=90)，算法的規劃路徑長度結果和構圖時間的差別逐漸增大。

由圖9可知，在其他條件相同的情況下，隨著采樣點數量的增加，路徑光滑程度逐漸改善，但路徑總體趨勢保持不變，說明修正PRM算法求解的路徑解質量較為穩定。

圖9 修正PRM算法規劃結果對比

以圖9(b)為基礎進行路徑平滑處理，得到圖10，藍色實線表示修正PRM算法規劃路徑，黑色空心圓形標識代表關鍵路點，將其作為貝塞爾曲線控制點，平滑處理后得到的路徑(紅線所示)更符合智能車輛行駛工況。

圖10 路徑平滑示意圖

3.2 路徑規劃效率對比

為驗證修正PRM算法的路徑規劃效率，以基本PRM算法為對比算法進行算例試驗。其中，算例1為方形迷宮，算例2為窄通道。在采樣點數量一致的情況下重復執行算例試驗11次。算例1和算例2的仿真試驗結果如圖11所示，11次仿真試驗規劃路徑長度和運行時間取均值，路徑搜索成功次數占總次數的比例為成功率，見表2。

表2 算法效率對比結果

由圖11可知，在算例1試驗中，2種算法中落入障礙空間的采樣點數量相當，但在PRM算法中自用空間內的采樣點分布廣泛，造成了冗余；在修正PRM算法中，采樣點集中分布在空間主軸線的兩側，提高了采樣點的利用率；在算例2試驗中，PRM算法中大部分采樣點落入障礙空間，自用空間內的采樣點極少，影響了路徑解的質量；修正PRM算法中采樣點沿著空間主軸線分布，自用空間內的采樣點數量較多為尋求更優質的路徑解提供了可能。

圖11 算法規劃結果對比

由表2和圖12可知，對于算例1試驗，在采樣點數量較低的情況下(N=30)，修正PRM算法不能成功規劃路徑；當采樣點數量為60時，2種算法規劃路徑長度、運行耗時和成功率差別不大；當采樣點增至90時，修正PRM算法規劃路徑長度和運行耗時明顯優于基本PRM算法。對于算例2試驗，在采樣點數量較低的情況下，2種算法均不能成功求解路徑；隨著采樣點數量的增加，修正PRM算法規劃路徑的成功率更高，且路徑解的質量更為可靠。

圖12 算法成功率對比

3.3 ROS仿真試驗

為了進一步驗證修正PRM算法的可實施性，基于ROS試驗平臺設計仿真試驗。ROS小車的構成如圖13所示。

圖13 ROS小車組成

本文主要討論二維平面環境下的智能車輛路徑規劃問題，使用ROS小車試驗平臺提供的功能包實現激光雷達構建地圖的功能，試驗場地如圖14所示，SLAM建圖效果如圖15所示。基于該環境地圖以ROS小車自身的定位結果為起始點，并指定目標點，執行修正PRM算法，路徑規劃結果如圖16所示。

圖14 場地情況

圖15 SLAM建圖結果

(a)路標圖

從試驗結果可知，修正PRM算法在SLAM構建的仿真的地圖中，通過偽隨機采樣策略與雙向增量式檢測策略，成功建立了路標圖，如圖16(a)所示，并搜索出一條連通起點與目標點的可行規劃路徑，驗證了算法的可行性。

4 結論

針對傳統PRM算法應用于智能車輛路徑規劃存在的不足，提出兩點改進策略。

(1)基于空間主軸線設計一種偽隨機采樣方式，引入隨機增量調節采樣點的波動范圍，優化采樣點的生成質量。

(2)采用雙向增量式碰撞檢測策略和鄰層路點連接策略，減少碰撞檢測的調用次數，提高路標圖的構建速率。

使用4階貝塞爾曲線對規劃路徑進行平滑處理，使其更加符合車輛行駛工況，最后利用Matlab、ROS搭建試驗平臺對修正PRM算法的正確性進行驗證分析。試驗結果表明，修正PRM算法在增強路標圖穩定性、縮短規劃路徑長度和提高算法搜索速率方面具有明顯的優勢。但無論是修正PRM算法、A*算法等都是一種模型驅動，還存在著一定的局限性，結合數據驅動、云網融合技術來進行智能車輛的路徑規劃及避障，最終使智能車輛的技術更加成熟。