基于融合改進(jìn)雙向RRT與人工勢(shì)場(chǎng)法路徑規(guī)劃

錢(qián) 程,劉興德,陳大光

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

隨著科技的發(fā)展,機(jī)器人應(yīng)用越來(lái)越廣泛,路徑規(guī)劃作為機(jī)器人導(dǎo)航的關(guān)鍵技術(shù)之一,近些年,關(guān)于路徑規(guī)劃的研究也逐漸增多。路徑規(guī)劃是機(jī)器人在一個(gè)有障礙物的環(huán)境中,可以在不接觸障礙物的同時(shí),從起始點(diǎn)到達(dá)目標(biāo)點(diǎn)所走過(guò)的路徑。路徑規(guī)劃主要分為全局路徑規(guī)劃和局部路徑規(guī)劃[1]。

在路徑規(guī)劃算法研究中,RRT算法簡(jiǎn)單,但存在隨機(jī)性大,效率低且搜索路徑并非最優(yōu)的問(wèn)題,針對(duì)RRT算法的缺陷,國(guó)內(nèi)外學(xué)者提出了關(guān)于RRT*算法的改進(jìn)[2]。Karaman等提出了漸進(jìn)最優(yōu)RRT算法,有助于提高路徑搜索速度[3];Gammell等提出了Informen-RRT*算法,對(duì)路徑進(jìn)行改進(jìn)和優(yōu)化[4];LaValle等提出了雙向搜索RRT算法,該算法提高了搜索效率,縮短了搜索時(shí)間[5]。

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

1 路徑規(guī)劃算法

1.1 快速擴(kuò)展隨機(jī)樹(shù)算法

RRT算法是一種常見(jiàn)的路徑規(guī)劃算法,屬于全局路徑規(guī)劃,它是由LaValle在1998年提出,該算法是通過(guò)構(gòu)建隨機(jī)樹(shù)的方法將起始點(diǎn)作為樹(shù)的根節(jié)點(diǎn),進(jìn)行隨機(jī)采樣獲取隨機(jī)點(diǎn),以這種方式反復(fù)尋找,直到生成一條由起始點(diǎn)到目標(biāo)點(diǎn)的路徑[7]。RRT算法的擴(kuò)展示意圖如圖1所示。

圖1 RRT算法擴(kuò)展示意圖

如圖1所示,首先,定義起始點(diǎn)Xstart和目標(biāo)點(diǎn)Xgoal,以起始點(diǎn)Xstart為根節(jié)點(diǎn)建立擴(kuò)展隨機(jī)樹(shù)。其次,根據(jù)起始點(diǎn)Xrand隨機(jī)產(chǎn)生一個(gè)采樣點(diǎn),計(jì)算擴(kuò)展節(jié)點(diǎn)與采樣點(diǎn)Xrand之間的距離,找到距離采樣點(diǎn)Xrand最近的節(jié)點(diǎn)Xnear。然后,在Xstart和Xnear的連線(xiàn)方向上根據(jù)固定步長(zhǎng)L,找到一個(gè)新節(jié)點(diǎn)Xnew,并判斷產(chǎn)生的新的節(jié)點(diǎn)Xnew與Xnear之間的連線(xiàn)是否存在障礙物并發(fā)生碰撞,若存在則舍棄該采樣點(diǎn),重新選取采樣新節(jié)點(diǎn),若不存在則保留。最后循環(huán)采樣,直至找到目標(biāo)點(diǎn)Xgoal,生成路徑。

1.2 人工勢(shì)場(chǎng)法

人工勢(shì)場(chǎng)法屬于局部路徑規(guī)劃,它是由Khatib在1985年提出,在路徑規(guī)劃中實(shí)現(xiàn)避障功能[8]。該方法通過(guò)勢(shì)場(chǎng),建立機(jī)器人與障礙物、機(jī)器人與目標(biāo)點(diǎn)之間的斥力勢(shì)場(chǎng)、引力勢(shì)場(chǎng),通過(guò)兩個(gè)勢(shì)場(chǎng)力的合力作用,使機(jī)器人向著目標(biāo)點(diǎn)的方向運(yùn)動(dòng)。

人工勢(shì)場(chǎng)的引力函數(shù)表示形式:

(1)

式中:Uatt為引力勢(shì)場(chǎng)函數(shù);X為機(jī)器人位置;Xgoal為目標(biāo)點(diǎn)位置;Katt為引力增益常數(shù)。

斥力函數(shù)表達(dá)形式:

(2)

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

由引力勢(shì)場(chǎng)的負(fù)梯度得到引力:

Fatt=-Katt(X-Xgoal) ,

(3)

由斥力勢(shì)場(chǎng)的負(fù)梯度得到斥力:

(4)

根據(jù)引力和斥力,計(jì)算出合力:

Ftotal=Fatt+Frep。

(5)

人工勢(shì)場(chǎng)法擴(kuò)展示意如圖2所示。

圖2 人工勢(shì)場(chǎng)法擴(kuò)展示意圖

1.3 雙向快速擴(kuò)展隨機(jī)樹(shù)算法

雙向RRT算法是將起始點(diǎn)和目標(biāo)點(diǎn)作為根節(jié)點(diǎn),構(gòu)造兩棵快速隨機(jī)擴(kuò)展樹(shù),進(jìn)行雙向快速擴(kuò)展,一側(cè)從起始點(diǎn)向目標(biāo)點(diǎn)方向隨機(jī)采樣,另一側(cè)從目標(biāo)點(diǎn)向起始點(diǎn)方向隨機(jī)采樣,兩棵樹(shù)隨機(jī)采樣產(chǎn)生的新節(jié)點(diǎn),當(dāng)一棵樹(shù)產(chǎn)生的新節(jié)點(diǎn)與另一棵樹(shù)的隨機(jī)采樣點(diǎn)之間的距離小于步長(zhǎng)閾值,將兩個(gè)節(jié)點(diǎn)相連,這樣兩棵樹(shù)合并成一棵樹(shù),并生成相關(guān)路徑。雙向RRT算法擴(kuò)展示意圖如圖3所示。

圖3 雙向RRT算法擴(kuò)展示意圖

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

2 改進(jìn)算法

2.1 APF與RRT融合

將人工勢(shì)場(chǎng)的思想引到RRT算法中,通過(guò)人工勢(shì)場(chǎng)的導(dǎo)向作用引導(dǎo)RRT進(jìn)行搜索,可以有效避障并快速形成搜索路徑,提高搜索效率。兩算法融合主要計(jì)算出隨機(jī)點(diǎn)在人工勢(shì)場(chǎng)的引導(dǎo)下新的節(jié)點(diǎn),在人工勢(shì)場(chǎng)法和RRT算法融合中產(chǎn)生的新節(jié)點(diǎn)[9]:

(6)

(7)

式中:Ftotal為機(jī)器人在Xnear中所受的合力;L為步長(zhǎng)閾值;ε為勢(shì)場(chǎng)分量因子。

APF與RRT算法融合擴(kuò)展示意圖如圖4所示。在人工勢(shì)場(chǎng)中,隨機(jī)點(diǎn)受到目標(biāo)引力和障礙物斥力作用,使其向著引力與斥力合力的方向擴(kuò)展,形成新的節(jié)點(diǎn),循環(huán)隨機(jī)采樣,直至形成路徑。

圖4 APF與RRT算法擴(kuò)展示意圖

2.2 APF與雙向RRT融合

將人工勢(shì)場(chǎng)法與雙向RRT算法融合,其目的是通過(guò)雙向RRT算法進(jìn)行全局路徑規(guī)劃,人工勢(shì)場(chǎng)法進(jìn)行局部路徑優(yōu)化,在兩者融合的算法中減少路徑冗余,從而提高路徑規(guī)劃的效率[10]。其具體流程圖如圖5所示。

圖5 APF與雙向RRT流程圖

(1)初始化定義起始點(diǎn)Xstart、目標(biāo)點(diǎn)Xgoal,樹(shù)T1和樹(shù)T2,在人工勢(shì)場(chǎng)中進(jìn)行導(dǎo)向隨機(jī)點(diǎn)采樣,樹(shù)T1和樹(shù)T2分別產(chǎn)生隨機(jī)點(diǎn)Xrand1和Xrand2。

(2)隨機(jī)采樣找到與Xrand1和Xrand2最近的節(jié)點(diǎn)Xrand1和Xnear1。

(3)確定Xrand1和Xnear1與目標(biāo)點(diǎn)與障礙物之間的距離,根據(jù)在Xrand1和Xnear1處的合力和步長(zhǎng),生成新節(jié)點(diǎn)Xnew1和Xnew2。

(4)檢測(cè)新節(jié)點(diǎn)是否滿(mǎn)足避障要求,若滿(mǎn)足,檢測(cè)兩新節(jié)點(diǎn)是否進(jìn)行相連,如不滿(mǎn)足,則重新采樣,更新雙向RRT隨機(jī)樹(shù),直至滿(mǎn)足避障要求。

(5)雙向隨機(jī)樹(shù)兩個(gè)新節(jié)點(diǎn)進(jìn)行連接,生成全局路徑。

3 仿真驗(yàn)證

為了驗(yàn)證算法的可行性,使用MATLAB 2022a,在二維環(huán)境中定義起始點(diǎn)(20,20),目標(biāo)點(diǎn)(650,650),步長(zhǎng)L為10,最大迭代次數(shù)為1 000,將以上四種算法,在同一環(huán)境中進(jìn)行路徑規(guī)劃,每種算法重復(fù)進(jìn)行60次實(shí)驗(yàn),得出數(shù)據(jù)的平均結(jié)果。數(shù)據(jù)對(duì)比結(jié)果見(jiàn)表1,搜索路徑對(duì)比圖如圖6所示。

表1 實(shí)驗(yàn)對(duì)比結(jié)果

圖6 搜索路徑對(duì)比圖

4 結(jié) 論

本文提出將人工勢(shì)場(chǎng)法與雙向RRT算法結(jié)合,首先介紹傳統(tǒng)RRT算法和人工勢(shì)場(chǎng)法,在其基礎(chǔ)上改進(jìn)雙向RRT算法,人工勢(shì)場(chǎng)法分別與RRT算法和雙向RRT算法相結(jié)合。通過(guò)對(duì)比得出,人工勢(shì)場(chǎng)法與雙向RRT算法融合使路徑規(guī)劃距離縮短,減少路徑規(guī)劃時(shí)間,后續(xù)對(duì)路徑規(guī)劃研究有一定參考作用。