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)作者简介：