工业机器人-运动学课件.ppt

1、第第3 3章章 机器人运动学机器人运动学3.1 3.1 坐标变换坐标变换3.2 3.2 运动学方程运动学方程 习题习题第第3 3章章 机器人运动学机器人运动学运动学研究的问题运动学研究的问题： 手在空间的运动手在空间的运动与与各个各个关节的运动关节的运动之间的关系。之间的关系。正问题正问题：已知关节运动，求：已知关节运动，求 手的运动。手的运动。逆问题逆问题：已知手的运动，求：已知手的运动，求 关节运动。关节运动。第第3 3章章 机器人运动学机器人运动学数学模型数学模型： 手的运动手的运动位姿变化位姿变化位姿矩阵位姿矩阵M 关节运动关节运动参数变化参数变化关节变量关节变量qi，i=1，n运动学

2、方程运动学方程： M=f(qi)， i=1，n正问题正问题：已知：已知qi，求，求M。逆问题逆问题：已知：已知M，求，求qi。第第3 3章章 机器人运动学机器人运动学预备知识预备知识、机器人位姿的表示、机器人位姿的表示、机器人的坐标系、机器人的坐标系第第3 3章章 机器人运动学机器人运动学、机器人位姿的表示、机器人位姿的表示机器人的位姿主要是指机器人手部在空间的位置机器人的位姿主要是指机器人手部在空间的位置和姿态，有时也会用到其它各个活动杆件在空间的位和姿态，有时也会用到其它各个活动杆件在空间的位置和姿态。置和姿态。 第第3 3章章 机器人运动学机器人运动学、机器人位姿的表示、机器人位姿的表示

3、位置可以用一个位置可以用一个3 31 1的位置矩阵来描述。的位置矩阵来描述。 zyxppppzyx(,)第第3 3章章 机器人运动学机器人运动学、机器人位姿的表示、机器人位姿的表示姿态可以用坐标系姿态可以用坐标系三个坐标轴两两夹角的三个坐标轴两两夹角的余弦值组成余弦值组成3 33 3的姿态的姿态矩阵来描述。矩阵来描述。 (,)hhhh),cos(),cos(),cos(),cos(),cos(),cos(),cos(),cos(),cos(hhhhhhhhhzzyzxzzyyyxyzxyxxxR第第3 3章章 机器人运动学机器人运动学、机器人位姿的表示、机器人位姿的表示 例：右图所示两坐例：右

4、图所示两坐标系的姿态为：标系的姿态为：0000111110000101001R第第3 3章章 机器人运动学机器人运动学2 2、机器人的坐标系、机器人的坐标系手部坐标系手部坐标系参考机器人手部的坐标系，也称机参考机器人手部的坐标系，也称机器人位姿坐标系，它表示机器人手部在指定坐标系中器人位姿坐标系，它表示机器人手部在指定坐标系中的位置和姿态。的位置和姿态。机座坐标系机座坐标系参考机器人机座的坐标系，它是机参考机器人机座的坐标系，它是机器人各活动杆件及手部的公共参考坐标系。器人各活动杆件及手部的公共参考坐标系。杆件坐标系杆件坐标系参考机器人指定杆件的坐标系，它参考机器人指定杆件的坐标系，它是在机器

5、人每个活动杆件上固定的坐标系，随杆件的是在机器人每个活动杆件上固定的坐标系，随杆件的运动而运动。运动而运动。绝对坐标系绝对坐标系参考工作现场地面的坐标系，它是参考工作现场地面的坐标系，它是机器人所有构件的公共参考坐标系。机器人所有构件的公共参考坐标系。 第第3 3章章 机器人运动学机器人运动学2 2、机器人的坐标系、机器人的坐标系手部坐标系手部坐标系 h 机座坐标系机座坐标系 0 杆件坐标系杆件坐标系 i i=1，n绝对坐标系绝对坐标系 B 3.1 3.1 坐标变换坐标变换1 1、直角坐标变换、直角坐标变换2 2、齐次坐标变换、齐次坐标变换3.1 3.1 坐标变换坐标变换1 1、直角坐标变换、

6、直角坐标变换iiiijjjj坐标之间的变换关系：坐标之间的变换关系：平移变换平移变换旋转变换旋转变换（1）平移变换）平移变换 设坐标系设坐标系 i 和坐标系和坐标系 j 具有相同的姿态，但它俩具有相同的姿态，但它俩的坐标原点不重合，若用的坐标原点不重合，若用 矢量表示坐标系矢量表示坐标系 i 和坐标系和坐标系 j 原点之间的矢量，则坐标系原点之间的矢量，则坐标系 j 就可以看成是由坐标就可以看成是由坐标系系 i 沿矢量沿矢量 平移变换而来的，所以称矢量平移变换而来的，所以称矢量 为为平移变平移变换矩阵换矩阵，它是一个，它是一个3 31 1的矩阵，即：的矩阵，即： 3.1 3.1 坐标变换坐标变

7、换ijp1 1、直角坐标变换、直角坐标变换 zyxijppppiiiijjjjijpijpijp3.1 3.1 坐标变换坐标变换1 1、直角坐标变换、直角坐标变换（1）平移变换）平移变换 若空间有一点在坐标系若空间有一点在坐标系 i 和坐标系和坐标系 j 中分别用矢中分别用矢量量 和和 表示，则它们之间有以下关系：表示，则它们之间有以下关系：称上式为称上式为坐标平移方程坐标平移方程。 irjrjijirpr iiiijjjjijpirjr（2）旋转变换）旋转变换 设坐标系设坐标系 i 和坐标系和坐标系 j 的原点重合，但它俩的姿的原点重合，但它俩的姿态不同，则坐标系态不同，则坐标系 j 就可以

8、看成是由坐标系就可以看成是由坐标系 i 旋转变旋转变换而来的，旋转变换矩阵比较复杂，最简单的是绕一根换而来的，旋转变换矩阵比较复杂，最简单的是绕一根坐标轴的旋转变换，下面以此来对旋转变换矩阵作以说坐标轴的旋转变换，下面以此来对旋转变换矩阵作以说明。明。 3.1 3.1 坐标变换坐标变换1 1、直角坐标变换、直角坐标变换iiiijjjj（2）旋转变换）旋转变换绕绕z轴旋转轴旋转角角 坐标系坐标系 i 和坐标系和坐标系 j 的原点重合，坐标系的原点重合，坐标系 j 的的坐标轴方向相对于坐标系坐标轴方向相对于坐标系 i 绕轴旋转了一个绕轴旋转了一个角。角。角的正负一般按右手法则确定，即由角的正负一般

9、按右手法则确定，即由z轴的矢端看，逆轴的矢端看，逆时钟为正时钟为正。3.1 3.1 坐标变换坐标变换1 1、直角坐标变换、直角坐标变换iiiijjjj（2）旋转变换）旋转变换绕绕z轴旋转轴旋转角角 若空间有一点若空间有一点p，则其，则其在坐标系在坐标系 i 和坐标系和坐标系 j 中中的坐标分量之间就有以下关系：的坐标分量之间就有以下关系： 3.1 3.1 坐标变换坐标变换1 1、直角坐标变换、直角坐标变换iiiijjjj jijjijjizzyxyyxx cossinsincos（2）旋转变换）旋转变换绕绕z轴旋转轴旋转角角 若补齐所缺的有些项，再作适当变形，则有：若补齐所缺的有些项，再作适当

10、变形，则有： 3.1 3.1 坐标变换坐标变换1 1、直角坐标变换、直角坐标变换 jjjijjjijjjizyxzzyxyzyxx1000cossin0sincos （2）旋转变换）旋转变换绕绕z轴旋转轴旋转角角 将上式写成矩阵的形式，则有：将上式写成矩阵的形式，则有： 3.1 3.1 坐标变换坐标变换1 1、直角坐标变换、直角坐标变换 jjjiiizyxzyx1000cossin0sincos （2）旋转变换）旋转变换绕绕z轴旋转轴旋转角角 再将其写成矢量形式，则有：再将其写成矢量形式，则有：称上式为坐标旋转方程，式中：称上式为坐标旋转方程，式中： p点在坐标系点在坐标系ii中的坐标列阵（矢

11、量）；中的坐标列阵（矢量）； 点在坐标系点在坐标系jj中的坐标列阵（矢量）；中的坐标列阵（矢量）； 坐标系坐标系jj变换到坐标系变换到坐标系ii的的旋转变换矩阵旋转变换矩阵，也称为也称为方向余弦矩阵方向余弦矩阵。 3.1 3.1 坐标变换坐标变换1 1、直角坐标变换、直角坐标变换jzijirRr ,irjr , zijR（2）旋转变换）旋转变换 旋转变换矩阵，也称为方向余弦矩阵，旋转变换矩阵，也称为方向余弦矩阵，是一个是一个3 33 3的矩阵，其中的每个元素就是坐标系的矩阵，其中的每个元素就是坐标系 i 和和坐标系坐标系 j 相应坐标轴夹角的余弦值，它表明坐标系相应坐标轴夹角的余弦值，它表明坐

12、标系 j 相对于坐标系相对于坐标系 i 的姿态（方向）。的姿态（方向）。3.1 3.1 坐标变换坐标变换1 1、直角坐标变换、直角坐标变换 , zijR（2）旋转变换）旋转变换绕绕x x轴旋转轴旋转角的角的 旋转变换矩阵为：旋转变换矩阵为： 3.1 3.1 坐标变换坐标变换1 1、直角坐标变换、直角坐标变换 cossin0sincos0001,xijRiiiijjjj（2）旋转变换）旋转变换绕绕y y轴旋转轴旋转角的角的 旋转变换矩阵为：旋转变换矩阵为： 3.1 3.1 坐标变换坐标变换1 1、直角坐标变换、直角坐标变换 cos0sin010sin0cos, yijRiiiijjjj（2）旋转

13、变换）旋转变换旋转变换矩阵的逆矩阵旋转变换矩阵的逆矩阵 旋转变换矩阵的逆矩阵既可以用线性代数的方法求旋转变换矩阵的逆矩阵既可以用线性代数的方法求出，也可以用逆向的坐标变换求出。以绕出，也可以用逆向的坐标变换求出。以绕z轴旋转轴旋转角角为例，其为例，其逆向变换即为绕逆向变换即为绕z轴旋转轴旋转- -角角，则其旋转变换，则其旋转变换矩阵就为：矩阵就为： 3.1 3.1 坐标变换坐标变换1 1、直角坐标变换、直角坐标变换 1000cossin0sincos, zijR（2）旋转变换）旋转变换旋转变换矩阵的逆矩阵旋转变换矩阵的逆矩阵 比较以下两式：比较以下两式： 结论结论： 3.1 3.1 坐标变换坐

14、标变换1 1、直角坐标变换、直角坐标变换 1000cossin0sincos, zjiR 1000cossin0sincos, zijRTzijzijRR)()(,1, （3）联合变换）联合变换 设坐标系设坐标系ii和坐标系和坐标系jj之间存在先平移变换，后之间存在先平移变换，后旋转变换，则空间任一点在坐标系旋转变换，则空间任一点在坐标系ii和坐标系和坐标系jj中中的矢量之间就有以下关系：的矢量之间就有以下关系： 称上式为直角坐标系中的坐标称上式为直角坐标系中的坐标联合变换方程联合变换方程。3.1 3.1 坐标变换坐标变换1 1、直角坐标变换、直角坐标变换jijijirRpr （3）联合变换）

15、联合变换 若坐标系若坐标系ii和坐标系和坐标系jj之间是先平移变换，后旋之间是先平移变换，后旋转变换，则上述关系是应如何变化？转变换，则上述关系是应如何变化？3.1 3.1 坐标变换坐标变换1 1、直角坐标变换、直角坐标变换)(jijijirpRr 例例：已知坐标系：已知坐标系BB的初始位置与坐标系的初始位置与坐标系AA重合，首先重合，首先 坐标系坐标系BB沿坐标系沿坐标系AA的的x x轴移动轴移动1212个单位，并沿坐个单位，并沿坐 标系标系AA的的y y轴移动轴移动6 6个单位，再绕坐标系个单位，再绕坐标系AA的的z z轴旋轴旋 转转3030，求平移变换矩阵和旋转变换矩阵。假设某，求平移变

16、换矩阵和旋转变换矩阵。假设某 点在坐标系点在坐标系BB中的矢量为中的矢量为 ，求该点，求该点 在坐标系在坐标系AA中的矢量。中的矢量。 3.1 3.1 坐标变换坐标变换1 1、直角坐标变换、直角坐标变换kjirB095 解解：由题意：由题意可得平移变换矩阵和旋转变换矩阵分别为：可得平移变换矩阵和旋转变换矩阵分别为： ，则：则： 3.1 3.1 坐标变换坐标变换1 1、直角坐标变换、直角坐标变换 0612ABp 1000866. 05 . 005 . 0866. 0100030cos30sin030sin30cosABR 0794.13830.110951000866. 05 . 005 . 0

17、866. 00612BABABArRpr（1 1）齐次坐标的定义）齐次坐标的定义 空间中任一点在直角坐标系中的三个坐标分量用空间中任一点在直角坐标系中的三个坐标分量用 表示，若有四个不同时为零的数表示，若有四个不同时为零的数 与三个直角坐标分量之间存在以下关系：与三个直角坐标分量之间存在以下关系： 则称则称 是空间该点的齐次坐标。是空间该点的齐次坐标。 3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换),(zyxkzzkyykxx ,),(kzyx ),(kzyx （1 1）齐次坐标的定义）齐次坐标的定义齐次坐标的性质齐次坐标的性质.空间中的任一点都可用齐次坐标表示；空间中的

18、任一点都可用齐次坐标表示；.空间中的任一点的直角坐标是单值的，但其对应的空间中的任一点的直角坐标是单值的，但其对应的齐次坐标是多值的；齐次坐标是多值的；.k是比例坐标，它表示直角坐标值与对应的齐次坐标是比例坐标，它表示直角坐标值与对应的齐次坐标值之间的比例关系；值之间的比例关系；.若比例坐标若比例坐标k=1，则空间任一点，则空间任一点( (x, y, z) )的齐次坐标的齐次坐标为为(x, y, z) ，以后用到齐次坐标时，一律默认，以后用到齐次坐标时，一律默认k=1 。 3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换（2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵

19、）矩阵） 若坐标系若坐标系jj是是ii先沿矢量先沿矢量 平移，再绕平移，再绕z轴旋转轴旋转角得到的，则空间任一点在坐标角得到的，则空间任一点在坐标系系ii和坐标系和坐标系jj中的矢量和对应的变换矩阵之间就中的矢量和对应的变换矩阵之间就有有 ，写成矩阵形式则为：，写成矩阵形式则为： 3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换kpjpippzyxij jzijijirRpr , jjjzyxiiizyxpppzyx1000cossin0sincos （2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）再用坐标分量等式表示，则有：再用坐标分量等式表示，则有： 3.

20、1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换 jzijjyijjxizpzyxpyyxpx cossinsincos（2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵） 引入齐次坐标，补齐所缺各项，再适当变形，则有：引入齐次坐标，补齐所缺各项，再适当变形，则有： 3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换 110001110010cossin10sincosjjjzjjjiyjjjixjjjizyxpzyxzpzyxypzyxx （2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）再将其写成矩阵形式则有：再将其写成矩阵形式则有：

21、 3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换 110001000cossin0sincos1jjjzyxiiizyxpppzyx （2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）由此可得联合变换的齐次坐标方程为：由此可得联合变换的齐次坐标方程为： 式中，式中， 齐次坐标变换矩阵齐次坐标变换矩阵， 它是一个它是一个4 44 4的矩阵。的矩阵。 3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换 11jijirMrijM（2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）齐次坐标变换矩阵的意义齐次坐标变换矩阵的意义若将齐次坐标变换

22、矩阵分块，则有：若将齐次坐标变换矩阵分块，则有：意义意义：左上角的：左上角的3 33 3矩阵是两个坐标系之间的矩阵是两个坐标系之间的旋转变换旋转变换矩阵，它描述了姿态关系矩阵，它描述了姿态关系；右上角的；右上角的3 31 1矩阵是两个矩阵是两个坐标系之间的坐标系之间的平移变换矩阵，它描述了位置关系平移变换矩阵，它描述了位置关系，所，所以齐次坐标变换矩阵又称为以齐次坐标变换矩阵又称为位姿矩阵位姿矩阵。 3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换 1010001000cossin0sincos,ijzijzyxijpRpppM 101000ijijzzzzyyyyxxxxi

23、jpRpaonpaonpaonM（2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）齐次坐标变换矩阵的意义齐次坐标变换矩阵的意义齐次变换矩阵的通式为：齐次变换矩阵的通式为： 式中，式中， jj的原点在的原点在ii中的坐标分量；中的坐标分量； jj的的x x轴对轴对ii的三个方向余弦；的三个方向余弦； jj的的y y轴对轴对ii的三个方向余弦；的三个方向余弦； jj的的z z轴对轴对ii的三个方向余弦。的三个方向余弦。3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换zyxppp,zyxnnn,zyxooo,zyxaaa,（2 2）齐次变换矩阵（）齐次变换矩阵（D-H

24、D-H矩阵）矩阵）单独的平移或旋转齐次坐标变换矩阵单独的平移或旋转齐次坐标变换矩阵 平移变换的齐次矩阵为：平移变换的齐次矩阵为：3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换 zyxijpppp已已知知： 101000100010001),(ijzyxzyxppEppppppTransM则：则：（2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）单独的平移或旋转齐次坐标变换矩阵单独的平移或旋转齐次坐标变换矩阵 旋转变换的齐次矩阵为：旋转变换的齐次矩阵为：3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换 1000cossin0sincos, zi

25、jR已已知知： 1001000010000cossin00sincos),(z,ijzRRzRotM 则：则：（2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）单独的平移或旋转齐次坐标变换矩阵单独的平移或旋转齐次坐标变换矩阵 同理可得：同理可得：3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换 cossin0sincos0001,xijR已已知知： 10010000cossin00sincos00001),( x,ijxRRxRotM则：则：（2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）联合变换与联合变换与单步齐次变换矩阵的关系单步齐次变换矩阵

26、的关系观察以下三个齐次变换矩阵的关系：观察以下三个齐次变换矩阵的关系： 3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换 1010001000cossin0sincos,ijzijzyxijpRpppM 1000100010001zyxppppM 1000010000cossin00sincos zRM（2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）联合变换与联合变换与单步齐次变换矩阵的关系单步齐次变换矩阵的关系经观察可得：经观察可得： 3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换zRpzyxzyxijMMppppppM 10000100

27、00cossin00sincos100010001000110001000cossin0sincos （2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵） 联合变换与联合变换与单步齐次变换矩阵的关系单步齐次变换矩阵的关系 任何一个任何一个齐次坐标变换矩阵齐次坐标变换矩阵均可分解为一个均可分解为一个平移变平移变换矩阵换矩阵与一个与一个旋转变换矩阵旋转变换矩阵的的乘积乘积，即：，即： 3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换 100000010001000100011000zzzyyyxxxzyxzzzzyyyyxxxxijaonaonaonppppaonpa

28、onpaonM（2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵） 联合变换与联合变换与单步齐次矩阵的关系单步齐次矩阵的关系 当空间有任意多个坐标系时，若当空间有任意多个坐标系时，若已知相邻坐标系之间的齐次坐标变换已知相邻坐标系之间的齐次坐标变换矩阵，则由坐标变换原理可知：矩阵，则由坐标变换原理可知： 由此可知，建立机器人的坐标系，由此可知，建立机器人的坐标系，可以通过齐次坐标变换，将机器人手可以通过齐次坐标变换，将机器人手部在空间的位置和姿态用齐次坐标变部在空间的位置和姿态用齐次坐标变换矩阵描述出来，从而建立机器人的换矩阵描述出来，从而建立机器人的运动学方程。运动学方程。 3.1

29、 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换0i-1innniinMMMMM1112010 （2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵） 相对变换相对变换 两个坐标系之间总的齐次坐标变换矩阵等于每两个坐标系之间总的齐次坐标变换矩阵等于每次单独变换的齐次坐标变换矩阵的乘积，而相对变次单独变换的齐次坐标变换矩阵的乘积，而相对变换则决定这些矩阵相乘的顺序，称其为左乘和右乘换则决定这些矩阵相乘的顺序，称其为左乘和右乘原则：原则：.若坐标系之间的变换是始终相对于原来的参考若坐标系之间的变换是始终相对于原来的参考坐标系，则齐次坐标变换矩阵坐标系，则齐次坐标变换矩阵左乘左

30、乘；.若坐标系之间的变换是相对于当前新的坐标系，若坐标系之间的变换是相对于当前新的坐标系，则齐次坐标变换矩阵则齐次坐标变换矩阵右乘右乘。 3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换（2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）相对变换相对变换例例：已知坐标系：已知坐标系BB是绕坐标系是绕坐标系AA的的zA轴旋转轴旋转9090，再绕，再绕AA的的xA轴旋转轴旋转9090，最后沿矢量，最后沿矢量 平移得到的，求坐标系平移得到的，求坐标系AA与与坐标系坐标系BB之间的齐次坐标变换矩阵。之间的齐次坐标变换矩阵。 3.1 3.1 坐标变换坐标变换2 2、齐次坐标

31、变换、齐次坐标变换kjipA953 （2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）相对变换相对变换解解：由于变换始终是相对于原来的参考坐标系，：由于变换始终是相对于原来的参考坐标系，所以满足左乘原则，即有：所以满足左乘原则，即有： 3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换)90,()90,()9 , 5, 3(AAABzRotxRotTransM 100001000090cos90sin0090sin90cos1000090cos90sin0090sin90cos000011000910050103001 1000900151003010（2 2）齐

32、次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）相对变换相对变换解解：若例中的变换是相对于每次变换后新的当前若例中的变换是相对于每次变换后新的当前坐标系，其就满足右乘原则，即有：坐标系，其就满足右乘原则，即有： 3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换)9 , 5, 3()90,()90,( TransxRotzRotMAB 10009100501030011000090cos90sin0090sin90cos00001100001000090cos90sin0090sin90cos 1000501030019100（2 2）齐次变换矩阵（）齐次变换矩阵（D-HD

33、-H矩阵）矩阵）逆变换逆变换 已知已知ii通过先平移，通过先平移，后旋转变成后旋转变成jj，则变换，则变换矩阵为：矩阵为： 3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换iiiijjjjijp , zijR 10001000cossin0sincoszyxijpppM （2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）逆变换逆变换逆变换时：逆变换时： 变换顺序颠倒变换顺序颠倒； 先平移，后旋转先平移，后旋转先旋转，后平移。先旋转，后平移。 变换参数取反变换参数取反。 旋转（旋转（） （ -），）， 平移（平移（px，py，pz） （-px，-py，-pz）。）

34、。 3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换（2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）逆变换逆变换 则则jj到到ii的变换矩阵为：的变换矩阵为： 3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换iiiijjjjjip , zjiR),(),(zyxjipppTranszRotM （2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）逆变换逆变换3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换 1000100cos)()sin(0cossinsin)(cos0sincos1000100010001100

35、0010000cossin00sincoszyxyxzyxjippppppppM （2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）逆变换逆变换3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换)(101000)(1)(0)(0100)(0)(cos)()sin(0cossin)(0)(sin)(cos0sincos1 ijTijijTijTijzyxzyxzyxjiRRpRRpppppppppM （2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）逆变换逆变换若齐次坐标变换矩阵为：若齐次坐标变换矩阵为：则：则： 3.1 3.1 坐标变换坐标变换2 2

36、、齐次坐标变换、齐次坐标变换 101000ijijzzzzyyyyxxxxijpRpaonpaonpaonM 1000101apaaaopooonpnnnpRRMMzyxzyxzyxijTijTijijji（2 2）齐次变换矩阵（）齐次变换矩阵（D-HD-H矩阵）矩阵）逆变换逆变换若齐次坐标变换矩阵为：若齐次坐标变换矩阵为：3.1 3.1 坐标变换坐标变换2 2、齐次坐标变换、齐次坐标变换 10ijijijpRM 10,0111111ijTijTijijjipRRMMCCBAAMCBAM可可知知：则则由由3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立

37、步骤 （1 1）建立坐标系）建立坐标系 （2 2）确定参数）确定参数 （3 3）相邻杆件的位姿矩阵）相邻杆件的位姿矩阵 （4 4）建立方程）建立方程2 2、运动学方程的解、运动学方程的解3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤运动学方程的模型：运动学方程的模型： M=f(qi)， i=1，n M机器人手在空间的位姿机器人手在空间的位姿 qi机器人各个关节变量机器人各个关节变量3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（1 1）建立坐标系）建立坐标系 机座坐标系机座坐标系00 杆件坐标系杆件

38、坐标系ii i=1 i=1，2 2，n n 手部坐标系手部坐标系hh3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（1 1）建立坐标系）建立坐标系 机座坐标系机座坐标系00 建立原则：建立原则：z z轴垂直，轴垂直， x x轴水平，轴水平，方向指向手部所在平面。方向指向手部所在平面。x0z0o03.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（1 1）建立坐标系）建立坐标系杆件坐标系杆件坐标系ii，i=1i=1，2 2，n n 建立原则：建立原则： z z轴与关节轴线重合，轴与关节轴线重合， x x轴

39、与两关节轴线的轴与两关节轴线的 距离距离重合，方向指向下一个杆件。重合，方向指向下一个杆件。 杆件坐标系有两种：杆件坐标系有两种： 第一种第一种： z z轴与轴与i+1i+1关节轴线重合；关节轴线重合； 第二种第二种： z z轴与轴与i i关节轴线重合。关节轴线重合。3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（1 1）建立坐标系）建立坐标系杆件坐标系杆件坐标系ii 第一种坐标系：第一种坐标系： z z轴与轴与i+1i+1关节轴关节轴线重合。线重合。x0z0o00123关节关节1关节关节2关节关节3z2x2o2x1y1o1z3x3o3x0z0o

40、00123关节关节1关节关节2关节关节3x2y2o2z3x3o3x1z1o13.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（1 1）建立坐标系）建立坐标系杆件坐标系杆件坐标系ii 第二种坐标系：第二种坐标系： z z轴与轴与i i关节轴线关节轴线重合。重合。3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（1 1）建立坐标系）建立坐标系手部坐标系手部坐标系hh 在第一种杆件在第一种杆件坐标系下，坐标系下，hh与与nn坐标系重合。坐标系重合。x0z0o00123关节关节1关节关节2关节关节3z2x2o2

41、x1y1o1Z3hx3ho3h3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（1 1）建立坐标系）建立坐标系手部坐标系手部坐标系hh 在第二种杆件在第二种杆件坐标系下，坐标系下，hh与与nn坐标系的方向保坐标系的方向保持一致。持一致。ohx0z0o00123关节关节1关节关节2关节关节3x2y2o2z3x3o3x1z1o1Zhxh3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（2 2）确定参数）确定参数杆件几何参数（不变）杆件几何参数（不变） I I、杆件长度、杆件长度li：两关节轴线的距离。两关节

42、轴线的距离。 IIII、杆件扭角、杆件扭角i：两关节轴线的夹角。两关节轴线的夹角。ilii3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（2 2）确定参数）确定参数关节运动参数关节运动参数 I I、关节平移量、关节平移量di： 相邻杆件的长度在关相邻杆件的长度在关节轴线上的距离。节轴线上的距离。 IIII、关节回转量、关节回转量i： 相邻杆件的长度在关相邻杆件的长度在关节轴线上的夹角。节轴线上的夹角。ili-1i-1lii关节关节idi3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（2 2）确定参数

43、）确定参数关节运动参数关节运动参数 关节变量：关节变量： di平移关节；平移关节； i回转关节。回转关节。iiiiidssq)1( 为移动关节为移动关节为转动关节为转动关节iisi, 0, 1ili-1i-1lii关节关节idi3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（3 3）相邻杆件位姿矩阵）相邻杆件位姿矩阵第一种坐标系第一种坐标系 建立坐标系建立坐标系i-1i-1、ii，试分析试分析i-1ii-1i的变换过的变换过程。程。ii-1关节关节iXi-1Z i-1Oi-1XiZiOi3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动

44、学方程建立步骤、运动学方程建立步骤（3 3）相邻杆件位姿矩阵）相邻杆件位姿矩阵第一种坐标系第一种坐标系 I I、i-1ii-1i变换过程变换过程 a a、Trans（0，0，di）；）； b b、Rot（z，i）；）；c c、Trans（li，0，0）；）； d d、Rot（x，i）。）。ii-1lii关节关节idiXi-1Z i-1Oi-1XiZiOii3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（3 3）相邻杆件位姿矩阵）相邻杆件位姿矩阵第一种坐标系第一种坐标系 IIII、单步齐次变换矩阵、单步齐次变换矩阵 10000cossin00sin

45、cos00001,1000010000100011000010000cossin00sincos,100010000100001iiiidiciiiibiaMlMMdM 3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（3 3）相邻杆件位姿矩阵）相邻杆件位姿矩阵第一种坐标系第一种坐标系 IIIIII、相邻杆件的位姿矩阵、相邻杆件的位姿矩阵 1000cossin0sinsincoscoscossincossinsincossincos10000cossin00sincos0001100010000cossin00sincos)()(1iiiiiiii

46、iiiiiiiiiiiiiiiiiiidcbaiidllldMMMMM 3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（3 3）相邻杆件位姿矩阵）相邻杆件位姿矩阵第一种坐标系第一种坐标系 IIIIII、相邻杆件的位姿矩阵、相邻杆件的位姿矩阵关关节节变变量量iqqfMdssqdllMiiiiiiiiiiiiiiiiiiiiiiiiiiii),()1(1000cossin0sinsincoscoscossincossinsincossincos11 3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（3 3

47、）相邻杆件位姿矩阵）相邻杆件位姿矩阵第一种坐标系第一种坐标系 注意注意：特例！：特例！ 10000cossin00sincos0001100010000cossin00sincosiiiiicddciiiiiabbalMMMMdMMMM 3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（3 3）相邻杆件位姿矩阵）相邻杆件位姿矩阵第二种坐标系第二种坐标系 建立坐标系建立坐标系i-1i-1、ii，试分析试分析i-1ii-1i的变换过的变换过程。程。ii-1关节关节iXi-1Z i-1Oi-1XiZiOi3.2 3.2 运动学方程的建立运动学方程的建立1

48、 1、运动学方程建立步骤、运动学方程建立步骤（3 3）相邻杆件位姿矩阵）相邻杆件位姿矩阵第二种坐标系第二种坐标系 I I、i-1ii-1i变换过程变换过程 a a、Trans（li-1，0，0）；）； b b、Rot（x，i-1）；）；c c、Trans（0，0，di）；）； d d、Rot（z，i）。）。ili-1i-1i关节关节idiXi-1Z i-1Oi-1XiZiOii-13.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（3 3）相邻杆件位姿矩阵）相邻杆件位姿矩阵第二种坐标系第二种坐标系 IIII、单步齐次变换矩阵、单步齐次变换矩阵 100

49、0010000cossin00sincos,10001000010000110000cossin00sincos00001,10000100001000111111iiiidiciiiibiaMdMMlM 3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（3 3）相邻杆件位姿矩阵）相邻杆件位姿矩阵第二种坐标系第二种坐标系 IIIIII、相邻杆件的位姿矩阵、相邻杆件的位姿矩阵 1000coscoscossinsinsinsinsincoscossincos0sincos100010000cossin00sincos10000cossin00sinco

50、s0001)()(111111111111111iiiiiiiiiiiiiiiiiiiiiiiiiiidcbaiiddldlMMMMM 3.2 3.2 运动学方程的建立运动学方程的建立1 1、运动学方程建立步骤、运动学方程建立步骤（3 3）相邻杆件位姿矩阵）相邻杆件位姿矩阵第二种坐标系第二种坐标系 IIIIII、相邻杆件的位姿矩阵、相邻杆件的位姿矩阵关关节节变变量量iqqfMdssqdlMiiiiiiiiiiiiiiiiiiiii),()1(100010000cossin00sincos10000cossin00sincos00011111111 3.2 3.2 运动学方程的建立运动学方程的建