STEP-SA1400型機器人運動學建模與仿真*

李 慶，謝一首，鄭力新，周凱汀,張裕坤

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

STEP-SA1400型機器人運動學建模與仿真*

李 慶1，謝一首1，鄭力新1，周凱汀2,張裕坤1

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

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

工業機器人；SA1400機器人；六自由度；逆運動學；最短行程；MATLAB仿真

0 引言

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

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

1 運動學模型的建立

本文根據上海新時達機器人有限公司SA系列工業機器人中的1400型機器人的特點進行研究。SA1400型機器人有6個自由度，而且6個關節均為旋轉關節。為了描述機器人各連桿之間的相對位置和方向關系，需要根據關節結構在每個連桿上建立一個連桿坐標系。常用的方法是D-H (Denavit-Hartenberg)參數法，即使用矩陣方法來描述運動學問題。只要已知各關節的D-H參數，就可根據正運動學公式A1A2A3A4A5A6=0T6得到機器人末端的位置和姿態[2]。

SA1400機器人各連桿坐標系如圖1所示，相鄰兩連桿n-1與n之間的相對關系能夠按照兩次旋轉和兩次平移的四次齊次變換來建立，并把齊次變換矩陣記為An。此關系式為：

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

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

圖1 SA1400機器人坐標系

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

根據表1可得各變換矩陣如下：

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

0T6=A1A2A3A4A5A6

2 逆運動學方程的推導及求解

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

(1)

等式左邊為

根據An矩陣的逆矩陣公式

其中：

(1)求θ1

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

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

則

(2)求θ2

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

(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。

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

3 計算實例

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

逆解得到A點各關節角度θ1～θ6依次為：0(180),0(83.12),0,0(90),0(0),0(0)(單位：度)；B點6個關節角度依次為：25(-155),-15(89.60),20,-30(-150),15,-35(-93.29)(單位：度)。括號中為該關節的第二個解，本計算實例中假定各關節上一時刻的角度都為0度，則選取與0度的歐式距離較小的解為最優解。正解得到末端位姿分別為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°(α,β,γ為位姿坐標系相對于機器人底座坐標系或基坐標系的RPY角)。計算結果表明，本文中的正逆解方程是正確的。

4 運動學仿真

為驗證本文正逆解方程的準確性和可行性，使用MATLAB軟件對機器人走曲線軌跡的運動過程進行仿真[11]。仿真過程三維動畫截圖與末端軌跡曲線如圖2，圖中的理論軌跡與實際軌跡重合，說明本文正逆解方程是正確的。運動過程中各關節角度值如圖3，從圖中可知，運動過程中各關節角度值變化連續，且都在表1所列的關節角度范圍內，說明本文所解方程是可行的，具有實用性[12]。

圖2 螺旋線軌跡

圖3 螺旋線軌跡各關節角度值

5 結束語

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

[1] 中國測控網.工業4.0時代:工業機器人行業發展機遇分析

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

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

[3] 王其軍,杜建軍.MOTOMAN機器人逆運動學新分析[J].哈爾濱工業大學學報,2010,42(3):451-454．

[4] 王雪松,許世范,郝繼飛.MOTOMAN機械手逆運動方程新的推導方法及求解[J].中國礦業大學學報,2001,30(1):73-76.

[5] 蔡自興.機器人學(第二版)[M].北京:清華大學出版社,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.機器人學導論(第三版)[M].贠超，譯.北京:機械工業出版社,2014.

[9] 崔建國,宋愛國,黃惟一.遙操作系統中MOTOMAN-SV3機器人的運動建模研究[J].東南大學學報:自然科學版,2003,33(4):424-429.

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

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

[12] 陳發堂,牛勇清,韓娜娜,等.協議一致性測試平臺的搭建及仿真實現[J].電子技術應用,2014,40(4):137-140.

李慶(1991- )，男，碩士研究生，主要研究方向：光電信息檢測與智能計算、工業自動化技術。

謝一首(1988- )，男，碩士研究生，主要研究方向：光電信息檢測與智能計算、工業自動化技術。

鄭力新(1967- )，通信作者，男，博士，教授，主要研究方向：人工智能、工業自動化技術。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

福建省科技廳項目(2013H2002)；華僑大學研究生科研創新能力培育計劃資助項目(1400422004)

TP242.2

A

1674-7720(2016)05-0004-04

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

2015-12-06)作者簡介：