基于matlab的六关节机器人运动学仿真机械制造专业

1、目 录基于 Matlab 的六关节机器人运动学仿真 目 录摘要 .IABSTRACT .II第一章 绪 论 .11.1 研究的目的及意义.51.2 国、内外研究现状.51.2.1 国外研究现状.51.2.2 国内研究现状.61.3 本文的研究内容.6第二章 机器人运动学分析 .72.1 机器人理论基础.72.1.1 位置描述.72.1.2 姿态描述.72.1.3 位姿描述.82.1.4 坐标变换.82.1.5D-H 方法.92.2 正运动学分析.102.2.1 正运动学实例验证.142.3 机器人逆分析.172.3.1 求解关节角 THETA1 .172.3.2 求解关节角 THETA3 .1

2、82.3.3 求解关节角 THETA2 .192.3.4 求解关节角 THETA4 .202.3.5 求解关节角 THETA5 .202.3.6 求解关节角 THETA6 .212.3.7 逆运动学实例.21第三章 机器人运动学仿真分析 .22目 录3.1 MATLAB 软件基础 .223.2 ROBOTICS TOOLBOX 机器人工具箱介绍.233.3 运动学建模.233.4 正运动学仿真验证.263.5 逆运动学仿真验证.29第四章 总结 .31参考文献 .32致谢 .34附页 .35摘要基于 Matlab 的六关节机器人运动学仿真摘要摘要：随着制造业的发展，机器人技术的应用越来越普遍，

3、本文以 ABB 公司的六自由度工业机器人 IRB 4600 为对象进行了相关研究。首先对 ABB IRB 4600-20/2.5 型号机器人进行了正运动学分析。简要描述了机器人学科的基本理论基础，利用机器人学 DH 方法对机器人进行正运动学建模，利用矩阵乘积等式建立了机器人的正运动学模型。然后，对 ABB IRB 4600-20/2.5 机器人进行了逆运动学分析，接着利用机器人学和数学知识求解了 6 个关节角实例。最后对 ABB IRB 4600-20/2.5 机器人进行了仿真分析。利用 MATALB 编写正运动学和逆运动学计算程序，并且利用机器人工具箱作为对比，验证了前面正逆运动学算法的推导

4、过程的正确性。通过上述工作，本文对六关节工业机器人的运动学控制算法等关键技术进行了分析与验证，为后续运动学控分析和动力学控制定了基础。关键词： 正运动学； 逆运动学； MATLAB 运动学仿真 AbstractIIKinematics simulation of six-joint robot based on MATLAB Abstract：With the development of manufacturing industry, the application of robotics technology is becoming more and more popular. This

5、paper studies the six-degree-of-freedom industrial robot IRB 4600 of ABB Company. Firstly, the forward kinematics of ABB IRB 4600-20/2.5 robot is analyzed. The basic theory of robotics is briefly described. The forward kinematics model of robots is established by using DH method of robotics, and the

6、 forward kinematics model of robots is established by using matrix product equation. Then, the inverse kinematics of ABB IRB 4600-20/2.5 robot is analyzed. The notices of inverse kinematics of robots are introduced, including the existence of solutions, multiple solutions, solution methods, etc. The

7、n the analytical forms of six joint angles are solved by using robotics and mathematical knowledge. Finally, the ABB IRB 4600-20/2.5 robot is simulated and analyzed. The forward kinematics and inverse kinematics algorithms are compiled by MATALB, and the correctness of the derivation process of the

8、forward and inverse kinematics algorithms is verified by comparing them with the robot toolbox. Through the above work, this paper analyzed and validated the kinematics control algorithm and other key technologies of six-joint industrial robot, which laid the foundation for the follow-up kinematics

9、control.Keyword: forward kinematics; inverse kinematic; MATLAB kinematic simulation1第第 1 章章 绪绪 论论随着劳动力成本不断攀升且机器人制造成本不断下降，工业机器人的使用数量逐年增加，工业机器人由于其机械性重复精度高，不仅能够提高企业的生产效率、而且避免了工人进行从事某些单调枯燥的体力劳动。所以，机器人产业的发展也越来越受到相关企业和国家的重视。世界各国也都加大了机器人的研发力度，争取在早日抢占机器人市场，而近年提出的“工业 4.0”概念和“中国制造 2025”的规划，使得中国的工业机器人市场迅猛增加。国外

10、工业机器人发展较早 ,有着几十年的研发和使用经验，其技术和产品慢慢的已经成为一种成熟设备 ,己被广泛地应用到各种行业中。 最早成功研制出工业机器人的国家是美国，并且已经成为机器人的强国。虽然日本制造的机器人，在国际市场上也有绝对的优势；但是日本的劳动力比较匮乏，所以国家在研制和推广工业机器人的使用上花费很大的成本，日本的机器人企业和其附属的上下游产业都处于一流水平，所以日本在机器人行业具有很大的优势，尤其是价格和质量。德国的制造业高端水平一直是世界公认的，德国生产的设备有很好的口碑，有着质量的保证，而其机器人品牌KUKA 在工业机器人行业也是代表了先进的技术水平。欧洲其他较为 知名工业机器人的

11、主要生产厂商有瑞士 ABB、史陶比尔。 图 1.1 ABB 以及 FANUC 机器人机械工程 三班 高杰 2图 1.2 KUKA 以及 YASKAWA 机器人图 1.3 爱普生机器人3图 1.4 史陶比尔机器人国内工业机器人起步较晚，但是随着近年来国家的扶持和国内企业和科研院校对工业机器人的重视，很多企业都开始投入大量人力物力进行机器人技术的研发，其中工业机器人的发展按国家区域可以划分为环渤海、长三角、珠三角和中西部等产业集聚区。长三角地区由于交通便利，且人才聚集量大、加上有很多著名的高校和科研单位，所以整体上工业机器人发展较快，在该地区企业达标有埃斯顿、新时达、沈阳新松(上海)分公司。图 1

12、.5 新松和埃斯顿机器人珠三角地区在工业机器人控制系统即控制器的研发力度上比较大，所以有多个企业在该领域都是国内的领头羊，加上该地区处于沿海地段、有着广大的市场空间，国内代机械工程 三班 高杰 4表企业有广州数控、固高等。 图 1.6 广州数控机器人、固高控制器环渤海地区属于北方、但其高校和研究机构在国内是一流水平，而且很多高校都是很早就开始研究工业机器人，像哈工大等高校有着很浓厚的制造业基础，由于这些得天独厚的基础，使得哈工大集团、哈博实等生产的机器人具有显著的影响，图 1.7 哈工大集团哈博实机器人图片中西部地区虽然在地理位置上没有优势，但其引进了外部的优势资源，加上自身的实力，其发展势头

13、也很强劲。企业代表有埃夫特等。5图 1.8 埃夫特机器人图片1.1. 研究的目的及意义我国虽然拥有巨大的机器人市场，但是大部分市场份额都被国外机器人垄断，这样的市场现状是不利于国内工业机器人发展的，更不利于从“制造大国”向“制造强国”的转变。造成这样局面的很重要的一个原因是国产工业机器人的在质量上,与国外相比还有很大的差距。底层控制算法是机器人控制器的核心，国外机器人公司由于其技术积累，所以它们的机器人运动速度快而且不会抖动，而国内机器人在节拍速度和稳定性上都赶不上国外机器人。本课题建立机器人是通过的 D-H 方法，建立的运动学模型，在此基础上，求解了机器人正逆运动学数学方程，然后继续进行了算

14、法编程，算法编程运用的是 Matlab 软件，得到计算结果，并与 Robotics Toolbox 机器人工具箱中的得到的结果进行对比，进一步验证了动力学算法的正确性。精确、快速的运动学算法可以为后续的机器人运动和力控制提供基础，具有一定的理论指导意义。1.2. 国、内外研究现状研究机器人运动控制的前提是研究工业机器人运动学，运动学位置分析是研究机构的关节输入和末端位姿输出之间的几何关系。通常给定输入，可以得到确定的几何构型输出，分析过程中不涉及机构部件的质量和惯量等动态参数信息。对于串联机器人，其正解是一系列旋转矩阵的乘积，一般情况下输入和输出是单一的映射关系，从而逆向计算得到当前末端位姿时

15、各个关节变量角度值。机器人运动学方程求解是一个很复杂的过程，而且是非线性的，因此在求解过程中经常会遇到解是否存在、多重解、求解方法等问题。1.2.1.国外研究现状国内外研究学者针对六自由度机器人的位置正逆解问题均做了大量的研究。Denavit 和 Hartenberg1-3 早在 1954 年就提出了 DH 方法。该方法可具体分为前置法及后置法，串联机器人实际上可以看成一系列连杆组成的运动链，该方法是目前分析串联机器人运动学最多的方法，只需要四个参数就能表达空间中刚体的的位姿，但使用该方法建立连杆机械工程 三班 高杰 6间的坐标系时需要遵循一定的原则。此外还有其他一些描述机构运动学参数的方法，

16、比如POE 法。1968 年，Pieper 4-6首次在他的博士论文中提出对特殊结构 6R 机器人的逆运动学的数学解法。Tsai 等7运用高位逼近方法分析逆运动学问题，从而将问题简化为 8 个二次方程，并运用多项式连续方法进行数值求解，最后推测出不同结构的 6R 机器人最多拥有 16 组逆解。Manocha 8运用 24 阶矩阵特征分解方法，从而进一步提高了逆运动学求解的稳定性和精度。 Lee 等 9 运用矢量乘法运算方法发现了 14 个线性无关的逆运动学方程，通过运用分离变量消元法，将逆运动学问题简化为了关节变量半角正切的一元 16 次方程求根问题。1.2.2.国内研究现状国内机器人研究起步

17、较晚，大多是高校和研究所在国外算法的基础上做改进，程永伦等10提出了一种基于旋转子矩阵正交的方法进行 6R 机器人逆运动学求解， 刘松国等人11提出了一种基于矩阵分解的一般 6R 机器人实时高精度逆运动学算法。李跃等人12提出了一种网络，该网络名字叫 RBF 网络，主要运用在 PUMA560 机器人上进行运动学逆解求解的方法，吕世增等13在论文中提出了基于一种吴方法的六关节机器人逆运动学旋量方程求解。钱东海等人14提出了一种 6 自由度机器人逆解算法，该算法是基于旋量理论和 Paden-Kahan 子问题。倪振松等人15提出了一种 6R 运动学解算法，他提出的算法，理论依据是四元数矩阵与 Gr

18、oebner 基。朱世强等人16提出了实时高精度逆运动学算法，该方法是针对一般 6R 机器人，并且依据是基于矩阵分解的方法。刘华山等人17提出了一种实时性逆解算法，该方法是基于向量内积的机器人实时性逆解算法。1.3. 本文的研究内容本文以 ABB IRB 4600-20/2.5 研究对象，运用机器人学理论，分析了其运动学位置正解、逆解、并在 MATLAB 中进行了运动学仿真等内容，为后续研究提供了理论基础。具体地，介绍了研究机器人学的理论基础，首先介绍了位置、姿态的概念，然后简述了坐标变换的相关内容，最后依据 D-H 参数方法建立了连杆坐标系，并确定各个连杆的 D-H 参数，通过应用坐标变换理

19、论，并基于 D-H 方法推导了机器人的正运动学方程。在正运动学算法的基础上介绍了逆运动学算法求解过程中遇到的解是否存在、多重解、求解方法等问题，并进行了分析，接着运用矩阵方程的恒等变换、三角函数替换等多种方法求解六自由度机器人的各个关节变量的解析表达式。为运动控制算法的进一步研究奠定了基础。为了进一步验证了本文正逆运动学求解结果的正确性，然后在 Matlab 环境下，基于 D-H 连杆参数法对 ABB IRB 4600-20/2.5 机器人建立数学模型，通过后调用机器人工具箱中的相关运动学函数来实现求解机器人的正运动学和逆运动学计算结果。基于 Matlab 的六关节机器人运动学仿真7第第 2

20、章章 机器人运动学分析机器人运动学分析研究机器人运动控制的前提是研究工业机器人运动学，运动学位置分析是研究机构的关节输入和末端位姿输出之间的几何关系。通常给定输入，可以得到确定的几何构型输出，分析过程中不涉及机构部件的质量和惯量等动态参数信息。对于串联机器人，其正解是一系列旋转矩阵的乘积，本章在根据具体的六自由度机器人参数建立了 ABB IRB 4600-20/2.5 机器人模型并推导机器人正逆运动学的结果。2.1. 机器人理论基础机构的运动学分析已经有许多方法，本文用于机构运动学分析基础理论主要包括刚体的位姿表示方法，旋转矩阵之间的转换关系，机器人学中 D-H 方法等内容，下面分别一一介绍。

21、2.1.1.位置描述对于任何坐标系，任何点都可以通过位置矢量来进行定位，具体可通过一个 31 的矢量就可以实现。但是不同的坐标系矢量的表示也不同，因此除此之外，还需位置矢量中附加一个信息，说明这个点是在哪一个多系中定义的。例如，空间点的位置可以用它相对于的P参考坐标系的坐标分量来表示，如图 2.1 所示。图 2.1 空间 P 点的坐标表示 * MERGEFORMAT (3.1)XAYZPPPP其中表示点在参考坐标系中的位置。APP A2.1.2.姿态描述之前的学者想了一种办法来描述空间中物体的姿态，他们是通过与要描述的物体连接成一体的物体的姿态来实现，假设有一物体 B，为了确定它的姿态，咋设一

22、个直角坐标系B与它刚性连接，可以用下述公式表示物体 B 相对于坐标系A的姿态： * MERGEFORMAT 111213212223313233AAAABBBBrrrRrrrrrrxyzPAOAXAYAZ机械工程 三班 高杰 8(3.2)为旋转矩阵，且为单位正交矩阵，在坐标变换中绕坐ABR1=1AATABBBRRR且XYZ标轴纯旋转的变换表示有： * MERGEFORMAT (3.3)cossin0( , )sincos0001R Z * MERGEFORMAT (3.4)cos0sin( ,)010sin0cosR Y * MERGEFORMAT (3.5)100(, )0cossin0si

23、ncosR X其中 表示分别绕 Z、Y、X 轴旋转的角度。, 2.1.3.位姿描述假设在某一个物体上，取一个坐标系 B 的原点 O，为了描述坐标系B的原点在坐标系A中的位置，通过位置矢量，用旋转矩阵描述坐标系B在坐标系A中的姿态。APABR物体 B 的位置位姿可用以下齐次矩阵的形式表示： * MERGEFORMAT (3.6)01AAABBBRPT2.1.4.坐标变换针对平移坐标变换，假设有两个坐标系A和B，我们在坐标系B取一点 P，用BP 表示它的位置，可用矢量APBORG表示 B原点相对于A的位置，当A和B的姿态相同时，只要矢量相加，就可以得到该矢量在A系中的表示，如下：ABABORGPP

24、PXYZAXYZBPBPAPAPBORG图 2.2 平移坐标变换基于 Matlab 的六关节机器人运动学仿真9针对两个坐标系A和B旋转坐标变换，假设坐标系A和坐标系B的原点相同，在坐标系B中有一个点 P，其位置用矢量BP 表示，旋转矩阵表示B系相对坐标系ABRA的旋转，则该矢量在A系中的表示即为：AABBPRP * MERGEFORMAT (3.7)ABBxAABByAABBzAPXPPYPPZP在在坐标系B中取一个点 P，其位置用矢量BP 表示，则AP 可以用先将B系变换为和A系姿态相同的中间系，再平移到A系。即：。该表示方式还可以AABABBORGPR PP改为用齐次矩阵表达的形式： *

25、MERGEFORMAT (3.8)01AABBAAABBORGBPT PRPT2.1.5.D-H 方法DH 法是机器人运动建模的标准方法，该方法是由 Denavit 和 Hartenberg 提出的。该方法可以把串联机器人看做是多个连杆的集合，在每个连杆上建立坐标系，坐标系之间的旋转变换矩阵的乘积可以表示基坐标系和工具坐标系之间的映射关系。DH 建模分为前置和后置两种方法，本文使用 DH 后置法对 ABB IRB 4600-20/2.5 机器人其建模，连杆坐标系如图 2.3所示，其中坐标轴的选取方式如下：Link n图 2.3 D-H 后置连杆坐标系坐标系到坐标系的变换顺序为：nnnno x

26、y z1111nnnnoxyz1)以为转轴转动角度，使与同向nz1nnx1nx机械工程 三班 高杰 102)沿轴移动距离，使与共轴线nz1ndnx1nx3)沿轴移动距离，使与两个坐标系的原点重合1nx1nanz1nz4)以为转轴转动角度，使与同向1nx1nnz1nz至此，坐标系完全变换到坐标系上，这种变换关系可以用齐次nnnno x y z1111nnnnoxyz变换表示为： * 1111111111111111111111( ,) rans( ,) rans(x,)(x,)cossincossinsincossincoscoscossinsin0sincos0001nnnnnnnnnnnnn

27、nnnnnnnnnnRot zTz dTaRotaadTMERGEFORMAT (3.9)2.2. 正运动学分析本文的研究的 ABB IRB 4600-20/2.5 型号的机器人，其模型和连杆参数如下图所示。 图 2.4 ABB IRB 4600-20/2.5 三维模型基于 Matlab 的六关节机器人运动学仿真11图 2.5 机器人参数图 2.6 后置 D-H 坐标系以 ABB IRB 4600-20/2.50 机器人作为研究对象，根据其连杆尺寸参数，建立机器人连杆坐标系，在此基础上建立机器人 DH 连杆参数表，如下表 2.1 所示。机械工程 三班 高杰 12表 2.1 连杆的参数表 连杆i

28、/i/idmm1/iamm1/i初始值/1110002201-90-903302004443-90055009006600-900d1=495a1=175a2=1095a3=175d4=1230.5其中，连杆长度指的是是 与轴的距离，连杆扭角指的是是 1/iamm1iziz1/i1iz与轴的夹角，连杆转角指的是是 与轴的夹角，连杆距离指的是是 iz/i1ixix/idmm1ix与轴的距离。ix机器人的正运动学建模，前提是已知机器人六个关节的转角，求解机器人末端工具相对于机器人基坐标系的位姿，根据表 2.1 所示 D-H 连杆参数，代入矩阵变换公式(此后长度单位均为 m) * 1cossinco

29、ssinsincossincoscoscossinsinT=0sincos0001nnnnnnnnnnnnnnnnnnnaadMERGEFORMAT (3.10)对每个连杆变换矩阵，分别将 D-H 参数表中的四个参数代入，得 * MERGEFORMAT (3.11)111101100000010001csscTd * MERGEFORMAT (3.12)221122200010000001csaTsc基于 Matlab 的六关节机器人运动学仿真13 * MERGEFORMAT (3.13)332332300000100001csascT * MERGEFORMAT (3.14)44343444

30、0001000001csadTsc * MERGEFORMAT (3.15)554555000010000001csTsc * MERGEFORMAT (3.16)665666000010000001csTsc式中cos;siniiiics将上述相邻齐次变换矩阵依次右乘则得到杆件坐标系到基坐标系的齐次变换矩阵i 0 * MERGEFORMAT 1 21 211 11 21 211 100121222100001c cc ssa cs cs sca sTT Tscd(3.17) * 1 231 2312 1 21 11 231 2312 1 21 100232323232 2100001c cc

31、 ssa c ca cs cs sca s ca sTT Tsca sdMERGEFORMAT (3.18)式中cos();sin()ijijijijcs* 1 23 41 41 23 41 41 234 1 233 1 232 1 21 11 23 41 41 23 41 41 234 1 233 1 232 1 21 100343423 423 4234 233 232 210001cc cs scc ss cc sd c sa cca cca cs c cc ss c sccs sd s sa s ca s ca sTT Ts cs scd ca sa sd机械工程 三班 高杰 14ME

32、RGEFORMAT (3.19)* 1 23 41 451 23 51 23 41 451 23 51 23 41 44 1 233 1 232 1 21 11 23 41 451 23 51 23 41 451 23 51 23 41 44 1 233 1 232 1 21 100454523 4 52()()()()cc cs s ccs scc cs s scs ccc sscd csa cca ccacsc ccs cs s ssc ccs ssc csc sccd s sa sca scasTT Ts c cc3 523 4 523 523 44 233 232 210001ss c

33、 sc cs sd ca sa sdMERGEFORMAT (3.20) * MERGEFORMAT (3.21)0056560001xxxxyyyyzzzznoawnoawTT Tnoaw式中1 23 41 45 61 23 5 61 23 41 46()()xnc c cs s c cc s s cc c ss c s1 23 41 45 61 23 5 61 23 41 46()()yns c cc s c cs s s cs c sc c s23 4 523 5623 4 6()zns c cc s cs s s 1 23 41 45 61 23 5 61 23 41 46()()xo

34、c c cs s c sc s s sc c ss c c 1 23 41 45 61 23 5 61 23 41 46()()yos c cc s c ss s s ss c sc c c 23 4 5 623 5 623 4 6zos c c sc s ss s c1 23 41 451 23 5()xac c cs s sc s c 1 23 41 451 23 5()yas c cc s ss c c 23 4 523 5zas c sc c4 1 233 1 232 1 21 1xwd c sa c ca c ca c 4 1 233 1 232 1 21 1ywd s sa s c

35、a s ca s 4 233 232 21zwd ca sa sd 通过上述公式，只要给出了各关节的指定角度后，就可以求出机器人末端的位置和姿态，而具体的数值验证会在第四章进行。2.2.1.正运动学实例验证为了简单的验证上述推导的正确性，将机器人初始位姿下的关节角参数0,pi/2,0,0,0,0，画后置 DH 模型带入连杆变换矩阵，如下所示：基于 Matlab 的六关节机器人运动学仿真15图 2.7 实例 DH 坐标系01100001000010.4950001T120100.175001010000001T231001.095010000100001T 341000.1750011.2305

36、01000001T451000001001000001T561000001001000001T将上述 6 个矩阵依次做乘积得到初始位姿下末端 6 坐标系相对于 0 坐标系的位姿矩阵为： * MERGEFORMAT (3.22)10011.05501001000.7750001T从几何上分析，由机器人初始位姿下的连杆坐标系（图 2.6）可知，6 坐标系 相666X Y Z对于基坐标系在 x 轴方向上的距离为;在 y 轴方向000X Y Z140.175 1.23051.055ad 上的距离为 0;在 z 轴方向上的距离为 ；而 61230.495 1.0950.1750.775daa 坐标系的

37、Z 轴方向是 0 坐标系的 X 轴负方向，所以 6 坐标系相对于 0 坐标系的变换矩阵的第一列为0 0 -1; 6 坐标系的 Y 轴方向是 0 坐标系的 Y 轴方向的负方向，所以 6 坐标系相对于0 坐标系的变换矩阵的第二列为0 -1 0; 6 坐标系的 X 轴方向是 0 坐标系的 Z 轴负方向，所以6 坐标系相对于 0 坐标系的变换矩阵的第三列为-1 0 0。所以从几何角度可以得知在初始位机械工程 三班 高杰 16姿下 6 坐标系相对于 0 坐标系的位姿矩阵为： * MERGEFORMAT (3.23)20011.05501001000.7750001T 和是相等的，从而可以验证上述运动学方

38、程的推导是正确的。1T2T2.3. 机器人逆分析算法的好坏直接决定了控制器性能的好坏。 机器人逆运动学方程求解作为一个非线性问题，在求解过程中会遇到解是否存在、多重解、求解方法等问题，下面一一说明和分析：对于解是否存在问题，由于机器人机械臂的长度是一定的，所以机器人末端工具所能到达的范围也是有限的，该范围就是机器人术语中的工作空间。对于多重解问题，因为机器人是 6 旋转关节机械臂，如果指定工作空间内的某一点，机器人可以以几种不同的位型到达，而对于运动控制系统而言，只能选取一个解，所以如何从多个解中选取一个合理的解是要考虑的问题，一般情况下工程的做法是选取“最短行程”解，即，选取使每个关节移动量

39、最小的那个解，这样能够提高机器人的速度。2.3.1. 求解关节角 theta1针对本文研究的 ABB IRB 4600-20/2.5 机器人，本文拟采用代数解法对其进行逆运动学进行求解，具体步骤如下所示：由于运动学逆解是必须知道机器人的末端位置姿态，然后才能计算机器人各个关节变量角度值，因此中的每个元素必须是已知的，由机器人正运动学方程知：06T * 11121312345212223006112345623456313233T=0 001xyzrrrprrrpTTTTTTrrrpMERGEFORMAT (3.1)其中，6个旋转变换矩阵已经在 012345112233445566TTTTTT、

40、第二章正运动学求解过程中求解完成。为了求解，将包含变量的变换矩阵移动到等式方11程左边，其它的变换矩阵放在等式右边： * MERGEFORMAT (3.2)0101166TTT * 23451 0116223456345016TTTTTTTMERGEFORMAT (3.3)基于 Matlab 的六关节机器人运动学仿真17 * MERGEFORMAT 1111121311212223163132330000001000010001xyzcsrrrpscrrrpTrrrp(3.4)上面三个方程式等价的。令上式 L(2,4) = R(2,4), 得 * MERGEFORMAT (3.5)113xys

41、 pc pd为了求解上式，需要利用数学中的三角恒等变换公式，令 * MERGEFORMAT (3.6)cossinxypp其中 * MERGEFORMAT (3.7)22tan2,xyvxppApp带入上式得 * MERGEFORMAT (3.8)311dc ss c利用差角公式，得 * MERGEFORMAT (3.9)31sind * MERGEFORMAT (3.10)2312cos1d 则 * MERGEFORMAT (3.11)23312 Atan 2,1dd * 222133=Atan2(p ,)tan2(,)yxxypadppdMERGEFORMAT (3.12)由上式， 对应有

42、两组解。1机械工程 三班 高杰 182.3.2. 求解关节角 theta3因为已经求解了，即下式中等式左边是已知的，由等式两边对应元素相等可知，令中1(1,4)(1,4); (3,4)(3,4)LRLR * MERGEFORMAT 1111121311212223163132330000001000010001xyzcsrrrpscrrrpTrrrp(3.13)可得 * MERGEFORMAT (3.14)113 234 23223 234232 2xyxc ps pa cd sa cpa sd ca s则 * MERGEFORMAT 2222(1,4)(3,4)(1,4)(3,4)LLRR(

43、3.15)令，则112(1,4),(3,4)kLa kL * MERGEFORMAT 22222122323324342c2kkaa as a dad(3.16)令，则22222122342()/(2)kkkaada * MERGEFORMAT (3.17)3433c as dk通过三角代换即可求解出：3 * 22233443Atan2(,)tan2( ,)a dakdakMERGEFORMAT (3.18)2.3.3. 求解关节角 theta2的求解是通过先求解出，然后又已知，相减就可以求解。2233由 * MERGEFORMAT (3.19)012163451230456()TTTTTTT

44、即基于 Matlab 的六关节机器人运动学仿真19 * 1 231 23232 31112131 231 23232 321222336113313233000010001xyzc cs csa crrrpc ss sca srrrpTscdrrrpMERGEFORMAT (3.20)令：，即：(1,4)(1,4); (2,4)(2,4)LRLR * y 1231231231 23z232331231 231 23z232 3y 1 234s c +c c - a c +d s - s - a cd c - + a s - + a s - xxpppap c sp cp s sdMERGEFO

45、RMAT (3.21)联立求解可得：2323,sc * 11142 3132 323221111142 311132 323221111()()()()()()()()()()()()xyzxyzzxyxyzc ps pada sdpaa csc ps padpdpda sc ps paaa ccc ps padpMERGEFORMAT (3.22)所以可得 * 2311142 3132 3142 311132 3tan2( ()()()(),()()()()xyzzxyac ps pada sdpaa cdpda sc ps paaa cMERGEFORMAT (3.23)则由和组成的四种解

46、的组合，也可以求出的 4 个可能解132 * MERGEFORMAT (3.24)22332.3.4. 求解关节角 theta4由上面的结果知，、是已知的，即下式等式左边是已知的。123 * 1 231 23232 31112131 231 23232 321222336113313233000010001xyzc cs csa crrrpc ss sca srrrpTscdrrrpMERGEFORMAT (3.25)则令，即：(1,3)(1,3); (3,3)(3,3)LRLR机械工程 三班 高杰 20 * MERGEFORMAT (3.26)x123z23y 1234 5y114 5c c

47、 - s + s c-c sc -ss sxaaaaa得 * MERGEFORMAT (3.27)1y 14x123z23y 123sctanc c - s + s cxaaaaa若，可得：50 * 11123123234tasc ,c cs c2(sn)yxxyzaaaaaaMERGEFORMAT (3.28)是否为奇异位可由上式的两个参数是否都接近零来判断。若都接近零，则为奇异位，否则不是奇异位，处于奇异位的时候，可以任取的值，在计算相应的的值，通常采用轨迹46前一时刻的的值作为当前的解。442.3.5. 求解关节角 theta5由公式 * MERGEFORMAT (3.29)01231

48、6451234056()TTTTTTT令，即：(1,3)(1,3); (3,3)(3,3)LRLR * 1 23 41 423 41 41 42351 23231 235()()yzxyzxas c cc sa s ca s sc c csa s sa ca c sc MERGEFORMAT (3.30)得: * MERGEFORMAT (3.31)555tan2(s ,)ac2.3.6. 求解关节角 theta6由公式 * MERGEFORMAT (3.32)012341651234506()=TTTTTTT令，即：(1,1)(1,1); (3,1)(3,1)LRLR* 54 1 234 1

49、1 23 554 1 234 11 23 55 4 2323 564 1 234 141234 14 236( ()( ()()()()xyzxyzn c c c cs sc s sn c c s cs cs s sn c c sc scn s c cc sn s s cc cn s ss MERGEFORMAT (3.33)得： * MERGEFORMAT (3.34)666tan2( ,)as c基于 Matlab 的六关节机器人运动学仿真21一般在实际应用中，当前机器人关节轴的解通常选择距离当前关节变量距离最小的一组。2.3.7. 逆运动学实例为了验证上述推到的正确性，我们进行简单的验证

50、，已知：10000.3631-0.6407-0.19060.74380.91160.5736-0.7628-0.2986-0.0931-0.5105 0.6179-0.598006T则根据公式（3.12）得：1=169.1230002.0, 0atan2-)0931.09116.0(2tan）（，a根据公式（3.18）得：330.286179. 0,3631. 0atan2-)2305. 11750. 0(2tan3）（，a根据公式（3.23）得：23 19.745106. 01750. 01750. 001906. 00230. 02305. 1.00260-.495006407. 0175

51、0. 05105. 04950. 05736. 02305. 11750. 006179. 02tan23）（，a 根据公式（3.24）得：89.4530.2819.742根据公式（3.27）得：488.170251.0-0325.0245.04258.04168.02tan4，a根据公式（3.30）得：577.91)2258.04528.0(2tan5，a根据公式（3.33）得：653.6)3625.02514.0(2tan6，a经过简单的计算我们得出此时六关节机器人的关节角参数为-123.69，45.89，28.30，17.88，-91.77，6.53我们可以初步认定上述的六关节机器人逆运

52、动学的推导过程是正确的在后面的第三章会用仿真软件进一步验证。机械工程 三班 高杰 22第第 3 章章 机器人运动学仿真分析机器人运动学仿真分析根据前面两章讨论的 ABB IRB 4600-20/2.5 机器人的运动学问题，并且对比分析了第二章、第三章的机器人正逆运动学求解结果，进一步验证结果的正确性。3.1. MATLAB 软件基础MATLAB 软件主要用于算法的快速开发和验证、以及数据可视化等方面，在工程分析中具有不可替代的地位。MATLAB 主界面如下图所示，最上面为主菜单栏，左边的当前文件夹和工作空间栏，还有右边的命令行窗口栏。当前文件夹内的.m 文件是可以随时运行的，工作空间部分可以让

53、我们方便的查看定义的变量的，利于调试和检查错误，而命令行窗口是可以一行一行输入命令，MATLAB 立即显示结果。图 4.1 MATLAB 主界面主页界面主要有“新建脚本”等命令，对 第二三章的运动学推导公式的程序化编写就是通过新建脚本文件完成的，绘图界面有”plot”等命令，可以方便可视化程序中的变量 ，而 APP界面是 MATLAB 集成的一些模块，便于快速验证算法的正确性。图 4.2 matlab 不同菜单栏基于 Matlab 的六关节机器人运动学仿真233.2. ROBOTICS TOOLBOX 机器人工具箱介绍Robotics Toolbox 工具箱包括了常用的一些机器人运动学、动力学

54、仿真模块，不同的用户可根据自己的实际需求，选取相应的模块，对机器人进行研究， ，因此可以最大程度上满足不同用户的需求。 图 4.3 机器人工具箱主页 Robotics Toolbox 工具箱特点如下： （1）该工具箱代码是完全开源的。（2）工具箱内含有针对多种机器人的算法代码。（3）算法包含了位姿变换、正运动学、逆运动学、动力学、控制等多种算法。（4）内置算法可以快速验证推导公式的正确性，提高效率。3.3. 运动学建模下面是建模流程：下载 Robotics Toolbox 机器人工具箱（.zip 格式） ，解压后会有一个 rvctools 文件夹，里面包含了机器人运动学、动力学、轨迹规划等仿真

55、所需的文件。将该文件夹存放到电脑 MATLAB 的 toolbox 文件下，如：D:Program FilesMATLABR2018atoolbox。然后在 MATLAB 的主页下找到“设置路径”按钮，打开“设置路径”按钮后，会弹出如下图所示对话框，选择“添加并包含子文件夹”按钮，然后找到上面放到 MATLAB 的 toolbox 文件下的 rvctools 文件，保存并关闭此对话框。机械工程 三班 高杰 24图 4.4 文件框为了验证是否安装成功机器人工具箱，可以在 MATLAB 命令行窗口输入 startup_rvc，会得到下图所示的内容，表明 Robotics Toolbox 机器人工具

56、箱安装成功。借助 Robotics Toolbox 机器人工具箱里面成熟的算法框架，只需要在其 Revolute()函数中填写正确第二章建立的关于 ABB 4600 机器人 DH 连杆参数表即可。编写的程序如下图所示。基于 Matlab 的六关节机器人运动学仿真25图 4.5 程序在 MATLAB 中打开编写好的正运动学程序 ABB_irb4600_mdh.m（该程序中 d 表示连杆篇置，a 表示连杆长度，albha 表示连杆扭转角，offet 表示偏置，modified 为修改后的 DH 表示是法），按下键盘上的 F5 键运行程序，可以在 MATLAB 工作空间中得到如下变量，图 4.6 工

57、作空间在 MATLAB 命令行窗口输入 irb4600.plot(ql)命令，得到如下图图形图 4.7 结果如果在 MATLAB 命令行窗口输入 irb4600.teach()命令，得到如下图图形，可以通过控制左侧的滑块来调整机器人的位姿，从而快速验证算法。其中 X、Y、Z 分别代表位置，R、P、Y 代表姿态，q1 到 q6 代表关节角的值。机械工程 三班 高杰 26图 4.8 结果姿态图 4.9 姿态结果3.4. 正运动学仿真验证国外学者建立的 Robotics Toolbox 机器人工具箱是一个基于 MATLAB 快速验证机器人算法的库，通过运动学仿真，我们可以检验理论推导的机器人正运动学

58、方程和逆运动学方程基于 Matlab 的六关节机器人运动学仿真27的正确性，本节依据该库的调用格式编写了针对 ABB IRB 140 机器人的运动学算法，如下图所示，该部分程序主要是依据 DH 参数规则建立各连杆之间的联系，程序能够用于正运动学和逆运动学算法验证。图 4.10 基于机器人工具箱的算法程序验证正运动学算，需要借助 fkine 函数，首先在 MATLAB 中按下键盘上的 F5 键运行正运动学程序 ABB_irb4600_mdh.m，可以在 MATLAB 工作空间中得到该函数生成的变量，然后在 MATLAB 命令行窗口输入 irb4600.fkine(ql)命令，也即是求解机器人在关

59、节角为0,pi/2,0,0,0,0下腕点的位姿。结果如下图所示。图 4.11 机器人工具箱结果当然也可以在 MATLAB 命令行窗口输入 irb4600.teach(ql)命令，得到如下图所示的可视化图形机械工程 三班 高杰 28图 4.12 机器人工具箱可视化结果从以上两种结果可以看出，机器人工具箱求取的结果是相同的，为了验证第二章正运动学方程推导的正确性，在 MATLAB 中编写 forward_kine_AbbIrb_4600.m 函数，如下图所示，该函数通过连杆变换矩阵求解机器人末端位姿。图 4.13 程序在 MATLAB 命令行窗口输入 forward_kine_AbbIrb_460

60、0(0 ,-pi/2 ,0 ,0 ,0 ,0)命令，得到如下图所示结果。基于 Matlab 的六关节机器人运动学仿真29图 4.14 正运动学算法结果通过对比发现，机器人工具箱结果和第二章推导的正运动学算法结果两者是相同的，从而验证了推导的正确性。3.5. 逆运动学仿真验证验证逆运动学算法，同样需要借助 ikine 函数完成求解，首先在 MATLAB 中按下键盘上的 F5 键运行运动学程序 ABB_irb4600_mdh.m，可以在 MATLAB 工作空间中得到该函数生成的变量，然后在 MATLAB 命令行窗口输入 irb4600.ikine(0 0 1 1.4055;0 -1 0 0;1 0