目標(biāo)區(qū)域偏置擴(kuò)展的RRT*路徑規(guī)劃算法

易馳，伍建輝，2

（1.湖南理工學(xué)院，湖南 岳陽(yáng) 414006；2.桂林電子科技大學(xué)，廣西 桂林 541004）

0 引 言

移動(dòng)機(jī)器人的路徑規(guī)劃，是按照一定的約束條件（如距離，安全，時(shí)效），規(guī)劃出一條從起始位置到目標(biāo)位置的可行路徑。目前主流的路徑規(guī)劃算法有基于勢(shì)場(chǎng)的人工勢(shì)場(chǎng)法[1]、基于仿生學(xué)算法的蟻群算法[2，3]、粒子群算法[4，5]和遺傳算法[6]、基于圖搜索方法的A*算法[7]和Voronoi 圖法[8]、基于采樣方法的RRT 算法[9，10]和PRM 算法[11]。其中，RRT 算法是基于采樣方法的經(jīng)典代表之一，通過(guò)隨機(jī)采樣和碰撞檢測(cè)的方法，獲得一條無(wú)碰撞路徑。由于RRT 算法沒(méi)有考慮到路徑花費(fèi)的原因，往往不能得到最優(yōu)路徑。針對(duì)RRT 算法的這一不足，Karaman 等人[12]提出RRT*算法，在RRT 算法的基礎(chǔ)上增加了鄰節(jié)點(diǎn)重新選擇和重連接的步驟來(lái)保證算法的漸進(jìn)最優(yōu)性。但是由于RRT*算法采樣的隨機(jī)性，導(dǎo)致算法對(duì)目標(biāo)的導(dǎo)向性弱，會(huì)對(duì)一些無(wú)效的區(qū)域進(jìn)行過(guò)多的探索，同時(shí)隨機(jī)樹(shù)中存在大量未起到優(yōu)化當(dāng)前路徑的節(jié)點(diǎn)，導(dǎo)致收斂到最優(yōu)或接進(jìn)最優(yōu)路徑的速度慢。

如何改善RRT*算法低收斂速度的問(wèn)題獲得了廣泛研究，Gammell 等人[13]提出Informed-RRT*算法，通過(guò)起始點(diǎn)、目標(biāo)點(diǎn)和當(dāng)前路徑來(lái)確定橢圓采樣區(qū)域，使算法減少對(duì)無(wú)效區(qū)域探索，從而加快算法的收斂速度，但是Informed-RRT*算法能夠限制采樣空間，但無(wú)法限制隨機(jī)樹(shù)的大小。Kim 等人[14]采用一種尺寸減小的方法來(lái)縮小Informed-RRT*算法的橢圓空間，提高了算法的收斂速度。Mashayekhi 等人[15]提出將RRT*-connect 算法與Informed-RRT*算法相結(jié)合，利用雙向樹(shù)快速找到初始路徑后利用橢圓形子集采樣，從而加快了算法的收斂速度。Nasir 等人[16]提出了RRT*-smart算法，通過(guò)智能采樣策略來(lái)去除不必要的節(jié)點(diǎn)，提高了算法的收斂速度。Adiyatov[17]提出RRT*-FN 算法，通過(guò)限制節(jié)點(diǎn)最大數(shù)量來(lái)提高算法收斂速度。

為改善RRT*算法對(duì)目標(biāo)導(dǎo)向性差的問(wèn)題，Moses等人[18]提出目標(biāo)定向的方法，通過(guò)選取一對(duì)隨機(jī)采樣點(diǎn)中更接近目標(biāo)點(diǎn)的作為采樣點(diǎn)，有效提升算法對(duì)目標(biāo)的導(dǎo)向性。Noreen等人[19]提出了有界目標(biāo)偏置采樣的方法，通過(guò)在路徑周圍區(qū)域內(nèi)選取采樣點(diǎn)，從而提高算法收斂速度和對(duì)目標(biāo)的導(dǎo)向性。劉建宇等[20]提出改進(jìn)的RRT*-connect 算法，在目標(biāo)偏向策略的基礎(chǔ)上對(duì)采樣區(qū)域進(jìn)行約束，保證每次采樣向著目標(biāo)方向，從而加快算法的計(jì)算效率。

基于上述研究，本文針對(duì)RRT*算法收斂速度慢和目標(biāo)導(dǎo)向性差的問(wèn)題，提出基于目標(biāo)區(qū)域偏置擴(kuò)展的RRT*路徑規(guī)劃算法。首先對(duì)目標(biāo)偏置方法進(jìn)行改進(jìn)，通過(guò)改變概率閾值和選取目標(biāo)區(qū)域范圍內(nèi)的點(diǎn)作為目標(biāo)偏置，在加強(qiáng)算法對(duì)目標(biāo)的導(dǎo)向性的同時(shí)，使算法能夠適應(yīng)不同環(huán)境。當(dāng)算法完成初始路徑規(guī)劃后，采用橢圓子集采樣的方法縮小算法的采樣空間，減少算法對(duì)無(wú)效空間的探索。其次，利用節(jié)點(diǎn)約束策略，計(jì)算新節(jié)點(diǎn)的潛在最小路徑長(zhǎng)度，并拒絕潛在最小路徑長(zhǎng)度大于當(dāng)前路徑長(zhǎng)度的新節(jié)點(diǎn)加入隨機(jī)樹(shù)中，有效地減少了隨機(jī)樹(shù)節(jié)點(diǎn)數(shù)量，從而加快了算法收斂速度。最后針對(duì)路徑曲折的問(wèn)題，采用路徑修正方法對(duì)路徑進(jìn)行平滑處理，得到最終可供移動(dòng)機(jī)器人行駛的路徑。

1 目標(biāo)區(qū)域偏置擴(kuò)展的RRT*路徑規(guī)劃算法

目標(biāo)區(qū)域偏置擴(kuò)展的RRT*路徑規(guī)劃算法主要由目標(biāo)區(qū)域偏置擴(kuò)展、橢圓子集采樣、節(jié)點(diǎn)約束策略和路徑修正四個(gè)部分組成

1.1 目標(biāo)區(qū)域偏置擴(kuò)展

本節(jié)提出目標(biāo)區(qū)域偏置擴(kuò)展的方法，來(lái)提高算法對(duì)目標(biāo)的導(dǎo)向性，通過(guò)比較目標(biāo)擴(kuò)展概率pgoal和隨機(jī)采樣概率prand這兩個(gè)隨機(jī)數(shù)的大小，來(lái)確定采樣方式。若pgoal大于prand，則隨機(jī)選取以目標(biāo)點(diǎn)zgoal為圓心，以目標(biāo)點(diǎn)zgoal到隨機(jī)樹(shù)的距離為半徑r 的圓內(nèi)一點(diǎn)為采樣點(diǎn)zrand，否則在地圖中隨機(jī)產(chǎn)生一個(gè)采樣點(diǎn)zrand。通過(guò)這種方式使得隨機(jī)樹(shù)在朝向目標(biāo)點(diǎn)方向拓展的前提下，仍保留隨機(jī)樹(shù)對(duì)未知區(qū)域的探索能力，能更好地適應(yīng)不同環(huán)境，有效提高算法對(duì)目標(biāo)的導(dǎo)向性。如圖1 所示，為目標(biāo)區(qū)域偏置擴(kuò)展的示意圖。

圖1 目標(biāo)區(qū)域偏置擴(kuò)展示意圖

采 樣點(diǎn)生成公式可表示為：

完 成采樣后借鑒引力場(chǎng)的思想，通過(guò)給采樣點(diǎn)和目標(biāo)點(diǎn)分配不同權(quán)重，使得每一次擴(kuò)展同時(shí)由采樣點(diǎn)和目標(biāo)點(diǎn)共同決定，從而克服新節(jié)點(diǎn)擴(kuò)展的盲目性。新節(jié)點(diǎn)生成公式可以寫(xiě)為

其中R 為地圖尺寸大小；znear為鄰近節(jié)點(diǎn)；zgoal和zrand分別為目標(biāo)點(diǎn)和隨機(jī)采樣點(diǎn)；δ 為擴(kuò)展步長(zhǎng)；λ 為權(quán)重系數(shù)；ran d 為（0，1）區(qū)間均勻分布的隨機(jī)函數(shù)。

1.2 橢圓空間采樣

為改善 RRT*算法收斂速度慢的問(wèn)題，本節(jié)通過(guò)當(dāng)前路徑長(zhǎng)度、起始點(diǎn)和目標(biāo)點(diǎn)生成一個(gè)橢圓空間來(lái)代替在全部狀態(tài)空間進(jìn)行采樣。由于橢圓的幾何性質(zhì)，橢圓內(nèi)的點(diǎn)到兩焦點(diǎn)的距離和小于橢圓的長(zhǎng)軸，所以能優(yōu)化當(dāng)前路徑的采樣點(diǎn)都存在于橢圓內(nèi)。隨著迭代次數(shù)不斷增加，橢圓空間不斷縮小，路徑長(zhǎng)度也不斷縮短，最終得 到最優(yōu)路徑。如圖2 所示，為橢圓空間采樣的示意圖。

圖2 橢圓子集采樣示意圖

假設(shè)ze是標(biāo)準(zhǔn)橢圓采樣節(jié)點(diǎn)，L 是當(dāng)前路徑長(zhǎng)度，是的x 軸坐標(biāo)，是的y 軸坐標(biāo)。標(biāo)準(zhǔn)橢圓采樣節(jié)點(diǎn)定義為

橢圓子集采樣節(jié)點(diǎn)zerand由標(biāo)準(zhǔn)橢圓采樣節(jié)點(diǎn)ze旋轉(zhuǎn)和平移來(lái)確定，因此橢圓子集采樣節(jié)點(diǎn)zerand可以表示為

其中α 是連接起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)的直線與x 軸的夾角。

1.3 節(jié)點(diǎn)約束 策略

為了減少需要添加到隨機(jī)樹(shù)中的無(wú)效節(jié)點(diǎn)數(shù)量，本節(jié)提出節(jié)點(diǎn)約束策略，通過(guò)比較新節(jié)點(diǎn)znew的潛在最小路徑成本F 與當(dāng)前路徑長(zhǎng)度L，來(lái)判斷該節(jié)點(diǎn)是否具備優(yōu)化當(dāng)前路徑的能力。潛在最小路徑成本F 為鄰節(jié)點(diǎn)znear回溯到起始點(diǎn)zinit的路徑長(zhǎng)度g（znear），加上新節(jié)點(diǎn)znew到鄰節(jié)點(diǎn)znear的直線距離d（znew，znear），再加上新節(jié)點(diǎn)znew鄰近區(qū)域內(nèi)的當(dāng)前路徑點(diǎn)到目標(biāo)點(diǎn)的路徑長(zhǎng)度的最小值fmin（znew）。若潛在最小路徑成本F 大于當(dāng)前路徑長(zhǎng)度L，則新節(jié)點(diǎn)znew不參與后續(xù)一系列的步驟。圖3 為潛在最小路徑成本示意圖，圖中綠色虛線為新節(jié)點(diǎn)znew最小路徑成本F。

圖3 潛在最小路徑 成本示意圖

潛在最小路徑成本F 的計(jì)算公式可表示為

1.4 路徑修正

本節(jié)采用逆 向?qū)?yōu)和路徑 平滑的方法對(duì)規(guī)劃得出的路徑進(jìn)行優(yōu)化和平滑處理。優(yōu)化過(guò)程為：首先判斷zinit與zgoal之間是否存在障礙物，若不存在，則將zinit作為zgoal的父節(jié)點(diǎn)；若存在障礙物，則判斷zgoal的父節(jié)點(diǎn)與zinit之間是否存在障礙物，并重復(fù)上述過(guò)程直到找到與zinit之間不存在障礙物的路徑點(diǎn)為止。得到與目標(biāo)點(diǎn)zinit之間沒(méi)有障礙物的路徑點(diǎn)后，以該路徑點(diǎn)作為下一步逆向?qū)?yōu)的起點(diǎn)，并重復(fù)上訴步驟直到完成整條路徑的尋優(yōu)。圖4 為路徑逆向?qū)?yōu)示意圖。

圖4 路徑逆向?qū)?yōu)示意圖

三次B 樣條曲線用于平滑路徑，3 次B 樣條曲線的基函數(shù)可表示為

3 次B 樣條曲線和控制點(diǎn)之間的關(guān)系可以表示為

其中Pi、Pi+1、Pi+2 和Pi+3 為路徑控制點(diǎn)。

2 算法實(shí)現(xiàn)

基于目標(biāo)區(qū)域偏置擴(kuò)展的RRT*路徑規(guī)劃算法實(shí)現(xiàn)的具體步驟描述如下：

Step1：初始化隨機(jī)樹(shù)，確定起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)。

Step2：迭代開(kāi)始，如果算法沒(méi)有搜索到初始路徑，則進(jìn)入Step3；否則，進(jìn)入Step4。

Step3：在（0，1）區(qū)間隨機(jī)生成采樣概率prand和目標(biāo)擴(kuò)展概率pgoal，若prand＜pgoal則在目標(biāo)區(qū)域內(nèi)進(jìn)行采樣；否則，使用全局均勻采樣。進(jìn)入Step5。

Step4：在以初始路徑長(zhǎng)度為長(zhǎng)軸，以起始點(diǎn)和目標(biāo)點(diǎn)為焦點(diǎn)的橢圓區(qū)域內(nèi)進(jìn)行采樣，并對(duì)執(zhí)行節(jié)點(diǎn)約束策略，若滿足節(jié)點(diǎn)約束條件進(jìn)入Step5；否則回到Step2。

Step5：選擇采樣點(diǎn)距離隨機(jī)樹(shù)中最近的節(jié)點(diǎn)作為鄰節(jié)點(diǎn)znear，并通過(guò)目標(biāo)擴(kuò)展生成新節(jié)點(diǎn)znew。

Step6：檢查新節(jié)點(diǎn)與鄰節(jié)點(diǎn)之間的路徑是否與障礙物發(fā)生沖突，如果沒(méi)有沖突則進(jìn)入Step7；反之，回到Step2重新進(jìn)行采樣。

Step7：在新節(jié)點(diǎn)znew 的鄰近區(qū)域內(nèi)重新選擇父節(jié)點(diǎn)，使得新節(jié)點(diǎn)znew到起始點(diǎn)的路徑長(zhǎng)度能夠最小，并將新節(jié)znew點(diǎn)插入到樹(shù)中。

Step8：在新節(jié)點(diǎn)znew鄰近區(qū)域內(nèi)執(zhí)行重連接策略，使鄰近區(qū)域內(nèi)所有節(jié)點(diǎn)到起始點(diǎn)的路徑長(zhǎng)度最小。

Step9：檢查新節(jié)點(diǎn)是否在目標(biāo)區(qū)域內(nèi)，如果新節(jié)點(diǎn)在目標(biāo)區(qū)域內(nèi)，則生成一條初始路徑，回到Step2；若達(dá)到設(shè)定迭代次數(shù)則退出程序，輸出最優(yōu)路徑坐標(biāo)點(diǎn)。

路徑修正實(shí)現(xiàn)的具體步驟描述如下：

Step1：通過(guò)逆向?qū)?yōu)得到新的路徑點(diǎn)坐標(biāo)。初始化曲線公式的各項(xiàng)參數(shù)。

Step2：將逆向?qū)?yōu)得出的路徑坐標(biāo)作為曲線的控制點(diǎn)。

Step3：通過(guò)公式（7）得到3 次曲線方程基函數(shù)。

Step4：將控制點(diǎn)坐標(biāo)和3 次曲線方程基函數(shù)帶入公式（8）得到優(yōu)化后的路徑坐標(biāo)，最后依次連接3 次曲線得到最終路徑。

3 算例仿真及分析

本節(jié)通過(guò)如圖5 所示的兩種不同 地圖環(huán)境，對(duì)本文所提算法、RRT*[12]、Informed-RRT*[13]和RRT*-FN[17]算法進(jìn)行仿真實(shí)驗(yàn)，通過(guò)對(duì)比初始路徑規(guī)劃結(jié)果和算法收斂所花費(fèi)時(shí)間來(lái)驗(yàn)證本文算法的有效性。其中圖5（a）為簡(jiǎn)單地圖環(huán)境，大小為800×800，圖5（b）為復(fù)雜地圖環(huán)境，大小為1 000×1 000。每種算法都進(jìn)行300 次仿真實(shí)驗(yàn)以減小實(shí)驗(yàn)的隨機(jī)性。算法仿真的軟件為64 位MATLAB 2018b，硬件條件為Intel? CoreTM i7-10700 CPU@2.90 GHz，內(nèi)存為8 GB。

圖5 地圖環(huán)境圖

3.1 簡(jiǎn)單地圖環(huán)境

本節(jié)將驗(yàn) 證本文算法在簡(jiǎn)單地圖環(huán)境中的有效性，并將其與其他三類算法進(jìn)行對(duì)比。路徑規(guī)劃的起始點(diǎn)坐標(biāo)為[10，10]，目標(biāo)點(diǎn)坐標(biāo)為[790，790]。如圖6 所示，為4 種路徑規(guī)劃算法在簡(jiǎn)單地圖環(huán)境的初始路徑規(guī)劃圖。對(duì)比圖6（a）（b）（c），可發(fā)現(xiàn)本文所提算法對(duì)無(wú)效區(qū)域的探索較少，擴(kuò)展節(jié)點(diǎn)少，初始路徑平滑。

圖6 簡(jiǎn)單地圖環(huán)境下4 種算法的初始路徑 規(guī)劃圖

如表1 所示，為簡(jiǎn)單地圖環(huán)境下4 種算法的初始路徑規(guī)劃結(jié)果。由表1 可知，本文提出的算法初始解的平均路徑長(zhǎng)度Lc有一定的提升，在擴(kuò)展節(jié)點(diǎn)數(shù)量Sc上相對(duì)RRT*、Informed-RRT*和RRT*-FN 算法分別減少了71.7%、72.1%和71.2%，初始路徑規(guī)劃時(shí)間Tc分別減少了80.0%、80.3%和80.6%，這也從側(cè)面反映本算法能有效提升目標(biāo)導(dǎo)向性。

表1 簡(jiǎn)單地圖環(huán)境下4 種算法的初始路徑規(guī)劃結(jié)果

如圖7 所示，為簡(jiǎn)單地圖環(huán)境下4 種算法收斂后的路徑規(guī)劃圖。由圖7 可發(fā)現(xiàn)上述4 種算法在簡(jiǎn)單地圖環(huán)境下都能找到漸進(jìn)最優(yōu)路徑。與RRT*、Informed-RRT*和RRT*-FN三類算法相比，本算法的擴(kuò)展節(jié)點(diǎn)數(shù)量較少，這也能說(shuō)明橢圓子集采樣和節(jié)點(diǎn)約束策略能有效地減少擴(kuò)展節(jié)點(diǎn)的數(shù)量。

圖7 簡(jiǎn)單地圖環(huán)境下4 種算法收斂后的路徑規(guī)劃圖

如表2 所示，為簡(jiǎn)單地圖環(huán)境下4 種算法收斂后的結(jié)果。由表2 可知，在路徑長(zhǎng)度上本算法與其他3 類算法近似。相對(duì)其他3 類算法，本算法在收斂時(shí)間上分別減少了69.3%、74.6%和56.2%，這也從側(cè)面反映本算法能提高收斂速度。

表2 簡(jiǎn)單地圖環(huán)境下4 種算法收斂后的結(jié)果

如圖8 所示，為簡(jiǎn)單地圖環(huán)境下路徑修正前后對(duì)比圖。由圖8 可知，路徑進(jìn)行修正后，變得比較平滑，一些小折角也被消除，且路徑的大體趨勢(shì)幾乎沒(méi)有改變。

圖8 簡(jiǎn)單地圖環(huán)境下路徑修正前后對(duì)比圖

3.2 復(fù)雜地圖環(huán)境

本節(jié)將驗(yàn)證本文算法在復(fù)雜地圖環(huán)境中的有效性，并將其與其他三類算法進(jìn)行對(duì)比。路徑規(guī)劃的起始點(diǎn)坐標(biāo)為[10，10]，目標(biāo)點(diǎn)坐標(biāo)為[900，900]。如圖9 所示，為復(fù)雜地圖環(huán)境下4 種算法的初始路徑規(guī)劃圖。對(duì)比圖7（a）（b）（c），可發(fā)現(xiàn)本文算法對(duì)無(wú)效區(qū)域的探索較少，路徑長(zhǎng)度更優(yōu)。以上說(shuō)明了目標(biāo)區(qū)域偏置擴(kuò)展能有效提高算法對(duì)目標(biāo)導(dǎo)向性。

圖9 復(fù)雜地圖環(huán)境下4 種算法的初始路徑規(guī)劃圖

如表3 所示，為復(fù)雜地圖環(huán)境下4 種算法的初始路徑規(guī)劃結(jié)果。由表3 可知，本文算法在擴(kuò)展節(jié)點(diǎn)數(shù)量Sc上相對(duì)RRT*、Informed-RRT*和RRT*-FN 算法分別減小了63.3%、63.7%和64.4%，初始路徑規(guī)劃時(shí)間Tc分別減少了71.6%、71.3%和70.8%，這也表明了本文算法能有效提高目標(biāo)導(dǎo)向性，更快規(guī)劃出一條初始路徑。

表3 復(fù)雜地圖環(huán)境下4 種算法的初始路徑規(guī)劃結(jié)果

如圖10 所示，為簡(jiǎn)單地圖環(huán)境下4 種算法收斂后的路徑規(guī)劃圖。由圖10 可發(fā)現(xiàn)與RRT*、Informed-RRT*和RRT*-FN 算法相比，本算法有效限制了采樣空間大小和采樣節(jié)點(diǎn)數(shù)量，且路徑長(zhǎng)度相對(duì)于其他3 類算法更加平滑，這說(shuō)明橢圓子集采樣和節(jié)點(diǎn)約束策略能有效地減少擴(kuò)展節(jié)點(diǎn)的數(shù)量。

圖10 簡(jiǎn)單地圖環(huán)境下4 種算法收斂后的路徑規(guī)劃圖

如表4 所示，為復(fù)雜地圖環(huán)境下4 種算法收斂后的結(jié)果。由表2 可知，在路徑長(zhǎng)度Ls上本算法與其他3 類算法近似。相對(duì)于其他3 類算法，本算法在收斂時(shí)間Ts上分別減少了70.9%、68.6%和43.8%，表明本文算法能提高收斂速度。

表4 復(fù)雜地圖環(huán)境下4 種算法收斂后的結(jié)果

如圖11 所示，為復(fù)雜地圖環(huán)境下路徑修正前后對(duì)比圖。由圖11 可知，在復(fù)雜環(huán)境中路徑修正方法同樣能在不改變路徑整體趨勢(shì)的前提下，對(duì)路徑進(jìn)行平滑處理。

圖11 復(fù)雜地圖環(huán)境下路徑修正前后對(duì)比圖

4 結(jié) 論

本文針對(duì)RRT*路徑規(guī)劃算法目標(biāo)導(dǎo)向性弱和收斂速度低，提出了基于目標(biāo)區(qū)域偏置和擴(kuò)展的RRT*路徑規(guī)劃方法，并給出了算法的實(shí)現(xiàn)過(guò)程。仿真結(jié)果驗(yàn)證了所提算法的有效性，并顯示了RRT*、Informed-RRT*、RRT*-FN 和本文算法在兩種不同環(huán)境下的結(jié)果。結(jié)果表明，本文算法可以增強(qiáng)目標(biāo)導(dǎo)向性，有效減少采樣節(jié)點(diǎn)數(shù)，加快算法的收斂速度。此外，本文算法使規(guī)劃的路徑更加平滑。