工业机器人的运动学

1、2021/6/1612021/6/1622021/6/163一、点的位置描述一、点的位置描述 在选定的直角坐标系在选定的直角坐标系A 中中,空间任一点空间任一点 P 的位置可用的位置可用31的位置矢量的位置矢量 p 表示表示,其左上标代表选定的参考坐标系其左上标代表选定的参考坐标系 px p= py pz 式中式中:p x 、p y 、p z 是点是点P在在坐标系坐标系A中的三个位置坐标分量中的三个位置坐标分量,如图如图2021/6/164 表示三维空间直角坐标系表示三维空间直角坐标系A中中点点P,则列阵则列阵 Px Py Pz 1 称为三称为三维空间点维空间点P的齐次坐标。的齐次坐标。必须注

2、意必须注意,齐次坐标的表示不是惟一的。我们将其各元齐次坐标的表示不是惟一的。我们将其各元素同乘一非零因子素同乘一非零因子 w 后后,仍然代表同一点仍然代表同一点P,即即 Px a P = Py = b Pz c 1 w式中式中: a = wpx; b= wpy; c = wpz。2021/6/1652021/6/166例例2-1 用齐次坐标写出图用齐次坐标写出图2-3中矢量中矢量 u 、v、w 的方向列阵。的方向列阵。解解 矢量矢量 u: cos =0, cos =0.7071067, cos =0.7071067 u=0 0.7071067 0.7071067 0 T 矢量矢量 v: cos

3、 =0.7071067, cos =0, cos =0.7071067 v=0.7071067 0 0.7071067 0 T 矢量矢量 w: cos =0.5, cos =0.5, cos =0.7071067 w=0.5 0.5 0.7071067 0 T 2021/6/167 四、动坐标系位姿的描述四、动坐标系位姿的描述 动坐标系位姿的描述就是对动坐标系原点位置的描述动坐标系位姿的描述就是对动坐标系原点位置的描述以及对动坐标系各坐标轴方向的描述。以及对动坐标系各坐标轴方向的描述。 1.刚体位置和姿态的描述刚体位置和姿态的描述 设有一刚体设有一刚体Q,如图如图2-4所示所示,O为刚为刚体上

4、任一点体上任一点,O X Y Z 为与刚体固连为与刚体固连的一个坐标系的一个坐标系,称为动坐标系刚体称为动坐标系刚体Q在固在固定坐标系定坐标系OXYZ中的位置可用齐次坐标中的位置可用齐次坐标形式的一个形式的一个(41)列阵表示为：列阵表示为： x0 p= y0 z0 12021/6/168 刚体的姿态可由动坐标系的坐标轴方向来表示。令刚体的姿态可由动坐标系的坐标轴方向来表示。令n 、o 、a分别为分别为 X 、Y、 Z坐标轴的单位方向矢量坐标轴的单位方向矢量,每个单位方向矢量每个单位方向矢量在固定坐标系上的分量为动坐标系各坐标轴的方向余弦在固定坐标系上的分量为动坐标系各坐标轴的方向余弦,用齐次

5、用齐次坐标形式的坐标形式的(41)列阵分别表示为列阵分别表示为n=nx ny nz 0 T, o=ox oy oz 0 T, a=ax ay az 0 T因此因此,图图2-4中刚体的位姿可用下面中刚体的位姿可用下面(44)矩阵来描述矩阵来描述: nx ox ax x0 T=n o a p= ny oy ay y0 nz oz az z0 0 0 0 1 很明显很明显,对刚体对刚体 Q 位姿的描述就是对固连于刚体位姿的描述就是对固连于刚体 Q 的坐标系的坐标系 O X Y Z位姿的描述。位姿的描述。2021/6/169例例2-2 图图2-5表示固连于刚体的坐标系表示固连于刚体的坐标系B位于位于O

6、B点，点， xb=10,yb=5,zb=0。ZB轴与画面垂直轴与画面垂直,坐标系坐标系B相对固定坐标相对固定坐标系系A有一个有一个30的偏转的偏转,试写出表示刚体位姿的坐标系试写出表示刚体位姿的坐标系B的的(44)矩阵表达式。矩阵表达式。解解 XB的方向列阵的方向列阵: n=cos30cos60cos900 T =0.866 0.500 0.000 0 T YB的方向列阵的方向列阵: o=cos120cos30cos900 T =-0.500 0.866 0.000 0 T ZB的方向列阵的方向列阵: a =0.000 0.000 1.000 0 T 坐标系坐标系B 的位置列阵的位置列阵: p

7、 =10.0 5.0 0.0 1 T 所以所以,坐标系坐标系B的的(44)矩阵表达式为矩阵表达式为 0.866 -0.500 0.000 10.0 TT= 0.500 0.866 0.000 5.0 0.000 0.000 1.000 0.0 0 0 0 12021/6/16102.手部位置和姿态的表示手部位置和姿态的表示 机器人手部的位置和姿态也可以用固连于手部的坐标机器人手部的位置和姿态也可以用固连于手部的坐标系系B的位姿来表示的位姿来表示,如图所示。如图所示。 手部的位姿可用手部的位姿可用(44)矩阵矩阵表示为：表示为： nx ox ax px n o a p = ny oy ay py

8、 nz oz az pz 0 0 0 12021/6/1611例例2-3 图表示手部抓握物体图表示手部抓握物体 Q ,物体为边长物体为边长2个单位的个单位的正立方体正立方体,写出表达该手部位姿的矩阵式。写出表达该手部位姿的矩阵式。 解解 因为物体因为物体 Q 形心与手部坐标系形心与手部坐标系 O X Y Z的坐标原点的坐标原点 O 相重合相重合,所所以手部位置的以手部位置的(41)列阵为列阵为 p = 1 1 1 1 T手部坐标系手部坐标系X轴的方向可用单位矢量轴的方向可用单位矢量n来表示来表示: =90,=180,=90 n: nx=cos=0 ny=cos=-1 nz=cos=0 同理同理

9、,手部坐标系手部坐标系 Y轴与轴与 Z 轴的方向可分别用单位矢量轴的方向可分别用单位矢量 o 和和 a 来表示，来表示，根据式根据式(2-8)可知可知,手部位姿可用矩阵表达为手部位姿可用矩阵表达为 0 -1 0 1 T=n o a p= -1 0 0 1 0 0 -1 1 0 0 0 12021/6/1612 2.2齐次变换及运算齐次变换及运算 刚体的运动是由转动和平移组成的。为了能用同一矩刚体的运动是由转动和平移组成的。为了能用同一矩阵表示转动和平移阵表示转动和平移,有必要引入有必要引入(44)的齐次坐标变换矩阵。的齐次坐标变换矩阵。 一、平移的齐次变换一、平移的齐次变换 首先首先,我们介绍

10、点在空间直角坐标系中的平移。我们介绍点在空间直角坐标系中的平移。如图所示如图所示,空间某一点空间某一点A ,坐标为坐标为( x , y ,z),当它平移至当它平移至A点后点后,坐标为坐标为(x,y,z)x=x+xy=y+yz=z+z 或写成或写成: x 1 0 0 x x y = 0 1 0 y y z 0 0 1 z z 1 0 0 0 1 1也可以简写为也可以简写为 A=Trans(x,y,z)A 式中式中: Trans ( x , y , z )表示齐次坐标变换表示齐次坐标变换的平移算子。的平移算子。2021/6/1613 例例2-4 有下面两种情况有下面两种情况(如图所示）动坐标系如图

11、所示）动坐标系A相对于固相对于固定坐标系的定坐标系的 X 0、Y 0、Z0轴作轴作(-1,2,2)平移后到平移后到 A ;动坐标系动坐标系A相对于自身坐标系相对于自身坐标系(即动系即动系)的的X、Y、Z轴分别作轴分别作(-1,2,2)平移后平移后到到A 。已知。已知 写出写出 A A 矩阵表达式。矩阵表达式。 0 -1 0 1A= -1 0 0 1 0 0 -1 1 0 0 0 1解解 动坐标系动坐标系A 的两个平移坐标变换算的两个平移坐标变换算子均为子均为 1 0 0 -1 Trans(x,y,z)= 0 1 0 2 0 0 1 2 0 0 0 1A坐标系是动系坐标系是动系A沿固定坐标系作沿

12、固定坐标系作平移变换得来的平移变换得来的,因此算子左乘因此算子左乘,A的的矩阵表达式为矩阵表达式为A=Trans(-1,2,2) A 0 -1 0 0 = -1 0 0 3 0 0 -1 3 0 0 0 1 A 坐标系是动系坐标系是动系A 沿自身坐标系作沿自身坐标系作平移变换得来的平移变换得来的,因此算子右乘因此算子右乘, A 的的矩阵表达式为矩阵表达式为A=ATrans(-1,2,2) 0 -1 0 -1= -1 0 0 2 0 0 -1 -1 0 0 0 12021/6/1614 二、旋转的齐次变换二、旋转的齐次变换 首先我们介绍点在空间直角坐标系中的旋转首先我们介绍点在空间直角坐标系中的

13、旋转,如图如图2-10所所示示,空间某一点空间某一点A,坐标为坐标为(x,y,z ),当它绕当它绕 Z 轴旋转轴旋转角后至角后至 A 点点,坐标为坐标为( x , y , z )。A 点和点和 A 点的坐标关系为点的坐标关系为:x=cosx-sinyy=sinx+cosyz=z或用矩阵表示为或用矩阵表示为 x cos -sin 0 x y = sin cos 0 y z 0 0 1 z x cos -sin 0 0 x y = sin cos 0 0 y z 0 0 1 0 z 1 0 0 0 1 1A 点和点和 A 点的齐次坐标分别为点的齐次坐标分别为 x y z 1 T和和 x y z 1

14、 T ,因此因此A点的旋转齐次变换过程为点的旋转齐次变换过程为a=Rot(z,)aRot ( z,)表示齐次坐标变换时表示齐次坐标变换时 绕绕 Z 轴的旋转算子轴的旋转算子,算子左乘表算子左乘表示相对于固定坐标系进行变换示相对于固定坐标系进行变换2021/6/1615 下图所示为点下图所示为点A绕任意过原点的单位矢量绕任意过原点的单位矢量k旋转旋转角的情况角的情况, kx, ky, kz分别为分别为 k 矢量在固定参考系坐标轴矢量在固定参考系坐标轴 X 、Y 、Z上的上的三个分量三个分量,且且 kx 2 + ky 2 + kz2=1。可以证得可以证得,绕任意过原点的单位矢量绕任意过原点的单位矢

15、量k转转角的旋转齐次变换公式为角的旋转齐次变换公式为 kxkxvers+c kykxvers-kzs kzkxvers+kys 0 Rot(k,)= kxkyvers+kzs kykyvers+c kzkyvers-kxs 0 kxkzvers-kys kykzvers+kxs kzkzvers+c 0 0 0 0 1 式中式中: vers=1- cos;值的正负号由右手螺旋法则决定。上式称为一值的正负号由右手螺旋法则决定。上式称为一般旋转齐次变换通式般旋转齐次变换通式,它概括了绕它概括了绕X、Y、Z轴进行旋转齐次变换的各种轴进行旋转齐次变换的各种特殊情况。特殊情况。2021/6/1616 三

16、、平移加旋转的齐次变换三、平移加旋转的齐次变换 平移变换和旋转变换可以组合在一个齐次变换中平移变换和旋转变换可以组合在一个齐次变换中, 点点W若还要作若还要作4i-3j+7k 的平移的平移,则如图则如图2-14所示所示,只要左乘上平移变只要左乘上平移变换算子换算子,即可得到最后即可得到最后 E 点的列阵表达式点的列阵表达式 e=Trans(4,-3,7)Rot(y,90)Rot(z,90) u 1 0 0 4 0 0 1 0 7= 0 1 0 -3 1 0 0 0 3 0 0 1 7 0 1 0 0 2 0 0 0 1 0 0 0 1 1 0 0 1 4 7 6= 1 0 0 -3 3 = 4

17、 0 1 0 7 2 10 0 0 0 1 1 1 0 0 1 4 1 0 0 -3 0 1 0 7 0 0 0 1其中其中, 为平移加旋转的一般齐次变换矩阵。为平移加旋转的一般齐次变换矩阵。2021/6/16172.3 工业机器人连杆参数及其齐次变换矩阵工业机器人连杆参数及其齐次变换矩阵 如图如图1所示所示,连杆两端有关节连杆两端有关节 n 和和 n +1。该连杆尺寸可以用两个量来描述。该连杆尺寸可以用两个量来描述:一一 个是两个关节轴线沿公垂线的距离个是两个关节轴线沿公垂线的距离an,称为连杆称为连杆 长度长度; 另一个是垂于另一个是垂于an的平面内两个轴线的夹的平面内两个轴线的夹 角角

18、n,称为连杆扭角。这两个参数为连杆的尺称为连杆扭角。这两个参数为连杆的尺 寸参数。寸参数。 一、连杆参数及连杆坐标系的建立一、连杆参数及连杆坐标系的建立 再考虑连杆再考虑连杆 n 与相邻连杆与相邻连杆 n -1的关系的关系,若它们通过若它们通过 关节相连关节相连,如图如图2所示所示,其相对位置可用两个参数其相对位置可用两个参数 dn和和n来确定来确定,其中其中dn是沿关节是沿关节n轴线两个公垂线轴线两个公垂线 的公距离的公距离,n是垂直于关节是垂直于关节n轴线的平面内两个公轴线的平面内两个公 垂线的夹角。这是表达相邻杆件关系的两个参数。垂线的夹角。这是表达相邻杆件关系的两个参数。图图1图图22

19、021/6/1618 二、连杆坐标系之间的变换矩阵二、连杆坐标系之间的变换矩阵 建立了各连杆坐标系后建立了各连杆坐标系后,n-1系与系与n系间的变换关系可以系间的变换关系可以用坐标系的平移、旋转来实现从用坐标系的平移、旋转来实现从 n -1系到系到 n 系的变换系的变换,可先可先令令 n -1系绕系绕 Zn-1轴旋转轴旋转n角角,再沿再沿 Zn-1轴平移轴平移 dn,然后沿然后沿 Xn轴轴平移平移an,最后绕最后绕Xn轴旋转轴旋转n角角,使得使得n-1系与系与n系重合。于是连系重合。于是连杆杆n的齐次变换矩阵为：的齐次变换矩阵为： An= Rot(z,n) Trans(0,0,dn) Tran

20、s(an,0,0) Rot(x,n) cn -sn 0 0 1 0 0 an 1 0 0 0 = sn cn 0 0 0 1 0 0 0 cn -sn 0 0 0 1 0 0 0 1 dn 0 sn cn 0 0 0 0 1 0 0 0 1 0 0 0 1 cn -sncn snsn ancn = sn cncn -cnsn ansn 0 sn cn dn 0 0 0 12021/6/1619一一 机器人运动学方程机器人运动学方程 通常把描述坐标系与下一个连杆间的相对关系的齐次变换通常把描述坐标系与下一个连杆间的相对关系的齐次变换矩阵叫做矩阵叫做A变换矩阵或变换矩阵或A矩阵。如果矩阵。如果A1

21、矩阵表示第一个连杆坐矩阵表示第一个连杆坐 标系相对于固定的坐标系的位置，标系相对于固定的坐标系的位置，A2矩阵表示第二个连杆坐标矩阵表示第二个连杆坐标系相对第一连杆坐标系的位置，那么第二个连杆坐标系系相对第一连杆坐标系的位置，那么第二个连杆坐标系 在固在固定坐标系中的位置可用定坐标系中的位置可用A1和和A2的乘积来表示：的乘积来表示：T2 =A1A2。 若若A3矩阵表示第三个连杆坐标系相对于第二个连杆坐标系矩阵表示第三个连杆坐标系相对于第二个连杆坐标系的位置，则有：的位置，则有：T3=A1A2A3。如此类推，对于六连杆机器人，。如此类推，对于六连杆机器人，有下列有下列T6矩阵：矩阵：T6=A1

22、A2A3A4A5A6 。计算结果计算结果T6是一个是一个(44)矩阵矩阵,即即 nx ox ax px T6= ny oy ay py nz oz az pz 0 0 0 1式中式中:前三列表示手部的姿态前三列表示手部的姿态;第四列表示手第四列表示手部的位置。部的位置。2021/6/16201.平面关节型机器人的运动学方程平面关节型机器人的运动学方程下图下图1所示为具有一个肩关节、一个肘关节和一个腕关节的所示为具有一个肩关节、一个肘关节和一个腕关节的SCARA机器机器人。机器人连杆的参数如表人。机器人连杆的参数如表1所示，机器人坐标系图所示，机器人坐标系图2所示。所示。图图1图图2表表1202

23、1/6/1621该平面关节型机器人的运动学方程为该平面关节型机器人的运动学方程为 T3=A1A2A3式中式中: A1表示连杆表示连杆1的坐标系的坐标系1相对于固定坐标系相对于固定坐标系0的齐次变换矩阵的齐次变换矩阵; A2表示连杆表示连杆2的坐标系的坐标系2相对于连杆相对于连杆1的坐标系的坐标系1的齐次变换矩阵的齐次变换矩阵; A3表示表示连杆连杆3的坐标系即手部坐标系的坐标系即手部坐标系3相对于连杆相对于连杆2的坐标系的坐标系2的齐次变换矩的齐次变换矩阵。参考图阵。参考图2,于是有于是有 A1 = Rot(z0,1)Trans(l1,0,0) A2 = Rot(z1,2)Trans(l2,0

24、,0) c1 -s1 0 l1c1 c2 -s2 0 l2c2 = s1 c1 0 l1s1 = s2 c2 0 l2s2 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1 A3 = Rot(z2,3)Trans(l3,0,0) c3 -s3 0 l3c3 = s3 c3 0 l3s3 0 0 1 0 0 0 0 1 c123 -s123 0 l3c123+l2c12+l1c1 T3= s123 c123 0 l3s123+l2s12+l1s1 0 0 1 0 0 0 0 12021/6/1622 T3是是A1、A2、A3连乘的结果连乘的结果,表示手部坐标系表示手部坐标系3(即手部即手部)的位的位置和姿态。置和姿态。 nx ox ax pxT3= ny oy ay py nz oz az pz 0 0 0 1于是于是,可写出手部位置可写出手部位置(41)列阵为列阵为 px l3c123+l2c12+l1c1P= py = l3s123+l3s12+l1s1 pz 0 1 1 表示手部姿态的方向矢量表示手部姿态的方向矢量n、o、a分别为分别为 nx c123n = ny = s123 nz 0 0 0 ox -s123 o = oy = c123 oz 0 0 0 ax 0a = ay = 0 az 1 0 0当转角变