毕业设计（论文）-基于动作的多轴机械臂的运动控制分析.doc

诚信声明本人郑重声明：本论文及其研究工作是本人在指导教师的指导下独立完成的，在完成论文时所利用的一切资料均已在参考文献中列出。 本人签名： 年 月 日毕业设计任务书设计题目： 基于动作的多轴机械臂的运动控制分析 系部： 机械工程系 专业： 机械电子工程 学号： 112012144 学生： 周贵鹍 指导教师： 刘嘉(讲师) 1课题意义及目标本课题通过研究多轴机械臂底层控制程序的控制策略，基于体感装置的相关信号对机械臂实现控制。2主要内容(1)完成多轴机械臂控制程序的学习，编写多轴机械臂的控制程序，实现机械臂的特定运动。(2)完成体感装置与机械臂接口的程序编写及控制，实现体感装置信号向机械臂控制器的传输。(3)完成在体感装置上的动作变换与机械臂运动的匹配，实现机械臂基于动作的自动运行。(4)提交设计说明书一份，控制程序及运动仿真过程视频各一份。3主要参考资料1 濮良贵.机械设计（第八版）M.北京:高等教育出版社,2010.6.2 孙恒.机械原理（第七版）M.北京:高等教育出版社,2012.7.3 刘金琨.机器人控制系统的设计与MATLAB仿真M.北京:清华大学出版社,2008.6.4进度安排设计各阶段名称起 止 日 期1学习MATLAB语言并学会运用相关软件3月3日3月23日2完成机械臂控制程序的初步建立3月24日4月13日3完成体感装置与机械臂的协同工作4月14日5月4日4完成分析、数据比较并撰写设计说明书5月5日6月1日5完成毕业论文及答辩工作6月2日6月22日审核人： 年 月 日 基于动作的多轴机械臂的运动控制分析摘 要：随着科学技术的迅速发展，机器人也越来越多的出现在日常的生活中。其中多自由度机械臂是最常见的机器人之一。本文以实验室七轴智能化机器人机械臂为研究对象，使用了D-H法和牛顿迭代算法，在运动学和控制方面进行建模，并实现机械臂与计算机串口之间的通信。随后在CytonVeiwer平台上，借助手柄对机械臂进行操控，通过手柄输出命令控制机械臂的运动轨迹，并将运动轨迹导入到MATLAB软件中进行分析仿真，最终实现对机械臂运动更加精确地控制。关键词：七自由度,机械臂,运动学建模,手柄遥控Based on The Action of Multiracial Arm Control AnalysisAbstract：With the rapid development of science and technology, robot is more And more appear in our dairy life .the many degrees of freedom mechanical arm is one of the most common type of robot. In this paper, our Laboratory use the seven axis mechanical arm of intelligent robot as the reach object, using the D-H method and Newton Iteration algorithm, then Start modeling with the kinematics and control, finally realizes the communication of mechanical arm and the computer serial port then on CytonVeiwer platform ,with the hand shank to control of mechanical arm, by Using the output command of hand shank control the trajectory of mechanical arm, and the trajectory imported into the MATLAB software began to analyze of simulation, ultimately achieve more accurate control of the Mechanical arm movement.Keywords: Seven degrees of freedom, The mechanical arm, Kinematics modeling, The hand of remote control目录1 绪论11.1 多轴机械臂的研究意义11.2 国内外研究进展11.3 7轴智能化机器人机械臂介绍21.3.1 机器人的组成21.3.2 7轴智能化机器人手臂61.3.3 机器人的通信方式72 7轴智能化机器人机械臂建模与正运动学分析112.1概述112.2 D-H方法112.3 机械臂的建模与仿真152.4 正运动学分析182.5 逆运动学分析193 7轴智能化机器人机械臂的路径规划233.1 机械臂空间的认识233.2 关节空间路径规划的基本方法243.2.1 点到点路径的规划243.2.2 连续路径的规划263.3 操作空间路径规划的基本方法304 七轴机械臂与其他设备间通信串口的建立324.1 控制系统324.2 通信模块334.2.1 上位机编程334.2.2 下位机编程345 基于MATLAB、CytonViewer软件对机械臂动作的仿真365.1 MATLAB动作仿真365.1.1 通信测试与设置365.1.2 路径的规划365.1.3 状态与控制375.2 CytonViewer动作仿真385.2.1 Jobstick插件385.2.2 RecordPlayback插件385.3 本章小结396 结论与展望416.1 研究工作总结416.2 后续工作展望41参考文献42致谢4343 太原工业学院毕业设计1 绪论1.1 多轴机械臂的研究意义 机器人领域集中了机械工程、电子工程、自动控制工程、计算机工程以及人工智能等多种学科的最新研究成果，是当前科技发展最活跃的领域之一。机器人作为一种新型生产工具在提高生产率以及减轻劳动强度等方面有着极大的优越性，并且正逐步代替人类从事危险、恶劣的工作。如今，由机器人与其它设备组成的产线极大地提高了企业的生产效率和产品质量，加快了产品更新换代的速度，改变了传统的生产模式。工业机器人水平不断提高的同时，各种用于非制造业的先进机器人系统也有了很大的发展。 进入20世纪80年代后，人们为提高机器人在复杂作业操作中的性能，增强灵活性和柔顺性，使其在不同环境下适应不同的任务。这就要求机器人具有更的手臂及关节，即增加机器人的自由度来提高效率、增加安全性和应用范围，人们对多自由度机器人投入了越来越多的兴趣和研究。 多自由度机器人是指关节空间维数大于任务空间维数的机器人，具有运动学冗余度，对于末端笛卡尔空间中确定的位姿，其关节空间存在着自运动，即关节具有多种位置形态。迄今为止，国内外已研制出多种多自由度度机器人，应用范围越来越广。1987年，日本机械工程实验室成功生产了一个用于缝纫的七自由度仿人形机械手。同年，美国公司研制成功了一个用于太空研究的自由度遥控机器人，它由两个七自由度的仿人臂和一个三自由度的躯干构成。国内北京航空航天大学于年研制成功了一个七自由度机械手。 多自由度机器人由于具有冗余性，带来了许多优势，例如：提高灵活性和柔顺性；能够避免奇异位姿，更方便地控制各关节的速度和加速度；容易避开关节的极限位置，减少到达极限位置引起停车的可能性；方便避开障碍物等。因此多自由度机器人的研究和推广应用受到了人们普遍的重视。本课题通过研究多轴机械臂底层控制程序的控制策略，基于体感装置的相关信号对机械臂实现控制。1.2 国内外研究进展目前国内多轴机械臂主要用于机床加工，铸锻，热处理等方面、数量、品种、性能方面都不能满足工业生产发展的需要。所以，在国内主要是逐步扩大应用范围，重点发展铸造，热处理方面的机械手，以减轻劳动强度，改善作业条件，在应用专业机械手的同时，相应发展通用机械手，有条件的还要研制示教式机械手、计算机机械手和组合机械手等。同时要提高速度，减少冲击，正确定位，以便更好地发挥机械臂的作用。国外机械手在机械制造行业中应用较多，发展也很快。目前主要用于机床，横断压力机的上下料，以及点焊、喷气等作业。国外机械手的发展趋势是大力研制具有某种具有智能的机械手。使它具有一定的传感能力，能反馈外界条件的变化，作相应变更。更重要的是将机械手，柔性制造单元和柔性制造单元相结合，从根本上改变目前机械制造系统的人工操作状态。同时，随着机械手的小型化和微型化，其应用领域将会突破的机械领域，而向着电子信息、生物技术、生命科学及航空航天等高端行业发展。1.3 7轴智能化机器人机械臂介绍工业机器人应用最广泛的形式是机械臂（Mechanical Manipulator），其主要应用于工业装配领域（Assembly）。在工业应用中，机器人分为固定基座的机器人手臂（Robot Manipulator）和移动基座的移动机器人（Mobile Robot）。1.3.1 机器人的组成 机器人手臂与人的手臂类似，由臂（arm）、腕（Wrist）、末端工具（End-Effector）三部分组成。机器人的臂：一端安装在固定的基座上，另一端连接机器人的腕。机器人的腕：一端连接在机器人的臂上，另一端安装末端工具。末端工具：可以在空间中自由运动，执行机器人的工作任务。机器人的臂和腕，均由连杆和关节组成。（1）连杆（Link）机器人手臂由多个连杆（Link）组成，连杆之间通过关节（Joint）连接，关节能够使得两个相邻连杆产生相对运动。连杆可以视为刚体（即运动时不会变形）。相邻关节之间的部分即为连杆。（2）关节（Joint）两个相邻的连杆通过关节连接，关节运动导致连杆之间产生相对运动。典型关节有旋转关节（revolute）和平移关节（prismatic）两种。图1.1，给出了旋转关节和平移关节的几何示意图。旋转关节（用R表示）能够使得两个连杆之间沿着关节轴线产生相对旋转；平移关节（用P表示）能够使得两个连杆之间沿着关节轴线产生相对位移。描述两个相邻连杆的相对位置的量，称为关节变量。对于旋转关节，该关节变量是角度；对于平移关节，该关节变量是位移。机器人一般在关节处安装有传感器（Sensor），用于测量关节变量的具体数值。图1.1 旋转关节和平移关节在机器人建模中，两种关节的图形分别如图1.2（a-c）和图1.3(a-c)所示。图1.2 旋转关节在机器人模型中的示意图图1.3 平移关节在机器人模型中的示意图机器人手臂自由度的个数等于独立关节变量的个数。机器人手臂的每个关节提供了一个自由度，因此，机器人手臂关节的数量就是该机器人手臂自由度的个数。典型的机器人手臂应该至少拥有6个自由度，3个自由度用于定末端工具的位置、3个自由度用于定末端工具的方向。如果机器人手臂多于6个自由度，则称其为具有运动学冗余的机器人手臂。7轴智能化机器人手臂，有7个自由度，因此是具有运动学冗余的机器人手臂。（3）机器人的臂（Manipulator或Arm）机器人的臂，是机器人的主体，由连杆（Link）、关节（Joint）构成。当将腕（Wrist）和末端工具（End-Effector）加在臂上，并具有相应的控制系统时，就构成了机器人。图1.4给出了具有3个旋转关节的机器人的臂。图1.4 具有3个旋转关节的机器人的臂（4）机器人的腕（Wrist）在机器人的前臂和末端工具之间的所有关节，构成了机器人的腕（Wrist）。通常设计为球形腕关节（Spherical Wrist），其由3个旋转关节组成，这3个旋转关节的关节轴线相交于同一点，该点称为腕关节点（Wrist Point）。图1.5给出了球形腕关节示意图。图1.5 球形腕关节示意图球形腕关节能够大大简化机器人的运动学分析，能够很方便的解耦末端工具的位置和方向。因此，一般来说，机器人的臂（有3个自由度）用于定末端工具的位置，末端工具方向的自由度取决于机器人的腕。腕的自由度可以是1、2或3取决于工业应用的要求。末端工具（End-Effector）末端工具附着在机器人的腕的最后一个连杆上。最简单的末端工具是夹子，其只有两个动作：打开和闭合。取决于机器人应用要求，末端工具可以是夹子、焊枪等。致动器（Actuator）致动器相当于机器人的肌肉，为机器人的运动提供力量。致动器提供的力量用于克服重力、惯性以及其他外力，从而改变末端工具的位置和方向。致动器也称为舵机。致动器有电子、液压和气动三种。液压和气动型致动器主要应用于力矩特别大、精度要求低的场合，电子型致动器用于高精度应用。传感器（Sensor）关节位置、速度、加速度、以及力，是最需要检测的量。传感器集成在机器人中，用于检测这些量，并将检测到的值传送到控制器中用于运动控制。控制器（Controller）控制器的作用有：（1）信息处理：收集及处理机器人传感器送来信息（关节位置、速度、加速度、力）。（2）位置伺服控制：综合速度、加速度，实现关节精确的位置伺服控制。（3）通信：能够与计算机进行通信，实现对机器人的控制。1.3.2 7轴智能化机器人手臂图1.6给出了7轴智能化机器人手臂实物图。肩关节旋转（shoulder Roll）肩关节俯仰（shoulder Pitch）肘关节旋转（Elbow Roll）肘关节俯仰（Elbow Pitch）腕关节偏航（Wrist Yaw）腕关节俯仰（Wrist Pitch）腕关节旋转（Wrist Roll）手指（End-effector, Gripper）图1.6 7轴智能化机器人手臂实物图如图1.6所示，两个肩关节、两个肘关节及连杆构成了机器人的臂（Arm），三个腕关节及连杆构成了机器人的腕（Wrist），末端工具由两个手指的夹子构成，用于夹取物体。7轴智能化机器人手臂的所有关节，均为旋转关节。旋转动作：关节轴线与其下一级连杆在同一直线上。当关节旋转时，连杆也跟着旋转。一个关节连接两个连杆，一个连杆位于基座一边，另一个连杆位于末端工具一边。下一级连杆指的是末端工具一边的连杆。俯仰动作：关节轴线与其下一级连杆垂直。关节旋转时，导致连杆上下作俯仰动作。偏航动作：关节轴线与其下一级连杆垂直。关节旋转时，导致连杆左右摆动。7轴智能化机器人手臂的关节编号与连杆编号，如图1.7所示。关节编号通常与下一级连杆的编号相同。图1.7 7轴智能化机器人手臂的关节编号与连杆编号图1.7中的尺寸单位为：毫米。图1.7中，手臂中的白线表示关节轴线。1.3.3 机器人的通信方式将7轴智能化机器人手臂与计算机连接起来，通过特定的通信协议，实现计算机对7轴智能化机器人手臂的各种控制。7轴智能化机器人手臂与计算机的连接，如图1.8所示。图1-8 机器人与计算机的连接7轴智能化机器人手臂与计算机的连接，只需要一条USB线，一端插入计算机的USB端口，另一端插入机械臂的USB接口即可。用机器人专用的电源适配器，给机器人供电后，就可以通过计算机控制机器人了。具体步骤如下：第一步：接通机械臂电源。将机械臂配备的专用电源适配器的一端连接220V市电插座，另一端连接机械臂的电源输入插座。第二步：将机械臂与PC进行连接。将通用的USB连接线，一端连接机器人的USB接口，另一端连接PC机的USB端口。上电以后，指示灯会闪烁一次。图1.9，给出了7轴智能化机器人手臂配件及其连接的实物图。直流电源输入 配件2：通用USB连接线USB接口配件1：电源适配器7轴智能化机器人手臂图1.9 7轴智能化机器人手臂及其配件机器人的USB端口是一个虚拟串行端口（VSP，Virtual Serial Port），因此，在计算机上，需要采用软件的方法，把计算机的USB端口模拟成串行端口，这个软件就是机器人专用的驱动程序。安装好驱动程序、连接好数据线、并给机器人上电后，就可以在计算机的设备管理器中，看到系统为该虚拟串行端口分配的端口号。不同的计算机系统，所分配的端口号不一定相同。如图1.10所示。图1.10 操作系统给USB虚拟串口分配的端口号图1.10中的COM4，COM表示串口（即串行端口，或串行口），4就是操作系统为机器人虚拟串行端口分配的端口号。机器人控制每个关节的转动是通过致动器（Actuator），也称为舵机。舵机是一个位置伺服电机，由微控制器、位置传感器、电机、变速齿轮组成，封装在机器人的连杆中。微控制器中包含有控制程序，该控制程序能够执行计算机传送过来的命令，也能将机器人的状态传回计算机中。机器人的舵机与舵机之间，通过3线电缆连接。其中，2根线为电源线，为舵机供电，另一根为通信线。如图1.11所示。3线电缆白色接插件3线电缆图1.11 7轴智能化机器人手臂及其配件用3线电缆将机器人的USB接口、以及每个舵机顺序串联起来，计算机就可以任意控制每个舵机了。7轴智能化机器人手臂，有8个舵机。其中7个舵机构成机器人的臂和腕，另1个舵机为末端工具，即夹子（或手爪、手指）。 2 7轴智能化机器人机械臂建模与正运动学分析2.1 概述正运动学分析是指：已知各个关节变量的值，计算末端工具在固定参考坐标系中的位置与朝向（也称为姿态）。反过来，已知末端工具在固定参考坐标系中的位置与姿态，计算各个关节变量的值。这种分析称为逆运动学分析。2.2 D-H方法在机器人学中，使用D-H方法为机器人指定各个坐标系，并确定相邻两个坐标系之间的几何位置关系，即D-H参数。D-H方法包含两个内容：1、指定坐标系2、确定两个相邻坐标系D-H参数D-H方法：（1） 编号方法：设机械臂有n个关节，则机械臂就有n个连杆和1个末端工具。因此需要为机械臂指定（n+1）个坐标系。1) 关节编号：1 n机械臂从下往上，关节编号为1n。共有n个关节编号。2) 连杆编号：0 n-1基座与关节1之间的部分连杆0；关节1与关节2之间的部分为连杆1；关节（n-1）与关节n之间的部分为连杆（n-1）。共有n个连杆编号。3) 坐标系编号：0 n为连杆0指定坐标系0；为连杆1指定坐标系1；为连杆n-1指定坐标系n-1；为末端工具指定坐标系n。（2） 坐标系0的确定方法：坐标系0是整个机械臂的参考坐标系。机械臂的所有连杆以及末端工具的位置与朝向，都是以坐标系0作为参考的。1） 先确定坐标系0的Z轴确定连杆坐标系Z轴的基本原则：连杆坐标系i的Z轴，就是关节i+1的运动轴线。Z轴的正方向，可以任意指定。因此，坐标系0的Z轴，就是关节1的旋转轴线。Z轴的正方向可任意指定2） 再确定坐标系0的原点O可以在坐标系0的Z轴上任取一点，作为坐标系0的原点。在实际中，则根据机械臂的实际情况，取一个合适的点作为原点。3） 再确定坐标系0的X轴和Y轴。坐标系0的X轴和Y轴可以任意指定，但必须满足右手螺旋定则。用右手握住Z轴，大拇指指向Z轴的正方向，四指的绕向即为X轴的正方向绕向Y轴的正方向，且绕过角度为90度。确定了坐标系0之后，就可以通过下一级坐标系i-1，来确定上一级坐标系i，然后再确定两个相邻坐标系的D-H参数。即，可以通过坐标系0来确定坐标系1；通过坐标系1，确定坐标系2，。（3） 由下向上确定其余连杆坐标系1到n-1及其 D-H参数： 已知坐标系i-1，确定坐标系i。由于前面已经确定了坐标系0，使用下面的方法，就能确定剩余的所有连杆坐标系：坐标系1到坐标系n-1。1） 先确定坐标系i的Z轴根据确定连杆坐标系Z轴的基本原则，坐标系i的Z轴，就是关节i+1的旋转轴线。Z轴的正方向可任意指定。2） 再确定坐标系i的原点O及X轴可分为3种情况：a) 坐标系i和i-1的两个Z轴不共面（即不在一个平面）坐标系i的X轴：坐标系i和i-1的两个Z轴的法线。正方向可以任意指定。b) 坐标系i和i-1的两个Z轴相交坐标系i的原点：坐标系i和i-1的两个Z轴的交点。坐标系i的X轴：坐标系i和i-1的两个Z轴相交所确定的平面的法线（即平面的垂线），正方向可以任意指定。c) 坐标系i和i-1的两个Z轴平行坐标系i的X轴：选择过坐标系i-1的原点，坐标系i和i-1的两个Z轴的法线，作为坐标系i的X轴。3） 再确定坐标系i的Y轴对于3维坐标系，只要确定了两个坐标轴，根据右手螺旋定则，即可以确定另一个坐标轴。4） 根据D-H参数的定义，确定坐标系i的D-H参数。注意：对于旋转关节，关节角是变量，其余3个参数是常量；对于平移关节，关节距离d是变量，其余3个参数是常量。（4） 确定末端工具的坐标系n末端工具坐标系n的确定，是非常自由的。但必须遵守DH方法的基本要求：坐标系i的X轴，必须与坐标系i-1的Z轴垂直且相交。对于末端工具是两个手指的夹子的情况，坐标系的建立如图2.1所示： 图2.1 夹子坐标系示意图Z轴表示了接近物体的运动方向；Y轴表示了夹子开合方向。实验室7轴智能化机器人机械臂各关节如图2.2图2.2 7轴机械臂各关节坐标系示意图根据D-H参数的定义，确定坐标系n的D-H参数。（5） 计算相邻坐标系之间的齐次变换矩阵,公式如下： (2.1) 计算末端工具相对于固定参考坐标系的齐次变换矩阵。 (2.2)例：本课题中中使用的机械臂的DH参数表2.1如下表2.1 实验室机械臂的DH参数表连杆编号/弧度a/毫米/弧度d/毫米1/201 (-3/2)1202/202 (0)03-/203 (0)140.84-/271.84(-/2)05/271.85 (-)06-/206 (-/2)07007 (-)129.6由上表得，机械臂各连杆的齐次变换矩阵依次为： (2.3)2.3 机械臂的建模与仿真1、根据D-H参数，使用link函数创建机械臂各连杆的模型。 格式： L =link(alpha A theta D sigma offset, CONVENTION) （1）sigma：0表示连杆对应的关节类型为旋转关节；1表示连杆对应的关节类型为平移关节。对应关系，连杆1对应关节1，以此类推。（2）CONVENTION：字符串standard表示标准D-H方法；字符串 modified表示非标准的D-H方法。（3）alpha A theta D：用D-H方法得到的D-H参数。特别注意：对于旋转关节，theta是关节变量，表示关节角，随关节的旋转而变化，其余三个参数则为常数，反映了连杆之间的几何关系。对于平移关节，D是关节变量，表示关节位移，随关节平移而变化，其余三个参数则为常数，反映了连杆之间的几何关系。在使用该函数建模时，关节变量一律取为0即可。其它参数则直接使用D-H参数值。（4）offset：使连杆模型与实际连杆一致的关节变量的补偿值。offset的计算方法：offset = D-H参数中的关节变量取值 - 建模时实际关节的值。前面为7轴智能化机器人手臂各连杆指定坐标系时，机械臂处于直立状态，各关节的角度值均为pi，因此，这里offset的计算方法为：offset = D-H参数中的关节变量取值 pi。7轴智能化机器人手臂的各连杆模型为：。连杆1：L1 = link(pi/2, 0, 0, 120, 0, -pi/2-pi , standard);连杆2：L2 = link(pi/2, 0, 0, 0, 0, pi-pi , standard);连杆3：L3 = link(pi/2, 0, 0, 140.8, 0, pi-pi , standard);连杆4：L4 = link(-pi/2,71.8, 0, 0, 0, pi/2-pi , standard);连杆5：L5 = link(pi/2, 71.8, 0, 0, 0, 0-pi , standard);连杆6：L6 = link(pi/2, 0, 0, 0, 0, pi/2-pi , standard);连杆7：L7 = link(0, 0, 0, 129.6, 0, 0-pi , standard)L是一个CELL数组。2 、根据连杆模型，使用robot函数创建机械臂模型有了机械臂的连杆模型之后，就可以用robot函数创建机械臂的计算模型。模型名称=robot(LINK, name)LINK:就是用link函数创建的各连杆模型。name：机械臂的名字，为字符串。例：给出一个5自由度关节型串联机械臂的DH参数表2.2如下：表2.2 5自由度机械臂D-H参数表I1002-90030040050-90 应用上述函数编写程序并在MATLAB中运行得到两个仿真图2.3 、图2.4图2.3 基于 Robotics Toolbox 的机械臂可视化仿真图2.4 基于 Robotics Toolbox 的机械臂运动学仿真2.4 正运动学分析正运动学的主要问题是：根据各个关节旋转的角度，确定末端工具在笛卡尔坐标系中的位置（即坐标）和朝向。在机器人学中，使用D-H方法为每个连杆及末端工具建立一个坐标系，根据D-H参数能够得到相邻两个坐标系的齐次矩阵；然后通过复合齐次变换，就能够得到末端工具坐标系到固定参考坐标系的齐次变换矩阵。设机械臂有n个关节，则末端工具坐标系到固定参考坐标系的齐次矩阵为： (2.4)表示关节1的关节变量取值为q1时，坐标系1到坐标系0的齐次矩阵。表示关节2的关节变量取值为q2时，坐标系2到坐标系1的齐次矩阵。以此类推，表示关节n的关节变量取值为qn时，坐标系n到坐标系n-1的齐次矩阵。这样，通过复合齐次变换，就可以得到坐标系n到坐标系0（即固定参考坐标系）的齐次矩阵。每对相邻坐标系i和i-1的齐次矩阵，可用以下公式得到： (2.5) 右边各个齐次矩阵的公式为： (2.6) (2.7) (2.8) (2.9)坐标系n的原点在坐标系0中的坐标，其X,Y,Z值对应为第4列的第1行到第3行的元素值。也能够得到坐标系n的x,y,z三个坐标轴，相对于坐标系0的三个坐标轴的方向余弦，从而确定其方向。2.5 逆运动学分析一般来说，机械臂的逆运动学解qk(k=1,2,n)可以通过求解以下非线性方程组得到。 (2.10)或 (2.11)其中n为机械臂的自由度。在(2)的12个方程中最多只有6个是独立的。方程(1)或(2)是一个超越方程组，可以使用数值方法得到它的零点，一般来说要使用迭代算法。最常用的是牛顿(Newton-Raphson) 迭代法。(1)单变量的牛顿迭代法牛顿法是解方程f(x)=0的一种迭代法。设有方程f(x)=0在a b上有且仅有一个根，可用牛顿法求的近似值。将f(x)=0在x0点按台劳级数展开 f(x)= f(x0)+ f (x0)(x- x0)+ f (x0)(x- x0)2+ (2.12)取前两项得到 f(x)= f(x0)+ f (x0)(x- x0)0，x x0- f(x0)/ f ( x0), x1=x0- f(x0)/ f ( x0) (2.13)具体计算步骤如下： 求出f(x)的导数f (x)，在a b上任取一个初始值x0； 计算x1= x0- f(x0)/ f ( x0)； 若| x1-x0|e (e为精度要求)，此时计算结束=x1，否则令x0=x1转。 (2)多变量的牛顿迭代法(以两个变量为例)设有非线性方程组u (x,y)=0，v (x,y)=0，在 (xn, yn) 按台劳级数展开，取展开式的前两项 (2.14) (2.15)（3）牛顿迭代算法在机械臂逆解中的应用为了用迭代算法运动学方程(1)或(2)中求出未知数q，首先要给出一个迭代的初始值 (2.16)利用正运动学方程得到 (2.17)由泰勒级数展开得到 (2.18)略去高次项，得到一个线性方程组 (2.19)其中称为雅克比矩阵。所以有 (2.20) (2.21)写成迭代公式(2.22)实现 (3)的算法如下 设k=0,给出关节的迭代初始值； 计算 计算； 如果，计算结束，就是希望的迭代解。 取k=k+1,转继续计算。对于冗余自由度机械臂，其雅可比矩阵是长方矩阵，因J满秩且方程个数少于未知数个数，所以有无穷多个解，这时，一般是求其中的最小范数解，或采用加权最小范数解。也就是说使最小的解，其中D是对称正定加权矩阵。此时的解是使机械臂在能量消耗最小的情况下的解。当D=I时，雅可比矩阵的逆阵用代替，称为雅可比矩阵的伪逆。例1 考虑两个自由度的平面机械臂 (2.23) (2.24)它的雅克比矩阵是 (2.25)雅克比矩阵的逆矩阵 (2.26)所以，迭代公式(3)表示为 (2.27)取， 雅克比矩阵和它的逆矩阵分别是所以迭代4次，得到 3 7轴智能化机器人机械臂的路径规划3.1 机械臂空间的认识（1）、关节空间 n个自由度的机械臂的末端位姿由n个关节变量所决定，这n个关节变量统称为n维关节矢量，记为q。所有的关节矢量构成的空间称为关节空间。由于机械臂各个关节的旋转受到物理条件的约束，每个关节的转动被限制在一定的范围内。本实验所用的机械臂有7个关节，所以有7个自由度，受到物理结构的限制，它们的转动范围分别是（单位是度）最小值 300 690 300 690 690 740 300 最大值 3290 2910 3290 2910 2910 2850 3290 （2）、操作空间 机械臂末端的位姿用6个变量描述，3个平移(x,y,z)和3个旋转(wx, wy, wz)，记d=(x,y,z, wx, wy, wz) d是机械臂末端在基坐标空间中的坐标，所有的矢量d构成的空间称为操作空间或作业定向空间。操作空间是操作臂的末端能够到达的空间范围，即末端能够到达的目标点集合。（3）、驱动空间 机械臂各关节驱动器的位置组成的矢量称为驱动矢量s，由这些矢量构成的空间称为驱动空间。本实验所用的机械臂有7个旋转关节，每个关节分别由一个舵机驱动，根据舵机的特性，换算关系1=0.088度，所以最小值 348 788 348 788 788 848 348 最大值 3748 3308 3748 3308 3308 3248 3748 舵机的速度可以设置，设置的范围是 (1-1023), 换算关系1=0.114转/分钟。3.2 关节空间路径规划的基本方法 关节空间中的路径规划是指，在关节空间中，给定关节的起点和终点，确定一条连接这两点的曲线。这条曲线称为路径函数。路径规划又可以分为点到点（Point-to-Point Motion）的路径规划问题和连续路径(Continuous-Path Motion )路径规划问题。点到点路径规划问题是指在给定关节的起点和终点，求出一个路径函数，在指定的时间，使关节从起点移动到终点。连续路径路径规划问题，是指在起点与终点之间，还有一些中间点，求出一个路径函数，使得关节在指定的时间段内从起点移动到终点的同时，还要求关节在指定的时刻，通过这些中间点。3.2.1 点到点路径的规划 三次多项式函数是一种最简单点到点（Point-to-Point Motion）的路径函数。在关节空间中，三次多项式函数描述了机械臂的关节在两个点q(t0) 和q(tf )之间随时间变化的过程，用q(t)表示 (3.1)满足约束条件 (3.2)利用以上方程和约束条件得到求解q(t)各个系数的方程组 (3.3)解这个方程组可以得到 (3.4) (3.5) （1）规划一条点到点路径，要求在路径的中间段t1 tt2保持匀速运动。这里 t0 t1 t2 tf 。约束条件是 (3.6)这个路径分为加速段，匀速段和减速段。加速段 (3.7)匀速段 (3.8)其中，积分常数C可以t=t1时的位置确定。 (3.9)减速段 对于减速段，所以可以用一个三次多项式表示 (3.10)满足有4个约束条件， (3.11)这些条件用以下方程表示 (3.12) （2）在关节空间中规划一条路径，在路径的两个端点处具有指定的位置，速度和加速度。 (3.13) 要能同时满足6个条件的多项式至少是5次多项式。 (3.14) 利用以上方程和约束条件可以得到5个方程组成的线性方程组 (3.15) 求解此线性方程组可以得到5次多项式的各个系数。3.2.2 连续路径的规划 关节空间中的连续路径规划问题，在机械臂的控制中，经常要求一个路径要在特定的时间经过一系列的中间点。考虑一个路径由4个点q0, q1, q2,和 q3组成，到达这些点的时间分别为t0, t1, t2和 t3，同时，要限制路径起始和终止时的速度和加速度。限制条件表示为 (3.16)要能同时满足8个条件的多项式至少是7次多项式。 (3.17) 利用以上方程和约束条件可以得到8个方程组成的线性方程组 (3.18)例1：给出一个3自由度关节型串联机械臂的D-H参数表3.1如下：表3.1 3自由度机械臂D-H参数表连杆编号/弧度a/毫米/弧度d/毫米1/201 (0)12/202 (0)13003 (0)1 规划一条关节空间中的路径，经过4个点P1(0,0,0)，P2(1,3,2) ，P3(2,2,1) ，P4(3,1,3)每个点之间的运行时间为1秒，采样周期为0.05秒。计算机械臂末端运行的位姿，并画出图形。%建立一个3个自由度的关节型串联机械臂模型L1=link(pi/2 0 0 1 0 0, sta);L2=link(pi/2 0 0 1 0 0, sta);L3=link( 0 0 0 1 0 0, sta);R=robot(L