机器人及其控制第三章

1、第三章第三章 操作臂运动学操作臂运动学3.1 概述；概述；3.2 连杆描述连杆描述3.3 关于连杆连接的描述关于连杆连接的描述3.4 对连杆附加坐标系的规定对连杆附加坐标系的规定3.5 操作臂运动学操作臂运动学3.6 驱动器空间、关节空间和笛卡尔空间驱动器空间、关节空间和笛卡尔空间3.7 PUMA560机器人的运动学问题（练习）机器人的运动学问题（练习）3.8 坐标系的标准命名坐标系的标准命名3.9 工具的定位工具的定位3.10 计算问题计算问题 23.1 概述概述操作臂的运动学：操作臂的运动学：研究操作臂的运动特性（位置、速度、研究操作臂的运动特性（位置、速度、加速度等），而不考虑所施加的力

2、和力矩。加速度等），而不考虑所施加的力和力矩。本章主要研究操作臂连杆的位置和姿态（位移）本章主要研究操作臂连杆的位置和姿态（位移）研究方法：在操作臂的每个连杆固定一个坐标系，然后研研究方法：在操作臂的每个连杆固定一个坐标系，然后研究这些连杆坐标系的相互关系。（究这些连杆坐标系的相互关系。（建立模型建立模型）3本章的研究重点是：把操作臂的关节变量作为本章的研究重点是：把操作臂的关节变量作为自变量，描述操作臂自变量，描述操作臂末端执行器的位置和姿态末端执行器的位置和姿态和操作臂基坐标系和操作臂基坐标系之间的关系，称为操作臂的之间的关系，称为操作臂的正向运动学（正向运动学（Forward Kinem

3、atics）。）。注意这本身就是建模过程注意这本身就是建模过程43.2 连杆描述连杆描述 两个刚体之间的相对两个刚体之间的相对运动是两个平面之间的滑运动是两个平面之间的滑动时，连接相邻两个刚体动时，连接相邻两个刚体运动副称为运动副称为低副。低副。运动副运动副：机构的每个构件：机构的每个构件以一定的方式连接而且可以一定的方式连接而且可以产生一定的相对运动，以产生一定的相对运动，这种使两构件直接接触而这种使两构件直接接触而又能产生一定形式的相对又能产生一定形式的相对运动的连接称运动副运动的连接称运动副 。5高副高副：两构件通过点或线：两构件通过点或线接触组成的运动副接触组成的运动副 。678910

4、11在操作臂结构设计时，优先考虑具有一个在操作臂结构设计时，优先考虑具有一个自由度自由度的关节作的关节作为连杆的连接方式，为连杆的连接方式， 一般包括一般包括移动关节和转动关节。移动关节和转动关节。12在操作臂结构设计时，优先考虑具有一个在操作臂结构设计时，优先考虑具有一个自由度自由度的关节作的关节作为连杆的连接方式，为连杆的连接方式， 一般包括一般包括移动关节和转动关节。移动关节和转动关节。这是因为：如果一个关节具有这是因为：如果一个关节具有n个自由度，这种关节可以个自由度，这种关节可以看作具有看作具有1个自由度的关节和个自由度的关节和n1个长度为个长度为0的连杆连接而的连杆连接而成，因此不

5、失一般性，仅对含有单自由度的关节的操作臂成，因此不失一般性，仅对含有单自由度的关节的操作臂进行研究。进行研究。连杆标号：连杆标号：基坐标系为基坐标系为0，第一个连杆为，第一个连杆为1，依次类推，依次类推，操作臂最末端的连杆为操作臂最末端的连杆为n。为了使问题简化，可以把为了使问题简化，可以把连杆连杆看作一个看作一个刚体刚体。13关节轴关节轴 i -1和关节轴和关节轴 i 之间的之间的公垂线公垂线的长度的长度称为称为连杆长度连杆长度。1ia关节轴关节轴 i 可以用一可以用一条直线表示，条直线表示，即用即用一个向量来表示。一个向量来表示。空间任意两轴之间空间任意两轴之间的距离是确定的。的距离是确定

6、的。两轴两轴不平行不平行，公垂，公垂线只有一条，平行线只有一条，平行时有无数条。时有无数条。141ia1i连杆转角连杆转角：假设做一：假设做一个平面，并使该平面个平面，并使该平面与两关节轴之间的公与两关节轴之间的公垂线垂直。然后把关垂线垂直。然后把关节轴节轴i1和关节轴和关节轴i投投影到该平面上，在平影到该平面上，在平面内轴面内轴i1按右手法则按右手法则 绕绕 转向轴转向轴i，测量，测量两轴线之间的夹角，两轴线之间的夹角，用回转角用回转角 定义连定义连杆杆i-1的扭转角。的扭转角。15例：例：163.3 连杆连接描述连杆连接描述连杆链中的中间连杆连杆链中的中间连杆相邻两个连杆之间有一个公共的关

7、节轴，沿两个相邻连杆公相邻两个连杆之间有一个公共的关节轴，沿两个相邻连杆公共轴线公垂线交点的距离共轴线公垂线交点的距离 ，称为，称为连杆偏距。连杆偏距。两个公垂线之间的夹角两个公垂线之间的夹角 ，称，称为为关节角关节角。iidid如果关节如果关节i i为转动关节为转动关节i为变量为变量如果关节如果关节i i为移动关节为移动关节为变量为变量171819连杆首尾规定连杆首尾规定假定有假定有n n个连杆，规定：个连杆，规定：0 . 01d0 . 00 . 000nnaa0 . 01如果关节如果关节1 1为转动关节，转角的零位可以任意选取，且规定为转动关节，转角的零位可以任意选取，且规定如果关节如果关

8、节1 1为移动关节，则偏距的零位可以任意选取，且规为移动关节，则偏距的零位可以任意选取，且规定定因为连杆长度和转角取决于关节轴线因为连杆长度和转角取决于关节轴线i i和和i+1i+1，因此连杆长度，因此连杆长度和转角从和转角从1 1到到n n1 1有规定。现在规定有规定。现在规定0 0和和n n。只是为了讨论问。只是为了讨论问题方便。题方便。20连杆参数连杆参数),(11iiiidai移动关节旋转关节iidqi对变量可以统一表示为：对变量可以统一表示为：id根据以上讨论，机器人的每个连杆都可以用根据以上讨论，机器人的每个连杆都可以用4 4个运动学参数个运动学参数表示：表示：前两个参数用于描述连

9、杆本身，另外两个参数用于描述连杆前两个参数用于描述连杆本身，另外两个参数用于描述连杆之间的连接关系。对于转动关节，之间的连接关系。对于转动关节， 为关节为关节变量变量，其余三，其余三个参数不变，对于移动关节，个参数不变，对于移动关节， 为关节为关节变量变量，其余三个，其余三个参数不变。参数不变。21上述参数表示方法称为：上述参数表示方法称为：Denavit-Hartenberg参数，或参数，或D-H参数。由两人于参数。由两人于1955年提出，后来成为机器人的标准建模方年提出，后来成为机器人的标准建模方法。法。 D-H参数一般由表格列出。参数一般由表格列出。对于对于6自由度关节机器人，有自由度关

10、节机器人，有18个固个固定参数定参数223.4 对连杆附加坐标系的规定对连杆附加坐标系的规定23对每个连杆对每个连杆i上面固连一个坐标系，记为上面固连一个坐标系，记为i。连杆链（即。连杆链（即一系列的连杆）中的中间连杆坐标系规定如下：一系列的连杆）中的中间连杆坐标系规定如下：对连杆上固连坐标系的规定：坐标系对连杆上固连坐标系的规定：坐标系ii的的 就是关节轴就是关节轴i i，坐标系坐标系ii的原点为公垂线的原点为公垂线 与关节轴与关节轴i i的交点处，的交点处， 沿沿 方向由关节轴方向由关节轴i i指向关节轴指向关节轴i+1. i+1. 由右手规则确定。由右手规则确定。iaiZ如果如果 ，则，

11、则 垂直于垂直于 和和 所在的平面，其余所在的平面，其余不变。不变。 为为 和和 的夹角。的夹角。iYiXia0iaiXiZ1iZiiZ1iZ2425基坐标系，记为基坐标系，记为0。这个坐标系是一个固定不动的坐标。这个坐标系是一个固定不动的坐标系。原点和坐标系系。原点和坐标系1重合。重合。0Z00iia010 . 01d 轴为轴轴为轴1的方向；的方向；并且当关节变量并且当关节变量1为为0时，设定参考系时，设定参考系1和和0重合。重合。按照这个规定，总有按照这个规定，总有另外当轴另外当轴1为转动关节时，为转动关节时，当轴当轴1为移动关节时，为移动关节时，26对转动关节对转动关节N，设定当，设定当

12、 ，此时，此时 与与轴的方向相同，选取坐标系轴的方向相同，选取坐标系N的原点位置使之满足的原点位置使之满足 ；对于移动关节；对于移动关节N，设定，设定 轴的方向使之轴的方向使之满满足足 ，当，当 时，选取坐标系时，选取坐标系N的原的原点位于点位于 轴与关节轴轴与关节轴N的交点位置。的交点位置。NX0 . 0Nd0 . 0N0 . 0Nd1NXNX0 . 0N1NX2728在连杆坐标系中对连杆参数在连杆坐标系中对连杆参数idiZiXiaiiXiZ 沿沿 轴，从轴，从 移动到移动到 的距离。的距离。iaiZ1iZ 绕绕 轴，从轴，从 移动到移动到 的角度。的角度。iX1iZ 沿沿 轴，从轴，从 移

13、动到移动到 的距离。的距离。iZiX1iX 绕绕 轴，从轴，从 移动到移动到 的角度。的角度。1iX0ia显然显然 ， 的方向有两种旋转，当关节轴相的方向有两种旋转，当关节轴相交时，交时， 有两种选择。当关节轴有两种选择。当关节轴i与与i1平行时，坐标平行时，坐标系系i的原点位置可以任意选取。的原点位置可以任意选取。iZiX2930建立连杆坐标系的步骤建立连杆坐标系的步骤1 1 找出各关节轴，并标出这些轴的延长线；找出各关节轴，并标出这些轴的延长线；2 2 找出关节轴找出关节轴i i和和i+1i+1的公垂线或关节轴的公垂线或关节轴i i和和i+1i+1的交点，以关的交点，以关节轴节轴i i和和

14、i+1i+1的交点或公垂线与关节轴的交点或公垂线与关节轴i i的交点为坐标系的交点为坐标系ii的的交点；交点；3 3 规定规定 的方向；的方向；iZiXiXiY4 4 规定规定 的沿公垂线的指向，如果关节轴的沿公垂线的指向，如果关节轴ii和和i+1i+1相交。相交。则规定则规定 轴垂直于关节轴轴垂直于关节轴ii和和i+1i+1所在的平面；所在的平面；5 5 按照右手定则确定按照右手定则确定 轴；轴；6 6 当第一个关节变量为当第一个关节变量为0 0时，规定坐标系时，规定坐标系00和和11重合。对重合。对于坐标系于坐标系NN，其原点和，其原点和X X的方向可以任意选取，当选取时，的方向可以任意选

15、取，当选取时，尽量使连杆参数为尽量使连杆参数为0.0.3132例：例：3R机器人，三个机器人，三个关节均为旋转关节。关节均为旋转关节。试建立这个机构的试建立这个机构的D-H参数。参数。33操作臂的简图，双斜线表示操作臂的简图，双斜线表示关节轴互相平行。关节轴互相平行。34首先建立基坐标系，即坐标首先建立基坐标系，即坐标系系0，它固连在基座上。当，它固连在基座上。当第一个关节变量第一个关节变量 时，坐时，坐标系标系0与坐标系与坐标系1相互重合，相互重合，因此建立的坐标系因此建立的坐标系0如图。如图。其中其中Z轴和关节轴和关节1的轴线重合。的轴线重合。这个操作臂的所有关节轴线这个操作臂的所有关节轴

16、线都与操作臂所在的平面垂直。都与操作臂所在的平面垂直。由于该操作臂位于一个平面由于该操作臂位于一个平面上，所以所有的上，所以所有的Z轴互相平轴互相平行，所以连杆偏距都为行，所以连杆偏距都为0.0135因为所有的关节轴互相平行，因为所有的关节轴互相平行，且所有的且所有的Z轴都垂直于纸面轴都垂直于纸面向外。因此向外。因此 。0i363R操作臂的操作臂的D-H参数参数373R操作臂的操作臂的D-H参数参数38例：三自由度机器人（例：三自由度机器人（RPR机构），机构），柱坐标机器人柱坐标机器人。最后。最后一个关节（关节一个关节（关节3）可提供机械手的转动。图）可提供机械手的转动。图b为简图。关节轴为

17、简图。关节轴1和关节轴和关节轴2是互相垂直的。是互相垂直的。39下图下图a为操作臂的最小伸展状态时的情况。图为操作臂的最小伸展状态时的情况。图b为连杆坐标系简为连杆坐标系简图。图。因为处于最小伸展状态，坐标系因为处于最小伸展状态，坐标系0和坐标系和坐标系1重合。重合。4041PRP操作臂的操作臂的D-H参数参数42例：表示一个三连杆，例：表示一个三连杆，3R操作臂。关节轴操作臂。关节轴1和关节轴和关节轴2相交，关相交，关节轴节轴2和关节轴和关节轴3平行。平行。431，2两种情况两种情况443，4两种情况两种情况453.5 操作臂运动学操作臂运动学这里指的是操作臂的正向运动学：这里指的是操作臂的

18、正向运动学：正向运动学：描述的是操作臂的关节空间到末端笛卡尔空间正向运动学：描述的是操作臂的关节空间到末端笛卡尔空间的映射关系。的映射关系。正式的说，就是给定关节变量的前提下，求末端执行器的位正式的说，就是给定关节变量的前提下，求末端执行器的位置和姿态。置和姿态。即求解关节即求解关节00到关节到关节NN的齐次变换矩阵。的齐次变换矩阵。)().()(12121010NNNNqTqTqTTH移动关节旋转关节iidqi46连杆变换的推导连杆变换的推导对每个连杆逐一建立坐标系，把运动学问题分解成对每个连杆逐一建立坐标系，把运动学问题分解成N N个子问个子问题，而每个子问题又分为四个次子问题。每个变换对

19、应着一题，而每个子问题又分为四个次子问题。每个变换对应着一个连杆参数。通过观察很容易写出它们的形式。个连杆参数。通过观察很容易写出它们的形式。在坐标系在坐标系i-1i-1和坐标系和坐标系ii之间定义三个坐标系之间定义三个坐标系PP，QQ，R R 。TTTTTPiQPRQiRii11基本指导思想是这样的，只要求出坐标系基本指导思想是这样的，只要求出坐标系i1到坐标系到坐标系i的齐次变换矩阵，然后标号从的齐次变换矩阵，然后标号从1到到N相乘即可。相乘即可。4748坐标系坐标系ii11到坐标系到坐标系RR，旋转，旋转 TTTTTPiQPRQiRii11id1iai1i坐标系坐标系RR到坐标系到坐标系

20、QQ，平移，平移坐标系坐标系QQ到坐标系到坐标系PP，旋转，旋转坐标系坐标系PP到坐标系到坐标系ii，平移，平移根据坐标系描述关系，坐标系根据坐标系描述关系，坐标系i-1i-1到和坐标系到和坐标系ii之间的齐之间的齐次变换矩阵可以写成：次变换矩阵可以写成：4950)()()()(111iZiZiXiXiidDRaDRTQ根据定义：根据定义：),(),(111iiZiiXiidScrewaScrewT),(rScrewQ写成如下形式：写成如下形式：其中其中表示绕表示绕 平移平移r r，再绕，再绕 旋转旋转 角。角。Q定理（莫茨定理）刚体最一般的位移是螺旋位移。定理（莫茨定理）刚体最一般的位移是螺

21、旋位移。51得到表达式：得到表达式：52例：例：PRP机器人的连杆变换矩阵机器人的连杆变换矩阵53连杆变换的推导连杆变换的推导如果已经定义了连杆坐标系和相应的连杆参数，就能直接建如果已经定义了连杆坐标系和相应的连杆参数，就能直接建立运动学方程。把这些连杆矩阵相乘就能得出坐标系立运动学方程。把这些连杆矩阵相乘就能得出坐标系00到到坐标系坐标系NN之间的连续变换：之间的连续变换：TTTTTNNN12312010.TN0TN0变换矩阵变换矩阵 是关于是关于N N个关节变量的函数，如果能够得到机个关节变量的函数，如果能够得到机器人关节位置传感器的值，机器人末端连杆在笛卡尔坐标系器人关节位置传感器的值，

22、机器人末端连杆在笛卡尔坐标系里的位置和姿态就能通过里的位置和姿态就能通过 计算出来。计算出来。 54引入以下表达式：引入以下表达式：jiTTjiITjiTTTTjiijijjjiiiiijif)(ifif.11121jjijjijioRoo111553.6 驱动器空间、关节空间和笛卡尔驱动器空间、关节空间和笛卡尔 空间空间对于一个具有对于一个具有N自由度的操作臂来说，它的所有连杆位置可由一自由度的操作臂来说，它的所有连杆位置可由一组组N个关节变量加以确定。这样的一组变量称为个关节变量加以确定。这样的一组变量称为 的关节变的关节变量。量。1NTNqqqq),.,(21所有关节矢量组成的空间称为关

23、节空间。所有关节矢量组成的空间称为关节空间。末端执行器末端执行器的位置和姿态组成的空间称为笛卡尔空间。有时称的位置和姿态组成的空间称为笛卡尔空间。有时称为任务空间或操作空间。为任务空间或操作空间。关节矢量可以看成驱动器矢量产生。关节矢量可以看成驱动器矢量产生。56一个操作臂的位姿描述可以有三种方式。一个操作臂的位姿描述可以有三种方式。575859603.7 PUMA560机器人运动学机器人运动学 programmable universal manipulator for assembly（PUMA） 可编程通用装配机械手可编程通用装配机械手 6162PUMA560是一个是一个6自由度机器人，

24、所有的关节为转动关节（即这自由度机器人，所有的关节为转动关节（即这是一个是一个6R机构）上图为所有关节角为零位时连杆坐标系的分布机构）上图为所有关节角为零位时连杆坐标系的分布情况。情况。011J 坐标系坐标系0和坐标系和坐标系1重合。重合。这台机器人和许多工业机器人一样，关节这台机器人和许多工业机器人一样，关节4、5和和6的轴线交于一的轴线交于一点。并且交点与坐标系点。并且交点与坐标系4、5和和6的原点重合。而且关节轴的原点重合。而且关节轴4、5、6互相互相垂直。垂直。关节轴线关节轴线 称为腰关节；称为腰关节；关节轴线关节轴线 称为肩关节；称为肩关节；654,JJJ3J2J关节轴线关节轴线 称

25、为肘关节；称为肘关节；关节轴线关节轴线 称为腕关节；称为腕关节；63PUMA560运动参数和坐标系分布运动参数和坐标系分布64PUMA560前臂运动参数和坐标系分布前臂运动参数和坐标系分布65下图为连杆参数，对于下图为连杆参数，对于PUMA560机器人，在操作臂的腕部有一机器人，在操作臂的腕部有一个轮系将关节个轮系将关节4、5、6的运动耦合在一起。因此针对这的运动耦合在一起。因此针对这3个关节，个关节，需要对关节空间和驱动空间加以区分。下面讨论从关节空间到笛需要对关节空间和驱动空间加以区分。下面讨论从关节空间到笛卡尔空间的运动学问题。卡尔空间的运动学问题。D-H参数参数66PUMA560前臂前

26、臂3R机构简图机构简图67根据连杆参数，可得连杆坐标变换矩阵根据连杆参数，可得连杆坐标变换矩阵68写出中间结果，第写出中间结果，第4连杆到第连杆到第6连杆的坐标变换矩阵为：连杆的坐标变换矩阵为：第第3连杆到第连杆到第6连杆的坐标变换矩阵为：连杆的坐标变换矩阵为：69关节轴关节轴2和关节轴和关节轴3互相平行，因此连杆互相平行，因此连杆1到连杆到连杆3的变换矩阵为：的变换矩阵为：这里用了二角和公式：这里用了二角和公式：70第第1连杆到第连杆到第6连杆的坐标变换矩阵为：连杆的坐标变换矩阵为：把矩阵各个元素写出来就是：把矩阵各个元素写出来就是：7172第第0连杆到第连杆到第6连杆的坐标变换矩阵为：连杆

27、的坐标变换矩阵为：把矩阵各个元素写出来就是：把矩阵各个元素写出来就是：73这就是这就是PUMA560机器人运动学方程。它说明坐标系机器人运动学方程。它说明坐标系6相对于坐标系相对于坐标系0的位姿。的位姿。743.8 坐标系的标准命名坐标系的标准命名给坐标系进行命名给坐标系进行命名75基坐标系基坐标系B腕坐标系腕坐标系W位于操作臂的基座上，坐标系位于操作臂的基座上，坐标系0的另外一个名称。的另外一个名称。位于机器人工作台的一个角上，对机器人用户来说，工位于机器人工作台的一个角上，对机器人用户来说，工作台坐标系作台坐标系S是一个通用坐标系，因为机器人所有的运是一个通用坐标系，因为机器人所有的运动都是相对于它来执行的。有时称它为任务坐标系，世动都是相对于它来执行的。有时称它为任务坐标系，世界坐标系或通用坐标系。工作台坐标系通常根据基坐标界坐标系或通用坐标系。工作台坐标系通常根据基坐标系确定。记作：系确定。记作：工作台坐标系工作台坐标系STBS腕部坐标系腕部坐标系W附于操作臂的末端连杆，也称为坐标系附于操作臂的末端连杆，也称为坐标系N。大多数情况下，腕部坐标系大多数情况下，腕部坐标系W的原点位于操作臂的手腕上，的原点位于操作臂的手腕上，它随着操作臂的末端运动。它相对于基坐标系定义，即它随着操