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

基于视觉的多轴机械臂的运动控制分析摘要:机器人技术在工业生产中运用的越来越多，尤其在一些高危及环境恶劣的工业中应用广泛。但由于其技术不够成熟，导致现在许多工业机器人的灵活度差，工作内容单一。视觉机器人相较于一般的机器人，信息获取量、检测精度、检测范围都有大幅度提升，解决了机器人只能实现点到点工作的问题，使机器人具有自主检测和判断的能力。本文以EF-IRC-I机械臂及机器人手臂视觉识别控制系统为基础，实现了对基于视觉的七轴机械臂的运动控制分析。首先对机械臂的运动学进行了分析，求解正、逆运动学方程；并通过视觉软件对目标物进行了标定，用于识别待抓取物体的位置。最终通过目标物体的位置用机械臂控制软件完成了抓取搬运实验。关键词：七自由度机械臂，机器视觉，运动控制，图像处理IMotionControlAnalysisofMulti-axisMechanicalArmBasedontheVisualAbstract:Moreandmorerobotictechnologiesareusedinindustrialproductionespeciallyinhigh-riskindustriesandsomeharshenvironment.Becauseofitstechnologyisnotcomplete,manyindustrialrobotscanonlydosomesimpleworks.Comparedwithgeneralrobots,visualrobotdoeswellingainingtheinformation,improvingtheaccuracyofchecking,enlargingtherangeofdetection.Visualrobotsolvedtheproblemthatgeneralrobotcanonlyworktorealizethepoint-to-point,makerobothastheabilityofautonomousdetectionandjudgment.TherealizationofsevenaxismotioncontrolanalysisofmechanicalarmisbasedontheEF-IRC-Irobotarmmanipulatorandvisualidentificationbasedcontrolsystem.Firstofall,analysesthekinematicsofroboticarm,positiveandinversekinematicsequation.Thenthevisualsoftwareidentifythepositionoftheobject.Finallythroughthelocationofthetargetobject,mechanicalarmcompletedtheexperimentaboutcarryingoutandgraspingwithcontrolsoftware.Keywords:sevendegreesoffreedommechanicalarm,machinevision,kinematicsanalysisandimageprocessing.II目录1.前言.11.1.选题背景及意义.11.2国内外研究现状.11.2.1国外研究现状.11.2.2国内研究现状.22.工业机器人的组成及运动控制分析.42.1机器人的组成.42.2坐标变换.102.2.1坐标正变换公式.102.2.2B绕G的坐标轴多次连续旋转的坐标正变换.112.2.3B绕G的坐标轴旋转的正变换与逆变换的关系.112.2.4刚体运动.122.2.5齐次变换.132.2.6齐次逆变换.142.2.7复合齐次变换.142.2.8D-H参数.152.3正运动学分析.182.4逆运动学分析.202.4.1奇异为姿.202.4.2数值迭代方法.212.4.3机械臂末端的奇异位姿.233.机器人路径规划.273.1机械臂的变量空间.273.1.1、关节空间.273.1.2操作空间.273.1.3驱动空间.273.2关节空间路径规划的基本方法.283.2.1三次多项式函数.28III3.2.2点到点的路径规划.293.2.3关节空间中的连续路径的路径规划问题.313.3操作空间路径规划的基本方法.324.目标标定及机械臂抓取.354.1图像获取及目标物的识别.354.1.1图像获取.354.1.2目标物的识别.354.1.3目标标定.364.1.4目标物标定具体步骤.374.2机械臂抓取.404.2.1机械臂抓取动作.404.2.2机械臂抓取的具体步骤.414.3本章小结.42结论.43参考文献.44致谢.46附录.4701.前言1.1.选题背景及意义随着计算机技术的发展，机器人已悄然进入我们生活的各个方面。可以帮助人类完成高危、繁重、以及人本身无法完成的工作。机器人是典型的机、电、控制一体化的产品，经过50多年的发展，机器人在工业、农业、国防、医疗、社会服务等各领域应用广泛，而且随着科技日新月异，许多功能在机器人上得以实现，比如：触觉、视觉与思维能力1。机器人的研究已经成为一项重要的课题，国内外都在努力研发更高效、先进的机器人以创造更高的经济效益，同时其研究方向也向大众化发展，让一些简单的机器人给人们的生活带来便利，在许多高校中针对机器人的课程以及创新项目也屡见不鲜。机器人在我们的生活中发挥了很大的作用，同时我们也应该看到其不足之处，目前工业生产中用的机器人，大多数只能实现点到点的动作，灵活性较差。而许多工作要求具有高精度同时机器人可以自主检测和判断，所以需要对这类机器人进行研发。视觉机器人主要计算机通过拍摄的图像对需要进行的操作进行自我判断，相较于一般的机器人其灵活性加强了，减少了人工操作，缩短了完成动作的时间，同时通过视觉处理，人们可以做出更准确的判断。1.2国内外研究现状1.2.1国外研究现状机器人技术的迅速发展，人们开始期望拥有“思维”能力的机器人，能够在不同的环境中自己做出判断，并执行相应的动作。上世纪六十年代中期，美国学者L.R.罗伯兹提出视觉机器人2。当时运用的预处理、边缘检测、对象建模、匹配等技术，后来一直在机器视觉的研究中继续使用。视觉机器人产生于二十世纪八十年代。机器视觉可以为机器人传送周围环境的图像信息，使机器人更加准确的进行判断，提高其灵活性并拓宽了其工作范围。目前，美国的机器人技术处于世界领先地位，而日本的机器人生产数量居世界首位3。美国斯坦福研究所（StanfordResearchInstitute）很早就研制成功了一个做SHAKRY的机器人4，这是典型的“眼车”系统。该机器人主要用于识别周围的目标物体，通过计算得到最优路径，从而达到识别和跟踪目标物体的目的。1SHAKRY还能在房间中自由穿行，进行一些“智能”操作。图1.1IRB340工业机器人ABB公司研发了一款IRB340机器人，和一套名叫FlexPicker的视觉系统5。这款工业机器人是并联型分拣机器人，只有4个轴，但是却可以每分钟做150次分拣活动。在FlexPicker视觉系统的引导下，利用安装在末端的真空手爪，机器人可以抓取传送带上的移动物体并将物体放到预先设定好的位置。1.2.2国内研究现状相较于国外的机器人技术而言，我国机器视觉技术发展缓慢。随着我国经济的快速发展，我国生产行业对机器人的需求不断增加，但是我国本土的机器人在性能与功能上都还不够完善，这引起了我国的重视，国家鼓励机器人的开发与研究，同时积极与国外的先进技术接轨，。视觉机器人在发展的过程中展现了其重要性，并带来了巨大的经济效益，成为我国机器人研究的重要方向。东华大学与德国TreiBoos公司共同研制了一款鸡蛋拾取机器人6。鸡蛋拾取夹具的制造运用了3D增量制造技术，夹具上配备具有力矩控制功能的SMC电缸，能够设定拾取力矩，方便机器人在高速拾取的过程中感知和控制夹具的力矩。该机器人是在视觉系统的引导下完成的，视觉系统对鸡蛋进行识别定位，实现自动拾取和放置。2图1.3电缸力矩控制鸡蛋拾取机器人32.工业机器人的组成及运动控制分析2.1机器人的组成工业机器人应用最广泛的形式是机械臂（MechanicalManipulator），其主要应用于工业装配领域（Assembly）。在工业应用中，机器人分为固定基座的机器人手臂（RobotManipulator）和移动基座的移动机器人（MobileRobot）。机器人手臂与人的手臂类似，由臂（arm）、腕（Wrist）、末端工具（End-Effector）三部分组成。机器人的臂：一端安装在固定的基座上，另一端连接机器人的腕。机器人的腕：一端连接在机器人的臂上，另一端安装末端工具。末端工具：可以在空间中自由运动，执行机器人的工作任务。机器人的臂和腕，均由连杆和关节组成。连杆（Link）机器人手臂由多个连杆（Link）组成，连杆之间通过关节（Joint）连接，关节能够使得两个相邻连杆产生相对运动。连杆可以视为刚体（即运动时不会变形）。相邻关节之间的部分即为连杆。关节（Joint）两个相邻的连杆通过关节连接，关节运动导致连杆之间产生相对运动。典型关节有旋转关节（revolute）和平移关节（prismatic）两种。图2.1，给出了旋转关节和平移关节的几何示意图。旋转关节（用R表示）能够使得两个连杆之间沿着关节轴线产生相对旋转；平移关节（用P表示）能够使得两个连杆之间沿着关节轴线产生相对位移。描述两个相邻连杆的相对位置的量，称为关节变量。对于旋转关节，该关节变量是角度；对于平移关节，该关节变量是位移7。机器人一般在关节处安装有传感器（Sensor），用于测量关节变量的具体数值。4（a）旋转关节（b）平移关节图2.1旋转关节和平移关节在机器人建模中，两种关节的图形分别如图2.2（a-c）和图2.3(a-c)所示。图2.2旋转关节在机器人模型中的示意图图2.3平移关节在机器人模型中的示意图机器人手臂自由度的个数等于独立关节变量的个数。机器人手臂的每个关节提供了一个自由度，因此，机器人手臂关节的数量就是该机器人手臂自由度的个数。典型的机器人手臂应该至少拥有6个自由度，3个自由度用于定末端工具的位置、3个自由度用于定末端工具的方向。如果机器人手臂多于6个自由度，则称其为具有运动学冗余的机器人手臂。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）致动器相当于机器人的肌肉，为机器人的运动提供力量。致动器提供的力量用于克服重力、惯性以及其他外力，从而改变末端工具的位置和方向。致动器也称为舵机。致动器有电子、液压和气动三种。液压和气动型致动器主要应用于力矩特别大、精度要求低的场合，电子型致动器用于高精度应用8。传感器（Sensor）关节位置、速度、加速度、以及力，是最需要检测的量。传感器集成在机器人中，用于检测这些量，并将检测到的值传送到控制器中用于运动控制。控制器（Controller）控制器的作用有：（1）信息处理：收集及处理机器人传感器送来信息（关节位置、速度、加速度、力）。（2）位置伺服控制：综合速度、加速度，实现关节精确的位置伺服控制。（3）通信：能够与计算机进行通信，实现对机器人的控制。图2.67轴智能化机器人手臂实物图如图2.6所示，两个肩关节、两个肘关节及连杆构成了机器人的臂（Arm），三个腕关节及连杆构成了机器人的腕（Wrist），末端工具由两个手指的夹子构成，用7于夹取物体。7轴智能化机器人手臂的所有关节，均为旋转关节。旋转动作：关节轴线与其下一级连杆在同一直线上。当关节旋转时，连杆也跟着旋转。一个关节连接两个连杆，一个连杆位于基座一边，另一个连杆位于末端工具一边。下一级连杆指的是末端工具一边的连杆。俯仰动作：关节轴线与其下一级连杆垂直。关节旋转时，导致连杆上下作俯仰动作。偏航动作：关节轴线与其下一级连杆垂直。关节旋转时，导致连杆左右摆动。7轴智能化机器人手臂的关节编号与连杆编号，如图2.7所示。关节编号通常与下一级连杆的编号相同。图2.77轴智能化机器人手臂的关节编号与连杆编号图2.7中的尺寸单位为：毫米。图2.7中手臂中的白线表示关节轴线。技术参数：（1）机械手臂垂直最大长度:53.4cm(从基座到顶端)（2）机械手臂水平最大长度:48cm8（3）重复性位置精度：+/-0.5mm（4）手指最大张度：3.5cm（5）角度限制，见表1。表2.1关节角度限制顺时针角度限制逆时针角度限制关节编号关节名称单位（度）单位（弧度）单位（度）单位（弧度）关节1肩关节旋转ShoulderRoll341C341R3755C3755R关节2肩关节俯仰ShoulderPitch853C853R3243C3243R关节3肘关节旋转ElbowRoll341C341R3755C3755R关节4肘关节俯仰ElbowPitch853C853R3243C3243R关节5腕关节偏航WristYaw853C853R3243C3243R关节6腕关节俯仰WristPitch853C853R3243C3243R关节7腕关节旋转WristRoll341C341R3755C3755R末端工具手指00515C515R说明：传感器将一个圆周分为4096份，因此，已知传感器的角度数字量，转换成度或孤度的系数为：9系数C=3604096（单位：度）（2.1）系数R=24096（单位：rad）（2.2）7轴智能化机器人手臂，有8个舵机。其中7个舵机构成机器人的臂和腕，令1个舵机为末端工具，即夹子（或手爪、手指）。位置传感器将旋转关节旋转的位置分成4096份（12位表示212=4096，即将360度分成了4096等份），因此，关节旋转的角度位置，通常用角度位置的数字量表示，该数字量与角度位置的关系为：（2.3）360=49关节旋转的角度位置数字量每个关节的旋转角度，是有限制的。见表1关节角度限制。计算机与机器人的各个舵机构成一个主从式通信网络，计算机是主机，舵机是从机9。每个从机都有一个唯一的编号，称为ID。主机可以通过舵机的ID准确控制指定的舵机。计算机既可以单独控制一个舵机，也可以同时控制多个舵机或所有舵机，因此，控制非常灵活，功能强大，这个特点对于提高机器人的控制性能来讲，是非常重要的一个特色与功能。图2.8机器人与计算机的连接2.2坐标变换机器人的运动学是描述机器人运动的几何学10。在机器人固定不动的基座，建立固定坐标系G；在机器人的每个连杆都建立一个局部坐标系B，连杆的运动，导致局部坐标系B也运动。将点P在B中的坐标，变换成在G中的坐标，称为坐标正变换；反之称为坐标逆变换。2.2.1坐标正变换公式（2.4）10Br：点P在B中的坐标列向量。Gr：点P在G中的坐标列向量。GdB：将B的原点平移到G的某个点，并用该点在G中的坐标列向量表示。GRB：B和G刚开始重合，将B旋转后，B中的点的坐标，变换为G中的点的坐标的旋转变换矩阵。R表示Rotation，旋转。2.2.2B绕G的坐标轴多次连续旋转的坐标正变换B绕G的坐标轴连续多次旋转，如何得到B到G的旋转矩阵?如果已知点P在B中的坐标，怎么计算点P在G中的坐标?现进行如下介绍：如果B绕G的坐标轴连续多次旋转，旋转矩阵按旋转的先后顺序分别为：Q1，Q2，Qn，则将点P在B中的坐标，转换为G中的坐标，公式为：（2.5）这里，（2.6）Gr：点P在G中的坐标。Br：点P在B中的坐标。GQB：B到G的旋转矩阵。将点P在B中的坐标，变换为G中的坐标。因为矩阵的乘法不满足交换律，因此，执行旋转的顺序是重要的。旋转矩阵是正交矩阵，其转置矩阵与其逆矩阵，是相等的。（2.7）正交矩阵一般用字母Q表示。因此，多次旋转，计算旋转变换矩阵时，一定要注意矩阵的乘法与旋转顺序的关系。2.2.3B绕G的坐标轴旋转的正变换与逆变换的关系B绕G的坐标轴旋转，已知点P在B中的坐标，计算点P在G中的坐标，这种B到G的旋转变换，称为旋转正变换11。反过来，已知点P在G中的坐标，计算点P在B中的坐标，这种G到B旋转变换，称为旋转逆变换。设B到G的旋转矩阵为GRB，G到B的旋转矩阵为BRG，两者的关系有：（2.8）11即：GRB的转置矩阵，即为BRG，同样，BRG的转置矩阵，即为GRB。2.2.4刚体运动设固定坐标系G（OXYZ），局部坐标系B（Oxyz）。设G和B刚开始是重合的。B先绕G的坐标轴旋转，然后B的原点又相对于G的原点平移到G中的某个点。如下图所示：图2.9刚体运动示例图如果用Gd表示将B的原点平移到G中的某个点，B中点P的坐标变换到G中的坐标的公式为：（2.9）这里，Gd：B相对于G平移后，B的原点在G中的坐标。GRB：Gd是当=0时，将B变换到G的旋转矩阵。Br：点P在B中的坐标，列向量。GrP：点P在G中的坐标。刚体旋转和平移的组合，称为刚体的运动。将刚体的运动分解成先旋转再平移，是表示空间位移最简单的方法。2.2.5齐次变换刚体的运动由旋转和平移两部分组成，旋转矩阵是33的矩阵，平移是坐标12列向量，将这两者综合成一个矩阵，可以完整描述刚体的运动。该矩阵称为齐次变换矩阵，是一个44的矩阵。（2.10）这里，（2.11）且，Gr,Br,Gd称为齐次坐标。任意一个齐次变换矩阵，总能分解成平移齐次变换矩阵乘以旋转齐次变换矩阵。（2.12）2.2.6齐次逆变换已知GTB，求BTG，这样，就能够根据点P在G中的坐标，计算点P在B中的坐标。13（2.13）2.2.7复合齐次变换假设有3个坐标系A,B,C。有如下齐次变换矩阵：（2.14）（2.15）则有，（2.16）其逆变换为，（2.17）一般，多个坐标系与固定坐标系的齐次变换公式，如下：（2.18）2.2.8D-H参数对于实际的机械臂，其连杆之间的几何关系是给定的。只要知道了各个关节变14量的值，总是能够确定机械臂每个连杆相对于固定参考坐标系的位置与朝向。在机器人学中，描述两个相邻连杆坐标系之间的几何关系，通常使用D-H参数。这种方法是由JacquesDenavit和Ri-chardS.Hargenberg两人在1955年发明的，因此，称为D-H参数，确定D-H参数的方法，称为D-H方法12。在3维空间中，要确定一个3维的坐标系，只需要确定其中两个坐标轴，另一个坐标轴可以使用右手螺旋定则确定13。如，只需要确定3维坐标系的Z轴和X轴，Y轴可用右手螺旋定则确定。用右手握住坐标系的Z轴，大拇指指向Z轴的正方向，四指的绕向即是X轴的正方向绕向Y轴的正方向，绕过的角度为90度。因此，要描述两个坐标系之间的相对关系，只需要描述两个坐标系中两对坐标轴之间的相对关系。D-H参数描述了两个坐标系中两个Z轴之间的相对关系和两个X轴之间的相对关系。设机械臂中，两个相邻连杆的几何关系以及指定的坐标系，如下图所示。图2.10两个相邻连杆的几何关系以及指定的坐标系D-H参数名称及含义：Linklengthi：连杆长度。描述了两个相邻坐标系i和i-1的两个Z轴之间的距离。Linktwisti：连杆扭角。描述了两个相邻坐标系i和i-1的两个Z轴之间的夹角。特别注意夹角如何取值：坐标系i-1的Z轴的正方向，绕坐标系i的X轴的正15方向，按右手螺旋法则旋转，使坐标系i和i-1的两个Z轴正方向一致。绕X轴正方向按右手螺旋法则旋转，角度为正值；绕X轴负方向按右手螺旋法则旋转，角度为负值。Jointdistancedi：关节距离。也称为连杆偏移。描述了两个相邻坐标系i和i-1的两个X轴之间的距离。Jointanglei：描述了两个相邻坐标系i和i-1的两个X轴之间的夹角。特别注意夹角如何取值：坐标系i-1的X轴的正方向，绕坐标系i-1的Z轴的正方向，按右手螺旋法则旋转，使坐标系i和i-1的两个X轴正方向一致14。绕Z轴正方向按右手螺旋法则旋转，角度为正值；绕Z轴负方向按右手螺旋法则旋转，角度为负值。D-H参数描述了两个相邻坐标系i与固定参考坐标系i-1之间的几何位置关系。在机器人学中，通常用D-H参数表来描述机器人的几何结构。坐标系i到坐标系i-1齐次变换矩阵为：（2.19）方程左边的符号，表示坐标系i到i-1的齐次变换矩阵；方程右边，第1项表示沿坐标系i-1的Z轴，平移d的齐次变换矩阵；第2项表示沿坐标系i-1的Z轴，旋转角度的齐次变换矩阵；第3项表示沿坐标系i的X轴，平移a的齐次变换矩阵；第4项表示沿坐标系i的X轴，旋转角度的齐次变换矩阵。其中，（2.20）（2.21）16（2.22）（2.23）因此，坐标系i中点的坐标变换到坐标系i-1中的坐标，公式为：（2.24）这里，（2.25）i-1Ti也能够分解为两部分：旋转矩阵和平移部分。（2.26）这里，（2.27）（2.28）i-1Ti的逆矩阵为：17（2.29）根据公式：计算相邻坐标系的齐次矩阵，可以用函数。（2.30）2.3正运动学分析正运动学的主要问题是：根据各个关节旋转的角度，确定末端工具在笛卡尔坐标系中的位置（即坐标）和朝向。在机器人学中，使用D-H方法为每个连杆及末端工具建立一个坐标系，根据D-H参数能够得到相邻两个坐标系的齐次矩阵；然后通过复合齐次变换，就能够得到末端工具坐标系到固定参考坐标系的齐次变换矩阵。设机械臂有n个关节，则末端工具坐标系到固定参考坐标系的齐次矩阵为（2.31）0T1(q1)表示关节1的关节变量取值为q1时，坐标系1到坐标系0的齐次矩阵。1T2(q2)表示关节2的关节变量取值为q2时，坐标系2到坐标系1的齐次矩阵。以此类推，n-1Tn(qn)表示关节n的关节变量取值为qn时，坐标系n到坐标系n-1的齐次矩阵。这样，通过复合齐次变换，就可以得到坐标系n到坐标系0（即固定参考坐标系）的齐次矩阵。每对相邻坐标系i和i-1的齐次矩阵，可用以下公式得到：（2.32）右边各个齐次矩阵的公式为：18（2.33）（2.34）（2.35）（2.36）注意：这种坐标变换，称为齐次坐标变换。必须使用齐次坐标。当得到齐次矩阵0Tn后，坐标系n的原点在坐标系0中的坐标，其X,Y,Z值对应为0Tn第4列的第1行到第3行的元素值。也能够得到坐标系n的x,y,z三个坐标轴，相对于坐标系0的三个坐标轴的方向余弦，从而确定其方向。坐标系n的x轴与坐标系0的x,y,z三个坐标轴的方向余弦，分别为：0Tn第1列的第1行到第3行的元素值。坐标系n的y轴与坐标系0的x,y,z三个坐标轴的方向余弦，分别为：0Tn第2列的第1行到第3行的元素值。坐标系n的z轴与坐标系0的x,y,z三个坐标轴的方向余弦，分别为：0Tn第3列的第1行到第3行的元素值。19设末端工具坐标系B与固定参考坐标系G的的齐次矩阵如下，可完成正运动学分析。第一列前三个数从上到下为B的X轴与C的X,Y,Z轴的方向余弦。第二列前三个数从上到下为B的Y轴与C的X,Y,Z轴的方向余弦。第三列前三个数从上到下为B的Z轴与C的X,Y,Z轴的方向余弦。第四列前三个数为B的原点在G中的坐标。2.4逆运动学分析已知末端工具在固定参考坐标系中的位置与姿态，计算各个关节变量的值。这种分析称为逆运动学分析。2.4.1奇异为姿如果对于机械臂某个特别的位姿，逆解不存在，称这个位姿为奇异位姿。机械臂的奇异性可能是由于机械臂中某些坐标轴的重合，或位置不能达到引起的。机械臂的奇异位姿分为两类：（1）边界奇异位姿当机械臂的关节全部展开或折起时，使得末端处于操作空间的边界或边界附近，雅克比矩阵奇异，机械臂的运动受到物理结构的约束，这时机械臂的奇异位姿称为边界奇异位姿15。（2）内部奇异位姿两个或两个以上的关节轴线重合时，机械臂各个关节的运动相互抵消，不产生操作运动，这时机械臂的奇异位姿称为内部奇异位姿。如果机械臂的关节变量的个数大于6，称为冗余机械臂，这时，对于机械臂末端的一个位置与姿态，可能有无穷多个关节值与之对应。因此，对于冗余的机械臂的逆解可能有无穷多个，实验中采用的机械臂为七自由度。202.4.2数值迭代方法数值解是当机械臂的末端位置与姿态已知时，用迭代的方法得到机械臂的各个关节角度的值，一般是近似的，本实验采用数值迭代法。一般来说，机械臂的逆运动学解qk(k=1,2,n)可以通过求解以下非线性方程组得到。(2.37）10)()()(343212112100rrqTqTnn该方程是一个超越方程，可以使用数值方法得到方程的零点。然而，一般来要使用迭代算法16。最常用的是Newton-Raphson方法。为了用迭代算法从求解运动学方程T(q)=0中求出未知数q，首先要给出一个迭代的初始值（2.38）q1利用正运动学方程得到（2.39）（2.40）由泰勒级数展开得到（2.41）)()(211qTqTO略去高次项，得到一个线性方程组（2.42）J其中因此为雅克比矩阵。jiqTJ)(（2.43）TJq1写成迭代公式21（2.44）)()(1)()1(kkkkqTJq实现该公式的算法如下（1）设k=0,给出关节的初始值；)0(q（2）计算)()(kkJqT（3）计算；)(11kT（4）如果，计算结束，就是希望的迭代解；)(k1（5）取k=k+1,转（2）继续计算。例如，考虑两个自由度的平面机械臂（2.45）)sin(ico2121llyx（2.46）（2.47）它的雅克比矩阵是（2.48）)cos()cos(ininsi212121lllJ雅克比矩阵的逆矩阵（2.49）)cos(sin)cos(sin2121212121lllllJ所以，迭代公式2.44表示为（2.50）)(1)(21)(21kkkyxJ取22，1,121yxlT3/)0(21)0(q2311)3/sin(3/ico1雅克比矩阵和它的逆矩阵分别是13021230JJ所以792.16452313021)0(21)(21TJ迭代4次，得到5708.1)4(21)4(q2.4.3机械臂末端的奇异位姿考虑图2.11中的两个自由度的平面机械臂的逆运动学问题图2.11两个自由度的平面机械臂23根据机械臂的正向运动学，这个机械臂的传递矩阵是100)()()(22212111210slcscscT（2.51）由（式2.50）中第1,2行，第4列可知，机械臂末端的位置是（2.52）（2.53）由21212cosllyx（2.54）得到212arcoslyx（2.55）应用半角公式（2.56）cos12tan得到（2.57）21212)()(tanlyxl24（2.58）211cosinta2tnlxy注意由2求1时正负号的顺序。对（2.51）两边微分后写成矩阵形式得到（2.59）2112121)cos()cos(ininsidllldyx记21212121)cos()cos(ininsidlllJdyxp（2.58）简写成，式中J就称为机械臂的雅可比（Jacobian）矩阵，它由函数x，y的偏微分组成，反映了关节微小位移d与机械臂末端微小运动dp之间的关系。将两边同除以dt得到：dp/dt=Jd/dt,因此机械臂的雅可比矩阵也可以看做是操作空间中的速度与关节空间中速度的线性变换。dp/dt称为机械臂末端在操作空间中的广义速度，简称操作速度18，d/dt为关节速度。可以看出，雅可比矩阵的每一列表示其它关节不动而某一关节以单位速度运动产生的末端速度。从)cos()cos(ininsi212121lllJ可以看出，J的值随末端位置的不同而不同，1和2的改变会导致J的变化。对于关节空间的某些位姿，机械臂的雅可比矩阵的秩减少，这些位姿称为机械臂的奇异位姿。不难看出，机械臂雅可比矩阵的行列式为，当2=0或21sin)det(lJ2=180时，机械臂的雅可比行列式为0，矩阵的秩为1，这时机械臂处于奇异位姿。机械臂在操作空间的自由度将减少。如果机械臂的雅可比矩阵J是满秩的方阵，相应的关节速度即可求出，即，机械臂的逆雅可比矩阵是xJ1)cos(sin)cos(sin1212121212lllllJ25（2.60）显然，当2趋于0（或180）时，机械臂接近奇异位姿，相应的关节速度将趋于无穷大。263.机器人路径规划3.1机械臂的变量空间机械臂的三个变量空间：关节空间，操作空间和驱动空间3.1.1、关节空间n个自由度的机械臂的末端位姿由n个关节变量所决定，这n个关节变量统称为n维关节矢量，记为q。所有的关节矢量构成的空间称为关节空间。由于机械臂各个关节的旋转受到物理条件的约束，每个关节的转动被限制在一定的范围内。本实验所用的机械臂有7个关节，所以有7个自由度，受到物理结构的限制，它们的转动范围分别是（单位是度）最小值300690300690690740300最大值32902910329029102910285032903.1.2操作空间机械臂末端的位姿用6个变量描述，3个平移(x,y,z)和3个旋转(x,y,z)，记d=(x,y,z,x,y,z)d是机械臂末端在基坐标空间中的坐标，所有的矢量d构成的空间称为操作空间或作业定向空间。操作空间是操作臂的末端能够到达的空间范围，即末端能够到达的目标点集合。值得指出的是，工作空间应该严格地说，可以分为两类：(1)灵活（操作）空间指机械臂末端能够以任意方位到达的目标点集合。因此，在灵活空间的每个点上，手爪的指向可任意规定。(2)可达（操作）空间指机械臂末端至少在一个方位上能够到达的目标点集合。3.1.3驱动空间机械臂各关节驱动器的位置组成的矢量称为驱动矢量s，由这些矢量构成的空间称为驱动空间。本实验所用的机械臂有7个旋转关节，每个关节分别由一个舵机驱动，根据舵机的特性，换算关系1=0.088度，所以最小值348788348788788848348最大值374833083748330833083248374827舵机的速度可以设置，设置的范围是(1-1023),换算关系1=0.114转/分钟。3.2关节空间路径规划的基本方法关节空间中的路径规划是指，在关节空间中，给定关节的起点和终点，确定一条连接这两点的曲线。这条曲线称为路径函数。路径规划又可以分为点到点（Point-to-PointMotion）的路径规划问题和连续路径(Continuous-PathMotion)路径规划问题19。点到点路径规划问题是指在给定关节的起点和终点，求出一个路径函数，在指定的时间，使关节从起点移动到终点。连续路径路径规划问题，是指在起点与终点之间，还有一些中间点，求出一个路径函数，使得关节在指定的时间段内从起点移动到终点的同时，还要求关节在指定的时刻，通过这些中间点。3.2.1三次多项式函数三次多项式函数是一种最简单点到点（Point-to-PointMotion）的路径函数。在关节空间中，三次多项式函数描述了机械臂的关节在两个点q(t0)和q(tf)之间随时间变化的过程，用q(t)表示（3.1）（3.2）满足约束条件ffffqttqtqt)()()()(00利用以上方程和约束条件得到求解q(t)各个系数的方程组ffffqatttt032120301（3.3）解这个方程组可以得到（3.4）20030020)()()(tqttqtqaffffff3002