机器人学-运动学部分

1、齐次坐标和齐次变换知识点： n点和面的齐次坐标和齐次变换n三个基本旋转矩阵n齐次变换的几何意义n绝对变换：如果所有的变换都是相对于固定坐标系中各坐标轴旋转或平移，则依次左乘，称为绝对变换。n相对变换：如果动坐标系相对于自身坐标系的当前坐标轴旋转或平移，则齐次变换为依次右乘，称为相对变换。补充总结：合成变换n只有平移合成n绕同一个坐标系的合成旋转变换n绕同一个坐标系的合成旋转变换+平移变换n绕不同坐标系的合成旋转变换：左右两侧顺次写n绕不同坐标系的合成旋转变换+平移：左右两侧顺次写，平移放左边平移矩阵间可交换n各变换矩阵不可交换例例1：动坐标系：动坐标系0起始位置与固定参考坐标系起始位置与固定参

2、考坐标系0重合重合,动坐标系动坐标系0做如下运动：做如下运动：R(Z,90) R（y,90） Trans(4，-3, 7)，求合成矩阵求合成矩阵 ozyx74-3owuvvuwzyxoo(o)xyzuvwzyxuwo(o) vTTrans(4, -3, 7) R(Y, 90 ) R(Z,90 )(start)例例2：先平移先平移Trans (4,-3,7)；绕当前绕当前 轴转动轴转动90； 绕当前绕当前 轴转动轴转动90；求合成旋转矩阵。；求合成旋转矩阵。 vw zyxo(o)vwuzyxoowuvozyxowvuxyzoowuv例题例题2 2：O O 与与O O初始重合，初始重合，O O 作

3、如下运动：作如下运动：绕绕X X轴转动轴转动9090 ；绕绕w w轴转动轴转动9090 ；绕绕Y Y轴转动轴转动9090 。求。求 T T；改变旋转顺序，如改变旋转顺序，如何旋转才能获得相同的结果。何旋转才能获得相同的结果。 x10000cos90-sin900R0sin90cos9000001cos90sin9000sin90cos900000100001wRcos900sin9000100sin900cos9000001yR1000001001000001yxwTR R R解解： 解解： 绕绕Z（w）？）？轴转动轴转动90； 绕绕X轴转动轴转动90； 绕绕Y轴转动轴转动90。 100000

4、1001000001yxwuvwxTR R Ryz 验证方法验证方法 ：计算加画图：计算加画图第三章 机器人运动学 n机器人运动学主要是把机器人相对于固定参考系的运动作为时间的函数进行分析研究，而不考虑引起这些运动的力和力矩n把机器人的空间位移解析地表示为时间的函数，研究机器人关节变量和机器人末端执行器位置和姿态之间的关系3.1 机器人运动学所讨论的问题 3.1.1 研究的对象n机器人在基本机构形式上分为两种:关节式串联机器人;并联机器人. PUMA560HexapodFanuc manipulatorn这两种机器人有所不同：串联机器人：工作空间大，灵活；刚度差，负载小，误差累积并放大。并联机

5、器人：刚性好，负载大，误差不积累；工作空间小，姿态范围不大。本章讲解以串联机器人为主。运动学研究的问题Where is my hand?Direct KinematicsHERE!How do I put my hand here?Inverse Kinematics: Choose these angles!研究的问题:n运动学正问题-已知杆件几何参数和关节角矢量，求操作机末端执行器相对于固定参考作标的位置和姿态（齐次变换问题）。n运动学逆问题-已知操作机杆件的几何参数，给定操作机末端执行器相对于参考坐标系的期望位置和姿态（位姿），操作机能否使其末端执行器达到这个预期的位姿？如能达到，那么操

6、作机有几种不同形态可以满足同样的条件？与前一次课的关系3.2 机器人杆件，关节和它们的参数 3.2.1 杆件，关节n操作机由一串用转动或平移（棱柱形）关节连接的刚体（杆件）组成n机座上建立一个固定参考坐标系，最后一个杆件与工具相连n关节和杆件均由底座向外顺序排列，每个杆件最多和另外两个杆件相联，不构成闭环。 关节：n一般说来，两个杆件间是用低付相联的n只可能有6种低付关节：旋转（转动）、棱柱（移动）、圆柱形、球形、螺旋和平面，其中只有旋转和棱柱形关节是串联机器人操作机常见的，各种低副形状如下图所示：旋转旋转棱柱形棱柱形柱形柱形球形球形螺旋形螺旋形平面平面AiAi+1Ai-1 杆件参数的定义 和

7、n li AA，由运动学的观点来看，杆件的作用在于保持其两由运动学的观点来看，杆件的作用在于保持其两端关节间的端关节间的结构形态结构形态不变。不变。由两个参数决定，一是杆件的长度由两个参数决定，一是杆件的长度 li，一个是杆，一个是杆件的扭转角件的扭转角 iAiAi+1iiliili 杆件参数的定义 和n L和L 在A轴线上的交点之间的距离n L和L 之间的夹角，由L 转向L，由右手定则决定正负，对于旋转关节它是个变量 确定确定杆件相对位置杆件相对位置关系，由另外关系，由另外2个参数决定，一个是杆个参数决定，一个是杆件的偏移量件的偏移量 ，一个是杆件的回转角，一个是杆件的回转角 iidiidi

8、AiAi+1iilid1iliAi-1id移动关节杆件参数的定义n确定杆件的结构形态的2个参数Li与i与旋转关节是一样的。确定位置关系的参数i为常数，di为变量。n上述4个参数，就确定了杆件的结构形态和相邻杆件相对位置关系，在转动关节中，Li, i, di是固定值，i是变量。在移动关节中，Li, i, i是固定值， di 是变量。n 对于每个杆件都可以在关节轴处建立一个正规的笛卡儿坐对于每个杆件都可以在关节轴处建立一个正规的笛卡儿坐标系（标系（xi, yi, zi），（），（i=1, 2, , n），），n是自由度数，再加是自由度数，再加上基座坐标系，一共有（上基座坐标系，一共有（n+1）个坐

9、标系。）个坐标系。n 基座坐标系基座坐标系 定义为定义为0号坐标系（号坐标系（x0, y0, z0）,它也是机它也是机器人的惯性坐标系，器人的惯性坐标系，0号坐标系在基座上的位置和方向可号坐标系在基座上的位置和方向可任选，任选，但但 轴线必须与关节轴线必须与关节1的轴线重合，位置和方向可的轴线重合，位置和方向可任选任选；n 最后一个坐标系（最后一个坐标系（n关节），可以设在手的任意部位，关节），可以设在手的任意部位，但但必须保证必须保证 与与 垂直垂直。o oO Oo on n- -1 1nX3.3 机器人关节坐标系的建立n机器人关节坐标系的建立主要是为了描述机器人各杆件和终端之间的相对运动。

10、nD-H方法中的坐标系建立原则如下： D-H关节坐标系建立原则u右手坐标系右手坐标系u原点原点Oi：设在：设在Li与与Ai+1轴线的交点上轴线的交点上 uZi轴：轴： 与与Ai+1关节轴重合，指向任意关节轴重合，指向任意 uXi轴：轴： 与公法线与公法线Li重合，指向沿重合，指向沿Li由由Ai轴线指向轴线指向Ai+1轴线轴线 uYi轴：轴： 按右手定则按右手定则 关节坐标系的建立原则AiAi+1iilid1iliAi-11iz1ix1iy1ioizixiyion原点Oi：设在Li与Ai+1轴线的交点上 nZi轴：与Ai+1关节轴重合，指向任意 nXi轴：与公法线Li重合，指向沿Li由Ai轴线指

11、向Ai+1轴线 nYi轴：按右手定则 沿 xi 轴， zi-1 轴与 xi 轴交点到 0i 的距离 绕 xi 轴，由 zi-1 转向zi 沿 zi-1 轴，zi-1 轴和 xi 交点至0i 1 坐标系原点的距离 绕 zi-1 轴，由 xi-1转向 xi 两种特殊情况n两轴相交，怎么建立坐标系？0iAi与Ai+1关节轴线的交点；ZiAi+1轴线；XiZi和Zi-1构成的平面的法线 ；Yi右手定则； i-1ii-1i AiA Ai i+ +1 1o oi iz zi i- -1 1z zi ix xi iy yi in两轴平行，怎么建立坐标系(Ai与Ai+1平行)？先建立 0i-1然后建立0i+1

12、最后建立 0ii-1i-1O OD D注意：注意： 由于由于Ai和和Ai+1平行，所以公法线位置任意平行，所以公法线位置任意 目的：使目的：使di+1=0 使计算简便使计算简便，此时，此时di= 相邻关节坐标系间的齐次变换过程 机器人运动学正解1.将xi-1轴绕 zi-1 轴转 i 角度，将其与xi轴平行；2.沿 zi-1轴平移距离 di ，使 xi-1 轴与 xi 轴重合；3.沿 xi 轴平移距离 Li，使两坐标系原点及x轴重合；4.绕 xi 轴转 i 角度，两坐标系完全重合AiAi+1iilid1iliAi-11iz1ix1iy1ioizixiy io111A(,)(,)( , ) ( ,

13、)iiiiransiiransiiiiR ZTZd Tx l R x右乘 根据上述坐标系建立原则，用下列旋转和位移我们根据上述坐标系建立原则，用下列旋转和位移我们可以建立相邻的可以建立相邻的 Oi-1 和和 Oi 坐标系之间的关系坐标系之间的关系 机器人的运动学正解方程001112iiiTAAA D-H变换矩阵变换矩阵iiA1100010000100001id1000010000cossin00sincosiiii100001000010001il10000cossin00sincos00001iiii1000cossin0sincossincoscossincossinsinsincosco

14、siiiiiiiiiiiiiiiiidll=例题：二连杆机械臂运动分析1l2l各变量111 1111 110000100001csc lscs lAil1l2l222 2222 220000100001csc lscs lA3.9 PUMA 560图 机器人的连杆坐标系( )a 结构图( )b 坐标图n根据齐次变换方法和表3.1所示连杆参数，可求得各连杆变换矩阵如下： 100001000000111110csscT100000100002222221csdscT100001000003323332csascT100000100044434443csdascT n各连杆变换矩阵相乘，得PUMA

15、560的机械手变换的T 矩阵： 即为关节变量 的函数。 该矩阵描述了末端连杆坐标系6相对基坐标系0的位姿。100000010000555554csscT100000010000666665csscT)()()()()()(66555444333222111060TTTTTTT 621,(3.59)n于是，可求得机械手的T 变换矩阵：n 其中nx, ny, nz, ox, oy, oz, ax, ay, az, px, py, pz见式（3.64）1000611060zzzzyyyyxxxxpaonpaonpaonTTT（3.64）1234 5 64 623 5 614 5 64 61234 5

16、 64 623 5 614 5 64 6234 5 64 623 5 61234 5 64 623 5 614 64 5 61234 5 64 623 5()(),()(),();()(),()xyzxync cc c cs ss s cs s c cc sns cc c cs ss s cc s c cc snsc c cs sc s coc cc c ss cs s ss c cs c sos cc c ss cs s s 614 64 5 6234 5 64 623 5 6123 4 523 51 4 5123 4 523 51 4 523 4 523 51223 234 232 112

17、23 234 232 13 232 242(),(),(),(),;,zxyzxyzc c cs c cosc c ss cc s sac c c ss cs s sas c c ss cc s sas c sc cpc a ca cd sd sps a ca cd sd cpa sa sd c 3.: 已知关节角度或位移，计算末端操作手的对应位姿.: 已知末端操作手的位姿，求解对应的关节变量.可能存在多解或无解通常需多次求解非线性超越方程3.6 运动学逆问题 解的存在性n目标点应位于工作空间内n可能存在多解，如何选择最合适的解？存在双解存在双解! 求解方法n如果各关节可用某算法获得，一个机械

18、手是有解的. 算法应包含所有可能解.封闭形式解（解析解）封闭形式解（解析解）数值解数值解n方法方法n我们对封闭形式的解法更感兴趣我们对封闭形式的解法更感兴趣 代数方法代数方法 几何方法几何方法n 可解性的重要结论是：可解性的重要结论是： 所有具有转动和移动关节的系统，在一个单一串联中所有具有转动和移动关节的系统，在一个单一串联中总共有总共有6 6个（或小于个（或小于6 6个）自由度时，是可解的，其通个）自由度时，是可解的，其通解一般是数值解，它不是解析表达式，而是利用数值解一般是数值解，它不是解析表达式，而是利用数值迭代原理求解，它的计算量要比解析解大。迭代原理求解，它的计算量要比解析解大。

19、但在某些特殊情况下，如若干个关节轴线相交和或多但在某些特殊情况下，如若干个关节轴线相交和或多个关节轴线等于个关节轴线等于 0 0 或或 9090的情况下，具有的情况下，具有6 6个自由度个自由度的机器人可得到解析解。的机器人可得到解析解。 为使机器人有解析解，一般设计时，使工业机器人足为使机器人有解析解，一般设计时，使工业机器人足够简单，尽量满足这些特殊条件。够简单，尽量满足这些特殊条件。n 对于给定的机器人，能否求得它的运动学逆解的解析式对于给定的机器人，能否求得它的运动学逆解的解析式（也叫封闭解）。（也叫封闭解）。0140i000222018040i100040i 2 2选择一个与前一采样

20、时间最接近的解，例如：选择一个与前一采样时间最接近的解，例如：0140i000222018040i 若该关节运动空间为若该关节运动空间为 ，且，且 ，则应选，则应选 25001160i0220i3 3根据避障要求，选择合适的解根据避障要求，选择合适的解4 4逐级剔除多余解逐级剔除多余解 对于具有对于具有n n个关节的机器人，其全部解将构成树形结构。个关节的机器人，其全部解将构成树形结构。为简化起见，应逐级剔除多余解。这样可以避免在树形解中为简化起见，应逐级剔除多余解。这样可以避免在树形解中选择合适的解。选择合适的解。 n迭代法计算量大n几何法适用于自由度较少的情况n反变换法 几何解法（适用于自

21、由度较少时）例：二自由度机械臂，已知(x,y)坐标求关节转角原则: 将原始空间几何问题转化为若干个平面几何问题应用 “余弦定理”:x2+y2=l12+l22 2l1l2cos(1802)则有:22221221 2cos2xylll l再次利用余弦定理得到: cos = (x2+y2+l12 - l22 )/2l1 (x2+y2) 在 0 180范围内求解，最后利用 1=转换为多项式n二、二、 逆向运动学解析法逆向运动学解析法（代数法）（代数法）n n 1000zzzzyyyyxxxxpaonpaonpaonA654321AAAAAA00011uvwxxyyAzz已知数据已知数据待求的各变量待求

22、的各变量公式A数据An用未知的逆变换逐次左乘，由乘得的矩阵方程的元素决定未知数，即用逆变换把一个未知数由矩阵方程的右边移到左边n考察方程式左、右两端对应元素相等，以产生一个有效方程式，理论上可得到12个方程。n然后求这个三角函数方程式，以求解未知数 n把下一个未知数移到左边n重复上述过程，直到解出所有解缺点：无法由数种可能的解中直接得出合适的解， 需要通过人为的选择Paul 等人提出的方法(1981年，也叫求逆的方法，是解析解): Paul 等人提出的方法00123456123456AA AA AA A 655443322160-110AAAAATA）（1 6554433260-110-121

23、AAAATAA）（）（26560-110121132143154)()()()(ATAAAAA-）（5 ETAA60-110-165) ( ）（6 060001nxsxaxpxnysyaypyAnzszazpz例：二自由度机械臂逆向运动学n不论关节变量有多少，最多只能得到12个有效等式。1l2l120001xxxxyyyyzzzznoapnoapAA Anoap111 1111 110000100001csc lscs lA222 2222 220000100001csc lscs lA111222 23311222 200001000010001xyXxyzc ps plcsc lRs pc

24、 pscs lp111111100000100001cslscA222221200000100001cslscA111222 211222 211111220000000100010000100010001xxxxyyyyzzzzcslnoapcsc lscnoapscs lA AA A AAnoapn左右平方相加1112 2112 2(1)xyxyc ps plc ls pc ps l222111112()()xyxyc ps pls pc pl n求解得1n代入（1）求22221112211111121211200000000100010000100010001xxxxyyyyzzzzcs

25、lcslnoapscscnoapA A AA A A AEnoap121 21 2122121 212121 21 21121010000010000100010000100010001xxxxyyyyzzzzc cs sc ss cll cnoapc ss cc cs sl snoapA A Anoap例2： PUMA 560求解可把PUMA 560的运动方程（3.64）写为：若末端连杆的位姿已经给定，即 为已知，则求关节变量 的值称为运动反解运动反解。用未知的连杆逆变换左乘方程(3.65)两边，把关节变量分离出来，从而求得 的解。 )()()()()()(10006655544433322

26、2111060TTTTTTpaonpaonpaonTzzzzyyyyxxxx(3.65)pao,n,和621,621,n1.求 用逆变换 左乘式(3.65)两边：1)()()()()()(100066555444333222111060TTTTTTpaonpaonpaonTzzzzyyyyxxxx(3.65)010123451162233445566()()()()()TTTTTTT0111T1111111111111611110 00 0001 00001000 10001xxxxxxxxyyyyyyyyzzzzzzzzcsnoapnoapscnoapnoapTnoapnoapn1.求 10

27、10123451162233445566()()()()()TTTTTTT1111111111111611110 00 0001 00001000 10001xxxxxxxxyyyyyyyyzzzzzzzzcsnoapnoapscnoapnoapTnoapnoap1112xyys pc ppd利用三角代换：cos ;sinxypp其中22;atan2,xyyxpppp(3.67)两边(2,4)项元素对应相等：(3.68)n求 式中，正、负号对应于 的两个可能解。 212122221222122sin()/;cos()1(/)atan2,1atan2(,)atan2(,)yxxyddddppdp

28、pd 1(3.70)11112xyys pc ppdcos ;sinxypp22;atan2,xyyxppppn2. 求 31111111111111611110 00 0001 00001000 10001xxxxxxxxyyyyyyyyzzzzzzzzcsnoapnoapscnoapnoapTnoapnoap(3.67)1113 234 232213 234232 2xyxzzc ps ppa cd sa cppa sd ca s 两边(1,4)项和(3,4)项元素对应相等：3 34 3a cd sk其中2222222232422xyzpppaaddkan求 1),(2atan),(2at

29、an22423433kdakda3(3.73)正、负号对应 的两种可能解。3 34 3a cd sk1112222122atan2(,)atan2(,)xyyyxxys pc ppdppdppdn求 3 式中，2222222232422xyzpppaaddkan3.求 201034531236445566,()()()TTTTT 1 231 23232 31 231 23232 336112000010001xxxxyyyyzzzzc cs csa cnoapc ss sca snoapTnoapscd1 231 23232 331 231 23232 34xyzxyzc c ps c ps

30、pa cac s ps s pc pa sd(3.75)两边(1,4)项和(2,4)项元素对应相等：n求根据 解的四种可能组合可以得到相应的四种可能值 ，于是可得到 的四种可能解：式中， 取与 相对应的值。231和2323232(3.78)2332 3112 3423221142 3112 33232211232332 3112 3442 3112 33atan2,zxyzxyzxyzxyzxyzxyaa cpc ps pa sdspc ps pda cpc ps pa cacpc ps paa cpc ps pa sdda cpc ps pa ca(3.77)n4.求 40103453123

31、6445566,()()()TTTTT 1 231 23232 31 231 23232 336112000010001xxxxyyyyzzzzc cs csa cnoapc ss sca snoapTnoapscd1 231 23234 5114 5xyzxyc c as c as ac ss ac as s (3.75)两边(1,3)项和(3,3)项元素对应相等：4111 231 2323atan2(,)xyxyza sa ca c ca s ca s(3.79)n5.求 5010454123465566,()()TTTT 1 23 41 41 23 41 423 42 3 42 43 4

32、1 23 41 41 23 41 423 42 3 4243 401412341 231 23232 34,0001c c cs ss c cc ss ca c cd sa cc c ss cs c sc cs sa c sd ca sTc ss sca sd 1 23 41 41 23 41 423 451 231 23235xyzxyzac c cs sas c cc sas csac sas sacc 两边(1,3)项和(3,3)项元素对应相等：555atan2( ,)s c(3.84)n6.求 60105512345666,()TTT 1 23 41 41 23 41 423 461

33、23 41 451 23 51 23 41 451 23 523 4 523 56xyzxyznc c ss cns c sc cns ssnc c cs scc s sns c cc scs s sns c cc sc两边(3,1)项和(1,1)项元素对应相等：666atan2(,)s c(3.84)nPUMA560的运动反解可能存在8种解。但是，由于结构的限制，例如各关节变量不能在全部360范围内运动，有些解不能实现。n在机器人存在多种解的情况下，应选取其中最满意的一组解，以满足机器人的工作要求。Four solutions of the PUMA560n代数法总结：代数法总结：n（1）左

34、乘逆阵，列方程，左右对应元素相等，）左乘逆阵，列方程，左右对应元素相等，可解方程，依次递推。可解方程，依次递推。n（2）递推一次，可解一个或多个变量。不需）递推一次，可解一个或多个变量。不需全推，方程可能已经全部解出。全推，方程可能已经全部解出。n（3）由实际判断伪根）由实际判断伪根n3.7 机器人的速度分析机器人的速度分析n前面位移问题前面位移问题n现在速度问题现在速度问题n末端夹持器速度：末端夹持器速度：n线速度线速度 Vx,Vy,Vz（相对固定坐标系度量）（相对固定坐标系度量）.n姿态速度姿态速度夹持器绕三个固定坐标系轴角速度夹持器绕三个固定坐标系轴角速度n x, y, z.n一、速度分

35、析（正向运动学）一、速度分析（正向运动学）n广义坐标：广义坐标： n 移动或转动移动或转动n位置：位置： Px , Py ,Pz .n姿态：可用方向余弦。或用绕三个固定坐标轴姿态：可用方向余弦。或用绕三个固定坐标轴角度度量表示角度度量表示,PPPnqqq2, 1n用广义坐标表示机器人末端夹持器运动方程用广义坐标表示机器人末端夹持器运动方程:)(2, 1nxxqqqPP)(2, 1nyyqqqPP)(2, 1nzzqqqPP)(2, 1nqqqPP)(2, 1nqqqPP)(2, 1nqqqPPn广义坐标矩阵通式：广义坐标矩阵通式：n 1PPx4PP 2PPy3PPz6PP 5PP 654321

36、PPPPPPPnP- 操作空间。操作空间。 nq- 关节空间。关节空间。n正向运动学正向运动学n逆向运动学逆向运动学n )(2, 1nqqqPPP(P1,P2,P3,P4,P5,P6) )6, 5, 4, 3, 2, 1(qqqqqqqP(P1,P2,P3,P4,P5,P6) )6, 5, 4, 3, 2, 1(qqqqqqq答案唯一多解性 n求导求导: 平移速度平移速度1112112nxnP dqP dqP dqVq dtqdtqdt2122212nynP dqP dqP dqVqdtqdtqdt3132312nznP dqP dqP dqVq dtqdtqdtn求导求导: 旋转速度旋转速度

37、dtdqqPdtdqqPx224114dtdqqPdtdqqPy225115dtdqqPdtdqqPz22611611211133313461656/xnyznxynzndpdtdqdtdpVpqpqdtVdpdqVdpdtdtpqpqdpdtdtpqpqdpdtdqdtdpdt dtdqqpdtdp广义坐标速度矩阵通式：广义坐标速度矩阵通式：n广义坐标导数矩阵广义坐标导数矩阵:n雅可比矩阵雅可比矩阵 111616/nnpqpqpJqpqpq例题例题 二自由度平面关节机器人，手部沿固定坐标二自由度平面关节机器人，手部沿固定坐标系系X0轴正方向移动速度轴正方向移动速度1m/s。两杆长。两杆长0.5m，求瞬，求瞬时关节速度？时关节速度？301602解：)cos(cos21211llX)(insin21211sllY1212( ,),( ,)XXYY 2211dYdYdY2211dXdXdX212121ddYYXXdYdXJdqdP 1221221112212211clclclslslslJ现在已知末端速度求关节速度：n需先求1