机械原理教案

1、13 工业机器人机构学工业机器人机构学13.1概述概述13.2工业机器人的组成工业机器人的组成13.3工业机器人的分类与性能工业机器人的分类与性能13.4工业机器人的运动学基础工业机器人的运动学基础 13.5工业机器人的正向运动学工业机器人的正向运动学 13.6工业机器人的逆向运动学工业机器人的逆向运动学 提提 要要研究了研究了坐标变换与空间物坐标变换与空间物体的位姿与位移的齐次坐标表体的位姿与位移的齐次坐标表达；达；研究了研究了已知各个关节的相已知各个关节的相对运动时，如何确定工业机器对运动时，如何确定工业机器人末端操作器的位姿；人末端操作器的位姿；研究了研究了已知目标对象的位姿时，如何已知

2、目标对象的位姿时，如何确定工业机器人各个关节的相确定工业机器人各个关节的相对运动量。对运动量。 介绍了工业机器人的组成原理、分类与工作性能特点。介绍了工业机器人的组成原理、分类与工作性能特点。chapter 13 kinematics of industrial robots13 工业机器人机构学工业机器人机构学13.1概述概述工业机器人是用来搬运材料、零件与工具，进行焊接与喷涂工业机器人是用来搬运材料、零件与工具，进行焊接与喷涂的可再编程的多功能机械手，通过调用不同的程序来完成预设的的可再编程的多功能机械手，通过调用不同的程序来完成预设的多种工作任务。多种工作任务。 图图131工业机器人工业

3、机器人(a) (b)13.2工业机器人的组成工业机器人的组成工业机器人由三大部分六个子系统组成。工业机器人由三大部分六个子系统组成。三大部分三大部分是机械是机械部分、传感部分和控制部分。部分、传感部分和控制部分。六个系统六个系统是驱动系统、机构与结是驱动系统、机构与结构系统、感觉系统、机器人与环境交互系统、人机交互系统和构系统、感觉系统、机器人与环境交互系统、人机交互系统和控制系统。控制系统。 图图132汽车生产线上的工业机器人汽车生产线上的工业机器人1. 机器人的机构与结构系统机器人的机构与结构系统 工业机器人的机械部分由三部分组成，即机身、手臂和末工业机器人的机械部分由三部分组成，即机身、

4、手臂和末端操作器。机身可以是固定的，也可以是移动的。手臂进一步端操作器。机身可以是固定的，也可以是移动的。手臂进一步划分为上臂和下臂，上臂与机身形成肩关节，上臂与下臂形成划分为上臂和下臂，上臂与机身形成肩关节，上臂与下臂形成肘关节，下臂与末端操作器形成碗关节，如图肘关节，下臂与末端操作器形成碗关节，如图13-3所示。所示。 图图133 机器人的机构与结构机器人的机构与结构(a)(b)图图13f01 喷涂机器人喷涂机器人(1) 喷涂机器的一种类型喷涂机器的一种类型2. 机器人手部的机构与结构系统机器人手部的机构与结构系统图图13-4 单自由度操作器单自由度操作器图图13-5 多自由度操作器多自由

5、度操作器(2) 具有多个自由具有多个自由 度的末端操作器度的末端操作器(1) 具有一个相对自具有一个相对自 由度的末端操作器由度的末端操作器13.3工业机器人的分类与性能工业机器人的分类与性能直角坐标型操作机如图直角坐标型操作机如图136所示，它有三个移动关节所示，它有三个移动关节(ppp)，可使末端操作器作三个方向的独立位移。，可使末端操作器作三个方向的独立位移。1. 直角坐标型直角坐标型yxzpxpzpy末端操作器123ppp图图136直角坐标型操作机直角坐标型操作机该种型式的工业机器人该种型式的工业机器人，定位精度较高，空间轨迹，定位精度较高，空间轨迹规划与求解相对较容易，计规划与求解相

6、对较容易，计算机控制相对较简单。它的算机控制相对较简单。它的不足是空间尺寸较大，运动不足是空间尺寸较大，运动的灵活性相对较差，运动的的灵活性相对较差，运动的速度相对较低。速度相对较低。2. 圆柱坐标型圆柱坐标型圆柱坐标型操作机如图圆柱坐标型操作机如图137所示，它有两个移动关节和一所示，它有两个移动关节和一个转动关节个转动关节(ppr)，末端操作器，末端操作器的安装轴线之位姿由的安装轴线之位姿由(z，r，)坐坐标予以表示。该种型式的工业标予以表示。该种型式的工业机器人，空间尺寸较小，工作机器人，空间尺寸较小，工作范围较大，末端操作器可获得范围较大，末端操作器可获得较高的运动速度。它的缺点是较高

7、的运动速度。它的缺点是末端操作器离末端操作器离z轴愈远，其切向轴愈远，其切向线位移的分辨精度就愈低。线位移的分辨精度就愈低。rz图图137圆柱坐标型操作机圆柱坐标型操作机3. 球坐标型球坐标型球坐标型操作机如图球坐标型操作机如图138所示，它有两个转所示，它有两个转动关节和一个移动关节动关节和一个移动关节(rrp)，末端操作器的安，末端操作器的安装轴线之位姿由装轴线之位姿由(， r)坐标予以表示。该种型式坐标予以表示。该种型式的工业机器人，空间尺寸的工业机器人，空间尺寸较小，工作范围较大。较小，工作范围较大。zrxyrrp图图138球坐标型操作机球坐标型操作机4. 关节型关节型腕关节的转动腕关

8、节的转动z3属属于末端操作器的自由度于末端操作器的自由度。该种结构的工业机器。该种结构的工业机器人，空间尺寸相对较小人，空间尺寸相对较小，工作范围相对较大，工作范围相对较大，还可以绕过机座周围的还可以绕过机座周围的障碍物，是目前应用较障碍物，是目前应用较多的一种机型。多的一种机型。控制控制系统系统z1y0谐波减速器谐波减速器光电编码盘光电编码盘末端执行器末端执行器x1伺服伺服电机电机y0z1z2z3上臂上臂下臂下臂rrry1x0z0图图139关节型操作机关节型操作机关节型操作机如图关节型操作机如图139所示，它有三个转动关节所示，它有三个转动关节(rrr)，即机身上部相对于下部的转动即机身上部

9、相对于下部的转动y0，肩关节的转动，肩关节的转动z1和肘关节和肘关节的转动的转动z2。13.4工业机器人的运动学基础工业机器人的运动学基础工业机器人是由若干个工业机器人是由若干个关节所联系起来的一种开链关节所联系起来的一种开链，其一端固结在机座上，另，其一端固结在机座上，另一端安装有末端操作器。一端安装有末端操作器。确确定定工业机器人末端操作器安工业机器人末端操作器安装轴线的方位，装轴线的方位，确定确定末端操末端操作器的位姿与位移，作器的位姿与位移，确定确定工工业机器人的操作对象，即目业机器人的操作对象，即目标物体的位姿与位移，构成标物体的位姿与位移，构成了工业机器人运动学基础应了工业机器人运

10、动学基础应该研究的一部分工作。该研究的一部分工作。图图133 机器人的机构与结构机器人的机构与结构末端操作器末端操作器(a)13.4.1目标物体的空间转动矩阵目标物体的空间转动矩阵一个通过坐标原点的矢量一个通过坐标原点的矢量v1绕通过坐标原点的单位矢量绕通过坐标原点的单位矢量u转动转动角到达角到达v2，要求确定，要求确定v2的位姿。为了确定矢量的位姿。为了确定矢量v1绕通过绕通过坐标原点的单位矢量坐标原点的单位矢量u转动转动角到达角到达v2的位姿，将它作如下转动。的位姿，将它作如下转动。图图1310目标物体的空间转动目标物体的空间转动ov1uzyxuxuz-uyv2+1.平面内平面内单位矢量单

11、位矢量绕绕坐标轴坐标轴的转动矩阵的转动矩阵cossinsincosyxyyxxuuuuuuyxyxuuuucossinsincosuuxyuxuyuyuxuxuy11000cossin0sincos1yxyxuuuu图图13.4f01平面内单平面内单位矢量绕坐标轴的转动位矢量绕坐标轴的转动2.空间内空间内单位矢量绕单位矢量绕坐标轴坐标轴的转动矩阵的转动矩阵) 113(cossin0sincos0001,rx)213(cos0sin010sin0cos,yr)313(1000cossin0sincos,zrouzyxuxuzuy图图1310(a)空间内单位空间内单位矢量绕矢量绕坐标轴线的坐标轴线

12、的转动转动矢量矢量v1绕通过坐标原点的单位矢量绕通过坐标原点的单位矢量u转动转动角的矩阵角的矩阵)513(22233,cvusuvuusuvuusuvuucvusuvuusuvuusuvuucvurzxzyyzxxzyyzyxyzxzyxxu)613(111222222zyxzxzyyzxxzyyzyxyzxzyxxzyxvvvcvusuvuusuvuusuvuucvusuvuusuvuusuvuucvuvvv)413(1,133,2vrrrrrvrvyxzxyu矢量矢量v1绕通过坐标原点的单位矢量绕通过坐标原点的单位矢量u转动转动角的矩阵角的矩阵)713(100000044,zzzyyyxx

13、xuqmnqmnqmnr当式当式(137)中的每一个元素为已知时，利用式中的每一个元素为已知时，利用式(135)中中的元素与式的元素与式(137)中的前中的前3行行3列元素对应相等，即可求出矢列元素对应相等，即可求出矢量量v1绕矢量绕矢量u转动的转转动的转角和矢量角和矢量u的姿态。的姿态。矢量矢量v1绕矢量绕矢量u转动的转动的角和矢量角和矢量u的姿态为的姿态为)813()()()(21sin222xyzxyzmnnqqm)913(1)()()(tan222zyxxyzxyzqmnmnnqqm)1013(sin2sin2sin2xyzzxyyzxmnunquqmu例例13-1图图1311为单臂操

14、作机械手，手臂相对于机身拥有一个为单臂操作机械手，手臂相对于机身拥有一个转动自由度，手腕相对于手臂拥有一个转动自由度。已知手转动自由度，手腕相对于手臂拥有一个转动自由度。已知手腕上的坐标系腕上的坐标系oxyz相对于机身坐标系相对于机身坐标系oxyz的位姿矩阵的位姿矩阵sw为为1000210060012010swxyyxzooz1y1x2z2y2o2o1z图图1311单臂操作机械手单臂操作机械手机身机身手臂手臂手腕手腕x1(2) 若手臂相对于机身不动，若手臂相对于机身不动，手腕上的坐标系手腕上的坐标系oxyz相对相对于手臂上的于手臂上的z轴转动轴转动90，则坐标系则坐标系oxyz转到坐标系转到坐

15、标系o2x2y2z2。试写出以上两种。试写出以上两种转动的矩阵转动的矩阵sw1、sw2。sw中前三行前三列的元素表示手腕坐标系的姿态，中前三行前三列的元素表示手腕坐标系的姿态，2,6,2t表示手腕坐标系原点的位置。表示手腕坐标系原点的位置。(1) 若手臂相对于机身坐标系若手臂相对于机身坐标系oxyz的的z轴转动轴转动90，则坐标，则坐标系系oxyz转到坐标系转到坐标系o1x1y1z1。xyyxzooz1y1x2z2y2o2o1z图图1311单臂操作机械手单臂操作机械手机身机身手臂手臂手腕手腕x1坐标系坐标系o1x1y1z1在固定坐标系在固定坐标系oxyz的位姿矩阵的位姿矩阵sw1为为10002

16、1002010600110002100600120101000010000010010,901swrswzo2x2y2z2坐标系在固定坐标系坐标系在固定坐标系oxyz的位姿矩阵的位姿矩阵sw2为为 100021006010200110000100000100101000210060012010,902zrswsw13.4.2坐标系之间的空间变换矩阵坐标系之间的空间变换矩阵 xyzxyzoovvzvxvyvxvyvzp图图1312点在坐点在坐标系之间的变换标系之间的变换设单位矢量设单位矢量v在坐标系在坐标系oxyz中中的投影分别为的投影分别为vx、vy和和vz；矢量矢量p在在坐标系坐标系oxyz

17、中的投影分别为中的投影分别为px，py和和pz；x轴在坐标系轴在坐标系oxyz中中x、y和和z上的投影分别为上的投影分别为txx、txy和和txz；y轴在坐标系轴在坐标系oxyz中中x、y和和z上的投上的投影分别为影分别为tyx、tyy和和tyz；z轴在坐标系轴在坐标系oxyz中中x、y和和z上的投影分别为上的投影分别为tzx、tzy和和tzz。为此，连杆坐标系为此，连杆坐标系o?xyz相对于固相对于固定坐标系定坐标系oxyz的位姿为的位姿为 )1113(zyxzyxzzyzxzzyyyxyzxyxxxzyxpppvvvtttttttttvvvxyzxyzoovvzvxvyvxvyvzp图图1

18、312点在坐点在坐标系之间的变换标系之间的变换txx、txy和和txz的表达式分别为的表达式分别为txxcos(x，x)，txycos(x，y)，txzcos(x，z)，其余的关系式类推。，其余的关系式类推。为了计算机求解方便，将上为了计算机求解方便，将上式改写为齐次坐标形式式改写为齐次坐标形式)1213(1)44(110001zyxzyxxzzyzxzxzyyyxyxzxyxxxzyxvvvtvvvptttptttptttvvvv13.4.3 目标物体的齐次坐标表示目标物体的齐次坐标表示在如图在如图1313a所示的坐标系所示的坐标系oxyz中放置一个楔块，在中放置一个楔块，在楔块上设置坐标系

19、楔块上设置坐标系oxyz，其上的特征点为，其上的特征点为a1，a2，a3，a4，a5和和a6。这些特征点在自身坐标系。这些特征点在自身坐标系oxyz中的坐标分别为中的坐标分别为a1（1,0,0），），a2（1,0,0），），a3（1,0,2），），a4（1,0,2），），a5（1,4,0），），a6（1,4,0）。）。xzyzxya4a3a2a5a6xzzyxya6a5a2a1a3a4图图1313目标物体的齐次坐标变换目标物体的齐次坐标变换(a)(b)ooa1用齐次坐标用齐次坐标wxyz(46)表示这些点在自身坐标系表示这些点在自身坐标系oxyz中的位置为中的位置为)1313(11111100

20、2200440000111111)64(xyzwxzyzxya4a3a2a5a6xzzyxya6a5a2a1a3a4图图1313目标物体的齐次坐标变换目标物体的齐次坐标变换(a)(b)ooa1若让楔块绕若让楔块绕z轴转过轴转过90，再绕，再绕y轴转过轴转过90，最后沿，最后沿x轴方轴方向平移向平移4，则楔块到达图，则楔块到达图12-13b所示的位置。以上的变换所示的位置。以上的变换txyzxyz 为为1000001000014100 1 , 0 , 0 , 4,90,90tzyrrtrxyzxyztxzyzxya4a3a2a5a6xzzyxya6a5a2a1a3a4图图1313目标物体的齐次坐

21、标变换目标物体的齐次坐标变换(a)(b)ooa1此时，楔块上的特征点在此时，楔块上的特征点在oxyz坐标系中的齐次坐标坐标系中的齐次坐标wxyz(46)为为 )64(xyzw)1413(1111110022004400001111111000001000014100111111440000111111446644xzyzxya4a3a2a5a6xzzyxya6a5a2a1a3a4图图1313目标物体的齐次坐标变换目标物体的齐次坐标变换(a)(b)ooa1由图由图1313b也可以得到坐标系也可以得到坐标系oxyz在坐标系在坐标系oxyz中中的齐次坐标的齐次坐标 。已知已知x轴轴的方位为的方位为0

22、,0,1,0t， y轴轴的方位为的方位为1,0,0,0t， z轴轴的方位为的方位为0,1,0,0t，坐标系，坐标系oxyz的原点的原点o在坐标系在坐标系oxyz中的位中的位置为置为0,0,4,1 t。xzyzxya4a3a2a5a6xzzyxya6a5a2a1a3a4图图1313目标物体的齐次坐标变换目标物体的齐次坐标变换(a)(b)ooa1为此，坐标系为此，坐标系oxyz在坐标系在坐标系oxyz中的位姿矩阵中的位姿矩阵txyzxyz为为1000400101000010 xyzxyztxzyzxya4a3a2a5a6xzzyxya6a5a2a1a3a4图图1313目标物体的齐次坐标变换目标物体

23、的齐次坐标变换(a)(b)ooa1可以证明可以证明，txy zxyz与与txyzxyz的乘积为单位矩阵，的乘积为单位矩阵，即即txy zxyzt xyzxyz。若。若txyzxyz的一般形式为的一般形式为)1513(1000zzzzyyyyxxxxpqmnpqmnpqmnxyzxyzt则则txyzxyz的逆变换矩阵的逆变换矩阵txyz xyz为为 1xyzxyztxyzxyzt)1613(1000qpqqqmpmmmnpnnnzyxzyxzyx13.4.4刚体的空间位移矩阵刚体的空间位移矩阵设已知设已知p1=p1x p1yp1zt，q1=q1x q1yq1zt，则，则q=qx qyqzt 的矢

24、量表达式的矢量表达式与矩阵表达式分别为与矩阵表达式分别为在如图在如图1314所示的坐标系所示的坐标系oxyz中有一个连杆，连杆的中有一个连杆，连杆的初始位置用初始位置用p1q1表示，终止位置用表示，终止位置用pq表示，表示，p1点的位置矢量用点的位置矢量用r表示，连杆上的表示，连杆上的p1点沿一单位矢量点沿一单位矢量u位移位移s，同时连杆绕矢量，同时连杆绕矢量u转转动动角，现在确定角，现在确定q点相对于点相对于q1点的位置。点的位置。p1zyxp=p1+suq1sqo图图1314 刚体的空间位移刚体的空间位移ruq式式(1319)中的中的 同式同式(135)。)1913(11000)(1111

25、1,133,zyxuuzyxqqqprsuprqqq式式(1319)右端左侧的矩阵称为刚右端左侧的矩阵称为刚体的有限螺旋位移矩阵。体的有限螺旋位移矩阵。p1zyxp=p1+suq1sqo图图1314 刚体的空间位移刚体的空间位移ruq33,ur13.4.5欧拉角表示的变换矩阵欧拉角表示的变换矩阵 在图在图1315a所示的固定坐标系所示的固定坐标系oxyz中放置一个矢量中放置一个矢量u，其初始位置为其初始位置为u1，坐标系，坐标系oxy z是由是由oxyz绕绕z轴转轴转角度而角度而得到的位置，此时，矢量得到的位置，此时，矢量u1转到转到u2的位置；坐标系的位置；坐标系oxyz是由是由oxyz绕绕

26、x轴转轴转角度而得到的位置，此时，矢量角度而得到的位置，此时，矢量u2转到转到u3的位置；矢量的位置；矢量u3再绕再绕z转动转动角而到达角而到达u4的位置。的位置。xyxyxzzyzo123u1u2u3u44xzyo1u1u2u3u4423(a)(b)图图1315欧拉角表示的变换欧拉角表示的变换在以上的相对转动中，每次都是相对于动坐标系进行的，在以上的相对转动中，每次都是相对于动坐标系进行的，而不是相对于固定坐标系进行的。而不是相对于固定坐标系进行的。、和和 称为欧拉角称为欧拉角。 若让所有的转动都是相对于固定坐标系若让所有的转动都是相对于固定坐标系oxyz进行的，如图进行的，如图13-15b

27、所示，且转动顺序为，先绕所示，且转动顺序为，先绕z轴转轴转角度，再绕角度，再绕x轴转轴转角度，角度，最后绕最后绕z轴转轴转角度。转动变换矩阵为角度。转动变换矩阵为 xyxyxzzyzo123u1u2u3u44xzyo1u1u2u3u4423(a)(b)图图1315欧拉角表示的变换欧拉角表示的变换,zxzrrre以上两种变换的展开式均为以上两种变换的展开式均为10000coscossinsinsin0sincoscoscoscossinsinsincoscoscossin0sinsincoscossinsincossincossincoscos,rxyxyxzzyzo123u1u2u3u44xz

28、yo1u1u2u3u4423(a)(b)图图1315欧拉角表示的变换欧拉角表示的变换13.4.6转动关节之间的位移矩阵转动关节之间的位移矩阵 连杆连杆n右端的坐标系右端的坐标系onxnynzn在左端的坐标系在左端的坐标系on-1xn-1yn-1zn-1中的齐次变换矩阵中的齐次变换矩阵tn为为10000cossin00sincos00001100010000100011000010000cossin00sincosnnnn1n1n1n1n1nxxxxznzzzzndatonznaxn连杆连杆nz1xnyn关节关节n-1关节关节n关节关节n1zz-1xn图图1316转动关节转动关节连杆连杆n-1连

29、杆连杆n1xn-1zn-1oz(n1)oz(n-2)yn-2zn-1zn-2yn-1化简后得转动关节之间的位移矩阵为化简后得转动关节之间的位移矩阵为)2113(1000cossin0sinsincoscoscossincossinsincossincos1nnn1nnn1nn1n1n1nnn1nn1n1nzxxzxxzxzzzxxzxzzndaatonznaxn连杆连杆nz1xnyn关节关节n-1关节关节n关节关节n1zz-1xn图图1316转动关节转动关节连杆连杆n-1连杆连杆n1xn-1zn-1oz(n1)oz(n-2)yn-2zn-1zn-2yn-113.5工业机器人的正向运动学工业机器

30、人的正向运动学工业机器人的工业机器人的正向运动正向运动学学是指已知各关节的类型、是指已知各关节的类型、相邻关节之间的尺寸和相邻相邻关节之间的尺寸和相邻关节相对运动量的大小时，关节相对运动量的大小时，如何确定工业机器人末端操如何确定工业机器人末端操作器在固定坐标系中的位姿。作器在固定坐标系中的位姿。(a)图图131工业机器人工业机器人设工业机器人中的一个连杆一端关节上的坐标系相对于另设工业机器人中的一个连杆一端关节上的坐标系相对于另一端关节上的坐标系的位姿由齐次变换矩阵一端关节上的坐标系的位姿由齐次变换矩阵ti表示，设表示，设t1表示表示第一个连杆一端动关节上的坐标系相对于另一端固定关节上的第一

31、个连杆一端动关节上的坐标系相对于另一端固定关节上的坐标系的位姿；设第二个连杆的一端与第一个连杆形成动关节，坐标系的位姿；设第二个连杆的一端与第一个连杆形成动关节，另一端与下一个连杆形成动关节，齐次变换矩阵用另一端与下一个连杆形成动关节，齐次变换矩阵用t2表示，则表示，则第二个连杆相对于固定关节上的坐标系的位姿第二个连杆相对于固定关节上的坐标系的位姿w2为为w2t1 t2。依次类推，若有六个连杆，则第六个连杆相对于固定关节上的依次类推，若有六个连杆，则第六个连杆相对于固定关节上的坐标系的位姿坐标系的位姿w6为为w6t1 t2 t3 t4 t5 t6 (1222) w6t1 t2 t3 t4 t5

32、 t6 (1322) w6的表现形式可以用以下的的表现形式可以用以下的 (44)矩阵予以表示矩阵予以表示 )2313(1000)44(6zzzzyyyyxxxxpqmnpqmnpqmnw式式(1323)右端的前三列前三行表示末端操作器的姿态，右端的前三列前三行表示末端操作器的姿态，第四列前三行表示末端操作器的位置。第四列前三行表示末端操作器的位置。13.5.1平面关节型机器人的正向运动方程平面关节型机器人的正向运动方程图图1317a所示为由一个肩关节、一个肘关节和一个腕关所示为由一个肩关节、一个肘关节和一个腕关节组成的平面关节型的机器人简图，它的三个关节的轴线节组成的平面关节型的机器人简图，它

33、的三个关节的轴线z0、z1、z2是平行的，它的结构参数如表是平行的，它的结构参数如表13-1所示。所示。表表131平面关节型的机器人的结构参数平面关节型的机器人的结构参数 连杆序连杆序号号n关于关于zn轴轴的转角的转角两连杆之间两连杆之间的距离的距离dzn-1连杆的长度连杆的长度axn连杆的连杆的扭角扭角xn1z1=1（逆时（逆时针为正）针为正）0ax1=a1=(100mm)x1=02z2=2（逆时（逆时针为正）针为正）0ax2=a2=(100mm)x2=03z3=3（逆时（逆时针为正）针为正）0ax3=a3=(100mm)x3=0图图1317a所示的平面关节型机器人的运动分析简图所示的平面关

34、节型机器人的运动分析简图如图如图1317b所示。该平面关节型机器人的运动学方程为所示。该平面关节型机器人的运动学方程为w3=t1 t2 t3 。图图1317 平面关节型机器人平面关节型机器人x0y0o0o1y1x1y2x2x3y3o2o3321(b)o0y0y1y2y3x0x1x2x3o1o2o3123(c)(a)z0z1z2z3o0o1o2o3y0y1y2y3x0x1x2x3123ax1ax2ax33210w3t1 t2 t3中每一项的矩阵表达式为中每一项的矩阵表达式为)2413(10000100sin0cossincos0sincos1000010000100011000010000cos

35、sin00sincos33333333333333aaat)2513(10000100sin0cossincos0sincos1000010000100011000010000cossin00sincos22222222222222aaat)2613(10000100sin0cossincos0sincos1000010000100011000010000cossin00sincos11111111111111aaatw3t1 t2 t3矩阵表达式为矩阵表达式为)2713(10000100sin)sin()sin(0)cos()sin(cos)cos()cos(0)sin()cos(11212

36、32133213211121232133213213aaaaaaw图图1317 平面关节型机器人平面关节型机器人x0y0o0o1y1x1y2x2x3y3o2o3321(b)o0y0y1y2y3x0x1x2x3o1o2o3123(c)(a)z0z1z2z3o0o1o2o3y0y1y2y3x0x1x2x3123ax1ax2ax3321若转角若转角1=30 ，2=60和和3=30 ，如图如图1317c所示，则所示，则该平面关节型机器人的该平面关节型机器人的手部坐标系手部坐标系o3x3y3z3在在固定坐标系固定坐标系o0x0y0z0中中的位姿的位姿w3为为1000010032.1705 . 0866.

37、 02 .1830866. 05 . 03w图图1317 平面关节型机器人平面关节型机器人x0y0o0o1y1x1y2x2x3y3o2o3321(b)o0y0y1y2y3x0x1x2x3o1o2o3123(c)(a)计计 算算 演演 示示z0z1z2z3o0o1o2o3y0y1y2y3x0x1x2x3123ax1ax2ax3321013.5.2斯坦福机器人的正向运动方程斯坦福机器人的正向运动方程 x1x0y10图图1318 斯坦福机器人斯坦福机器人z0y0o0x2y6y2z2z1y3d3d24321x3z365x4z4z5x5z6x6y5y4h1. 坐标系坐标系x1y1z1相对于固相对于固定坐

38、标系定坐标系x0y0z0的位姿的位姿图图1318所示为斯坦福机器人的结构简图，针对图示的坐所示为斯坦福机器人的结构简图，针对图示的坐标系，其参数关系如表标系，其参数关系如表122所示。所示。首先，将坐标系首先，将坐标系x0y0z0绕绕x0轴转动轴转动x190得坐得坐标系标系x1y1z1，然后，绕，然后，绕z0轴转轴转动动z0。该变换矩阵为。该变换矩阵为下面求末端操作器的位姿下面求末端操作器的位姿表表132斯坦福机器人的结构参数斯坦福机器人的结构参数杆件编号杆件编号关节关于关节关于zi轴的转角轴的转角关节关于关节关于xi轴的扭转角轴的扭转角xn杆件长度杆件长度axn1z01(90)9002z12

39、(90)90d2=100mm3z2300d3=300mm4z34(90)9005z45(90)9006z56(90)0h=50mm1) 坐标系坐标系x1y1z1相对于固定坐标系相对于固定坐标系x0y0z0的位姿的位姿10000)90cos()90sin(00)90sin()90cos(000011000010000cossin00sincos11110, 10, 01xxzzrrtx1x0y10图图1318 斯坦福机器人斯坦福机器人z0y0o0x2y6y2z2z1y3d3d24321x3z365x4z4z5x5z6x6y5y4h)2913(100000100cos0sin0sin0cos111

40、12)坐标系坐标系x2y2z2相对于相对于x1y1z1的位姿的位姿 1000090cos90sin0090sin90cos000011000100001000011000010000cossin00sincos222222dtx1x0y10图图1318 斯坦福机器人斯坦福机器人z0y0o0x2y6y2z2z1y3d3d24321x3z365x4z4z5x5z6x6y5y4h)3013(10000100cos0sin0sin0cos22222d3)坐标系坐标系x3y3z3相对于相对于x2y2z2的位姿的位姿 )3113(10001000010000133dtx1x0y10图图1318 斯坦福机器

41、人斯坦福机器人z0y0o0x2y6y2z2z1y3d3d24321x3z365x4z4z5x5z6x6y5y4h4)手腕坐标系相对于手腕坐标系相对于x3y3z3的位姿的位姿 (1) 坐标系坐标系x4y4z4相对于相对于x3y3z3的变换矩阵的变换矩阵 )3213(100000100cos0sin0sin0cos10000)90cos()90sin(00)90sin()90cos(000011000010000cossin00sincos444444443, 43, 34xxzxrrtx1x0y10图图1318 斯坦福机器人斯坦福机器人z0y0o0x2y6y2z2z1y3d3d24321x3z3

42、65x4z4z5x5z6x6y5y4h(2) 坐标系坐标系x5y5z5相对于相对于x4y4z4的变换矩阵的变换矩阵)3313(100000100cos0sin0sin0cos1000090cos90sin0090sin90cos000011000010000cossin00sincos555555554, 54, 45xxzzrrtx1x0y10图图1318 斯坦福机器人斯坦福机器人z0y0o0x2y6y2z2z1y3d3d24321x3z365x4z4z5x5z6x6y5y4h(3) 坐标系坐标系x6y6z6相对于相对于x5y5z5的变换矩阵的变换矩阵 )3413(100010000coss

43、in00sincos1000100001000011000010000cossin00sincos 1 , 0 , 066666666t5, 56hhhtrrtzzx1x0y10图图1318 斯坦福机器人斯坦福机器人z0y0o0x2y6y2z2z1y3d3d24321x3z365x4z4z5x5z6x6y5y4h一旦知道一旦知道了了t1t6，则任意杆件之间的变换矩阵可以使用，则任意杆件之间的变换矩阵可以使用以上公式求解出来。以上公式求解出来。下面依次给出下面依次给出5t6、非相邻杆件之间的变换矩阵、非相邻杆件之间的变换矩阵4t6、3t6、2t6和和1t6的矩阵乘积形式，即的矩阵乘积形式，即 5

44、t6 =t6 ，4t6 = t5t6 ，3t6t4t5t6，2t6t3t4t5t6和和1t6t2t3t4t5t6的表达式。首先，给的表达式。首先，给出杆件出杆件6相对于相对于4的位姿矩阵为的位姿矩阵为)3513(100000cossincoscossinsincossinsinsinsincoscoscos100010000cossin00sincos100000100cos0sin0sin0cos66556565556565666655556564hhhttt其次，给出杆件其次，给出杆件6相对于相对于3的位姿矩阵的位姿矩阵3t6为为)3613(1000cossinsinsincos10000

45、0cossincoscossinsincossinsinsinsincoscoscos100000100cos0sin0sin0cos556565545464654646545454646546465466556565556565444465463hcsscshcsccscsscccshsccssccssccchhtttt再次，给出杆件再次，给出杆件6相对于相对于2的位姿矩阵的位姿矩阵2t6 为为 )3713(1000cossinsinsincos1000cossinsinsincos1000100001000013556565545464654646545454646546465455656

46、554546465464654545464654646543654362dhcsscshcsccscsscccshsccssccssccchcsscshcsccscsscccshsccssccsscccdttttt最后，给出杆件最后，给出杆件6相对于相对于1的位姿矩阵的位姿矩阵1t6为为 1000cossinsinsincos10000100cos0sin0sin0cos35565655454646546465454546465464654222226543261dhcsscshssccscsscccshsccssccsscccdtttttt00)()()()(6465464654652646

47、542652646542652646542652646542ccscsscccsssccssccscscsscccsssscssccccsssscccc)3813(10)()(254543525425254235254252542dshsssdhccschsccscsdhcsschccsscc于是，手部坐标系于是，手部坐标系x6y6z6相对于固定坐标系相对于固定坐标系x0y0z0的变换的变换矩阵矩阵0t6为为0t6t1t2t3t4t5t6(1339) 若给定各个关节关于若给定各个关节关于zi轴的转角轴的转角i，d2、d3和和h的大小，的大小，如表如表132所示，设所示，设0t6的矩阵元素如下的

48、矩阵元素如下）（4013100065432160ttttttpqmnpqmnpqmntzzzzyyyyxxxx式式(1337)中各个元素的表达式分别为中各个元素的表达式分别为nxc1c2(c4c5c6s4s6)c6s2s5 (c5c6 s4c4s6) s1nyc2(c4c5c6s4s6)c6s2s5 s1 c1 (c5c6 s4c4s6) nz(c4c5c6s4s6) s2c2 c6s5mxc1c2(c4c5s6c6s4)s2s5s6 (c5 s4s6c4c6) s1mys1c2(c4c5s6c6s4)s2s5s6 c1 (c5 s4s6c4c6) mz(c4c5s6c6s4) s2c2s5

49、s6qxc1 (c2c4s5c5s2)s1s4 s5qy(c2c4s5c5s2) s1c1s4 s5qzc4s2 s5c2c5pxc1 d3s2d2s1h(c1c2c4s5c1c5s2s1s4s5)pys1 d3s2c1d2h(s1c2c4s5c5s1s2c1s4s5)pzc2d3h(c4s2s5c2c5)以上诸式中，以上诸式中，cicosi，sisini，i16。则。则0t6的运算结果为的运算结果为 1000000130001015010060tx1x0y10图图1218 斯坦福机器人斯坦福机器人z0y0o0x2y6y2z2z1y3d3d24321x3z365x4z4z5x5z6x6y5y4

50、h13.6工业机器人的逆向运动学工业机器人的逆向运动学 工业机器人的逆向运动学是工业机器人的逆向运动学是指已知被作对象的初始位姿与终指已知被作对象的初始位姿与终止位姿时，止位姿时，如何确定工业机器人如何确定工业机器人各关节的相对运动量的大小以及各关节的相对运动量的大小以及末端操作器的相对位姿末端操作器的相对位姿。根据被。根据被作对象的初始位姿与终止位姿，作对象的初始位姿与终止位姿，确定工业机器人各关节的相对运确定工业机器人各关节的相对运动量的大小是对工业机器人进行动量的大小是对工业机器人进行运动控制的基础。运动控制的基础。 x1x0y10图图1318 斯坦福机器人斯坦福机器人z0y0o0x2y

51、6y2z2z1y3d3d24321x3z365x4z4z5x5z6x6y5y4h下面以图下面以图1318所示的斯坦福机器人为例，说明工所示的斯坦福机器人为例，说明工业机器人的逆向运动学的求解方法。业机器人的逆向运动学的求解方法。 斯坦福机器人手部坐标系斯坦福机器人手部坐标系x6y6z6相对于固定坐标系相对于固定坐标系x0y0z0的变换矩阵的变换矩阵0t6如式如式( 12- 39) 所 示 ， 即所 示 ， 即 0t6 t1t2t3t4t5t6，0t6的矩阵形的矩阵形式如式式如式(13-40) 所示。设给定所示。设给定了所有的结构参数并已知手部了所有的结构参数并已知手部坐标系坐标系x6y6z6相

52、对于固定坐标相对于固定坐标系系x0y0z0的位姿的位姿(式式1340)，令令h。下面求各个关节的。下面求各个关节的相对运动量的大小。相对运动量的大小。x1x0y10图图1318 斯坦福机器人斯坦福机器人z0y0o0x2y6y2z2z1y3d3d24321x3z365x4z4z5x5z6x6y5y4h(1) 求坐标系求坐标系x1y1z1相对于相对于x0y0z0的转角的转角1用用t11左乘式左乘式(1340)，得得t11 0t6t11t1t2t3t4t5t6t2t3t4t5t6，)4113(100000cossin010000sincos111111tx1x0y10图图1318 斯坦福机器人斯坦福

53、机器人z0y0o0x2y6y2z2z1y3d3d24321x3z365x4z4z5x5z6x6y5y4h其中其中t11由式由式(1329)以及式以及式(1315) 与与(1316)的变换关系得的变换关系得到，于是到，于是t11和和t11 0t6分别分别为为求坐标系求坐标系x1y1z1相对于相对于x0y0z0的转角的转角1)4213(1000100000cossin010000sincos6543211116011tttttpqmnpqmnpqmnttzzzzyyyyxxxx式式(1342)右端右端t2t3t4t5t6的展开矩阵如式的展开矩阵如式(1338)所示，所示，只要令式只要令式(1338

54、)中的中的h0即可。即可。下面展开式下面展开式(1342)中间两个矩阵的乘积，得中间两个矩阵的乘积，得t11 0t6为为1000cossincossincossincossinsincossincossincossincos11111111111111116011yxyxyxyxzzzzyxyxyxyxppqqmmnnpqmnppqqmmnntt00)()()()(6465464654652646542652646542652646542652646542ccscsscccssscccsccscscsscccsssscssccccsssscccc)4313(102543252542325254

55、2dssdcccscsdscsscc令式令式(1343)两个矩阵的第三行第四列的对应元素相等，两个矩阵的第三行第四列的对应元素相等，得含有得含有1的三角方程以及的三角方程以及1的解分别为的解分别为 pxsin1pycos1d2 (1344) )4513()(arctan2)(arctan22222221yyyxxpdpdpdppx(2) 求坐标系求坐标系x2y2z2相对于相对于x1y1z1的转角的转角2 pxcos1pysin1d3sin2 (1346)pzd3cos2 (1347) 式式(1346) 除以式除以式(1347)得得2为为)4813(sincosarctan1122zyyppp令

56、式令式(1343) 两个矩阵的第一行第四列的对应元素相等，两个矩阵的第一行第四列的对应元素相等，第二行第四列的对应元素相等，得含有第二行第四列的对应元素相等，得含有2的三角方程为的三角方程为(3) 求坐标系求坐标系x3y3z3相对于相对于x2y2z2的位移的位移d3 将式将式(1346)两端乘以两端乘以sin2 ，式，式(1347)两端乘以两端乘以cos2 ，然后相加得然后相加得d3为为x1x0y10图图1318 斯坦福机器人斯坦福机器人z0y0o0x2y6y2z2z1y3d3d24321x3z365x4z4z5x5z6x6y5y4h)4913(cossin)sincos(22113zyxpp

57、pd(4) 求坐标系求坐标系x4y4z4相对于相对于x3y3z3的转角的转角4由于由于3t6t4t5t6，t4 13t6t4 1t4t5t6t5t64t6t4 1 t3 1 t2 1 t1 10t6。)5013(100000cossin010000sincos444414tx1x0y10图图1318 斯坦福机器人斯坦福机器人z0y0o0x2y6y2z2z1y3d3d24321x3z365x4z4z5x5z6x6y5y4h所以，由式所以，由式(1332)、 (1331)、(1330)以及式以及式(1315) 与与(1316)的变换关系，首先求出的变换关系，首先求出t4 、t3和和t2的逆矩阵的逆

58、矩阵t41 、t31和和 t21分别为分别为 (4) 求坐标系求坐标系x4y4z4相对于相对于x3y3z3的转角的转角4)5113(10001000010000113dt)5213(100000cossin10000sincos2222212dtx1x0y10图图1318 斯坦福机器人斯坦福机器人z0y0o0x2y6y2z2z1y3d3d24321x3z365x4z4z5x5z6x6y5y4h为此，为此，4t6t4 1 t3 1 t2 1 t1 10t6的表达式为的表达式为 100010000100001100000cossin010000sincos34444601112131464dttt

59、ttt1000100000cossin010000sincos100000cossin10000sincos111122222zzzzyyyyxxxxpqmnpqmnpqmnd将将4t6t4 1 t3 1 t2 1 t1 10t6展开展开00)()()()()()()()()()(1142411241142241124211221121142411241142241124cmsmcsmssmcmcscnsncsnssncncscmsmcmscnsncnscmsmssmcsmcmcccnsnssncsncnccyxzyxyxyxzyxzyxyxzyxyxyx10)()()()()()()()()()(21142411241142411243211221122114241124114241124dcpspcspsspcpcscqsqcsqssqcqcsdspspcpscqsqcqsdcpspsspcspcpcccqsqssqcsqcqccyxzyxyxzyxzyxzyxyxzyxyxzyx4t6t4 1 t3 1 t2 1 t1 10t6(13-53)令式令式(1353) 与式与式(1335)的第三行第三列的矩阵元素的第三行第三列的矩阵元素对应相等，得关于转角对应