基于融合改進雙向RRT與人工勢場法路徑規劃

錢 程,劉興德,陳大光

(1．吉林化工學院 信息與控制工程學院,吉林 吉林 132022;2．吉林化工學院 機電工程學院,吉林 吉林 132022)

隨著科技的發展,機器人應用越來越廣泛,路徑規劃作為機器人導航的關鍵技術之一,近些年,關于路徑規劃的研究也逐漸增多。路徑規劃是機器人在一個有障礙物的環境中,可以在不接觸障礙物的同時,從起始點到達目標點所走過的路徑。路徑規劃主要分為全局路徑規劃和局部路徑規劃[1]。

在路徑規劃算法研究中,RRT算法簡單,但存在隨機性大,效率低且搜索路徑并非最優的問題,針對RRT算法的缺陷,國內外學者提出了關于RRT*算法的改進[2]。Karaman等提出了漸進最優RRT算法,有助于提高路徑搜索速度[3];Gammell等提出了Informen-RRT*算法,對路徑進行改進和優化[4];LaValle等提出了雙向搜索RRT算法,該算法提高了搜索效率,縮短了搜索時間[5]。

針對快速隨機樹(RRT)算法的不足,本研究采用將雙向RRT算法與人工勢場法改進融合的方法進行路徑規劃,本方法在提高路徑規劃效率的基礎上,還可以有效避開路徑規劃中的障礙物,最后利用Matlab進行仿真驗證此方法的有效性[6]。

1 路徑規劃算法

1.1 快速擴展隨機樹算法

RRT算法是一種常見的路徑規劃算法,屬于全局路徑規劃,它是由LaValle在1998年提出,該算法是通過構建隨機樹的方法將起始點作為樹的根節點,進行隨機采樣獲取隨機點,以這種方式反復尋找,直到生成一條由起始點到目標點的路徑[7]。RRT算法的擴展示意圖如圖1所示。

圖1 RRT算法擴展示意圖

如圖1所示,首先,定義起始點Xstart和目標點Xgoal,以起始點Xstart為根節點建立擴展隨機樹。其次,根據起始點Xrand隨機產生一個采樣點,計算擴展節點與采樣點Xrand之間的距離,找到距離采樣點Xrand最近的節點Xnear。然后,在Xstart和Xnear的連線方向上根據固定步長L,找到一個新節點Xnew,并判斷產生的新的節點Xnew與Xnear之間的連線是否存在障礙物并發生碰撞,若存在則舍棄該采樣點,重新選取采樣新節點,若不存在則保留。最后循環采樣,直至找到目標點Xgoal,生成路徑。

1.2 人工勢場法

人工勢場法屬于局部路徑規劃,它是由Khatib在1985年提出,在路徑規劃中實現避障功能[8]。該方法通過勢場,建立機器人與障礙物、機器人與目標點之間的斥力勢場、引力勢場,通過兩個勢場力的合力作用,使機器人向著目標點的方向運動。

人工勢場的引力函數表示形式:

(1)

式中:Uatt為引力勢場函數;X為機器人位置;Xgoal為目標點位置;Katt為引力增益常數。

斥力函數表達形式:

(2)

式中:Urep為斥力勢場函數;X為機器人位置;Xobs為障礙物位置;(X-Xobs)為機器人與障礙物之間距離;P為障礙物影響范圍;Krep為斥力增益常數。

由引力勢場的負梯度得到引力:

Fatt=-Katt(X-Xgoal) ,

(3)

由斥力勢場的負梯度得到斥力:

(4)

根據引力和斥力,計算出合力:

Ftotal=Fatt+Frep。

(5)

人工勢場法擴展示意如圖2所示。

圖2 人工勢場法擴展示意圖

1.3 雙向快速擴展隨機樹算法

雙向RRT算法是將起始點和目標點作為根節點,構造兩棵快速隨機擴展樹,進行雙向快速擴展,一側從起始點向目標點方向隨機采樣,另一側從目標點向起始點方向隨機采樣,兩棵樹隨機采樣產生的新節點,當一棵樹產生的新節點與另一棵樹的隨機采樣點之間的距離小于步長閾值,將兩個節點相連,這樣兩棵樹合并成一棵樹,并生成相關路徑。雙向RRT算法擴展示意圖如圖3所示。

圖3 雙向RRT算法擴展示意圖

如圖3所示,首先,構建兩棵隨機樹T1和T2,樹T1以Xstart作為樹的根節點進行擴展,樹T2以Xgoal為樹的根節點進行擴展。其次,根據兩個根節點隨機產生兩個采樣點Xrand1和Xrand2,計算樹T1、T2擴展節點與采樣點Xrand1之間的距離,找到距離采樣點Xrand1最近的節點Xnear1,距離采樣點Xrand2最近的節點Xnear2。然后,在Xstart和Xnear1的連線方向上根據固定步長L,找到一個新節點Xnew1,在Xgoal和Xnear2的連線方向上根據固定步長L,找到一個新節點Xnew1,判斷產生的新的節點Xnew1與Xnear1和Xnew2與之間的連線是否存在障礙物并發生碰撞,若存在則舍去,重新采樣,反之保留新節點。最后,按照上述方法繼續擴展,直至兩棵樹的新節點之間的距離小于步長閾值時,兩個新節點進行相連,此時樹T1和樹T2連通,生成路徑。

2 改進算法

2.1 APF與RRT融合

將人工勢場的思想引到RRT算法中,通過人工勢場的導向作用引導RRT進行搜索,可以有效避障并快速形成搜索路徑,提高搜索效率。兩算法融合主要計算出隨機點在人工勢場的引導下新的節點,在人工勢場法和RRT算法融合中產生的新節點[9]:

(6)

(7)

式中:Ftotal為機器人在Xnear中所受的合力;L為步長閾值;ε為勢場分量因子。

APF與RRT算法融合擴展示意圖如圖4所示。在人工勢場中,隨機點受到目標引力和障礙物斥力作用,使其向著引力與斥力合力的方向擴展,形成新的節點,循環隨機采樣,直至形成路徑。

圖4 APF與RRT算法擴展示意圖

2.2 APF與雙向RRT融合

將人工勢場法與雙向RRT算法融合,其目的是通過雙向RRT算法進行全局路徑規劃,人工勢場法進行局部路徑優化,在兩者融合的算法中減少路徑冗余,從而提高路徑規劃的效率[10]。其具體流程圖如圖5所示。

圖5 APF與雙向RRT流程圖

(1)初始化定義起始點Xstart、目標點Xgoal,樹T1和樹T2,在人工勢場中進行導向隨機點采樣,樹T1和樹T2分別產生隨機點Xrand1和Xrand2。

(2)隨機采樣找到與Xrand1和Xrand2最近的節點Xrand1和Xnear1。

(3)確定Xrand1和Xnear1與目標點與障礙物之間的距離,根據在Xrand1和Xnear1處的合力和步長,生成新節點Xnew1和Xnew2。

(4)檢測新節點是否滿足避障要求,若滿足,檢測兩新節點是否進行相連,如不滿足,則重新采樣,更新雙向RRT隨機樹,直至滿足避障要求。

(5)雙向隨機樹兩個新節點進行連接,生成全局路徑。

3 仿真驗證

為了驗證算法的可行性,使用MATLAB 2022a,在二維環境中定義起始點(20,20),目標點(650,650),步長L為10,最大迭代次數為1 000,將以上四種算法,在同一環境中進行路徑規劃,每種算法重復進行60次實驗,得出數據的平均結果。數據對比結果見表1,搜索路徑對比圖如圖6所示。

表1 實驗對比結果

圖6 搜索路徑對比圖

4 結 論

本文提出將人工勢場法與雙向RRT算法結合,首先介紹傳統RRT算法和人工勢場法,在其基礎上改進雙向RRT算法,人工勢場法分別與RRT算法和雙向RRT算法相結合。通過對比得出,人工勢場法與雙向RRT算法融合使路徑規劃距離縮短,減少路徑規劃時間,后續對路徑規劃研究有一定參考作用。