STEP-SA1400型機(jī)器人運(yùn)動(dòng)學(xué)建模與仿真*

李 慶，謝一首，鄭力新，周凱汀,張?jiān)＠?/p>

(1.華僑大學(xué) 工學(xué)院，福建 泉州 362021；2.華僑大學(xué) 信息科學(xué)與工程學(xué)院，福建 廈門(mén) 361021)

STEP-SA1400型機(jī)器人運(yùn)動(dòng)學(xué)建模與仿真*

李 慶1，謝一首1，鄭力新1，周凱汀2,張?jiān)＠?

(1.華僑大學(xué) 工學(xué)院，福建 泉州 362021；2.華僑大學(xué) 信息科學(xué)與工程學(xué)院，福建 廈門(mén) 361021)

根據(jù)STEP-SA1400型工業(yè)機(jī)器人的具體結(jié)構(gòu)特點(diǎn)，建立了機(jī)器人的運(yùn)動(dòng)學(xué)方程，使用只需一次矩陣逆乘的逆解方法，求出逆解。與常規(guī)求解方法相比，此方法減少了多次矩陣逆乘帶來(lái)的計(jì)算量。在解的表達(dá)式中，采用雙變量正切函數(shù)以避免解的丟失。針對(duì)多重解問(wèn)題，采用“最短行程”原則，選取與當(dāng)前關(guān)節(jié)角度值的歐氏距離較小的解作為逆解結(jié)果。最后，使用MATLAB編寫(xiě)程序，對(duì)文中推導(dǎo)出的方程進(jìn)行驗(yàn)證與仿真，實(shí)驗(yàn)結(jié)果證明了解的準(zhǔn)確性和可行性。對(duì)該型機(jī)器人的運(yùn)動(dòng)學(xué)分析與仿真為其后的離線編程、軌跡規(guī)劃等打下了基礎(chǔ)，同時(shí)，文中的方法與思想也適用于其他關(guān)節(jié)型機(jī)器人。

工業(yè)機(jī)器人；SA1400機(jī)器人；六自由度；逆運(yùn)動(dòng)學(xué)；最短行程；MATLAB仿真

0 引言

近年來(lái)，隨著經(jīng)濟(jì)和社會(huì)的發(fā)展，我國(guó)出現(xiàn)人力成本上漲、勞動(dòng)力供給減少以及制造業(yè)就業(yè)意愿下降的現(xiàn)象，這些現(xiàn)象嚴(yán)重制約了我國(guó)制造業(yè)的國(guó)際競(jìng)爭(zhēng)力。于是一些企業(yè)開(kāi)始把目光投向“機(jī)器換人”，利用自動(dòng)化技術(shù)來(lái)建設(shè)無(wú)人化工廠以解決當(dāng)前困局，制造業(yè)的轉(zhuǎn)型升級(jí)已是大勢(shì)所趨。面對(duì)德國(guó)提出的“工業(yè)4.0”，我國(guó)出臺(tái)的“中國(guó)制造2025”將重點(diǎn)發(fā)展工業(yè)機(jī)器人與新一代信息技術(shù)等領(lǐng)域，“智能制造”成為了中國(guó)制造的主攻方向，而機(jī)器人也成為這一主題下最受關(guān)注的領(lǐng)域之一。實(shí)現(xiàn)“中國(guó)制造2025”，最重要的智能部件就是網(wǎng)絡(luò)化的機(jī)器人，機(jī)器人產(chǎn)業(yè)將成為未來(lái)幾十年內(nèi)全球制造業(yè)的角力場(chǎng)。2013年，中國(guó)工業(yè)機(jī)器人的總銷(xiāo)量為3.7萬(wàn)臺(tái)，成為世界第一的機(jī)器人大國(guó)，也是全球增長(zhǎng)速度最快的機(jī)器人市場(chǎng)。2014年，全球工業(yè)機(jī)器人的銷(xiāo)量為22.9萬(wàn)臺(tái)，中國(guó)內(nèi)地售出5.7萬(wàn)臺(tái)，占全球銷(xiāo)量的四分之一[1]。

目前，機(jī)器人正解的求法已比較統(tǒng)一，而逆解的求解方法有多種，主要分為封閉解法和數(shù)值解法。封閉解法又分為代數(shù)解法和幾何解法。封閉解法計(jì)算速度快、效率高、便于實(shí)時(shí)控制，而數(shù)值解法因其迭代性質(zhì)，使其求解速度較慢，所以大多數(shù)情況下都是使用封閉解法[2-3]。逆解過(guò)程中，一般在關(guān)節(jié)角度范圍內(nèi)計(jì)算機(jī)器人關(guān)節(jié)角度，文獻(xiàn)[4]在解關(guān)節(jié)角時(shí)采用單變量反正切函數(shù)，可能造成一個(gè)解的丟失。機(jī)器人逆解存在多解，如文獻(xiàn)[5]中就有8組解，但控制機(jī)器人只能有一組解，而文中沒(méi)有給出選取最優(yōu)解的方法。

1 運(yùn)動(dòng)學(xué)模型的建立

本文根據(jù)上海新時(shí)達(dá)機(jī)器人有限公司SA系列工業(yè)機(jī)器人中的1400型機(jī)器人的特點(diǎn)進(jìn)行研究。SA1400型機(jī)器人有6個(gè)自由度，而且6個(gè)關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié)。為了描述機(jī)器人各連桿之間的相對(duì)位置和方向關(guān)系，需要根據(jù)關(guān)節(jié)結(jié)構(gòu)在每個(gè)連桿上建立一個(gè)連桿坐標(biāo)系。常用的方法是D-H (Denavit-Hartenberg)參數(shù)法，即使用矩陣方法來(lái)描述運(yùn)動(dòng)學(xué)問(wèn)題。只要已知各關(guān)節(jié)的D-H參數(shù)，就可根據(jù)正運(yùn)動(dòng)學(xué)公式A1A2A3A4A5A6=0T6得到機(jī)器人末端的位置和姿態(tài)[2]。

SA1400機(jī)器人各連桿坐標(biāo)系如圖1所示，相鄰兩連桿n-1與n之間的相對(duì)關(guān)系能夠按照兩次旋轉(zhuǎn)和兩次平移的四次齊次變換來(lái)建立，并把齊次變換矩陣記為An。此關(guān)系式為：

An=R(z,θ)T(0,0,dn)T(an,0,0)R(x,an)

式中：θn為關(guān)節(jié)n的旋轉(zhuǎn)角度，即兩連桿夾角，符合右手定則為正；dn為關(guān)節(jié)n的偏距，即兩連桿距離；αn為關(guān)節(jié)n和n-1軸線之間的夾角，即連桿扭角，符合右手定則為正；an為關(guān)節(jié)n和n-1軸線之間的公法線距離，即連桿長(zhǎng)度，n=1,2,3…6[5]。D-H參數(shù)表如表1。

圖1 SA1400機(jī)器人坐標(biāo)系

連桿nθn/(°)dn/mman/mmαn/(°)關(guān)節(jié)角度范圍/(°)關(guān)節(jié)速度/(°/s)1-θ1415180-90±1601502θ2-9005900-80～+1451503θ30115-90-190～+651604-θ4625090±1653605θ500-90±1153206-θ69800±360360

根據(jù)表1可得各變換矩陣如下：

式中：Sn=sinθn,Cn=cosθn,下同。所以末端執(zhí)行器的位姿方程為：

0T6=A1A2A3A4A5A6

2 逆運(yùn)動(dòng)學(xué)方程的推導(dǎo)及求解

一般具有6個(gè)自由度的機(jī)器人沒(méi)有逆運(yùn)動(dòng)學(xué)封閉解，但某些特殊結(jié)構(gòu)的機(jī)器人還是可以得到多組封閉解的，大多數(shù)工業(yè)機(jī)器人都可用Pieper提出的方法來(lái)求解，這種方法是針對(duì)6個(gè)關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié)且后3個(gè)關(guān)節(jié)軸線相交的操作臂。此方法也可應(yīng)用于包括移動(dòng)關(guān)節(jié)的其他形式的操作臂。觀察圖1中機(jī)器人，其3、4、5關(guān)節(jié)的軸線Z3、Z4、Z5交于一點(diǎn)，因此，這3個(gè)關(guān)節(jié)的運(yùn)動(dòng)不能產(chǎn)生沿Z2軸線方向的運(yùn)動(dòng)，所以這3個(gè)關(guān)節(jié)的變換矩陣乘積A3A4A5的第3行第4列上的元素為零。具有此特點(diǎn)的機(jī)器人，其運(yùn)動(dòng)學(xué)逆解存在以下簡(jiǎn)便求解方法[6-9]：

(1)

等式左邊為

根據(jù)An矩陣的逆矩陣公式

其中：

(1)求θ1

令式(1)左右兩邊矩陣的(3, 4)元素(表示矩陣的第三行第四列，下同)相等，得：

S1(px-d6ax)+C1(py-d6ay)=0

則

(2)求θ2

令式(1)左右兩邊(2, 4)和(1, 4)元素分別相等，并化簡(jiǎn)得：

(2)

C2v+S2u=w

(3)求θ3

式(2)中①×d4-②×a3,①×a3+②×d4得:

式中:m=-a2-vC2-uS2,n=-uC2+vS2,則

θ3=Atan2(S3,C3)

(4)求θ5

令式(1)左右兩邊(2, 3)與(1, 3)元素分別相等，可得:

①×S3-②×C3可得:

C5=(axC1-ayS1)cos(θ2+θ3)-azsin(θ2+θ3)

則θ5=Atan2(S5,C5)

(5)求θ4

令式(1)左右兩邊(3, 3)元素相等，可得:

則θ4=Atan2(S4,C4)

(6)求θ6

令式(1)左右兩邊(3, 2)元素相等，可得:

JC6-KS6=-C4

式中：J=oxS1+oyC1,K=nxS1+nyC1。

從以上各角度表達(dá)式可知，逆解存在多解，而控制機(jī)器人各關(guān)節(jié)的角度是唯一的。若忽略避障要求和軌跡優(yōu)化問(wèn)題，可按照以下步驟得到唯一解。首先，如有必要，將所得解加減360°，以補(bǔ)出關(guān)節(jié)角度表達(dá)式值域沒(méi)有包含的其他可能解；其次，由于關(guān)節(jié)運(yùn)動(dòng)范圍的限制，應(yīng)舍去其中一些解(甚至全部)；最后，根據(jù)“最短行程”原則，選取一個(gè)最近解，使得每一個(gè)運(yùn)動(dòng)關(guān)節(jié)的移動(dòng)量最小，以保證運(yùn)動(dòng)的連續(xù)、快速和低能耗，同時(shí)可用加權(quán)法使得解側(cè)重于移動(dòng)小連桿而不是移動(dòng)大連桿[8，10]。

3 計(jì)算實(shí)例

已知空間中的A,B兩點(diǎn)，其位姿矩陣分別為：

逆解得到A點(diǎn)各關(guān)節(jié)角度θ1～θ6依次為：0(180),0(83.12),0,0(90),0(0),0(0)(單位：度)；B點(diǎn)6個(gè)關(guān)節(jié)角度依次為：25(-155),-15(89.60),20,-30(-150),15,-35(-93.29)(單位：度)。括號(hào)中為該關(guān)節(jié)的第二個(gè)解，本計(jì)算實(shí)例中假定各關(guān)節(jié)上一時(shí)刻的角度都為0度，則選取與0度的歐式距離較小的解為最優(yōu)解。正解得到末端位姿分別為XA=903 mm,YA=0,ZA=1 120 mm,αA=0°,βA=-90°,γA=180°；XB=687.2 mm,YB=-306.5 mm,ZB=1 014.9 mm,αB=-99.6°,βB=-22.2°,γB=-109.4°(α,β,γ為位姿坐標(biāo)系相對(duì)于機(jī)器人底座坐標(biāo)系或基坐標(biāo)系的RPY角)。計(jì)算結(jié)果表明，本文中的正逆解方程是正確的。

4 運(yùn)動(dòng)學(xué)仿真

為驗(yàn)證本文正逆解方程的準(zhǔn)確性和可行性，使用MATLAB軟件對(duì)機(jī)器人走曲線軌跡的運(yùn)動(dòng)過(guò)程進(jìn)行仿真[11]。仿真過(guò)程三維動(dòng)畫(huà)截圖與末端軌跡曲線如圖2，圖中的理論軌跡與實(shí)際軌跡重合，說(shuō)明本文正逆解方程是正確的。運(yùn)動(dòng)過(guò)程中各關(guān)節(jié)角度值如圖3，從圖中可知，運(yùn)動(dòng)過(guò)程中各關(guān)節(jié)角度值變化連續(xù)，且都在表1所列的關(guān)節(jié)角度范圍內(nèi)，說(shuō)明本文所解方程是可行的，具有實(shí)用性[12]。

圖2 螺旋線軌跡

圖3 螺旋線軌跡各關(guān)節(jié)角度值

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

本文采用D-H參數(shù)法建立了STEP-SA1400型機(jī)器人的連桿坐標(biāo)系，確定了該型機(jī)器人的D-H參數(shù)及連桿間的位姿變換矩陣，求出了正運(yùn)動(dòng)學(xué)方程。針對(duì)3個(gè)相鄰軸相交于一點(diǎn)的6自由度操作臂，在研究總結(jié)了三軸相交的Pieper解法后，使用了一種避免大量矩陣逆乘運(yùn)算的逆解方法?？紤]到三角函數(shù)的取值范圍和機(jī)器人各關(guān)節(jié)角之間的影響，角度值方程采用了雙變量反正切函數(shù)，通過(guò)自變量的符號(hào)確定關(guān)節(jié)角度所在的象限，進(jìn)而取得合理解。針對(duì)逆解過(guò)程中出現(xiàn)的多解問(wèn)題，采用基于歐氏距離的“最短行程”原則選取最優(yōu)解。為了驗(yàn)證所求解方程的準(zhǔn)確性和可行性，使用MATLAB進(jìn)行了運(yùn)動(dòng)學(xué)仿真，仿真過(guò)程較真實(shí)地模擬了實(shí)際機(jī)器人的運(yùn)動(dòng)情況，仿真結(jié)果達(dá)到預(yù)期目標(biāo)。本文為該型機(jī)器人的應(yīng)用及其動(dòng)力學(xué)與控制器的研究打下基礎(chǔ)，同時(shí)，也為其他六自由度機(jī)器人運(yùn)動(dòng)學(xué)分析提供參考。

[1] 中國(guó)測(cè)控網(wǎng).工業(yè)4.0時(shí)代:工業(yè)機(jī)器人行業(yè)發(fā)展機(jī)遇分析

[EB/OL].(2015-12-02)[2015-12-04] http://www.ck365. cn/wap/index.php?mid=21&itemid=39875.

[2] 韓建海.工業(yè)機(jī)器人(第三版)[M].武漢:華中科技大學(xué)出版社,2015.

[3] 王其軍,杜建軍.MOTOMAN機(jī)器人逆運(yùn)動(dòng)學(xué)新分析[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào),2010,42(3):451-454．

[4] 王雪松,許世范,郝繼飛.MOTOMAN機(jī)械手逆運(yùn)動(dòng)方程新的推導(dǎo)方法及求解[J].中國(guó)礦業(yè)大學(xué)學(xué)報(bào),2001,30(1):73-76.

[5] 蔡自興.機(jī)器人學(xué)(第二版)[M].北京:清華大學(xué)出版社,2009.

[6] PAUL R, SHIMANO B, MAYER G. Differential kinematic control equations for simple manipulators[J]. IEEE Trans SMC,1981,11(6):449-455.

[7] PIEPER D, ROTH B. The kinematics of manipulaor under computer control[C]. Proceeding of the Second International Congress on Theory of Machines and Mechanisms,1969(2):159-169.

[8] CRAIG J J.機(jī)器人學(xué)導(dǎo)論(第三版)[M].贠超，譯.北京:機(jī)械工業(yè)出版社,2014.

[9] 崔建國(guó),宋愛(ài)國(guó),黃惟一.遙操作系統(tǒng)中MOTOMAN-SV3機(jī)器人的運(yùn)動(dòng)建模研究[J].東南大學(xué)學(xué)報(bào):自然科學(xué)版,2003,33(4):424-429.

[10] 葉上高,劉電霆.機(jī)器人運(yùn)動(dòng)學(xué)逆解及奇異和多解的處理[J].機(jī)床與液壓,2014,42(3):27-29.

[11] 林海峰,王姮.多關(guān)節(jié)機(jī)器人工作空間仿真方法[J].微型機(jī)與應(yīng)用,2014,33(2):72-74.

[12] 陳發(fā)堂,牛勇清,韓娜娜,等.協(xié)議一致性測(cè)試平臺(tái)的搭建及仿真實(shí)現(xiàn)[J].電子技術(shù)應(yīng)用,2014,40(4):137-140.

李慶(1991- )，男，碩士研究生，主要研究方向：光電信息檢測(cè)與智能計(jì)算、工業(yè)自動(dòng)化技術(shù)。

謝一首(1988- )，男，碩士研究生，主要研究方向：光電信息檢測(cè)與智能計(jì)算、工業(yè)自動(dòng)化技術(shù)。

鄭力新(1967- )，通信作者，男，博士，教授，主要研究方向：人工智能、工業(yè)自動(dòng)化技術(shù)。E-mail: 1275373176@qq.com。

STEP-SA1400 robot kinematics modeling and simulation

Li Qing1, Xie Yishou1, Zheng Lixin1, Zhou Kaiting2, Zhang Yukun1

(1.College of Engineering, Huaqiao University, Quanzhou 362021, China;2.College of Information Science and Engineering, Huaqiao University, Xiamen 361021, China)

According to the specific structural features of STEP-SA1400 industrial robots the robot kinematics equation is established. The solution method of only once multiplication of inverse matrix is used to find the solution. Compared with traditional methods, this method greatly reduces the amount of computation of the multiplication of inverse matrix. In the expression of the solution, using dual variable tangent function to avoid the loss of solutions. For the multiple solutions problem, the “shortest distance” criterion is used, choosing the solution that has the smaller Euclidean distance with the current joint angle values as the result of the inverse solution. Finally, MATLAB is used to program and carry out verification and simulation of the equations. The experimental results show the accuracy and feasibility of the solutions. Kinematic analysis and simulation of the robot lays a foundation for its trajectory planning, at the same time, the methods and ideas are also applicable to other articulated robot.

industrial robot; SA1400 robot; six degrees of freedom; inverse kinematics; the shortest distance; MATLAB simulation

福建省科技廳項(xiàng)目(2013H2002)；華僑大學(xué)研究生科研創(chuàng)新能力培育計(jì)劃資助項(xiàng)目(1400422004)

TP242.2

A

1674-7720(2016)05-0004-04

李慶，謝一首，鄭力新，等.STEP-SA1400型機(jī)器人運(yùn)動(dòng)學(xué)建模與仿真[J].微型機(jī)與應(yīng)用,2016,35(5):4-7,11.

2015-12-06)作者簡(jiǎn)介：