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

李瓊瓊，徐溢琪，布升強(qiáng),2，楊家富*，陳勇

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

0 引言

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

本研究針對(duì)PRM算法采樣點(diǎn)冗余、路標(biāo)圖構(gòu)建質(zhì)量不穩(wěn)定等缺點(diǎn)，基于均勻采樣設(shè)計(jì)一種偽隨機(jī)采樣方法，采用雙向增量式碰撞檢測(cè)機(jī)制，設(shè)置路點(diǎn)之間的距離連接閾值，提取規(guī)劃路徑的關(guān)鍵路點(diǎn)進(jìn)行平滑處理，改善了概率地圖算法在規(guī)劃路徑時(shí)的不足。

1 修正PRM算法

1.1 PRM算法

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

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

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

1.2 偽隨機(jī)采樣

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

圖1 空間主軸線

構(gòu)建空間主軸信息。令起始點(diǎn)s坐標(biāo)為(xs,ys)、目標(biāo)點(diǎn)g坐標(biāo)為(xg,yg)，空間主軸線的長(zhǎng)度L與偏角θ為

L=||g-s||2

。

(1)

(2)

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

(3)

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

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

。

(4)

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

。

(5)

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

。

(6)

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

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

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

依據(jù)均勻采樣的特點(diǎn)，統(tǒng)計(jì)自由空間內(nèi)采樣點(diǎn)的個(gè)數(shù)p，定義橫向采樣層的有效采樣率為R

(7)

式中：N為當(dāng)前采樣層的總采樣次數(shù)；R為有效采樣率，R越大，表示該采樣層的連通性越好，若R過(guò)小，表示該采樣層中大部分采樣點(diǎn)落入障礙空間。

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

圖3 基于偽隨機(jī)的采樣方式

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

(8)

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

圖4 采樣點(diǎn)調(diào)整示意圖

1.3 雙向增量式碰撞檢測(cè)

在傳統(tǒng)的PRM算法中，碰撞檢測(cè)采取增量式檢測(cè)策略，規(guī)劃器按照固定的步長(zhǎng)選取路點(diǎn)并檢測(cè)該點(diǎn)是否落入障礙空間，為了提高碰撞檢測(cè)執(zhí)行效率，將增量式檢測(cè)方法與二分法結(jié)合，提出一種雙向增量式檢測(cè)策略。

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

圖5 雙向增量式檢測(cè)策略示意圖

1.4 鄰層連接策略

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

選取基于偽隨機(jī)采樣策略和碰撞檢測(cè)策略生成的路點(diǎn)(N=20)，得到種連接策略驅(qū)動(dòng)下構(gòu)建的路標(biāo)圖，如圖6所示。圖6(a)為全連接策略生成的路標(biāo)圖，紅色實(shí)線代表被篩選出的路徑，圖6(b)鄰層連接策略生成的路標(biāo)圖。從耗時(shí)上分析，采用不同連接策略的構(gòu)圖用時(shí)分別為0.906 s 和0.437 s，鄰層連接的連接方法使得構(gòu)圖效率優(yōu)化了48.2%。

圖6 路標(biāo)圖對(duì)比情況

2 路徑平滑

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

(9)

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

(10)

本文選用4階塞爾曲線對(duì)修正PRM算法的規(guī)劃路徑進(jìn)行平滑處理，4階塞爾曲線表達(dá)式為

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

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

(11)

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

(12)

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

(13)

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

(14)

在具體實(shí)施過(guò)程中，提取修正PRM算法搜索路徑的關(guān)鍵路點(diǎn)，對(duì)關(guān)鍵路點(diǎn)間的連線做離散化處理得到貝塞爾曲線的離散控制點(diǎn)Pi，經(jīng)過(guò)公式(9)對(duì)離散點(diǎn)做插值擬合，實(shí)現(xiàn)路徑的平滑處理。

3 仿真試驗(yàn)及分析

為驗(yàn)證修正PRM算法的構(gòu)圖效率和路徑規(guī)劃效率，以基本PRM算法為對(duì)比算法，搭建Matlab仿真試驗(yàn)平臺(tái)和ROS小車試驗(yàn)平臺(tái)對(duì)修正PRM算法的正確性進(jìn)行驗(yàn)證分析，計(jì)算機(jī)參數(shù)為：Windows10，硬盤512 GB，內(nèi)存為8 G。

3.1 算法構(gòu)圖效率對(duì)比

已知規(guī)劃空間如圖7和圖8所示。基本PRM算法和修正PRM算法在采樣階段保持采樣點(diǎn)總數(shù)N=m×n相同，其中，m、n分別代表算法的橫、縱向采樣點(diǎn)個(gè)數(shù)，重點(diǎn)關(guān)注規(guī)劃路徑長(zhǎng)度和路標(biāo)圖構(gòu)建時(shí)間，重復(fù)多次試驗(yàn)(記錄10次)結(jié)果取均值見(jiàn)表1。

表1 算法對(duì)比結(jié)果

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

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

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

表1可知，在采樣點(diǎn)數(shù)量不多的情況下(N=30,N=60)，算法規(guī)劃路徑的長(zhǎng)度和路標(biāo)圖的構(gòu)建耗時(shí)差別不明顯，隨著采樣點(diǎn)數(shù)量增加(N=90)，算法的規(guī)劃路徑長(zhǎng)度結(jié)果和構(gòu)圖時(shí)間的差別逐漸增大。

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

圖9 修正PRM算法規(guī)劃結(jié)果對(duì)比

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

圖10 路徑平滑示意圖

3.2 路徑規(guī)劃效率對(duì)比

為驗(yàn)證修正PRM算法的路徑規(guī)劃效率，以基本PRM算法為對(duì)比算法進(jìn)行算例試驗(yàn)。其中，算例1為方形迷宮，算例2為窄通道。在采樣點(diǎn)數(shù)量一致的情況下重復(fù)執(zhí)行算例試驗(yàn)11次。算例1和算例2的仿真試驗(yàn)結(jié)果如圖11所示，11次仿真試驗(yàn)規(guī)劃路徑長(zhǎng)度和運(yùn)行時(shí)間取均值，路徑搜索成功次數(shù)占總次數(shù)的比例為成功率，見(jiàn)表2。

表2 算法效率對(duì)比結(jié)果

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

圖11 算法規(guī)劃結(jié)果對(duì)比

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

圖12 算法成功率對(duì)比

3.3 ROS仿真試驗(yàn)

為了進(jìn)一步驗(yàn)證修正PRM算法的可實(shí)施性，基于ROS試驗(yàn)平臺(tái)設(shè)計(jì)仿真試驗(yàn)。ROS小車的構(gòu)成如圖13所示。

圖13 ROS小車組成

本文主要討論二維平面環(huán)境下的智能車輛路徑規(guī)劃問(wèn)題，使用ROS小車試驗(yàn)平臺(tái)提供的功能包實(shí)現(xiàn)激光雷達(dá)構(gòu)建地圖的功能，試驗(yàn)場(chǎng)地如圖14所示，SLAM建圖效果如圖15所示。基于該環(huán)境地圖以ROS小車自身的定位結(jié)果為起始點(diǎn)，并指定目標(biāo)點(diǎn)，執(zhí)行修正PRM算法，路徑規(guī)劃結(jié)果如圖16所示。

圖14 場(chǎng)地情況

圖15 SLAM建圖結(jié)果

(a)路標(biāo)圖

從試驗(yàn)結(jié)果可知，修正PRM算法在SLAM構(gòu)建的仿真的地圖中，通過(guò)偽隨機(jī)采樣策略與雙向增量式檢測(cè)策略，成功建立了路標(biāo)圖，如圖16(a)所示，并搜索出一條連通起點(diǎn)與目標(biāo)點(diǎn)的可行規(guī)劃路徑，驗(yàn)證了算法的可行性。

4 結(jié)論

針對(duì)傳統(tǒng)PRM算法應(yīng)用于智能車輛路徑規(guī)劃存在的不足，提出兩點(diǎn)改進(jìn)策略。

(1)基于空間主軸線設(shè)計(jì)一種偽隨機(jī)采樣方式，引入隨機(jī)增量調(diào)節(jié)采樣點(diǎn)的波動(dòng)范圍，優(yōu)化采樣點(diǎn)的生成質(zhì)量。

(2)采用雙向增量式碰撞檢測(cè)策略和鄰層路點(diǎn)連接策略，減少碰撞檢測(cè)的調(diào)用次數(shù)，提高路標(biāo)圖的構(gòu)建速率。

使用4階貝塞爾曲線對(duì)規(guī)劃路徑進(jìn)行平滑處理，使其更加符合車輛行駛工況，最后利用Matlab、ROS搭建試驗(yàn)平臺(tái)對(duì)修正PRM算法的正確性進(jìn)行驗(yàn)證分析。試驗(yàn)結(jié)果表明，修正PRM算法在增強(qiáng)路標(biāo)圖穩(wěn)定性、縮短規(guī)劃路徑長(zhǎng)度和提高算法搜索速率方面具有明顯的優(yōu)勢(shì)。但無(wú)論是修正PRM算法、A*算法等都是一種模型驅(qū)動(dòng)，還存在著一定的局限性，結(jié)合數(shù)據(jù)驅(qū)動(dòng)、云網(wǎng)融合技術(shù)來(lái)進(jìn)行智能車輛的路徑規(guī)劃及避障，最終使智能車輛的技術(shù)更加成熟。