韓曉微,石澤亮,王 曉

(沈陽(yáng)大學(xué) a.科技創(chuàng)新學(xué)院,b.信息工程學(xué)院,遼寧 沈陽(yáng) 110044)

隨著社會(huì)的快速發(fā)展,智能機(jī)器人在生活中的應(yīng)用越來(lái)越廣泛[1],應(yīng)用環(huán)境也越來(lái)越復(fù)雜。機(jī)器人規(guī)劃路徑的好壞直接決定了機(jī)器人工作的穩(wěn)定性和智能性。智能機(jī)器人的無(wú)碰撞運(yùn)動(dòng)規(guī)劃受到國(guó)內(nèi)外學(xué)者的廣泛關(guān)注。同時(shí),機(jī)器人無(wú)碰撞運(yùn)動(dòng)規(guī)劃的快速發(fā)展也影響和改變著各行各業(yè)的生產(chǎn)生活方式[2]。

常見(jiàn)的運(yùn)動(dòng)規(guī)劃方式分為搜索、智能、采樣等3類[3]。其中基于搜索的路徑規(guī)劃算法包括Dijkstra和A*等;基于智能的路徑規(guī)劃算法包括遺傳算法、蟻群算法等;基于采樣的路徑規(guī)劃算法包括RRT(rapidly-exploring random trees)、RRT*等[4]。RRT類算法不僅能夠廣泛應(yīng)用于復(fù)雜高維空間,而且無(wú)需對(duì)構(gòu)型空間障礙物進(jìn)行描述,適合應(yīng)用于多自由度機(jī)器人在復(fù)雜環(huán)境下和動(dòng)態(tài)環(huán)境中的路徑規(guī)劃問(wèn)題[5]。RRT是由LaValle等[6]在1998年提出的一種基于概率采樣的運(yùn)動(dòng)規(guī)劃算法,它通過(guò)狀態(tài)空間的隨機(jī)采樣導(dǎo)向空白區(qū)域,從而找到一條可行的規(guī)劃路徑,能快速有效地搜索高維空間,但是其生成的路徑非最優(yōu)[7],且環(huán)境復(fù)雜度會(huì)影響收斂速度。鑒于RRT規(guī)劃的路徑非最優(yōu),Karaman等[8]在2011年提出RRT*算法,通過(guò)對(duì)指定范圍內(nèi)為當(dāng)前節(jié)點(diǎn)重新尋找父節(jié)點(diǎn)和范圍內(nèi)節(jié)點(diǎn)的重新連接2步來(lái)產(chǎn)生一個(gè)最優(yōu)路徑,但是由于其搜索范圍大且需要修剪冗余的枝葉導(dǎo)致速度變慢。Gammell等[9]在2014年提出Informed-RRT*算法,主要針對(duì)RRT*在最后2步修剪枝葉的時(shí)候采樣范圍過(guò)大導(dǎo)致速度變慢的問(wèn)題,在初始路徑的基礎(chǔ)上建立一個(gè)漸變橢圓來(lái)規(guī)定采樣空間,效率更高[10]。國(guó)內(nèi)外針對(duì)上述變種的RRT類算法的不足提出了不同的方法來(lái)改進(jìn)和實(shí)現(xiàn)。張建東等[11]針對(duì)RRT搜索速度慢的問(wèn)題,將子目標(biāo)搜索方式和目標(biāo)偏向策略引入采樣過(guò)程,雖然算法搜索速度較快,但路徑還是非最優(yōu)解。劉頓等[12]針對(duì)路徑規(guī)劃耗時(shí)長(zhǎng)與非最優(yōu)等問(wèn)題提出在機(jī)械臂構(gòu)型空間中預(yù)設(shè)采摘點(diǎn),提出4棵樹(shù)同時(shí)采樣的方法,加入自適應(yīng)步長(zhǎng)函數(shù)可以保證采樣質(zhì)量和降低路徑規(guī)劃時(shí)間。歐陽(yáng)子路等[13]在保留RRT*算法特性的同時(shí),通過(guò)改進(jìn)碰撞評(píng)價(jià)函數(shù)、引入動(dòng)態(tài)目標(biāo)區(qū)域采樣等方式來(lái)保證規(guī)劃速度的提高。Kim等[14]提出的基于包裹的知情RRT*將稱為“包裹過(guò)程”的尺寸減小過(guò)程與知情RRT*相結(jié)合,效率雖然提高了,但是容易陷入局部最優(yōu)解。Ghosh等[15]提出了一種運(yùn)動(dòng)學(xué)約束B(niǎo)i-RRT(KB-RRT)算法,生成平滑軌跡,擴(kuò)大重選父節(jié)點(diǎn)和剪枝的范圍,路徑更好地收斂達(dá)到漸進(jìn)最優(yōu),但導(dǎo)致了搜索效率變低。

本文研究分析了目前已有的RRT類路徑規(guī)劃算法,針對(duì)路徑規(guī)劃過(guò)程收斂速度慢、效率低等問(wèn)題,從規(guī)劃方式、采樣范圍、生長(zhǎng)轉(zhuǎn)角和生長(zhǎng)步長(zhǎng)等4個(gè)方面提出一種雙向Informed-RRT*的路徑規(guī)劃算法。

1 Informed-RRT*算法

1.1 RRT算法

圖1 RRT算法搜索路徑Fig.1 Search path of RRT algorithm

Informed-RRT*算法是基于RRT算法發(fā)展到RRT*算法后的變種。RRT主要優(yōu)勢(shì)為能快速地找到一條可實(shí)現(xiàn)路徑,主要取決于其概率采樣的隨機(jī)性[16],正因如此才能快速找到一條可實(shí)現(xiàn)路徑,但這種規(guī)劃方式導(dǎo)致了其解非最優(yōu)或漸近最優(yōu),RRT*算法通過(guò)修枝來(lái)解決這個(gè)問(wèn)題[17],但其煩瑣的遍歷過(guò)程導(dǎo)致效率低下,而Informed-RRT*算法就是為了減少其采樣空間范圍而實(shí)現(xiàn)搜索效率提升。原始的RRT算法利用初始點(diǎn)作為根節(jié)點(diǎn),擴(kuò)展新的子節(jié)點(diǎn)以完成隨機(jī)樹(shù)的構(gòu)建,見(jiàn)圖1。

RRT算法的主體偽代碼如下:

1)V←{qinit};E←Φ;i←0;#對(duì)頂點(diǎn)集V和邊集E進(jìn)行初始化,其中,i代表迭代次數(shù);

2) whilei

3)G←(V,E);#設(shè)置一個(gè)新的圖集;

4)qrand←sample(i);i←i+1;#利用sample(i)函數(shù)采樣一個(gè)新的采樣點(diǎn)qrand;

5) (V,E)←extend(G,qrand);#更新V和E,每次更新完的節(jié)點(diǎn)與終點(diǎn)求歐氏距離。

表1為實(shí)驗(yàn)仿真中部分歐氏距離d,若d小于規(guī)定閾值,即步長(zhǎng)2.00,且通過(guò)碰撞檢測(cè),滿足算法結(jié)束條件,直接連接最新節(jié)點(diǎn)和終點(diǎn),如第40號(hào)節(jié)點(diǎn)歐氏距離為1.70,滿足結(jié)束條件。

表1 更新節(jié)點(diǎn)與終點(diǎn)的歐氏距離Table 1 Euclidean distance of updated node from the end point

RRT算法的核心為extend函數(shù)[18],偽代碼如下:

1)V′←V;E′←E;#暫存現(xiàn)有頂點(diǎn)集和邊集;

2)qnearst←nearst(G,q);#利用nearst函數(shù)求在圖集中離采樣點(diǎn)歐氏距離最小的節(jié)點(diǎn)qnearst;

3)qnew←steer(qnearst,q);#利用steer函數(shù)求得qnearst到q的方向,以規(guī)定步長(zhǎng)r擴(kuò)展新的節(jié)點(diǎn)qnew;

4) if ObstacleFree(qnearst,qnew) then #碰撞檢測(cè),以圓形障礙物為例,碰撞檢測(cè)具體過(guò)程即求所有障礙物圓心到最新擴(kuò)展的邊的距離,比較此距離與當(dāng)前障礙物半徑的大小,若小于半徑即碰撞檢測(cè)失敗,反之通過(guò)檢測(cè);

5)V′←V′∪{qnew};#若通過(guò)碰撞檢測(cè)則更新頂點(diǎn)集;

6)E′←E′∪{(qnearst,qnew) };#若通過(guò)碰撞檢測(cè)則更新邊集;

7) returnG′=(V′,E′);#更新整體圖集。

圖2為在碰撞檢測(cè)后頂點(diǎn)集與邊集的更新過(guò)程,所有折線拐點(diǎn)的集合即頂點(diǎn)集。

圖2 碰撞檢測(cè)與頂點(diǎn)集、邊集更新過(guò)程Fig.2 Clash detection and vertex set and edge set update process

1.2 RRT*算法

基于采樣的運(yùn)動(dòng)規(guī)劃算法中,隨著采樣點(diǎn)趨近于無(wú)窮可以證明其收斂到最優(yōu)解的概率為0;RRT*算法即為基于此提出的一種漸進(jìn)最優(yōu)算法。相較于RRT算法最主要的區(qū)別是修剪枝的2個(gè)步驟保證了漸近最優(yōu)解,第1步是形成1個(gè)節(jié)點(diǎn)后在規(guī)定的范圍內(nèi)為當(dāng)前節(jié)點(diǎn)重新選擇父節(jié)點(diǎn);第2步是在當(dāng)前規(guī)定范圍內(nèi)為節(jié)點(diǎn)重新連接。

RRT*算法整體的路徑規(guī)劃步驟與RRT算法相似,extend函數(shù)是它與RRT算法的主要差距。偽代碼如下:

1)V′←V;E′←E;#暫存現(xiàn)有頂點(diǎn)集和邊集;

2)qnearst←nearst(G,q);#利用nearst函數(shù)求在圖集中離采樣點(diǎn)歐氏距離最小的節(jié)點(diǎn)qnearst;

3)qnew←steer(qnearst,q);#利用steer函數(shù)求得qnearst到q的方向,以規(guī)定步長(zhǎng)r擴(kuò)展新的節(jié)點(diǎn)qnew;

4) if ObstacleFree(qnearst,qnew) then #碰撞檢測(cè),以圓形障礙物為例,碰撞檢測(cè)具體過(guò)程即求所有障礙物圓心到最新擴(kuò)展的邊的距離,比較此距離與當(dāng)前障礙物半徑的大小,若小于半徑即碰撞檢測(cè)失敗,反之通過(guò)檢測(cè);

5)V′←V′∪{qnew};#若通過(guò)碰撞檢測(cè)則更新頂點(diǎn)集;

6)qmin←qnearst; #把離當(dāng)前采樣點(diǎn)最近的節(jié)點(diǎn)qnearst暫存于代價(jià)最小的變量qmin;

7)Cnear←near(G,qnew,|V|); #根據(jù)當(dāng)前最新節(jié)點(diǎn)規(guī)定一個(gè)范圍,即RTT*算法修剪枝葉的范圍;

8) for allqnear∈Cneardo #遍歷規(guī)定范圍內(nèi)的所有節(jié)點(diǎn),為新節(jié)點(diǎn)重新尋找父節(jié)點(diǎn);

9) if ObstacleFree(qnear,qnew) then #把新節(jié)點(diǎn)和遍歷的每一個(gè)節(jié)點(diǎn)做碰撞檢測(cè);

10)c′←cost(qnear)+pc(line(qnear,qnew));

11) ifc′

12)qmin←qnear;

13)E′←E′{(qmin,qnew)};

14) for allqnew∈Cnear{qmin} do

15) if ObstacleFree(qnew,qnear) and cost (qnew)>cost(qnew)+c(line(qnew,qnear)) then

16)qparent←parent(qnear);

17)E′←E′{(qparent,qnear)}; #把原來(lái)的邊集去掉當(dāng)前遍歷點(diǎn)和它原來(lái)父節(jié)點(diǎn)的路徑一次更新邊集;

18)E′∪{(qnew,qnear)}; #增加當(dāng)前遍歷點(diǎn)和新節(jié)點(diǎn)的連接路徑到邊集中,至此RTT*算法2步修剪完成;

19) returnG′=(V′,E′) #更新整個(gè)圖集。

其中前5步與RRT算法相同;6)～13)表示為當(dāng)前節(jié)點(diǎn)重新尋找父節(jié)點(diǎn),其中10)～12)表示通過(guò)碰撞檢測(cè)的點(diǎn)qnear的代價(jià)加上新節(jié)點(diǎn)qnew和qnear連線的代價(jià)暫存于c′中,如果新構(gòu)造父節(jié)點(diǎn)時(shí)qnew的路徑代價(jià)c′比原來(lái)qnew的代價(jià)小,則把當(dāng)前遍歷點(diǎn)qnear存入當(dāng)前新節(jié)點(diǎn)qnew的最小代價(jià)父節(jié)點(diǎn)qmin中,這樣遍歷完后就可以找到當(dāng)前新節(jié)點(diǎn)在規(guī)定范圍內(nèi)的最小代價(jià)父節(jié)點(diǎn);14)～16)是為了計(jì)算規(guī)定范圍Cnear內(nèi)(除了上面最優(yōu)父節(jié)點(diǎn)外的其他節(jié)點(diǎn))把新節(jié)點(diǎn)qnew重新作為自己的父節(jié)點(diǎn),比較之前的代價(jià),遍歷的這些點(diǎn)和新節(jié)點(diǎn)若是通過(guò)碰撞檢測(cè),且這些遍歷點(diǎn)原來(lái)的路徑代價(jià)比新連接的代價(jià)大,則更新當(dāng)前遍歷點(diǎn)的父節(jié)點(diǎn)。

圖3是關(guān)于RRT*算法重選父節(jié)點(diǎn)示意圖,點(diǎn)4是根據(jù)采樣點(diǎn)qrand新生成的節(jié)點(diǎn)qnew?，F(xiàn)在在灰色范圍內(nèi)遍歷節(jié)點(diǎn)為點(diǎn)4尋找最優(yōu)父節(jié)點(diǎn)。把點(diǎn)1和點(diǎn)3作為點(diǎn)4的新父節(jié)點(diǎn)與原來(lái)對(duì)比,若為點(diǎn)4重新尋找點(diǎn)1為父節(jié)點(diǎn),則點(diǎn)4的路徑代價(jià)更小。于是更新點(diǎn)1與點(diǎn)4之間的連線加入邊集,如圖3中虛線所示。

圖4是RRT*算法重新連接圖,點(diǎn)4為新生長(zhǎng)的節(jié)點(diǎn)qnew,重新連接即遍歷點(diǎn)2、3且它們以點(diǎn)4為父節(jié)點(diǎn)來(lái)進(jìn)行路徑代價(jià)比較,點(diǎn)2保持原來(lái)路徑較好,點(diǎn)3以點(diǎn)4為父節(jié)點(diǎn)重新連接,路徑代價(jià)比原來(lái)更小,于是去除點(diǎn)2與點(diǎn)3之間的邊,重新連接點(diǎn)4與點(diǎn)3之間的連線更新到邊集中,以此完成范圍內(nèi)重新連接的枝修剪。

圖3 RRT*算法重選父節(jié)點(diǎn)Fig.3 Parent node reselecting of RRT* algorithm

圖4 RRT*算法重新連接Fig.4 Reconnecting of RRT* algorithm

圖5 Informed-RRT*采樣空間Fig.5 Sampling space of Informed-RRT*

1.3 Informed-RRT*算法

Informed-RRT*和傳統(tǒng)的RRT*基本原理相似,只是在生成基本路徑后的采樣中改變了采樣空間,以此加快導(dǎo)向漸近最優(yōu)化路徑解的速度[19]。

2 改進(jìn)的雙向Informed-RRT*算法

2.1 初始路徑引入雙向搜索方式

圖6 雙向搜索Fig.6 Two-way search

Informed-RRT*算法在進(jìn)行變橢圓采樣時(shí)需先生成一條初始路徑,為了加快整體算法的搜索效率,考慮在生成初始路徑的過(guò)程中加快搜索速度,本文提出在生成初始路徑時(shí)引入一種雙向搜索的方式,如圖6所示。

引入的雙向搜索算法的具體步驟偽代碼如下:

1)V1←{qinit};E1←Φ;G1←(V1,E1);#初始化樹(shù)1;

2)V2←{qgoal};E2←Φ;G2←(V2,E2);i←0;#初始化樹(shù)2;

3) whilei

4)qrand←sample(i);i←i+1;#利用sample(i)函數(shù)采樣一個(gè)新的采樣點(diǎn)qrand;

5)qnearst←nearst(G1,qrand);#利用nearst函數(shù)求在圖集中離采樣點(diǎn)歐氏距離最小的節(jié)點(diǎn)qnearst;

6)qnew←steer(qnearst,qrand);#利用steer函數(shù)qnearst到q的方向,以規(guī)定步長(zhǎng)r擴(kuò)展新的節(jié)點(diǎn)qnew;

7) if ObstacleFree(qnearst,qnew) then #碰撞檢測(cè),以圓形障礙物為例,碰撞檢測(cè)具體過(guò)程即求所有障礙物圓心到最新擴(kuò)展的邊的距離,比較此距離與當(dāng)前障礙物半徑的大小,若小于半徑即碰撞檢測(cè)失敗,反之通過(guò)檢測(cè);

8)V1←V1∪{qnew};#若通過(guò)碰撞檢測(cè)則更新頂點(diǎn)集;

9)E1←E1∪{(qnearst,qnew)};#若通過(guò)碰撞檢測(cè)則更新邊集;

10)q′nearst←nearst(G2,qnew);#尋找樹(shù)1新產(chǎn)生的節(jié)點(diǎn)qnew到樹(shù)2上所有節(jié)點(diǎn)的歐氏距離最近節(jié)點(diǎn)的q′nearst,即把樹(shù)1的新節(jié)點(diǎn)qnew作為樹(shù)2的采樣點(diǎn)以此生長(zhǎng),這也是同樹(shù)1不同的地方;

11)q′new←steer(q′nearst,qnew);#從樹(shù)2上的最近節(jié)點(diǎn)q′nearst沿著樹(shù)1的新節(jié)點(diǎn)qnew的方向以規(guī)定步長(zhǎng)r生長(zhǎng)到樹(shù)2的新節(jié)點(diǎn)q′new;

12) if ObstacleFree(q′nearst,qnew);#進(jìn)行q′nearst和qnew的碰撞檢測(cè);

13)V2←V2∪{q′new};#若通過(guò)碰撞檢測(cè)則把樹(shù)2新產(chǎn)生的節(jié)點(diǎn)加入樹(shù)2的頂點(diǎn)集V2;

14)E2←E2∪{(q′nearst,q′new)};#若通過(guò)碰撞檢測(cè)則把樹(shù)2新產(chǎn)生的q′nearst和q′new之間的連線加入樹(shù)2的邊集E2中;

15) do

16)q″new←stree(q′new,qnew);

17) if ObstacleFree(q″new,q′new);

18)V2←V2∪{q″new};

19)E2←E2∪{q″new,q′new};

20)q′new←q″new;

21) else break;

22) while not ‖q′new-qnew‖

23) if ‖q′new-qnew‖

24) if |V2|<|V1| then return(V1,E1);#考慮2棵樹(shù)節(jié)點(diǎn)的平衡性,通過(guò)比較頂點(diǎn)集的大小交換次序選擇小數(shù)的擴(kuò)展。

其中步驟15)～22)表示基于樹(shù)2的擴(kuò)展方向繼續(xù)擴(kuò)展,直到樹(shù)2新擴(kuò)展的節(jié)點(diǎn)q′new連接上樹(shù)1新擴(kuò)展的節(jié)點(diǎn)qnew才結(jié)束樹(shù)2的擴(kuò)展。

2.2 引入P概率扇形約束采樣

作為一種基于采樣的運(yùn)動(dòng)規(guī)劃算法,隨機(jī)采樣決定了新節(jié)點(diǎn)的生長(zhǎng)方向,這種隨機(jī)性必然會(huì)導(dǎo)致缺乏目標(biāo)導(dǎo)向性[20]。本文為了改善目標(biāo)規(guī)劃效率,從采樣的角度提出一種P概率扇形約束采樣的方法。即以1～P的概率采用傳統(tǒng)的隨機(jī)采樣,這樣能增加算法的容錯(cuò)率,消除局部最優(yōu)。以概率P執(zhí)行扇形約束采樣增加目標(biāo)導(dǎo)向性。以下將從初始路徑規(guī)劃和橢圓規(guī)劃2方面討論。

如圖7所示,在生成初始路徑的時(shí)候由于沒(méi)有橢圓約束,連接上一次的更新節(jié)點(diǎn)qnew(i-1)和目標(biāo)節(jié)點(diǎn)qgoal作為扇形的中線,以2θ作為扇形夾角,射線L(qnew(i-1),A)與射線L(qnew(i-1),B)即為采樣的扇形區(qū)域,qrand即為本次扇形隨機(jī)采樣點(diǎn),r為生長(zhǎng)步長(zhǎng),qnew(i)為新生長(zhǎng)的節(jié)點(diǎn),再進(jìn)行碰撞檢測(cè),若通過(guò)則加入圖集。圖8是橢圓區(qū)域P概率篩選采樣示意圖,取橢圓和扇形的交集區(qū)域,采樣范圍為qnew(i-1)、A、C、B所圍成的區(qū)域。

圖7 P概率扇形采樣Fig.7 P probability sector sampling

圖8 橢圓區(qū)域P概率扇形采樣Fig.8 P probability sector sampling in elliptic region

2.3 引入生長(zhǎng)轉(zhuǎn)角偏置

圖9 生長(zhǎng)轉(zhuǎn)角偏置Fig.9 Growth corner offset

由1.3節(jié)可知傳統(tǒng)Informed-RRT*算法的采樣橢圓嚴(yán)重依賴初始路徑的質(zhì)量[21],這直接決定了后續(xù)橢圓規(guī)劃的效率,因此初始路徑的優(yōu)化也是值得考慮的方面;而且在橢圓規(guī)劃過(guò)程中如果能加速導(dǎo)向目標(biāo)點(diǎn)也會(huì)大大加速算法的收斂速度。傳統(tǒng)算法在采樣結(jié)束后新節(jié)點(diǎn)的生長(zhǎng)方向往往是沿著采樣點(diǎn)qrand的方向,這會(huì)使得靠近目標(biāo)點(diǎn)的速度變慢,于是提出引入生長(zhǎng)轉(zhuǎn)角偏置策略。

如圖9所示,夾角α為直線最近節(jié)點(diǎn)qnear1與目標(biāo)節(jié)點(diǎn)qgoal所得向量與qnear1到采用節(jié)點(diǎn)qrand所得向量之間的夾角;若是沒(méi)有生長(zhǎng)方向偏置則原本生長(zhǎng)的節(jié)點(diǎn)為q′new1,但是通過(guò)添加可調(diào)偏置系數(shù)μ(0<μ<1),便可改變偏置方向,更加有利于導(dǎo)向目標(biāo)生長(zhǎng)。

2.4 引入變步長(zhǎng)生長(zhǎng)

在確定搜索方式、采樣空間和生長(zhǎng)方向后,生長(zhǎng)步長(zhǎng)也是可以考慮的方面。傳統(tǒng)的RRT類算法都是以固定步長(zhǎng)r生長(zhǎng)[22]。若是環(huán)境復(fù)雜,障礙物密集度較高,固定步長(zhǎng)將很容易使得樹(shù)的生長(zhǎng)收斂速度減慢。為了在障礙物密集的復(fù)雜環(huán)境中路徑規(guī)劃有更好的適用性,根據(jù)環(huán)境復(fù)雜度引入動(dòng)態(tài)步長(zhǎng)生長(zhǎng)策略。

如圖10所示(見(jiàn)封2),以qnearst為半圓的圓心,綠色半圓直徑為2M(M>r),方向與qnearst到qrand的方向垂直,r為初始化固定步長(zhǎng),綠色半圓構(gòu)成的區(qū)域?yàn)閝nearst環(huán)境復(fù)雜度所考慮的范圍,在范圍內(nèi)的節(jié)點(diǎn)1、2、3分別做半徑為N的藍(lán)色圓,黑色代表障礙物,設(shè)藍(lán)色圓相交的黑色障礙物的個(gè)數(shù)為S,相交個(gè)數(shù)設(shè)置2個(gè)閾值等級(jí)U和W(0

圖10 變步長(zhǎng)生長(zhǎng)Fig.10 Variable step growth

(1)

通過(guò)每次采樣可求得S的大小,根據(jù)環(huán)境復(fù)雜度定義動(dòng)態(tài)步長(zhǎng)rd如下:

(2)

通過(guò)以上4個(gè)方面完成對(duì)于本文算法的改進(jìn),圖11是改進(jìn)算法的整體流程。

圖11 改進(jìn)算法流程Fig.11 Flowchart of the improved algorithm

3 實(shí)驗(yàn)與分析

3.1 實(shí)驗(yàn)設(shè)計(jì)

本文設(shè)計(jì)了2組測(cè)試實(shí)驗(yàn),第1組為不同環(huán)境復(fù)雜度下的針對(duì)規(guī)劃路徑效果測(cè)試的對(duì)比實(shí)驗(yàn);第2組為針對(duì)算法性能指標(biāo)的測(cè)試實(shí)驗(yàn),可證明本文算法的正確性。系統(tǒng)環(huán)境為Windows 10操作系統(tǒng)和PyCharm 2020.3.3 (Community Edition)。

3.2 結(jié)果與分析

第1組仿真實(shí)驗(yàn)主要針對(duì)相同步長(zhǎng)和采樣次數(shù)下,不同環(huán)境復(fù)雜度的RRT、RRT*、Informed-RRT*算法和本文改進(jìn)的雙向Informed-RRT*算法進(jìn)行規(guī)劃路徑的質(zhì)量對(duì)比,此處以2維規(guī)劃為例,具體仿真結(jié)果如圖12、圖13所示。

圖12 簡(jiǎn)單環(huán)境下算法仿真結(jié)果Fig.12 Simulation results of the algorithm in a simple environment

圖12是在簡(jiǎn)單環(huán)境下對(duì)比算法的規(guī)劃結(jié)果,圖12(a)顯然非漸近最優(yōu)路徑,圖12(b)～圖12(d)的路徑逐漸變優(yōu)。在簡(jiǎn)單環(huán)境下傳統(tǒng)Informed-RRT*算法由于在橢圓內(nèi)部采樣是隨機(jī)的,雖然路徑比之前的2種傳統(tǒng)算法好,但是可以看出在采樣次數(shù)受限制下,改進(jìn)的Informed-RRT*的路徑導(dǎo)向性更好,雖然路徑長(zhǎng)短相差不大,但是路徑轉(zhuǎn)彎幅度更小。

圖13是在復(fù)雜環(huán)境下對(duì)比算法的規(guī)劃結(jié)果,在相同采樣迭代次數(shù)相同的情況下,圖13(d)改進(jìn)算法的規(guī)劃路徑比其他路徑長(zhǎng)度和轉(zhuǎn)彎幅度更小,且相比較于其他規(guī)劃的路徑,在采樣次數(shù)有限且相同的情況下,對(duì)于復(fù)雜度較高的環(huán)境其適應(yīng)性更強(qiáng)。

第2組實(shí)驗(yàn)對(duì)算法耗時(shí)進(jìn)行定量測(cè)試。首先在相同環(huán)境和基礎(chǔ)步長(zhǎng)2下對(duì)比傳統(tǒng)Informed-RRT*算法和本文算法進(jìn)行20次實(shí)驗(yàn),測(cè)試初始路徑平均規(guī)劃時(shí)間和總規(guī)劃時(shí)間,可以看出無(wú)論在初始路徑的規(guī)劃過(guò)程中還是在總規(guī)劃過(guò)程中總體性能都有一定的提升,且總規(guī)劃性能提升相較于初始路徑更加明顯,具體實(shí)驗(yàn)結(jié)果如表2所示。

表2 引入生長(zhǎng)轉(zhuǎn)角偏置性能比較Table 2 Introduced growth corner bias performance comparison table

在相同環(huán)境和基礎(chǔ)步長(zhǎng)2下對(duì)比傳統(tǒng)Informed-RRT*算法和本文改進(jìn)的算法進(jìn)行20次實(shí)驗(yàn),可以看出隨著采樣迭代次數(shù)的增加,2種算法耗時(shí)差距開(kāi)始拉大,本文路徑規(guī)劃算法效率更高,算法平均實(shí)時(shí)性變化曲線如圖14所示。

圖13 復(fù)雜環(huán)境下算法仿真結(jié)果Fig.13 Algorithm simulation results in complex environments

圖14 算法運(yùn)行時(shí)間折線圖Fig.14 Algorithm running time line chart

(3)

式中:n為樹(shù)上已規(guī)劃路徑的總節(jié)點(diǎn)數(shù);第i號(hào)節(jié)點(diǎn)qi的坐標(biāo)為(xi,yi);IAT為平均轉(zhuǎn)彎指數(shù)值。不同環(huán)境相同迭代次數(shù)下,分別進(jìn)行20組實(shí)驗(yàn)得到的性能比較結(jié)果如表3所示。

無(wú)論是在簡(jiǎn)單環(huán)境下還是在復(fù)雜環(huán)境下,平均規(guī)劃路徑長(zhǎng)度、平均規(guī)劃時(shí)間、初始路徑迭代次數(shù)、規(guī)劃成功率和ATI等參數(shù)性能都有所提升;初始路徑平均迭代次數(shù)在復(fù)雜環(huán)境和簡(jiǎn)單環(huán)境性能提升差不多;除了初始路徑平均迭代次數(shù)和規(guī)劃成功率,其他參數(shù)的參數(shù)性能提升復(fù)雜環(huán)境都更優(yōu)于簡(jiǎn)單環(huán)境,這是因?yàn)楦倪M(jìn)算法在生成初始路徑后完成了對(duì)生成初始路徑后規(guī)劃過(guò)程的優(yōu)化。對(duì)于ATI參數(shù),因?yàn)楦倪M(jìn)算法應(yīng)用了變步長(zhǎng)思想,本文改進(jìn)的算法對(duì)于復(fù)雜環(huán)境的性能提升更加明顯,更適用于復(fù)雜環(huán)境的路徑規(guī)劃。整體改進(jìn)算法對(duì)于規(guī)劃效率、規(guī)劃成功率、規(guī)劃路徑的質(zhì)量和環(huán)境適用性都有較大的提升。

表3 算法性能比較Table 3 Algorithm performance comparison

4 結(jié) 論

針對(duì)Informed-RRT*算法應(yīng)用于運(yùn)動(dòng)規(guī)劃過(guò)程中存在規(guī)劃效率低、規(guī)劃路徑冗余且轉(zhuǎn)彎較多等問(wèn)題,提出一種雙向Informed-RRT*算法。首先提出初始路徑生成采用雙向搜索的方式,有效提高了初始路徑生成效率;其次提出一種P概率扇形約束采樣的方法,增加了路徑規(guī)劃導(dǎo)向性和容錯(cuò)率;接著提出在節(jié)點(diǎn)擴(kuò)展時(shí)引用生長(zhǎng)轉(zhuǎn)角偏置,有效加快算法收斂速度;最后提出變步長(zhǎng)生長(zhǎng)的方式有效提高了環(huán)境自適應(yīng)性。

全文設(shè)計(jì)了大量的對(duì)比仿真實(shí)驗(yàn)。與傳統(tǒng)的Informed-RRT*路徑規(guī)劃算法相比,改進(jìn)算法的平均規(guī)劃路徑長(zhǎng)度、平均規(guī)劃時(shí)間、初始化路徑平均迭代次數(shù)、平均轉(zhuǎn)彎指數(shù)分別減少了3.63%、19.55%、18.99%、32.55%,規(guī)劃成功率提高了9.45%,實(shí)驗(yàn)驗(yàn)證了該路徑規(guī)劃算法的正確性和可行性。進(jìn)一步的研究工作希望能在本算法的基礎(chǔ)上解決動(dòng)態(tài)障礙物的局部路徑規(guī)劃問(wèn)題和路徑平滑問(wèn)題。