(南京林業(yè)大學(xué)機(jī)械電子工程學(xué)院，江蘇 南京 210037)

隨著無(wú)人駕駛技術(shù)的快速發(fā)展，作為無(wú)人車駕駛關(guān)鍵技術(shù)之一的路徑規(guī)劃得到了廣泛的關(guān)注和研究。路徑規(guī)劃是無(wú)人車完成各種任務(wù)的基礎(chǔ)，也是無(wú)人駕駛研究的核心問(wèn)題。無(wú)人車路徑規(guī)劃是指在一定的環(huán)境模型基礎(chǔ)上，給定起始點(diǎn)和目標(biāo)點(diǎn)，按照性能指標(biāo)規(guī)劃出一條無(wú)碰撞、能安全到達(dá)目標(biāo)點(diǎn)的有效路徑。近年來(lái)，研究人員提出了各種有效的算法：如基于圖像檢測(cè)的算法[1]，基于節(jié)點(diǎn)搜索的Dijkstra算法[2]和A*算法[3]，基于啟發(fā)式搜索的蟻群算法[4-5]和粒子群算法[6]，基于地圖搜索的快速擴(kuò)展隨機(jī)樹算法[7]和概率圖算法[8]。

其中，以概率路圖法(Probabilistic RoadMap，PRM)為代表的基于地圖搜索的路徑規(guī)劃技術(shù)在實(shí)踐中表現(xiàn)出較好的性能，在工作空間中的通道狹窄時(shí)，由于均勻采樣的使用，所得的多數(shù)采樣點(diǎn)將落在自由空間內(nèi)，窄通道采樣點(diǎn)數(shù)目的不足將導(dǎo)致概率路線圖連通性降低，最終導(dǎo)致路徑規(guī)劃失敗。Boor等[9]提出了高斯采樣策略，通過(guò)對(duì)窄通道采樣點(diǎn)密度的提高來(lái)增加構(gòu)成連通圖的概率，但采樣點(diǎn)數(shù)目過(guò)多會(huì)降低算法的效率，Hsu等[10]提出了一種橋測(cè)試法，通過(guò)測(cè)試中點(diǎn)是否在自由空間內(nèi)來(lái)確定其是否位于窄通道中，該方法得到的采樣點(diǎn)位置更精確，但每次要檢測(cè)三個(gè)點(diǎn)的位置，PARK等[11]提出了一種自適應(yīng)采樣方法，通過(guò)對(duì)邊界點(diǎn)的移動(dòng)，使窄通道中增加一些采樣點(diǎn)，但采樣點(diǎn)數(shù)量要求還是很難滿足。

基于以上各方法的特點(diǎn)，本文提出一種改進(jìn)的PRM算法，運(yùn)用人工勢(shì)場(chǎng)法優(yōu)化其采樣方式，克服均勻采樣的弊端，同時(shí)使落在障礙物中的點(diǎn)移動(dòng)到自由空間內(nèi)，達(dá)到增加自由空間內(nèi)節(jié)點(diǎn)數(shù)的效果，再利用A*算法搜索路徑，從而提高算法的效率。

1 PRM算法

PRM算法一般可分為兩個(gè)階段：學(xué)習(xí)階段和查詢階段。

(1)學(xué)習(xí)階段：隨機(jī)采樣定量節(jié)點(diǎn)，即均勻選取N個(gè)節(jié)點(diǎn)，為每個(gè)節(jié)點(diǎn)搜索鄰居節(jié)點(diǎn)并建立連接，除去與障礙物接觸的連線，構(gòu)建出一個(gè)無(wú)向路徑網(wǎng)絡(luò)圖G=(V，E)，其中V為節(jié)點(diǎn)集，E為所有可能的兩點(diǎn)之間的路徑集。PRM算法流程圖如圖1所示。

圖1 PRM算法流程圖

(2)查詢階段：根據(jù)起始點(diǎn)、目標(biāo)點(diǎn)、路標(biāo)地圖信息，采用啟發(fā)式搜索算法從路標(biāo)圖中搜索出一條可行路徑。

2 PRM算法的改進(jìn)

2.1 優(yōu)化采樣點(diǎn)

傳統(tǒng)的PRM算法運(yùn)用在構(gòu)型空間中均勻撒點(diǎn)的采樣策略，采樣點(diǎn)數(shù)目與空間的大小成正比，整個(gè)空間中采樣的概率處處相等，而落在空間中障礙物上的采樣點(diǎn)會(huì)被直接舍棄。為了滿足構(gòu)建路標(biāo)地圖時(shí)的連通性要求，往往需要增加采樣次數(shù)，導(dǎo)致落在自由空間中的采樣點(diǎn)增多，降低了算法效率。

如果能對(duì)落在障礙物上的采樣點(diǎn)進(jìn)行二次利用，就可以避免因多次采樣導(dǎo)致的算法效率降低。針對(duì)傳統(tǒng)PRM算法在采樣方式上的不足，本文通過(guò)引入人工勢(shì)場(chǎng)法對(duì)采樣點(diǎn)進(jìn)行優(yōu)化。利用人工勢(shì)場(chǎng)法可以使采樣點(diǎn)在勢(shì)場(chǎng)中運(yùn)動(dòng)這一思想，把構(gòu)型空間看成一個(gè)虛擬力場(chǎng)，虛擬力場(chǎng)是由自由空間對(duì)采樣點(diǎn)的引力場(chǎng)和障礙物內(nèi)部對(duì)采樣點(diǎn)的斥力場(chǎng)組成。

U(q)=Uatt(q)+Urep(q)

(1)

式中：q為采樣點(diǎn)；Uatt(q)為引力場(chǎng)；Urep(q)為斥力場(chǎng)；U(q)為勢(shì)場(chǎng)和。

當(dāng)采樣點(diǎn)落在障礙物內(nèi)時(shí)，因?yàn)槭艿秸系K物內(nèi)部的斥力作用而向障礙物外移動(dòng)；當(dāng)采樣點(diǎn)移動(dòng)到障礙物外時(shí)，由于自由空間引力場(chǎng)的作用，采樣點(diǎn)的移動(dòng)速度會(huì)逐漸減小至零并停止運(yùn)動(dòng)。在引力和斥力的合力下，采樣點(diǎn)在虛擬勢(shì)場(chǎng)中的動(dòng)能公式如下：

(2)

(3)

式中：Ep為采樣點(diǎn)的動(dòng)能；F(r)為采樣點(diǎn)受到的合力；C為采樣點(diǎn)經(jīng)過(guò)的軌跡；x(t)、y(t)為軌跡C在i，j方向的分量。

在構(gòu)型空間中，已知?jiǎng)輬?chǎng)分布和初始采樣點(diǎn)的位置，利用式(3)就可以得出采樣點(diǎn)在勢(shì)場(chǎng)作用下的停止位置。這樣，原本落在障礙物內(nèi)的采樣點(diǎn)就會(huì)變成自由空間內(nèi)的點(diǎn)，既保證了采樣點(diǎn)的數(shù)量，也增加了窄通道的采樣密度，從而提高了算法效率。

2.2 不同障礙物的勢(shì)場(chǎng)規(guī)劃

在考慮勢(shì)場(chǎng)規(guī)劃問(wèn)題時(shí)，認(rèn)為自由空間中的點(diǎn)不受勢(shì)場(chǎng)影響，不考慮其在勢(shì)場(chǎng)中的運(yùn)動(dòng)；認(rèn)為落在障礙物內(nèi)的點(diǎn)只受其所在障礙物內(nèi)的勢(shì)場(chǎng)和障礙物周圍勢(shì)場(chǎng)的影響，忽略其他障礙物的影響。對(duì)于復(fù)雜的障礙物，將其分解成多個(gè)構(gòu)型簡(jiǎn)單的障礙物進(jìn)行考慮，避免出現(xiàn)局部極小值的現(xiàn)象。

而在實(shí)際環(huán)境中進(jìn)行勢(shì)場(chǎng)規(guī)劃時(shí)，可以將空間中的障礙物分為兩種類型：①靜態(tài)障礙物，即固定原地不動(dòng)的障礙物，如建筑物等；②動(dòng)態(tài)障礙物，即隨時(shí)可能移動(dòng)的障礙物，如行人、車輛等。

對(duì)于靜態(tài)障礙物，可以采用均勻的勢(shì)場(chǎng)：

(4)

式中：k為障礙物內(nèi)勢(shì)力場(chǎng)強(qiáng)度；-1為障礙物外勢(shì)力場(chǎng)強(qiáng)度。

對(duì)于動(dòng)態(tài)障礙物，假定其運(yùn)動(dòng)軌跡已知，不考慮突發(fā)障礙物，利用采樣點(diǎn)到障礙物中心的徑向距離來(lái)進(jìn)行計(jì)算，勢(shì)場(chǎng)函數(shù)如下：

(5)

式中：d(r-r0)為采樣點(diǎn)到障礙物中心的徑向距離。

通過(guò)改變障礙物內(nèi)的勢(shì)力場(chǎng)強(qiáng)度，采樣點(diǎn)移動(dòng)距離的大小就得到改善。當(dāng)k值較大時(shí)，采樣點(diǎn)的移動(dòng)距離較大，在障礙物周圍的分布范圍較廣，采樣點(diǎn)分布更均勻；當(dāng)k值較小時(shí)，采樣點(diǎn)的移動(dòng)距離較小，在障礙物周圍的分布范圍較小，更適用于在有窄通道的構(gòu)型空間進(jìn)行勢(shì)場(chǎng)規(guī)劃。

當(dāng)空間中出現(xiàn)突發(fā)障礙物時(shí)，若該障礙物與已規(guī)劃路徑相交，則需要重新生成路徑。對(duì)落在突發(fā)障礙物內(nèi)的點(diǎn)再次施加勢(shì)場(chǎng)力，使其移動(dòng)到自由空間中，如果該點(diǎn)能與其他路徑點(diǎn)相連則不需要重新施加勢(shì)場(chǎng)力，否則繼續(xù)施加勢(shì)場(chǎng)力，直至能與其他路徑點(diǎn)相連。

2.3 人工勢(shì)場(chǎng)法仿真試驗(yàn)

針對(duì)人工勢(shì)場(chǎng)(APF)的構(gòu)建過(guò)程，傳統(tǒng)的做法是通過(guò)構(gòu)建包裹障礙物的外接圓，進(jìn)而生成斥力場(chǎng)，該做法需要知道外接圓的圓心。對(duì)于不規(guī)則的非圓形障礙物，搜索外接圓圓心將會(huì)消耗部分算力，不利于提高算法的執(zhí)行效率。本文期望通過(guò)障礙物的邊緣區(qū)域去構(gòu)建斥力場(chǎng)，獲得規(guī)劃地圖后，通過(guò)對(duì)于規(guī)劃地圖進(jìn)行圖形學(xué)濾波，以獲取障礙物邊緣的信息。依據(jù)式(1)～(3)，構(gòu)建該障礙物對(duì)于當(dāng)前位置的斥力場(chǎng)。整體算法流程圖如圖2所示。

圖2 算法流程圖

為驗(yàn)證基于障礙物邊緣構(gòu)建的斥力場(chǎng)對(duì)采樣點(diǎn)分布的調(diào)節(jié)作用，假設(shè)規(guī)劃仿真地圖中存在三處不規(guī)則障礙物，經(jīng)過(guò)圖形學(xué)濾波處理后得到障礙物邊緣如圖3所示。算法在選定采樣點(diǎn)的生成位置時(shí)，將受到基于障礙物邊緣的斥力場(chǎng)與目標(biāo)點(diǎn)構(gòu)建引力場(chǎng)的綜合作用，如圖4所示，當(dāng)障礙物中存在采樣點(diǎn)時(shí)，采樣點(diǎn)受合力場(chǎng)的作用將沿著合力方向進(jìn)行移動(dòng)。設(shè)定合力場(chǎng)的調(diào)節(jié)步長(zhǎng)為20 rpx，迭代5次，采樣點(diǎn)沿著合力方向運(yùn)動(dòng)至合力為零的位置，或者滿足設(shè)定迭代次數(shù)，該采樣點(diǎn)停止動(dòng)作，下一個(gè)落入障礙物的采樣點(diǎn)重復(fù)上述動(dòng)作，直至障礙物中無(wú)采樣點(diǎn)，經(jīng)由APF作用后的采樣點(diǎn)分布情況如圖5所示。參照?qǐng)D5，其中，“?”表示自由空間中的點(diǎn)，“+”表示落在障礙物內(nèi)的點(diǎn)，“○”表示從障礙物中移到自由空間中的點(diǎn)。自由空間中的點(diǎn)不受勢(shì)場(chǎng)影響，不考慮其在勢(shì)場(chǎng)中的運(yùn)動(dòng)。通過(guò)調(diào)整障礙物內(nèi)勢(shì)力場(chǎng)作用，使障礙物內(nèi)的點(diǎn)在引力和斥力作用下移動(dòng)到自由空間。結(jié)果表明，落入障礙內(nèi)的采樣點(diǎn)(標(biāo)記為“+”)均有效移出障礙物內(nèi)，體現(xiàn)了算法思想的可行性。對(duì)于算法的學(xué)習(xí)階段，在采樣點(diǎn)的數(shù)量同一量級(jí)下，通過(guò)改善采樣點(diǎn)分布情況提高采樣點(diǎn)的生成質(zhì)量，有效增加了算法對(duì)于窄通道的描述細(xì)節(jié)，進(jìn)而提高規(guī)劃路徑的生成質(zhì)量。

圖3 含邊緣障礙物

圖4 合力場(chǎng)作用

圖5 人工勢(shì)場(chǎng)法作用

3 仿真試驗(yàn)與結(jié)果分析

為了檢驗(yàn)改進(jìn)算法的有效性，在硬件環(huán)境最低配置為CPU：Intel Core i5，主頻：2.4 GHz，內(nèi)存：4 G，硬盤：120 GB，利用MATLAB 2018b 對(duì)改進(jìn)PRM算法進(jìn)行仿真。首先建立一個(gè)含有障礙物的500*500的地圖，為便于仿真，將無(wú)人車視為一個(gè)質(zhì)點(diǎn)。設(shè)定無(wú)人車的起始點(diǎn)坐標(biāo)為(10，10)，目標(biāo)點(diǎn)坐標(biāo)為(490，490)。障礙物和邊界位置固定，在采樣地圖相同條件下，傳統(tǒng)PRM 算法仿真試驗(yàn)如圖6所示，改進(jìn)后的 PRM 算法仿真試驗(yàn)如圖7所示，兩種算法試驗(yàn)結(jié)果對(duì)比見(jiàn)表1。其中，start表示起始節(jié)點(diǎn)，goal表示目標(biāo)節(jié)點(diǎn)，連線為仿真試驗(yàn)規(guī)劃出的路徑；保證起始點(diǎn)和目標(biāo)點(diǎn)位置不變，在同樣的地圖環(huán)境中，對(duì)傳統(tǒng) PRM算法和改進(jìn)后的PRM算法分別進(jìn)行25次仿真試驗(yàn)，兩者時(shí)間對(duì)比結(jié)果如圖8所示；選取其中10次仿真試驗(yàn)，將兩種算法路徑長(zhǎng)度對(duì)比結(jié)果列入表2。

表2 兩種算法仿真試驗(yàn)數(shù)據(jù)

圖6 傳統(tǒng)PRM算法仿真試驗(yàn)

圖7 改進(jìn)PRM算法仿真試驗(yàn)

圖8 時(shí)間對(duì)比

由圖 3和圖4可知，相對(duì)于傳統(tǒng)PRM算法，改進(jìn)PRM算法通過(guò)引入人工勢(shì)場(chǎng)法使落在障礙物內(nèi)的采樣點(diǎn)變成自由空間內(nèi)的點(diǎn)，對(duì)落在障礙物上的采樣點(diǎn)進(jìn)行了二次利用，保證了采樣點(diǎn)的數(shù)量，增加了狹窄區(qū)域的采樣密度，從而提高了狹窄區(qū)的采樣效率。由表1可知，改進(jìn)后的PRM算法仿真試驗(yàn)所得到的路徑軌跡轉(zhuǎn)折點(diǎn)較少，路徑軌跡更加平滑。路徑長(zhǎng)度的縮短減少了路徑規(guī)劃所需的時(shí)間，無(wú)人車能順利到達(dá)目標(biāo)位置，提高了搜索效率，實(shí)現(xiàn)了路徑優(yōu)化目標(biāo)。

表1 兩種算法仿真試驗(yàn)結(jié)果

由圖7和表2多次重復(fù)試驗(yàn)結(jié)果可知，改進(jìn)后的PRM算法轉(zhuǎn)折點(diǎn)個(gè)數(shù)均少于傳統(tǒng)PRM算法，實(shí)現(xiàn)了路徑平滑，通過(guò)比較時(shí)間結(jié)果方差和路徑長(zhǎng)度結(jié)果方差可知，改進(jìn)后的PRM算法所用時(shí)間和路徑長(zhǎng)度的波動(dòng)范圍比傳統(tǒng)的小。該仿真試驗(yàn)表明，相比于傳統(tǒng)PRM算法，改進(jìn)后的PRM算法在路徑轉(zhuǎn)折點(diǎn)、搜索效率和路徑長(zhǎng)度方面都有明顯的優(yōu)化，且更具穩(wěn)定性。

4 結(jié)束語(yǔ)

針對(duì)無(wú)人車路徑規(guī)劃問(wèn)題，以傳統(tǒng)PRM算法為基礎(chǔ)，引入人工勢(shì)場(chǎng)，通過(guò)基于障礙物邊緣的斥力場(chǎng)與目標(biāo)點(diǎn)構(gòu)建的引力場(chǎng)的綜合作用，使落在障礙物內(nèi)的點(diǎn)移動(dòng)到自由空間內(nèi)，保證了采樣點(diǎn)的數(shù)量，再利用A*算法搜索路徑。試驗(yàn)表明改進(jìn)的PRM算法不但減少了搜索路徑中轉(zhuǎn)折點(diǎn)個(gè)數(shù)，而且有效縮短了路徑規(guī)劃時(shí)間和路徑長(zhǎng)度，進(jìn)一步滿足了無(wú)人車路徑規(guī)劃的實(shí)時(shí)性要求。