具備啟發式路徑優化的機械臂路徑規劃算法

鄧鈃中 張偉軍

(上海交通大學機械與動力工程學院,上海 200240)

0 路徑規劃算法發展現狀

如今,機械臂和移動機器人在許多領域扮演著重要角色。移動機器人被廣泛應用于未知環境的探測[1］、導航、定位[2］和制圖等[3］。多關節機械臂在制造業領域發揮著重要的作用,能夠完成搬運,組裝和加工等任務。它們不僅能提高生產效率和產品質量,還能減少人力成本和操作風險。路徑規劃是一項必要而關鍵的技術,能幫助機器人在這些任務中更加高效智能地完成任務。

為解決路徑規劃問題,需要在給定狀態空間中找到一條連接起始點和目標點的路徑并保證路徑不會與任何障礙物發生碰撞。從1970年開始,學術界涌現了各類路徑規劃算法包括基于搜索的算法、基于采樣的算法、基于人工勢場的算法。各類算法在不同場景各有優勢[4-9］,算法的耗時和路徑的距離通常是評價該算法最有效的指標?；谒阉鞯乃惴ㄈ鏏*[10］和Dijkstra[11］算法需要把問題的狀態空間劃分成網格,所以基于搜索的算法的效率和可行性依賴于網格的劃分,并且很難適應不同問題中狀態空間的維度變化。

基于人工勢場的算法APF的路徑規劃方法通過引力和斥力勢場來實現機器人的導航和避障,根據環境感知實時計算勢場,適用于動態環境中的移動物體?；谌斯輬鏊惴ㄔ跇嫿ǖ貓D勢能場時需要獲知全局地圖,但這在機械臂運動的高維狀態空間中是相當困難的?；谌斯輬鏊惴ㄈ菀紫萑刖植孔钚≈岛拖葳?難以完成復雜環境中的規劃任務。

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

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

1 算法描述

1.1 算法符號說明

路徑規劃問題的目標是在狀態空間中找到一條連通起始點和目標點的無碰路徑,這條連續的路徑被表示為P=(V,E),此處V表示路徑中的節點,E?V×V代表連接兩個節點的一條邊。每個節點V均屬于n維路徑規劃問題的狀態空間,表示為X?n。在機器人路徑規劃問題中,n則代表機器人構型空間的維度數目。在路徑規劃問題的狀態空間中存在的障礙表示為Xobs?X,無障礙空間則表示為Xfree=XXobs。

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

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

(1)

節點無碰連接檢測:Connectfree(xi,xj,xk)表示對從xi、xj到xk,依次直接相連接的路徑進行碰撞檢測,如果全程沒有碰撞則返回真,否則返回假。

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

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

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

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

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

表1 Informed RRT*SR算法

Informed RRT*SR算法在Informed RRT*的基礎上引入了路徑優化步驟。算法1中的Line1-Line36為Informed RRT*算法。算法1中的Line38-42表明在Informed RRT*算法搜尋得到無碰路徑后對該路徑進行進一步優化以縮短路徑距離。Informed RRT*算法依賴迭代過程中搜尋得到的最短路徑的距離以壓縮算法后續的搜索空間,在算法迭代過程中盡可能找到更短的路徑能加速算法的搜索速度。Informed RRT*SR中的路徑優化步驟能有效對路徑距離進行優化,從而在相同時間內搜索到更短的無碰路徑。

1.2 路徑優化

算法1中的Line39代表路徑優化的核心算法。SRTL(Smart Rope-Tightening-Loosening)算法的具體實現見算法 2。SRTL算法負責將得到的無碰路徑進行進一步的優化。SRTL包含循環執行n次的兩個步驟,即路徑收縮SRT和路徑松弛SRL。

表2 SRTL算法

1.3 路徑收縮

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

表3 SRT算法

表5 SRL算法

表6 MOVE算法

表7 FIND算法

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

圖1 SRT 算法演示

1.4 路徑松弛

通常情況下,SRT的優化效果有限,空間中復雜的障礙會降低SRL的優化性能,導致SRT陷入局部極小值陷阱。SRL算法配合SRT算法組合成SRTL算法能有效地完成路徑優化。SRL算法實現見算法 4。

在算法 4中,Line2代表對路徑P中的每一個非端點進行遍歷。Line3意味著對每一個非端節點執行MOVE操作,對節點嘗試進行啟發式位置擾動。Line4到Line10會對擾動后的節點和路徑執行碰撞檢測,如果是無碰的,則擾動成功,對路徑的節點和邊線進行更新。MOVE函數是SRL算法的核心,帶來的擾動能幫助路徑擺脫局部極小值陷阱,具體的算法實現見算法 5。

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

(2)

算法5的Line1到Line3用于

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

Line8意味著從Null(C)

空間中挑選擾動向量施加在Xi上使得Xi位置產生變化。如果擾動后的節點仍然使得路徑無碰,則擾動成功,進而對路徑的節點和邊線進行更新?；诙炙阉鲗崿F的FIND函數實現請參考算法 6。

SRL算法的演示步驟如圖2所示。在三維空間中存在的一個不規則障礙和一條無碰路徑A-B-C。對于節點C來說,節點A和B分別是C的父節點與子節點。C1、C2和C3是C節點對應是三個錨點。三個錨點組成的局部區域在一定程度上描述了障礙物的局部區域特征。錨點區域的法向量f將會作用在節點上對其產生擾動。

圖2 SRL算法演示

圖3能演示了擾動帶來的優化,在SRT算法很難在圖3-1中繼續對路徑A-B-C進行優化。但是SRL算法對節點B施加擾動得到節點B1后,SRT算法能夠進一步對路徑進行優化得到更優的路徑。在SRL和SRT算法的交替作用下,路徑如同一根不斷張弛的繩子,無碰路徑將被優化為一條更短的路徑。SRTL作為一種路徑優化算法同樣能與其他路徑規劃算法組合用于加速迭代優化的過程。

2 仿真實驗分析

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

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

圖4中的立方體和球體等物體代表障礙,藍線代表RRT類算法生成的路徑,紅線代表經SRTL路徑優化算法優化后的路徑。圖4表明由RRT類算法采樣得到的藍色路徑點通常是蜿蜒曲折的,而優化后得到的紅色路徑往往更加便捷。實驗表明,經路徑優化算法優化后得到的路徑將縮短15%左右。

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

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

在Vrep中搭建了機械臂仿真環境為以進一步驗證算法的有效性,利用Informed RRT*SR算法驅動六軸機械臂UR5中在障礙空間中成功進行了路徑規劃實驗,結果如圖6所示。圖6表明,在相同的運行時間內,Informed RRT*SR能在六維空間搜尋得到一條更短的路徑。

圖6 Informed RRT*(左)與Informed RRT*SR(右)算法在機械臂路徑規劃仿真實驗結果

在上述機械臂作業仿真環境中進行了上百次實驗以對比測試算法的路徑搜索能力。具體結果如圖7與圖8所示。

圖7 Informed RRT*與Informed RRT*SR算法在近距離機械臂路徑規劃仿真實驗結果

圖8 Informed RRT*與Informed RRT*SR算法在遠距離機械臂路徑規劃仿真實驗結果

圖7為近距離機械臂路徑規劃場景的結果,圖8為遠距離機械臂路徑規劃場景的結果。圖中的每一個圓點的橫縱坐標值代表每一次路徑規劃算法實驗中找到的路徑的時間與距離值。圖中的曲線是基于樣本點經LOESS算法擬合得到的結果,該曲線被認為是表征樣本散點的一條局部平均值曲線。Informed RRT*SR算法的實驗樣本點與圖7和圖8的實驗結果表明不論在近距離路徑規劃場景還是遠距離路徑規劃場景,Informed RRT*SR算法都能在相同時間內找到一條更短的路徑。Informed RRT*SR算法在近距離路徑規劃場景中對路徑距離的優化率約為7%,而在遠距離路徑規劃場景中對路徑距離的優化率約為16%。

圖9是依據實驗結果繪制的算法時間與路徑距離的直方圖,圖9-A是Informed RRT*算法的算法時間直方圖,圖9-B是Informed RRT*算法搜尋的路徑距離直方圖,圖9-C是Informed RRT*SR算法的算法時間直方圖,圖9-B是Informed RRT*SR算法搜尋的路徑距離直方圖。圖9-A和9-C表明,兩類算法的運行時間分布大致相當,均呈現出“頭部集中,尾部稀疏”的特點。圖9-B和9-D表明,Informed RRT*SR算法搜尋的路徑距離集中在頭部附近,且整體分布優于Informed RRT*。

圖9 Informed RRT*與Informed RRT*SR算法在機械臂路徑規劃仿真實驗結果的算法時間與路徑距離的直方圖

3 總 結

在本文中,在Informed RRT*基礎上提出了一種基于采樣的路徑規劃算法Informed RRT*SR,通過指定啟發式的探索和優化規則,Informed RRT*SR算法能更加高效地搜索得到一條便捷的無碰路徑。本文提出的算法結合了基于采樣算法的優點,能很好適應高維度空間的路徑搜索問題。Informed RRT*SR算法中基于現有路徑的優化方法能對狀態空間進行更有針對性的探索。同時,Informed RRT*SR算法步驟中SRTL優化算法的模塊化設計有助于工程師修改以適配不同的工業應用場景,這同樣允許SRTL算法與其他基于采樣的算法相互組合以更高效地解決路徑規劃問題。