多轴机械臂的双臂协同工作研究

多轴机械臂的双臂协同工作研究摘要：随着时代的发展，机器人技术开始运用于工业生产中，尤其在一些比较复杂的生产环境中应用广泛。单臂机器人表现出价格昂贵、灵活性差、能力差、效率低等缺点，这就需要双臂机器人协同工作来代替人去完成一些装配复杂，超重作业或一些人无法完成的作业。本文以EF-IRC-I七轴智能化机械臂为基础，采用D-H法以及代数法，对其正、逆运动学进行了分析，编写了MATLAB控制程序，实现了双机械臂的协同工作。关键词：七自由度机械臂，双臂协同，运动学分析，MATLABResearchformulti-axismanipulatorarmscooperationAbstract：Withthedevelopmentofthetimes,robottechnologyappliedtoindustrialproduction,especiallyinsomecomparativelycomplexproductionenvironment.One-armrobotneedshighcostbuthaveshortagesinflexibility,capacityandefficiency,thusitneedstworobotsworktogethertocompletesomecomplicatedassembly,overweightortheworkthatpeoplecantfinish.ThisthesisisbasedontheEF-IRC-ISevenaxisintelligentroboticarm,usethed-hmethodandAlgebraicmethod,analyzeitspositiveandinversekinematics,writetheMATLABcontrolprogramandrealizethedoublemechanicalarmtocooperate.Keywords:sevendegreesoffreedommechanicalarm,cooperation,kinematicsanalysis,MATLAB4目录1绪论.21.1研究背景及意义.21.2多臂机器人协调操作任务的分类及国内外研究现状.21.2.1多臂机器人协调操作任务的分类.21.2.2国外研究进展.31.2.3国内研究进展.32工业机器人系统概述.52.1机械臂简单介绍.52.1.1机器人的组成.52.1.27轴智能化机器人手臂.82.1.3机器人的通信方式.102.1.4机器人的控制方式.132.2坐标变换.132.2.1坐标正变换.142.2.2B绕G的坐标轴多次连续旋转的坐标正变换.142.2.3B绕G的坐标轴旋转的正变换与逆变换的关系.142.2.4刚体运动.152.2.5齐次变换.162.2.6齐次逆变换.172.2.7复合齐次变换.173机器人建模与正运动学分析.193.1概述.193.2D-H方法与齐次变换矩阵.193.2.1D-H方法.193.2.2齐次变换矩阵.193.47轴智能化机器人手臂的D-H参数.203.57轴智能化机器人手臂的建模与仿真.203.5.1创建机械臂各连杆的模型.203.5.2创建机械臂模型.203.6正运动学分析.214机器人建模与逆运动学分析.234.1概述.234.2逆运动学分析的一般方法.234.3机械臂末端的奇异位姿.255机器人路径规划.275.1机械臂的三个变量空间认识.275.2关节空间路径规划的基本方法.275.3操作空间路径规划的基本方法.316MATLAB编程基础及机器人通信测试程序设计.336.1MATLAB编程基础.336.2通信测试的编程方法.336.3程序调试方法.347双臂协同工作研究.367.1双臂协同工作设计流程.367.2MATIAB仿真.367.2.1单臂运动仿真.377.2.2协同工作仿真.377.3本章小结.38总结.40参考文献.41致谢.43附录.4401绪论1.1研究背景及意义随着机器人执行任务的复杂性不断增加，需求日益商品化，并要求它能够适应比较复杂的生产环境，单个机器人表现出价格昂贵、灵活性差、能力差、效率低等缺点。就目前的机器人技术水平而言，单个机器人在信息的获取、处理、控制及操作能力等方面都存在较大的局限性，对于复杂的工作任务和多变的工作环境，它的能力更显不足，如复杂的装配作业、搬运较重的物体或柔软物体、安装或维修复杂的零件等。随着操作环境和任务要求的复杂化，一般需要机器人既要有很高的可靠性又要有良好的协调性。于是人们考虑用两个或多个机器人的协调作业来完成单个机器人无法完成或难以完成的工作。这使得多臂机器人表现出了很大的优越性，因而多机器人系统逐渐成为机器人发展的主要趋势。多机器人系统可以描述为一些机器人在同样的环境下协作完成任务的系统。目前大多数的多机器人系统使用同种类型的机器人，由于有着相同的结构。人们希望通过多机器人间的协调与合作,来提高机器人系统在作业过程中的效率,进而当机器人工作环境发生变化或系统局部发生故障时,多机器人之间仍可通过本身具有的协调与合作关系完成预定的任务。多机器人协调与合作作为一种新的机器人应用形式日益引起国内外学术界的兴趣与关注。1.2多臂机器人协调操作问题的分类及国内外研究现状1.2.1多臂机器人协调操作问题的分类及本文研究任务1一般来说，多臂机器人的协调操作问题可分为两种类型：松协调和紧协调。松协调是指多臂机器人在同一个工作空间中分别执行各自无关的作业任务；而紧协调是指多臂机器人在同一个工作空间内执行同一或多项作业任务。松协调任务的特点是在共享工作空间内，每一个机器人独立执行各自的任务，避碰路径规划是它的主要研究问题，比如，组装和拆卸操作，拧螺母作业，多臂抓持物体打磨操作等。紧协调任务的特点是多臂机器人是强耦合的，而目标物体的期望路径完全决定了每一个操作机械臂的操作空间运动轨迹。本文研究讨论的任务类型主要有四项：1)第一项任务是完成多轴机械臂控制程序的学习，实现原程序的动作。2)第二项任务是基于笛卡尔坐标系，用D-H及代数法进行机械臂的正逆运动学分析，并求得正逆运动学方程。3)第三项任务是用MATLAB编写串口函数及运动函数。4）第四项任务是用MATLAB软件仿真，实现双机械臂的协同工作。1.2.2国外研究进展目前，国外已经制定了多项多臂空间机器人的研究计划，典型的有美国的SUMO/FREND计划，NM5计划以及凤凰计划。对于自由浮动空间机器人地面实验平台的研制，美国和日本做了许多卓有成效的工作典型的实验平台包括：美国俄亥俄大学S.K.Agrawal等人研制的双臂自由浮游装配机器人模型、麻省理工大学的S.Dubowsky建立的PUMA560空间机器人实验平台VES-II等、美国斯坦福大学空间机器人实验室研制的双臂FFSR地面实验平台、日本东京大学Y.Umetani等人研制的二维自由飞行机器人卫星地面实验平台、日本电子实验室的K.Machida等人研制的参考宇航员的自由飞行双臂遥控空间机器人地面实验平台。1.2.3国内研究进展受许多相关技术和研究条件的制约，目前国内对多臂机器人的研究主要涉及运动轨迹规划，动力学以及协调控制等方面。在非空间机器人方面，较多的对普通的双机械臂机器人进行了研究，如上海交大机器人研究所对双臂机器人时间最优轨迹2规划问题作了深入研究。国防科大的研究人员提出了速度可操作性测度和力可操作性测度概念用于指导确定双臂机器人的最佳操作位姿。首钢莫托曼机器人有限公司开发了很对弧焊、电焊、涂胶、切割、搬运和码垛等高性能、高精度、高可靠性的机器人和应用系统。在空间机器人方面，哈尔滨工业大学的研究人员以双臂自由飞行空间机器人为背景的自主规划运动控制研究，北京航空航天大学机器人研究所设计搭建了面向空间舱作业的冗余度双臂空间机器人系统实验平台，较早的开展了冗余度双臂空间机器人的协调操作关键技术的研究。两个机械臂集成系统可以分别在两导轨上运动。32工业机器人系统概述2.1机械臂简单介绍工业机器人应用最广泛的形式是机械臂（MechanicalManipulator），其主要应用于工业装配领域（Assembly）。在工业应用中，机器人分为固定基座的机器人手臂（RobotManipulator）和移动基座的移动机器人（MobileRobot）。2.1.1机械臂的组成机械臂由臂（arm）、腕（Wrist）、末端工具（End-Effector）三部分组成。机器人的臂：一端安装在固定的基座上，另一端连接机器人的腕。机器人的腕：一端连接在机器人的臂上，另一端安装末端工具。末端工具：可以在空间中自由运动，执行机器人的工作任务。机器人的臂和腕，均由连杆和关节组成。连杆（Link）机器人手臂由多个连杆（Link）组成，连杆之间通过关节（Joint）连接，关节能够使得两个相邻连杆产生相对运动。连杆可以视为刚体（即运动时不会变形）。相邻关节之间的部分即为连杆。关节（Joint）两个相邻的连杆通过关节连接，关节运动导致连杆之间产生相对运动。典型关节有旋转关节（revolute）和平移关节（prismatic）两种。图2.1，给出了旋转关节和平移关节的几何示意图。4（a）旋转关节（b）平移关节图2.1旋转关节和平移关节旋转关节（用R表示）能够使得两个连杆之间沿着关节轴线产生相对旋转；平移关节（用P表示）能够使得两个连杆之间沿着关节轴线产生相对位移。在机器人建模中，两种关节的图形分别如图2.2和图2.3所示。图2.2旋转关节在机器人模型中的示意图图2.3平移关节在机器人模型中的示意图7轴智能化机器人手臂，有7个自由度，因此是具有运动学冗余的机器人手臂。机器人的臂（Manipulator或Arm）机器人的臂，是机器人的主体，由连杆（Link）、关节（Joint）构成。当将腕（Wrist）和末端工具（End-Effector）加在臂上，并具有相应的控制系统时，就构成了机器人。图2.4给出了具有3个旋转关节的机器人的臂。5图2.4具有3个旋转关节的机器人的臂机器人的腕（Wrist）在机器人的前臂和末端工具之间的所有关节，构成了机器人的腕（Wrist）。通常设计为球形腕关节（SphericalWrist），其由3个旋转关节组成，这3个旋转关节的关节轴线相交于同一点，该点称为腕关节点（WristPoint）。图2.5给出了球形腕关节示意图。图2.5球形腕关节示意图腕关节能够大大简化机器人的运动学分析，能够很方便的解耦末端工具的位置和方向。因此，一般来说，机器人的臂（有3个自由度）用于定末端工具的位置，末端工具方向的自由度取决于机器人的腕。腕的自由度可以是1、2或3，取决于工业应用的要求。末端工具（End-Effector）末端工具附着在机器人的腕的最后一个连杆上。最简单的末端工具是夹子，其只有两个动作：打开和闭合。取决于机器人应用要求，末端工具可以是夹子、焊枪等。6致动器（Actuator）致动器相当于机器人的肌肉，为机器人的运动提供力量。致动器提供的力量用于克服重力、惯性以及其他外力，从而改变末端工具的位置和方向。致动器也称为舵机。致动器有电子、液压和气动三种。液压和气动型致动器主要应用于力矩特别大、精度要求低的场合，电子型致动器用于高精度应用。传感器（Sensor）关节位置、速度、加速度、以及力，是最需要检测的量。传感器集成在机器人中，用于检测这些量，并将检测到的值传送到控制器中用于运动控制。控制器（Controller）控制器的作用有：（1）信息处理：收集及处理机器人传感器送来信息（关节位置、速度、加速度、力）。（2）位置伺服控制：综合速度、加速度，实现关节精确的位置伺服控制。（3）通信：能够与计算机进行通信，实现对机器人的控制。2.1.27轴智能化机器人手臂图2.6给出了7轴智能化机器人手臂实物图。如图2.6所示，两个肩关节、两个肘关节及连杆构成了机器人的臂（Arm），三个腕关节及连杆构成了机器人的腕（Wrist），末端工具由两个手指的夹子构成，用于夹取物体。7图2.67轴智能化机器人手臂实物图7轴智能化机器人手臂的所有关节，均为旋转关节。旋转动作：关节轴线与其下一级连杆在同一直线上。当关节旋转时，连杆也跟着旋转。一个关节连接两个连杆，一个连杆位于基座一边，另一个连杆位于末端工具一边。下一级连杆指的是末端工具一边的连杆。俯仰动作：关节轴线与其下一级连杆垂直。关节旋转时，导致连杆上下作俯仰动作。偏航动作：关节轴线与其下一级连杆垂直。关节旋转时，导致连杆左右摆动。7轴智能化机器人手臂的关节编号与连杆编号，如图2.7所示。关节编号通常与下一级连杆的编号相同。8图2.77轴智能化机器人手臂的关节编号与连杆编号图2.7中的尺寸单位为：毫米。图2.7中，手臂中的白线表示关节轴线。技术参数：（1）机械手臂垂直最大长度:53.4cm(从基座到顶端)（2）机械手臂水平最大长度:48cm（3）重复性位置精度：+/-0.5mm（4）手指最大张度：3.5cm（5）角度限制，见表2.1。表2.1关节角度限制9顺时针角度限制逆时针角度限制关节编号关节名称单位（度）单位（弧度）单位（度）单位（弧度）关节1肩关节旋转ShoulderRoll341C341R3755C3755R关节2肩关节俯仰ShoulderPitch853C853R3243C3243R关节3肘关节旋转ElbowRoll341C341R3755C3755R关节4肘关节俯仰ElbowPitch853C853R3243C3243R关节5腕关节偏航WristYaw853C853R3243C3243R关节6腕关节俯仰WristPitch853C853R3243C3243R关节7腕关节旋转WristRoll341C341R3755C3755R末端工具手指00515C515R说明：传感器将一个圆周分为4096份，因此，已知传感器的角度数字量，转换成度或孤度的系数为：系数C=3604096（单位：度）；系数R=24096（单位：rad）2.1.3机械臂的通信方式将7轴智能化机器人手臂与计算机连接起来，通过特定的通信协议，实现计算机对7轴智能化机器人手臂的各种控制。7轴智能化机器人手臂与计算机的连接，如10图2.8所示。图2.87轴智能化机器人手臂与计算机的连接7轴智能化机器人手臂与计算机的连接，只需要一条USB线，一端插入计算机的USB端口，另一端插入机械臂的USB接口即可。用机器人专用的电源适配器，给机器人供电后，就可以通过计算机控制机器人了。具体步骤如下：第一步：接通机械臂电源。将机械臂配备的专用电源适配器的一端连接220V市电插座，另一端连接机械臂的电源输入插座。第二步：将机械臂与PC进行连接。将通用的USB连接线，一端连接机器人的USB接口，另一端连接PC机的USB端口。上电以后，指示灯会闪烁一次。图2.9，给出了7轴智能化机器人手臂配件及其连接的实物图。图2.97轴智能化机器人手臂配件及其连接的实物图11机器人的USB端口是一个虚拟串行端口（VSP，VirtualSerialPort），因此，在计算机上，需要采用软件的方法，把计算机的USB端口模拟成串行端口，这个软件就是机器人专用的驱动程序。安装好驱动程序、连接好数据线、并给机器人上电后，就可以在计算机的设备管理器中，看到系统为该虚拟串行端口分配的端口号。不同的计算机系统，所分配的端口号不一定相同。如图2.10所示。图2.10操作系统给USB虚拟串口分配的端口号机器人的USB端口是一个虚拟串行端口（VSP，VirtualSerialPort），因此，在计算机上，需要采用软件的方法，把计算机的USB端口模拟成串行端口，这个软件就是机器人专用的驱动程序。安装好驱动程序、连接好数据线、并给机器人上电后，就可以在计算机的设备管理器中，看到系统为该虚拟串行端口分配的端口号。不同的计算机系统，所分配的端口号不一定相同。如图2.10所示机器人控制每个关节的转动是通过致动器（Actuator），也称为舵机。舵机是一个位置伺服电机，由微控制器、位置传感器、电机、变速齿轮组成，封装在机器人的连杆中。微控制器中包含有控制程序，该控制程序能够执行计算机传送过来的命令，也能将机器人的状态传回计算机中。机器人的舵机与舵机之间，通过3线电缆连接。其中，2根线为电源线，为舵机供电，另一根为通信线。如图2.8所示。用3线电缆将机器人的USB接口、以及每个舵机顺序串联起来，计算机就可以12任意控制每个舵机了。7轴智能化机器人手臂，有8个舵机。其中7个舵机构成机器人的臂和腕，另1个舵机为末端工具，即夹子（或手爪、手指）。位置传感器将旋转关节旋转的位置分成4096份（12位表示212=4096，即将360度分成了4096等份），因此，关节旋转的角度位置，通常用角度位置的数字量表示，该数字量与角度位置的关系为：计算机与机器人的各个舵机构成一个主从式通信网络，计算机是主机，舵机是从机。每个从机都有一个唯一的编号，称为ID。主机可以通过舵机的ID准确控制指定的舵机。每个舵机有一个红色的LED指示灯，用于指示相应的舵机是否上电。2.1.4机械臂的控制方式机械臂的控制表，其实质是舵机的控制表。在该控制表中，存储着舵机当前的工作状态和操作命令。机械臂的控制方式是：读取舵机控制表中的状态参数，可以得知舵机的工作状态；设置舵机控制表中命令参数的值，可以控制舵机执行指定的操作。每个舵机内部有两种存储器：EEPROM和RAM。EEPROM能够永久存储数据（即掉电后仍能保持数据），RAM只能在上电时才能保存数据，掉电后不再保持数据。上电后，系统自动给RAM赋一个初值。舵机的控制表就分别存储在这两种存储器中。2.2坐标变换机器人的运动学是描述机器人运动的几何学。在机器人固定不动的基座，建立固定坐标系G；在机器人的每个连杆都建立一个局部坐标系B，连杆的运动，导致局部坐标系B也运动。将点P在B中的坐标，变换成在G中的坐标，称为坐标正变换；反之称为坐标逆变换。360=49关节旋转的角度位置数字量132.2.1坐标正变换坐标正变换公式：BBdrRrG：点P在B中的坐标列向量。r：点P在G中的坐标列向量。：将B的原点平移到G的某个点，并用该点在G中的坐标列向量表示。d：B和G刚开始重合，将B旋转后，B中的点的坐标，变换为G中的点的R坐标的旋转变换矩阵。R表示Rotation，旋转。2.2.2B绕G的坐标轴多次连续旋转的坐标正变换B绕G的坐标轴连续多次旋转，如何得到B到G的旋转矩阵?如果已知点P在B中的坐标，怎么计算点P在G中的坐标?现进行如下介绍：如果B绕G的坐标轴连续多次旋转，旋转矩阵按旋转的先后顺序分别为：Q1，Q2，Qn，则将点P在B中的坐标，转换为G中的坐标，公式为：rQrB这里，123.nQBGr：点P在G中的坐标。Br：点P在B中的坐标。GQB：B到G的旋转矩阵。将点P在B中的坐标，变换为G中的坐标。因为矩阵的乘法不满足交换律，因此，执行旋转的顺序是重要的。旋转矩阵是正交矩阵，其转置矩阵与其逆矩阵，是相等的。1QT正交矩阵一般用字母Q表示。因此：多次旋转，计算旋转变换矩阵时，一定要注意矩阵的乘法与旋转顺序的关系。142.2.3B绕G的坐标轴旋转的正变换与逆变换的关系B绕G的坐标轴旋转，已知点P在B中的坐标，计算点P在G中的坐标，这种B到G的旋转变换，称为旋转正变换。反过来，已知点P在G中的坐标，计算点P在B中的坐标，这种G到B旋转变换，称为旋转逆变换。设B到G的旋转矩阵为GRB，G到B的旋转矩阵为BRG，两者的关系有：TBRR1即：GRB的转置矩阵，即为BRG，同样，BRG的转置矩阵，即为GRB。2.2.4刚体运动设固定坐标系G（OXYZ），局部坐标系B（Oxyz）。设G和B刚开始是重合的。B先绕G的坐标轴旋转，然后B的原点又相对于G的原点平移到G中的某个点。如图2.11所示：如果用Gd表示将B的原点平移到G中的某个点，B中点P的坐标变换到G中的坐标的公式为：dRrpBrp这里，Gd：B相对于G平移后，B的原点在G中的坐标。GRB：Gd是当=0时，将B变换到G的旋转矩阵。Br：点P在B中的坐标，列向量。Gr：点P在G中的坐标。刚体旋转和平移的组合，称为刚体的运动。PrpZYXOdZYXprzyx15图2.11刚体运动示例图2.2.5齐次变换刚体的运动由旋转和平移两部分组成，旋转矩阵是33的矩阵，平移是坐标列向量，将这两者综合成一个矩阵，可以完整描述刚体的运动。该矩阵称为齐次变换矩阵，是一个44的矩阵。rTrBG这里，101032311dRTZrYXGBB且，Gr,Br,Gd称为齐次坐标。任意一个齐次变换矩阵，总能分解成平移齐次变换矩阵乘以旋转齐次变换矩阵。1prZYXG1przyxB10ZYXGd1610101032311423432121rrRDrrT2.2.6齐次逆变换已知GTB，求BTG，这样，就能够根据点P在G中的坐标，计算点P在B中的坐标。10101dRdRTGBTBTBBB2.2.7复合齐次变换假设有3个坐标系A,B,C。有如下齐次变换矩阵：1021dRTdTBACCBB则有，101002221dRdRRdRdRTTAABACCBCBCBC其逆变换为，1710102212dRdRddRTABACTCTCTCTCTBBTCTCA一般，多个坐标系与固定坐标系的齐次变换公式，如下：43214TGT183机器人建模与正运动学分析3.1概述对于实际的机械臂，其连杆之间的几何关系是给定的。只要知道了各个关节变量的值，总是能够确定机械臂每个连杆相对于固定参考坐标系的位置与朝向。对基座、每个连杆以及末端工具都指定一个坐标系，可以用刚体运动的方法，分析相邻连杆两个坐标系之间的坐标变换关系，用复合齐次变换方法，可以得到任一连杆以及末端工具相对于固定坐标系（基座）的位置与朝向，这种分析称为正运动学分析。3.2D-H方法与齐次变换矩阵3.2.1D-H方法在机器人学中，描述两个相邻连杆坐标系之间的几何关系，通常使用D-H参数。这种方法是由JacquesDenavit和Ri-chardS.Hargenberg两人在1955年发明的，因此，称为D-H参数，确定D-H参数的方法，称为D-H方法。在机器人学中，使用D-H方法为机器人指定各个坐标系，并确定相邻两个坐标系之间的几何位置关系，即D-H参数。D-H方法包含两个内容：（1）指定坐标系（2）确定两个相邻坐标系D-H参数3.2.2齐次变换矩阵1.计算相邻坐标系之间的齐次变换矩阵iiiiiiiixaxzdziiRDRT,11111192.计算末端工具相对于固定参考坐标系的齐次变换矩阵。nnqTqTqT143210.3.47轴智能化机器人手臂的D-H参数7轴智能化机器人手臂，7个关节都是旋转关节。7轴智能化机器人手臂的D-H参数表：L1=alpha,A,theta,D=pi/2,0,-pi/2,120L2=alpha,A,theta,D=pi/2,0,pi,0L3=alpha,A,theta,D=pi/2,0,pi,140.8L4=alpha,A,theta,D=-pi/2,71.8,pi/2,0L5=alpha,A,theta,D=pi/2,71.8,0,0L6=alpha,A,theta,D=pi/2,0,pi/2,0L7=alpha,A,theta,D=0,0,0,129.63.5.轴智能化机器人手臂的建模与仿真3.5.1创建机械臂各连杆的模型根据D-H参数，使用link函数创建机械臂各连杆的模型。格式：L=link(alphaAthetaDsigmaoffset,CONVENTION)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数组。203.5.2创建机械臂模型根据连杆模型，使用robot函数创建机械臂模型有了机械臂的连杆模型之后，就可以用robot函数创建机械臂的计算模型。格式：模型名称=robot(LINK,name)LINK:就是用link函数创建的各连杆模型。name：机械臂的名字，为字符串。创建7轴智能化机器人手臂模型的函数：functionR=efircmdl3.6正运动学分析正运动学的主要问题是：根据各个关节旋转的角度，确定末端工具在笛卡尔坐标系中的位置（即坐标）和朝向。在机器人学中，使用D-H方法为每个连杆及末端工具建立一个坐标系，根据D-H参数能够得到相邻两个坐标系的齐次矩阵；然后通过复合齐次变换，就能够得到末端工具坐标系到固定参考坐标系的齐次变换矩阵。设机械臂有n个关节，则末端工具坐标系到固定参考坐标系的齐次矩阵为：nnqTqTqT143210.表示关节1的关节变量取值为q1时，坐标系1到坐标系0的齐次矩阵。10qT表示关节2的关节变量取值为q2时，坐标系2到坐标系1的齐次矩阵。2以此类推，表示关节n的关节变量取值为qn时，坐标系n到坐标系n-1的齐次矩nq1阵。这样，通过复合齐次变换，就可以得到坐标系n到坐标系0（即固定参考坐标系）的齐次矩阵。nT0每对相邻坐标系i和i-1的齐次矩阵，可用以下公式得到：iiiiiiiixaxzdziRDRT,11111右边各个齐次矩阵的公式为：2110100cossini1,11ixixiiDR10100cossini,11idzizdDRii这种坐标变换，称为齐次坐标变换。必须使用齐次坐标。使用fkine函数，可以根据机械臂模型以及每个关节的角度值，计算末端工具到固定坐标系的齐次矩阵。格式：TR=fkine(ROBOT,Q)224机器人建模与逆运动学分析4.1概述正运动学分析是指：已知各个关节变量的值，计算末端工具在固定参考坐标系中的位置与朝向（也称为姿态）。反过来，已知末端工具在固定参考坐标系中的位置与姿态，计算各个关节变量的值。这种分析称为逆运动学分析。4.2逆运动学分析的一般方法机械臂运动学逆解的方法可以分为两类：封闭解和数值解。封闭解是直接求出关节角度与末端位置与姿态之间的表达式。在进行逆解时总是力求得到封闭解。因为封闭解的计算速度快，效率高，便于实时控制。而数值法不具有些特点。数值解是当机械臂的末端位置与姿态已知时，用迭代的方法得到机械臂的各个关节角度的值，一般是近似的。机械臂的运动学逆解封闭解可通过两种途径得到：几何解法和代数解法。几何解法是根据机械臂的几何结构，找出机械臂末端位置与姿态与各关节角之间的关系式。代数解法中常用的有位置姿态分解法，逆变换法。如果对于机械臂某个特别的位姿，逆解不存在，称这个位姿为奇异位姿。机械臂的奇异位姿分为两类：（1）边界奇异位姿（2）内部奇异位姿如果机械臂的关节变量的个数大于6，称为冗余的，这时，对于机械臂末端的一个位置与姿态，可能有无穷多个关节值与之对应。因此，对于冗余的机械臂的逆解可能有无穷多个。1.位置姿态分解法对于具有某些特殊结构的机械臂，可以将逆运动学问题分解为逆位置和逆姿态问题。也就是说，利用6个方程中的3个求出机械臂末端位置所需要的3个关节角，23用其它3个方程得到机械臂末端的姿态。(4.1)101066066RdIRDdT表示机械臂末端位置相对于基坐标系的平移变换阵，与机械臂的3个关节有60D关，其中只有3个未知数。表示机械臂末端姿态相对于基坐标系的旋转变换阵，60R与机械臂的其它3个关节有关系，其中也只有3个未知数。2.逆变换方法设表示一个6自由度机械臂的末端相对于基础坐标系的位置和姿态，根据机60T械臂的正向运动学，我们有(4.2)10)()()(343212165432106rrqTqTq这时，可以通过求解以下矩阵方程解决逆运动学问题(4.3)102134156623450116602316601TTIT对于某个特定的机械臂，这些方程不一定要全部用到，求解时，应该根据具体情况，选择其中一些比较简单的。3.数值迭代方法一般来说，机械臂的逆运动学解qk(k=1,2,n)可以通过求解以下非线性方程组得到。(4.4)10)()()(343212112100rrqTqTnn方程(4.4)是一个超越方程，可以使用数值方法得到方程的零点。然而，一般来24说要使用迭代算法。最常用的是Newton-Raphson方法。求得TJq1写成迭代公式(4.5)()(1)()1(kkkkq实现(4.5)的算法如下（1）设k=0,给出关节的初始值；)0(（2）计算)()(kkqJT（3）计算；)(11kT（4）如果，计算结束，就是希望的迭代解；)(k1q（5）取k=k+1,转（2）继续计算。4.3机械臂末端的奇异位姿考虑图4.1中的两个自由度的平面机械臂的逆运动学问题图4.1两个自由度的平面机械臂根据机械臂的正向运动学，这个机械臂的传递矩阵是(4.6)100)()()(22212111210slcscscT25由（4.6）中第1,2行，第4列可知，机械臂末端的位置是(4.7)sin(ico2121llyx对（4.7）两边微分后写成矩阵形式得到(4.8)2112121)cos()cos(inisidllldyx记21212121)cos()cos(ininsidlllJdyxp（4.8）简写成，式中J就称为机械臂的雅可比（Jacobian）矩阵。可以看出，雅可比矩阵的每一列表示其它关节不动而某一关节以单位速度运动产生的末端速度。从(4.9)cos()cos(ininsi212121lllJ可以看出，J的值随末端位置的不同而不同，1和2的改变会导致J的变化。对于关节空间的某些位姿，机械臂的雅可比矩阵的秩减少，这些位姿称为机械臂的奇异位姿。如果机械臂的雅可比矩阵J是满秩的方阵，相应的关节速度即可求出，即，机械臂的逆雅可比矩阵是xJ1(4.10)cos(sin)c