第2章 工业机器人运动学

1、.第2章 工业机器人运动学章节题目：第2章 工业机器人运动学教学内容2-1 齐次坐标及对象物的描述2-2 齐次变换及运算2-3 工业机器人连杆参数及其齐次变换矩阵2-4 工业机器人运动学方程教学安排第2章安排6学时，其中介绍点的位置描述10分钟，齐次坐标10分钟，坐标轴方向的描述10分钟，动坐标系位姿的描述20分钟，目标物齐次矩阵表示10分钟，平移的齐次变换30分钟，旋转的齐次变换30分钟，平移加旋转的齐次变换15分钟，连杆参数及连杆坐标系的建立20分钟，连杆坐标系之间的变换矩阵15分钟，机器人运动学方程10分钟，正向运动学及实例45分钟，反向运动学及实例30分钟，x=x(q)形式运动学方程1

2、5分钟。通过多媒体课件结合板书的方式，采用课堂讲授和课堂讨论相结合的方法，介绍齐次坐标的概念及各种对象的齐次坐标方法，进而向学生讲述齐次变换及运算方法，通过上述内容的讲解，进一步让学生掌握连杆参数及其齐次变换矩阵，最终引出工业机器人运动学方程。知识点及其基本要求1、点的位置描述（掌握）2、齐次坐标（掌握）3、坐标轴方向的描述（掌握）4、动坐标系的描述（掌握）5、齐次变换（重点掌握）6、连杆参数及其齐次变换矩阵（掌握）7、运动学方程（掌握）重点和难点重点：1、对象的齐次坐标表示2、齐次变换3、机器人运动学方程难点：1、连杆参数2、机器人运动学方程教学法设计引入新课：机器人实际上可认为是由一系列关

3、节连接起来的连杆所组成。我们把坐标系固连在机器人的每一个连杆关节上，可以用齐次变换来描述这些坐标系之间的相对位置和方向。齐次变换具有较直观的几何意义，而且可描述各连杆之间的关系，所以常用于解决运动学问题。新课讲解：第一次课第2章 工业机器人运动学2-1 齐次坐标及对象物的描述一、点的位置描述在选定的直角坐标系a中，空间任一点p的位置可用31的位置矢量ap表示，其左上标代表选定的参考坐标系：精品.。二、齐次坐标如用四个数组成的（41）列阵表示三维空间直角坐标系a中点p，则列阵px py pz 1t称为三维空间点p的齐次坐标。必须注意，齐次坐标的表示不是唯一的，将其各元素同乘一非零因子w后，仍然代

4、表同一点p，即。三、坐标轴方向的描述i，j，k分别表示直角坐标系中x、y、z坐标轴的单位向量。若用齐次坐标来描述x、y、z轴的方向，则。从上可知，规定：（41）列阵a b c 0t中第四个元素为零，且a2+b2+c2=1，则表示某轴（某矢量）的方向；（41）列阵a b c wt中第四个元素不为零，则表示空间某点的位置。四、动坐标系位姿的描述动坐标系位姿的描述就是对动坐标系原点位置的描述以及对动坐标系各坐标轴方向的描述。1、刚体位置和姿态的描述机器人的一个连杆可以看做一个刚体。若给定了刚体上某一点的位置和该刚体在空间的姿态，则这个刚体在空间上是完全确定的。o为刚体上任一点，oxyz为与刚体固连的

5、一个坐标系，称为动坐标系。刚体q在固定坐标系oxyz中的位置可用齐次坐标形式的一个（41）列阵表示：。刚体的姿态可由动坐标系的坐标轴方向来表示。令n、o、a分别为x、y、z坐标轴的单位方向矢量，每个单位方向矢量在固定坐标系上的分量为动坐标系各坐标轴的方向余弦，用齐次坐标形式的（41）列阵分别表示为：n=nx ny nz 0t，o=ox oy oz 0t，a=ax ay az 0t。因此，刚体的位姿可用下面（44）矩阵来描述：精品.。2、手部位置和姿态的表示机器人手部的位置和姿态也可以用固连于手部的坐标系b的位姿来表示。坐标系b可以这样来确定：取手部的中心点ob；关节周为zb轴，zb轴的单位方向

6、矢量a称为接近矢量，指向朝外；二手指的连线为yb轴，yb轴的单位方向矢量o称为姿态矢量，指向可任意选定；xb轴与yb轴及zb轴垂直，xb轴的单位方向矢量n称为法向矢量，且n=oa，指向符合右手法则。手部的位置矢量为固定参考系原点指向手部坐标系b原点的矢量p，手部的方向矢量为n、o、a。于是手部的位姿可用（44）矩阵表示为：。五、目标物齐次矩阵表示2-2 齐次变换及运算刚体的运动是由转动和平移组成的。为了能用同一矩阵表示转动和平移，有必要引入（44）的齐次坐标变换矩阵。一、平移的齐次变换空间某一点a，坐标为（x，y，z），当它平移至a点后，坐标为（x，y，z），以及，或写成如下形式：，也可以简写

7、为a=trans(x，y，z)a，其中，trans(x，y，z)表示齐次坐标变换的平移算子。且，其中第四列元素分别表示沿坐标轴x，y，z的移动量。若算子左乘，表示坐标变换是相对固定坐标系进行的；假如相对动坐标系进行坐标变换，则算子应该右乘。第二次课二、旋转的齐次变换空间某一点a，坐标为（x，y，z），当它绕z轴旋转角后至a点，坐标为（x,y,z），a点和a点的坐标关系为：，或用矩阵表示为：。精品.a点和a点的齐次坐标分别为x y z 1t和x y z 1t，因此a点的旋转齐次变换过程为：，也可简写为：a=rot(z,)a，其中，rot(z,)表示齐次坐标变换时绕z轴的旋转算子，算子左乘表示相对

8、于固定坐标系进行变换，算子的内容为：。同理，可写出绕x轴旋转的算子和绕y轴旋转的算子分别为：，。点a绕任意过原点的单位矢量k旋转角时，kx，ky，kz分别为k矢量在固定参考系坐标轴x、y、z上的三个分量，且kx2+ky2+kz2=1。可以证明，绕任意过原点的单位矢量k转角的旋转齐次变换公式为：式中，vers=(1-cos)。上式称为一般旋转齐次变换通式，它概括了绕x轴、y轴、z轴进行旋转齐次变换的各种特殊情况。反之，若给出某个旋转齐次变换矩阵，则可根据变换通式求出其等效转轴矢量k及等效转角：式中，当取0到180之间的值时，式中的符号取号；当转角很小时，公式很难确定转轴；当精品.接近0或180时

9、，转轴完全不确定。和平移变换一样，旋转变换算子公式以及一般旋转变换算子公式，不仅仅适用于点的旋转变换，而且也适用于矢量、坐标系、物体等旋转变换计算。若相对固定坐标系进行变换，则算子左乘；若相对动坐标系进行变换，则算子右乘。三、平移加旋转的齐次变换平移变换和旋转变换可以组合在一个齐次变换中。2-3 工业机器人连杆参数及其齐次变换矩阵一、连杆参数及连杆坐标系的建立连杆两端有关节n和n+1。该连杆尺寸可以用两个量来描述：一个是两个关节轴线沿公垂线的距离an称为连杆长度；另一个是垂直于an的平面内两个轴线的夹角n，称为连杆扭角。这两个参数为连杆的尺寸参数。再考虑连杆n与相邻连杆n-1的关系，若它们通过

10、关节相连，其相对位置可用两个参数dn和n来确定，其中dn是沿关节n轴线两个公垂线的距离，n是垂直于关节n轴线的平面内两个公垂线的夹角。这是表达相邻杆件关系的两个参数。这样，每个连杆可以由四个参数所描述：其中两个描述连杆尺寸，另外两个描述连杆与相邻杆件的连接关系。对于旋转关节，n是关节变量，其它三个参数固定不变；对于移动关节，dn是关节变量，其它三个参数固定不变。连杆坐标系的建立按下面的规则进行：连杆n坐标系（简称n系）的坐标原点设在关节n的轴线和关节n+1的轴线的公垂线与关节n+1的轴线相交之处，n系的z轴与关节n+1的轴线重合，x轴与上述公垂线重合，且方向从关节n指向关节n+1，y轴则按右手

11、系确定。二、连杆坐标系之间的变换矩阵建立了各连杆坐标系后，n-1系与n系之间的变换关系可以用坐标系的平移、旋转来实现。从n-1系到n系的变换，可先令n-1系绕zn-1轴旋转n角，再沿zn-1轴平移dn，然后沿xn轴平移an，最后绕xn轴旋转n角，使得n-1系与n系重合。用一个变换矩阵an来综合表示上述四次变换时应注意到坐标系在每次旋转或平移后发生了变动，后一次变换都是相对动系进行的，因此在运算中变换算子应该右乘。于是连杆n的齐次变换矩阵为：2-4 工业机器人运动学方程一、机器人运动学方程为机器人的每一个连杆建立一个坐标系，并用齐次变换来描述这些坐标系间的相对关系，也叫相对位姿。通常把描述一个连

12、杆坐标系与下一个连杆坐标系间相对关系的齐次变换矩阵叫做a变换矩阵或a矩阵。如果a1矩阵表示第一个连杆坐标系相对于固定坐标系的位姿，a2矩阵表示第二个连杆坐标系相对于第一个连杆坐标系的位姿，那么第二个连杆坐标系在固定坐标系中的位姿可用a1和a2的乘积来表示：t2=a1a2。同理，若a3矩阵表示第三个连杆坐标系相对于第二个连杆坐标系的位姿，则有精品.t3=a1a2a3，如此类推，对于六连杆机器人，有下列t6矩阵：t6=a1a2a3a4a5a6。此式右边表示了从固定参考系到手部坐标系的各连杆坐标系之间的变换矩阵的连乘，左边t6表示这些变换矩阵的乘积，也就是手部坐标系相对于固定参考系的位姿，称上式为机

13、器人运动学方程，计算结果t6是一个如下的（44）矩阵：，式中，前三列表示手部的姿态，第四列表示手部的位置。第三次课二、正向运动学及实例正向运动学主要解决机器人运动学方程的建立及手部位姿的求解问题。1、平面关节型机器人的运动学方程具有一个肩关节、一个肘关节和一个腕关节的scara机器人的机械结构特点是三个关节轴线是平行的。固定坐标系0和连杆1、连杆2、连杆3的坐标系1、2、3坐落在关节1、关节2、关节3和手部中心。坐标系3也就是手部坐标系。连杆参数中为变量，其余参数d、a、均为常量。考虑到关节轴线平行，并且连杆都在一个平面内的特点，列出scara机器人连杆的参数如下表所示。连杆转角（变量）两连杆

14、间距离d连杆长度a连杆扭角连杆11d1=0a1=l1=1001=0连杆22d2=0a2=l2=1002=0连杆33d3=0a3=l3=203=0该平面关节型机器人的运动学方程为t3=a1a2a3，式中a1表示连杆1的坐标系1相对于固定坐标系0的齐次变换矩阵；a2表示连杆2的坐标系2相对于连杆1的坐标系1的齐次变换矩阵；a3表示连杆3的坐标系即手部坐标系3相对于连杆2的坐标系2的齐次变换矩阵。于是有：。即因此，可写出：精品.t3是a1、a2、a3连乘的结果，表示手部坐标系3（即手部）的位置和姿态。可写出手部位置（41）列阵为：。表示手部姿态的方向矢量n、o、a分别为：2、斯坦福机器人的运动学方程

15、杆号关节转角扭角杆长a距离d123456120456-90900-909000000000d2d300h上表给出了斯坦福机器人各连杆的参数。现在根据各连杆坐标系的关系写出齐次变换矩阵ai。1系与0系是旋转关节连接。坐标系1相对于固定坐标系0的z0轴的旋转为变量1，然后绕自身坐标系x1轴作1的旋转变换，1=90。所以2系与1系是旋转关节连接，连杆距离为d2。坐标系2相对于坐标系1的z1轴的旋转为变量2，然后绕自身坐标系z2轴正向作d2距离的平移变换及绕x2轴作2的旋转坐标变换，2=90。所以3系与2系是移动关节连接。坐标系3相对于坐标系2的z2轴德平移为变量d3。所以精品.斯坦福机器人手腕三个关

16、节都是转动关节，关节变量为4，5及6，并且三个关节的中心重合。系4对系3的旋转变量为4，然后绕自身坐标轴x4作4的旋转变换，4=-90。所以系5对系4的旋转变量为5，然后绕自身坐标轴x5作5的旋转变换，5=90。所以系6相对于系5的旋转变量为6，并移动距离h。所以这样，所有杆的a矩阵已建立。如果要知道非相邻杆件间的关系，只要用相应的a矩阵连乘即可。如：则斯坦福机器人的运动学方程为。方程右边的结果就是最后一个坐标系6的位置和姿态矩阵，各元素均为和d的函数，当和d给出后，可以计算出斯坦福机器人手部坐标系6的位置p和姿态n、o、a。这就是斯坦福机器人手部位姿的解，这个求解过程叫做斯坦福机器人运动学正

17、解。三、反向运动学实例在机器人控制中，往往在已知手部要到达的目标位姿的情况下如何求出关节变量，以驱动各关节的马达，使手部的位姿得到满足，这就是反向运动学问题，也称求运动学逆解。精品.以斯坦福机器人为例介绍反向求解的一种方法。假设h=0，即坐标系6与坐标系5原点重合。已知斯坦福机器人的运动学方程为：t6=a1a2a3a4a5a6，给出t6矩阵及各杆的参数a、d，求关节变量16，其中3=d3。（1）求1：用a1-1左乘运动学方程，得：1t6=a1-1t6=a2a3a4a5a6，左右展开得：取上式左、右两边第三行第四列相等，即：-pxs1+pyc1=d2，引入中间变量r及，令px=rcos，py=r

18、cos，r=(px2+py2)1/2，=arctg(py/px)，则该式化为：cos1sin-cossin1=d2/r。利用和差公式，上式又可化为：sin(-1)=d2/r。这里0d2/r1，0-1，又cos(-1)= 1-(d2/r)21/2，故有：，所以。这里，“+”号对应右肩位姿，“-”号对应左肩位姿。（2）求2：取上面矩阵等式左、右两边第一行第四列相等和第二行第四列相等，即：故：（3）求3：在斯坦福机器人中，3=d3，有（2）中等式可解得：（4）求4：由于3t6=a4a5a6，所以a4-1 3t6=a5a6，左右两边展开后取其左、右两边第三行第三列相等，得：，所以及（5）求5：取（4）

19、中矩阵等式展开左、右两边第一行第三列相等及第二行第三列相等，有：精品.所以：（6）求6：采用方程a5-1 4t6=a6，展开并取其左、右两边第一行第二列相等及第二行第二列相等，有：所以：。至此，1、2、d3、4、5、6全部求出。从以上解的过程看出，这种方法就是将一个未知数由矩阵方程的右边移向左边，使其与其它未知数分开，解出这个未知数，再把下一个未知数移到左边，重复进行，直至解出所有未知数。所以这种方法也叫变量分离法。还应注意到机器人运动学逆解问题的求解存在如下三个问题：（1）解可能不存在。机器人具有一定的工作域，假如给定手部位置在工作域之外，则解不存在。（2）解的多重性。机器人的逆运动学问题可

20、能出现多解。在多解情况下，一定有一个最接近解，即最接近起始点的解。（3）求解方法的多样性。机器人逆运动学求解有多种方法，一般分为两类：封闭解和数值解。应该从计算方法的计算效率、计算精度等要求出发，选择较好的解法。四、x=x(q)形式运动学方程“角度设定法”是采用相对参考坐标系或相对运动坐标系作三次连续转动来规定姿态的方法。机器人手部位姿可用一个6维列矢量来表示：x=px py pz x y zt，或写成x=x y z x y zt。式中，x、y、z表示手部位置，x、y、z分别是用角度设定法来规定手部姿态时绕x轴、y轴和z轴的转角。1、rpy角法和欧拉角法rpy角法和欧拉角法是角度设定法中常用的

21、方法。rpy角法是手部相对参考坐标系轴作三次连续转动获得规定的姿态：先绕x轴转动x角，称为偏转，再绕y轴转动y角，称为俯仰，最后绕z轴转动z角，称为翻滚，得到相应的旋转矩阵为：rpy(x，y，z)=rot(z，x)rot(y，y)rot(x，x)，该式也称为“xyz”rpy角设定法。欧拉角法是手部相对运动坐标系轴作三次连续转动获得规定的姿态：如果转动顺序为zyx，则相应的旋转矩阵为：euler(z，y，x)= rot(z，x)rot(y，y)rot(x，x)，该式也称为“zyx”欧拉角设定法。以上两式结果恰巧完全相同。如果用其它顺序进行欧拉角三次连续转动，结果便不相同了。不论用什么角度设定法来

22、规定手部姿态，姿态的实现，事实上是由关节变量作决定的。知道旋转矩阵则可由以上两式逆解出手部姿态的规定角x、y、z，并且x=x(q)、y=y(q)、z=z(q)，q为广义关节变量，q=q1 q2 qnt。2、运动学方程x=x(q)用a矩阵确定t6，可写成t6=a1(q)a2(q)a3(q)a4(q)a5(q)a6(q)，或写成t6=t(q)。该式表示机器人手部位姿（n，o，a，p）与关节变量q之间的关系。应用第一次课精品.第二次课1、点矢量v为10.00 20.00 30.00t，相对参考系作如下齐次变换，写出变换后点矢量v的表达式。并说明是什么性质的变换，写出rot(?，?)，tran(?，?

23、，?)。2、有一旋转变换，先绕固定坐标系z0轴转45，再绕其x0轴转30，最后绕其y0轴转60，试求该齐次变换矩阵。3、坐标系b起初与固定坐标系0相重合，现坐标系b绕zb轴旋转30，然后绕旋转后的动坐标系xb轴旋转45，试写出该坐标系b的起始矩阵表达式和最后矩阵表达式。4、坐标系a及b在固定坐标系0中的矩阵表达式如下，画出它们在0坐标系中的位置和姿态。5、写出齐次变换矩阵，它表示坐标系b连续相对固定坐标系a作以下变换：（1）绕za轴旋转90；（2）绕xa轴旋转-90；（3）移动3，7，9t。6、写出齐次变换矩阵，它表示坐标系b连续相对自身运动坐标系b作以下变换：（1）移动3，7，9t；（2）绕xb轴旋转-90；（3）绕zb轴旋转90。7、图2-28（a）表示两个楔形物体，试用两个变换序列分别表示两个楔形物体的变换过程，使最后的状态如图（b）所示。第三次课8、如图2-29所示二自由度平面机械手，关节1为转动关节，关节变量为1；关节2为移动关节，关节