机器人学导论第4章操作臂逆运动学

1、第4章操作臂逆运动学4.1概述4.2可解性4.3当6时操作臂子空间的描述4.4代数解法与几何解法4.5通过化简为多项式的代数解法4.6三轴相交的PIEPER解法4.7操作臂逆运动学实例4.8标准坐标系4.9操作臂求解4.10重复精度和定位精度4.11计算问题4.1概述在上一章中讨论了已知操作臂的关节角，计算工具 坐标系相对于用户工作台坐标系的位置和姿态的问 题。在本章中，将研究难度更大的运动学逆问题:已 知工具坐标系相对于工作台坐标系的期望位置和姿 态，如何计算一系列满足期望要求的关节角？第3章重点讨论操作臂的运动学正问题，而本章重点 讨论操作臂的运动学逆问题。图3J6不同运动学描述之间的映射

2、4.2可解性求解操作臂运动学方程是一个非线性问题。己知册的数值，试图求出 。对于PUMA560操作臂来说，目前对这个问题的确切表述如下：已 知暂的16个数值(其中4个无意义)，求解式(3.14)的6个关节理鬲。超越方程：当一元方程f(x)=O的左端函数f(x)不是x的多项式时，称之为超越方 程。如指数方程，对数方程，三角方程和反三角方程等。sinx+x=O 对于具有6个自由度的操作臂来说，有12个方程，其中6个是未知的。而且在由 汀的旋转矩阵分量生成的9个方程中，只有3个是独立的。将这3个方程与由暂 的位置矢量分量生成的3个方程相加，总共给出的6个方程，其中含有6个未知 量。这些方程为非线性超

3、越方程，很难求解。方程组(3.14所描述的机器人 的连杆参数很简单，多个他是0或±90度，许多连杆偏距和长度分别为0。显而 易见，一般6自由度机构(所有连杆参数均不为0)的运动方程要比(3.14)复 杂得多。同任何非线性方程组一样，我们必须考虑其解的存在性、多重性以及 亲解方法。(1)解的存在性；(2)多重性；(3)求解方法兀1兀1 為 兀2工22兀2珀3兀3。1。23(C4C5C6 S4S5) 82385(5+ slsqGQ$1。23 G4C5C6 SqSe) S23S5C6 - C1G4C5C6 + 4*6) C4C5C6 S4S6) 一 C23S5C6C1 23 (_ C4C5

4、S6 Sqq) + S23S5S6 + S(qC6 S4C5S6) $1。23( C4C5S6 Sq) + *23*5*6 _ 1(46 S) S2s(- C1C5S6 一 了点)+ 23S5S6-Q1C23C4S5 + S23C5) - S1S4S5_+$23。)+ C1S4S5_S23兀3PXPyS23C4S5 C23C5(色笑 + “303 4*23 _ dgS、 + &3C23 di% + dgC、a2C2 + a3C23图4"连杆长度为厶和£的两连杆操作臂解的存在性解是否存在的问题完全取决于操作臂的工作空间。简单地说，工作空间是操 作臂末端执行器所能到达的

5、范围。若解存在，则被指定的目标点必须在工作 空间内。灵巧工作空间：指机器人的末端执行器能够从各个方向到达的空间区域。也 就是说，机器人末端执行器可以从任意方向到达灵巧工作空间的每一个点。可达工作空间：机器人至少从一个方向上有一个方位可以达到的空间。显然, 灵巧工作空间是可达工作空间的子集。当一个操作臂少于6自由度时，它在三维空间内不能达到全部位姿。显然，图牛1中的平 面操作臂不能伸出平面，因此凡是Z坐标不为0的目标点均不可达。在很多实际情况中， 具有四个或五个自由度的操作臂能够超出平面操作，但显然不能达到全部目标点。必须 研究这种操作臂以便弄清楚它的工作空间。通常这种机器人的工作空间是一个子空

6、间， 这个空间是由特定的机器人的工作空间确定的。一个值得研究的问题是，对于少于6个 自由度的操作臂来说，给定一个确定的一般目标坐标系，什么是最近的可达目标坐标系?工作空间也取决于工具坐标系的变换，因为所讨论的工具端点一般就是我们所说的可达 空间点。一般来说，工具坐标系的变换与操作臂的运动学和逆向运动学无关，所以一般 常去研究腕部坐标系W的工作空间。对于一个给定的末端执行器，定义工具坐标系 T,给定目标坐标系G,去计算相应的坐标系W。接着我们会问:W的期望位姿 是否在这个工作空间内？这里，我们所研究的工作空间（从计算的角度出发）与用户关 心的工作空间是有区别的，用户关心的是末端执行器的工作空间（

7、T坐标系）。如果腕部坐标系的期望位姿在这个工作空间内，那么至少存在一个解。图4"连杆长度为厶和£的两连杆操作臂多重解问题在求解运动学方程时可能遇到的另一个问题就是多重解问题。一个具有3个旋转关节的 平面操作臂，由于从任何方位均可到达工作空间内的任何位置，因此在平面中有较大的 灵巧工作空间（给定适当的连杆长度和大的关节运动范围）。图42所示为在某一位姿 下带有末端执行器的三连杆平面操作臂。虚线表示第二个可能的位形，在这个位形下， 末端执行器的可达位姿与第一个位形相同。图4-2三连杆操作臂，虚线代表第二个解图43到达B点有两个解，其中 一个解会引起干涉因为系统最终只能选择一个解

8、，因此操作臂的多重解现象会产生一些问题。解的选择标 准是变化的，然而比较合理的选择应当是取“最短行程”解。例如，在图43中，如果 操作臂处于A点，我们希望它移动到点B,最近解就是使得每一个运动关节的移动量最 小。因此，在没有障碍的情况下，可选择图43中上部虚线所示的位形，这表明对于操 作臂的当前位置来说只需要对逆运动学程序输入一个小位移量即可。这样利用算法能够 选择关节空间内的最短行程解。解的个数取决于操作臂的关节数量，它也是连杆参数和关节运动范围的函数。例如， PUMA560机器人到达一个确定的目标有8个不同的解。图44所示为其中的4个解，它们 对于手部来说具有相同的位姿。对于图中所示的每一

9、个解，存在另外一种解，其中最后 三个关节变为另外一种位形，如下式所示：fl.解的个数fij = 45 0毛40« 16所有的绚* 00 16通常，连杆的非零参数越多，达到某一 特定目标的方式也越多。以一个具有6 个旋转关节的操作臂为例，图45表明解 的最大数目与等于零的连杆长度参数的 数目有关。非零参数越多，解的最大数 目就越大。对于一个全部为旋转关节的 6自由度操作臂来说，可能多达16种解。解法与线性方程组不同，非线性方程组没有通用的求解算法。我们把操作臂的全部求解方法分成两大类：封闭解和数值解法。由于数值解 法的迭代性质，因此它一般要比相应的封闭解法的求解速度慢很多。实际上在大多

10、数情况下，我们并不喜欢用数值解法求解运动学问题。因为封闭解的计算速度快,效率高，便于实时控制。而数值法不具有些特点为。“封闭形式”意指基于解析形式的解法，或者意指对于不高于四次的多项式 不用迭代便可完全求解。可将封闭解的求解方法分为两类：代数法和几何法。 有时它们的区别又并不明显：任何几何方法中都引入了代数描述，因此这两 种方法是相似的。这两种方法的区别或许仅是求解过程的不同。牛顿法基本思想：将非线性方程线性化，以线性方程的解逼近非线性方程的解。牛顿迭代法的几何解释：方程/（劝=0的根兀在几何上是曲线y = /（丸）与兀轴的交 点的横坐标。若Xk是根疋的一个近似,过曲线上横坐标为Xk 的点以作

11、曲线V = /（兀）的切线，则该切线与兀轴交点的横坐 标即为无+1。4.3当nv6时操作臂子空间的描述对于一个给定的操作臂来说，一系列可达目标坐标系构成了可达工作空间。对于一个n 自由度操作臂（刃6）,可达工作空间可看成是n自由度子空间的一部分。同样，一个 6自由度操作臂的工作空间是一个空间的子集，更简单的操作臂的工作空间是这个子空 间的子集。例如，图41中两连杆机器人的子空间是一个平而，其工作空间是该平面的 一个子集，即当h =乙时为一个半径为仃+ D的圆。确定n自由度操作臂子空间的一种方法就是给出腕部坐标系或工具坐标系的表达式，它 是含有n个变量的函数。如果将这n个变量看着自由变量，那么它

12、们所有的可能取值就 构成了这个子空间。例4.1试描述第3章中图所示三连杆操作臂衿T的子空间。0.00.00 00.0wT =0.01.00y0.01例4.2试描述图46所示两自由度极坐标操作臂眾的子空间。0例4.2试描述图46所示两自由度极坐标操作臂眾的子空间。X° P V厂2ORG 一图4-6两连杆极坐标操作臂J宀/-X00°z2 =0为了对具有n个自由度操作臂的目标点进行定义，通常采用n个参数来确定这个目标点。 也就是说，如果给定的目标点有6个自由度，一般自由度料6的操作臂是无法到达这个 冃标点的。在这种情况下，可寻找一个位于操作臂子空间内的可达目标点代替目标点, 并且

13、和原期望目标点尽可能“靠近” o因此，对于少于6个自由度的操作臂来说，当确定一般目标点吋，求解方法如下：（1）已知一般目标坐标系鸟7计算一个修正的目标坐标系使得/T位于操作臂的 子空间内，并且和gT尽可能取靠近” o应预先选择一个“靠近”标准。（2）将$厂作为期望目标，计算逆运动学，求关节角。注意，如果目标点不在操作臂的 工作空间内，将可能没有解。8-11 5自由度焊接机器人的典型腕关节设计首先确定工具坐标系原点到期望目标点的位置，然后选择一个接近期望姿态的可达姿 态。正如我们在例4.1和4.2中所见的，子空间的计算取决于操作臂的几何特征。对每个 操作臂必须单独考虑，从而得到相应的计算方法。为

14、了计算关节角使得操作臂能够到达距期望坐标系最近的可达坐标系，在4.7节中给出 了将一个一般目标点投影到五自由度操作臂子空间的例子。4.4代数解法与几何解法代数解法：以第三章所介绍三连杆平面操作臂为例，其坐标和连杆参数如下图4-7平面三连杆操作臂和它的连杆参数按第三章的方法，应用这些连杆参数可以求得这个机械臂的运动学方程:'123$1230wT=°J =$1230°123001+ “2*120（4-6）0001为了集中讨论逆运动学问题，我们假设腕部坐标系相对于基坐标系的变换， 即矗T已经完成。这个操作臂通过三个量x,y和（p很容易确定这些目标点。如下 给出的怡丁就确定

15、了目标点的位姿，这个变换矩阵如下。0X3丁 一*%0yW1 -00100001（4 7）令式（4-6）(4-7)相等,可以求得四个非线性方程，进而求出Z, 02和03：= C123=51232C122*125 =X = lxcx + I y = lxsx +1（4-8）（4-9）（4-10）（4-H）s®0C123$1230ZC + 】2。20y$123C1230/S + <2*120010001000010001现在用代数方法求解方程（4®（4-11）。将式（牛6）和（4-7）相等，可以 求得四个非线性方程，进而求出9lf62f和叭：兀2 + y2 =/2 + +

16、21林2（42 f 2 z2 ；2x + y L 解得C2 =!-2 2仏S2的表达式为$2 = 土 J1 _ C；最后利用2幅角反正切公式计算&2,得 & =4tan2（»,cJ式(4-15)是多解的，可选择“正”解或“负”解确定式(4-15)的符号。在确定时， 再次应用求解运动学参数的方法，即常用的先确定期望关节角的正弦和余弦，然后应 用2幅扁反正切公式的方法。这样确保得出所有的解，且所求的角度是在适当的象限里。求出了02,可以根据式(4-10)和(4-11)求出。将式(4J0)和(411)写成如下形式为了求解这种形式的方程，可进行变量代换,实际上就是改变常数他和

17、他的形式。丫 = Atan2 伙2虫） kx -厂cosy k2 = rsin/式（4-17）和（4-18）可以写成：X=cosycosO -sin/sinj r=cos/sin +sin/cos r因此xcos（/+q）=rsin0 + q）=r利用2幅角反正切公式，得/从而卩 + q A tan 2| , = Atan2（y，x） U厂丿&i = A tan 2（y, x） - A tan 2（k2. ）注意如果x二y二0,则是(4-27)不确定，此时81可取任意值。最后，由式(4-8)(4-9)能够求出印，02, %的和:+ = Atan2(S0,C0)= 0(4 16)由于81

18、 , 02已知，从而可以解出03总之，用代数方法求解运动学方程是求解操作臂的基本方 法之一。几何解法在几何方法中，为求岀操作臂的解，须将操作臂的空间几何参 数分解成为平面几何参数。用这种方法在求解操作臂时（特别 是al二0或±90° ）是相当容易的。然后应用平面几何方法可以 求岀关节角度。利用余弦定理求解。2x2+y2 =/j2 +Zf -2/1/2cos(180+6)现在，cos(180+ e2)=-cos(e2),所以有 _x2 -by2 -Zj2 -Zf5=可2讨论：为了保证解存在，目标点 (x,y)应满足；在满足解存在的前提 下，有两个解&2 = ©

19、;(1 80" < g < 0 )z/z.为了求解 需要建立图48所示的屮和卩 角的表达式。首先，卩可以位于任意象 限，这是由x和y的符号决定的。为此， 应用2幅角反正切公式：P Atan2（” 兀）A克再利用余弦定理解出屮:图48平面三连杆机器人的平面几何关系22 J2_J2cos 屮=X + 1 A (0°<<180°)2lJx2 + y2其中当32<0°时取“尸号 当02 >0°时取“一”号FoLyO-O可由0=G + $ + &3解出关节角&345通过化简为多项式的代数解法超越方程往往

20、很难求解，即使只有一个变量，因为它一般常以sin 和cos的形式岀现。可进行下列变换，用单一变量U来表示：cos。=1-M21 + w2sin。=2u1 + w2这是在求解运动学方程中经常用到的一种很重要的几何变换方 法。这个变换是把超越方程变换成关于U的多项式方程。例4.3将超越方程acosO + bsin。 多项式，以求解。6z(1-m2) + 2Z?m = c(l + w2)2(a + c)u 2bu+(c a) = 0b±lb2 +a2 c2u =a + c6> = 2tan_1、a + c=C变换成含有半角正切的一次Xy z1图3J9 PUMA560前肾的运动鑫数和坐

21、标系分布4.6三轴相交的PIEPER解法如前所述，尽管一般具有6个自由度的机器人没有封闭解，但在 某些特殊情况下还是可解的。Pieper研究了3个相邻的轴交于一点 的6自由度操作臂。在本节中，我们将介绍Pieper提出的方法，这 种方法是针对6个关节均为旋转关节、且后面3个轴相交的操作臂。当最后3根轴相交时，连杆坐标系4, 5和6原点均位于这个 交点上。这点的基坐标如下0 p_ Onr np 2p3p_r4ORG = X1 21 y r4ORG =厂1 p11IJII2 V 1"1-H1wM1.jp1& s1w1&07WM1& o& s1£g

22、r厂EPg s1e p g oooog s1& ooog 彳ooE s1o s1&3gso s17gso£Jmw<1& s wo丿gJ&3 Esws Eso-,、g7 £ssso即，对于i=4,由式(3-6)的第4列有即，对于i=4,由式(3-6)的第4列有0 p_Onr rp2rp4ORG _ 卩 21 y或0比ORG斗卩丫f«3)f?>(。3)17i_/fl=tdsoc中=(3d4ca311式中,在式-d4sa3d4sa31昭0-璃C0cy_icQs(Xi_00一叫cai-0ai-i(4-44)中，对于应用式(3-6

23、)得出下列的表达式:(3-6)即，对于i=4,由式(3-6)的第4列有即，对于i=4,由式(3-6)的第4列有f = a3c3 +d4sa3s3 +a2f2 = a3ca2s3 d4sa3ca2c3 d4sa2ca3 d3sa2f3 = a3sa2s3 dAsa3sa2c3 +d4ca2ca3 +d3ca2在式(443)中，对于丁和丫应用式(3-6)得qgiSg2P40RG =$lgl+Clg2g3式中,gl = C2</1 slfl +6Z1g2 = s2caf +c2cai/2 -saf3 -d2sa g3 = s2saifi +c2saf2 +caxf3 -¥d2cax现

24、在写出/妣 平方的表达式，这里r = x2 + y2 + ?,从式(4-46) 可以看出2 ( 2 , 2 厂=gl +&2+&3所以，对于g.由式(4-47)得r = ff+济 +d? +22/3 +2«1(c2/1 s2f2现在，由式(4-46)写出z方向分量的方程，那么表系统的 两个方程如下：r = (ks + k2s2)2ax + k3Z = (kis2-k2c2)sai-k4式中，k、= j*2 = fl龟=f +/；+a +盃 +22/3k4 = f3cax +d2caxr = (kC2 + kqsJZa、+ k3Z = &S2 k2c2)sa +

25、 红(4-50)方程式(4-50)很有用，因为它消去了因变量色，并且简化了因变量§的形式。 现在讨论如何由式(4-50)求解妇 分三种情况：1.若al=0,则r=k3,这里是已知的。K3的右边仅是关于03的函数。代入式 (4-35)后，由包含tan ( 03 /2)的一元二次方程可以解出03。2若sal=0,则z=k4,这里z是已知的。再次代入式(4-35)后，利用上面的一 元二次方程可以解岀03。3否则,从方程(4-50)中消去s2和c2,得到(r-)2 (L 褊 A _ 2 卜 24/+孑 -心+爲解岀B3后，就可以根据式(4-50)解出02,再根据式(4-46)解出01。92

26、s为了完成求解工作，还需要求出04、95x 96o由于这些轴相交，故这些关节 角只影响最后连杆的方位，我们只需计算指定目标的方向煦。在求出81、因此这个问题可以通过如下计算得出结果63后，当04=0时，可以由连杆坐标系4相对于基坐标系的方位计算出 |04 = 0o坐标系6的期望方位与连杆坐标系4的方位的差别仅在于最后 三个关节的作用。由于煦已知，It = lTlT=对于大多数操作臂来说，完全可以将第2章介绍的ZYZ欧拉角解法应用于 t/?|e4 = 0,解岀最后三个关节角。对于任何一个4、5、6轴相交的操作臂来说, 最后三个关节角能够通过一组合适的欧拉角求出。最后的三个关节通常有两 种解，因此

27、这种操作臂的总数就是前三个关节解的数量的2倍。4.7操作臂逆运动学实例在本节中，将研究两个工业机器人的逆运动学问题。一种是仅用 代数方法求操作臂的解，第二种是用部分代数方法和部分几何方 法求操作臂的解。下面的解法并不是适用于解决所有机器人运动 学问题，但是对于大多数通用操作臂来说，这些解法是最常用的。 注意，前面提到的Pieper的解法能够适用于这些操作臂，但在这 里我们选择一种不同的方法来求解，以便对各种有效解法有所了 解。The Unimation PUMA 560机器人将PUMA 560的运动方程写为:210% f Py =2（q）Z（0）Z（a）迅（0）7（仇）5人（仇） 32$3 P

28、z0 0 1若末端连杆的位姿已经给定，即,o,讲叨为已知，则 求关节变量q,&2,厲的值称为运动反解.1. 求 ex但)x = zG) T(a)Z(©) Z偲)Z(4)q 60 0"rH r!2 G_SCj00G hl 金 Py0 0 10勺勺金 pz_ 0 0 0 1_0 0 0 1 _=sin(0q)= d3 / p;(4-55)(牛 56)cos" e)= 土 Ji-a / pyp Vp )(p_0 - atan2 =atan2(py,pj atan2(d3,土式中，正、负号对应于仇的两个可能解.治XPx111r22723Py雳2if33lPz001

29、，z21 雳1 00o-112“3px00rTlr23Py10r32r33化01_0001 _元素(1,4)和(3,4)分别对应相等C Px + XPy 。3°23 4*23 +p = 3*23 + 4Co3 +2乡°23C4C5C6 S4S6. S23S6S4S6C23S5C5+ S 23S 5S 5ril-21 = S4C5C6 46_ ly, lj22 = S4C5S6 C4C&一日3S23 - <32S2 - 4。231-/32 = S23 C4C56 + 耳幺+ C23S5S6乜3 = -。压-S23C5匕 3 = S4S5-33=S23C4S5C2

30、3C5Px = a2C2 + a3C23 4S23XPy = £'Pz2. 求 e3再令矩阵方程(464)两端的元素(1，4)和4)分别对应相等， 则得两方程：(4-65)C Px + SPy =。3°23 _ “4*23 + PZ = a3S23 + 如23 + a2S2 d3=-SPXCPy式(465)与式(4-57)的平方和为:&3 =+ dj-K2)(牛 68)式中，正、负号对应于&3的两个可能解.(4-69)(牛 70)CC23SC 23$23a2C3“213 P-C*23®$23C23a2S3223 PyC0切r32金 pz00

31、01_000 1 _3. 求 02红何,&2屁) = Z(&4)Z(d)Z(&6)令矩阵方程(4-70)两端的元素(1,4)和(2,4)分别对应相等, 则得两方程：ClC23x + SlC23Py _ S2PZ _ «2C3 = 3C S2Px _ SS2Py C2Pz +°2*3 = 4lT =C4C5C6 一S4S6CAC5SQ一 ss一 C4S5Sr-C_ sSgCv泗A56565-S4C5C6-叽S4C5S6 "-gS4S5I000C1C23SC 23$23a2C3_rllrn 心pxC$23'1*23_C23a2S3丫 2

32、223Py_耳q0_£r32r33P._ 0001 _00 01 _、%0b元素(1,4)和(2,4)分别对应相等CC2Px + SC23Py S2Pz °2°3 = °3 CS2Px SlS2Py C2Pz +。2$3 = 4元素(1,3)和(3,3)分别对应相等人3°1°23 + 23123 匕3$23 = C4S5131 231-3s1+r23c1=s4s5CC2Px + SC2?>Py _ S2Pz _ a2C3 = c S23 Px SxS21>Py C23 Pz +°2$3 = 4联立上述两个方程可以

33、解出s23和c23,结果为=（勺勺。3 ）代 + Cl 代 + 几卩 J （。2$3 dj2® 一 4 ）代 一 （。3 + s） （5 Px + ® Py ）C23 色=Q tan 2(他ei2C3) pz +(5久 +J (。2$3 )，(a2s3-d4)pz-(a3 + a2c3) (q px + 亠 pj&2 二 &23 _。3根据q和解的四种可能组合可以 得到相应的四种可能值&23,于是可 得到。2的四种可能解.令式(4-70炳端的元素(1,3)和(3,3)分别对应相等，则得两方程：人3°1°23 十 r23SlC23

34、$3*23 = C4S5>一人3耳+却=邛5当$5工0641 = atan2(耳3$1 + r23Clr3CC2?> r23SC23 + 勺3$23)642 = atan2(3$i r23Cl ri3ClC23 + r23SlC23 3323)当S5二0时,机械手处于奇异形位此时，关节轴4和6重合，只能解出84 和06的和或差.奇异形位可以由式(3.79)中的atan2的两个变量是否 接近零来判别若都接近零，则为奇异形位，否则，不是奇异形位.在奇 异形位时，可任意选取B4值,再计算相应的86值.5求35丁何施雄&川勺二乂阎贺刎二览偲，) (4-77) 令矩阵方程(4-77)

35、两端的元素(1,3)和(3,3)分别对应相等, 则得两方程：斤3 (CC23C4 +) + 勺3 (51C23C4 CS4 ) _ 33 ($23°4 ) _55“3 (_C1523)+Z23 (一邛23 ) + $3 (一°23 ) = G05 =atan2(55,c5)( °、qq s5 °It = ItIt=(GS&00习“ S5S6 C5 °< 0001丿X 何,02雄厲)X = Z(Q)%(Q)=览偲,4)元素(1,3)和(3,3)分别对应相等斤3 ( C1C23C4 + 护4 ) + 勺3 (51C23C4 Cl54)

36、 33 (523C4 ) = S5斤 3 (-加23 ) + % (一护23 ) + $3 (_C23)= C56.求 &6% (&,2，&3，&4禺)汛=%(Q)(4$1)令矩阵方程(4-77)两端的元素(3,1)和(1,1)分别对应相等2 =atan2(56,c6)式中S6 = rU (C1C2354 _ SlC4 ) _ 21 (51C2354 +C1C4)+ /31 ($23$4)C6 = r I |(C1C23C4 +) C5 吋23$5 + 石|(51C23C4 _ C14)C5 _ 51*235531 (23C4C5 + C23S5 )由于在式（4

37、-64）和（4-68）中出现了 土号，因此这些方程可能 有4种解。另外，由于作臂腕关节“翻转”可得到另外4个解。对 于以上计算出的4种解，由腕关节的"翻转”可得到& 二&4+180°二 -&5& 二&§+180°严计算出所有8种答案以后，由于关节运动范围的限制要将其中 專最詈雷1 蛊勰省鑿。在余下的有效解中，通常选取-解的多重性图44 PUMA560的4个解PUMA560的运动反解可能存在8种解.但是，由于结构的 限制侧如各关节变量不能在全部360度范围内运动，有些 解不能实现.在机器人存在多种解的情况下，应选取其

38、中 最满意的一组解,以满足机器人的工作要求.Yasukawa Motoman L3型机器人由第3章中第二个例子，这里将要求解Yasukawa Motman L-3型机 器人的运动学方程。这个解将由部分代数解以及部分几何解组成。 Motman L-3有三个特征使它的逆运动学特性不同于PUMA机器人。如果考虑Motoman操作臂子空间的特性，可以很快看到这个子空 间可以通过一个可达方位的约束来描述。工具端的指向（Zt轴） 必须位于“操作臂的平面内”，该面是一个包含关节1的轴线以 及轴4和轴5交点的垂直面。以最小旋转量改变工具端的指向从而 获得离一般方位最近的方位，该方位位于上述平面内。不需要给 出

39、这个子空间的显函数方程，而只需要建立一种将一般目标坐标 系投影到这个子空间的方法。注意，对于这种情况的讨论仅是由 于腕部坐标系和工具坐标系沿Zw的平移不同。图49中所示为操作臂平面的法向M以及工具端的期望指向Zt。为了在 这个平面内产生一个新的指向Z, 该指向必须绕某一矢量K旋转8角。显然，使6最小的K位于这个平面内，同时Zt与Z”和正交。对于任何给定的目标坐标系，M的 定义如下将目标型标系旋转到Motoman操作臂的了空间K=MxZt/ t / /Zt=KxM图49将目标坐标系旋转到Motoman操作臂的了空间的旋转量为cos 3 = ZT Zfsin = zt xZ't) R 应用

40、Rodrique公式，有/g/八 /、/ 八八 八Yt =詔耳+詔(Kx£)+ (l_cO)(K YtJKXT =Yrx Zr假定已知腕部坐标系在操作臂的子空间内，我们按照下述方法求 解运动学方程。在求解Motoman L3运动学方程时，可以构造连 杆变换矩阵的乘积：丁 =7了；巧卩了11“2厂 13P：如果令題=乙py切 % 金代0 0 0 1 _式(4-90)两边同时前乘It-1,得到莎巴7羽7V訴勿式中左边为C/11+S1 厂215厂 12+屮22的3+$13Px+SlPy-r3r32r33PzSpll+C厂21屮 12+*22$1 厂 13+5厂23SPxCPy0001右边为

41、*5 234*_C234*亍5C5000001式(4-94)中一些元素并没有给出。令方程两边的元素sPx+cPy =0（4-93）（3,4）相等，得（4-95）（4-96）（4-97）（4-98）（4-99）由此求得&i = Atan2(p尸久)令元素(3,1)和(3,2)相等，得s5 = srn +5 厂21C5 = Sr2 +5 厂22由上式计算尙得 = Atan2(Ci 住山)令元素(2,3)和(1,3)相等，得C234 = %5234 = Clrl3 +$1 厂23由此得图324 Yasukawa驱动2S和连杆3的运动示童图图4J0平面内的Motoman操作臂为了分别解出色，念和為，可应用几何方法。图4/0所示为操作臂平面内关 节轴2处的点A、关节轴3处的点B和关节轴4处的点C。对于三角形ABC应用余 弦定理，有COS%（4-101）（4-102）则有.&3 = Atan2（Jl-cos2 03，cos%）从图4-10rJ以看出。2 =卩，即从图4-10rJ以看出。2 =卩，即92 = -Atan2(p_.,p； +