高春艷,唐家豪,呂曉玲,張明路

(河北工業(yè)大學(xué) 機(jī)械工程學(xué)院,天津 300130)

冗余機(jī)械臂因其較好的避障和躲避奇異點(diǎn)的特性受到了學(xué)者們的廣泛研究[1];但求逆解比較困難,目前,求逆解主要有3種方法:代數(shù)數(shù)值法、迭代算法和幾何參數(shù)法。代數(shù)數(shù)值法主要有雅可比偽逆法[2-3]、二次規(guī)劃反向算法[4];迭代算法主要有神經(jīng)網(wǎng)絡(luò)算法[5-6]、遺傳算法[7-8]、粒子群算法[9-10]。魏延輝等采用了在求機(jī)械臂逆解中運(yùn)用了構(gòu)形平面幾何參數(shù)方法,將機(jī)械臂三維問(wèn)題轉(zhuǎn)化為二維問(wèn)題進(jìn)行求解[11],但此方法對(duì)本次求解的機(jī)械臂不適用;韓書葵等采用代數(shù)法對(duì)機(jī)械臂進(jìn)行計(jì)算[12],此方法主要同過(guò)聯(lián)立方程對(duì)機(jī)械臂進(jìn)行求逆解,易于理解但計(jì)算較為復(fù)雜;王鵬程等提出了“自由度模塊化”的概念,通過(guò)將復(fù)雜的空間機(jī)構(gòu)運(yùn)動(dòng)學(xué)問(wèn)題轉(zhuǎn)變?yōu)槠矫鏅C(jī)構(gòu)問(wèn)題,大大提高了計(jì)算速度[13],但對(duì)此機(jī)械臂不適應(yīng);劉世平等采用BP神經(jīng)網(wǎng)絡(luò)對(duì)機(jī)械臂進(jìn)行求解,主要通過(guò)神經(jīng)網(wǎng)絡(luò)的訓(xùn)練數(shù)據(jù)擬合來(lái)實(shí)現(xiàn)各關(guān)節(jié)角度的反解[14];此方法需要進(jìn)行訓(xùn)練數(shù)據(jù),故對(duì)求解速度有所影響;胡奎等針對(duì)關(guān)節(jié)限位的機(jī)械臂求出了逆解[15]。上述幾種求逆解的方法,有的因其求解過(guò)程復(fù)雜,影響計(jì)算效率,有的不適用于此機(jī)械臂。

本文針對(duì)一種S-R-S構(gòu)形的機(jī)械臂,在建立機(jī)械正運(yùn)動(dòng)學(xué)模型的基礎(chǔ)上,提出了一種幾何法和代數(shù)法相結(jié)合的冗余自由度求逆解的方法,此方法形式簡(jiǎn)單,能夠快速得到所需關(guān)節(jié)轉(zhuǎn)角。

1 機(jī)械臂正解

首先根據(jù)機(jī)械臂模型采用D-H法對(duì)機(jī)械臂進(jìn)行建模，機(jī)械臂模型如圖1所示。

圖1 機(jī)械臂實(shí)體模型圖

根據(jù)機(jī)械臂模型得到機(jī)械臂參數(shù)如表1所示。

表1 機(jī)械臂D-H參數(shù)表

機(jī)械臂正向運(yùn)動(dòng)學(xué)中,根據(jù)D-H參數(shù)模型可以得到兩個(gè)相鄰坐標(biāo)系之間的轉(zhuǎn)換關(guān)系為

(1)

式中:Rot(z,θi)為坐標(biāo)系繞z軸的旋轉(zhuǎn)矩陣;Trans(z,di)為坐標(biāo)軸沿z軸平移矩陣;Trans(x,ai)為坐標(biāo)軸沿x軸平移矩陣;Rot(x,αi)為坐標(biāo)系繞x軸的旋轉(zhuǎn)矩陣。

根據(jù)計(jì)算可得

(2)

將機(jī)械臂參數(shù)表中的參數(shù)代入式(2),并令cosθi=ci、sinθi=si可以得到:

(3)

(4)

(5)

(6)

(7)

(8)

求得正解為

(9)

2 機(jī)械臂逆解

圖2 自運(yùn)動(dòng)示意圖

2.1 θ4的求解

為求解θ4將機(jī)械臂模型簡(jiǎn)化,如圖3所示。

圖3 θ4求解示意圖

在圖3中,S點(diǎn)的坐標(biāo)為(0 0d1),W的坐標(biāo)為末端位置坐標(biāo),為(pxpypz)。

由圖3可知:

(10)

∠SEW0=2arctan(lED/lSD)

(11)

根據(jù)幾何關(guān)系可得θ4=∠SEW0-∠SEW。

2.2 臂型角θ0=0時(shí)，θ1、θ2、θ3的求解

(12)

又根據(jù)幾何關(guān)系,坐標(biāo)軸3中的y軸在基坐標(biāo)系下的投影為

(13)

根據(jù)旋轉(zhuǎn)矩陣的定義,可得

(14)

(15)

(16)

(17)

2.3 求解θ1、θ2、θ3

根據(jù)Rodrigues 公式可得

(18)

(19)

根據(jù)D-H坐標(biāo)變換有

(20)

將式(19)與(20)聯(lián)立,即可求得:

(21)

2.4 求解其他關(guān)節(jié)轉(zhuǎn)角

其中θ5、θ6和θ7可通過(guò)下式求得

(22)

等式左邊已知,等式右邊根據(jù)D-H參數(shù)可得

(23)

聯(lián)立式(22)和(23),即可求得

(24)

至此,求出所有關(guān)節(jié)轉(zhuǎn)角。

3 仿真

使用MATLAB中的Robotics Toolbox進(jìn)行仿真,根據(jù)上文所述方法,在MATLAB中進(jìn)行編程,首先隨機(jī)給定50組角度進(jìn)行求正解,根據(jù)正解得到的位姿矩陣進(jìn)行求逆,將求得的逆解再次代入正解矩陣求得位姿矩陣,將所給位姿與求得位姿對(duì)比,得到誤差如圖4所示。

圖4 位置誤差圖

由圖4可知,位置誤差最大不超過(guò)6×10-13mm,故滿足其精度要求。

從隨機(jī)的50組數(shù)據(jù)中再挑出3組進(jìn)行驗(yàn)證,第一組數(shù)據(jù)為

(25)

圖5 第一組數(shù)據(jù)姿態(tài)

第二組數(shù)據(jù)為

(26)

圖6 第二組仿真位姿

第三組數(shù)據(jù)為

(27)

圖7 θ0分別取0和機(jī)械臂位姿

通過(guò)上述實(shí)驗(yàn)計(jì)算逆解，并觀察逆解計(jì)算時(shí)間，發(fā)現(xiàn)此逆解算法耗時(shí)0.50 ms。之后，對(duì)機(jī)械臂的逆解進(jìn)行運(yùn)動(dòng)仿真，隨機(jī)給定機(jī)械臂初始位姿和目標(biāo)位姿，并在其中插值50次，機(jī)械臂從初始位姿運(yùn)動(dòng)到末端位姿每個(gè)關(guān)節(jié)角度隨時(shí)間變化如圖8所示，機(jī)械臂末端位置變化如圖9所示。通過(guò)此仿真可以看出此逆解方法得到的軌跡順滑，并且仿真結(jié)果正確，因此求逆解的方法正確有效。

圖8 各關(guān)節(jié)角度隨時(shí)間變化圖

圖9 機(jī)械臂末端變化圖

4 結(jié)束語(yǔ)

本文針對(duì)一種特殊機(jī)械臂對(duì)其進(jìn)行了運(yùn)動(dòng)學(xué)分析,提出了一種幾何法和解析法相結(jié)合的求逆解方法,并對(duì)這種方法進(jìn)行了仿真實(shí)驗(yàn),實(shí)驗(yàn)結(jié)果表明此方法具有高精度,運(yùn)算簡(jiǎn)單的優(yōu)點(diǎn),對(duì)之后的運(yùn)動(dòng)學(xué)控制和路徑規(guī)劃具有較好的實(shí)用價(jià)值。