復雜多場景下機械臂避障運動規劃方法研究

宋勇, 張蕾, 田榮, 王曉華

1.西安工程大學電子信息學院， 陜西 西安 710048；2.陜西人工智能聯合實驗室（西安工程大學分部）， 陜西 西安 710048

工業機械臂因自身定位精度高、穩定性強和執行效率高等優異性能,廣泛應用在裝配、焊接、分揀等工業生產領域中,是推動智能化工業生產的重要工具[1]。考慮到其工作環境復雜多樣,研究多場景情況下的避障運動規劃具有重要的工程價值。

機械臂在傳統運行操作上依賴于工人的示教編程,避障規劃效率低且適應性差。要解決這一問題,需要機械臂能夠自主規劃出一條無碰撞路徑以適應不同的環境。機械臂運動規劃是根據起始點到目標點的信息,搜索出一組滿足碰撞檢測條件的關節角度信息,驅動機械臂從初始點運動到目標點,順利完成作業任務。采用高性能路徑規劃算法,是實現機械臂高效、安全、柔順作業的關鍵。

目前,路徑規劃算法的研究主要分為2類,按照是否在構型空間中顯示障礙物區域,分為組合運動規劃算法和基于采樣的運動規劃算法。組合運動規劃算法包括人工勢場法[2]、蟻群算法[3]、遺傳算法[4]、A*算法[5]、Dijkstra算法[6]等。其中人工勢場法在全局路徑規劃上非常有效,但是存在局部極小值;蟻群與遺傳算法運算量太大且耗時長;A*與Dijkstra算法需要較大空間存儲環境信息,在低維空間中的路徑規劃這些算法有一定優勢,但對于在高維空間中執行作業任務的多自由度機械臂則不太適用[7]。

基于采樣的路徑規劃方法通過碰撞檢測模塊,不需要顯式構建整個配置空間和邊界,主要算法包含概率路圖法(PRM)和快速擴展隨機樹算法(rapidly-exploring random tree,RRT)[8],適用于高維空間和復雜約束下的運動規劃問題。其中最典型的是基于采樣的快速擴展隨機樹,此算法具有搜索速度快、探索能力強、概率完備性等優點[9-10],且不需要對障礙環境進行建模,能有效解決復雜的高維空間規劃問題,但也存在規劃路徑長度不是最優、隨機性大等缺點。Karaman和Frazzoli于2011年提出漸進最優性(最短距離)的RRT*算法[11],該算法在RRT的基礎上增加了父節點擇優過程和鄰節點回溯重構過程,使規劃的路徑具有漸進最優性,弊端是收斂速度慢,收斂時間逐步增長,產生的冗余節點過多[12],以及在避障過程中所規劃出的路徑表現為鋸齒狀,并不平滑,從而影響機械臂的平穩運行[13]。文獻[14]中生成的新節點以一定的概率向目標方向擴展,提升節點擴展的導向性,同時對所規劃的路徑進行分段優化,優化了路徑曲率。文獻[15]提出改進的雙樹拓展策略RRT-connect算法,通過三角不等式的重接線方法,提高了搜索效率。文獻[16]將雙向人工勢場與RRT*結合提出P-RRT*算法,縮短路徑長度優化搜索效率,但是勢場構建可能會導致局部最優。針對陷入局部最優問題,文獻[17]引入回歸機制策略與RRT算法結合,可以逃逸極小值點。上述研究極大推動了機械臂自主路徑規劃的進程,然而在未來可預見的復雜多場景下適應性受限。

綜上所述,為了解決RRT*算法在不同復雜的二維和三維等多場景下,規劃效率不高、冗余節點多、路徑曲折程度大,導致機械臂運行不平穩的問題,本文提出一種啟發式概率融合人工勢場的改進型RRT*算法(PAPF-RRT*),并在剔除冗余節點后進行B樣條插值平滑優化,規劃出一條滿足運動學約束的平滑路徑,使機械臂可以平穩運行。

1 機械臂建模與碰撞檢測

1.1 機械臂運動學原理及其模型構建

本文以工業六自由度機械臂puma560為研究對象,其URDF模型如圖1所示,其D-H參數如表1所示。

表1 D-H參數表

圖1 puma560機械臂模型

表1中各個參數的含義為當前關節標號為i,逐次遞推其余關節標號為i-1,di表示2個相鄰連桿之間的距離,ai-1代表連桿長度,αi-1代表連桿i的扭角,θi代表關節轉角的范圍,其中d2=148.09 mm,d4=433.07 mm,a2=431.8 mm,a3=20.32 mm。

連桿i相對于連桿i-1的坐標變換通式為

(1)

式中:根據D-H參數及公式(1)可以得出機械臂末端位姿變換矩陣如下所示。

(2)

式中:向量n,o,a代表機械臂末端姿態;x,y,z代表機械臂末端坐標位置;p代表三維坐標。

1.2 機械臂連桿與障礙物空間位置關系的描述

機械臂路徑規劃的前提是要具備精準有效的碰撞檢測。但是在實際運行的環境中,機械臂本體和障礙物都是不規則的物體,兩者之間的碰撞檢測條件很難設定,因此需要對機械臂和障礙物進行模型簡化[18-19]。本文用幾何包絡法簡化機械臂與障礙之間的碰撞模型,用球型包圍盒對障礙物進行包絡,為了精確逼近空間障礙物的占位,包絡球的直徑定義為障礙物徑向最長的線段距離,其表達式為

(3)

式中:(a,b,c)為包絡球的球心坐標;r為包絡球的半徑;(x,y,z)為障礙物的任意一點坐標。

將機械臂連桿用圓柱體完全包裹,如圖2所示,其徑向最大包絡半徑為R1;將障礙物用球體進行完全包裹, 其徑向最大包絡半徑為R2;進而將機械臂與障礙物的碰撞檢測轉化成判斷規則幾何體是否相交問題。經過包絡化處理,機械臂各連桿與障礙物可以簡化為線段與球體的空間位置關系,如圖3所示。根據末端位姿的信息,通過逆運動學求出各個關節的最優逆解,再進行正解求出空間位置坐標,即連桿AB的狀態方程。設點Q到線段AB的直線距離為dmin,設R=R1+R2,根據線段AB與球體的空間位置關系歸納如下幾種情況:

圖2 模型簡化示意圖

圖3 碰撞檢測判斷示意圖

1) 如圖3a)所示,dmin>R,則不發生碰撞。

2) 如圖3b)所示,dmin

3) 如圖3c)所示,dminR并且|QB|>R,點A、B在點Q的兩側,有2個交點,則發生碰撞。

4) 如圖3d)所示,dminR并且|QB|>R,點A、B在點Q的同側,無交點,則不發生碰撞。

2 基本算法原理簡介

2.1 RRT及RRT*算法原理簡介

快速擴展隨機樹算法(RRT)是一種基于隨機采樣的單步查詢搜索算法,對環境障礙物無需建模,因此很適合高維空間路徑規劃。

RRT*算法在RRT節點擴展的基礎上,增加了路徑代價函數,依靠父節點優選過程和鄰節點回溯重構,重新計算新節點,具備漸進最優性,且收斂到當前最優,具體過程如圖4所示。

圖4 父節點優選過程

父節點擇優選擇過程,如圖4所示,其目的是通過鄰域r內某個備選節去替換新節點,從而降低新節點的路徑代價,并更新其權重。

鄰節點回溯重構過程,如圖5所示,在沒有發生碰撞的情況下,若存在其他節點以Xnew做為父節點并索引至根節點的代價減小,則用Xnew替換該節點作為新的父節點。

圖5 鄰節點回溯重構過程

2.2 改進型RRT*算法(PAPF-RRT*)

機械臂路徑規劃問題是滿足自身運動學約束和工作空間的條件下,規劃從指定起點到目標點的路徑,該路徑沒有發生碰撞,使機械臂能高效完成任務。該問題特征總結為3類:

1) 在復雜受限等特殊環境下,RRT*算法規劃效率低,產生過多冗余節點;

2) 盡可能優化規劃時間和路徑長度;

3) 規劃出來的路徑必須滿足機械臂運動學約束,機械臂才能夠平穩運行。

2.2.1 混合約束采樣策略

針對RRT*算法在復雜受限環境下搜索效率低的問題以及避免無效區域的搜索,借鑒啟發式思想,提出目標偏向采樣策略來優化采樣過程。預先設置一個目標偏向閾值Pbias(設置為0.3),然后由均勻概率分布隨機產生一個概率值P,將隨機樹擴展分為2個階段,分別為探索階段和收斂階段。核心思想是在隨機樹生長過程中,如果P≥Pbias,隨機樹處于探索階段如圖6a)所示,通過RandomState函數隨機生成采樣點Xsample;隨著迭代次數的增加,如果P

圖6 概率指數示意圖

(4)

如果隨機值小于預設的目標偏向閾值,則通過位置約束來優化采樣點。在采樣過程中建立采樣區,每次采樣時可將多組采樣點放入采樣區中,定義采樣區中距離目標點最近且生成的節點不與障礙物發生碰撞的點為最優點。建立采樣區過程示意圖如圖7所示。判斷條件為:計算當前采樣點與目標點的距離是否小于上一個采樣點與目標點的距離。若是,則符合判斷條件,采樣成功;否則,繼續循環采樣直到滿足條件。

圖7 采樣點優選示意圖

圖7設置第一個參考采樣點為Xsample1,圖中按照采樣次序以及采樣點到目標點的歐氏距離進行大小排序Xsample1>Xsample2>Xsample3

2.2.2 隨機樹生長優化策略

采樣優化函數雖然能提高采樣的導向性和收斂速度,但未改變新節點生長過程的盲目性,在環境復雜時可能陷入局部極小點和重復偏置問題,降低算法尋跡效率。本文融合人工勢場法,將目標引力函數G(n)和障礙物斥力函數R(n)疊加到隨機樹節點Xnear上,則隨機樹節點生長受采樣點函數P(n)的約束,同時也受到目標引力和障礙物斥力合力的約束。APF對隨機樹節點擴展的影響如圖8所示,生長函數由(5)式表示。

圖8 APF對節點生長的影響

F(n)=P(n)+G(n)+R(n)

(5)

人工勢場法是局部路徑規劃方法,基本思想在目標點設置作用于全局的引力場,對擴展樹節點產生引力作用,障礙物設置作用于局部范圍的斥力場,對擴展樹節點產生斥力作用,引力與斥力的合力方向決定隨機樹生長方向,從而降低隨機樹生長的盲目性,加快算法收斂速度。(6)～(7)式為引力場和斥力場勢函數

(6)

(7)

Uatt代表引力函數,Ka代表引力常數,Xnear表示隨機樹擴展的當前節點,Xgoal為目標點,Urep表示斥力函數,Kr為斥力系數,l表示采樣點與障礙物的距離,l0為斥力場作用的范圍,‖Xgoal-Xnear‖=ρ。

目標點對隨機樹節點Xnear的引力為其對應引力場的負梯度

FA=-Uatt=-Ka‖Xgoal-Xnear‖Δρ

(8)

由(8)式構造出節點Xnear處的目標引力函數G(n),其中FA表示引力方向上的擴展系數。

(9)

同理,環境中障礙物對隨機樹節點Xnear產生的斥力為對應斥力場的負梯度

(10)

由(10)式構造出節點Xnear處的障礙物斥力函數R(n),其中P0表示環境中障礙物位置信息,FR表示斥力方向上的擴展系數。

(11)

通過引入人工勢場法將斥力與引力疊加到隨機樹擴展方向上,對節點擴展方向進行修正,引導隨機樹生長的方向和擴展步長大小,促使每一次擴展過程更接近目標點,加快收斂,增加了算法搜索的靈活性。因此,新節點被重新配置如公式(12)所示。

(12)

式中:δ為擴展固定步長;Xnear為距離采樣點最近鄰節點;‖Xrand-Xnear‖代表Xrand與Xnear之間的歐氏距離。

2.2.3 自適應步長策略

基于隨機樹生長優化策略的算法沒有有效利用擴展的節點信息,忽略了碰撞概率較小的方向,如果算法繼續朝這個方向擴展,成功擴展的可能性會更大。此外,當隨機樹向目標方向生長時,如果擴展方向上存在障礙物,與隨機擴展相比,該策略將顯著降低算法的避障能力。因此,本文引入了一種自適應步長策略,新節點的生成方法配置為

(13)

當機械臂的操作環境比較復雜或者有較大障礙物時,融合了人工勢場法的RRT*算法,若始終是固定步長進行搜索,則不能有效利用擴展過程中收集到的信息提高尋跡效率。因此,本文采用自適應步長策略,可以有效利用擴展過程中所收集到的障礙物和環境信息,更好適應節點的生長狀況,使得算法更快地穿越障礙物較少的區域。在隨機樹進行拓展時首先采用初始步長,在每一次擴展生成新節點Xnew時,進行碰撞檢測,如檢測到附近沒有障礙物,則調整步長。具體過程:

1) 預設一個φ值,取值范圍在[0,1],初值設為零,表示隨機樹向通過采樣獲得的節點方向擴展。

2) 當機械臂操作環境比較簡單,增大φ值以加快偏向目標生長。但是φ值不宜設置過大,以避免陷入局部最優,根據實際的任務場景,合理設置[20]。

3) 當機械臂操作環境復雜或需要跨過較大障礙物時,降低φ值,從而使擴展步長減小并保持擴展的隨機性,使得隨機樹繞開障礙物生長。

2.2.4 路徑優化策略

RRT*算法及其衍生算法在三維空間中搜索得到一條由離散點所組成的連續線段,其路徑的形狀比較曲折,平滑性極差,產生大量冗余節點,不利于機械臂平穩運行。假設RRT*算法產生的路徑節點向量q[q1,q2,q3…qn],對節點qa和qb之間路徑進行碰撞檢測(a

圖9 優化冗余節點原理圖

刪除冗余節點之后,得到的路徑長度較短,路徑曲率小,且轉折點較少。但在轉折點處還需要進行平滑處理[21],以提高機械臂運行時的柔順性。三次B樣條曲線具有局部可控的特點,能較好滿足路徑規劃中對拐點的局部擬合,且不影響整條路徑的基本形狀,增加了軌跡平滑性,保證了各關節速度、加速度變化連續性,且不發生突變,提高軌跡柔順性。因此,選用B樣條函數構造軌跡,其K階B樣條曲線公式為

(14)

式中:di表示給定曲線的控制點;Ni,k(u)表示K階B樣條基函數。三次B樣條基函數為

(15)

式中,u表示均勻節點向量,當給定控制頂點后,可以求出一些列滿足B樣條插值的空間序列點,從而構造出插值曲線。

3 算法仿真分析

為了驗證算法PAPF-RRT*的有效性和可靠性,與RRT*、GB-RRT*算法分別在二維和三維場景下進行仿真與實驗分析。

3.1 二維仿真

仿真環境設置為[100,100],起點設為[30,80],目標點為[70,10],隨機設置障礙物,對RRT*、GB-RRT*和PAPF-RRT*算法進行100余次仿真實驗,迭代次數設置為100,步長設置為5,3種算法的仿真結果和各項指標如圖10和表2所示。

表2 二維場景中各算法性能對比

圖10 二維環境中各算法性能對比

紅色線條表示算法尋跡的最終路徑,黑色線條表示拓展樹分支。通過對比可以看出PAPF-RRT*算法相對于RRT*、GB-RRT*算法在復雜狹窄環境中生成的節點數較少。通過圖12和表2數據對比可以看出PAPF-RRT*算法相比RRT*算法,擴展無效結點數變少,其節點使用率由11.89%提升到35.62%,因此占用的內存更少;在搜索時間層面上由2.874 s降低到1.243 s,降低了約56.75%,提高了路徑規劃效率;平均路徑長度上從124.63 cm縮短到102.51 cm,減少了約17.74%,且路徑轉折程度較低,有利于路徑平滑優化;相比GB-RRT*算法,PAPF-RRT*算法在路徑代價上減少了16.41%,節點使用率提高了24.50%,搜索時間上減少了13.5%。相比以上2種算法,本文所提PAPF-RRT*算法在復雜多障礙環境中具有良好的路徑規劃能力。

3.2 三維仿真

為了驗證本文所提算法PAPF-RRT*在三維環境中的有效性,在MATLAB環境下搭建三維避障環境,仿真實驗中設置算法搜索步長為5,迭代次數設置為100,設置搜索起點為[20,55,10],目標點為[40,15,40],仿真環境地圖設置為[60×60×60]的三維空間,考慮到算法的隨機性,重復進行實驗100余次,實驗結果和各項指標如圖11和表3所示。

表3 三維場景中各算法性能對比

圖11 三維環境中各算法仿真結果

藍色線條表示隨機樹的分支,紅色線條表示最終尋跡的路徑,如圖11所示,本文所提算法PAPF-RRT*在三維空間的采樣點比傳統RRT*減少了很多,且規劃的路徑長度短、曲折度低。結合表3中的數據可得,本文所提算法相比基本RRT*算法平均路徑長度由42.73 cm降低到32.93 cm,減少了約22.93%;平均運行時間由6.743 s降低到1.548 s,降低了約77.04%;節點使用率由9.54%提升到63.21%,從而說明規劃效率的提升,且路徑中折點數較少,曲線轉折程度較低,有利于路徑平滑優化。雖然GB-RRT*算法尋跡的路徑代價相對較短,因為目標偏向方法在三維空間中起到良好作用,但是該算法易陷入局部極小。然而本文所提PAPF-RRT*算法,在搜索路徑的同時提高了節點使用率,降低路徑規劃長度,且規劃效率較高。

3.3 路徑平滑性分析

為了進一步驗證本文所提算法PAPF-RRT*規劃出的路徑是否滿足機械臂的運動學約束,將機械臂模型導入,進行運動規劃仿真實驗。因為PAPF-RRT*算法所規劃出來的路徑是由空間一系列離散的采樣點組成,路徑平滑性差,所以,本文采用基于三次B樣條對所規劃出來的路徑映射到關節空間進行插值優化,平滑優化效果如圖12所示。

圖12 機械臂末端軌跡優化效果

圖12表示在局部空間內,機械臂末端路徑規劃圖,其中黑色實線代表未進行插值的路徑,紅色實線代表經過三次B樣條插值平滑處理后末端的路徑。由仿真結果發現,插值擬合處理后末端軌跡光滑性明顯增強,各關節運動學曲線如圖13所示。

圖13 各關節擬合后角度、速度及加速度曲線

圖13為機械臂避障運行過程中各關節位移、速度、加速度變化曲線,其中各關節的起始速度、加速度和終止速度、加速度數值大小都為零,符合實際工作情況。在平滑處理機械臂末端軌跡的同時,機械臂各關節角度、速度和角加速度3項運動學參數曲線變化連續、穩定,無跳變,降低了各關節在運行過程中的沖擊和抖動,保證了機臂在規劃路徑上的高效、平穩運行,并且各連桿與障礙之間擁有安全裕度,能夠滿足機械臂的實際應用需求。

為了進一步驗證本文所提算法對不同環境的適應性,設置障礙物個數不同的3種環境。仿真結果如圖14所示,紅色線條表示本文所提算法插值擬合后規劃出來的路徑,黑色線條表示未進行路徑優化的軌跡,可以看出改進后算法規劃出來的路徑明顯光滑,路徑中的震蕩現象也有效得到改善;并且隨著障礙物個數的增加,本文所提PAPF-RRT*依然能夠引導機械臂順利避障,證明了算法的有效性。

圖14 算法對環境復雜程度的適應性實驗

4 結 論

本文以六自由度工業機械臂在智能工廠操作業務為背景,在不同復雜程度的的多場景中,針對傳統RRT*算法進行路徑規劃時存在隨機性強、冗余節點過多、路徑不平滑等問題進行改進,提出一種基于PAPF-RRT*結合B樣條插值的機械臂運動規劃算法。本文主要改進有如下三點:

1) 在隨機樹擴展過程中引入混合約束采樣策略,在采樣點偏向擴展的基礎上,通過采樣區優選策略,提高采樣點的導向性與質量。

2) 將節點擴展方向與人工勢場合力方向進行融合,使得擴展方向具備目標點吸引和障礙物排斥的趨勢,同時引入自適應步長策略,減少擴展的盲目性,加快收斂速度。

3) 通過去除冗余節點并進行二次插值優化,改善路徑平滑性,以滿足機械臂運動學約束。

仿真結果表明PAPF-RRT*算法在有效避障的同時,提高了規劃效率以及節點使用率,使規劃的路徑長度更優。同時在規劃后的路徑點間進行B樣條插值構造軌跡,提升路徑平滑性。導入機械臂模型進行仿真,關節空間的各項運動學參數變化連續、穩定,同時也優化了機械臂末端執行器的軌跡,降低了規劃時間,使規劃的軌跡更有利于機器人的跟蹤執行,具有較好的發展應用前景。