易馳，伍建輝，2

（1.湖南理工學院，湖南 岳陽 414006；2.桂林電子科技大學，廣西 桂林 541004）

0 引 言

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

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

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

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

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

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

1.1 目標區(qū)域偏置擴展

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

圖1 目標區(qū)域偏置擴展示意圖

采 樣點生成公式可表示為：

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

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

1.2 橢圓空間采樣

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

圖2 橢圓子集采樣示意圖

假設ze是標準橢圓采樣節(jié)點，L 是當前路徑長度，是的x 軸坐標，是的y 軸坐標。標準橢圓采樣節(jié)點定義為

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

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

1.3 節(jié)點約束 策略

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

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

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

1.4 路徑修正

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

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

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

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

其中Pi、Pi+1、Pi+2 和Pi+3 為路徑控制點。

2 算法實現(xiàn)

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

Step1：初始化隨機樹，確定起始節(jié)點和目標節(jié)點。

Step2：迭代開始，如果算法沒有搜索到初始路徑，則進入Step3；否則，進入Step4。

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

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

Step5：選擇采樣點距離隨機樹中最近的節(jié)點作為鄰節(jié)點znear，并通過目標擴展生成新節(jié)點znew。

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

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

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

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

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

Step1：通過逆向?qū)?yōu)得到新的路徑點坐標。初始化曲線公式的各項參數(shù)。

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

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

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

3 算例仿真及分析

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié) 論

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