机器人运动学

1、运动学研究的问题： 手在空间的运动与各个关节的运动之间的关系。正问题： 已知关节运动，求手的运动。逆问题： 已知手的运动，求关节运动。数学模型： 手的运动位姿变化位姿矩阵M 关节运动参数变化关节变量qi，i=1，n运动学方程： M=f(qi)， i=1，n正问题：已知qi，求M。逆问题：已知M，求qi。2.1 机器人的位姿描述2.2 齐次变换及运算2.3 机器人运动学方程2.4 机器人微分运动 习题2.1.1 机器人位姿的表示2.1.2 机器人的坐标系2.1 机器人的位姿描述2.1.1 机器人位姿的表示机器人的位姿主要是指机器人手部在空间的位置和姿态，有时也会用到其它各个活动杆件在空间的位置和

2、姿态。2.1 机器人的位姿描述2.1.1 机器人位姿的表示位置可以用一个31的位置矩阵来描述。 (,)2.1 机器人的位姿描述2.1.1 机器人位姿的表示姿态可以用坐标系三个坐标轴两两夹角的余弦值组成33的姿态矩阵来描述。 (,)hhhh2.1 机器人的位姿描述2.1.1 机器人位姿的表示 例：右图所示两坐标系的姿态为：000011112.1 机器人的位姿描述2.1.2 机器人的坐标系手部坐标系参考机器人手部的坐标系，也称机器人位姿坐标系，它表示机器人手部在指定坐标系中的位置和姿态。机座坐标系参考机器人机座的坐标系，它是机器人各活动杆件及手部的公共参考坐标系。杆件坐标系参考机器人指定杆件的坐标

3、系，它是在机器人每个活动杆件上固定的坐标系，随杆件的运动而运动。绝对坐标系参考工作现场地面的坐标系，它是机器人所有构件的公共参考坐标系。 2.1 机器人的位姿描述2.1.2 机器人的坐标系手部坐标系h机座坐标系0 杆件坐标系i i=1，n绝对坐标系B 2.1 机器人的位姿描述2.2.1 直角坐标变换2.2.2 齐次坐标变换2.2 齐次变换及运算2.2.1 直角坐标变换iiiijjjj坐标之间的变换关系：平移变换旋转变换2.2 齐次变换及运算1、平移变换 设坐标系i和坐标系j具有相同的姿态，但它俩的坐标原点不重合，若用 矢量表示坐标系i和坐标系j原点之间的矢量，则坐标系j就可以看成是由坐标系i沿

4、矢量 平移变换而来的，所以称矢量 为平移变换矩阵，它是一个31的矩阵，即： iiiijjjj2.2 齐次变换及运算2.2.1 直角坐标变换1、平移变换 若空间有一点在坐标系i和坐标系j中分别用矢量 和 表示，则它们之间有以下关系：称上式为坐标平移方程。 iiiijjjj2.2 齐次变换及运算2.2.1 直角坐标变换2、旋转变换 设坐标系i和坐标系j的原点重合，但它俩的姿态不同。则坐标系j就可以看成是由坐标系i旋转变换而来的，旋转变换矩阵比较复杂，最简单的是绕一根坐标轴的旋转变换。下面以此来对旋转变换矩阵作以说明。 iiiijjjj2.2 齐次变换及运算2.2.1 直角坐标变换2、旋转变换绕z轴

5、旋转角 坐标系i和坐标系j的原点重合，坐标系j的坐标轴方向相对于坐标系i绕轴旋转了一个角。 角的正负一般按右手法则确定，即由z轴的矢端看，逆时钟为正。iiiijjjj2.2 齐次变换及运算2.2.1 直角坐标变换2、旋转变换绕z轴旋转角变换矩阵推导 若空间有一点p，则其在坐标系i和坐标系j中的坐标分量之间就有以下关系： iiiijjjj2.2 齐次变换及运算2.2.1 直角坐标变换2、旋转变换绕z轴旋转角 若补齐所缺的有些项，再作适当变形，则有： 2.2 齐次变换及运算2.2.1 直角坐标变换2、旋转变换绕z轴旋转角 将上式写成矩阵的形式，则有： 2.2 齐次变换及运算2.2.1 直角坐标变换

6、2、旋转变换绕z轴旋转角 再将其写成矢量形式，则有： 称上式为坐标旋转方程，式中： p点在坐标系i中的坐标列阵（矢量）； p点在坐标系j中的坐标列阵（矢量）； 坐标系j变换到坐标系i的旋转变换矩阵，也称为方向余弦矩阵。 2.2 齐次变换及运算2.2.1 直角坐标变换2、旋转变换绕z轴旋转角 旋转变换矩阵，也称为方向余弦矩阵，是一个33的矩阵，其中的每个元素就是坐标系i和坐标系j相应坐标轴夹角的余弦值，它表明坐标系j相对于坐标系i的姿态（方向）。2.2 齐次变换及运算2.2.1 直角坐标变换2、旋转变换绕z轴旋转角 旋转变换矩阵：2.2 齐次变换及运算2.2.1 直角坐标变换iiiijjjj2、

7、旋转变换绕x轴旋转角的 旋转变换矩阵为： iiiijjjj2.2 齐次变换及运算2.2.1 直角坐标变换2、旋转变换绕y轴旋转角的 旋转变换矩阵为： iiiijjjj2.2 齐次变换及运算2.2.1 直角坐标变换2、旋转变换旋转变换矩阵的逆矩阵 旋转变换矩阵的逆矩阵既可以用线性代数的方法求出，也可以用逆向的坐标变换求出。 以绕z轴旋转角为例，其逆向变换即为绕z轴旋转-角，则其旋转变换矩阵就为：2.2 齐次变换及运算2.2.1 直角坐标变换2、旋转变换旋转变换矩阵的逆矩阵 比较以下两式： 结论： 2.2 齐次变换及运算2.2.1 直角坐标变换3、联合变换 设坐标系i和坐标系j之间存在先平移变换，

8、后旋转变换，则空间任一点在坐标系i和坐标系j中的矢量之间就有以下关系： 称上式为直角坐标系中的坐标联合变换方程。2.2 齐次变换及运算2.2.1 直角坐标变换例：已知坐标系B的初始位置与坐标系A重合，首 先坐标系B沿坐标系A的x轴移动12个单位， 并沿坐标系A的y轴移动6个单位，再绕坐标系 A的z轴旋转30，求平移变换矩阵和旋转变换 矩阵。假设某点在坐标系B中的矢量为： ，求该点在坐标系A中的矢量？2.2 齐次变换及运算2.2.1 直角坐标变换解：由题意可得平移变换矩阵和旋转变换矩阵分别为： 则： 2.2 齐次变换及运算2.2.1 直角坐标变换3、联合变换 若坐标系i和坐标系j之间是先旋转变换

9、，后平移变换，则上述关系是应如何变化？问题： 当坐标系之间存在多次变换时，直角坐标变换就无法用同一规整的表达式表示了！2.2 齐次变换及运算2.2.1 直角坐标变换2.2.2 齐次坐标变换1、齐次坐标的定义 空间中任一点在直角坐标系中的三个坐标分量用 表示，若有四个不同时为零的数与三个直角坐标分量之间存在以下关系： 2.2 齐次变换及运算则称 是空间该点的齐次坐标。1、齐次坐标的定义齐次坐标的几点说明：.空间中的任一点都可用齐次坐标表示；.空间中的任一点的直角坐标是单值的，但其对应的齐次坐标是多值的；.k是比例坐标，它表示直角坐标值与对应的齐次坐标值之间的比例关系；.若比例坐标k=1，则空间任

10、一点(x, y, z)的齐次坐标为(x, y, z, 1) ，以后用到齐次坐标时，一律默认k=1 。 2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵） 若坐标系j是i先沿矢量平移，再绕z轴旋转角得到的，则空间任一点在坐标系i和坐标系j中的矢量和对应的变换矩阵之间就有 ，写成矩阵形式则为： 2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵）再用坐标分量等式表示，则有： 2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵）引入齐次坐标，补齐所缺各项，再适当变形，则有： 2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐

11、次变换矩阵（D-H矩阵）再将其写成矩阵形式则有： 2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵）由此可得联合变换的齐次坐标方程为：2.2 齐次变换及运算2.2.2 齐次坐标变换齐次坐标变换矩阵， 它是一个44的矩阵。 2、齐次变换矩阵（D-H矩阵）齐次坐标变换矩阵的意义若将齐次坐标变换矩阵分块，则有：2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵）齐次坐标变换矩阵的意义意义： 左上角的33矩阵是两个坐标系之间的旋转变换矩阵，它描述了姿态关系。 右上角的31矩阵是两个坐标系之间的平移变换矩阵，它描述了位置关系。 所以齐次坐标变换矩阵又称为

12、位姿矩阵。 2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵）齐次坐标变换矩阵的意义齐次变换矩阵的通式为： j的原点在i中的坐标分量； j的x轴对i的三个方向余弦； j的y轴对i的三个方向余弦； j的z轴对i的三个方向余弦。2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵）单独的平移或旋转齐次坐标变换矩阵 平移变换的齐次矩阵为：2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵）单独的平移或旋转齐次坐标变换矩阵 旋转变换的齐次矩阵为：2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵）单独的平移

13、或旋转齐次坐标变换矩阵 同理可得：2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵）联合变换与单步齐次变换矩阵的关系观察以下三个齐次变换矩阵的关系： 2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵）联合变换与单步齐次变换矩阵的关系经观察可得： 2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵） 联合变换与单步齐次变换矩阵的关系 任何一个齐次坐标变换矩阵均可分解为一个平移变换矩阵与一个旋转变换矩阵的乘积，即： 2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵） 联合变换与单步齐次矩阵的关系

14、当空间有n个坐标系时，若已知相邻坐标系之间的齐次变换矩阵，则： 由此可知，建立机器人的坐标系，将机器人手部在空间的位姿用齐次坐标变换矩阵描述出来，从而建立机器人的运动学方程。 0i-1in2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵） 相对变换 坐标系之间多步齐次变换矩阵等于每次单独变换的齐次变换矩阵的乘积，而相对变换则决定这些矩阵相乘的顺序，其分为左乘和右乘： .若坐标系之间的变换是始终相对于原来的参考坐标系，则齐次坐标变换矩阵左乘； .若坐标系之间的变换是相对于当前新的坐标系，则齐次坐标变换矩阵右乘。 2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换

15、矩阵（D-H矩阵）相对变换 例：已知坐标系B是绕坐标系A的zA轴旋转90，再绕A的xA轴旋转90，最后沿矢量： 平移得到的，求坐标系A与坐标系B之间的齐次坐标变换矩阵。 2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵） 相对变换解：由题意可知满足左乘原则，即有： 2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵） 相对变换解：若满足右乘原则，则有： 2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵） 逆变换 已知i通过先平移，后旋转变成j，则变换矩阵为：iiiijjjj2.2 齐次变换及运算2.2.2 齐次坐标变换

16、2、齐次变换矩阵（D-H矩阵） 逆变换逆变换时：变换顺序颠倒 先平移，后旋转先旋转，后平移变换参数取反 旋转（） （ -） 平移（px，py，pz） （-px，-py，-pz） 2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵） 逆变换 则j到i的变换矩阵为：iiiijjjj2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵） 逆变换2.2 齐次变换及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵） 逆变换2.2 齐次变换及运算2.2.2 齐次坐标变换 2、齐次变换矩阵（D-H矩阵） 逆变换 若齐次变换矩阵为： 则： 2.2 齐次变换

17、及运算2.2.2 齐次坐标变换2、齐次变换矩阵（D-H矩阵） 逆变换 若齐次变换矩阵为：2.2 齐次变换及运算2.2.2 齐次坐标变换2.3.1 运动学方程建立步骤 1、建立坐标系 2、确定参数 3、相邻杆件的位姿矩阵 4、建立方程2.3.2 运动学方程的解2.3 机器人运动学方程2.3.1 运动学方程建立步骤回顾：运动学方程的模型： M=f(qi)， i=1，n M机器人手在空间的位姿 qi机器人各个关节变量2.3 机器人运动学方程1、建立坐标系 机座坐标系0 杆件坐标系i i=1，2，n 手部坐标系h注意：杆件编号关节编号2.3 机器人运动学方程oh0123关节1关节2关节3x1z1o1Z

18、hxhx0z0o0z3x3o3y2x2o22.3.1 运动学方程建立步骤1、建立坐标系机座坐标系0建立原则：z轴垂直， x轴水平，方向指向手部所在平面。2.3 机器人运动学方程2.3.1 运动学方程建立步骤x0z0o01、建立坐标系杆件坐标系i，i=1，2，n 建立原则： z轴与关节轴线重合， x轴与两关节轴线的距离重合，方向指向下一个杆件。杆件坐标系有两种： 第一种： z轴与i+1关节轴线重合 第二种： z轴与i关节轴线重合2.3 机器人运动学方程2.3.1 运动学方程建立步骤1、建立坐标系杆件坐标系i第一种坐标系： z轴与i+1关节轴线重合。x0z0o00123关节1关节2关节3z2x2o

19、2x1y1o1z3x3o32.3 机器人运动学方程2.3.1 运动学方程建立步骤1、建立坐标系杆件坐标系i第二种坐标系： z轴与i关节轴线重合。2.3 机器人运动学方程2.3.1 运动学方程建立步骤x0z0o00123关节1关节2关节3x1z1o1y2x2o2z3x3o3x0z0o00123关节1关节2关节3z2x2o2x1y1o1z3x3o31、建立坐标系手部坐标系h 在第一种杆件坐标系下，h与末端杆件坐标系n重合。zhxhoh2.3 机器人运动学方程2.3.1 运动学方程建立步骤1、建立坐标系手部坐标系h 在第二种杆件坐标系下，h建立在手部中心，方向与末端杆件坐标系n保持一致。2.3 机器

20、人运动学方程2.3.1 运动学方程建立步骤x0z0o00123关节1关节2关节3x1z1o1y2x2o2z3x3o3ohZhxh2、确定参数杆件几何参数（不变）I、杆件长度li：两关节轴线的距离。II、杆件扭角i：两关节轴线的夹角。ilii2.3 机器人运动学方程2.3.1 运动学方程建立步骤2、确定参数关节运动参数I、关节平移量di：相邻杆件的长度在关节轴线上的距离。II、关节回转量i：相邻杆件的长度在关节轴线上的夹角。ili-1i-1lii关节idi2.3 机器人运动学方程2.3.1 运动学方程建立步骤2、确定参数关节运动参数关节变量： di平移关节； i回转关节。ili-1i-1lii关

21、节idi2.3 机器人运动学方程2.3.1 运动学方程建立步骤3、相邻杆件位姿矩阵第一种坐标系 建立坐标系i-1、i。试分析i-1i的变换过程!ii-1关节iXi-1Z i-1Oi-1XiZiOi2.3 机器人运动学方程2.3.1 运动学方程建立步骤3、相邻杆件位姿矩阵第一种坐标系I、i-1i变换过程a、Trans（0，0，di）；b、Rot（z，i）；c、Trans（li，0，0）；d、Rot（x，i）。ii-1lii关节idiXi-1Z i-1Oi-1XiZiOii2.3 机器人运动学方程2.3.1 运动学方程建立步骤cbad3、相邻杆件位姿矩阵第一种坐标系 II、单步齐次变换矩阵2.3

22、机器人运动学方程2.3.1 运动学方程建立步骤a、Trans（0，0，di）b、Rot（z，i）3、相邻杆件位姿矩阵第一种坐标系 II、单步齐次变换矩阵2.3 机器人运动学方程2.3.1 运动学方程建立步骤c、Trans（li，0，0）d、Rot（x，i）3、相邻杆件位姿矩阵第一种坐标系 III、相邻杆件的位姿矩阵2.3 机器人运动学方程2.3.1 运动学方程建立步骤3、相邻杆件位姿矩阵第一种坐标系 III、相邻杆件的位姿矩阵2.3 机器人运动学方程2.3.1 运动学方程建立步骤3、相邻杆件位姿矩阵第一种坐标系注意：特例！ 2.3 机器人运动学方程2.3.1 运动学方程建立步骤3、相邻杆件位姿

23、矩阵第二种坐标系 建立坐标系i-1、i。 试分析i-1i的变换过程！ii-1关节iXi-1Z i-1Oi-1XiZiOi2.3 机器人运动学方程2.3.1 运动学方程建立步骤3、相邻杆件位姿矩阵第二种坐标系I、i-1i变换过程a、Trans（li-1，0，0）；b、Rot（x，i-1）；c、Trans（0，0，di）；d、Rot（z，i）。ili-1i-1i关节idiXi-1Z i-1Oi-1XiZiOii-12.3 机器人运动学方程2.3.1 运动学方程建立步骤cbad3、相邻杆件位姿矩阵第二种坐标系 II、单步齐次变换矩阵2.3 机器人运动学方程2.3.1 运动学方程建立步骤a、Trans

24、（li-1，0，0）b、Rot（x，i-1）3、相邻杆件位姿矩阵第二种坐标系 II、单步齐次变换矩阵2.3 机器人运动学方程2.3.1 运动学方程建立步骤c、Trans（0，0，di）d、Rot（z，i）3、相邻杆件位姿矩阵第二种坐标系 III、相邻杆件的位姿矩阵2.3 机器人运动学方程2.3.1 运动学方程建立步骤3、相邻杆件位姿矩阵第二种坐标系 III、相邻杆件的位姿矩阵2.3 机器人运动学方程2.3.1 运动学方程建立步骤4、建立方程2.3 机器人运动学方程2.3.1 运动学方程建立步骤例：已知三自由度平面关节机器人如图所示。 设机器人杆件1、2、3的长度为l1，l2，l3。试建立机器人

25、的运动学方程。 l1l3l22.3 机器人运动学方程2.3.1 运动学方程建立步骤解：（1）建立坐标系(第一种) a、机座坐标系0 b、杆件坐标系i c、手部坐标系h （与末端杆件坐 标系n重合） l1l3l2x0y0y1x1y2x2y3hx3h2.3 机器人运动学方程2.3.1 运动学方程建立步骤解：（2）确定参数（第一种）di相邻坐标系x轴之间的距离；i相邻坐标系x轴之间的夹角；li相邻坐标系z轴之间的距离；i相邻坐标系z轴之间的夹角。注意：根据方向确定参数的正负！l1l3l2x0y0y1x1y2x2y3hx3h3212.3 机器人运动学方程2.3.1 运动学方程建立步骤解：（2）确定参数

26、（第一种） i dii lii qi 1 01 l1 01 2 02 l2 02 3 03 l3 03l1l3l2x0y0y1x1y2x2y3hx3h3212.3 机器人运动学方程2.3.1 运动学方程建立步骤解：（3）相邻杆件位姿矩阵（第一种）l1l3l2x0y0y1x1y2x2y3hx3h3212.3 机器人运动学方程2.3.1 运动学方程建立步骤解：（3）相邻杆件位姿矩阵（第一种）l1l3l2x0y0y1x1y2x2y3hx3h3212.3 机器人运动学方程2.3.1 运动学方程建立步骤解：（3）相邻杆件位姿矩阵（第一种）l1l3l2x0y0y1x1y2x2y3hx3h3212.3 机器

27、人运动学方程2.3.1 运动学方程建立步骤解：（4）建立方程（第一种）将相邻杆件位姿矩阵依次相乘，则有： 2.3 机器人运动学方程2.3.1 运动学方程建立步骤解：（4）建立方程（第一种）若用矩阵形式表示，则为： 2.3 机器人运动学方程2.3.1 运动学方程建立步骤解：（4）建立方程（第一种）若用方程组形式表示，则为： 2.3 机器人运动学方程2.3.1 运动学方程建立步骤解：（1）建立坐标系(第二种) a、机座坐标系0 b、杆件坐标系i c、手部坐标系h （与末端杆件坐 标系n方向 一致） l1l3l2x0y0y1x1y2x2y3x3yhxh2.3 机器人运动学方程2.3.1 运动学方程建

28、立步骤l1l3l2x0y0y1x1y2x2y3x3yhxh3212.3 机器人运动学方程2.3.1 运动学方程建立步骤解：（2）确定参数（第二种）li-1相邻坐标系z轴之间的距离；i-1相邻坐标系z轴之间的夹角；di相邻坐标系x轴之间的距离；i相邻坐标系x轴之间的夹角。注意：根据方向确定参数的正负！解：（2）确定参数(第二种） i li-1i-1 dii qi 1 0 0 011 2 l1 0 022 3 l2 0 033l1l3l2x0y0y1x1y2x2y3x3yhxh3212.3 机器人运动学方程2.3.1 运动学方程建立步骤解：（3）相邻杆件位姿矩阵（第二种）l1l3l2x0y0y1x

29、1y2x2y3x3yhxh3212.3 机器人运动学方程2.3.1 运动学方程建立步骤解：（3）相邻杆件位姿矩阵（第二种）l1l3l2x0y0y1x1y2x2y3x3yhxh3212.3 机器人运动学方程2.3.1 运动学方程建立步骤解：（3）相邻杆件位姿矩阵（第二种）l1l3l2x0y0y1x1y2x2y3x3yhxh3212.3 机器人运动学方程2.3.1 运动学方程建立步骤解：（3）相邻杆件位姿矩阵（第二种）l1l3l2x0y0y1x1y2x2y3x3yhxh3212.3 机器人运动学方程2.3.1 运动学方程建立步骤解：（4）建立方程（第二种）将相邻杆件位姿矩阵依次相乘，则有： 2.3

30、 机器人运动学方程2.3.1 运动学方程建立步骤解：（4）建立方程（第二种）若用矩阵形式表示，则为： 2.3 机器人运动学方程2.3.1 运动学方程建立步骤解：（4）建立方程（第二种）若用方程组形式表示，则为： 2.3 机器人运动学方程2.3.1 运动学方程建立步骤回顾：运动学方程的模型： M0h=f(qi)， i=1，n正问题：已知关节变量qi的值，求手在空间的位姿M0h。逆问题：已知手在空间的位姿M0h，求关节变量qi的值。2.3.2 运动学方程的解2.3 机器人运动学方程1、运动学方程的正解 正问题：已知关节变量qi的值， 求手在空间的位姿M0h。 正解特征：唯一性。 用处：检验、校准机

31、器人。2.3 机器人运动学方程2.3.2 运动学方程的解2、运动学方程的逆解逆问题：已知手在空间的位姿M0h，求关节变量qi的值。逆解特征分三种情况：多解、唯一解、无解。多解的选择原则：最接近原则。计算方法：递推逆变换法，即2.3 机器人运动学方程2.3.2 运动学方程的解例：已知四轴平面关节SCARA机器人如图所示。试计算：（1）机器人的运动学方程；（2）当关节变量取qi=30，-60，120，90T时，机器人手部的位置和姿态；（3）机器人运动学逆解的数学表达式。 8004003002002.3 机器人运动学方程2.3.2 运动学方程的解解：（1）运动学方程a、建立坐标系（第一种） 机座坐标

32、系0 杆件坐标系i 手部坐标系hx0z0 x1z1x4hz4h800400300200 x2z2x3z32.3 机器人运动学方程2.3.2 运动学方程的解解：（1）运动学方程b、确定参数（第一种） i dii lii qi 1 8001400 01 2 02300 02 3 d3 0 0 0 d3 4-2004 0 04800400300200 x0z0 x1z1x2z2x3z3x4hz4h2.3 机器人运动学方程2.3.2 运动学方程的解解：（1）运动学方程c、相邻杆件位姿矩阵（第一种）2.3 机器人运动学方程2.3.2 运动学方程的解解：（1）运动学方程 c、相邻杆件位姿矩阵（第一种）2.

33、3 机器人运动学方程2.3.2 运动学方程的解解：（1）运动学方程 c、相邻杆件位姿矩阵（第一种）2.3 机器人运动学方程2.3.2 运动学方程的解解：（1）运动学方程 c、相邻杆件位姿矩阵（第一种）2.3 机器人运动学方程2.3.2 运动学方程的解解：（1）运动学方程d、建立方程（第一种） 2.3 机器人运动学方程2.3.2 运动学方程的解解：（2）已知qi=30，-60，120，90T， 代入（1）中的运动学方程，则得：2.3 机器人运动学方程2.3.2 运动学方程的解解：（3）逆解数学表达式已知运动学方程，用通式表示为：2.3 机器人运动学方程2.3.2 运动学方程的解解：（3）逆解数学

34、表达式 联立方程：2.3 机器人运动学方程2.3.2 运动学方程的解解：（3）逆解数学表达式由上面（a）、（b）两式可得 ：2.3 机器人运动学方程2.3.2 运动学方程的解解：（3）逆解数学表达式由上面（c）、（d）两式平方再相加可得 ：2.3 机器人运动学方程2.3.2 运动学方程的解解：（3）逆解数学表达式由上面（c）、（d）两式展开可得 ：2.3 机器人运动学方程2.3.2 运动学方程的解解：（3）逆解数学表达式由上面两式可得 ：2.3 机器人运动学方程2.3.2 运动学方程的解解：（3）逆解数学表达式由上面两式可得 ：2.3 机器人运动学方程2.3.2 运动学方程的解解：（3）逆解数学表达式已知1，2可得 ：2.3 机器人运动学方程2.3.2 运动学方程的解解：（3）逆解数学表达式最后由（e)式可得 ：2.3 机器人运动学方程2.3.2 运动学方程的解解：（3）逆解数学表达式2.3 机器人运动学方程2.3.2 运动学方程的解2.4.1 微分变换2.4.2 雅可比矩阵2.4 机器人微分运动 设机器人运动链中某一杆件相对于机座坐标系的位姿为 ，经过微运动后该杆件的位姿变为 ，若位姿是某个变量q的函数，则： 若位姿是若干个变量的函数，则： 2.4.1 微分变换