634522416毕业设计（论文）机械手逆运动学分析、仿真及轨迹规划.doc

《634522416毕业设计（论文）机械手逆运动学分析、仿真及轨迹规划.doc》由会员分享，可在线阅读，更多相关《634522416毕业设计（论文）机械手逆运动学分析、仿真及轨迹规划.doc（73页珍藏版）》请在三一办公上搜索。

1、毕业设计（论文）设计(论文)题目机械手逆运动学分析、仿真及轨迹规划姓 名：学 号：学 院：机电与信息工程学院专 业：机械设计制造及其自动化年 级指导教师： 毕业设计（论文）开题报告论文题目：机械手逆运动学分析、仿真及轨迹规划姓 名：学 号：学 院：机电与信息工程学院专 业：机械设计制造及其自动化年 级：指导教师：一、 课题来源随着机械及其控制科学的发展，具有模拟人体手臂运动功能，用以完成按固定程序抓取、搬运物件操作的机械手应运而生。它不但可以实现生产的机械化和自动化，而且能够在特殊环境下工作以保护人身安全，因此它广泛应用于机械制造、冶金、电子、轻工和原子能等部门。为了适应先进社会生产力的发展要

2、求，我们应对其原理技术有更深的了解和掌握。然而由于设备、专业知识储备以及时间的限制，此课题仅针对机械手的逆运动学分析、逆雅可比矩阵求解及轨迹规划进行理论性研究。机器人的基本运动学分析可分为正运动学分析与逆运动学分析两个范畴：所谓正运动学，就是解决将运动参数由关节空间向操作空间映射的问题；而所谓逆运动学，就是解决将运动参数由操作空间向关节空间映射的问题。换言之，正运动学是结果逻辑，而逆运动学是条件逻辑。机器人的轨迹规划是通过具体的技术手段使机器人端部执行机构按预定的轨迹实现实时运动。本课题正是通过Matlab编程的方法实现机器人逆运动学分析及轨迹规划两大问题运算结果的可视化。二、 本课题的基本内

3、容1. 了解关于机械手的相关知识、表示术语等；2. 学习并掌握机械手逆运动学的分析方法坐标变换法；3. 研究机械手的逆雅可比矩阵的求解方法；4. 基于Matlab实现教学机械手逆运动学编程，并封装；5. 研究直线运动和圆弧运动轨迹规划算法及基于Matlab的编程实现。三、 本课题的重点和难点1. 编制出实现机器人运动学分析的Matlab程序，实现运算结果的可视化；2. 系统的正确封装；四、 论文提纲1. 绪论；2. 机械手概况；3. 根据机械手的模型图建立D-H坐标系；4. 确定机械手连杆及关节参数；5. 确定相邻两连杆间的坐标变换矩阵及其逆变换；6. 为所研究的机械手做正运动学分析；7. 为

4、所研究的机械手做逆运动学分析；8. 研究机械手对应形位逆雅可比矩阵的算法；9. 在关节空间实现机械手的轨迹规划；10. 用Matlab实现上述操作并进行仿真验证；11. 参考文献，附录及谢辞。五、 进度安排2011-3-262011-3-31. 了解机器人学基础知识，学习机器人运动学分析、雅可比矩阵及轨迹规划的相关理论知识；2011-4-012011-4-15. 对所研究的机械手进行运动学理论计算，并为其设计出合适的轨迹规划方案；2011-4-162011-4-24. 阅读Matlab相关书籍，初步掌握Matlab相关操作，寻找理论与程序二者的结合点并完成对该系统的程序编制工作，与此同时，对前

5、面理论计算之不合理处做出必要的修改；2011-4-252011-5-10. 有步骤，有计划地完成对所编程序的纠错调试工作，在调试程序的过程中逐步形成论文；2011-5-112011-5-25. 修改论文；2011-5-262011-5-27. 准备答辩。指导教师意见：（请手写意见和签名）（对本课题的深度、广度及工作量的意见）指导教师：（签字）年 月 日教研室审查意见：（请手写意见和签名） 教研室负责人：（签字） 年 月 日毕 业 论 文 开 题 报 告毕业设计（论文）任务书（由理工科指导教师填写）学 生 姓 名李金铭学号20078003020指导教师宁淑荣设计（论文）题目机械手逆运动学分析、仿

6、真及轨迹规划（13506304895）主要 研究 内容 本设计题目针对机械手逆运动学分析及仿真。拟定主要研究内容：了解关于机器人的相关知识、表示术语等；学习并掌握机器人逆运动学的分析方法；研究机械手的逆雅可比矩阵的求解算法；基于Matlab实现教学机器人逆运动学编程，并封装；研究点位运动和直线运动轨迹规划算法及基于Matlab的编程实现研 究 方 法在明确机械手的本体结构及位姿表示方法的基础上，利用坐标变换法实现其运动学逆解分析；利用Matlab软件实现逆运动学编程；学习机器人轨迹规划及插值算法相关知识以实现机械手直线和圆弧轨迹设计及基于Matlab的编程实现主要技术指标(或研究目标)正确给出

7、教学机器人的运动学逆解算法；编制出实现机器人运动学分析的Matlab程序，实现运算结果的可视化。主要 参考 文献1 庞淑娟 倪受东. 五自由度教学机器人的运动学分析及仿真J. 现代制造工程-2007,62 程永伦 朱世强. 基于Matlab的QJ-6R焊接机器人运动学分析及仿真. 机电工程-2007,113 蔡自兴. 机器人学M. 清华大学出版社2000.4 尚涛. 工程计算可视化与MATLAB实现M.毕业设计（论文）成绩评定表学院： 机电与信息工程学院 专业：机械设计制造及其自动化 年级： 07级姓名李金铭学号20078003020设计（论文）总成绩设计（论文）题目机械手逆运动学分析、仿真及

8、轨迹规划指导教师评语评定成绩： 60%= 签名： 年 月 日答辩 小 组 评 语答辩成绩： 40%= 组长签名： 年 月 日目 录摘 要- 1 -Abstract- 1 -一、绪论- 3 -(一)机械手概述- 3 -(二)课题研究的目的、意义及主要研究内容- 4 -二、机械手整体分析- 4 -(一)机械手简介- 4 -(二)机械手的D-H坐标系- 6 -(三)机械手的连杆及关节参数- 6 -(四)机械手相邻两连杆间的坐标变换矩阵及其逆变换- 7 -三、机械手运动学分析- 8 -(一)正运动学分析- 8 -(二)逆运动学分析- 11 -四、机械手逆雅可比矩阵算法- 15 -五、机械手轨迹规划-

9、18 -(一)点位运动- 18 -(二)直线运动- 18 -(三)圆弧运动- 19 -六、机械手Matlab仿真验证- 22 -(一)正运动学程序仿真验证- 22 -(二)逆运动学程序仿真验证- 23 -(三)逆雅可比矩阵程序仿真验证- 25 -(四)直线运动轨迹规划程序仿真验证- 26 -(五)圆弧运动轨迹规划程序仿真验证- 28 -七、结论- 32 -参考文献- 33 -附 录- 34 -(一)正运动学分析程序- 34 -(二)逆运动学分析程序- 39 -(三)雅可比矩阵求解程序- 47 -(四)空间直线运动轨迹规划插值运算程序- 50 -(五)空间圆弧运动轨迹规划插值运算程序- 52 -

10、(六)机械手逆运动学运算系统封装程序- 54 -谢 辞- 64 -摘 要随着社会的发展和科技的进步，作为高科技一个重要研究领域的机器人技术，业已受到各国政府的重视。机器人及其相关技术已是机械制造业中不可缺少的重要装备和技术手段，机器人技术发展水平也成为衡量一个国家机械制造业发展水平与科技创新水平的重要标尺。显而易见，机器人技术研究的目的在于采用仿生学原理，通过机械方法模拟人体器官的运动，使之能实现预定的运动形式，代替人力从事生产劳动，将人类从繁重危险的劳动作业中解放出来，从而实现劳动生产率的大幅度提高。故机器人研究的重点在于使所研究的机构按照预定的运动方式与运动状态发生运动。鉴于此，机器人的运

11、动分析显得尤为重要，它为对机器人进一步进行静态特性分析、动力学分析、设计及轨迹规划奠定了基础。本文采用坐标变换的方法对机器人的正运动学、逆运动学及雅可比矩阵的求解做出了整体性研究，同时对在关节空间内采用样条曲线插值的方法进行轨迹规划做出了理论性研究，为机器人的Simulink机械仿真做出了理论上的准备。关 键 字运动分析；逆雅可比矩阵；轨迹规划；MatlabAbstractWith the development of society and the progress of science, robotics，as an important sphere in high technology

12、researching, is paid attention to by governments all over the world. It has become a significant benchmark measuring the technological level of an economy, as well as an indispensable and technical theory in the manufacturing industry. To liberate human from the heavy and dangerous labour, obviously

13、, robotics research aims to make the machine achieve the predetermined motion by adopting the principle of bionics to simulate the movement of human organs. As a result, we could use robot to improve the labor productivity greatly. Therefore the key of robotics research is to make the manipulator to

14、 move in the predetermined movement state. In view of this, the robotic motion analysis, which lays a foundation for the statics analysis, dynamics analysis, designing and trajectory planning of robot, appears particularly vital. This thesis makes an overall study of the solution of normal kinematic

15、s, inverse kinematics and Jacobian matrix of robot by means of coordinate transformation, and systematically researches the trajectory planning by means of joint interpolated motion. These jobs could make theoretical preparations for the researches of using modules in SimMechanics to found the right

16、 model of mechanical hand, getting the inverse solution under the condition of Matlab/Simulink, and doing the graphical simulation with the 6 DOF mechanical hand in 3D form by SimMechanics.KeywordsKinematic analysis; Inverse Jacobian matrix; Trajectory planning; Matlab一、 绪论(一) 机械手概述我国科学家对机械手的定义是：“能模

17、仿人手臂的某些动作功能，用以按固定程序抓取、搬运物件或操作工具的自动操作装置。”它是一种由手臂、关节、传动执行装置和末端执行装置(工具等)构成的，各构件相互连接相互依赖的开式运动机构，是机器人的一种。不同的机械手具有不同的结构类型，大多数机械手是具有几个自由度的关节式机械结构，从理论上讲，任何功能不明确的机械手至少应有六个自由度。机械手按关节的联接方式可分为串联机械手和并联机械手1，如图1、2： 图1 串联机械手 图2 并联机械手其中串联机械手的发明和实用比较早，主要有以下特点：1.结构紧凑，能动性和灵活性高，具有广阔的工作空间；2.控制操作简洁，是进行运动规划和编程系统设计的理想对象；3.在

18、运动求解上，串联机构正解容易而反解困难2。相对于串联机器人来说，并联机器人具有以下特点2：1.刚度大，结构稳定，承载能力强2；2.运动惯性小，精度高2；3.在运动求解上，并联机器人正解困难而反解容易2。目前，机械手的发展动向主要有以下几个方面3：高速操作臂它的使用可以大大提高机器人的工作效率，为此，必须开展对新的手腕机构和伺服驱动装置，以及能适应机械臂高速运动的变转动惯量动态控制方法等的研究。4柔性操作臂目前的操作臂机身质量较它所能抓起的质量要大的多，如机身质量为30kg的操作臂，所能搬运物体的质量尚不及10kg，这个比例与人体手臂相比要小的多。其主要原因有两点：一是驱动装置拥有较大质量；二是

19、机身未采用轻型材料。4冗余自由度操作臂要实现狭小空间的操作，研制超多自由度的机械手是完全必要的。4高精度、多自由度的力控制操作臂基于位置控制的力控制系统适用于大减速比的工业机器人操作臂；基于力矩控制的力控制系统适用于常用动力学研究的低减速比或者直接驱动型机器人操作臂。微型操作臂主要指微操作机器人的研究范畴。5(二) 课题研究的目的、意义及主要研究内容如上文所言，理论上讲，任何功能不明确的机械手至少应具有六个自由度，本文的研究工作正是围绕六自由度串联机械手展开的，该类型的机械手广泛用于工业生产中，若将其固定在设备旁边，便可完成机械加工过程中的上、下料作业，通过输送线运送工件，实现物流的运转，其次

20、它还可以完成一些物流运转过程中人力难以实现的搬运作业，大幅度提高工作效率。由此可知，机械手研究重点应是让所研究的机械手按照预定的运动方式与运动状态发生运动。鉴于此，机器人的运动分析显得尤为重要，它为对机器人进一步进行静态特性分析、动力学分析、设计及轨迹规划奠定了基础。此外，机器人的运动分析为机器人的自动控制研究提供了重要的理论依据并为其指明了方向，同时为机械手控制系统类型的选用及功能的设计提供了理论依据与现实依据。本论文主要包括以下内容：1. 对本文要研究的机械手从整体上做出了分析并根据约定为其建立了D-H坐标系；2. 用坐标变换法对本文要研究的机械手分别进行了正运动学分析与逆运动学分析6；3

21、. 用微分变换法研究了机械手逆雅可比矩阵的求解7；4. 用直角空间插值法研究了机械手的轨迹规划并对在关节空间内实现机械手的轨迹规划做了相关探讨；5. 用Matlab程序实现机器人运动学的上述运算并为机械手的三维图形仿真奠定了基础8。综上所述，本文的研究目的在于通过坐标变换的方法对机器人运动学进行整体性研究9。机器人是一种由多个构件及运动副构成的，可以从事特定生产作业的机构10。从物理学的角度可以知道，当选用系统为研究对象时，有必要对系统内部各部分之间的相对运动和相互作用进行研究。自然而然，各构件及运动副之间的相对运动的研究成为机器人运动学研究的精髓11。而坐标变换法正是研究相对运动的一大法宝，

22、因此，对机器人运动学进行研究宜采用坐标变换法8。二、 机械手整体分析(一) 机械手简介这部分目的在于利用机械原理的知识从整体的角度探讨所研究机构的各构件间相对位置关系及相对运动关系。本文所研究的是RBT-6T/SO1S机器人，根据该机械手的实际结构可以得到它的实际运动模型图，如图3所示12：图3 RBT-6T/SO1S机器人实际运动模型图通过图3所反映的运动信息，据机械原理的相关知识，可以画出此机械手的运动简图，如图4所示：图4 RBT-6T/SO1S机器人运动简图由上图可知，RBT-6T/SO1S机械手的自由度为13其中表示自由度数，为所研究机构包含的活动构件数，为所研究的机构包含的空间五级

23、副数目，为所研究机构包含的空间四级副数目，为所研究机构包含的空间三级副数目，为所研究机构包含的空间二级副数目，为所研究机构包含的空间一级副数目。14一般而言，机械手之自由度数与其所包含的关节数目相等，这是因为对机械手的关节而言，只允许一种运动方式的存在。通过上面的论述我们可以知道，RBT-6T/SO1S机器人是包含六个连杆，六个转动关节，零个移动关节的，自由度数为六的串联机械手。其六个自由度分别对应于转动基座、大臂、肘、小臂、腕等六个关节之转轴，每个转轴均可在大角度范围内旋转运动。它是一个多输入，多输出的动力学复杂系统，故在对它进行动力学分析时，宜采用分析力学之法。(二) 机械手的D-H坐标系

24、这部分目的在于利用相关约定为本文所研究的机械手建立D-H坐标系。本文所研究的机器人有六个关节 ，且六个关节都是转动关节，15其中关节1，2的轴线垂直交于一点，关节2，3的轴线相互平行，关节3，4的轴线垂直交错，关节4，5，6的轴线交于一点，8根据转动坐标系的设定原则及所研究机器人的实际运动模型图确定各连杆所对应的坐标系如图5所示【注】转动坐标系的设定原则为：连杆的坐标系原点位于关节和轴线的公法线与关节轴线的交点上；如果两相邻连杆的轴线相交于一点，那么原点就在这一交点上；如果两轴线相互平行，那么就选择原点使对下一连杆（其坐标原点已确定）的距离为零；连杆的轴与关节的轴线在同一直线上，而轴则在连杆和

25、的公法线上，其方向从指向；当两关节轴线相交时，轴正向与共线；当两轴与同向时，第个转动关节的为零。16图5 RBT-6T/SO1S机器人D-H坐标系图(三) 机械手的连杆及关节参数这部分目的在于根据机械手D-H坐标系确定机械手运动学分析的必要参数。机械手的连杆及关节参数如下表所示：连杆1000290003004905-900069000【注】各参数的含义及判定方法如下：为连杆夹角，为轴正向与轴正向的夹角在坐标系中的值；为连杆扭角，为轴正向与轴正向的夹角在坐标系中的值；为连杆长度，为坐标系之坐标原点与坐标系之坐标原点在轴上的距离；为连杆距离，为坐标轴与坐标轴之距离。17(四) 机械手相邻两连杆间的

26、坐标变换矩阵及其逆变换这部分目的在于依据机械手连杆及关节参数确定机械手运动学分析所需要的坐标变换矩阵。通过上文所列之表可以确定相邻两连杆间的坐标变换关系，分析知，从坐标系到坐标系的坐标变换矩阵的通式为。16显然，R为正交阵，是以。而在与两矩阵均可逆的情况下，有等式成立。证明：设与均为可逆阵，则有且，从而有。是故，。【注】选用坐标变换的方法对机械手进行研究的理由有两点：其一，如上文所言，机械手运动学研究的精髓在于用相对运动的观点对机械手各连杆之间的运动关系及机械手端部的运动状态进行分析，而坐标变换则是研究相对运动的一大法宝；其二，利用坐标变换法对机械手的运动状态进行研究，可以为用微分变换法构造机

27、械手的雅可比矩阵奠定基础。由以上的公式可以推算出以下结果：；。及；。三、 机械手运动学分析(一) 正运动学分析这一步目的在于求出机器人的运动学方程，实现运动学参数从关节空间到操作空间的映射，并求出。； ； ； ； 通过计算可以求得： ； ； ； ；。其中，上式中即为机械手的运动学方程。(二) 逆运动学分析此步据机械手端部位姿反解机械手关节变量，从而实现运动参数从操作空间到关节空间的映射。设机械手在基坐标系中的位姿为。求由机械手的运动学正解可知，机器人的运动学方程矩阵中无一元素是常数，而前文所求的坐标变换矩阵中第二行，第四列之元素为常数0，故用左乘得，求第二行第四列元素并建立以下等式，由可得的封

28、闭解，。求在确定之解后，再求得之第一行第四列元素与第三行第四列元素，令其分别与之对应元素相等，从而建立等式，取以上，式之平方和得，其中在此式中。，而，考虑到为封闭解，故有两解。求在确定之解后，再用左乘得，。利用上面的方法可得，求得。其中与的分母相等且皆为正，考虑到为封闭解，而有两解，故由此可得有两解，有而则取与，相对应的解。求当确定，及的值后，可以据此求解，再求出之第一行第三列元素与第三行第三列元素，分别建立等式，当时，可求得之封闭解，即。求在确定的解之后，可据此求出，用左乘得，用上法得，；。故由此可得的封闭解：。求在确定的解之后，可据此再求，用左乘得，用上法得，；。故由此可得之封闭解：。上文

29、中所求，与是建立在的条件之下的，接下来我们探讨的情况，当时，易知。当时，有。则，而，故。用左乘有，。从而得等式；。从而得，与的分母相等，且皆为正，由此可得的封闭解为：。当时，有。则，而，故。用左乘有，。从而得等式；。从而得，与的分母相等，且皆为正，由此可得的封闭解为：。故当时，机械手处于奇异形位，关节轴4与6重合，此时只能求出或。16具体地讲，当时，我们可以解出；当时，我们只能解出。奇异形位可由当时计算式中两个变量是否都接近于0来判别：若都接近于0，则为奇异形位；否则便不是奇异形位。至于则可根据下式结果的正负性来判别，在奇异形位时，可任意选择之值，再求。四、 机械手逆雅可比矩阵算法16此步目的

30、在于研究机械手雅可比矩阵算法及其逆解。要求机械手的雅可比矩阵，就要把一个坐标系内的位姿的微小变化，变换为另一个坐标系内的等效表达式。设坐标变换的微分变换为，相对于基础坐标系为，相对于坐标系为，则有且，是以，从而得下式，由此可进一步求得，。其中，而下面，我们来解释一下机械手的雅可比矩阵。设机器人操作空间中的微分运动为，关节空间中的微分运动为，则对于自由度为的机械手而言，有。通过数学分析方法进行的理论计算可知，有关系式成立，则称为机器人的雅可比矩阵。由此可见，机器人的雅可比矩阵可以实现微分运动从关节空间到操作空间的映射。将式两边同除以，得，因此，雅可比矩阵同时还实现了速度从关节空间到操作空间的映射

31、。其中，而，。将式再做变形如下 由此可以得出结论，机械手雅可比矩阵的第列可以将坐标系中的微分运动变换到操作空间中。这一点，为使用坐标变换法计算机械手的雅可比矩阵提供了可能性。综合以上两点，可以确定一个自由度为的机械手的雅可比矩阵必为阶矩阵，对于转动关节而言，其第列可表示为而若，则。故用微分变换法构造机械手雅可比矩阵的思路为：1. 计算两相邻连杆间的坐标变换矩阵；2. 计算各连杆到末端连杆的坐标变换；3. 计算的各列元素，其中第列由决定。由此可得本文所研究的机械手的雅可比矩阵为：。其中；。对于机械手某一确定形位而言，在求出其所对应的雅可比矩阵之后，当首先判断其是否满秩，若满秩，再求其逆（本文所研

32、究之机械手为六自由度机器人，故其雅可比矩阵恰好为方阵），以实现微分运动从操作空间到关节空间的映射；若不满秩，则说明此雅可比矩阵所对应的机械手形位为奇异形位，此处不可逆。选择此方法求机械手的逆雅可比矩阵的理由在于：本文所研究的机械手存在奇异形位，此处所对应的雅可比矩阵不可逆，也就是说，对于某一特定机械手而言，其逆雅可比矩阵未必存在，故倘若直接求其逆雅可比矩阵，会使问题变得复杂起来，况且求矩阵的秩，求矩阵的逆之类的运算在Matlab语言环境下可以很轻松地实现。【注】关于用雅可比矩阵进行机械手速度反解与加速度反解的一点说明：1. 在知道机械手手爪端部运动速度的情况下，对于所研究机械手之非奇异形位，可

33、通过式反解得到机械手关节的运动速度。其中为机械手手爪端部的运动线加速度与角加速度组合而成的六维列向量；2. 对于机械手的运动加速度反解，同样要求所研究机械手的形位为非奇异形位，在知道机械手手爪端部运动加速度的情况下，将式两边对时间求导得，则，由此式可反解得到机械手关节的运动加速度。其中为机械手手爪端部的运动线加速度与角加速度组合而成的六维列向量，而。根据矩阵分析的理论可知，若，则在对求导时有下式成立，。五、 机械手轨迹规划17本文所提出的机械手轨迹规划方案为在关节空间内进行，其具体思路为：1. 利用几何关系，寻找各个插值点的坐标，并求机械手手爪在各个插值点处的运动速度与加速度，确定机械手端部在

34、各个插值点处的位姿；2. 利用机器人学的相关知识（主要是逆运动学分析与逆雅可比矩阵计算），将上面所求的机械手手爪的位姿、速度与加速度转化为各关节的位姿、速度与加速度；3. 将上面所求的各关节运动参数进行样条曲线拟合插值。【注】设定插补周期为，对点位运动设定其运动时间为，对直线运动和圆弧运动设定其运动速度为。(一) 点位运动1. 设定起始点与终止点；2. 分别对起始点与终止点进行逆运动学计算得及；3. 分别对进行线性插值。(二) 直线运动1. 设定起始点与终止点；2. 计算直线长度，机械手手爪在各点处的运动速度，插补步长，插补总步数及插补运动中各坐标轴的增量、和：；，故；、和。3. 由此可以求得

35、各插值点的坐标值：；及。4. 分别对各插值点进行逆运动学计算，并求其对应的逆雅可比矩阵，得。5. 对所得的关节变量进行三次多项式插值：在之间，令，；令，则且故由可以解得。由此逐段插值得到的样条曲线必光滑。(三) 圆弧运动1. 设定起始点，中间点及终点；2. 首先计算出，三点所确定圆弧的半径和圆心坐标：(1)半径 先计算出的三边长，。 令。 。(2)圆心据几何关系可知，点满足两个条件：其一，点位于，三点所确定的平面上；其二，点到，三点的距离相等且皆为。根据解析几何的知识，我们可以求得，三点所确定的平面的方程为，其中有；故满足方程组化简得，其中。令，。分析知，方程组有唯一解，故。3. 建立插补直角

36、坐标系，并确定其与基础坐标系之间的坐标变换关系：以为原点，以方向为轴，以方向为轴，建立插补直角坐标系，而基础坐标系为。下面求两坐标系间的变换关系。据前文可知，而。而正方向为，正方向为。是以若轴正向与轴正向之间的夹角为，则有且；若轴正向与轴正向之间的夹角为，则有且。由到的坐标变换矩阵为。故易知由到的坐标变换矩阵为。4. 分别将起始点齐次坐标，中间点齐次坐标及终点齐次坐标左乘，将其变换为坐标系中的坐标值，分别为起始点，中间点及终点。将其简写为，及。5. 计算总圆心角，插补步长，插补总步数及在坐标系中观察所得的机械手手爪在各点处的运动速度：在此坐标环境下，圆弧圆心位于坐标原点，而圆弧半径在前文已经求

37、出，故，从而有；根据可判断是否到插补终点若，则继续插补下去；，则修正最后一步步长为。令，则有。在基础坐标系中观察，有，其中、，而。6. 计算各插值点的坐标值：易知为绕轴旋转角度得到的，即有。由此可得。7. 分别将与都左乘坐标变换矩阵，将其变换为基础坐标系中的位置与速度，分别为与，。从而，。8. 分别对各插值点进行逆运动学计算，并求其对应的逆雅可比矩阵，得。9. 对所得的关节变量进行三次多项式插值：在之间，令，；令，则且故由可以解得。由此逐段插值得到的样条曲线必光滑。【注】关于轨迹规划插值运算中各插值点处加速度计算方法的一点说明：为了使机械手手爪端部在工作过程中能够保持平稳，免受冲击，故要求机械

38、手手爪端部在工作过程中做匀速运动，对直线运动而言为匀速运动，而对圆弧运动而言为匀速圆周运动，故有以下结论，1. 对于直线运动而言，机械手手爪端部的线加速度与角加速度均为0；2. 对于圆弧运动而言，机械手手爪端部的角加速度为0，故其切向加速度亦必为0，而其法向加速度可按照其线速度的求法进行求解，即令，则有。再分别将都左乘坐标变换矩阵，将其变换为基础坐标系中的加速度，分别为，。六、 机械手Matlab仿真验证根据上面的理论研究与计算过程，我们可以通过Matlab高级语言对机械手运动学分析计算与轨迹规划进行程序编制与仿真验证。机械手的Matlab运动学分析实现程序，将在附录中详细列出，下面对编程中要

39、注意的几个问题进行一下说明：1. 对数值类型进行划分是计算机高级语言的普遍特点，所以在Matlab编程过程中要特别注意数值类型的问题；2. 由于计算机计算误差的原因，在书写判断两数值是否相等的逻辑表达式时，宜采用数学分析中定义邻域的方式，这样可以得出正确的结果；3. 根据物理原则，较真值低两个数量级的误差值便可忽略，所以在判断两数值A与B是否相等时，只要判断A是不是在B的0.01邻域内即可；4. 在Matlab程序中，某语句之后缀以分号，该语句之运行结果便不在命令窗口中输出，否则便要在命令窗口中输出其运行结果。5. 因Matlab软件在脚本程序执行过程中可以以矩阵的形式保存所有变量，所以当多个

40、程序在同一命令窗口下执行时，必须保证这些程序中所涉及的变量具有相容性。下文分别列出六自由度串联机械手RBT-6T/SO1S之正运动学分析、逆运动学分析、逆雅可比矩阵求解及直线运动与圆弧运动插值法轨迹规划五段程序验证结果，这五段程序均是用脚本文件的形式编成，既可以在同一命令窗口中执行，又可以分别在多个命令窗口中执行。在本文最后，列出了六自由度串联机械手逆运动学分析系统封装程序。它分别同直线运动与圆弧运动插值程序配合使用，共同用于六自由度串联机械手三维图形机械动态仿真系统之中。该仿真系统的运行结果将再一次验证本文对六自由度串联机械手所做运动学分析及所提出的轨迹规划方案的准确性。下面便是各段程序之仿

41、真验证：(一) 正运动学程序仿真验证输入数值输出结果(二) 逆运动学程序仿真验证输入数值输出结果小结：上文所列乃六自由度串联机械手运动学分析计算程序（包括正运动学分析、逆运动学分析）的执行结果。将机械手正运动学运算程序的输出数值当作机械手逆运动学运算程序的输入数值，机械手逆运动学运算程序可以在误差允许范围内将机械手正运动学运算程序的输入数值输出。由此可见，本文提出的机器人运动学分析方法可以在误差允许的范围内对六自由度串联机械手做出包括位置，速度与加速度在内的正解与反解计算。(三) 逆雅可比矩阵程序仿真验证由上文分析可知，当接近于0时，本文所研究的机械手出现奇异形位，其自由度出现退化现象，此时机

42、械手的雅可比矩阵不可逆。小结：上文所列乃六自由度串联机械手逆雅可比矩阵计算程序的仿真验证结果。通过以上结果可以看出，本文提出的机器人运动学分析方法在误差允许的范围内可以对六自由度串联机械手做出逆雅可比矩阵的计算求解，同时该程序可以通过雅可比矩阵的可逆性对六自由度串联机械手的奇异形位做出相应的分析。(四) 直线运动轨迹规划程序仿真验证下面列出直线运动轨迹规划插值计算程序的执行结果，包括插值轨迹图（如图6）、关于X、Y、Z三个坐标轴的插值结果（如图7-9）与各插值点处的机械手手爪端部运动速度计算结果（如图10-15）与加速度计算结果（如图16-21）。图6 直线运动插值轨迹图 图7 直线运动X坐标

43、插值结果 图8 直线运动Y坐标插值结果 图9 直线运动Z坐标插值结果 图10 直线运动线速度X分量插值结果 图11 直线运动线速度Y分量插值结果 图12 直线运动线速度Z分量插值结果 图13 直线运动角速度X分量插值结果 图14 直线运动角速度Y分量插值结果 图15 直线运动角速度Z分量插值结果 图16 直线运动线加速度X分量插值结果 图17 直线运动线加速度Y分量插值结果 图18 直线运动线加速度Z分量插值结果 图19 直线运动角加速度X分量插值结果 图20 直线运动角加速度Y分量插值结果图21 直线运动角加速度Z分量插值结果小结：以上程序运行结果说明本文所提出的轨迹规划方案可以对机械手手爪端部的直线运动做出位置、速度与加速度的插值运算。(五) 圆弧运动轨迹规划程序仿真验证下面列出圆弧运动轨迹规划插值计算程序的执行结果，包括插值轨迹图（如图22）、关于X、Y、Z三个坐标轴的插值结果（如图23-25）与各插值点处的机械手手爪端部运动速度计算结果（如图26-31）与加速度计算结果（如图32-37）。图22 圆弧运动插值轨迹图 图23 圆弧运动X坐标插值结果 图24 圆弧运动Y坐标插值结果 图25 圆弧运动Z坐标插值结果 图26 圆弧运动线速度X分量插值结果 图27 圆弧运动线速度Y分量插值结果 图28 圆弧运动线速度Z分量插值结果