尹慶文

(昆明理工大學機電工程學院,昆明 650500)

0 引言

隨著智能制造技術的不斷發(fā)展,機械臂廣泛應用于工業(yè)發(fā)展,太空探索,醫(yī)療輔助等行業(yè),減輕了人為因素對工作的影響,提高了工作效率,給制造業(yè)帶來了許多便利[1-3]。然而,當前機械臂的自動化和智能化水平依舊發(fā)展緩慢,在多數環(huán)境狹小作業(yè)場合,需要依賴人為的示教,工作人員難以直接參與該環(huán)境,導致了機械臂發(fā)展受到局限性。因此,機械臂能自主的、高效的規(guī)劃出一條可行路徑成為當前的研究熱點。

機械臂路徑規(guī)劃算法主要有A*[4]算法,蟻群算法[5],人工勢場法[6],快速隨機樹算法(rapidly-exploring random trees,RRT)[7]等,前兩種算法直觀易修改,但是存在不穩(wěn)定性,取決于具體問題和設計者的經驗;人工勢場法是一種反饋控制策略,對控制和傳感誤差具有一定魯棒性,但是很容易陷入局部最小值;RRT適用于高維度空間,它們以概率完備性來代替完備性,從而提高搜索效率,但是存在盲目性高,導向性差等特點。本文綜合考慮研究熱點和機械臂所處的高緯度空間,選取RRT算法作為基礎算法進行研究。許多學者對該算法提出了改進思路,張振等[8]提出了一種基于采樣的RRT算法,改進稀疏點產生機制,減少重復性采樣,提出動態(tài)采樣策略,提高了擴展方向的確定性,但是保留了RRT算法的部分缺陷,規(guī)劃的路徑不為最優(yōu);魏武等[9]提出了一種雙樹Quick-RRT*算法,起點和終點分別生成一棵隨機樹,起點樹和終點樹輪流生長,提高了路徑收斂速度,但是該學者只運用于二維環(huán)境移動機器人,對機械臂所屬于的高緯度空間未進行分析;陳法法等[10]設計了一種多次根節(jié)點快速擴展隨機樹算法,采用多個有限擴展次數的次根節(jié)點生成局部連通路徑,減少了采樣點落在障礙物的情況次數,該算法解決了采樣點生成的情況,未解決RRT本身盲目性的問題;張婷婷等[11]提出一種目標導向和冗余刪除策略,使路徑更加平滑,但是在復雜環(huán)境中,容易陷入局部最小值,無法逃離障礙物環(huán)境。

本文基于RRT算法本身存在的盲目性差,導向性不明顯提出了一種基于人工勢場引導的概率偏置雙向最優(yōu)RRT算法(GB-APFB-RRT*)。首先,把單樹搜索改為雙向搜索,在采樣時,利用目標偏置閾值控制采樣點方向,再重選父節(jié)點重布線,保證每次迭代點為最優(yōu);其次,加入人工勢場思想引導新節(jié)點成長,控制新節(jié)點方向接近終點并且避開障礙物;最后,刪除冗余點用三次B樣條曲線對路徑進行擬合,使得機械臂在關節(jié)空間下運動平穩(wěn)。

1 機械臂運動學分析及碰撞檢測技術

1.1 機械臂運動學分析及模型建立

本文采用埃夫特ERF-900六軸六自由度機械臂為研究對象如圖1a所示,在SolidWorks中裝配關系如圖1b所示,其改進型D-H模型坐標系如圖1c所示,D-H參數表如表1所示。

(a) 六自由度機械臂 (b) SolidWorks中裝配關系

(c) 改進型D-H模型坐標系圖1 機械臂圖

表1 改進型D-H參數表

表中:i為第i個關節(jié),αi-1為連桿扭轉角,ai-1為連桿長度,di為連桿偏距,θi為關節(jié)角。

根據改進型D-H參數表,可得機械臂相鄰兩關節(jié)齊次變換矩陣為:

(1)

式中:cθi表示cos(θi),sθi表示sin(θi),cαi-1表示cos(αi-1),sαi-1表示sin(αi-1)。

(2)

式中:n、o、α分別表示機械臂末端坐標系X、Y、Z相對于基坐標姿態(tài)的單位矢量,p表示末端坐標系相對于基座的位置矢量。

1.2 碰撞檢測分析

在工業(yè)環(huán)境中,障礙物模型具有不規(guī)則,多排列,密集分布等特點。因此要根據機械臂機身尺寸與障礙物物理信息建立數學模型。針對障礙物物理信息建立數學模型方法中,主要有包絡球[12],包絡盒[13]等。兩種建立數學模型方法核心思想類似,用大體積的模型集合來代替本體模型。針對本文機械臂結構形態(tài)和障礙物為空間緊密分布的方體,為確保碰撞檢測技術的準確性,采用包圍盒包絡法來描述障礙物簡化數學模型。

在此基礎上,為了提高碰撞檢測技術的穩(wěn)定性,使用圓柱體包絡盒來簡化機械臂連桿模型和包圍球簡化機械臂關節(jié),包圍球和圓柱體半徑均為ri,并且以包圍球半徑ri對障礙物模型進行膨脹處理,數學模型如圖2所示。定義機械臂包絡盒數學模型為R{L1,L2,L3,L4,L5,L6},膨脹后的障礙物數學模型6個面為C{O1,O2,O3,O4,O5,O6},只要滿足兩個數學模型沒有交集則機械臂障礙物不會發(fā)生碰撞,如式(3)所示。

圖2 機械臂碰撞檢測模型

C{O1,O2,O3,O4,O5,O6}∩C{O1,O2,O3,O4,O5,O6}=?

(3)

2 GB-APFB-RRT*算法原理

2.1 采樣方式和最優(yōu)擴展

在RRT算法中加入了雙樹思想,在起始點xstart和目標點xgoal生成兩顆隨機樹Tree1和Tree2,同時進行迭代擴展[14]。如圖3所示。

圖3 雙向擴展圖

在搜索空間內采用目標偏置策略來控制隨機樹采樣點xrand1,xrand2的生長,引導隨機樹擴展的大概生長方向。當隨機概率點Prandom小于目標偏置閾值xvalve時,隨機點xrand的生長偏向于未知區(qū)域擴展,即在空間中隨機選取采樣點,增加隨機樹向位置區(qū)域的遍歷。如果概率點大于等于閾值點,則隨機樹的擴展方向為另一顆隨機樹的起點。目標偏置策略公式為:

(4)

在此基礎上,加入RRT*擴展思想,重新為新枝葉點xnew重新選擇父節(jié)點,以節(jié)點xnew為圓心定義半徑為r(r一般選取迭代步長的3～5倍)做圓,尋找最優(yōu)點,作為替換xnew父節(jié)點的備選,依次計算xnew到鄰域r內所有節(jié)點的路徑代價,如果存在某一節(jié)點xmin到達xnew路徑代價小于xnearest到xnew的路徑代價最小并且不發(fā)生碰撞,則刪除xnew與xnearest的連線,將xmin作為新父節(jié)點,并且重新畫線路徑線,使得路徑代價降低,重布線示意圖如圖4所示。

圖4 最優(yōu)擴展

基于目標偏置雙向RRT*算法保留了RRT算法的整體性和擴展性,并且對隨機xrand點的生成增加了偏置,通過偏置閾值來引導隨機點的生成,當環(huán)境復雜且障礙物較多時提高閾值xvalve使隨機樹向未知區(qū)域進行遍歷,反之降低xvalve的數值,然后對xnew點增加了重選最優(yōu)點重布線過程,使得到的路徑接近最短路徑。但是該算法存在復雜環(huán)境下收斂速度過慢,搜索時間較長等問題。

2.2 新節(jié)點擴展策略

針對上文算法存在搜索目的性較差的特點,加入人工勢場思想[15]控制新節(jié)點的生成。

人工勢場的引力勢函數為:

Uatt(q)=1/2ξd2(q,qgoal)

(5)

人工勢場的引力函數為:

Fatt(q)=▽Uatt(q)=ξd(q,qgoal)

(6)

式中:ξ為引力常數,q和qgoal分別為規(guī)定區(qū)域中起點位置和目標位置,d(q,qgoal)表示兩個位置之間的歐氏距離,為設定的距離閾值。

人工勢場的斥力力勢函數為:

(7)

人工勢場的斥力函數為:

(8)

人工勢場法的合力勢函數為:

U(q)=Uatt(q)+Urep(q)

(9)

人工勢場法的合力函數為:

F(q)=Fatt(q)+Frep(q)

(10)

新節(jié)點Xnew的生長方式原理圖如圖5所示。圖中,xnew為點xnearest沿xrand方向擴展一個步長得到的初步新節(jié)點,Frep為障礙物對點xnearest的排斥力向量,Fatt為目標點xgoal對點xnearest的引力向量,F為點xnearest在搜索區(qū)域內所受的人工勢場和向量,Frand代表由點xnearest指向xrand的向量,F1為F和Frand的合向量。新節(jié)點的生長方向由父節(jié)點xnearest的人工勢場合向量和指向向量的合力方向確定,在該方向上擴展一個步長xstepsize則得到新節(jié)點Xnew,如式(11)所示。

圖5 新節(jié)點擴展原理圖

(11)

式中:xstepsize為偏置RRT*算法的迭代步長,λ為人工勢場的迭代步長,λ

2.3 路徑優(yōu)化

本文算法得出的路徑點P={P1,…,Pi,…,Pk}不是最優(yōu)路徑,充滿了冗余點,需要對冗余點進行去除。冗余點去除的思想為若Pi與Pk-1(k-1-i>1)間的連線與障礙物沒有發(fā)生碰撞則把Pi與Pk-1中的冗余點去除,如圖6所示。

圖6 冗余點刪除示意圖

在此基礎上,路徑中的冗余點已經全部刪除,但是存在大量拐點,不是一條平滑的曲線。機械臂在跟隨路徑運動時會造成抖動,不穩(wěn)定等特點,因此使用三次B樣條曲線對路徑進行平滑處理[16]。

K次B樣條曲線方程為:

(12)

式中:K表示B樣條的次數,K+1表示曲線階數,i表示B樣條的序號,控制點di個數為n+1,Ni,k(u)b表示i個K次B樣條基函數,使用Cox-deBoor遞推公式獲取:

(13)

因此三次均勻B樣條曲線為:

好的。雷果子還告訴我說只有屎克郎有點慘，當兵頭一年就上了南邊戰(zhàn)場，被敵人坦克輾死了，像輾一只屎克郎那樣輾得骨頭渣都沒剩了，真是慘哩。我聽后心里也怪不是味兒的。這樣沉默了好一會兒，我說明我的來意，我說我已經在天地人大酒店君子蘭廳訂了一桌飯，請他、螃蟹、屁斑蟲和茄子幾個一起吃。他說可以，還說螃蟹、屁斑蟲和茄子幾個沒忘了他這個小學老師，時常請他吃飯的。我說想請他以他的名義請他們幾個。他問為啥。我說長時間沒回來，他們幾個不一定還認得我，還是他出面請為好。他狠想了想，答應了。我說聲不見不散，然后告辭了。

p0,3(u)=d0*N0,3(u)+d1*N1,3(u)+
d2*N2,3(u)+d3*N3,3(u)u∈[0,1]

(14)

當給定控制點di后,利用上式就可以求出一條三次B樣式的平緩曲線點。

3 算法實現

本文算法流程圖如圖7所示。

圖7 算法流程圖

步驟1:初始化柵格地圖,初始化障礙物環(huán)境,定義初始參數,起末坐標點和起末兩顆隨機樹;

步驟2:利用概率偏置策略控制采樣點生成,并向采樣點方向擴展一個步長得到初步新節(jié)點,在次基礎上重選父節(jié)點重布線,使每次迭代的新節(jié)點路徑長度最短;

步驟3:引入人工勢場思想,算出父節(jié)點的人工勢場合力向量,并且和父節(jié)點指向初步新節(jié)點方向求出指向向量,利用式(11)求出最終新節(jié)點的位置;

步驟4:判斷新節(jié)點與障礙物是否發(fā)生碰撞,若發(fā)生碰撞則去除新節(jié)點,重新通過目標概率閾值選取采樣點重新迭代;否則進行兩個新節(jié)點的位置判斷,若不為同一個點,再次進入目標概率閾值選取采樣點重新迭代,若為同一個點,則進入下一步;

步驟5:合并兩棵樹節(jié)點為路徑點,進行路徑優(yōu)化處理,先刪除冗余點后通過三次B樣條進行曲線擬合;

4 算法仿真實驗分析

為了驗證本文算法的高效性和應用性,設置兩組仿真實驗。第一組實驗把機械臂看成一個質點尋找避障路徑,通過與傳統(tǒng)RRT算法、B-RRT*算法、APF-RRT算法的對比,來證實本文算法高效、準確的特點;第二組實驗把本文算法應用在具體的工程環(huán)境中,考慮機械臂放置工件時機械臂與障礙物的碰撞檢測,規(guī)劃出一條平緩的路徑。兩種實驗分析均在同一臺計算機上進行,計算機型號為80WW,在MATLAB 2021a上進行實驗仿真。

4.1 算法高效性仿真實驗

基于本文算法高效性仿真實驗分析,設置搜索區(qū)域大小為[0 1000;0 1000;0 1000],為搜索區(qū)域建立柵格地圖,起始點為原點[0 0 0],目標點為[1000 1000 1000],自定義參數迭代步長xstepsize為5,目標偏置概率為0.8,取20次數據的平均值做表,如表2所示。障礙物為形狀不同,大小不一的方體,立體和圓柱體,布局環(huán)境為不規(guī)則布局。其中,方體障礙物顏色為透明度是0.3的淺灰色,球體障礙物顏色為透明度是0.3的黑色,圓柱體障礙物顏色為透明度是0.3的深灰色,仿真實驗如圖8所示。

(a) RRT (b) B-RRT*

(c) APF-RRT(d) 本文算法圖8 算法對比圖

表2 算法數據對比圖

實驗結果表明,本文算法規(guī)劃時間、迭代步長、節(jié)點數、路徑長度皆為最優(yōu),證明了本文算法的準確性和高效性。分析表格數據可得,本文算法與傳統(tǒng)RRT算法相比規(guī)劃時間減少了95%,迭代次數降低了98%,路徑節(jié)點數降低了32%,路徑長度減少了20%;與Goal-bias BRRT*算法相比比較指標分別減少了89%、98%、5%、12%;與APF-RRT算法相比比較指標分別減少了54%、88%、38%、15%。

4.2 機械臂算法仿真實驗分析

首先,在MATLAB平臺搭建運動規(guī)劃場景。起始點為機械臂末端執(zhí)行器位置,坐標[0.68,0,0.75],終點為數控銑床工作臺工件裝夾定位中心[0.15,0.9,0.54]。在此基礎上,根據真實情況再設置兩個機械臂必經坐標點,第一個坐標點為機械臂面向銑刀位置點[-0.15,0.4,0.58],第二個坐標點為機械臂到達放置工件區(qū)域位置初始點[-0.15 0.9 0.58],以上4點均用紅點表示。障礙物環(huán)境為數控銑床長方體防護擋板,大小為[0.30,0.03,0.72]和[0.52,0.03,0.72],夾具4個裝夾工件[0.08,0.02,0.02]和銑刀,如圖9所示。

圖9 仿真環(huán)境圖

其次,ERF-900機械臂在關節(jié)空間中采樣規(guī)劃,采用包圍盒的方式對機械臂與障礙物進行碰撞檢測,將本文算法規(guī)劃出的路徑進行冗余點刪除和三次B樣條曲線優(yōu)化得到最終平滑路徑。

最后,記錄了機械臂通過本文算法規(guī)劃路徑中的4個運行位姿,分別是:面向銑床防護板位姿圖10a,到達第一個路徑點位姿,機械臂末端執(zhí)行器X軸坐標面向銑床夾具;通過銑床防護板位姿圖10b,機械臂與銑床防護板無碰撞;夾具上方姿態(tài)圖10c,到達第二個路徑點,機械臂到工件放置區(qū)域初始位置,準備放入夾具;機終點位姿圖10d,機械臂到達夾具裝夾工件位置,完成工件的放置。機械臂4個運行姿態(tài)如圖10所示。

(a) 面向銑床防護板位姿圖 (b) 銑床防護板位姿圖

(c) 夾具上方姿態(tài)圖 (d) 機終點位姿圖圖10 機械臂4個運行姿態(tài)圖

5 結論

為了解決路徑規(guī)劃中機械臂RRT算法存在盲目性高,耗時長等問題,本文提出了一種GB-APFB-RRT*算法。仿真實驗表明,本文算法搜索時間與RRT算法相比縮短了95%,迭代次數減少了98%,路徑節(jié)點數減少了32%,路徑長度縮短了了20%,驗證了本文算法的高效性。本文具體改進思路如下:

(1)從單向搜索改為雙向搜索,極大程度上縮短了搜索時間。

(2)加入目標閾值控制采樣點生成,減少了搜索的盲目性。

(3)新節(jié)點的生長引入人工勢場思想,增強了枝葉節(jié)點擴展的目的性,使新節(jié)點避障障礙物,又接近目標點。

本文提出的GB-APFB-RRT*算法,能極大縮短機械臂路徑搜索所需要的時間,提高機械臂的路徑規(guī)劃效率,這為機械臂對路徑搜索時長有要求的場景方面提供有益參考。