1、第八章第八章 空间单链连杆机构的运动分析空间单链连杆机构的运动分析 若连杆机构中各个构件不都在同一平面内或平行平面内运动，则称该机构为空空间连杆机构间连杆机构。组成空间连杆机构的运动副，不仅有转动副R和移动副P，还常有圆柱副C、螺旋副H、球面副S和虎克铰U 等，在科学研究和生产实际中，空间连杆机构常以机构中所含各运动副的代表符号来命名，如RSSR机构、RSUR机构、RCCR机构、3-RPS机构等。若按运动链的数量来区分，空间连杆机构可分为单链式和多链式。不过，更常见的区分方式是运动链是否封闭，即开链型和闭链型两种。前者主要用于机械手和机器人中；后者又有单环和多环之分，单环机构主要在轻工机械、农
2、业机械、航空运输机械、汽车和各种仪表中已得到较多的应用；而空间多环机构构成了并联机构的主要机型，在运动模拟、精密操纵、多轴加工等众多领域应用也越来越广。本章主要介绍单链式空间连杆机构的运动分析方法。与平面连杆机构相比，空间连杆机构结构紧凑，运动可靠、灵活。许多用平面连杆机构根本无法实现的运动规律和空间轨迹曲线，可以通过空间连杆机构来实现。当然，空间连杆机构的分析和设计要比平面连杆机构复杂得多，但随着科学技术的发展和计算机的普遍使用，已建立了空间连杆机构分析和综合的理论基础，尤其是计算机辅助分析和综合的解析法已成为研究空间连杆机构的主要方法。本章给出几种典型空间连杆机构的运动分析方法，详细给出坐
3、标系的选取方法和位置、速度和加速度的求解过程。第八章第八章 空间单链连杆机构的运动分析空间单链连杆机构的运动分析8.1、RCCC机构运动分析机构运动分析8.2、串联机器人机构的位置分析、串联机器人机构的位置分析 8.3、串联机器人的雅可比矩阵、串联机器人的雅可比矩阵第八章第八章 空间单链连杆机构的运动分析空间单链连杆机构的运动分析 8.1.18.1.1、位置分析、位置分析 8.1.28.1.2、速度分析、速度分析8.1.38.1.3、加速度分析、加速度分析 8.1.4 8.1.4、构件上任意点的运动分析、构件上任意点的运动分析 8.1、RCCC机构运动分析机构运动分析 8.1、RCCC机构运动
4、分析机构运动分析8.1、RCCC机构运动分析机构运动分析 传统意义上的空间连杆机构，其运动分析通常采用两大类基本方法：一类是类似平面机构运动分析时所采用的封闭封闭矢量矢量多边形法多边形法，但由于矩阵形式的封闭向量方程几乎包含了所有运动变量，在求解时为消去中间变量会比较困难。另一类是拆杆拆副法拆杆拆副法，即在建立机构的分析方程时，假想将机构的环路从某个运动副处拆开，或把某个杆(或部分运动链)拆掉，然后基于几何同一性条件建立约束方程。这种方法可使一些中间变量不在方程中出现，而使分析过程得以简化。8.1.1、位置分析、位置分析图8-1 RCCC机构 如图8-1所示为RCCC空间机构。机构的已知结构参
5、数为S0、h1、h2、h3、h0、01、12、23、30，设杆1为原动件而1为输入转角，要求分析运动参数0、2、3、S3、S2、S1。图8-1中，RCCC机构的各杆均固结有相应的坐标系，其中zi轴分别沿有关运动副的轴线，而xi轴则依次与相邻两个z轴的最短距离线相重合，例如x1轴为z1和z2轴的公垂线等。进行坐标系变换所用的方向余弦矩阵 、可由式(1-20)计算得到，满足如下形式01C12C23C30C（8-1）式中，下标ij=01,12,23,30cossincossinsinsincoscoscossin0sincosjjijjijijjjijjijijijC8.1.1、位置分析、位置分析
6、为了直接建立输出转角0和输入转角1的关系式，假想将杆2拆离，则根据几何等同性条件有（8-2）即（8-3）由此可得各输入输出角位置方程式（8-4）上式可写为（8-5）式中2112cos(,)coszz23300112(0,0,1)(0,0,1)cosTCCC0123011010233023300102330233012sinsinsinsincossin(cossincoscossin)cos(cossinsincoscos)cos00sincos0ABC13001301122330013010123sin,sincotcoscoscoscot(coscotsincos)sinsinABC 8.
7、1.1、位置分析、位置分析求转角2与1的关系式时，可直接利用式(8-4)将字母符号排列由0-1-2-3轮换成1-2-3-0，于是可得（8-6）求3与1的关系式时，可假想将机构拆分为浮动链2-3和连架链0-1。由这两个分链求出的cos(z1，z3)应该等同。即（8-7）由此可得（8-8）或（8-9）1230122121300101301213001300123sinsinsinsincossin(cossincossincos)cos(cossinsincoscos)cosTT12233001(0,0,1)(0,0,1)(0,0,1)(0,0,1)CCCC312231223130013001co
8、ssinsincoscoscossinsincoscos 130013001312231223cossinsincoscoscoscotcotsinsin8.1.1、位置分析、位置分析仿上将该式(8-9)中符号下标数字的排列由0-1-2-3轮换成3-0-1-2，可得由0求2的关系式为（8-10）为了便于用计算机选择合适的角度值，往往需要其他计算sin2和sin3的式子。为此，可按cos(x1，z2)写出下列等同关系式（8-11）由此可得（8-12）如将下标排列由0-1-2-3轮换成1-2-3-0，则得（8-13）023302330201120112cossinsincoscoscoscotco
9、tsinsinTT12233001(1,0,0)(0,0,1)(0,0,1)(1,0,0)CCCC2122301013012330sinsinsin(sincoscossincos)sincossin3233012120123001sinsinsin(sincoscossincos)sincossin8.1.1、位置分析、位置分析求各圆柱副中的相对线位移参数时，应将机构的封闭矢量多边形 向合适的坐标轴xi上进行投影，以避开一些运动变量。例如求S3时，应将机构的矢量封闭形向x2轴上取投影，得（8-14）利用方向余弦矩阵可得S3的关系式如下（8-15）如将式(8-15)的下标排列由0-1-2-3分
10、别轮换成3-0-1-2及1-2-3-0，则可得求S2及S1的关系式为（8-16）（8-17）在上面的两个例子中，我们在每个构件上建立局部坐标系时，实际上都采用了D-H坐标系。3320020021122332cos(,)cos(,)cos(,)cos(,)cos(,)0Sz xhx xSzxhx xhhx x33230121201020112233sinsin(coscossinsincos)sinsincoscos0ShShhh22123010130313001122sinsin(coscossinsincos)sinsincoscos0 ShShhh00301232312131223300s
11、insin(coscossinsincos)sinsincoscos0 ShShhhDD AA BB CC D8.1.2、速度分析、速度分析空间RCCC机构的角速度和线速度量可由其角位置和线位移参数对时间求导得到。首先求出输入输出角速度之间的关系，为此把式(8-4)对时间求导得（8-18）式中求转角 角速度时，可将式(8-10)对时间求导，整理后得（8-19）求解 与 的角速度关系式时，可将(8-8)对时间求导，整理后得（8-20）31300113112233sinsinsinsinsinsin233002001122sinsinsinsinsinsin210130023300100cossi
12、nsin(coscoscotsin)cossinAB13001301sin,sincotsincosAB 8.1.2、速度分析、速度分析 同理，求解线速度参数时，对相应的线位移参数表达式求时间的一次导数即可。对式(8-15)、(8-16)和(8-17)分别对时间求导可得 、和 的线速度表达式（8-21）（8-22）（8-23）至此，空间RCCC机构的角速度和沿圆柱副方向的线速度参数都已求出。3S2S1S3012 112200112 11220012212233332333233(sincoscossin)cos(cossinsincos)sincossinsinsincos/(sinsin)S
13、hhShhS 2301001 133001001 13301 1301301 122221222122(sincoscossin)cos(cossinsincos)sincossinsinsinsinsincos/(sinsin)ShhSShhS 112322331122322330300023300011233123(sincoscossin)cos(cossinsincos)sincossinsinsincos/(sinsin)ShhShhS 8.1.3、加速度分析、加速度分析 空间RCCC机构的角加速度和线加速度量可由其角速度和线速度参数对时间求导得到。首先求出输入输出角加速度之间的关系
14、，为此把式(8-18)对时间求导得（8-24）式中求转角 角加速度时，可将式(8-19)对时间求导，整理后得（8-25）求转角 角加速度时，可将式(8-20)对时间求导，整理后得（8-26）20100cossinCAB13001301200000130012111300302301sin,sincotcoscos(sincos)2(coscoscossinsin)cos(coscossincot)sinsinABCAB 2222330000022201122sinsin(sincos)cotsinsinsin 32230011313312233sinsincoscotsinsinsin 8.1
15、.3、加速度分析、加速度分析 同理，求解线加速度参数时，对相应的线速度参数表达式求时间的一次导数即可。对式(8-21)、(8-22)和(8-23)分别对时间求导,可得 、和 的线加速度表达式为（8-27）（8-28）3S2S1S23012 112 112 122012221212 12200112 1221 112 100112 121(coscossincossinsin)(coscossincossinsin)cos(coscossincossinsin)cos(coscossincosShhhh 22212222001222212222223333323333333333233sinsi
16、n)sin(cossin)(cossin)(cossin)sin(2coscossin)/(sinsin)ShhSSS 223010010010 12301 110 1010 12330010 1100010330010 10(coscossincossinsin)(coscossincossinsin)cos(coscossincossinsin)cos(coscossincosShhhh 21 101 12301 131 131 13301 13132201 11 12222221222222222212sinsin)sin(cossincos)sin(cossin)(cossin)(co
17、ssin)sin(2coscossin)/(sinsinSSSSShhSSS 2)8.1.3、加速度分析、加速度分析（8-29）至此，空间RCCC机构的角加速度和线加速度参数都已求出。21123223223232123332323232112232332223211223232(coscossincossinsin)(coscossincossinsin)cos(coscossincossinsin)cos(coscossincosShhhh 233233212331331331222333300000230000000000123sinsin)sin(2cossincos)(cossin)(
18、cossin)sin(coscossin)/(sinsin)SSShhSSS 8.1.4、构件上任意点的运动分析、构件上任意点的运动分析 在空间RCCC机构的动力学分析中，将会用到各构件的质心点的线加速度、构件角速度、角加速度等参数。下面我们将利用前面运动分析的结果来分析求解构件上任意点(这里求解构件质心点)的运动学参数。为了统一各运动参数的分析，我们设机架坐标系 为参考坐标系，将各构件的运动学参数都转换到机架坐标系下。000 x y z8.1.4、构件上任意点的运动分析、构件上任意点的运动分析图8-2 空间RCCC机构简图(1)构件1上任意点的运动分析 如图8-2所示，设构件1的质量为 ，在
19、其自身坐标系 中的质心点 的坐标为 ，也就是 。那么在坐标系 中有（8-30）易知 因此可得1m11 1Bx y z1G111111(,)xyzGGG1 1 1111T111()(,)B x y zxyzGGG1BG000Ax y z00 000 01 1 101()()()()()()()()()A x y zA x y zB x y zC1111AGABBGAAABBGAAABBGTT01111()(0,0,),()(cos,sin,0)ShhAAAB（8-31）111111111110110111110110110010111111101110110coscossincossinsin(
20、)0sinsincoscoscossin00sincos()cossincossinsin(xyzxyzGhhGSGGhGGG 1A G1111111110111011011010)sincoscoscossinsincosxyzyzhGGGGS8.1.4、构件上任意点的运动分析、构件上任意点的运动分析 对公式(8-31)求时间的一次导数和二次导数即可得构件1质心 在机架坐标系 中的速度和加速度。（8-32）（8-33）构件1为原动件，一般情况下为匀速转动。由多体运动学的知识可得，构件1在机架坐标系 中的绝对角速度和绝对角加速度为：（8-34）（8-35）1G000Ax y z11111111
21、1110111011111110111011()sincoscoscossin()()cossincoscossin0 xyzxyzGhGGGhGG1v111111111112111110111011111110111011211111011101111111011()cossincossinsin()sincoscoscossin()()sincoscossinsin()cossincosxyzxyzxyzxyGhGGGhGGGhGGGhGG 1a11011cossin0z000Ax y zT001()()(0,0,)110T10100()()(0,0,0)8.1.4、构件上任意点的运动分析
22、、构件上任意点的运动分析(2)构件2上任意点的运动分析 如图8-2所示，设构件2的质量为 ，在其自身坐标系 中的质心点 的坐标为 ，也就是 。那么在坐标系 中有（8-36）图8-2 空间RCCC机构简图2m222Cx y z2G222222(,)xyzGGG1 1 1222T222()(,)B x y zxyzGGG2CG11 1Bx y z1 1 11 1 122 212()()()()()()()()()B x y zB x y zC x y zC2222BGBCCGBBBCCGBBBCCG易知 ，因此可得（8-37）TT12222()(0,0,),()(cos,sin,0)ShhBBBC
23、222222222221221222221221221121222222212221220coscossincossinsin()0sinsincoscossinsin00sincos()cossincossinsin(xyzxyzGhhGSGGhGGG 2B G2222222221222122122121)sincoscossinsinsincosxyzyzhGGGGS8.1.4、构件上任意点的运动分析、构件上任意点的运动分析 然后求在坐标系 中 的坐标，它的方法与前面所述求 坐标是一样的，因此可以写出（8-38）式中 对公式(8-38)求时间的一次导数和二次导数即可得构件2质心 在机架坐标
24、系 中的速度和加速度。（8-39）000Ax y z()2AG1()AG1101101110110101010cossincossinsin()sincoscossinsinsincosPQRPQRQRS2AG22222222222221222121222221222122122121()cossincossinsin()sincoscossinsinsincosxyzxyzyzPGhGGhQGhGGRGGS2G000Ax y z1 111 1011011 1011 111 1011011 10101sincossincossincoscossin()cossinsincoscoscoscos
25、sinsinPPQQRPPQQRQ2v8.1.4、构件上任意点的运动分析、构件上任意点的运动分析式中（8-40）式中22222222222122122222222122221222()sin(cossin)cos()coscossinsincosxyzxyzPGhGGQGhGG 21111111111101211011101101110121101110111011cossinsincossinsincossincoscoscossincoscoscoscossincossinsinsin()sinPPPPPQQQQQRRRP 2a2211111111110111011101101110121
26、1011101110101coscossincoscoscossincossincoscoscossincossinsincossincossinsinPPPPQQQQQRRRQ 222222222222222 2222 222122122 22122122 22222 2222 222122 22122 221()cos()sin(cossin)sin(cossin)cos()sin()coscoscoscossinsinxxyzyzxxyyzPGhGhGGGGQGhGhGGG2222 22122 2sinsincoszG8.1.4、构件上任意点的运动分析、构件上任意点的运动分析 由多体运动
27、学的知识可得，构件2在机架坐标系 中的绝对角速度和角加速度为即（8-41）（8-42）000Ax y zTT202001000121 11012()()()()(0,0,)(0,0,)CC2 010 00121 110121 110 00121 10121 1()()()()()()()CCC10122010121012sinsin()cossincos 1012101 12201012101 121012sinsincossin()cossinsinsincos 8.1.4、构件上任意点的运动分析、构件上任意点的运动分析(3)构件3上任意点的运动分析 如图8-2所示，设构件3的质量为 ，在其
28、自身坐标系 中的质心点 的坐标为 ，也就是 。那么在坐标系中 有图8-2 空间RCCC机构简图3m333Dx y z3G333333(,)xyzGGG3 3 3333T333()(,)D x y zxyzGGG3DG000Ax y z（8-43）00 000 03 3 3130()()()()()()()()()A x y zA x y zD x y zC3333AGADDGADDDDGADDDDG易知 ，因此可得（8-44）TT0330330()(,0,0),()(sin,cos,0)hSS ADDD3333333300033003003030333003003030330300303030
29、cossin0()0sinsincoscoscossin0cossinsinsinsincoscossinsincoscxyzxyxyGhSGSGGGhGG 3AG33330303330303030303330oscos()sinsinsincossin()coszxyzGSGGGS8.1.4、构件上任意点的运动分析、构件上任意点的运动分析 对公式(8-44)求时间的一次导数和二次导数即可得构件3质心 在机架坐标系 中的速度和加速度。（8-45）（8-46）3G000Ax y z33333330300303030300303303030300303(cossin)()(coscossincos
30、)sin(cossinsinsin)cosyxxyxyGGGGSGGS 3v333333333332303003030030303030030303030030330303030030303(cossin)(sincos)(coscossincos)(sincoscoscos)sin()(cossinsinsin)(sinsinyxyxxyxyxyxGGGGGGGGSGGGG 3a320300303cossin)cosyS8.1.4、构件上任意点的运动分析、构件上任意点的运动分析208.1.4、构件上任意点的运动分析、构件上任意点的运动分析 由多体运动学的知识可得，构件3在机架坐标系 中的绝对
31、角速度和绝对角加速度为 和 ，即（8-47）（8-48）至此，空间RCCC机构各构件的基本运动学参数都已求出，当然也可以求出机构上其它任何一点的运动学参数。000Ax y zT3 030003 00303 3030()()()()(0,0,)CC T3 030003 00303 3030()()()()(0,0,)CC 3 03003000()sincos 3 03003000()sincos 8.2.18.2.1、机械手的位姿描述、机械手的位姿描述 8.2.28.2.2、连杆变换矩阵、连杆变换矩阵 8.2.3 8.2.3、3R3R机器人机构正向位置求解机器人机构正向位置求解 8.2.4 8.
32、2.4、空间、空间3R3R机器人正、反向位置求解机器人正、反向位置求解 8.2.5 8.2.5、PUMA560PUMA560机器人机构位置正反解分析机器人机构位置正反解分析 8.2、串联机器人机构的位置分析、串联机器人机构的位置分析 8.2、串联机器人机构的位置分析、串联机器人机构的位置分析8.2、串联机器人机构的位置分析、串联机器人机构的位置分析8.2.1、机械手的位姿描述、机械手的位姿描述图8-3 机器人夹手 作为分析示例本节给出实际机器人机构的分析方法。工业机器人机构有串联和并联两种形式，本节主要介绍串联机器人机构的运动方程及其求解方法。串联机器人机构大都是由R副和P副组成，称为关节。可
33、以把任何机器人的机械手看做是一系列由关节连接起来的连杆构成的。我们将为机械手的每一连杆设定一个坐标系，并用齐次坐标变换来描述这些坐标系间的相对位置和姿态。图8-3 机器人夹手一个六连杆机械手可具有六个自由度，每个连杆间具有一个自由度，并能在其运动范围内任意定位与定向。其中，三个自由度用于规定位置，而另外三个自由度用来规定姿态。第一章所述的不共原点的坐标变换矩阵 (其特殊形式即为D-H坐标变换矩阵)可用来描述各连杆间的一系列变换。在机器人描述中通常用 表示，为方便读者阅读相关机器人书籍，这里的矩阵描述符号及表示方式均与机器人书籍中所采用的符号与表示方式相同。ijMijT 串联机器人的正向运动学是
34、指：在给定相邻连杆的相对位置情况下，确定机器人末端执行器的位姿。串联机器人的反向运动学(亦称运动学反解)是指给定工具坐标系所期望的位姿，找出与该位姿相对应的各个关节的输出。8.2.1、机械手的位姿描述、机械手的位姿描述8.2.1、机械手的位姿描述、机械手的位姿描述 从经典理论角度来看，串联机器人的正向运动学可以通过将各个关节引起的刚体运动加以合成。如果利用传统的DH参数法来计算末端杆坐标系n相对参考坐标系0的位姿，需在各个关节上建立连杆坐标系i，然后利用连杆坐标系将相邻的刚体运动联系起来。定义 为相邻连杆坐标系间的齐次变换矩阵，则对于具有n个关节的串联机器人正解的一般计算公式为(8-49)式中
35、，表示机器人运动学的正解。1,iiT0nT001 121,1,niinnTT TTT图8-3 机器人夹手 图8-3表示工业机器人的一个手腕。所描述的坐标系的原点置于手腕指尖的中心,此原点由矢量p p表示。描述手腕方向的3个单位矢量的指向如下：z向矢量处于夹手进入物体的方向上，并称之为接近矢量a a；y向矢量的方向从一个指尖指向另一个指尖，处于规定夹手方向上，称之方向矢量o o；最后一个矢量称为法线矢量n n，它与矢量o o和a a一起构成一个右手坐标系，并由矢量的叉乘所规定：。因此，齐次变换 具有下列形式 (8-50)no a06T06 0 0 0 1xxxxyyyyzzzznoapnoapT
36、noap8.2.1、机械手的位姿描述、机械手的位姿描述8.2.1、机械手的位姿描述、机械手的位姿描述 运动姿态运动姿态可以采用前述任一个三参数的方向余弦矩阵。其中绕三个坐标轴的旋转也称为横滚(roll)、俯仰(pitch)和偏转(yaw)。见图8-4，想象有只船沿着z轴方向航行，横滚对应绕z轴旋转 角，俯仰对应于绕y轴旋转 角，而偏转则对应于绕x轴旋转 角。适用于机械手端部执行装置的旋转，如图8-4b所示。图8-4 用横滚、俯仰和偏转表示机械手运动姿态对于旋转次序作如下规定（8-51）式中，RPY表示横滚、俯仰和偏转三旋转的组合变换，即先绕x轴旋转 角，再绕y轴旋转 角，最后绕z轴旋转 角。(
37、,)(,)(,)(,)RPYR zR yR x 8.2.1、机械手的位姿描述、机械手的位姿描述8.2.2、连杆变换矩阵、连杆变换矩阵 相邻坐标系间可以用齐次变换矩阵来表示。要求出机械手所需要的变换矩阵,每个连杆都要用广义连杆来描述。机器人机械手是由一系列连接在一起的连杆(杆件)构成的。连接两连杆的运动副称为关节。除了首端和末端的两个构件外，每一连杆有两个关节元素，靠近机架端为前端，靠近执行构件(或手部)的为末端。需要用两个参数来描述一个连杆,即两个关节的公共法线距离 和垂直于 所在平面内两轴的夹角 ;规定前端关节与连杆标号相同。第 个连杆坐标系有两种设置方法，即把 轴设置在前关节上，称为前置坐
38、标系，或把 轴设置在后关节上，称为后置坐标系。无论是前置坐标系还是后置坐标系，其 轴永远是自身两关节的公垂线，方向是由前关节指向后关节。两杆坐标系之间的关系需要另外两个参数来表示,即两连杆的相对位置 和两连杆法线的夹角 ,如图8-5所示。iaiaiaiizizix.qi图8-5 前置坐标系8.2.2、连杆变换矩阵、连杆变换矩阵8.2.2、连杆变换矩阵、连杆变换矩阵图8-5 前置坐标系 除第一个和最后一个构件外,每个连杆两端的轴线各有一条法线,分别为前、后相邻连杆的公共法线,这两法线间的距离即为 。我们称为 连杆长度,为连杆扭角,为两连杆距离，为两连杆的夹角。一旦对全部连杆规定坐标系之后，就可以
39、按照D-H矩阵的建立方法逐个建立相邻连杆的变换矩阵。下面给出前后两种坐标系的建立方法和结果。idiaiiid(1)iiT1ix1ix1i1iaizizidi11111(1)111100001iiiiiiiiiiiiiiiiiiicsacsccsd sTsssccd c8.2.2、连杆变换矩阵、连杆变换矩阵8.2.2、连杆变换矩阵、连杆变换矩阵(1)前置坐标系 图8-5中所选择的坐标系为前置坐标系。变换矩阵 可以认为是通过下列组合变换过程而得到的，即绕轴 旋转 沿轴 平移 沿轴 平移 绕轴 旋转 ，把这四个变换矩阵按上述顺序连乘得到（8-52）8.2.2、连杆变换矩阵、连杆变换矩阵8.2.2、连
40、杆变换矩阵、连杆变换矩阵(2)后置坐标系 由图8-6可知，坐标系后置时，变换矩阵 可以认为是通过下列组合变换过程而得到的，即绕轴 旋转 沿轴 平移 沿轴 平移 绕轴 旋转 ，把这四个变换矩阵按上述顺序连乘得到（8-53）(1)iiTixixiia1iz1izidi(1)00001iiiiiiiiiiiiiiiiiiics cs sacsc cc sa sTscd 图8-6 后置坐标系8.2.2、连杆变换矩阵、连杆变换矩阵8.2.3、3R机器人机构正向位置求解机器人机构正向位置求解图 8-7 3自由度机器人的连杆坐标系及其D-H参数 利用坐标系后置方式标出图8-7所示的3自由度3R机器人的连杆坐
41、标系，并给出其D-H参数。这样可以由式(8-53)建立相邻连杆坐标系间的齐次变换矩阵（8-54）1,cossin0cossincos0sin (1,2,3)00100001iiiiiiiiiiaaiT8.2.3、3R机器人机构正向位置求解机器人机构正向位置求解8.2.3、3R机器人机构正向位置求解机器人机构正向位置求解进而根据式(8-49)对其正向运动学进行求解。（8-55）因此，将式(8-54)代入到式(8-55)中，得到（8-56）式中，是 的简写。如果用表示机器人末端的位姿，则由式(8-56)可以得到（8-57）03011122233()()()TTTT12312311212312312
42、312311212312303cossin0coscoscossincos0sinsinsin=00100001llllllTijij112123123112123123123coscoscossinsinsinxlllylll8.2.3、3R机器人机构正向位置求解机器人机构正向位置求解8.2.4、空间、空间3R机器人正、反向位置求解机器人正、反向位置求解图8-8 空间3R机器人及其D-H参数 首先给出该机器人的D-H参数(连杆坐标系前置)，如图8-8所示。然后建立相邻连杆坐标系间的齐次变换矩阵。（8-58）1122110112223323323cossin00cossin00sincos00
43、0010,001sincos0000010001cossin0sincos0000100001AslTTT8.2.4、空间、空间3R机器人正、反向位置求解机器人正、反向位置求解8.2.4、空间、空间3R机器人正、反向位置求解机器人正、反向位置求解由此可求得从坐标系0到坐标系3的组合坐标变换矩阵（8-59）由于末端参考点D在3系中的坐标表示为 ，因此，根据（8-60）求得空间3R机器人的正项位置解（8-61）下面根据式(8-61)求该机器人的位置反解。由上式已知（8-62）对上式两边求平方和相加得到（8-63）03T0301 1223TT T T3T3(,0,0)Dlr0303DDrTr2123
44、1230212312322323cos coscos cos()sin cossin cos()sinsin()DAllllsllr2123123212312322323cos coscos cos()sin cossin cos()sinsin()DDDAxllyllzsll22222233 33()2cosDDDAxyzslll l8.2.4、空间、空间3R机器人正、反向位置求解机器人正、反向位置求解8.2.4、空间、空间3R机器人正、反向位置求解机器人正、反向位置求解由此可得（8-64）对应有两组解。再由式(8-62)可得（8-65）（8-66）式中222222333 3()arccos
45、2DDDAxyzslll l1arctanDDyx2331332233331()(cos)cossinarctan(cos)()sincosDADDDAzsllx lxllzsl233sin1 cos 8.2.4、空间、空间3R机器人正、反向位置求解机器人正、反向位置求解8.2.5、PUMA560机器人机构位置正反解分析机器人机构位置正反解分析图8-9 PUMA560机器人及其坐标系选取 PUMA 560 机器人如图8-9a所示，其连杆参数见表8-1。据式(8-52)和表8-1所示连杆参数，可求得各连杆变换矩阵如下（8-67）112233211233011223244355434454455c
46、s00000sc0000100,00100000100001000100010000010010,00000001cscsadscsccsacsdscscTTTTT665666000010,0000010001csscT8.2.5、PUMA560机器人机构位置正反解分析机器人机构位置正反解分析8.2.5、PUMA560机器人机构位置正反解分析机器人机构位置正反解分析各连杆变换矩阵相乘，得PUMA560的机械手变换矩阵（8-68）为关节变量的函数。取不同值时，将得到不同的变换矩阵 ，即（8-69）式中的元素是 的函数。有06011122233344455566()()()()()()TTTTTT
47、T(1,2,6)ii06T06 0 0 0 1xxxxyyyyzzzznoapnoapnoapT(1,2,6)ii1234 5 64 623 5 614 5 64 61234 5 64 623 5 614 5 64 6234 5 64 623 5 61234 5 64623 5 614 64 5 61234 5 64623 5(-s s)-()(-s s)-()(-s s)-(-s c)()(-s c)xyzxync cc c cs s cs s c cc sns cc c cs s cc s c cc snsc c cc s coc cc c ss s ss c cs c sos cc c
48、ss s s 614 64 5 6234 5 64 623 5 6c()(-s s)czc cs c cosc c cs s 123 4 523 51 4 5123 4 523 51 4 523 4 523 51223 234 232 11223 234 232 13 232 2423()()(-)(-)-xyzxyzac c c ss cc s sas c c ss cc s sas c sc cpc a ca cd sd sps a ca cd sd cpa sa s d c 8.2.5、PUMA560机器人机构位置正反解分析机器人机构位置正反解分析8.2.5、PUMA560机器人机构位置
49、正反解分析机器人机构位置正反解分析式中 为校核所得 的正确性，计算 ，时手臂变换矩阵 的值。计算结果为（8-70）与图8-9所示的情况完全一致。下面给出PUMA 560 机器人的位置反解。重新写出运动方程如下（8-71）sin,cos,sin(),cos()iiiiijijijijscsc06T06T19020 390 4560 2240630100011000001dadaT06011122233344455566 ()()()()()()0 0 0 1xxxxyyyyzzzznoapnoapnoapTTTTTTT8.2.5、PUMA560机器人机构位置正反解分析机器人机构位置正反解分析8.
50、2.5、PUMA560机器人机构位置正反解分析机器人机构位置正反解分析 若末端连杆的位姿已经给定，即上式中的元素n,o,a和p为已知，则求关节变量 的值称为运动反解。用未知的连杆逆变换左乘方程式(8-68)两边，把关节变量分离出来，从而求解。下面先把求解中要用到的几个中间变换矩阵求出如下（8-72）矩阵中126,5 65 654 5 64 64 5 64 64 53665 65 6544645563634465 65 654 5 64 64 5 64 64 523232 2213122323232 2000,00000100010001000c cc ssc c cs sc c ss cc s
