基于动作的多轴机械臂的运动控制分析

基于动作的多轴机械臂的运动控制分析摘要：随着科学技术的迅速发展，机器人也越来越多的出现在日常的生活中。其中多自由度机械臂是最常见的机器人之一。本文以实验室七轴智能化机器人机械臂为研究对象，使用了D-H法和牛顿迭代算法，在运动学和控制方面进行建模，并实现机械臂与计算机串口之间的通信。随后在CytonVeiwer平台上，借助手柄对机械臂进行操控，通过手柄输出命令控制机械臂的运动轨迹，并将运动轨迹导入到MATLAB软件中进行分析仿真，最终实现对机械臂运动更加精确地控制。关键词：七自由度,机械臂,运动学建模,手柄遥控IBasedonTheActionofMultiracialArmControlAnalysisAbstract：Withtherapiddevelopmentofscienceandtechnology,robotismoreAndmoreappearinourdairylife.themanydegreesoffreedommechanicalarmisoneofthemostcommontypeofrobot.Inthispaper,ourLaboratoryusethesevenaxismechanicalarmofintelligentrobotasthereachobject,usingtheD-HmethodandNewtonIterationalgorithm,thenStartmodelingwiththekinematicsandcontrol,finallyrealizesthecommunicationofmechanicalarmandthecomputerserialportthenonCytonVeiwerplatform,withthehandshanktocontrolofmechanicalarm,byUsingtheoutputcommandofhandshankcontrolthetrajectoryofmechanicalarm,andthetrajectoryimportedintotheMATLABsoftwarebegantoanalyzeofsimulation,ultimatelyachievemoreaccuratecontroloftheMechanicalarmmovement.Keywords:Sevendegreesoffreedom,Themechanicalarm,Kinematicsmodeling,ThehandofremotecontrolII目录1绪论.11.1多轴机械臂的研究意义.11.2国内外研究进展.11.37轴智能化机器人机械臂介绍.21.3.1机器人的组成.21.3.27轴智能化机器人手臂.61.3.3机器人的通信方式.727轴智能化机器人机械臂建模与正运动学分析.112.1概述.112.2D-H方法.112.3机械臂的建模与仿真.152.4正运动学分析.182.5逆运动学分析.1937轴智能化机器人机械臂的路径规划.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.1MATLAB动作仿真.365.1.1通信测试与设置.365.1.2路径的规划.365.1.3状态与控制.37III5.2CytonViewer动作仿真.385.2.1Jobstick插件.385.2.2RecordPlayback插件.385.3本章小结.396结论与展望.416.1研究工作总结.416.2后续工作展望.41参考文献.42致谢.4301绪论1.1多轴机械臂的研究意义机器人领域集中了机械工程、电子工程、自动控制工程、计算机工程以及人工智能等多种学科的最新研究成果，是当前科技发展最活跃的领域之一。机器人作为一种新型生产工具在提高生产率以及减轻劳动强度等方面有着极大的优越性，并且正逐步代替人类从事危险、恶劣的工作。如今，由机器人与其它设备组成的产线极大地提高了企业的生产效率和产品质量，加快了产品更新换代的速度，改变了传统的生产模式。工业机器人水平不断提高的同时，各种用于非制造业的先进机器人系统也有了很大的发展。进入20世纪80年代后，人们为提高机器人在复杂作业操作中的性能，增强灵活性和柔顺性，使其在不同环境下适应不同的任务。这就要求机器人具有更的手臂及关节，即增加机器人的自由度来提高效率、增加安全性和应用范围，人们对多自由度机器人投入了越来越多的兴趣和研究。多自由度机器人是指关节空间维数大于任务空间维数的机器人，具有运动学冗余度，对于末端笛卡尔空间中确定的位姿，其关节空间存在着自运动，即关节具有多种位置形态。迄今为止，国内外已研制出多种多自由度度机器人，应用范围越来越广。1987年，日本机械工程实验室成功生产了一个用于缝纫的七自由度仿人形机械手。同年，美国公司研制成功了一个用于太空研究的自由度遥控机器人，它由两个七自由度的仿人臂和一个三自由度的躯干构成。国内北京航空航天大学于年研制成功了一个七自由度机械手。多自由度机器人由于具有冗余性，带来了许多优势，例如：提高灵活性和柔顺性；能够避免奇异位姿，更方便地控制各关节的速度和加速度；容易避开关节的极限位置，减少到达极限位置引起停车的可能性；方便避开障碍物等。因此多自由度机器人的研究和推广应用受到了人们普遍的重视。本课题通过研究多轴机械臂底层控制程序的控制策略，基于体感装置的相关信号对机械臂实现控制。11.2国内外研究进展目前国内多轴机械臂主要用于机床加工，铸锻，热处理等方面、数量、品种、性能方面都不能满足工业生产发展的需要。所以，在国内主要是逐步扩大应用范围，重点发展铸造，热处理方面的机械手，以减轻劳动强度，改善作业条件，在应用专业机械手的同时，相应发展通用机械手，有条件的还要研制示教式机械手、计算机机械手和组合机械手等。同时要提高速度，减少冲击，正确定位，以便更好地发挥机械臂的作用。国外机械手在机械制造行业中应用较多，发展也很快。目前主要用于机床，横断压力机的上下料，以及点焊、喷气等作业。国外机械手的发展趋势是大力研制具有某种具有智能的机械手。使它具有一定的传感能力，能反馈外界条件的变化，作相应变更。更重要的是将机械手，柔性制造单元和柔性制造单元相结合，从根本上改变目前机械制造系统的人工操作状态。同时，随着机械手的小型化和微型化，其应用领域将会突破的机械领域，而向着电子信息、生物技术、生命科学及航空航天等高端行业发展。1.37轴智能化机器人机械臂介绍工业机器人应用最广泛的形式是机械臂（MechanicalManipulator），其主要应用于工业装配领域（Assembly）。在工业应用中，机器人分为固定基座的机器人手臂（RobotManipulator）和移动基座的移动机器人（MobileRobot）。1.3.1机器人的组成机器人手臂与人的手臂类似，由臂（arm）、腕（Wrist）、末端工具（End-Effector）三部分组成。机器人的臂：一端安装在固定的基座上，另一端连接机器人的腕。机器人的腕：一端连接在机器人的臂上，另一端安装末端工具。末端工具：可以在空间中自由运动，执行机器人的工作任务。机器人的臂和腕，均由连杆和关节组成。2（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旋转关节在机器人模型中的示意图3图1.3平移关节在机器人模型中的示意图机器人手臂自由度的个数等于独立关节变量的个数。机器人手臂的每个关节提供了一个自由度，因此，机器人手臂关节的数量就是该机器人手臂自由度的个数。典型的机器人手臂应该至少拥有6个自由度，3个自由度用于定末端工具的位置、3个自由度用于定末端工具的方向。如果机器人手臂多于6个自由度，则称其为具有运动学冗余的机器人手臂。7轴智能化机器人手臂，有7个自由度，因此是具有运动学冗余的机器人手臂。（3）机器人的臂（Manipulator或Arm）机器人的臂，是机器人的主体，由连杆（Link）、关节（Joint）构成。当将腕（Wrist）和末端工具（End-Effector）加在臂上，并具有相应的控制系统时，就构成了机器人。图1.4给出了具有3个旋转关节的机器人的臂。图1.4具有3个旋转关节的机器人的臂（4）机器人的腕（Wrist）4在机器人的前臂和末端工具之间的所有关节，构成了机器人的腕（Wrist）。通常设计为球形腕关节（SphericalWrist），其由3个旋转关节组成，这3个旋转关节的关节轴线相交于同一点，该点称为腕关节点（WristPoint）。图1.5给出了球形腕关节示意图。图1.5球形腕关节示意图球形腕关节能够大大简化机器人的运动学分析，能够很方便的解耦末端工具的位置和方向。因此，一般来说，机器人的臂（有3个自由度）用于定末端工具的位置，末端工具方向的自由度取决于机器人的腕。腕的自由度可以是1、2或3取决于工业应用的要求。末端工具（End-Effector）末端工具附着在机器人的腕的最后一个连杆上。最简单的末端工具是夹子，其只有两个动作：打开和闭合。取决于机器人应用要求，末端工具可以是夹子、焊枪等。致动器（Actuator）致动器相当于机器人的肌肉，为机器人的运动提供力量。致动器提供的力量用于克服重力、惯性以及其他外力，从而改变末端工具的位置和方向。致动器也称为舵机。致动器有电子、液压和气动三种。液压和气动型致动器主要应用于力矩特别大、精度要求低的场合，电子型致动器用于高精度应用。传感器（Sensor）关节位置、速度、加速度、以及力，是最需要检测的量。传感器集成在机器人中，用于检测这些量，并将检测到的值传送到控制器中用于运动控制。控制器（Controller）5控制器的作用有：（1）信息处理：收集及处理机器人传感器送来信息（关节位置、速度、加速度、力）。（2）位置伺服控制：综合速度、加速度，实现关节精确的位置伺服控制。（3）通信：能够与计算机进行通信，实现对机器人的控制。1.3.27轴智能化机器人手臂图1.6给出了7轴智能化机器人手臂实物图。图1.67轴智能化机器人手臂实物图如图1.6所示，两个肩关节、两个肘关节及连杆构成了机器人的臂（Arm），三个腕关节及连杆构成了机器人的腕（Wrist），末端工具由两个手指的夹子构成，用于夹取物体。7轴智能化机器人手臂的所有关节，均为旋转关节。旋转动作：关节轴线与其下一级连杆在同一直线上。当关节旋转时，连杆也跟着旋转。一个关节连接两个连杆，一个连杆位于基座一边，另一个连杆位于末端工具一边。下一级连杆指的是末端工具一边的连杆。俯仰动作：关节轴线与其下一级连杆垂直。关节旋转时，导致连杆上下作俯仰动作。偏航动作：关节轴线与其下一级连杆垂直。关节旋转时，导致连杆左右摆肩关节旋转（shoulderRoll）肩关节俯仰（shoulderPitch）肘关节旋转（ElbowRoll）肘关节俯仰（ElbowPitch）腕关节偏航（WristYaw）腕关节俯仰（WristPitch）腕关节旋转（WristRoll）手指（End-effector,Gripper）6动。7轴智能化机器人手臂的关节编号与连杆编号，如图1.7所示。关节编号通常与下一级连杆的编号相同。图1.77轴智能化机器人手臂的关节编号与连杆编号图1.7中的尺寸单位为：毫米。图1.7中，手臂中的白线表示关节轴线。1.3.3机器人的通信方式将7轴智能化机器人手臂与计算机连接起来，通过特定的通信协议，实现计算机对7轴智能化机器人手臂的各种控制。7轴智能化机器人手臂与计算机的连接，如图1.8所示。7图1-8机器人与计算机的连接7轴智能化机器人手臂与计算机的连接，只需要一条USB线，一端插入计算机的USB端口，另一端插入机械臂的USB接口即可。用机器人专用的电源适配器，给机器人供电后，就可以通过计算机控制机器人了。具体步骤如下：第一步：接通机械臂电源。将机械臂配备的专用电源适配器的一端连接220V市电插座，另一端连接机械臂的电源输入插座。第二步：将机械臂与PC进行连接。将通用的USB连接线，一端连接机器人的USB接口，另一端连接PC机的USB端口。上电以后，指示灯会闪烁一次。图1.9，给出了7轴智能化机器人手臂配件及其连接的实物图。图1.97轴智能化机器人手臂及其配件机器人的USB端口是一个虚拟串行端口（VSP，VirtualSerialPort），因此，7轴智能化机器人手臂配件1：电源适配器配件2：通用USB连接线直流电源输入USB接口8在计算机上，需要采用软件的方法，把计算机的USB端口模拟成串行端口，这个软件就是机器人专用的驱动程序。安装好驱动程序、连接好数据线、并给机器人上电后，就可以在计算机的设备管理器中，看到系统为该虚拟串行端口分配的端口号。不同的计算机系统，所分配的端口号不一定相同。如图1.10所示。图1.10操作系统给USB虚拟串口分配的端口号图1.10中的COM4，COM表示串口（即串行端口，或串行口），4就是操作系统为机器人虚拟串行端口分配的端口号。机器人控制每个关节的转动是通过致动器（Actuator），也称为舵机。舵机是一个位置伺服电机，由微控制器、位置传感器、电机、变速齿轮组成，封装在机器人的连杆中。微控制器中包含有控制程序，该控制程序能够执行计算机传送过来的命令，也能将机器人的状态传回计算机中。机器人的舵机与舵机之间，通过3线电缆连接。其中，2根线为电源线，为舵机供电，另一根为通信线。如图1.11所示。9图1.117轴智能化机器人手臂及其配件用3线电缆将机器人的USB接口、以及每个舵机顺序串联起来，计算机就可以任意控制每个舵机了。7轴智能化机器人手臂，有8个舵机。其中7个舵机构成机器人的臂和腕，另1个舵机为末端工具，即夹子（或手爪、手指）。3线电缆白色接插件3线电缆1027轴智能化机器人机械臂建模与正运动学分析2.1概述正运动学分析是指：已知各个关节变量的值，计算末端工具在固定参考坐标系中的位置与朝向（也称为姿态）。反过来，已知末端工具在固定参考坐标系中的位置与姿态，计算各个关节变量的值。这种分析称为逆运动学分析。2.2D-H方法在机器人学中，使用D-H方法为机器人指定各个坐标系，并确定相邻两个坐标系之间的几何位置关系，即D-H参数。D-H方法包含两个内容：1、指定坐标系2、确定两个相邻坐标系D-H参数D-H方法：（1）编号方法：设机械臂有n个关节，则机械臂就有n个连杆和1个末端工具。因此需要为机械臂指定（n+1）个坐标系。111)关节编号：1n机械臂从下往上，关节编号为1n。共有n个关节编号。2)连杆编号：0n-1基座与关节1之间的部分连杆0；关节1与关节2之间的部分为连杆1；关节（n-1）与关节n之间的部分为连杆（n-1）。共有n个连杆编号。3)坐标系编号：0n为连杆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轴12可分为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.213图2.27轴机械臂各关节坐标系示意图根据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由上表得，机械臂各连杆的齐次变换矩阵依次为：14(2.3)2.3机械臂的建模与仿真1、根据D-H参数，使用link函数创建机械臂各连杆的模型。格式：L=link(alphaAthetaDsigmaoffset,CONVENTION)（1）sigma：0表示连杆对应的关节类型为旋转关节；1表示连杆对应的关节类型为平移关节。对应关系，连杆1对应关节1，以此类推。（2）CONVENTION：字符串standard表示标准D-H方法；字符串modified表示非标准的D-H方法。（3）alphaAthetaD：用D-H方法得到的D-H参数。特别注意：对于旋转关节，theta是关节变量，表示关节角，随关节的旋转而变化，其余三个参数则为常数，反映了连杆之间的几何关系。15对于平移关节，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如下：16表2.25自由度机械臂D-H参数表I1ia1iidi1001121-900232a0034300450-905d5应用上述函数编写程序并在MATLAB中运行得到两个仿真图2.3、图2.4图2.3基于RoboticsToolbox的机械臂可视化仿真17图2.4基于RoboticsToolbox的机械臂运动学仿真2.4正运动学分析正运动学的主要问题是：根据各个关节旋转的角度，确定末端工具在笛卡尔坐标系中的位置（即坐标）和朝向。在机器人学中，使用D-H方法为每个连杆及末端工具建立一个坐标系，根据D-H参数能够得到相邻两个坐标系的齐次矩阵；然后通过复合齐次变换，就能够得到末端工具坐标系到固定参考坐标系的齐次变换矩阵。设机械臂有n个关节，则末端工具坐标系到固定参考坐标系的齐次矩阵为：(2.4)表示关节1的关节变量取值为q1时，坐标系1到坐标系0的齐次矩阵。表示关节2的关节变量取值为q2时，坐标系2到坐标系1的齐次矩阵。以此类推，表示关节n的关节变量取值为qn时，坐标系n到坐标系n-1的齐次矩阵。这样，通过复合齐次变换，就可以得到坐标系n到坐标系0（即固定参考18坐标系）的齐次矩阵。每对相邻坐标系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)可以通过求解以下非线性方19程组得到。10)()()(343212112100rrqTqTnn(2.10)或kqrijij,1)(2.11)其中n为机械臂的自由度。在(2)的12个方程中最多只有6个是独立的。方程(1)或(2)是一个超越方程组，可以使用数值方法得到它的零点，一般来说要使用迭代算法。最常用的是牛顿(Newton-Raphson)迭代法。(1)单变量的牛顿迭代法牛顿法是解方程f(x)=0的一种迭代法。设有方程f(x)=0在ab上有且仅有一个根，可用牛顿法求的近似值。将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，xx0f(x0)/f(x0),x1=x0f(x0)/f(x0)(2.13)具体计算步骤如下：求出f(x)的导数f(x)，在ab上任取一个初始值x0；计算x1=x0f(x0)/f(x0)；若|x1x0|e(e为精度要求)，此时计算结束=x1，否则令x0=x1转。(2)多变量的牛顿迭代法(以两个变量为例)设有非线性方程组u(x,y)=0，v(x,y)=0，在(xn,yn)按台劳级数展开，取展开式的前两项(2.14)0)y(,vx)(xy,v),(x,u,unnnnny),v(x),(uJnnn20(2.15)（3）牛顿迭代算法在机械臂逆解中的应用为了用迭代算法运动学方程(1)或(2)中求出未知数q，首先要给出一个迭代的初始值(2.16)q0利用正运动学方程得到(2.17)000TT,）（由泰勒级数展开得到(2.18)q()(q)(q()2000O略去高次项，得到一个线性方程组(2.19)(00JT其中称为雅克比矩阵。所以有q)(2.20)q(q)()01010TJT(2.21)1J写成迭代公式(2.22)q(q)(1)()1(kkkkJ实现(3)的算法如下设k=0,给出关节的迭代初始值；)0(计算)()()(qqkkJT计算；)(11kT如果，计算结束，就是希望的迭代解。)(k1取k=k+1,转继续计算。对于冗余自由度机械臂，其雅可比矩阵是长方矩阵，因J满秩且方程个数少于未知数个数，所以有无穷多个解，这时，一般是求其中的最小范数解，或采用加权最小范数解。也就是说使最小的解，其中D是对称正定加权矩qD阵。此时的解是使机械臂在能量消耗最小的情况下的解。当D=I时，雅可比矩阵的逆阵用代替，称为雅可比矩阵的伪逆。1)(TJ例1考虑两个自由度的平面机械臂(2.23)sin(ico2121llyx21(2.24)yxT21q它的雅克比矩阵是(2.25)cos()cos(ininsi212121lllJ雅克比矩阵的逆矩阵(2.26)cos(sin)cos(sin2121212121lllllJ所以，迭代公式(3)表示为(2.27)(1)(21)(21kkkyxJ取，1,121yxTl3/q)0(21)0(21)3/sin(3/icoT雅克比矩阵和它的逆矩阵分别是13021230JJ所以792.16452313021)0(21)(21TJ迭代4次，得到5708.1q)4(21)4(2237轴智能化机器人机械臂的路径规划3.1机械臂空间的认识（1）、关节空间n个自由度的机械臂的末端位姿由n个关节变量所决定，这n个关节变量统称为n维关节矢量，记为q。所有的关节矢量构成的空间称为关节空间。由于机械臂各个关节的旋转受到物理条件的约束，每个关节的转动被限制在一定的范围内。本实验所用的机械臂有7个关节，所以有7个自由度，受到物理结构的限制，它们的转动范围分别是（单位是度）最小值300690300690690740300最大值3290291032902910291028503290（2）、操作空间机械臂末端的位姿用6个变量描述，3个平移(x,y,z)和3个旋转(x,y,23z)，记d=(x,y,z,x,y,z)d是机械臂末端在基坐标空间中的坐标，所有的矢量d构成的空间称为操作空间或作业定向空间。操作空间是操作臂的末端能够到达的空间范围，即末端能够到达的目标点集合。（3）、驱动空间机械臂各关节驱动器的位置组成的矢量称为驱动矢量s，由这些矢量构成的空间称为驱动空间。本实验所用的机械臂有7个旋转关节，每个关节分别由一个舵机驱动，根据舵机的特性，换算关系1=0.088度，所以最小值348788348788788848348最大值3748330837483308330832483748舵机的速度可以设置，设置的范围是(1-1023),换算关系1=0.114转/分钟。3.2关节空间路径规划的基本方法关节空间中的路径规划是指，在关节空间中，给定关节的起点和终点，确定一条连接这两点的曲线。这条曲线称为路径函数。路径规划又可以分为点到点（Point-to-PointMotion）的路径规划问题和连续路径(Continuous-PathMotion)路径规划问题。点到点路径规划问题是指在给定关节的起点和终点，求出一个路径函数，在指定的时间，使关节从起点移动到终点。连续路径路径规划问题，是指在起点与终点之间，还有一些中间点，求出一个路径函数，使得关节在指定的时间段内从起点移动到终点的同时，还要求关节在指定的时刻，通过这些中间点。3.2.1点到点路径的规划三次多项式函数是一种最简单点到点（Point-to-PointMotion）的路径函数。在关节空间中，三次多项式函数描述了机械臂的关节在两个点q(t0)和q(tf)之间随时间变化的过程，用q(t)表示23210)(tatqt24(3.1)满足约束条件ffffqttqtqt)()()()(00(3.2)利用以上方程和约束条件得到求解q(t)各个系数的方程组ffffqatttt032120301(3.3)解这个方程组可以得到(3.4)20030020)()()(tqttqtqafffffff302230016tttttfffff30022302)()()()ttqqttqqaffffff(3.5)0032ttff（1）规划一条点到点路径，要求在路径的中间段t1tt2保持匀速运动。这里t0t1t2tf。约束条件是ffffcqtqttt)()(2100(3.6)这个路径分为加速段，匀速段和减速段。加速段121010)(ttqtc(3.7)匀速段212)(tCtqc(3.8)25其中，积分常数C可以t=t1时的位置确定。cccqtC102(3.9)减速段对于减速段，所以可以用一个三次多项式表示23130)(tbtqt(3.10)满足有4个约束条件，ccffqttqqtt)(210)(22303(3.11)这些条件用以下方程表示cffffqttbtttt102321023210(3.12)（2）在关节空间中规划一条路径，在路径的两个端点处具有指定的位置，速度和加速度。ffffqttqtt)()(00(3.13)要能同时满足6个条件的多项式至少是5次多项式。(3.14)43210)(tatattq利用以上方程和约束条件可以得到5个方程组成的线性方程组26(3.15)ffffffffffqqaatttttttt054321235420203542016011求解此线性方程组可以得到5次多项式的各个系数。3.2.2连续路径的规划关节空间中的连续路径规划问题，在机械臂的控制中，经常要求一个路径要在特定的时间经过一系列的中间点。考虑一个路径由4个点q0,q1,q2,和q3组成，到达这些点的时间分别为t0,t1,t2和t3，同时，要限制路径起始和终止时的速度和加速度。限制条件表示为0)()()()()()(33321000tqtqtttt(3.16)要能同时满足8个条件的多项式至少是7次多项式。(3.17)76543210)(tatattatq利用以上方程和约束条件可以得到8个方程组成的线性方程组(3.18)004230216207654314230216203216543354237262315141500637542qqaattttttttttttttt例1：给出一个3自由度关节型串联机械臂的D-H参数表3.1如下：表3.13自由度机械臂D-H参数表连杆编号/弧度a/毫米/弧度d/毫米1/201(0)1272/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/200100,sta);L2=link(pi/200100,sta);L3=link(000100,sta);R=robot(L);%取时间tt=0123;%得到采样时间t_sample=linspace(0,3,60);%输入P点坐标p=000;132;221;313;%计算关节1的3次样条插值多项式cs=spline(t,0;p(:,1);0);%计算关节1在插值点处值qc1_sample=ppval(cs,t_sample);%计算关节2的3次样条插值多项式cs=spline(t,0;p(:,2);0);%计算关节2在插值点处值qc2_sample=ppval(cs,t_sample);%计算关节3的3次样条插值多项式cs=spline(t,0;p(:,3);0);%计算关节3在插值点处值qc3_sample=ppval(cs,t_sample);%画3个关节的随时间变化的曲线28-1-0.500.511.5-2-101200.511.52plot(t_sample,qc1_sample,t_sample,qc2_sample,t_sample,qc3_sample)，gridonxlabel(时间t/秒)ylabel(3个关节的变化/弧度)legend(关节1,关节2,关节3)%计算机械臂末端的位姿qc=qc1_sample;qc2_sample;qc3_sampletc=fkine(R,qc)%画机械臂末端位置随时间变化曲线plot3(squeeze(tc(1,4,:),squeeze(tc(2,4,:),squeeze(tc(3,4,:)gridon%计算机械臂末端的z轴与基础坐标系X,Y,Z轴的夹角bx=acos(squeeze(tc(1,3,:);by=acos(squeeze(tc(2,3,:);bz=acos(squeeze(tc(3,3,:);%画机械臂末端坐标系的z轴与基础坐标系X,Y,Z轴的夹角随时间变化曲线plot(t_sample,bx,t_sample,by,t_sample,bz),gridonxlabel(时间t/秒),gridonylabel(末端坐标系的z轴与基础坐标系X,Y,Z轴的夹角)legend(z-Z,z-Y,z-Z)00.511.522.5300.511.522.533.5个个t/个3个个个个个个/个个个个1个个2个个3图3.1a3个关节的位置曲线图3.1b3个关节的速度曲线00.511.522.53-3-2-1012345个个t/个3个个个个个个/个个/个个个1个个个个个2个个个个个3个个个29图3.1c机械臂末端位置随时间变化曲线00.511.522.5300.511.522.533.5?t/?z?X,Y,Z?z-Zz-Yz-Z图3.1d机械臂末端坐标系z轴与基础坐标系X,Y,Z轴的夹角3.3操作空间路径规划的基本方法操作空间中的路径规划是指，在操作空间中，给定机械臂末端的起点和终点，确定一条连接这两点的曲线。这条曲线称为路径函数。因为机械臂末端的位置与姿态一般是以直角坐标系描述，所以，操作空间的路径规划又称为直角坐标空间的路径规划(Cartesianpathplanning)。一般操作空间中的路径规划又可以分为点到点（Point-to-PointMotion）的路径规划问题和连续路径(Continuous-PathMotion)规划问题。点到点路径规划问题是指给定机械臂末端的起点和终点，求出一个路径函数，在指定的时间，使机械臂末端从起点移动到终点。连续路径路径规划问题是指在机械臂末端起点与终点之间，还要求机械臂末端经过中间点，求出一个路径函数，使得机械臂末端在指定的时间段内从起点移动到终点的同时，还要求机械臂末端在指定的时刻，通过或接近这些中间点。特别要注意的在操作空间中的路径规划中，有些中间点只是要求接近而不是严格的通过。从数学的角度来说，操作空间中的路径规划和关节空间中的路径规划是类似的。要使机械臂能够完成特定的工作，不但要求机械臂末端到达指定的位置，还要求机械臂末端保持一定的姿态。所以，机械臂末端的状态要用6个变量描述，3个平移(x,y,z)和3个旋转(x,y,z)。因此，在操作空间中的路径规划中，30要同时考虑机械臂末端的移动和旋转。d=(x,y,z,x,y,z)(3.19)实际上所以用于关节空间路径规划的方法都可以用于直角坐标空间的路径规划。最根本的区别是直角坐标空间的路径规划必须通过求机械臂运动的逆解得到对应的关节角度。也就是说，对于关节空间路径规划，路径函数生成的值就是关节值，而操作空间的路径规划函数得到的值是机械臂末端位置与姿态，需要求解逆运动学方程才能得到对应的关节值。以上过程可以总结为如下计算循环：1)将时间增加一个增量；tt2)用所选择的路径函数计算出机械臂末端的位置与姿态；3)利用解逆运动学方程得到对应的关节值；4)将关节信息送给控制器，使机械臂的末端运动到新的位置与姿态；5)