具備啟發(fā)式路徑優(yōu)化的機(jī)械臂路徑規(guī)劃算法

鄧鈃中 張偉軍

(上海交通大學(xué)機(jī)械與動(dòng)力工程學(xué)院,上海 200240)

0 路徑規(guī)劃算法發(fā)展現(xiàn)狀

如今,機(jī)械臂和移動(dòng)機(jī)器人在許多領(lǐng)域扮演著重要角色。移動(dòng)機(jī)器人被廣泛應(yīng)用于未知環(huán)境的探測(cè)[1］、導(dǎo)航、定位[2］和制圖等[3］。多關(guān)節(jié)機(jī)械臂在制造業(yè)領(lǐng)域發(fā)揮著重要的作用,能夠完成搬運(yùn),組裝和加工等任務(wù)。它們不僅能提高生產(chǎn)效率和產(chǎn)品質(zhì)量,還能減少人力成本和操作風(fēng)險(xiǎn)。路徑規(guī)劃是一項(xiàng)必要而關(guān)鍵的技術(shù),能幫助機(jī)器人在這些任務(wù)中更加高效智能地完成任務(wù)。

為解決路徑規(guī)劃問題,需要在給定狀態(tài)空間中找到一條連接起始點(diǎn)和目標(biāo)點(diǎn)的路徑并保證路徑不會(huì)與任何障礙物發(fā)生碰撞。從1970年開始,學(xué)術(shù)界涌現(xiàn)了各類路徑規(guī)劃算法包括基于搜索的算法、基于采樣的算法、基于人工勢(shì)場(chǎng)的算法。各類算法在不同場(chǎng)景各有優(yōu)勢(shì)[4-9］,算法的耗時(shí)和路徑的距離通常是評(píng)價(jià)該算法最有效的指標(biāo)。基于搜索的算法如A*[10］和Dijkstra[11］算法需要把問題的狀態(tài)空間劃分成網(wǎng)格,所以基于搜索的算法的效率和可行性依賴于網(wǎng)格的劃分,并且很難適應(yīng)不同問題中狀態(tài)空間的維度變化。

基于人工勢(shì)場(chǎng)的算法APF的路徑規(guī)劃方法通過(guò)引力和斥力勢(shì)場(chǎng)來(lái)實(shí)現(xiàn)機(jī)器人的導(dǎo)航和避障,根據(jù)環(huán)境感知實(shí)時(shí)計(jì)算勢(shì)場(chǎng),適用于動(dòng)態(tài)環(huán)境中的移動(dòng)物體?；谌斯?shì)場(chǎng)算法在構(gòu)建地圖勢(shì)能場(chǎng)時(shí)需要獲知全局地圖,但這在機(jī)械臂運(yùn)動(dòng)的高維狀態(tài)空間中是相當(dāng)困難的?；谌斯?shì)場(chǎng)算法容易陷入局部最小值和陷阱,難以完成復(fù)雜環(huán)境中的規(guī)劃任務(wù)。

基于采樣的規(guī)劃算法如RRT[12］通過(guò)在問題狀態(tài)空間中采樣增量式構(gòu)建一個(gè)以機(jī)器人初始點(diǎn)為根節(jié)點(diǎn)并向無(wú)障礙可達(dá)空間擴(kuò)展的搜索樹。通過(guò)對(duì)狀態(tài)空間的隨機(jī)采樣將極大降低算法的時(shí)間復(fù)雜度?；诓蓸拥囊?guī)劃算法適用于高維狀態(tài)空間的路徑規(guī)劃并能方便地?cái)U(kuò)展到不同維度的問題中。RRT能在大多數(shù)場(chǎng)景中高效地找到路徑,但搜索時(shí)間和路徑距離仍然存在較大優(yōu)化空間。研究人員基于RRT提出了許多改進(jìn)。改進(jìn)的方向大概分為兩類,即初始路徑前優(yōu)化OBIP(Optimize before Initial Path)和初始路徑后優(yōu)化OAIP(Optimize after Initial Path)。OBIP意味著在找到初始路徑前或在尋找初始路徑過(guò)程中進(jìn)行優(yōu)化。RRT*[13］在構(gòu)建搜索樹的過(guò)程中對(duì)新節(jié)點(diǎn)執(zhí)行“Rwire”步驟能幫助新節(jié)點(diǎn)找到代價(jià)更低的父節(jié)點(diǎn)以進(jìn)一步減小路徑代價(jià)。RRT-Connect[14］算法分別以起始點(diǎn)和目標(biāo)點(diǎn)為根節(jié)點(diǎn)生成兩棵搜索樹,當(dāng)兩棵樹互相連接時(shí)認(rèn)為路徑規(guī)劃成功,其中的啟發(fā)式步驟極大加快了搜索速度,并在狹窄路徑場(chǎng)景中表現(xiàn)出優(yōu)異性能。FMT*[15］算法采用先采樣,后連接的方法完成路徑規(guī)劃,添加新節(jié)點(diǎn)時(shí)只會(huì)發(fā)生一次碰撞檢測(cè),是一種更快的漸近最優(yōu)路徑規(guī)劃算法。通常來(lái)說(shuō),想要一次性找到最優(yōu)路徑是比較困難的,OAIP意味著在找到初始路徑后再對(duì)路徑進(jìn)行尋優(yōu)。Informed RRT*算法利用RRT*算法找到初始路徑,再根據(jù)初始路徑劃分出超橢球子集作為后續(xù)采樣空間,Informed RRT*通過(guò)縮小采樣空間的方法加快了路徑的尋優(yōu)速度[16］。RRT*-Smart[17］算法在RRT*算法基礎(chǔ)上提出了智能采樣和路徑優(yōu)化方法,在找到初始路徑后快速尋優(yōu)得到近似最優(yōu)路徑。BIT*[18］算法吸取了RRT*、Informed RRT*和FMT*算法的特點(diǎn),使用多批多點(diǎn)采樣方法和有序搜索方法提高了搜索效率。

為了盡可能搜索得到一條代價(jià)更小的路徑,我們引入了Informed RRT*SR(Informed RRT*with Smart Rope)算法?；贗nformed RRT*的工作,Informed RRT*SR能夠高效地在高維空間中搜索到無(wú)碰路徑,并通過(guò)在無(wú)碰路徑附近的狀態(tài)空間進(jìn)行啟發(fā)式探索對(duì)無(wú)碰路徑進(jìn)行進(jìn)一步優(yōu)化。Informed RRT*SR繼承了Informed RRT*高效搜索的優(yōu)點(diǎn),并利用路徑優(yōu)化算法克服了路徑蜿蜒曲折的缺點(diǎn)。在路徑優(yōu)化階段,相比于其他基于采樣的對(duì)狀態(tài)空間進(jìn)行完全隨機(jī)搜索的路徑探索算法,具備啟發(fā)式探索規(guī)則的Informed RRT*SR能更有針對(duì)性地探索狀態(tài)空間從而減少路徑搜索時(shí)間。Informed RRT*SR通常能找到一條更快捷的無(wú)碰路徑,且路徑的轉(zhuǎn)折點(diǎn)能很好地適應(yīng)障礙物的外形。本文首先描述了通篇使用的Informed RRT*SR的符號(hào)和算法描述,給出了基于采樣算法和Informed RRT*SR在實(shí)際問題中的仿真結(jié)果。

1 算法描述

1.1 算法符號(hào)說(shuō)明

路徑規(guī)劃問題的目標(biāo)是在狀態(tài)空間中找到一條連通起始點(diǎn)和目標(biāo)點(diǎn)的無(wú)碰路徑,這條連續(xù)的路徑被表示為P=(V,E),此處V表示路徑中的節(jié)點(diǎn),E?V×V代表連接兩個(gè)節(jié)點(diǎn)的一條邊。每個(gè)節(jié)點(diǎn)V均屬于n維路徑規(guī)劃問題的狀態(tài)空間,表示為X?n。在機(jī)器人路徑規(guī)劃問題中,n則代表機(jī)器人構(gòu)型空間的維度數(shù)目。在路徑規(guī)劃問題的狀態(tài)空間中存在的障礙表示為Xobs?X,無(wú)障礙空間則表示為Xfree=XXobs。

判斷碰撞:函數(shù)Obstaclefree(xa,xb)用于判斷Xline?Xfree是否成立,Xline的定義是公式1。

Xline={x|x=αxa+(1-α)xb,α∈[0,1]}

(1)

節(jié)點(diǎn)無(wú)碰連接檢測(cè):Connectfree(xi,xj,xk)表示對(duì)從xi、xj到xk,依次直接相連接的路徑進(jìn)行碰撞檢測(cè),如果全程沒有碰撞則返回真,否則返回假。

空間取基:Base(A)表示取得向量空間A的所有基向量。

計(jì)算路徑:Dist({A,B})表示計(jì)算由A和B組成的邊的距離。

回溯路徑:getPath(x)表示從節(jié)點(diǎn)x開始回溯得到路徑P。

取路徑終點(diǎn):getEnd(P)表示取得路徑P的終點(diǎn)x。

算法中的Cost、Sample、Nearest、Steer、Line、Parent、InGoalRegion函數(shù)均沿用于Informed RRT*論文[16］。

表1 Informed RRT*SR算法

Informed RRT*SR算法在Informed RRT*的基礎(chǔ)上引入了路徑優(yōu)化步驟。算法1中的Line1-Line36為Informed RRT*算法。算法1中的Line38-42表明在Informed RRT*算法搜尋得到無(wú)碰路徑后對(duì)該路徑進(jìn)行進(jìn)一步優(yōu)化以縮短路徑距離。Informed RRT*算法依賴迭代過(guò)程中搜尋得到的最短路徑的距離以壓縮算法后續(xù)的搜索空間,在算法迭代過(guò)程中盡可能找到更短的路徑能加速算法的搜索速度。Informed RRT*SR中的路徑優(yōu)化步驟能有效對(duì)路徑距離進(jìn)行優(yōu)化,從而在相同時(shí)間內(nèi)搜索到更短的無(wú)碰路徑。

1.2 路徑優(yōu)化

算法1中的Line39代表路徑優(yōu)化的核心算法。SRTL(Smart Rope-Tightening-Loosening)算法的具體實(shí)現(xiàn)見算法 2。SRTL算法負(fù)責(zé)將得到的無(wú)碰路徑進(jìn)行進(jìn)一步的優(yōu)化。SRTL包含循環(huán)執(zhí)行n次的兩個(gè)步驟,即路徑收縮SRT和路徑松弛SRL。

表2 SRTL算法

1.3 路徑收縮

路徑收縮SRT(Smart Rope-Tightening)的作用是將無(wú)碰路徑收縮,能有效縮短無(wú)碰路徑的距離,其算法實(shí)現(xiàn)見算法 3。在算法 3中,Line2代表對(duì)路徑的每個(gè)一節(jié)點(diǎn)進(jìn)行遍歷。Line4和Line5在邊線{Xi-1,Xi}和{Xi,Xi+1}中分別取點(diǎn)得到Xleft和Xright。Line6到Line14嘗試直接連接Xleft和Xright得到新的路徑,如果該路徑是無(wú)碰的,則對(duì)路徑中節(jié)點(diǎn)和邊線進(jìn)行修改。

表3 SRT算法

表5 SRL算法

表6 MOVE算法

表7 FIND算法

算法3的具體演示如圖1所示。對(duì)于路徑A-B-C,SRT算法先后構(gòu)建出A-C,A1-C1,A2-C2三條潛在近路,A1和A2分別是A-B的1/2點(diǎn)和1/4點(diǎn)。SRT會(huì)依次對(duì)三條近路執(zhí)行碰撞檢測(cè),如果某一條近路是無(wú)碰的,則直接連接該近路徑。在圖1中是A1-C1,顯然Dist(A1-C1)

圖1 SRT 算法演示

1.4 路徑松弛

通常情況下,SRT的優(yōu)化效果有限,空間中復(fù)雜的障礙會(huì)降低SRL的優(yōu)化性能,導(dǎo)致SRT陷入局部極小值陷阱。SRL算法配合SRT算法組合成SRTL算法能有效地完成路徑優(yōu)化。SRL算法實(shí)現(xiàn)見算法 4。

在算法 4中,Line2代表對(duì)路徑P中的每一個(gè)非端點(diǎn)進(jìn)行遍歷。Line3意味著對(duì)每一個(gè)非端節(jié)點(diǎn)執(zhí)行MOVE操作,對(duì)節(jié)點(diǎn)嘗試進(jìn)行啟發(fā)式位置擾動(dòng)。Line4到Line10會(huì)對(duì)擾動(dòng)后的節(jié)點(diǎn)和路徑執(zhí)行碰撞檢測(cè),如果是無(wú)碰的,則擾動(dòng)成功,對(duì)路徑的節(jié)點(diǎn)和邊線進(jìn)行更新。MOVE函數(shù)是SRL算法的核心,帶來(lái)的擾動(dòng)能幫助路徑擺脫局部極小值陷阱,具體的算法實(shí)現(xiàn)見算法 5。

在算法5中,對(duì)于已經(jīng)得到的路徑P(V,E),?xi∈V{xstart,xgoal},總是存在父節(jié)點(diǎn)xparent和子節(jié)點(diǎn)xchild。路徑將被視作一條n維空間的繩子,vparent=xparent-xi和vchild=xchild-xi表征收縮時(shí)子節(jié)點(diǎn)和父節(jié)點(diǎn)對(duì)xi施加力的方向,其合力為vsum=vparent+vchild。如果Connectfree(xparent,xchild)為假且Connectfree(xparent,xi,xchild)為真,將xi沿著vsum方向移動(dòng),則?δ∈R,使得Connectfree(xparent,xi+(δ+ε)vsum,xchild)為假且Connectfree(xparent,xi+δvsum,xchild)為真。我們定義同樣的,如果對(duì)vsum施加微小的擾動(dòng)得到由于公式2。

(2)

算法5的Line1到Line3用于

得到Vsum,Line4意味著在Vsum方向找到主錨點(diǎn),也就是障礙物空間與非障礙物空間的交界面上的點(diǎn)。Line5和Line6表示對(duì)Vsum向量施加微小的擾動(dòng),從而得到更多的次錨點(diǎn)。依次將次錨點(diǎn)和主錨點(diǎn)連接后不難得到錨點(diǎn)向量空間C。

Line8意味著從Null(C)

空間中挑選擾動(dòng)向量施加在Xi上使得Xi位置產(chǎn)生變化。如果擾動(dòng)后的節(jié)點(diǎn)仍然使得路徑無(wú)碰,則擾動(dòng)成功,進(jìn)而對(duì)路徑的節(jié)點(diǎn)和邊線進(jìn)行更新?；诙炙阉鲗?shí)現(xiàn)的FIND函數(shù)實(shí)現(xiàn)請(qǐng)參考算法 6。

SRL算法的演示步驟如圖2所示。在三維空間中存在的一個(gè)不規(guī)則障礙和一條無(wú)碰路徑A-B-C。對(duì)于節(jié)點(diǎn)C來(lái)說(shuō),節(jié)點(diǎn)A和B分別是C的父節(jié)點(diǎn)與子節(jié)點(diǎn)。C1、C2和C3是C節(jié)點(diǎn)對(duì)應(yīng)是三個(gè)錨點(diǎn)。三個(gè)錨點(diǎn)組成的局部區(qū)域在一定程度上描述了障礙物的局部區(qū)域特征。錨點(diǎn)區(qū)域的法向量f將會(huì)作用在節(jié)點(diǎn)上對(duì)其產(chǎn)生擾動(dòng)。

圖2 SRL算法演示

圖3能演示了擾動(dòng)帶來(lái)的優(yōu)化,在SRT算法很難在圖3-1中繼續(xù)對(duì)路徑A-B-C進(jìn)行優(yōu)化。但是SRL算法對(duì)節(jié)點(diǎn)B施加擾動(dòng)得到節(jié)點(diǎn)B1后,SRT算法能夠進(jìn)一步對(duì)路徑進(jìn)行優(yōu)化得到更優(yōu)的路徑。在SRL和SRT算法的交替作用下,路徑如同一根不斷張弛的繩子,無(wú)碰路徑將被優(yōu)化為一條更短的路徑。SRTL作為一種路徑優(yōu)化算法同樣能與其他路徑規(guī)劃算法組合用于加速迭代優(yōu)化的過(guò)程。

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

為直觀展示SRTL算法的路徑優(yōu)化效果,構(gòu)建了8張三維地圖,如圖4,其中包含了簡(jiǎn)單的單個(gè)障礙地圖(Map1-3)、復(fù)雜的單個(gè)障礙環(huán)境(Map7)、稀疏多障礙環(huán)境(Map4)、稠密多障礙環(huán)境(Map8)和狹窄通道多障礙環(huán)境(Map5-6)等各類環(huán)境。我們測(cè)試了SRTL路徑優(yōu)算法在各類地圖中路徑優(yōu)化的表現(xiàn)。

圖4 Informed RRT*SR算法在三維空間的仿真結(jié)果

圖4中的立方體和球體等物體代表障礙,藍(lán)線代表RRT類算法生成的路徑,紅線代表經(jīng)SRTL路徑優(yōu)化算法優(yōu)化后的路徑。圖4表明由RRT類算法采樣得到的藍(lán)色路徑點(diǎn)通常是蜿蜒曲折的,而優(yōu)化后得到的紅色路徑往往更加便捷。實(shí)驗(yàn)表明,經(jīng)路徑優(yōu)化算法優(yōu)化后得到的路徑將縮短15%左右。

RRT*-Smart同樣是一種優(yōu)秀的具備路徑優(yōu)化能力的路徑規(guī)劃算法,圖5將Informed RRT*SR與RRT*-Smart算法進(jìn)行對(duì)比。圖5的球體是障礙物,藍(lán)線是算法生成的中間結(jié)果路徑,紅線是對(duì)藍(lán)線優(yōu)化后的路徑。實(shí)驗(yàn)表明,RRT*-Smart的智能采樣和路徑優(yōu)化能力在高維空間中是有限的,其優(yōu)化的結(jié)果受到了初始條件的影響,難以有效優(yōu)化路徑。Informed RRT*SR算法在空間中能得到一條更短的路徑,圖4和圖5表明,由基于啟發(fā)式擾動(dòng)和二分采樣的Informed RRT*SR算法得到的優(yōu)化路徑往往更加便捷。

圖5 RRT*-Smart(左)與Informed RRT*SR(右)算法在三維空間的仿真結(jié)果

在Vrep中搭建了機(jī)械臂仿真環(huán)境為以進(jìn)一步驗(yàn)證算法的有效性,利用Informed RRT*SR算法驅(qū)動(dòng)六軸機(jī)械臂UR5中在障礙空間中成功進(jìn)行了路徑規(guī)劃實(shí)驗(yàn),結(jié)果如圖6所示。圖6表明,在相同的運(yùn)行時(shí)間內(nèi),Informed RRT*SR能在六維空間搜尋得到一條更短的路徑。

圖6 Informed RRT*(左)與Informed RRT*SR(右)算法在機(jī)械臂路徑規(guī)劃仿真實(shí)驗(yàn)結(jié)果

在上述機(jī)械臂作業(yè)仿真環(huán)境中進(jìn)行了上百次實(shí)驗(yàn)以對(duì)比測(cè)試算法的路徑搜索能力。具體結(jié)果如圖7與圖8所示。

圖7 Informed RRT*與Informed RRT*SR算法在近距離機(jī)械臂路徑規(guī)劃仿真實(shí)驗(yàn)結(jié)果

圖8 Informed RRT*與Informed RRT*SR算法在遠(yuǎn)距離機(jī)械臂路徑規(guī)劃仿真實(shí)驗(yàn)結(jié)果

圖7為近距離機(jī)械臂路徑規(guī)劃場(chǎng)景的結(jié)果,圖8為遠(yuǎn)距離機(jī)械臂路徑規(guī)劃場(chǎng)景的結(jié)果。圖中的每一個(gè)圓點(diǎn)的橫縱坐標(biāo)值代表每一次路徑規(guī)劃算法實(shí)驗(yàn)中找到的路徑的時(shí)間與距離值。圖中的曲線是基于樣本點(diǎn)經(jīng)LOESS算法擬合得到的結(jié)果,該曲線被認(rèn)為是表征樣本散點(diǎn)的一條局部平均值曲線。Informed RRT*SR算法的實(shí)驗(yàn)樣本點(diǎn)與圖7和圖8的實(shí)驗(yàn)結(jié)果表明不論在近距離路徑規(guī)劃場(chǎng)景還是遠(yuǎn)距離路徑規(guī)劃場(chǎng)景,Informed RRT*SR算法都能在相同時(shí)間內(nèi)找到一條更短的路徑。Informed RRT*SR算法在近距離路徑規(guī)劃場(chǎng)景中對(duì)路徑距離的優(yōu)化率約為7%,而在遠(yuǎn)距離路徑規(guī)劃場(chǎng)景中對(duì)路徑距離的優(yōu)化率約為16%。

圖9是依據(jù)實(shí)驗(yàn)結(jié)果繪制的算法時(shí)間與路徑距離的直方圖,圖9-A是Informed RRT*算法的算法時(shí)間直方圖,圖9-B是Informed RRT*算法搜尋的路徑距離直方圖,圖9-C是Informed RRT*SR算法的算法時(shí)間直方圖,圖9-B是Informed RRT*SR算法搜尋的路徑距離直方圖。圖9-A和9-C表明,兩類算法的運(yùn)行時(shí)間分布大致相當(dāng),均呈現(xiàn)出“頭部集中,尾部稀疏”的特點(diǎn)。圖9-B和9-D表明,Informed RRT*SR算法搜尋的路徑距離集中在頭部附近,且整體分布優(yōu)于Informed RRT*。

圖9 Informed RRT*與Informed RRT*SR算法在機(jī)械臂路徑規(guī)劃仿真實(shí)驗(yàn)結(jié)果的算法時(shí)間與路徑距離的直方圖

3 總 結(jié)

在本文中,在Informed RRT*基礎(chǔ)上提出了一種基于采樣的路徑規(guī)劃算法Informed RRT*SR,通過(guò)指定啟發(fā)式的探索和優(yōu)化規(guī)則,Informed RRT*SR算法能更加高效地搜索得到一條便捷的無(wú)碰路徑。本文提出的算法結(jié)合了基于采樣算法的優(yōu)點(diǎn),能很好適應(yīng)高維度空間的路徑搜索問題。Informed RRT*SR算法中基于現(xiàn)有路徑的優(yōu)化方法能對(duì)狀態(tài)空間進(jìn)行更有針對(duì)性的探索。同時(shí),Informed RRT*SR算法步驟中SRTL優(yōu)化算法的模塊化設(shè)計(jì)有助于工程師修改以適配不同的工業(yè)應(yīng)用場(chǎng)景,這同樣允許SRTL算法與其他基于采樣的算法相互組合以更高效地解決路徑規(guī)劃問題。