一種五自由度機(jī)械手直線軌跡規(guī)劃方法研究

，，

(1.華中科技大學(xué)船舶與海洋工程學(xué)院，湖北 武漢 430074；2.空軍預(yù)警學(xué)院，湖北 武漢 430010)

一種五自由度機(jī)械手直線軌跡規(guī)劃方法研究

劉通1，唐國元1，黃道敏2

(1.華中科技大學(xué)船舶與海洋工程學(xué)院，湖北 武漢 430074；2.空軍預(yù)警學(xué)院，湖北 武漢 430010)

針對一種有特定作業(yè)要求的五自由度機(jī)械手進(jìn)行了軌跡規(guī)劃研究。在運(yùn)動學(xué)模型和工作空間的基礎(chǔ)上，根據(jù)機(jī)械手末端的直線運(yùn)動需求，不失一般性，設(shè)定所研究機(jī)械手的末端直線軌跡，分析了相應(yīng)的軌跡規(guī)劃的約束條件并建立約束方程。得到了在軌跡生成過程中各關(guān)節(jié)角和末端執(zhí)行器作業(yè)中心的對應(yīng)關(guān)系。詳細(xì)分析了2種軌跡規(guī)劃的算法，即多項式線性插值法和有界偏差關(guān)節(jié)路徑法(BDJP)。并用這2種方法對機(jī)械手末端作業(yè)所需要的直線軌跡進(jìn)行了仿真，給出仿真結(jié)果。

機(jī)械手；工作空間；直線軌跡規(guī)劃；有界偏差關(guān)節(jié)路徑法

0 引言

機(jī)械手的軌跡，指操作臂在機(jī)械手運(yùn)動過程中的位移、速度和加速度。軌跡規(guī)劃是根據(jù)作業(yè)任務(wù)的要求，計算預(yù)期的運(yùn)動軌跡[1]，也就是求出每個關(guān)節(jié)的關(guān)節(jié)角度隨時間的變化函數(shù)。因此，針對一種有特殊作業(yè)要求的五自由度機(jī)械手，進(jìn)行了三維空間的連續(xù)直線軌跡規(guī)劃研究。機(jī)械手須滿足對作業(yè)目標(biāo)實現(xiàn)直線插拔或者旋進(jìn)旋出動作，機(jī)械手末端執(zhí)行器的姿態(tài)在運(yùn)動過程中保持不變。連續(xù)直線軌跡規(guī)劃，是在末端執(zhí)行器保持姿態(tài)的情況下要求機(jī)械手末端執(zhí)行器時刻按照預(yù)定直線軌跡勻速運(yùn)動。在計算得出的工作空間的基礎(chǔ)上，詳細(xì)研究了末端執(zhí)行器沿直線軌跡運(yùn)動過程中各個關(guān)節(jié)角隨時間的變化函數(shù)。分別用預(yù)先解算關(guān)節(jié)變量解和實時解算關(guān)節(jié)變量2種算法對該模型進(jìn)行了仿真研究。

1 機(jī)械手系統(tǒng)及功能

機(jī)械手具有五自由度6功能。作業(yè)動作由大臂的擺動、大臂俯仰、中臂俯仰、小臂俯仰、手腕的旋轉(zhuǎn)和手爪的夾持等動作組合組成，三維模型如圖1所示。

圖1 機(jī)械手結(jié)構(gòu)

根據(jù)實際作業(yè)要求，結(jié)合圖2，對要求作出具體說明，即末端執(zhí)行器須沿直線AB運(yùn)動，實現(xiàn)插拔功能；末端執(zhí)行器沿直線AB前進(jìn)或者后退的速度v和腕關(guān)節(jié)旋轉(zhuǎn)速度ω，須滿足關(guān)系ω=tv(t是不為零的常數(shù))，實現(xiàn)旋進(jìn)旋出螺栓的功能；末端執(zhí)行器軸線始終與AB連線方向保持一致。

圖2 機(jī)械手作業(yè)路徑要求

2 運(yùn)動學(xué)模型及工作空間計算

利用D-H法描述機(jī)械手連桿間關(guān)系，建立運(yùn)動學(xué)模型。模型關(guān)節(jié)角范圍、機(jī)械手參數(shù)分別如表1和表2所示。拓?fù)浣Y(jié)構(gòu)如圖3所示。

表1 關(guān)節(jié)角度范圍 (°)

表2 機(jī)械手參數(shù)表

圖3 機(jī)械手拓?fù)?/p>

根據(jù)各個坐標(biāo)系之間的變換關(guān)系，可以得到從零坐標(biāo)系到第5個坐標(biāo)系的轉(zhuǎn)換矩陣，即

(1)

(2)

c2=cosθ2；s2=sinθ2；c2+3+4=cos(θ2+θ3+θ4)，其他類似，下同。

式(2)就是將末端執(zhí)行器的位置表示在基坐標(biāo)下的結(jié)果。如果零坐標(biāo)原點(diǎn)位置記為基坐標(biāo)原點(diǎn)，上述4×4矩陣第4列前3行就是末端執(zhí)行器在三維坐標(biāo)中的位置，即

(3)

根據(jù)式(3)，可知末端執(zhí)行器作業(yè)中心位置與各關(guān)節(jié)角的關(guān)系，并可利用變換矩陣和蒙特卡洛法繪制其工作空間[2]。

3 軌跡生成及仿真

3.1 軌跡生成的約束條件

(4)

由式(4)可以看出，處于第5坐標(biāo)系的末端執(zhí)行器的接近矢量由前4個關(guān)節(jié)角度決定。當(dāng)指定了這個矢量的各個分量之間的關(guān)系，就固定了矢量的方向，即末端執(zhí)行器的接近矢量。該模型中，第1個關(guān)節(jié)繞基坐標(biāo)Z軸旋轉(zhuǎn)變化，每一個固定值對應(yīng)一個作業(yè)面，每一個作業(yè)面的范圍由第2、3、4關(guān)節(jié)決定。為了簡便直觀，將第1個關(guān)節(jié)角即圖2所示的擺動關(guān)節(jié)固定，使之處于運(yùn)動學(xué)模型的零位，此時末端執(zhí)行器工作面就在基坐標(biāo)的X-Z平面，只需關(guān)注一個二維平面即可。

設(shè)定目標(biāo)物為一直線桿件，用一個基坐標(biāo)下的方向向量(1，0，-1)表示其姿態(tài)。作業(yè)要求為將桿件夾持，沿著桿件長度方向在基坐標(biāo)中(1 440，0，-400)到(1 300，0，-260)的直線上運(yùn)動，實現(xiàn)接插件的插拔，末端執(zhí)行器在有姿態(tài)要求的機(jī)械手作業(yè)區(qū)域內(nèi)，完成預(yù)定直線軌跡運(yùn)動。

根據(jù)作業(yè)要求為式(3)添加約束條件，即

(5)

利用逆運(yùn)動學(xué)，可由末端執(zhí)行器位置反向推算角度空間中各關(guān)節(jié)角大小[3]。令式(3)分別等于作業(yè)起點(diǎn)位置[1 440 0 -400]T和終點(diǎn)位置[1 300 0 -260]T，再加上約束條件式(5)，合并得方程組為：

(6)

計算出在作業(yè)起點(diǎn)和終點(diǎn)的關(guān)節(jié)角度分別為：

確定了軌跡的起點(diǎn)到終點(diǎn)，接下來就要求在一定的時間內(nèi)，4個角度值同時從起點(diǎn)值變化到終點(diǎn)值。最終需要求出這些關(guān)節(jié)角度的時間函數(shù)，也就確定了末端執(zhí)行器作業(yè)中心的運(yùn)行軌跡。假定這段路程所需時間為15 s。

3.2 軌跡規(guī)劃算法

機(jī)械手軌跡規(guī)劃的算法很多，插值法和有界偏差關(guān)節(jié)路徑法是較常用的算法。插值法是利用有限數(shù)量的已知點(diǎn)構(gòu)造一個有限次數(shù)的多項式來代替未知函數(shù)，再根據(jù)這個替代函數(shù)計算已知點(diǎn)以外的其他點(diǎn)的函數(shù)值。在數(shù)值計算理論中插值的方法有很多，如Lagrange插值、Newton插值和Hermite插值等[4]。其中簡單有效的是Newton插值法，構(gòu)造一個三次多項式來擬合幾個角度的時間函數(shù)。構(gòu)造三次Newton插值，至少需要4個已知點(diǎn)，故還須另外解出2個點(diǎn)。

求出θ2、θ3和θ4的時間函數(shù)分別為：

(7)

式(7)代入式(3)，利用Matlab的可視化仿真功能[5]，得到擬合結(jié)果。

有界偏差關(guān)節(jié)路徑法BDJP(bound deviation joint path)是Taylor提出的一種關(guān)節(jié)空間的運(yùn)動算法[6]。在軌跡規(guī)劃過程中，根據(jù)直線軌跡首尾2點(diǎn)，依次取已知點(diǎn)的中間節(jié)點(diǎn)，直到機(jī)械手在每段運(yùn)動中偏離預(yù)定軌跡的最大偏差在規(guī)定范圍δmax內(nèi)。該方法實質(zhì)是遞歸兩分法求中間點(diǎn)。

計算軌跡起點(diǎn)P0和終點(diǎn)P1關(guān)節(jié)矢量q0和q1，根據(jù)關(guān)節(jié)矢量求得中間點(diǎn)關(guān)節(jié)角度值為：

qm=q0+(q1-q0) /2

(8)

根據(jù)qm求得中間點(diǎn)Pm。而直接由起點(diǎn)和重點(diǎn)算得的中間點(diǎn)位置為：

(9)

計算Pm和Pc的偏差為：

δp=|Pc-Pm|

(10)

如果δp<δmax，則停止進(jìn)一步尋找中間點(diǎn)，采樣下一段軌跡。否則計算中間點(diǎn)Pc的關(guān)節(jié)矢量，并以Pc代替P1，P0代替Pc，進(jìn)一步尋找滿足條件的中間點(diǎn)。其流程算法如圖4所示。

圖4 BDJP算法流程

3.3 仿真結(jié)果及分析

從圖5可以看出，Newton插值法能很好地逼近預(yù)期的軌跡，在已知點(diǎn)附近，誤差是很小的，誤差最大的地方出現(xiàn)在2個已知點(diǎn)之間中心位置，約為2.5 mm。這個精度仍無法滿足如旋動螺栓或者插拔高精度接插件等特定作業(yè)要求。此外，如果軌跡很長，就需要解出很多個中間節(jié)點(diǎn)，以便求出高次擬合多項式來保證精度。但是，機(jī)械手軌跡插值法的多項式次數(shù)一般不多于3次，次數(shù)大于3次的關(guān)節(jié)函數(shù)積分得到加速度不是一個常數(shù)，加速不穩(wěn)定會帶來機(jī)械結(jié)構(gòu)的振動[7]。多次求解帶來的累積誤差，也會使末端執(zhí)行器位置精度不能保證。

圖5 插值法和BDJP法仿真結(jié)果

計算發(fā)現(xiàn)用BDJP法在X-Z平面跟蹤預(yù)定軌跡時，X方向的偏差大于Z方向的偏差10倍以上，所以只需關(guān)注X方向的偏差Xerr。圖5所示的結(jié)果是Xerr=0.5 時的結(jié)果。對比圖5中的2種算法，可以發(fā)現(xiàn)偏差明顯減小。為進(jìn)一步對比2種算法誤差大小，另再繪制誤差對比圖，如圖6所示。

圖6 插值法和BDJP法偏差對比

由圖6可知，多項式插值的方法擬合的最大誤差約為2.6；而用BDJP法最大誤差為0.2，為插值法的1/13。

由分析仿真過程與結(jié)果可知，針對所研究的五自由度機(jī)械手，BDJP法有以下優(yōu)點(diǎn)：算法收斂性好，仿真結(jié)果表明，每次遞歸之后的偏差減少60%以上；算法簡單，容易操作；可以仿真任意軌跡；仿真過程中，誤差可見而且能在算法中得以控制。

對比插值法和BDJP法，發(fā)現(xiàn)后者有明顯優(yōu)勢，在精度要求方面和算法實施方面都能滿足該項目實際要求。

4 結(jié)束語

以機(jī)械手運(yùn)動學(xué)模型為基礎(chǔ)，計算出工作空間，規(guī)劃直線運(yùn)動軌跡。并用Matlab仿真2種軌跡跟蹤的算法，比較2種算法優(yōu)劣，BDJP法在機(jī)械手工程實踐中能很好地應(yīng)用。

[1] 熊有倫.機(jī)器人技術(shù)基礎(chǔ)[M].武漢:華中科技大學(xué)出版社，2008.

[2] 趙燕江，張永德.基于Matlab的機(jī)器人工作空間求解方法[J].機(jī)械科學(xué)與技術(shù)，2009，28(9):1658-1661.

[3] Craig J J.Introduction to robotics: mechanics and control[M].New York: Addison-Wesky Publishing Co.，1989.

[4] 李紅.數(shù)值分析[M].武漢:華中科技大學(xué)出版社，2010.

[5] 周建興，等.Matlab從入門到精通[M].北京:人民郵電大學(xué)出版社，2009.

[6] Russell H.Taylor，Planning and execution of straight line manipulator trajectories in robot motion[M].Massachusetts: The MIT，1983.

[7] Macfarlane Sonja，Croft Elizabeth A.Jerk-bounded manipulator trajectory planning: design for real-time application [J].IEEE Transactions on Robotics and Automation，2003，19(1):42-52.

Study of Linear Trajectory Planning Method for Five-freedom Robot

LIUTong1，TANGGuoyuan1，HUANGDaomin2

(1.School of Naval Architecture and Ocean Engineering，Huazhong University of Science and Technology，Wuhan 430074，China；2.Air Force Early-warning Academy of PLA，Wuhan 430010，China)

Trajectory planning for a five-DOF manipulator is studied in this paper.On the basis of the kinematic model and workspace，according to linear motion needs of manipulator ends，a straight trajectory was set to analyze constraint and constraints equations were established.The correspondence between each joint angle and end effector operations center was obtained in the trajectory generation process.Details of the two kind of trajectory planning algorithm were introduced，that is polynomial interpolation and bound deviation joint path.A straight path which is used in actual operation was simulated with the two algorithms and the result was shown.

robot； workspace； straight trajectory generation；BDJP

2014-08-18

國家科技重大專項資助項目(2011ZX05027-004-02-03)

TP241.2

A

1001-2257(2014)12-0008-04

劉通(1987-)，男，四川通江人，碩士研究生，研究方向為艦船與水下航行體運(yùn)動控制，水下作業(yè)技術(shù)；唐國元(1973-)，男，湖北利川人，工學(xué)博士，碩士研究生導(dǎo)師，副教授，研究方向為艦船與水下航行體運(yùn)動控制，艦船機(jī)電控制系統(tǒng)及特種裝置系統(tǒng)，艦船動力裝置智能化理論與技術(shù)；黃道敏(1971-)，女，安徽鳳陽人，博士，研究方向為水下機(jī)器人，機(jī)電控制技術(shù)。