融合神經(jīng)網(wǎng)絡(luò)GRU和RRT*算法的機(jī)械臂路徑規(guī)劃*

周 哲,歐陽(yáng)勇

(湖北工業(yè)大學(xué)計(jì)算機(jī)學(xué)院,武漢 430064)

0 引言

隨著工業(yè)時(shí)代的快速發(fā)展,機(jī)器人在各行各業(yè)都得到廣泛應(yīng)用,包括工業(yè)制造、醫(yī)療、運(yùn)輸?shù)萚1-2],如何在不觸碰障礙物的條件下完成軌跡規(guī)劃任務(wù)是研究的重點(diǎn)。

理想的軌跡規(guī)劃算法通常具有以下特性:完備性、最優(yōu)性以及算法效率。目前機(jī)械臂軌跡規(guī)劃算法通?？梢苑譃榛趫D搜索的規(guī)劃算法和基于隨機(jī)采樣的規(guī)劃算法[3]?；趫D搜索的規(guī)劃算法有深度優(yōu)先搜索[4]、廣度優(yōu)先搜索[5]、Dijkstra算法[6]和A*算法[7]等。這類基于圖搜索的算法適用于低維空間,具有較好的完備性,但是在高維空間算法效率大大降低。

為了解決維度問(wèn)題,專家學(xué)者們提出了基于采樣的規(guī)劃算法,這類算法通過(guò)在構(gòu)型空間(C-space)中隨機(jī)采樣構(gòu)建無(wú)碰撞路徑圖,主要算法包括概率路圖算法(PRM)[8]和快速隨機(jī)搜索樹算法(RRT)[9],以及RRT算法的衍生,RRT-Connect、RRT*[10]等。這類基于采樣的算法都具備完備性和快速性,但仍有如下缺點(diǎn):①生成的軌跡主要由折線組成,因此平滑性較差;②由于是隨機(jī)采樣,所以每次采樣點(diǎn)較多,造成資源的浪費(fèi)。

為了解決上述問(wèn)題,提出了融合神經(jīng)網(wǎng)絡(luò)GRU和RRT*的路徑規(guī)劃算法。GRU為了解決其他神經(jīng)網(wǎng)絡(luò)算法存在的梯度下降和梯度爆炸的問(wèn)題[11],通過(guò)重置門和更新門決定哪些信息將最后被門控循環(huán)單元輸出,并且能夠長(zhǎng)期的保存長(zhǎng)期序列中的信息。利用該采樣器,GRU-RRT*算法有效的避免了搜索的隨機(jī)性,引導(dǎo)RRT*向著終點(diǎn)延伸,減少規(guī)劃次數(shù),增強(qiáng)算法探索能力,提高了算法搜索樹的探索效率,減少規(guī)劃的時(shí)間。

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

1.1 RRT*算法

RRT*算法是一種基于隨機(jī)采樣的規(guī)劃算法,傳統(tǒng)的RRT*算法在RRT算法的基礎(chǔ)上增加一個(gè)重連的操作,以初始點(diǎn)在配置空間中構(gòu)建隨機(jī)采樣樹擴(kuò)展,滿足RRT算法快速搜索的同時(shí),達(dá)到漸進(jìn)最優(yōu)的目的。

RRT*算法流程圖如圖1所示,從初始點(diǎn)qinit出發(fā),構(gòu)建初始隨機(jī)搜索樹Tinit,利用GRU-Sampler進(jìn)行采樣,選擇無(wú)碰撞的節(jié)點(diǎn)qrandom,選擇搜索樹中距離qrandom最近的節(jié)點(diǎn)qnear,從qnear向qrandom方向延伸一條步長(zhǎng)step的路徑,生成最新的節(jié)點(diǎn)qnew,同時(shí)判斷新生成的節(jié)點(diǎn)路徑是否有碰撞,如果有碰撞,再找下一個(gè)采樣點(diǎn),否則,將最新的節(jié)點(diǎn)qnew加入隨機(jī)搜索樹T中,qnear為其父節(jié)點(diǎn),重復(fù)以上過(guò)程,直到隨機(jī)搜索樹搜索到目標(biāo)點(diǎn)qgoal為止。在搜索樹插入新節(jié)點(diǎn)的過(guò)程中,RRT*算法會(huì)進(jìn)行一個(gè)重選父節(jié)點(diǎn)的操作,在找到qnew節(jié)點(diǎn)后,在范圍內(nèi)重新選擇父節(jié)點(diǎn),如果該父節(jié)點(diǎn)代價(jià)更小,則以該點(diǎn)作為父節(jié)點(diǎn)進(jìn)行延伸;重選父節(jié)點(diǎn)之后,以qnew作為父節(jié)點(diǎn)遍歷范圍內(nèi)的其他點(diǎn),尋找代價(jià)最小的一條為搜索樹的路徑。

1.2 簡(jiǎn)單采樣和GRU門控神經(jīng)網(wǎng)絡(luò)集成

1.2.1 GRU門控神經(jīng)網(wǎng)絡(luò)

GRU門控神經(jīng)網(wǎng)絡(luò)是在LSTM神經(jīng)網(wǎng)絡(luò)的基礎(chǔ)上進(jìn)行的簡(jiǎn)化,只留下重置門和更新門,減少了權(quán)重參數(shù)的個(gè)數(shù),GRU門控單元結(jié)構(gòu)如圖2所示。

圖2 GRU門控單元結(jié)構(gòu)圖

圖中,Rt為重置門,Zt是更新門,Xt為輸入序列,Ht-1為上一個(gè)時(shí)間步的節(jié)點(diǎn)狀態(tài),Ht為當(dāng)前節(jié)點(diǎn)狀態(tài),br,bh,bz為偏置項(xiàng)。更新門決定上一節(jié)點(diǎn)隱藏狀態(tài)的更新,將上一個(gè)時(shí)間步的Ht-1和輸入序列Xt投入到sigmoid激活函數(shù)中,選擇重要的信息進(jìn)行保留;重置門對(duì)當(dāng)前輸入隱藏狀態(tài)的記憶進(jìn)行重置,忘掉上一個(gè)時(shí)間步不重要的信息。

Rt=σ(XtWxr+Ht-1Whr+br)

(1)

Zt=σ(XtWxz+Ht-1Whz+bz)

(2)

輸入?yún)?shù)通過(guò)重置門與上一個(gè)時(shí)間步的狀態(tài)的哈達(dá)瑪乘積得到候選隱藏狀態(tài)。

(3)

真正的隱藏狀態(tài)是更新門與上一個(gè)時(shí)間步的狀態(tài)和當(dāng)前步的候選隱藏狀態(tài)哈達(dá)瑪乘積之和。Zt的值決定上一個(gè)時(shí)間步的狀態(tài)和候選隱藏狀態(tài)有多少能夠傳遞下去。

(4)

1.2.2 GRU采樣器

GRU-RRT*算法于RRT*算法的主要區(qū)別在于采樣方式的不同,RRT*算法在給定范圍內(nèi)進(jìn)行隨機(jī)采樣,時(shí)間代價(jià)大,收斂速度降低,而GRU-RRT*算法采用了一種新的方法生成節(jié)點(diǎn),將GRU應(yīng)用于基于采樣的規(guī)劃器。

GRU采樣器是簡(jiǎn)單采樣方法和GRU網(wǎng)絡(luò)的集成,這種集成使采樣器能夠從早期的搜索經(jīng)驗(yàn)中學(xué)習(xí),以生成預(yù)測(cè)節(jié)點(diǎn),并引導(dǎo)通向目標(biāo)狀態(tài)的路徑,同時(shí)更有效的避免障礙。GRU采樣器通過(guò)消除不滿足期望的節(jié)點(diǎn)來(lái)提高路徑規(guī)劃過(guò)程的效率,如圖3和圖4所示。

圖3 隨機(jī)采樣 圖4 GRU采樣

GRU的構(gòu)圖如圖5所示,將預(yù)處理數(shù)據(jù)(初始點(diǎn),目標(biāo)點(diǎn),障礙物)傳入GRU采樣器中,GRU采樣器包括輸入層和輸出層, GRU神經(jīng)網(wǎng)絡(luò)隱藏層和dropout層,dropout層從GRU神經(jīng)網(wǎng)絡(luò)中獲取輸出并將其轉(zhuǎn)化為GRU隱藏層期望的格式,最后通過(guò)線性輸出層將采樣點(diǎn)傳出到RRT*算法當(dāng)中,引導(dǎo)算法對(duì)生成節(jié)點(diǎn)的預(yù)測(cè)。

圖5 GRU-RRT*算法結(jié)構(gòu)圖

(5)

表1 GRU-RRT*算法偽代碼

圖6 GRU-RRT*算法流程圖

1.3 自適應(yīng)步長(zhǎng)

標(biāo)準(zhǔn)RRT算法不會(huì)利用擴(kuò)展時(shí)收集到的信息去動(dòng)態(tài)調(diào)整步長(zhǎng),固定步長(zhǎng)過(guò)大或過(guò)小都會(huì)影響算法收斂速度,最后在搜索樹接近目標(biāo)點(diǎn)時(shí),如果qnearest與qgoal的距離大于qgoal的搜索范圍γ,則容易在目標(biāo)點(diǎn)處振蕩,增加規(guī)劃的時(shí)間和路徑長(zhǎng)度,因此需要對(duì)擴(kuò)展樹步長(zhǎng)進(jìn)行自適應(yīng)操作。因此,使用文獻(xiàn)[12]的策略去自適應(yīng)擴(kuò)展步長(zhǎng)提高算法擴(kuò)展效率。

(6)

式中:s為初始步長(zhǎng),l為qnearest與qgoal的距離。

1.4 三次樣條軌跡平滑

在多項(xiàng)式樣條中,三次樣條是保證加速度和速度的延續(xù)以及有限抖動(dòng)的最低階多項(xiàng)式軌跡。高階多項(xiàng)式會(huì)導(dǎo)致更平滑的輪廓,但有更長(zhǎng)的運(yùn)動(dòng)時(shí)間。運(yùn)用關(guān)節(jié)空間軌跡規(guī)劃方法中的三次樣條插值函數(shù)方法,可以得到位置與時(shí)間的函數(shù),及其一階導(dǎo)數(shù)和二階導(dǎo)數(shù),一階導(dǎo)數(shù)和二階導(dǎo)數(shù)連續(xù)即速度與加速度連續(xù)。

分段式三次樣條函數(shù)的通式如下:

s(t)={qk(t),t∈[tk,tk+1],k=0,1,2,…,n-1}

(7)

qk(t)=ak0+ak1(t-tk)+ak2(t-tk)2+ak3(t-tk)3

(8)

上式將整條線段分隔出n條線段,構(gòu)造出n條三次多項(xiàng)式,ak0,ak1,ak2,ak3均為表達(dá)式待定系數(shù),每條多項(xiàng)式有4個(gè)未知數(shù),因此總共需要確認(rèn)4n個(gè)未知數(shù)。每條三次函數(shù)必須滿足以下條件:

①區(qū)間分隔點(diǎn)xi(xi∈(x0,xn))處的位置,速度和加速度連續(xù);

②初始位置和終點(diǎn)位置的速度為0;

(9)

4n個(gè)表達(dá)式可以求解出4n個(gè)未知系數(shù),利用上述等式,可以得到以下結(jié)果:

(10)

(11)

式中:Tk=tk+1-tk,Qk=qk+1-qk。知道每段三次樣條曲線所需時(shí)間T,即可利用追趕法可以求出中間的速度值,從而得到分段式三次樣條插值平滑曲線。

2 實(shí)驗(yàn)仿真

為了驗(yàn)證GRU-RRT*算法的有效性,從二維平面和三維空間兩個(gè)角度分別對(duì)RRT,RRT*和GRU-RRT*算法進(jìn)行驗(yàn)證,通過(guò)不同的算法在路徑長(zhǎng)度,規(guī)劃時(shí)間等方面進(jìn)行算法驗(yàn)證。

GRU采樣器參數(shù)設(shè)置如下,GRU神經(jīng)層設(shè)置80個(gè)神經(jīng)元,利用Adam優(yōu)化器調(diào)整學(xué)習(xí)率,將學(xué)習(xí)率設(shè)置為0.000 1,中間dropout層選擇0.5的丟失率來(lái)最小化損失值,激活函數(shù)為tanh。

2.1 二維地圖仿真

二維地圖的環(huán)境場(chǎng)景大小設(shè)置為500×500,初始坐標(biāo)點(diǎn)qinit為(1,1),目標(biāo)點(diǎn)qgoal為(19,19),隨機(jī)在地圖中擺放障礙物,GRU-RRT*設(shè)置起始步長(zhǎng)為1,目標(biāo)點(diǎn)區(qū)域半徑設(shè)置為0.35。4種算法在二維平面下的規(guī)劃如圖7～圖10所示。

圖9 RRT*二維規(guī)劃圖 圖10 GRU-RRT*二維規(guī)劃圖

圖7 PRM二維規(guī)劃圖圖8 RRT二維規(guī)劃圖

如表2所示,分別將PRM、RRT、RRT*和GRU-RRT*算法在二維地圖上做20次實(shí)驗(yàn)取平均值,平均路徑長(zhǎng)度為31.192 3,耗時(shí)0.893 s,相比于PRM、RRT和RRT*,平均路徑長(zhǎng)度縮短了11%、18%、6%,在時(shí)間損耗上分別降低了1.04 s、0.76 s、0.233 s。在二維柵格地圖中,改進(jìn)算法無(wú)論在長(zhǎng)度規(guī)劃還是在算法運(yùn)行時(shí)間上,效率都高于原始算法。

表2 算法結(jié)果對(duì)比圖

2.2 三維地圖仿真

三維地圖的環(huán)境場(chǎng)景大小設(shè)置為100×100×100,初始坐標(biāo)點(diǎn)qinit為(1,1,1),目標(biāo)點(diǎn)qgoal為(99,99,99),隨機(jī)在地圖中擺放障礙物,GRU-RRT*設(shè)置起始步長(zhǎng)為8,目標(biāo)點(diǎn)區(qū)域半徑設(shè)置為3,4種算法在二維平面下的規(guī)劃如圖11～圖13所示。

圖11 RRT算法三維規(guī)劃圖 圖12 RRT*算法三維規(guī)劃圖

圖13 GRU-RRT*算法三維規(guī)劃圖

由表3可知,在經(jīng)過(guò)20次實(shí)驗(yàn)之后,改進(jìn)算法平均耗時(shí)7.692 s,平均路徑長(zhǎng)度為172.961 1,較改進(jìn)之前快了2.293 s,長(zhǎng)度也縮短了17.632 5。

表3 算法結(jié)果對(duì)比圖

利用睿爾曼機(jī)械臂模型在rviz上進(jìn)行仿真,圖14為GRU-RRT*算法的仿真效果圖,灰色部分為設(shè)置5個(gè)障礙物a,b,c,d,e。在機(jī)械臂從起始點(diǎn)到目標(biāo)點(diǎn)移動(dòng)的過(guò)程中,GRU-RRT*算法很好的避開了所有的障礙物,同時(shí)運(yùn)行軌跡也十分平滑。

圖14 睿爾曼機(jī)械臂軌跡規(guī)劃

3 結(jié)束語(yǔ)

針對(duì)機(jī)械臂的軌跡規(guī)劃,提出了融合神經(jīng)網(wǎng)絡(luò)GRU的RRT*算法,解決了原始RRT*算法存在的收斂速度慢,路徑質(zhì)量差的問(wèn)題。

(1)提出將門控神經(jīng)網(wǎng)絡(luò)算法GRU算法作為采樣器,替代RRT*算法中的隨機(jī)采樣來(lái)生成節(jié)點(diǎn),預(yù)測(cè)擴(kuò)展樹節(jié)點(diǎn)生成方向,同時(shí)將RRT*算法步長(zhǎng)進(jìn)行自適應(yīng),減少軌跡搜索的時(shí)間,提高了算法的收斂速度。

(2)利用分段三次樣條曲線對(duì)規(guī)劃的路徑進(jìn)行平滑,最后得到一條滿足要求的平滑的曲線。

分別在二維、三維環(huán)境下使用不同地圖對(duì)該算法進(jìn)行仿真測(cè)試,GRU-RRT算法在軌跡規(guī)劃和平滑上都有顯著的效果,驗(yàn)證了該算法的可行性。