第三章机器人运动学

第三章机器人运动学机器人的工作是由控制器指挥的，对应驱动末端位姿运动的各关节参数是需要实时计算的。当机器人执行工作任务时，其控制器根据加工轨迹指令规划好位姿序列数据，实时运用逆向运动学算法计算出关节参数序列，并依此驱动机器人关节，使末端按照预定的位姿序列运动。机器人运动学或机构学从几何或机构的角度描述和研究机器人的运动特性，而不考虑引起这些运动的力或力矩的作用。机器人运动学中有两类基本问题：(1)机器人运动方程的表示问题，即正向运动学：对一给定的机器人，已知连杆几何参数和关节变量，欲求机器人末端执行器相对于参考坐标系的位置和姿态。这就需要建立机器人运动方程。运动方程的表示问题，正向运动学，属于问题分析。因此，也可以把机器人运动方程的表示问题称为机器人运动的分析。(2)机器人运动方程的求解问题，即逆向运动学：已知机器人连杆的几何参数，给定机器人末端执行器相对于参考坐标系的期望位置和姿态(位姿)，求机器人能够达到预期位姿的关节变量。这就需要对运动方程求解。机器人运动方程的求解问题，逆向运动学，属于问题综合。因此，也可以把机器人运动方程的求解问题称为机器人运动的综合。要知道工作物体和工具的位置，就要指定手臂逐点运动的速度。雅可比矩阵是由某个笛卡儿坐标系规定的各单个关节速度对最后一个连杆速度的线性变换。大多数工业机器人具有六个关节，这意味着雅可比矩阵是个6阶方阵。本章第一节和第二节将分别讨论机器人运动方程的表示与求解方法，第三节举例介绍PUMA560机器人的运动方程的分析与综合。机器人运动方程的表示机械手是一系列由关节连接起来的连杆构成的一个运动链。将关节链上的一系列刚体称为连杆，通过转动关节或平动关节将相邻的两个连杆连接起来。六连杆机械手可具有六个自由度，每个连杆含有一个自由度，并能在其运动范围内任意定位与定向。按机器人的惯常设计，三个自由度用于规定位置，而另外三个自由度用来规定姿态。T6表示机械手的位置和姿态。运动姿态和方向角机械手的运动方向图3.1表示机器人的一个夹手。把所描述的坐标系的原点置于夹手指尖的中心，此原点由矢量p表示。描述夹手方向的三个单位矢量的指向如下：z向矢量处于夹手进入物体的方向上，并称之为接近矢量a；y向矢量的方向从一个指尖指向另一个指尖，处于规定夹手方向上，称为方向矢量o;最后一个矢量叫做法线矢量n,它与矢量o和a一起构成一个右手矢量集合，并由矢量的交乘所规定：n=oa。因此，变换T6具有下列元素。nxOxaxPx(3.1)nyoyaypynzOzazPz0001图3.1矢量n、o、a和p（原图3.1）六连杆机械手的T矩阵（T6）可由指定其16个元素的数值来决定。在这16个元素中,只有12个元素具有实际含义。底行由三个零和一个1组成。左列矢量n是第二列矢量o和第三列矢量a的交乘。当对p值不存在任何约束时，只要机械手能够到达期望位置，那么矢量o和a两者都是正交单位矢量，并且互相垂直，即有：o?o=1，a?a=1，o?a=0。这些对矢量o和a的约束，使得对其分量的指定成为困难，除非是末端执行装置与坐标系处于平行这种简单情况。也可以应用第二章讨论过的通用旋转矩阵，把机械手端部的方向规定为绕某轴f旋转角，即Rot（f,）。遗憾的是，要达到某些期望方向，这一转轴没有明显的直观感觉。用旋转序列表示运动姿态机械手的运动姿态往往由一个绕轴x,y和z的旋转序列来规定。这种转角的序列，称为的z轴（z）旋转角来描述任何可能的姿态，见图欧拉（Euler）角。欧拉角用一个绕z轴旋转角，再绕新的y轴（y）旋转角，最后围绕新墨霑!,11£wy八rUrv3.2。图3.2欧拉角的定义（原图3.2）来解释：先绕z轴旋转角，再绕y轴旋转角，最后围绕z轴旋转角。欧拉变换Euler()可由连乘三个旋转矩阵来求得，即Euler(,,)Rot(z,)Rot(y,)Rot(z,)Euler(,,)c0s0cs00cs00sc000100sc000010s0c00010000100010001cccssccssccs0scccsscsccss0scssc00001(3.2)用横滚、俯仰和偏转角表示运动姿态另一种常用的旋转集合是横滚(roll)、俯仰(pitch)和偏转(yaw)。如果想象有只船沿着z轴方向航行，见图3.3(a)，那么这时，横流对应于围绕z轴旋转角，俯仰对应于围绕y轴旋转角，而偏转则对应于围绕x轴旋转角。适用于机械手端部执行装置的这些旋转，示于图3.3(b)。对于旋转次序，我们作如下规定：RPY(,,)Rot(z,)Rot(y,)Rot(x,)(3.3)式中，RPY表示横滚、俯仰和偏转三旋转的组合变换。也就是说，先绕x轴旋转角，再绕y轴旋转角，最后围绕z轴旋转角。(a)运动示意图(b)旋转运动坐标系图3.3用横滚、俯仰和偏转表示机械手运动姿态(原图3.3)此旋转变换计算如下:cs00c0s01000sc0001000cs0RPY(,,)°0010s0c00sc0000100010001cccsssccscss0scsssccssccs0scscc000013.1.2运动位置和坐标一旦机械手的运动姿态由某个姿态变换规定之后,(3.4)它在基系中的位置就能够由左乘一个对应于矢量p的平移变换来确定:100Px0T610Py［某姿态变换］001Pz0001(3.5)这一平移变换可用不同的坐标来表示。除了已经讨论过的笛卡儿坐标外，还可以用柱面坐标和球面坐标来表示这一平移。1.用柱面坐标表示运动位置首先用柱面坐标来表示机械手手臂的位置,即表示其平移变换。这对应于沿x轴平移r，再绕z轴旋转，最后沿z轴平移z，如图3.4(a)所示,(a)柱面坐标表示(b)球面坐标表示图3.4用柱面坐标和球面坐标表示位置(原图3.4)即有Cyl(z,,r)Trans(0,0,z)Rot(z,)Trans(r,O,O)式中，Cyl表示柱面坐标组合变换。计算上式并化简得:1000cs00100rCyl(z,,r)0100sc000100001z00100010000100010001cs0rcsc0rs(3.6)001z0001如果，用某个如式(3.5)所示的姿态变换右乘上述变换式，那么，手臂将相对于基系绕z轴旋转角。要是需要变换后机器人末端相对基系的姿态不变，那么就应对式(3.6)绕z轴旋转一个角，实际可理解为：圆柱坐标机器人末端坐标系是这样从基坐标系变换来的：开始末端系与基系重合，然后末端系相对基系的X、z和z各轴，依次平移r、旋转a与平移z,最后再绕变换后的末端坐标系z轴自转即有：cs0rcc()s()00sc0rss()c()00Cyl(z,,r)001z001000010001100rc010rs(3.7)001z0001这就是用以解释柱面坐标Cyl(z,,r)的形式。2.用球面坐标表示运动位置现在讨论用球面坐标表示手臂运动位置矢量的方法。这个方法对应于沿z轴平移r,再绕y轴旋转角,最后绕z轴旋转角,如图3.4(b)所示，即为：Sph(,,r)Rot(z,)Rot(y,)Trans(0,0,r)(3.8)式中,Sph表示球面坐标组合变换。对上式进行计算结果如下：cs00c0s1000sc000100100Sph(,,r)0010s0c001r00010000001ccscsrcssccssrss(3.9)s0crc0001如果希望变换后机器人末端坐标系相对基系的姿态不变，那么就必须用Rot(y,)和Rot(z,)右乘式(3.9)，实际可理解为：球坐标机器人末端坐标系是这样从基坐标系变换来的：开始末端系与基系重合，然后末端系相对基系的z、y和z各轴，依次平移r、旋转B与平移a,最后再绕变换后的末端坐标系的y轴和z轴自转和即Sph(,,r)Rot(z,)Rot(y,)Trans(O,O,r)Rot(y,)Rot(z,)100rcs010rss001rc(3.10)00这就是我们用于解释球面坐标的形式。013.1.3连杆变换矩阵及其乘积将为机器人的每一连杆建立一个坐标系，并用齐次变换来描述这些坐标系间的相对位置和姿态。可以通过递归的方式获得末端执行器相对于基坐标系的齐次变换矩阵，即求得了机器人的运动方程。广义连杆相邻坐标系间及其相应连杆可以用齐次变换矩阵来表示。要求出操作手所需要的变换矩阵，每个连杆都要用广义连杆来描述。在求得相应的广义变换矩阵之后，可对其加以修正，以适合每个具体的连杆。从机器人的固定基座开始为连杆进行编号，一般称固定基座为连杆0。第一个可动连杆为连杆1，以此类推，机器人最末端的连杆为连杆n。为了使末端执行器能够在三维空间中达到任意的位置和姿态，机器人至少需要6个关节(对应6个自由度，3个位置和3个方位)。机器人机械手是由一系列连接在一起的连杆(杆件)构成的。可以将连杆各种机械结构抽象成两个几何要素及其参数，即公共法线距离ai和垂直于ai所在平面内两轴的夹角j另外相邻杆件之间的连接关系也被抽象成两个量，即两连杆的相对位置di和两连杆法线的夹角i，如图3.5所示。图3.5连杆四参数及坐标系建立示意图（新图）[资料来源：CraigJJ2004]Craig参考坐标系建立约定如图3.5所示,其特点是每一杆件的坐标系z轴和原点固连在该杆件的前一个轴线上。除第一个和最后一个连杆外，每个连杆两端的轴线各有一条法线，分别为前、后相邻连杆的公共法线。这两法线间的距离即为di。我们称ai为连杆长度，i为连杆扭角，di为两连杆距离，i为两连杆夹角。机器人机械手连杆连接关节的类型有两种—转动关节和棱柱联轴节。对于转动关节，i为关节变量。连杆i的坐标系原点位于轴i-1和i的公共法线与关节i轴线的交点上。如果两相邻连杆的轴线相交于一点，那么原点就在这一交点上。如果两轴线互相平行，那么就选择原点使对下一连杆（其坐标原点已确定）的距离di,为零。连杆i的z轴与关节i1的轴线在一直线上，而X轴则在轴i和i1的公共法线上，其方向从i指向i1，见图3.5。当两关节轴线相交时，X轴的方向与两矢量的交积zi1乙平行或反向平行，X轴的方向总是沿着公共法线从转轴n指向i1。当两轴Xi1和Xi平行且冋向时，第i个转动关节的i为零。在建立机器人杆件坐标系时，首先在每一杆件i的首关节轴i上，建立坐标轴Zi，zi正向在2个方向中选一个方向即可，但所有z轴应尽量一致。ai、i、i和di四个参数，除了ai0外，其他三个值皆有正负，因为i、i分别是围绕xi、zi轴旋转定义的，它们的正负就根据判定旋转矢量方向的右手法则来确定。di为沿zi轴，由xi-1垂足到Xi垂足的距离，距离移动时与Zi正向一致时符号取为正。图3.6连杆两端相邻坐标系变换示意图（新图）[资料来源：CraigJJ2004]广义变换矩阵在对全部连杆规定坐标系之后，就能够按照下列顺序由两个旋转和两个平移来建立相邻两连杆坐标系i1与i之间的相对关系，见图3.6。绕x!轴旋转ii角，使Zi!转到Zr,同Zi方向一致，使坐标系｛i-1｝过渡到｛R｝。坐标系｛R｝沿xi1或xR轴平移一距离ai1，把坐标系移到i轴上，使坐标系｛R｝过渡到｛Q｝。坐标系｛Q｝绕zQ或Zi轴转动i角，使｛Q｝过渡到｛P｝。坐标系｛P｝再沿Zi轴平移一距离di，使｛P｝过渡到和i杆的坐标系｛i｝重合。这种关系可由表示连杆i对连杆i1相对位置的四个齐次变换来描述。根据坐标系变换的链式法则，坐标系｛i-1｝到坐标系｛i｝的变换矩阵可以写成i1iTiR1TQRTQPTPiT(3.11)式(3.11)中的每一个变换都是仅有一个连杆参数的基础变换(旋转或平移变换)，根据各中间坐标系的设置，式(3.11)可以写成i1iTRotx,i1Transai1,0,0RotZ,iTrans0,0,di(3.12)由4矩阵连乘可以计算出式(3.12)，即i1t的变换通式为cisi0ai1i1sici1cici1si1disi1i1iTii1(3.13)sisi1cisi1ci1dici10001机械手端部对基座的关系06T为：06T=01T12T23T34T45T56T如果机器人6个关节中的变量分别是：1、2、d3、4、5、6则末端相对基座的齐次矩阵也应该是包含这6个变量的4X4矩阵，即：6T(1,2,d3,4,5,6)1T(1)12T(2)23T(d3)43T(4)45T(5)56T(6)(3.14)上式就是机器人正向运动学的表达式，即：已知机器人各关节值，计算出末端相对于基座的位姿。若机器人基座相对工件参照系有一个固定变换为Z,机器人工具末端相对手腕端部坐标系｛6｝也有一个固定变换为E，则机器人工具末端相对工件参照系的变换X为：XZ06TE3.2机械手运动方程的求解在上一节中讨论了机器人的正向运动学，本节将研究难度更大的逆向运动学问题，即机器人运动方程的求解问题：已知工具坐标系相对于工作台坐标系的期望位置和姿态，求机器人能够达到预期位姿的关节变量。大多数机器人程序设计语言，是用某个笛卡儿坐标系来指定机械手末端位置的。这一指定可用于求解机械手最后一个连杆的姿态T6。不过，在机械手能够被驱动至这个姿态之前，必须知道与这个位置有关的所有关节的位置。欧拉变换解1.基本隐式方程的解首先令Euler(,,)T(3.15)式中，Euler(,,)Rot(z,)Rot(y,)Rot(z,)(3.3)已知任一变换T，要求得,和。也就是说，如果已知T矩阵各元的数值，那么其所对应的,和值是什么？由式(3.3)和(3.15)，我们有下式nxoxaxpxcccssccssccs0nyoyaypysccssscsccss0(3.16)nzozazpzscssc000010001令矩阵方程两边各对应元素一一相等，可得16个方程式，其中有12个为隐式方程。我们将从这些隐式方程求得所需解答。在式(3.16)中，只有9个隐式方程，因为其平移坐标也是明显解。这些隐式方程如下：nxcccss(3.17)nyscccs(3.18)nzsc(3.19)oxccssc(3.20)oyscscc(3.21)ozss(3.22)axcs(3.23)ayss(3.24)azc(3.25)2.用双变量反正切函数确定角度可以试探地对,和进行如下求解。据式(3.25)得：1c1(az)(3.26)据式(3.23)和(3.26)有：1c1(ax/s)(3.27)又据式(3.19)和(3.26)有：c1(nz/s)(3.28)但是，这些解答是无用的，因为：当由余弦函数求角度时，不仅此角度的符号是不确定的，而且所求角度的准确程度又与该角度本身有关，即cos()cos()以及dcos()/d|0,1800。在求解和时，见式(3.17)和(3.18)，我们再次用到反余弦函数，而且除式的分母为sin。这样，当sin接近于0时，总会产生不准确。当0或180时，式(3.27)和(3.28)没有定义。因此，在求解时，总是采用双变量反正切函数atan2来确定角度。atan2提供二个自变量，即纵坐标y和横坐标x，见图3.7。当，由atan2反求角度时，同时检查y和x的符号来确定其所在象限。这一函数也能检验什么时候x或y为0，并反求出正确的角度。atan2的精确程度对其整个定义域都是一样的。图3.7反正切函数atan2(原图3.8)用显式方程求各角度要求得方程式的解，采用另一种通常能够导致显式解答的方法。用未知逆变换依次左乘已知方程，对于欧拉变换有：1Rot(z,)1TRot(y,)Rot(z,)(3.29)11Rot(y,)1Rot(z,)1TRot(z,)(3.30)式(3.29)的左式为已知变换T和的函数，而右式各元素或者为0,或者为常数。令方程fii(n)fii(o)fii(a)fii(P)fi2(n)fi2（O）fi2(a)fi2(P)fi3(n)fi3(0)fi3(a)fi3(P)0001cs0nxOxaxPxcccss0sc0nyOyayPysc0000iOzazPzscssc000000010001式的两边对应元素相等，对于式（3.29）即有:在计算此方程左式之前，我们用下列形式来表示乘积:(3.31)其中，忖cxsy,f12sxcy,fi3z，而X,y和z为fn,fi2和fi3的各相应分量,例如:fi2(a)saxcayfii(p)cpxspy检查上式右式可见，于是，我们可把式（3.3i）重写为:fii(n)fii(o)fii(a)fii(P)cccss0fi2(n)fi2（0）fi2(a)fi2(P)sc00c(3.32)fi3(n)fi3（0）fi3(a)fi3(P)scssc000010001Px，Py和Pz均为0。这是我们所期望的，因为欧拉变换不产生任何平移。此外，位于第二行第三列的元素也为0。所以可得fi2（a）0，即saxcay0（3.33）上式两边分别加上sax,再除以cax可得：saytancax这样，即可以从反正切函数atan2得到atan2（ay,ax）（3.34）对式（3.33）两边分别加上cay,然后除以cax，可得：tan—丄cax这时可得式（3.33）的另一个解为:atan2(ay,ax)(3.35)式(3.35)与式(3.34)两解相差180除非出现ay和ax同时为0的情况，我们总能得到式(3.33)的两个相差180。的解。当ay和ax均为0时，角度没有定义。这种情况是在机械手臂垂直向上或向下，且和两角又对应于同一旋转时出现的，参阅图3.4(b)。这种情况称为退化(degeneracy)。这时，我们任取0。求得值之后，式(3.32)左式的所有元素也就随之确定。令左式元素与右边对应元素相等，可得：sfn(a),cfi3(a),或scaxsay,caz。于是有atan2(caxsay,az)(3.36)当正弦和余弦都确定时，角度总是唯一确定的，而且不会出现前述角度那种退化问题。最后求解角度。由式(3.32)有：sf12(n),c切(0),或ssnxcny,csOxcOy从而得到atan2(snxcny,sOxcOy)(3.37)概括地说，如果已知一个表示任意旋转的齐次变换，那么就能够确定其等价欧拉角atan2(ay,ax),180atan2(caxsay,az)(3.38)atan2(snxcny,sOxcOy)滚、仰、偏变换解在分析欧拉变换时，已经知道，只有用显式方程才能求得确定的解答。所以在这里直接从显式方程来求解用滚动、俯仰和偏转表示的变换方程。式(3.5)和(3.6)给出了这些运动方程式。从式(3.4)得：1ROt(z,)1TROt(y,)ROt(x,)f11(n)f11(O)f11(a)f11(p)csssc0f12(n)f12(O)f12(a)f12(p)0cs0(3.39)f13(n)f13(O)f13(a)f13(p)scscc000010001式中，f11,f12和f13的定义同前。令f12(n)与式(3.47)右式的对应元素相等，可得:snxcny0从而得：(3.40)atan2(ny,nx)180(3.41)又令式(3.39)中左右式中的(3，1)及(1，1)元素分别相等，有：snz,ccnxsny，于是得：atan2(nz,cnxsny)(3.42)最后令第(2，3)和(2，2)对应元素分别相等，有ssaxcay，csoxcoy，据此可得：atan2(saxcay,soxcay)(3.43)综上分析可得RPY变换各角如下：atan2(ny,nx)180(344)atan2(nz,cnxsny)atan2(saxcay,soxcoy)3.2.3.球面变换解也可以把上述求解技术用于球面坐标表示的运动方程，这些方程如式(3.9)和(3.12)所示。由式(3.9)可得：)1TRot(y,)Trans(0,0,r)(3.45)Rot(z,cs00nxoxaxpxc0srssc00nyoyaypy01000010nzozazpzs0crc000100010001f11(n)f11(o)f11(a)f11(p)c0srsf12(n)f12(o)f12(a)f12(p)0100f13(n)f13(o)f13(a)f13(p)s0crc00010001令上式两边的右列相等，即有：cpxspyrsspxcpy0pzrc1atan2(py,px)(3.46)180(3.47)以及cpxspyrs,pzrc。当r0时atan2(cpxspy,pz)(3.48)要求得z，必须用Rot(y,)1左乘式(3.45)的两边，由此可得：spxcpy，11Rot(y,)1Rot(z,)1TTrans(0,0,r)计算上式(请读者自己推算)后，让其右列相等：c(cpxspy)spz00spxcpys(cpxspy)cpzr11从而可得：rs(cpxspy)cpz(3.49)综上讨论可得球面变换的解为：atan2(py,px),180atan2(cpxspy,pz)(3.50)rs(cpxspy)cpz3.3PUMA560机器人运动方程能够由式(3.13)等来求解用笛卡儿坐标表示的运动方程。这些矩阵右式的元素，或者为零，或者为常数，或者为第n至第6个关节变量的函数。矩阵相等表明其对应元素分别相等，并可从每一矩阵方程得到12个方程式，每个方程式对应于四个矢量n,o,a和p的每一分量。下面将以PUMA560机器人为例来求解以关节角度为变量的运动学方程。PUMA560运动分析PUMA560是属于关节式机器人，6个关节都是转动关节。前3个关节确定手腕参考点的位置，后3个关节确定手腕的方位。和大多数工业机器人一样，后3个关节轴线交于一点。该点选作为手腕的参考点，也选作为连杆坐标系{4}，{5}和{6}的原点。关节1的轴线为铅直方向，关节2和3的轴线水平，且平行，距离为a2。关节1和2的轴线垂直相交，关节3和4的轴线垂直交错，距离为a3。各连杆坐标系如图3.8所示，相应的连杆参数列于表3.1。其中，a2=431.8mm,a3=20.32mm,d2=149.09mm,d4=433.07mm。表3.1PUMA560机器人的连杆参数连杆i变量ii1ai1di变量范围11(90)0°00-160°~160°22(0)-90°0d2-225°~45°33(90)0°a20-45°~225°44(0)-90°a3d4-110°~170°55(0)90°00-100°~100°66(0)-90°00-266°~266°(a)(b)图3.8PUMA560机器人的连杆坐标系(原图3.9)根据式(3.13)和表3.1所示的连杆参数，可求得各连杆变换矩阵如下:c1s100c2s20001Ts1c10012T001d20010s2c20000010001c3s30a2c4s40a32Ts3c3003T001d43T00104Ts4c40000010001c5s500c6s6004T00105T00105Ts5c5006Ts6c60000010001将各连杆变换矩阵相乘，即可得到PUMA560的运动学方程06T01T(1)12T(2)23T(3)34T(4)45T(5)56T(6)(3.51)该方程为关节变量126的函数。要求解此运动方程，需先计算某些中间结果(这些中间结果有助于求解下一节中的逆运动学问题)c5c6c5s6s5064T45T65Ts6c600(3.52)s5c6s5s6c500001c4c5c6s4s6c4c5s6s4c6c4s5a334T46Ts5c6s5s6c5d4(3.53)s4c5c6c4s6s4c5s6c4c6s4s500001其中S4是sin4的缩写，c4是cos4等等(脚注：这取决于表达式占用空间的大小，下列三由于PUMA560的关节2和3相互平行，把种表示：sin4、s4、S4都是可以的)c23s230a2c231T12T23T001d2(3.54)s23c230a2s200012T(2)和3T(3)相乘，可得其中，c23cos(23)c2c3s2s3;s23sin(23)c2s3s2c3。可见，当两旋转关节平行时，利用角度之和的公式，可以得到比较简单的表达式。再将式(3.18)与式(3.17)相乘，可得：61T13T63T1nx1ox1ax11px1ny1oy1ay1py1nz1oz1az1pz00011nx1ny11nz1ox11oy1oz1ax1ay1az1px1py1pzc23(c4c5c6s4s6)s23s5c6s4c5c6c4s6s23(c4c5c6s4s6)c23s5c6c23(c4c5s6s4c6)s4c5s6c4c6s23(c4c5s6s4c6)c23c4s5s23c5s23s5s6c23s5s6s4s5s23c4s5c23c5a2c2a3c23d4s23d2a3s23a2s2d4c23(3.55)其中PUMA560型机器人的正向运动学方最后，可求得六个连杆坐标变换矩阵的乘积，即程为：nxoxaxpx06T01T16Tnyoyaypynzozazpz0001其中nxc1c23(c4c5c6s4s6)s23s5c6s1(s4c5c6c4s6)nys1c23(c4c5c6s4s6)s23s5c6c1(s4c5c6c4s6)nzs23(c4c5c6s4s6)c23s5c6oxc1[c23(c4c5s6s4c6)s23s5s6]s1(c4c6s4c5s6)oys1[c23(c4c5s6s4c6)s23s5s6]c1(c4c6s4c5c6)ozs23(c4c5s6s4c6)c23s5s6axc1(c23c4s5s23c5)s1s4s5ays1(c23c4s5s23c5)c1s4s5azs23c4s5c23c5pxc1[a2c2a3c23d4s23]d2s1pys1[a2c2a3c23d4s23]d2c1pza3s23a2s2d4c23(3.56)式(3.20)表示的PUMA560手臂变换矩阵06T，描述了末端连杆坐标系{6}相对于基坐标系{0}的位姿，式(3.20)是PUMA560全部运动学分析的基本方程。为校核所得60T的正确性，计算变换矩阵06T的值。计算结果为：90,20,390,4560时手臂a2d2d4a31与图3.8所示的情况完全一致。PUMA560运动综合节已介绍了几种。矩阵相等表明其对应元机器人运动方程的求解或综合方法很多，素分别相等，并可从每一矩阵方程得到12个方程式，每个方程式对应于四个矢量n,o,a和p的每一分量。现以PUMA560机器人为例来阐述这些方程的求解。将PUMA560的运动方程(3.56)写为：nxoxaxpx06Tnyoyaypy01T(1)21T(2)23T(3)43T(4)45T(5)56T(6)(3.57)nzozazpz0001若末端连杆的位姿已经给定，即n,o,a和p为已知，则求关节变量「2,,6的值称为运动反解。用未知的连杆逆变换同时左乘方程(3.53)的两边，可以把关节变量分离出来，从而进行求解。具体步骤如下：1.求1用逆变换01T1(1)左乘方程(3.53)两边，(3.58)TOC\o"1-5"\h\zHYPERLINK\l"bookmark84"\o"CurrentDocument"0T1(i)°T2t(2)；T(3)：T(4)；T(5)66t(6)即有C1q00nxOxaxPxqG00n：OyayPy1T(3.59)0010nzOzazPz00010001令矩阵方程(3.59)两端的兀素(2,4)对应相等，可得S1PxC1pyd2(3.60)利用三角代换：Pxcos;Pysin(3.61)TOC\o"1-5"\h\z式中，•，p2p：;atan2(Py,Px)。把代换式(3.61)代入式(3.60)，得到i的解：sin(i)d2/；cos(i),1(d?/)2HYPERLINK\l"bookmark86"\o"CurrentDocument"d2d221atan2—,Ji—(3.62)HYPERLINK\l"bookmark90"\o"CurrentDocument"f222HYPERLINK\l"bookmark92"\o"CurrentDocument"1atan2(py,Px)atan2(d2,、PxPyd?)式中，正、负号对应于1的两个可能解。2.求3在选定1的一个解之后，再令矩阵方程(3.59)两端的元素(1,4)和(3,4)分别对应相等，即得两方程:PzSPya3s23a3C23d4s23a2C2d4C23a2s2(3.63)式(3.60)与(3.63)的平方和为：a3C3d4s3k(3.64)22222.2.2PxPyPza2a3d?d4式中k-y--2a2方程(3.64)中已经消去2，且方程(3.64)与(3.60)具有相同形式，因而可由三角代换求解(3.65)atan2@3,d4)atan2(k,.a；d4k2)式中，正、负号对应3的两种可能解。3.求2为求解2，在矩阵方程(3.53)两边左乘逆变换(3t1,°T1(1,2,3)°T汀(4)X(5)6T(6)(3.66)即有C1C23S1C23S23a2C3nxOxaxPxC1S23S1S23C23a2S3nyOyayPy3丁6t(3.67)SiC|0d2压OzazPz00010001式中，变换；T由公式(3.53)给出。令矩阵方程(3.67)两边的元素(1,4)和(2,4)分别对应相等,可得：TOC\o"1-5"\h\zC1C23pxSIC23pyS23pza2C3a3C|S23pxS1S23pyC23pza2S3d4联立求解得s23和c23:s(a3a2Q)Pz(ClPxSPy)(a2S3d4)s2322Pz(GPxS1Py)(d4a2S3)Pz(GPxSPy)(a2C3a3)C2322Pz(C1PxSPy)S>3和C23表达式的分母相等，且为正。于是233atan2@3a2G)Pz(GPxSPy)(a2S3d4),(3.70)根据1和3解的四种可能组合,由式(3.70)可以得到相应的四种可能值23，于是可得到2(d4a2S3)Pz(GPxSPy)(a2Oa3)的四种可能解:2233(3.71)4.求4因为式(3.67)的左边均为已知，令两边元素(1,3)和(3,3)分别对应相等，则可得:axs1ayc1s4s5(3.72)只要s50，便可求出44atan2(axs1ayc1,axc1c23ays1c23azs23)(3.73)4与6的和当S50时，机器人处于奇异位形。此时，关节轴4和6重合，只能解出或差。奇异位形可以由式(3.73)中atan2的两个变量是否都接近零来判别。若都接近零，则为奇异位形。在奇异位形时，可任意选取4的值，再计算相应的6值。5.求51,2,3,4)，根据求出的4，可进一步解出5，将式(3.57)两端同时左乘逆变换04T1(因式(3.04T(1,2,3,4)06T45T(5)56T(6)1,2,3和4均已解出，逆变换04T1(1,2,3,4)为：c1c23c4s1s4s1c23c4c1s4s23c4a2c3c4d2s4c1c23s4s1c4s1c23s4c1c4s23s4a2c3s4d2c4c1s23s1s23c23a2s3d40001a3s4a3c4(3.74)方程式(3.74)的右边46T(5,6)45T(5)56T(6)，由式(3.52)给出。据矩阵两边元素(1,3)和(3,3)分别对应相等，可得ax(c1c23c4ax(c1s23)s1s4)ay(s1c23c4c1s4)az(s23c4)s5ay(s1s23)az(c23)c5(3.75)由此得到5的封闭解：5atan2(s5,c5)(3.76)6.求6将式(3.57)两端同时左乘逆变换5T(1,2,3,4,5)，可得05T1(1,2,L,5)06T65T(6)12(3.77)令矩阵方程(3.77)两边元素(1,1)和(3,1)分别对应相等可得：nx（c1c23s4nx[（c1c23c4从而可求出6的封闭解：s1c4)ny(s1c23s4c1c4)nz(s23s4)s6s1s4)c5c1s23s5]ny[(s1c23c4c1s4)c5s1s23s5]nz(s23c4c5c23s5)c6(3.78)6atan2(s6,c6)(3.79)PUMA560的运动逆解可能存在8种解。但是，由于结构的限制，例如各个关节变量不能在全部360°范围内运动，有些解不能实现。在机器人存在多种解的情况下，应选取其中最满意的一组解，以满足机器人的工作要求。3.4本章小结本章讨论机器人的运动学问题，包括机器人运动方程的表示、求解与实例。这些 内容 财务内部控制制度的内容财务内部控制制度的内容人员招聘与配置的内容项目成本控制的内容消防安全演练内容 是研究机器人动力学和控制的重要基础。对于机器人运动方程的表示，即正向运动学，首先用变换矩阵表示机械手的运动方向，用转角（即欧拉角）变换序列表示运动姿态，或用横滚．俯仰和偏转角表示运动姿态。一旦机械手的运动姿态由某个姿态变换矩阵确定之后，它在基系中的位置就能够由左乘一个对应于矢量p的平移变换来确定。这一平移变换，可由笛卡儿坐标、柱面坐标或球面坐标来表示。为了进一步讨论机器人运动方程，还给出并分析了广义连杆的变换矩阵，得到通用连杆变换矩阵。对于机器人运动方程的求解，即逆向运动学，分别讨论了欧拉变换解、滚-仰-偏变换解和球面变换解，得出各关节位置的求解公式。本章第三节举例介绍了PUMA560机器人运动方程的表示（分析）和求解（综合）。根据第一节和第二节得到的方程式，结合PUMA560机器人的实际连杆参数，可求得各连杆的变换矩阵和机械手的变换矩阵。然后，根据矢量n,o,a和p的位姿和机械手连杆参数，就可逐一求得关节变量1,2,,6，即求得PUMA560机器人运动方程的解。 习题 有理数乘除混合运算习题护理管理学习题以及答案高等数学极限习题过敏性休克习题与答案诫子书习题及答案 3.1图3.9和表3.2表示PUMA560的某些机构参数和指定坐标轴。今另有一台工业机器人，除关节3为棱柱型关节外，其它关节情况同PUMA560。设关节3沿着x“的方向滑动，其位移为d3。可提出任何必要的附加假设。试求其运动方程式。表3.2PUMA560的连杆参数连杆ad10°0012-90°00230°a2d334-90°a3d44590°0056-90°0063.2图3.10给出一3自由度机械手的机构。轴1与轴2垂直。试求其运动方程式。图3.10三连杆非平面机械手（见原版图3.11）3.3图3.11所示三自由度机械手，其关节1与2相交，而关节2与关节3平行。图中所有关节均处于零位。各关节转角的正向均由箭头示出。指定本机械手各连杆的坐标系，然后求各变换矩阵0??1??2??图3.11三连杆机械手的两个视图（见原版图3.12）3.4图3.12和表3.4表示PUMA250机器人的几何结构和连杆参数。表3.4PUMA250连杆参数连杆.—J-P—rV-曰关节变量变化范围ad11315°9°0022320°0°8b1b233285°90°0044240°-90°0855535°-90°00图3.12PUMA250工业机器人结构(见原版图3.13)3.5图3.13示出机器人视觉系统的坐标配置。令｛Bi｝表示机器人机座坐标系，｛B2｝表示被移动箱子的坐标系，｛T｝表示工作台的坐标系,｛G｝表示夹手的坐标系，｛C｝表示摄66575°0°0bs象机的坐标系。图3.13机器人视觉系统坐标系配置(见原版图3.14)已知下列变换：010100101c100201003CtbCt00110001800010001100410020G01020100Gtb；t0017001000010001(1)画出有向变换图。(2)求变换矩阵B2T,并说明其各列的含义。求矩阵。求矩阵BTtO求矩阵Gto(6)如果让摄像机绕zc轴旋转90°,求TT,B2T和cT各矩阵。试求PUMA250各关节变量的解(1,2,,6。已知TTrans(5,10,0)Rot(x,90)Rot(z,90)Trans(0,1,0)Rot(r,0.1,rad)I式中r1/.3,1/,3,1/3,1用4X4矩阵表示T和据下式确定TAoT式中，T1可由式(2.27)和(3.28)计算。