基于改進(jìn)RRT*算法的移動機(jī)器人路徑規(guī)劃研究*

印 峰，謝青松

(湘潭大學(xué) 自動化與電子信息學(xué)院，湖南 湘潭 411105)

0 引言

移動機(jī)器人路徑規(guī)劃研究的目標(biāo)是在具有障礙的環(huán)境中，在滿足不發(fā)生碰撞的條件下搜索，依據(jù)路徑和時間最短原則，搜索一條從起始點到目標(biāo)點的最優(yōu)可行路徑[1-2].目前路徑規(guī)劃研究的主要方法[3]有A*算法[4]、Dijkstra算法[5]、人工勢場法[6]和快速探索隨機(jī)樹(RRT)算法[7]，以及漸進(jìn)最優(yōu)快速隨機(jī)搜索樹(RRT*)算法.其中RRT的主要思想是從起始點開始隨機(jī)生成一個樹狀的網(wǎng)絡(luò)圖，通過迭代地探索一系列中繼點(即父節(jié)點)，最終形成一條到達(dá)目標(biāo)點的可行路徑.傳統(tǒng)的RRT算法不具有目標(biāo)導(dǎo)向特性，其搜索路徑過程具有較強(qiáng)的隨機(jī)性，針對這個問題，Karaman等[8]提出了一種具有目標(biāo)導(dǎo)向型的漸進(jìn)最優(yōu)快速隨機(jī)搜索樹RRT*算法，其原理是通過對父節(jié)點進(jìn)行優(yōu)化選擇以提高原有RRT算法的搜索效率.通過改變父節(jié)點選擇的方式，采用代價函數(shù)來選取拓展節(jié)點范圍內(nèi)最小代價節(jié)點為父節(jié)點，同時，每次迭代后都會重新連接現(xiàn)有樹上的節(jié)點，保證計算以漸進(jìn)方式探索到最優(yōu)解.

為了優(yōu)化節(jié)點，RRT*算法是采用高斯采樣策略，由于摒棄的隨機(jī)節(jié)點中有可能包含了當(dāng)前的最優(yōu)節(jié)點，因此算法無法保證通過計算代價最小值的方式來獲得當(dāng)前最優(yōu)節(jié)點.同時，由于搜索最優(yōu)父節(jié)點的過程需要多次迭代計算，這無疑降低了整體規(guī)劃的時效性.針對上述問題，研究者對RRT*做出了一系列的改進(jìn)工作.Gammell等[9]提出的Informed-RRT*算法，通過引入空間約束以減少冗余節(jié)點的數(shù)量，降低搜索過程的隨機(jī)性.但是，該方法花銷的時間仍然過長，目前僅適用于狹窄區(qū)域的路徑搜索[10-12].

針對上述問題，本文提出了一種基于RRT*的改進(jìn)算法，其主要思想是應(yīng)用動態(tài)步態(tài)延伸幫助RRT*算法找到更優(yōu)的潛在父節(jié)點集，然后結(jié)合一種新穎的類三維地圖中的駐點信息，在可行的范圍內(nèi)確定最優(yōu)的父節(jié)點，從而實現(xiàn)更為高效的路徑規(guī)劃過程.為了驗證本文文法的有效性，從路徑長度以及時間性能兩個方面，對比了本文方法和最新的Informed-RRT*算法.

1 研究的理論基礎(chǔ)

1.1 RRT算法

RRT算法是一種隨機(jī)采樣方法，它可以是在二維空間甚至是在三維空間中都很有效的規(guī)劃方法，RRT算法從起始點出發(fā)，通過對未知空間以隨機(jī)采樣的方式增加子節(jié)點，最終生成一個隨機(jī)擴(kuò)展樹，當(dāng)隨機(jī)擴(kuò)展樹中的子節(jié)點含有目標(biāo)點時，則生成一條從初始點到目標(biāo)點的目標(biāo)路徑，RRT算法擴(kuò)展方式如圖1所示.

RRT算法以Xinit為出發(fā)點進(jìn)行擴(kuò)展，在陌生環(huán)境隨機(jī)選取一個采樣點Xrand，然后選取距離該采樣點最近的一個節(jié)點Xnear，在Xnear和Xrand的連線上以固定區(qū)間的步長ε選取一個新的采樣點Xnew.如果當(dāng)前的采樣點Xnew沒有與已知的障礙物發(fā)生碰撞，那么將采樣點Xnew加入隨機(jī)樹中，反之則將該采樣點舍棄，并重新選取采樣點Xrand進(jìn)行擴(kuò)展.重復(fù)以上步驟直到擴(kuò)展樹找到一條從Xinit到Xgoal的無碰撞路徑.

1.2 RRT*算法

研究表明，RRT算法仍存在許多有待提升的空間[13].例如對于特點形狀的應(yīng)用場景(例如狹窄通道)，RRT算法收斂速度會有較明顯的下降.針對該問題，文獻(xiàn)[14-15]在RRT算法上提出了一種改進(jìn)的RRT*算法.后者重新定義了父節(jié)點的搜索方式，對父節(jié)點采取一種導(dǎo)向式的引導(dǎo).相較于RRT算法，RRT*算法最大程度保留了RRT算法的概率完備性，由于采用了父節(jié)點引導(dǎo)機(jī)制，同時提高了算法的收斂速度，使得可以以更少的迭代生成更優(yōu)的路徑，提高了規(guī)劃的效率.RRT*路徑的擴(kuò)展方式如圖2所示.

圖2 RRT*的可行路徑擴(kuò)展示意圖Fig.2 Schematic diagram of feasible path extension for RRT*

RRT*算法首先產(chǎn)生一個隨機(jī)點Xrand，然后基于RRT算法找到隨機(jī)樹上距離Xrand最近的一個節(jié)點Xnearest并與之相連.以Xrand為中心，r為半徑，在隨機(jī)樹上搜索潛在父節(jié)點Xpotential，通過碰撞測試計算路徑代價，并與之前的路徑做比較，選取代價較小的節(jié)點為最新父節(jié)點，切斷之前較長的路徑并更新，直到擴(kuò)展樹到達(dá)目標(biāo)點為止.

1.3 類三維地圖

類三維地圖是在普通的二值柵格地圖上人為地按照一定規(guī)則添加高度值，使其在路徑規(guī)劃中不需要借助其他信息即可通過障礙物本身的高度信息就可以完成部分避障工作.

在文獻(xiàn)[16]中將位于障礙物范圍內(nèi)高度值最高的點定義為起點，之后垂直于這個路徑內(nèi)的箭頭方向為下降方向，同時將修正路徑段到被修正路徑段中最大的點定義為一個犄角點，而位于路徑同一側(cè)中距離這段路徑最遠(yuǎn)的犄角點則定義為駐點，駐點的詳細(xì)尋找圖如圖3所示.

圖3 類三維地圖駐點以及路徑尋找方式Fig.3 Three-dimensional-like map stationary point and path finding way

首先先確定起始點A和目標(biāo)點B，假設(shè)要從A點運動到B點，需要經(jīng)過中間一大片障礙物，對于自由區(qū)域所有柵格點的高度值都賦值為0，然后定義高度值大于1的點，之后對于障礙物賦予坐標(biāo)中的最大值以及最小值，并對所有柵格點的高度值賦予1，對于最大點附近的柵格點依次減1，同時對最小點附近的柵格點依次加1，連接AB兩點經(jīng)過障礙物與障礙物接觸點分別定義為CD兩點，如圖3上的弧線，CD兩點之間可以進(jìn)一步優(yōu)化為折線，從下降方向判定可以得知F點為駐點，連接AFB即為最短路徑.

由于類三維地圖中駐點定義的特殊性，可以依據(jù)駐點的定義加入新算法中，RRT*算法定義的父節(jié)點是以Xrand為半徑的范圍內(nèi)確定的，當(dāng)加入駐點定義到新算法中，依據(jù)駐點的定義是根據(jù)路徑箭頭方向為下降方向，其具有一定的目標(biāo)偏向性，如果此時再以駐點為中心去搜索新的潛在父節(jié)點，則相比于RRT*算法搜索的潛在父節(jié)點會更具有目的性.

2 RRT*算法改進(jìn)

2.1 算法設(shè)計原理

RRT*采用漸進(jìn)最優(yōu)的路徑搜索模式，在潛在父節(jié)點的選取時會產(chǎn)生大量無用的結(jié)點，造成其計算量隨著樣本的數(shù)量增加而急劇增加，為了減少冗余節(jié)點的數(shù)量，本文首先基于RRT*算法的采樣模式進(jìn)行初采樣；然后，以提取的類三維地圖中駐點為中心，在一定的半徑范圍內(nèi)尋找潛在父節(jié)點，通過具有目標(biāo)偏向目標(biāo)的搜索可以得到更為理想的潛在父節(jié)點，進(jìn)而減少路徑長度和規(guī)劃時間.為了便于描述，將改進(jìn)算法記為Class3D-RRT*算法.

RRT*隨機(jī)點采樣模式為高斯采樣，具有在一定小范圍內(nèi)采樣多個點并選擇最優(yōu)點的特征.與之相比較， Class3D-RRT*算法不僅可以具有偏向目標(biāo)方向的作用，而且可以有效縮小搜索父節(jié)點的范圍，使其在選取父節(jié)點時相比于原算法具有更強(qiáng)的導(dǎo)向性.而使用了更為合適的潛在父節(jié)點可以得到更優(yōu)的規(guī)劃結(jié)果.

Class3D-RRT*算法的詳細(xì)步驟如圖4所示.

圖4 算法示意圖Fig.4 Schematic diagram of algorithm

首先以潛在父節(jié)點為中心，Rp為半徑，在其半徑范圍內(nèi)做下降方向去搜索駐點，同時以駐點為中心，Rh為半徑，從該半徑范圍內(nèi)去搜索潛在的父節(jié)點，當(dāng)搜索到潛在父節(jié)點時連線，同時以當(dāng)前潛在父節(jié)點為中心繼續(xù)尋找下一個駐點，如未尋找到，則依據(jù)RRT*算法尋找潛在父節(jié)點并連線，最終通過此方法搜尋到一條從初始點到目標(biāo)點的路徑.下面為算法的詳細(xì)步驟以及算法實現(xiàn).

2.2 算法實現(xiàn)

在確定父節(jié)點時，搜索樹探索到各個節(jié)點，以r為半徑搜索最近的一個節(jié)點Xnearest，通過碰撞測試并與原路徑相比較，若所得新的路徑更近，則將Xnearest記為Xpotential-parent，然后，設(shè)搜索樹T=(V,E)，以Xpotential-parent為中心，記坐標(biāo)為(Ix,Iy)為Rp的球形空間Bpotential-parent，此時：

S(Xpotential-parent，T，Rp)={v∈V；v∈Bpotential-parent}，Xstagnationpoint?V.

(1)

式中，S表示駐點的集合.

半徑r為：

(2)

式中：δ為常數(shù)；v為節(jié)點集合；Num(v)為集合v中所有節(jié)點的數(shù)目；n為配置空間維度，此時此刻r隨著節(jié)點數(shù)的增大減小，而此時球形空間Bpotential-parent的半徑Rp亦等于r.

當(dāng)確定最佳潛在父節(jié)點時，判斷下降方向，若此時下降路徑穿過了某個障礙物，則定義位于障礙區(qū)域路徑下降方向一側(cè)的點(Iy，Jy)，其值滿足下式：

(3)

式中：k=Im1-In1/Jm1-Jn1，(Im1，Jm1)是當(dāng)前穿過障礙物路徑的起始點坐標(biāo)，(In1，Jn1)是當(dāng)前穿過障礙物的終點坐標(biāo)，(Iam，Jam)是當(dāng)前障礙物最大的坐標(biāo)點.在確定下降方向后，再尋找當(dāng)前犄角點.在規(guī)劃實施過程中，將修正路徑段到被修正路徑長度距離最大的點定義為犄角點，此時修正路徑段上的點(Iy，Jy)與被修正路徑段的距離為h=s/2c，其中：

(4)

(5)

(6)

(7)

(8)

式中：(Iy，Jy)為修正路徑段上的點坐標(biāo)；(Im2，Jm2)為被修正路徑段上的起點坐標(biāo)；(In2，Jn2)為被修正路徑段的終點坐標(biāo).在獲取不同障礙物的犄角點后，再從中選取駐點，應(yīng)當(dāng)選擇路徑同一側(cè)距離這段路徑最遠(yuǎn)的犄角點為駐點.此時以駐點為中心，Rh為半徑，并且令Rh=Rp，在半徑范圍內(nèi)選取最佳潛在父節(jié)點，直至到達(dá)目標(biāo)點為止.算法的主要計算步驟如下：

步驟1：將當(dāng)前的普通地圖轉(zhuǎn)換為類三維地圖并產(chǎn)生一個隨機(jī)點Xrand.通過隨機(jī)點Xrand生成一個隨機(jī)樹,并在其中找到與Xrand最近的一個節(jié)點Xnearest.

步驟2：以Xrand為中心，Ri為半徑，在節(jié)點樹中對當(dāng)前的Xnearest進(jìn)行碰撞測試，如果此時Xnearest穿越到了柵格點大于0的點則定義為穿過障礙區(qū)域.

步驟3：如果此時可以通過碰撞測試，并通過計算相對于原先路徑更近，則可以成為當(dāng)前的Xpotential-parent.

步驟4：以Xpotential-parent為中心，Rp為半徑，作下降方向，若路徑穿過某個障礙區(qū)域，則依據(jù)類三維地圖法犄角點的定義去確定犄角點，再依據(jù)相對距離進(jìn)而判斷駐點，若未穿過，則定義目前的Xpotential-parent轉(zhuǎn)變?yōu)樾碌腦init，重新產(chǎn)生隨機(jī)點Xrand并重復(fù)上述步驟.

步驟5：當(dāng)確定駐點Xstationary point時，以此點為中心，Rh為半徑，在該半徑范圍內(nèi)尋找下一個潛在父節(jié)點，之后應(yīng)用原RRT*算法繼續(xù)搜索下一個潛在父節(jié)點，直到找到一條從初始點到目標(biāo)點的路徑.

圖5給出了本文算法主要步驟的圖示過程.

圖5 Class3D-RRT*路程圖Fig.5 Class3D-RRT* road map

3 實驗仿真與分析

3.1 單障礙物地圖構(gòu)建

為了驗證Class3D-RRT*的性能，本文在pycharm社區(qū)版中進(jìn)行相關(guān)測試實驗，并將RRT*算法、Informed-RRT*算法以及Class3D-RRT*的規(guī)劃結(jié)果進(jìn)行了對比.本實驗分別在單障礙物地形和多障礙物地形情況下進(jìn)行實驗仿真，計算機(jī)處理器采用Intel-i7 11800H，內(nèi)存16 GB.

首先是對單障礙物地形情況進(jìn)行模擬，在單一障礙物中計算出的路徑時間進(jìn)行比較，起始點在(0,2)，目標(biāo)點在(8，8).圖6為單障礙物環(huán)境路徑對比圖.圖中，圈代表圓形障礙物，線為實際算法運算路徑.

圖6 單障礙物情況地圖路徑對比Fig.6 Map path comparison for single obstacle case

如表1所示，在單障礙物地圖的仿真過程中，通過實驗結(jié)果得知，本文算法相對于RRT*算法以及Informed-RRT*算法，平均路徑更短，平均耗時也更短.

表1 單障礙物地圖仿真實驗數(shù)據(jù)對比

3.2 多障礙物地圖應(yīng)用

在多圖形障礙物的復(fù)雜情況下，多障礙地圖1的初始點坐標(biāo)為(4，5)，目標(biāo)點坐標(biāo)為(14，10)，多障礙地圖2的初始點坐標(biāo)為(0,2)，目標(biāo)點坐標(biāo)為(10，10)，多障礙地圖3的初始點坐標(biāo)為(0，2)，目標(biāo)點坐標(biāo)為(14，10).圖7、8、9所示分別為在多障礙環(huán)境地圖1、2、3中，同方法規(guī)劃路徑的實驗結(jié)果對比.圖中的圓圈或者方塊代表障礙物.

圖7 多障礙環(huán)境地圖1中規(guī)劃路徑對比Fig.7 Comparison of planned paths in Multi-obstacle environment Map 1

圖9 在多障礙環(huán)境地圖3中規(guī)劃路徑對比Fig.9 Comparison of planned paths in Multi-obstacle environment Map 3

如表2所示，在多障礙物地圖的三組仿真實驗中，第一組與第二組多障礙物地圖仿真實驗中，只有單一樣式不同大小的障礙物，第三組實驗中同時有樣式不同、大小不一的障礙物，無論是對比平均路徑長度還是平均耗時來看，本文算法更優(yōu).

表2 多障礙物地圖仿真實驗數(shù)據(jù)對比

4 結(jié)束語

本文的主要目的是為了路徑更優(yōu)同時也是為了正確地避開障礙物，以盡量最短的路程規(guī)劃從初始點到達(dá)目標(biāo)點，本文提出了一種Class3D-RRT*算法，其主要方法是在搜索到潛在父節(jié)點后，在其半徑范圍內(nèi)搜索是否有駐點，依據(jù)駐點在此范圍內(nèi)尋找下一個父節(jié)點，這樣更具有目的性，使用新的改進(jìn)算法在簡單以及復(fù)雜的障礙物中反復(fù)進(jìn)行仿真實驗，最終的仿真結(jié)果也表明新的改進(jìn)方法可以更加有效地選擇更優(yōu)父節(jié)點，進(jìn)而使得路徑規(guī)劃的路程更短，時間更優(yōu).