基于A*算法與組合函數(shù)的分揀機(jī)械臂軌跡規(guī)劃*

王成軍,姚星宇,楊 林,丁 凡

(安徽理工大學(xué) a.人工智能學(xué)院 b.機(jī)械工程學(xué)院,淮南 232001)

0 引言

在智能化鑄造生產(chǎn)線上常需要采用六軸工業(yè)機(jī)械臂完成上下料以及分揀的工作任務(wù)。鑄件分揀機(jī)械臂工作前必須對機(jī)械臂的末端軌跡進(jìn)行規(guī)劃,使其可在有障礙環(huán)境下,執(zhí)行分揀操作。常用的路徑規(guī)劃算法在動態(tài)環(huán)境和靜態(tài)環(huán)境下用法不同。在靜態(tài)路網(wǎng)中主要是以Dijkstra算法和A*算法為主[1],Dijkstra算法其實(shí)質(zhì)為廣度優(yōu)先搜索,因?yàn)檫@種算法不能確定目標(biāo)點(diǎn)的位置,需朝著所有可能方向遍歷大量節(jié)點(diǎn),導(dǎo)致工作效率低下,而A*實(shí)際上是一個帶有啟發(fā)性Dijkstra算法,由于引入啟發(fā)函數(shù),遍歷節(jié)點(diǎn)時更具有一定目的性,遍歷的中間節(jié)點(diǎn)更少,算法效率更高[2]。

針對傳統(tǒng)A*算法路徑規(guī)劃速率低、耗時長的問題,鄒文等[3]提出一種融合改進(jìn)A*算法與動態(tài)窗口法的路徑規(guī)劃算法,實(shí)現(xiàn)了快速規(guī)劃出一條折點(diǎn)少、耗時短的最優(yōu)全局路徑。高揚(yáng)等[4]通過引入克拉美-羅下界預(yù)測柵格定位方差的上界,設(shè)計新的代價函數(shù),實(shí)現(xiàn)更高定位性的尋找最優(yōu)路徑。但是A*算法計算通常在柵格中,得到的軌跡在速度、加速度上不連續(xù),且軌跡不平滑對機(jī)構(gòu)也會產(chǎn)生沖擊振動問題,廖志煒等[5]針對此類問題提出一種通過擺線函數(shù)和矩形函數(shù)相結(jié)合的方法,減小加速度所帶來的沖擊。張玲[6]基于三次非均勻B樣條曲線,通過曲線連續(xù)性對所求路徑逼近,成功減小加減速在運(yùn)行過程中的振動問題。但是目前將算法最短路徑和平滑軌跡相結(jié)合,針對于工業(yè)用六軸機(jī)械臂軌跡規(guī)劃的研究較少。

本文在對六軸分揀機(jī)械臂模型進(jìn)行運(yùn)動學(xué)分析后,采用A*算法規(guī)劃一條從起始點(diǎn)到目標(biāo)點(diǎn)的最短路徑,在所得出的路徑基礎(chǔ)上與組合函數(shù)相融合得到平滑軌跡,并與常用五次多項(xiàng)式插值函數(shù)模擬仿真對比分析,確保在組合函數(shù)形式下的軌跡規(guī)劃,機(jī)械臂能穩(wěn)定、平滑的運(yùn)行。

1 機(jī)械臂運(yùn)動學(xué)分析

以ER20-C10六軸分揀機(jī)械臂為研究對象進(jìn)行運(yùn)動學(xué)分析,為控制末端執(zhí)行器的運(yùn)動位置,需要建立末端執(zhí)行器和基座之間的關(guān)系坐標(biāo)矩陣,需要在各個關(guān)節(jié)上建立坐標(biāo)系,采用Modified DH參數(shù)法[7]對分揀機(jī)械臂建立坐標(biāo)系,如圖1所示。

圖1 六軸分揀機(jī)械臂連桿坐標(biāo)系

得到相應(yīng)的DH參數(shù)表如表1所示。表中i為連桿序號;di為沿著Zi軸方向,Xi-1軸與Xi軸之間的直線距離;ai-1為沿著Xi-1軸的方向,Zi-1軸和Zi軸之間的距離(ai>0);αi表示從Xi-1軸的方向看,Zi-1軸和Zi軸間的夾角;θi表示從Zi軸的方向看,Xi-1軸和Xi軸間的夾角。

表1 六軸分揀機(jī)械臂的DH參數(shù)表

1.1 正運(yùn)動學(xué)分析

運(yùn)用DH參數(shù)法可得到初始位置的齊次變換矩陣,相鄰兩連桿的變化矩陣公式為:

(1)

基于表1的DH參數(shù),得到各連桿的變換矩陣:

(2)

(3)

式中:[r11,r21,r31]T,[r12,r22,r23]T,[r31,r32,r33]T代表末端執(zhí)行器x,y,z的方向向量,[Px,Py,Pz]T代表末端執(zhí)行器的位置向量。

1.2 逆運(yùn)動學(xué)分析

由正運(yùn)動學(xué)可得到分揀機(jī)械臂末端位姿,而將末端執(zhí)行器的位置控制轉(zhuǎn)換為關(guān)節(jié)空間的角度控制,需要對分揀機(jī)械臂求逆解,一般所求的逆解不唯一,通過對比各關(guān)節(jié)的運(yùn)動參數(shù)依次排除可得到正確的角度。

六軸分揀機(jī)械臂前三軸能確定腕部中心的位置,后三軸能確定末端執(zhí)行器的姿態(tài),其腕部和姿態(tài)可相互解耦,用代數(shù)法[8]求解前三軸,根據(jù)機(jī)械臂的特點(diǎn)可得到:

(4)

(5)

(6)

計算0P4ORG坐標(biāo)平方和,用r表示,采用換元法消除關(guān)節(jié)角θ1,在a1≠0,sα1≠0的情況下得:

(7)

(8)

代入r中可求解出θ2θ3,θ4θ5θ6后三軸交于一點(diǎn)由姿態(tài)決定,數(shù)值可由歐拉角公式和旋轉(zhuǎn)矩陣求出[9]。

2 A*算法路徑搜索

2.1 A*算法

A*算法是一種用于靜態(tài)網(wǎng)路中取路徑最優(yōu)最有效的一種啟發(fā)式搜索方法。其算法核心是在最佳優(yōu)先算法和Dijkstra算法(廣度優(yōu)先)基礎(chǔ)上[10],添加heuristic啟發(fā)式函數(shù)。采用A*算法路徑搜索時,每遍歷一點(diǎn)都會通過估算當(dāng)前位置到最終目標(biāo)點(diǎn)的代價來判斷路徑的優(yōu)劣,將估算函數(shù)值較小的作為下一點(diǎn),直到到達(dá)目標(biāo)點(diǎn)。A*算法的路徑優(yōu)劣評價公式[11]為:

f(n)=g(n)+h(n)

(9)

式中:f(n)表示從路徑通過初始點(diǎn)到中間點(diǎn)n最終到達(dá)目標(biāo)點(diǎn)的代價估計,g(n)表示路徑從初始點(diǎn)到中間點(diǎn)n的代價估計,h(n)表示路徑從中間點(diǎn)到達(dá)目標(biāo)點(diǎn)的代價估計。A*算法路徑搜索前需定義兩種列表openlist表和closedlist表,將待考察的點(diǎn)放入openlist表中,已經(jīng)遍歷過的,或離自身距離為0的點(diǎn)放入closedlist表。路徑評價中h(n)估算函數(shù)的設(shè)計較為重要,若h(n)總是較小,此時A*算法能確定找到最短路徑,但是遍歷的節(jié)點(diǎn)也會增加,導(dǎo)致openlist表中節(jié)點(diǎn)過多,降低算法的速度。若h(n)等于中間點(diǎn)n到目標(biāo)點(diǎn)的代價,此時最優(yōu),能找到最優(yōu)路徑的同時且速度最快,所以設(shè)置一個合適的h(n)可以影響整個A*算法的效率。本文采用的啟發(fā)式函數(shù)h(n)可根據(jù)歐式距離函數(shù)進(jìn)行計算,即:

(10)

式中:xp,yp,zp為當(dāng)前點(diǎn)的坐標(biāo),xg,yg,zg分別為目標(biāo)點(diǎn)坐標(biāo),cP為當(dāng)前點(diǎn)的路徑代價。

本文對三維空間節(jié)點(diǎn)離散化分析,得到許多個離散點(diǎn),通過啟發(fā)式函數(shù)估算結(jié)束后,選出子節(jié)點(diǎn),再以子節(jié)點(diǎn)為父節(jié)點(diǎn)逐漸靠近目標(biāo)點(diǎn)。根據(jù)計算的啟發(fā)函數(shù)可使遍歷帶有一定方向的朝目標(biāo)點(diǎn)步進(jìn),而不用完全遍歷所有空間節(jié)點(diǎn)。

2.2 障礙物建立與避障實(shí)現(xiàn)

機(jī)械臂在實(shí)際工作環(huán)境中,障礙物的外形各異,為了方便模型模擬,采用較為常規(guī)、規(guī)則的空間集合體建立障礙物模型。障礙物模型中心坐標(biāo)為A(x1,y1,z1),半徑為r1球形物和中心坐標(biāo)為B(x2,y2),半徑為r2,高度為h的圓柱體。通過簡化模型,降低了計算難度,提升了算法效率,且所設(shè)障礙物大小較小,確保了機(jī)械臂模擬運(yùn)動可行性。

為了實(shí)現(xiàn)避障,采用同樣計算距離的方式,采用歐幾里得距離公式,避開球形障礙物時,通過反復(fù)循環(huán)計算子節(jié)點(diǎn)與球心坐標(biāo)之間的距離實(shí)現(xiàn)[12]。避開圓柱形障礙物時,在循環(huán)計算子節(jié)點(diǎn)XY軸坐標(biāo)與圓柱距離的同時且比較Z軸坐標(biāo)與圓柱高度,就可判斷是否發(fā)生碰撞,其公式表達(dá)為:

(11)

式中:x1、y1、z1為球形障礙物中心坐標(biāo),r1、r2分別為球半徑和圓柱障礙物半徑,x2、y2為圓柱中心坐標(biāo),h為圓柱高度。

3 三次樣條插值函數(shù)

A*算法求出的軌跡,雖然路徑距離最短,但結(jié)合實(shí)際機(jī)械臂操作存在軌跡不夠平滑、角度難以實(shí)現(xiàn),以及速度、加速度不連貫的問題。通常這些問題容易導(dǎo)致機(jī)械臂損傷,需要著重考慮。先對軌跡進(jìn)行平滑,通過A*算法的父子節(jié)點(diǎn)遍歷,把所取最優(yōu)路徑點(diǎn)放入所設(shè)Openlist列表中,節(jié)點(diǎn)間直線相連形成軌跡。

對求出的軌跡進(jìn)行平滑處理,一般常采用曲線插值的方法得出曲線的方程,但該種方法通常需要已知節(jié)點(diǎn)間的速度、加速度條件,而現(xiàn)已知條件只有通過的插值點(diǎn),需要定義一條平滑曲線,則采用均勻三次B樣條曲線[13]進(jìn)行平滑。

B樣條曲線方程:

(12)

式中:p(u)為B樣條曲線函數(shù),di為控制頂點(diǎn),Ni,k(u)(i=0,1,…,n)為第k次規(guī)范B樣條基函數(shù),n為插值點(diǎn)個數(shù),控制點(diǎn)個數(shù)為n+2,u為函數(shù)的參數(shù)軸,則整個B樣條方程就是在序列U:u0≤u1≤…≤ui+k+1所有k次分段多項(xiàng)式組合而成,通過控制點(diǎn)來影響曲線繪制。所用的均勻B樣條曲線,其任意節(jié)點(diǎn)區(qū)間的B樣條基在大的參數(shù)軸下都可由另一節(jié)點(diǎn)上的B樣條基平移得到,故可將[ui,ui+1]上整體參數(shù)u變換成局部參數(shù)t,做簡單參數(shù)變換。

u=u(t)=(1-t)ui+tui+1,t∈[0,1]

(13)

則B樣條曲線方程變?yōu)?

(14)

可得出三次均勻B樣條曲線方程的矩陣形式:

(15)

圖2為A*算法所求得的路徑圖,圖3為采用三次均勻B樣條函數(shù)對軌跡進(jìn)行平滑后的圖像,可看出融合了三次均勻B樣條后的軌跡已經(jīng)相對平滑。

圖2 A*算法最優(yōu)路徑軌跡圖

4 基于組合函數(shù)的軌跡規(guī)劃

為保證運(yùn)行過程中速度、加速度連續(xù)性在多個運(yùn)行階段不出現(xiàn)較大波動,確保分揀機(jī)械臂工作的平穩(wěn)性,啟動階段和停止階段不能存有沖擊,先采用擺線函數(shù)擬合,以速度、加速度及位置變量為約束條件,設(shè)加速度變量隨時間周期變化的函數(shù)方程為:

(16)

積分得其速度函數(shù)方程:

(17)

邊界約束條件:

(18)

設(shè)置組合函數(shù),將擺線函數(shù)速度、加速度峰值降低,平滑過渡將整段運(yùn)動時長分為8段,其中1/4周期的加速度值為矩形函數(shù)值,重新設(shè)置放大參數(shù),得組合函數(shù)擬合關(guān)系式[14]:

(19)

對上式進(jìn)行積分,代入邊界條件可得速度與時間的變量函數(shù):

(20)

通過上述分段函數(shù)積分求出軌跡線的關(guān)節(jié)角函數(shù):

(21)

帶入根據(jù)軌跡約束得:

(22)

為研究組合函數(shù)擬合曲線與五次多項(xiàng)式插值擬合曲線的優(yōu)異性,需要具體的函數(shù)圖像進(jìn)行比較,將所優(yōu)化軌跡和未優(yōu)化軌跡的點(diǎn)帶入到由DH參數(shù)表建立的MATLAB六軸機(jī)械臂模型中,采用機(jī)器人工具箱自帶的五次多項(xiàng)式插值模擬仿真得到其動力學(xué)參數(shù)圖。圖4為MATLAB模擬仿真圖,圖5為關(guān)節(jié)角位移對比圖,圖6為關(guān)節(jié)角速度對比圖,圖7為關(guān)節(jié)角加速度對比圖。

圖4 機(jī)械臂末端軌跡模擬仿真圖 圖5 關(guān)節(jié)角位移對比圖

圖6 關(guān)節(jié)角速度對比圖 圖7 關(guān)節(jié)角加速度對比圖

由圖5可知,所擬合過的組合函數(shù)軌跡圖與模擬仿真得出的關(guān)節(jié)角軌跡相差不大,從曲線波動來看25 s時位移突變較大,之后逐漸趨于平穩(wěn)。從極值來看組合函數(shù)的位移數(shù)值基本不大,相對來看較為穩(wěn)定。

由圖6可知,五次多項(xiàng)式插值求出的角速度關(guān)節(jié)1、2、3波動幅值較大,其幅值按關(guān)節(jié)順序逐漸減小,關(guān)節(jié)1幅值最大約為3 rad/s,關(guān)節(jié)6幅值最小,不超過0.2 rad/s,相對來看,組合函數(shù)的曲線波動更為穩(wěn)定,曲線更為平滑。在運(yùn)行周期的啟停階段除后3個關(guān)節(jié)外,組合函數(shù)的曲線均更為平緩,速度增長更平穩(wěn),關(guān)節(jié)處所造成的沖擊更小。

由圖7可知,所設(shè)置的組合函數(shù)具有較好的連續(xù)性,其1/4周期矩形部分很好地限制了關(guān)節(jié)的最大加速度和最小加速度,且相較于五次多項(xiàng)式插值得到的所有關(guān)節(jié)角加速度,就幅值和波動來看曲線變化基本一致。在設(shè)置良好的放大系數(shù)后,組合函數(shù)的峰值被穩(wěn)定在6.4 rad/s2,對機(jī)械臂而言加速度越大,所造成的慣性力越大,對機(jī)械臂的運(yùn)動、工作效率影響也越大,故所設(shè)組合函數(shù)更優(yōu),且在峰值部分與五次多項(xiàng)式插值函數(shù)對比,維持時間更長,工作效率更高。關(guān)節(jié)3、4為小幅精度調(diào)節(jié)運(yùn)動,增長速度較緩,關(guān)節(jié)3、4的增長幅度與組合函數(shù)增長幅度大相徑庭,從而所設(shè)計的組合函數(shù)對機(jī)械臂穩(wěn)定運(yùn)行有著良好的影響。

5 結(jié)論

針對所用鑄件分揀的機(jī)械臂軌跡規(guī)劃難題,通過采用A*算法,設(shè)置三維節(jié)點(diǎn)反復(fù)遍歷,利用啟發(fā)式函數(shù)獲取搜尋方向,尋最短、最優(yōu)路徑,路徑點(diǎn)間用直線段相連,形成軌跡。整體軌跡采用三次B樣條對所得路徑進(jìn)行平滑處理,為保證機(jī)械臂運(yùn)動過程平穩(wěn)性,通過一種由擺線函數(shù)和常數(shù)函數(shù)相結(jié)合的組合函數(shù)對角位移、角速度、角加速度進(jìn)行擬合,通過模擬仿真,得出仿真曲線圖。由仿真結(jié)果可知,在啟停階段用組合函數(shù)擬合的關(guān)節(jié)角速度更為平緩,關(guān)節(jié)角加速度在加速與減速階段增長趨勢和減小趨勢較小,且最大幅值保持時間維持約1/4周期,工作效率高。該軌跡優(yōu)化方法可有效降低機(jī)械臂關(guān)節(jié)的最大和最小速度和加速度,從而減少了機(jī)械臂振動和慣性力的影響,且末端執(zhí)行器軌跡最短,提高了機(jī)械臂工作效率。本研究為靜態(tài)路徑下的軌跡規(guī)劃提供了研究基礎(chǔ)。