机器人运动学课件

运动学研究的问题运动学研究的问题：手在空间的运动手在空间的运动与与各个各个关节的运动关节的运动之间的关系。之间的关系。正问题正问题：已知关节运动，已知关节运动，求手的运动。求手的运动。逆问题逆问题：已知手的运动，已知手的运动，求关节运动。求关节运动。运动学研究的问题：数学模型数学模型：手的运动手的运动位姿变化位姿变化位姿矩阵位姿矩阵M 关节运动关节运动参数变化参数变化关节变量关节变量qi，i=1，n运动学方程运动学方程：M=f(qi)，i=1，n正问题正问题：已知：已知qi，求，求M。逆问题逆问题：已知：已知M，求，求qi。数学模型：2.1 2.1 机器人的位姿描述机器人的位姿描述2.2 2.2 齐次变换及运算齐次变换及运算2.3 2.3 机器人运动学方程机器人运动学方程2.4 2.4 机器人微分运动机器人微分运动 习题习题2.1 机器人的位姿描述2.1.1 机器人位姿的表示机器人位姿的表示2.1.2 机器人的坐标系机器人的坐标系2.1 2.1 机器人的位姿描述机器人的位姿描述2.1.1 机器人位姿的表示2.1 机器人的位姿描述2.1.1 机器人位姿的表示机器人位姿的表示机器人的位姿主要是机器人的位姿主要是指机器人手部在空间的位指机器人手部在空间的位置和姿态，有时也会用到置和姿态，有时也会用到其它各个活动杆件在空间其它各个活动杆件在空间的位置和姿态。的位置和姿态。2.1 2.1 机器人的位姿描述机器人的位姿描述2.1.1 机器人位姿的表示2.1 机器人的位姿描述2.1.1 机器人位姿的表示机器人位姿的表示位置位置可以用一个可以用一个31的位置矩阵来描述。的位置矩阵来描述。(,)2.1 2.1 机器人的位姿描述机器人的位姿描述2.1.1 机器人位姿的表示(,)2.1 2.1.1 机器人位姿的表示机器人位姿的表示姿态姿态可以用坐标系可以用坐标系三个坐标轴两两夹角的三个坐标轴两两夹角的余弦值组成余弦值组成33的姿态的姿态矩阵来描述。矩阵来描述。(,)hhhh2.1 2.1 机器人的位姿描述机器人的位姿描述2.1.1 机器人位姿的表示(,)h2.1.1 机器人位姿的表示机器人位姿的表示 例：右图所示两坐例：右图所示两坐标系的姿态为：标系的姿态为：000011112.1 2.1 机器人的位姿描述机器人的位姿描述2.1.1 机器人位姿的表示00001112.1.2 机器人的坐标系机器人的坐标系手部坐标系手部坐标系参考机器人手部的坐标系，也称机参考机器人手部的坐标系，也称机器人位姿坐标系，它表示机器人手部在指定坐标系中器人位姿坐标系，它表示机器人手部在指定坐标系中的位置和姿态。的位置和姿态。机座坐标系机座坐标系参考机器人机座的坐标系，它是机参考机器人机座的坐标系，它是机器人各活动杆件及手部的公共参考坐标系。器人各活动杆件及手部的公共参考坐标系。杆件坐标系杆件坐标系参考机器人指定杆件的坐标系，它参考机器人指定杆件的坐标系，它是在机器人每个活动杆件上固定的坐标系，随杆件的是在机器人每个活动杆件上固定的坐标系，随杆件的运动而运动。运动而运动。绝对坐标系绝对坐标系参考工作现场地面的坐标系，它是参考工作现场地面的坐标系，它是机器人所有构件的公共参考坐标系。机器人所有构件的公共参考坐标系。2.1 2.1 机器人的位姿描述机器人的位姿描述2.1.2 机器人的坐标系2.1 机器人的位姿描述2.1.2 机器人的坐标系机器人的坐标系手部坐标系手部坐标系h机座坐标系机座坐标系0 杆件坐标系杆件坐标系i i=1，n绝对坐标系绝对坐标系B 2.1 2.1 机器人的位姿描述机器人的位姿描述2.1.2 机器人的坐标系2.1 机器人的位姿描述2.2.1 直角坐标变换直角坐标变换2.2.2 齐次坐标变换齐次坐标变换2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换2.2 齐次变换及运算2.2.1 直角坐标变换直角坐标变换iiiijjjj坐标之间的变换关系：坐标之间的变换关系：平移变换平移变换旋转变换旋转变换2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换iiiijjjj坐标1、平移变换、平移变换 设坐标系设坐标系i和坐标系和坐标系j具有相同的姿态，但它俩具有相同的姿态，但它俩的坐标原点不重合，若用的坐标原点不重合，若用 矢量表示坐标系矢量表示坐标系i和坐标和坐标系系j原点之间的矢量，则坐标系原点之间的矢量，则坐标系j就可以看成是由坐就可以看成是由坐标系标系i沿矢量沿矢量 平移变换而来的，所以称矢量平移变换而来的，所以称矢量 为为平平移变换矩阵移变换矩阵，它是一个，它是一个31的矩阵，即：的矩阵，即：iiiijjjj2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换1、平移变换iiiijjjj2.2 齐次变换1、平移变换、平移变换 若空间有一点在坐标系若空间有一点在坐标系i和坐标系和坐标系j中分别用矢中分别用矢量量 和和 表示，则它们之间有以下关系：表示，则它们之间有以下关系：称上式为称上式为坐标平移方程坐标平移方程。iiiijjjj2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换1、平移变换iiiijjjj2.2 齐次变换2、旋转变换、旋转变换 设坐标系设坐标系i和坐标系和坐标系j的的原点重合，但它俩的姿态不同。原点重合，但它俩的姿态不同。则坐标系则坐标系j就可以看成是由坐就可以看成是由坐标系标系i旋转变换而来的，旋转旋转变换而来的，旋转变换矩阵比较复杂，最简单的变换矩阵比较复杂，最简单的是绕一根坐标轴的旋转变换。是绕一根坐标轴的旋转变换。下面以此来对旋转变换矩阵作下面以此来对旋转变换矩阵作以说明。以说明。iiiijjjj2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换2、旋转变换iiiijjjj2.2 齐次变换2、旋转变换、旋转变换绕绕z轴旋转轴旋转角角 坐标系坐标系i和坐标系和坐标系j的原点重合，坐标系的原点重合，坐标系j的的坐标轴方向相对于坐标系坐标轴方向相对于坐标系i绕轴旋转了一个绕轴旋转了一个角。角。角的正负一般按右角的正负一般按右手法则确定，即由手法则确定，即由z轴的轴的矢端看，逆时钟为正矢端看，逆时钟为正。iiiijjjj2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换2、旋转变换iiiijjjj2.2 齐次2、旋转变换、旋转变换绕绕z轴旋转轴旋转角角变换矩阵推导变换矩阵推导 若空间有一点若空间有一点p，则其，则其在坐标系在坐标系i和坐标系和坐标系j中中的坐标分量之间就有以下关系：的坐标分量之间就有以下关系：iiiijjjj2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换2、旋转变换iiiijjjj2.2 齐次变2、旋转变换、旋转变换绕绕z轴旋转轴旋转角角 若补齐所缺的有些项，再作适当变形，则有：若补齐所缺的有些项，再作适当变形，则有：2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换2、旋转变换2.2 齐次变换及运算2.2.1 直角坐标变换2、旋转变换、旋转变换绕绕z轴旋转轴旋转角角 将上式写成矩阵的形式，则有：将上式写成矩阵的形式，则有：2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换2、旋转变换2.2 齐次变换及运算2.2.1 直角坐标变换2、旋转变换、旋转变换绕绕z轴旋转轴旋转角角 再将其写成矢量形式，则有：再将其写成矢量形式，则有：称上式为称上式为坐标旋转方程坐标旋转方程，式中：，式中：p点在坐标系点在坐标系i中的坐标列阵（矢量）；中的坐标列阵（矢量）；p点在坐标系点在坐标系j中的坐标列阵（矢量）；中的坐标列阵（矢量）；坐标系坐标系j变换到坐标系变换到坐标系i的的旋转变换矩阵旋转变换矩阵，也称为也称为方向余弦矩阵方向余弦矩阵。2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换2、旋转变换2.2 齐次变换及运算2.2.1 直角坐标变换2、旋转变换、旋转变换绕绕z轴旋转轴旋转角角 旋转变换矩阵，也称为方向余弦矩阵，旋转变换矩阵，也称为方向余弦矩阵，是一个是一个33的矩阵，其中的每个元素就是坐标系的矩阵，其中的每个元素就是坐标系i和和坐标系坐标系j相应坐标轴夹角的余弦值，它表明坐标系相应坐标轴夹角的余弦值，它表明坐标系j相对于坐标系相对于坐标系i的姿态（方向）。的姿态（方向）。2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换2、旋转变换2.2 齐次变换及运算2.2.1 直角坐标变换2、旋转变换、旋转变换绕绕z轴旋转轴旋转角角 旋转变换矩阵旋转变换矩阵：2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换iiiijjjj2、旋转变换2.2 齐次变换及运算2.2.1 直角坐标变换2、旋转变换、旋转变换绕绕x轴旋转轴旋转角的角的 旋转变换矩阵为：旋转变换矩阵为：iiiijjjj2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换2、旋转变换iiiijjjj2.2 齐次2、旋转变换、旋转变换绕绕y轴旋转轴旋转角的角的 旋转变换矩阵为：旋转变换矩阵为：iiiijjjj2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换2、旋转变换iiiijjjj2.2 齐次2、旋转变换、旋转变换旋转旋转变换矩阵的逆矩阵变换矩阵的逆矩阵 旋转变换矩阵的逆矩阵既可以用旋转变换矩阵的逆矩阵既可以用线性代数的方法线性代数的方法求求出，也可以用出，也可以用逆向的坐标变换逆向的坐标变换求出。求出。以绕以绕z轴旋转轴旋转角为例，其角为例，其逆向变换即为绕逆向变换即为绕z轴旋转轴旋转-角角，则其旋转变换矩阵就为：，则其旋转变换矩阵就为：2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换2、旋转变换2.2 齐次变换及运算2.2.1 直角坐标变换2、旋转变换、旋转变换旋转旋转变换矩阵的逆矩阵变换矩阵的逆矩阵 比较以下两式：比较以下两式：结论结论：2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换2、旋转变换2.2 齐次变换及运算2.2.1 直角坐标变换3、联合变换、联合变换 设坐标系设坐标系i和坐标系和坐标系j之间存在先平移变换，之间存在先平移变换，后旋转变换，则空间任一点在坐标系后旋转变换，则空间任一点在坐标系i和坐标系和坐标系j中的矢量之间就有以下关系：中的矢量之间就有以下关系：称上式为直角坐标系中的坐标称上式为直角坐标系中的坐标联合变换方程联合变换方程。2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换3、联合变换2.2 齐次变换及运算2.2.1 直角坐标变换例例：已知坐标系：已知坐标系B的初始位置与坐标系的初始位置与坐标系A重合，首重合，首 先坐标系先坐标系B沿坐标系沿坐标系A的的x轴移动轴移动12个单位，个单位，并沿坐标系并沿坐标系A的的y轴移动轴移动6个单位，再绕坐标系个单位，再绕坐标系 A的的z轴旋转轴旋转30，求平移变换矩阵和旋转变换，求平移变换矩阵和旋转变换 矩阵。假设某点在坐标系矩阵。假设某点在坐标系B中的矢量为：中的矢量为：，求该点在坐标系，求该点在坐标系A中的矢量？中的矢量？2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换例：已知坐标系B的初始位置与坐标系A重合，首2.2 解解：由题意：由题意可得平移变换矩阵和旋转变换矩阵分别为：可得平移变换矩阵和旋转变换矩阵分别为：则：则：2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换解：由题意可得平移变换矩阵和旋转变换矩阵分别为：2.2 齐次3、联合变换、联合变换 若坐标系若坐标系i和坐标系和坐标系j之间是先旋转变换，后平之间是先旋转变换，后平移变换，则上述关系是应如何变化？移变换，则上述关系是应如何变化？问题：问题：当坐当坐标系之系之间存在多次存在多次变换时，直角坐，直角坐标变换就无就无法用同一法用同一规整的表达式表示了！整的表达式表示了！2.2 2.2 齐次次变换及运算及运算2.2.1 直角坐标变换直角坐标变换3、联合变换2.2 齐次变换及运算2.2.1 直角坐标变换2.2.2 齐次坐标变换齐次坐标变换1 1、齐次坐标的定义、齐次坐标的定义 空间中任一点在直角坐标系中的三个坐标分量用空间中任一点在直角坐标系中的三个坐标分量用 表示，若有四个不同时为零的数表示，若有四个不同时为零的数与三个直角坐标分量之间存在以下关系：与三个直角坐标分量之间存在以下关系：2.2 2.2 齐次次变换及运算及运算则称则称 是空间该点的是空间该点的齐次坐标齐次坐标。2.2.2 齐次坐标变换1、齐次坐标的定义2.2 齐次变换及1 1、齐次坐标的定义、齐次坐标的定义齐次坐标的几点说明：齐次坐标的几点说明：.空间中的任一点都可用齐次坐标表示；空间中的任一点都可用齐次坐标表示；.空间中的任一点的直角坐标是单值的，但其对应的空间中的任一点的直角坐标是单值的，但其对应的齐次坐标是多值的；齐次坐标是多值的；.k是比例坐标，它表示直角坐标值与对应的齐次坐标是比例坐标，它表示直角坐标值与对应的齐次坐标值之间的比例关系；值之间的比例关系；.若比例坐标若比例坐标k=1，则空间任一点，则空间任一点(x,y,z)的齐次坐标的齐次坐标为为(x,y,z,1)，以后用到齐次坐标时，一律默认，以后用到齐次坐标时，一律默认k=1。2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换1、齐次坐标的定义2.2 齐次变换及运算2.2.2 齐次坐标2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）若坐标系若坐标系j是是i先沿矢量先沿矢量平移，再绕平移，再绕z轴旋转轴旋转角得到的，则空间任一点在坐标角得到的，则空间任一点在坐标系系i和坐标系和坐标系j中的矢量和对应的变换矩阵之间就中的矢量和对应的变换矩阵之间就有有 ，写成矩阵形式则为：，写成矩阵形式则为：2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）若坐标系j是i2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）再用坐标分量等式表示，则有：再用坐标分量等式表示，则有：2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）引入齐次坐标，补齐所缺各项，再适当变形，则有：引入齐次坐标，补齐所缺各项，再适当变形，则有：2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）再将其写成矩阵形式则有：再将其写成矩阵形式则有：2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）由此可得联合变换的齐次坐标方程为：由此可得联合变换的齐次坐标方程为：2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换齐次坐标变换矩阵齐次坐标变换矩阵，它是一个它是一个44的矩阵。的矩阵。2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）齐次坐标变换矩阵的意义齐次坐标变换矩阵的意义若将齐次坐标变换矩阵分块，则有：若将齐次坐标变换矩阵分块，则有：2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）齐次坐标变换矩阵的意义齐次坐标变换矩阵的意义意义意义：左上角的左上角的33矩阵是两个坐标系之间的矩阵是两个坐标系之间的旋转变换矩旋转变换矩阵，它描述了姿态关系阵，它描述了姿态关系。右上角的右上角的31矩阵是两个坐标系之间的矩阵是两个坐标系之间的平移变换矩平移变换矩阵，它描述了位置关系阵，它描述了位置关系。所以所以齐次坐次坐标变换矩矩阵又称又称为位姿矩阵位姿矩阵。2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）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.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）单独的平移或旋转齐次坐标变换矩阵单独的平移或旋转齐次坐标变换矩阵 平移变换的齐次矩阵为：平移变换的齐次矩阵为：2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）单独的平移或旋转齐次坐标变换矩阵单独的平移或旋转齐次坐标变换矩阵 旋转变换的齐次矩阵为：旋转变换的齐次矩阵为：2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-HD-H矩阵）矩阵）单独的平移或旋转齐次坐标变换矩阵单独的平移或旋转齐次坐标变换矩阵 同理可得：同理可得：2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）联合变换与联合变换与单步齐次变换矩阵的关系单步齐次变换矩阵的关系观察以下三个齐次变换矩阵的关系：观察以下三个齐次变换矩阵的关系：2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）联合变换与联合变换与单步齐次变换矩阵的关系单步齐次变换矩阵的关系经观察可得：经观察可得：2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）联合变换与联合变换与单步齐次变换矩阵的关系单步齐次变换矩阵的关系 任何一个任何一个齐次坐标变换矩阵齐次坐标变换矩阵均可分解为一个均可分解为一个平移变平移变换矩阵换矩阵与一个与一个旋转变换矩阵旋转变换矩阵的的乘积乘积，即：，即：2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）联合变换与联合变换与单步齐次矩阵的关系单步齐次矩阵的关系 当空间有当空间有n个坐标系时，若已知相个坐标系时，若已知相邻坐标系之间的齐次变换矩阵，则：邻坐标系之间的齐次变换矩阵，则：由此可知，建立机器人的坐标系，由此可知，建立机器人的坐标系，将机器人手部在空间的位姿用齐次坐标将机器人手部在空间的位姿用齐次坐标变换矩阵描述出来，从而建立机器人的变换矩阵描述出来，从而建立机器人的运动学方程。运动学方程。0i-1in2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）0i-1in22 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）相对变换相对变换 坐标系之间多步齐次变换矩阵等于每次单独变换坐标系之间多步齐次变换矩阵等于每次单独变换的齐次变换矩阵的乘积，而相对变换则决定这些矩阵的齐次变换矩阵的乘积，而相对变换则决定这些矩阵相乘的顺序，其分为相乘的顺序，其分为左乘左乘和和右乘右乘：.若坐标系之间的变换是始终相对于原来的参若坐标系之间的变换是始终相对于原来的参考坐标系，则齐次坐标变换矩阵考坐标系，则齐次坐标变换矩阵左乘左乘；.若坐标系之间的变换是相对于当前新的坐标若坐标系之间的变换是相对于当前新的坐标系，则齐次坐标变换矩阵系，则齐次坐标变换矩阵右乘右乘。2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）相对变换相对变换 例例：已知坐标系：已知坐标系B是绕坐标系是绕坐标系A的的zA轴旋转轴旋转90，再绕，再绕A的的xA轴旋转轴旋转90，最后沿矢量：，最后沿矢量：平移得到的，求坐标系平移得到的，求坐标系A与坐标系与坐标系B之间的齐次坐标变换矩阵。之间的齐次坐标变换矩阵。2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）相对变换相对变换解解：由题意可知满足左乘原则，即有：由题意可知满足左乘原则，即有：2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）相对变换相对变换解解：若满足右乘原则，则有：若满足右乘原则，则有：2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）逆变换逆变换 已知已知i通过先平移，通过先平移，后旋转变成后旋转变成j，则变换，则变换矩阵为：矩阵为：iiiijjjj2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）iiiijjj2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）逆变换逆变换逆变换时：逆变换时：变换顺序颠倒变换顺序颠倒 先平移，后旋转先平移，后旋转先旋转，后平移先旋转，后平移变换参数取反变换参数取反 旋转（旋转（）（-）平移（平移（px，py，pz）（-px，-py，-pz）2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）逆变换逆变换 则则j到到i的变换矩阵为：的变换矩阵为：iiiijjjj2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）iiiijjj2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）逆变换逆变换2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）逆变换逆变换2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）逆变换逆变换 若齐次变换矩阵为：若齐次变换矩阵为：则：则：2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换 2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算22 2、齐次变换矩阵（、齐次变换矩阵（D-H矩阵）矩阵）逆变换逆变换 若齐次变换矩阵为：若齐次变换矩阵为：2.2 2.2 齐次次变换及运算及运算2.2.2 齐次坐标变换齐次坐标变换2、齐次变换矩阵（D-H矩阵）2.2 齐次变换及运算2.2.2.3.1 运动学方程建立步骤运动学方程建立步骤 1 1、建立坐标系、建立坐标系 2 2、确定参数、确定参数 3 3、相邻杆件的位姿矩阵、相邻杆件的位姿矩阵 4 4、建立方程、建立方程2.3.2 运动学方程的解运动学方程的解2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤2.3 机器人运动学方程2.3.1 运动学方程建立步骤运动学方程建立步骤回顾：回顾：运动学方程的模型：运动学方程的模型：M=f(qi)，i=1，n M机器人手在空间的位姿机器人手在空间的位姿 qi机器人各个关节变量机器人各个关节变量2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤回顾：运动学方程的模型：2.31 1、建立坐标系、建立坐标系 机座坐标系机座坐标系0 杆件坐标系杆件坐标系i i=1，2，n 手部坐标系手部坐标系h注意：注意：杆件编号杆件编号关节编号关节编号2.3 2.3 机器人运机器人运动学方程学方程oh0123关关节1关关节2关关节3x1z1o1Zhxhx0z0o0z3x3o3y2x2o22.3.1 运动学方程建立步骤运动学方程建立步骤1、建立坐标系2.3 机器人运动学方程oh0123关节1关节1 1、建立坐标系、建立坐标系机座坐标系机座坐标系0建立原则：建立原则：z轴垂直，轴垂直，x轴水平，轴水平，方向指向手部所在平面。方向指向手部所在平面。2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤x0z0o01、建立坐标系2.3 机器人运动学方程2.3.1 运动学方程1 1、建立坐标系、建立坐标系杆件坐标系杆件坐标系i，i=1，2，n 建立原则：建立原则：z轴与关节轴线重合，轴与关节轴线重合，x轴与两关节轴线的距离重轴与两关节轴线的距离重合，方向指向下一个杆件。合，方向指向下一个杆件。杆件坐标系有两种：杆件坐标系有两种：第一种第一种：z轴与轴与i+1关节轴线重合关节轴线重合 第二种第二种：z轴与轴与i关节轴线重合关节轴线重合2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤1、建立坐标系2.3 机器人运动学方程2.3.1 运动学方程1 1、建立坐标系、建立坐标系杆件坐标系杆件坐标系i第一种坐标系：第一种坐标系：z轴与轴与i+1关节关节轴线重合。轴线重合。x0z0o00123关关节1关关节2关关节3z2x2o2x1y1o1z3x3o32.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤1、建立坐标系x0z0o00123关节1关节2关节3z2x21 1、建立坐标系、建立坐标系杆件坐标系杆件坐标系i第二种坐标系：第二种坐标系：z轴与轴与i关节轴线关节轴线重合。重合。2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤x0z0o00123关关节1关关节2关关节3x1z1o1y2x2o2z3x3o31、建立坐标系2.3 机器人运动学方程2.3.1 运动学方程x0z0o00123关关节1关关节2关关节3z2x2o2x1y1o1z3x3o31 1、建立坐标系、建立坐标系手部坐标系手部坐标系h 在第一种杆件坐在第一种杆件坐标系下，标系下，h与末端与末端杆件坐标系杆件坐标系n重合。重合。zhxhoh2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤x0z0o00123关节1关节2关节3z2x2o2x1y1o1 1、建立坐标系、建立坐标系手部坐标系手部坐标系h 在第二种杆件坐在第二种杆件坐标系下，标系下，h建立在建立在手部中心，方向手部中心，方向与末与末端杆件端杆件坐标系坐标系n保保持一致。持一致。2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤x0z0o00123关关节1关关节2关关节3x1z1o1y2x2o2z3x3o3ohZhxh1、建立坐标系2.3 机器人运动学方程2.3.1 运动学方程2 2、确定参数、确定参数杆件几何参数（不变）杆件几何参数（不变）I I、杆件长度、杆件长度li：两关节轴线的距离两关节轴线的距离。IIII、杆件扭角、杆件扭角i：两关节轴线的夹角两关节轴线的夹角。ilii2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤2、确定参数ilii2.3 机器人运动学方程2.3.1 运2 2、确定参数、确定参数关节运动参数关节运动参数I I、关节平移量、关节平移量di：相邻杆件的长度相邻杆件的长度在关节轴线上的距离在关节轴线上的距离。IIII、关节回转量、关节回转量i：相邻杆件的长度相邻杆件的长度在关节轴线上的夹角在关节轴线上的夹角。ili-1i-1lii关节关节idi2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤2、确定参数ili-1i-1lii关节idi2.3 机器人2 2、确定参数、确定参数关节运动参数关节运动参数关节变量：关节变量：di平移关节；平移关节；i回转关节。回转关节。ili-1i-1lii关节关节idi2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤2、确定参数ili-1i-1lii关节idi2.3 机器人3 3、相邻杆件位姿矩阵、相邻杆件位姿矩阵第一种坐标系第一种坐标系 建立坐标系建立坐标系i-1、i。试分析分析i-1i的的变换过程程!ii-1关节关节iXi-1Z i-1Oi-1XiZiOi2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤3、相邻杆件位姿矩阵ii-1关节iXi-1Z i-1Oi-13 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 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤cbad3、相邻杆件位姿矩阵ii-1lii关节idiXi-1Z i3 3、相邻杆件位姿矩阵、相邻杆件位姿矩阵第一种坐标系第一种坐标系 II、单步齐次变换矩阵、单步齐次变换矩阵2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤a、Trans（0，0，di）b、Rot（z，i）3、相邻杆件位姿矩阵2.3 机器人运动学方程2.3.1 运动3 3、相邻杆件位姿矩阵、相邻杆件位姿矩阵第一种坐标系第一种坐标系 II、单步齐次变换矩阵、单步齐次变换矩阵2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤c、Trans（li，0，0）d、Rot（x，i）3、相邻杆件位姿矩阵2.3 机器人运动学方程2.3.1 运动3 3、相邻杆件位姿矩阵、相邻杆件位姿矩阵第一种坐标系第一种坐标系 III、相邻杆件的位姿矩阵、相邻杆件的位姿矩阵2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤3、相邻杆件位姿矩阵2.3 机器人运动学方程2.3.1 运动3 3、相邻杆件位姿矩阵、相邻杆件位姿矩阵第一种坐标系第一种坐标系 III、相邻杆件的位姿矩阵、相邻杆件的位姿矩阵2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤3、相邻杆件位姿矩阵2.3 机器人运动学方程2.3.1 运动3 3、相邻杆件位姿矩阵、相邻杆件位姿矩阵第一种坐标系第一种坐标系注意注意：特例！：特例！2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤3、相邻杆件位姿矩阵2.3 机器人运动学方程2.3.1 运动3 3、相邻杆件位姿矩阵、相邻杆件位姿矩阵第二种坐标系第二种坐标系 建立坐标系建立坐标系i-1、i。试分析分析i-1i的的变换过程程！ii-1关节关节iXi-1Z i-1Oi-1XiZiOi2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤3、相邻杆件位姿矩阵ii-1关节iXi-1Z i-1Oi-13 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 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤cbad3、相邻杆件位姿矩阵ili-1i-1i关节idiXi-1Z3 3、相邻杆件位姿矩阵、相邻杆件位姿矩阵第二种坐标系第二种坐标系 II、单步齐次变换矩阵、单步齐次变换矩阵2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤a、Trans（li-1，0，0）b、Rot（x，i-1）3、相邻杆件位姿矩阵2.3 机器人运动学方程2.3.1 运动3 3、相邻杆件位姿矩阵、相邻杆件位姿矩阵第二种坐标系第二种坐标系 II、单步齐次变换矩阵、单步齐次变换矩阵2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤c、Trans（0，0，di）d、Rot（z，i）3、相邻杆件位姿矩阵2.3 机器人运动学方程2.3.1 运动3 3、相邻杆件位姿矩阵、相邻杆件位姿矩阵第二种坐标系第二种坐标系 III、相邻杆件的位姿矩阵、相邻杆件的位姿矩阵2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤3、相邻杆件位姿矩阵2.3 机器人运动学方程2.3.1 运动3 3、相邻杆件位姿矩阵、相邻杆件位姿矩阵第二种坐标系第二种坐标系 III、相邻杆件的位姿矩阵、相邻杆件的位姿矩阵2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤3、相邻杆件位姿矩阵2.3 机器人运动学方程2.3.1 运动4 4、建立方程、建立方程2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤4、建立方程2.3 机器人运动学方程2.3.1 运动学方程建例：例：已知三自由度平面关节机器人如图所示。已知三自由度平面关节机器人如图所示。设机器人杆件设机器人杆件1、2、3的长度为的长度为l1，l2，l3。试建立。试建立机器人的运动学方程。机器人的运动学方程。l1l3l22.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤例：已知三自由度平面关节机器人如图所示。l1l3l22.3 解解：（1）建立坐标系建立坐标系(第一种第一种)a、机座坐标系、机座坐标系0 b、杆件坐标系、杆件坐标系i c、手部坐标系、手部坐标系h （与末端杆件坐（与末端杆件坐 标系标系n重合）重合）l1l3l2x0y0y1x1y2x2y3hx3h2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（1）建立坐标系(第一种)l1l3l2x0y0y1x1y解解：（2）确定参数（第一种）确定参数（第一种）di相相邻坐坐标系系x轴之之间的距离；的距离；i相相邻坐坐标系系x轴之之间的的夹角；角；li相相邻坐坐标系系z轴之之间的距离；的距离；i相相邻坐坐标系系z轴之之间的的夹角。角。注意：注意：根据方向确定参数的正根据方向确定参数的正负！l1l3l2x0y0y1x1y2x2y3hx3h3212.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（2）确定参数（第一种）l1l3l2x0y0y1x1y2解解：（2）确定参数（第一种）确定参数（第一种）i dii lii qi 1 01 l1 01 2 02 l2 02 3 03 l3 03l1l3l2x0y0y1x1y2x2y3hx3h3212.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（2）确定参数（第一种）i dii lii 解解：（3）相邻杆件位姿矩阵（第一种）相邻杆件位姿矩阵（第一种）l1l3l2x0y0y1x1y2x2y3hx3h3212.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（3）相邻杆件位姿矩阵（第一种）l1l3l2x0y0y1解解：（3）相邻杆件位姿矩阵（第一种）相邻杆件位姿矩阵（第一种）l1l3l2x0y0y1x1y2x2y3hx3h3212.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（3）相邻杆件位姿矩阵（第一种）l1l3l2x0y0y1解解：（3）相邻杆件位姿矩阵（第一种）相邻杆件位姿矩阵（第一种）l1l3l2x0y0y1x1y2x2y3hx3h3212.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（3）相邻杆件位姿矩阵（第一种）l1l3l2x0y0y1解解：（4）建立方程（第一种）建立方程（第一种）将相邻杆件位姿矩阵依次相乘，则有：将相邻杆件位姿矩阵依次相乘，则有：2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（4）建立方程（第一种）2.3 机器人运动学方程2.3.解解：（4）建立方程（第一种）建立方程（第一种）若用矩阵形式表示，则为：若用矩阵形式表示，则为：2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（4）建立方程（第一种）2.3 机器人运动学方程2.3.解解：（4）建立方程（第一种）建立方程（第一种）若用方程组形式表示，则为：若用方程组形式表示，则为：2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（4）建立方程（第一种）2.3 机器人运动学方程2.3.解解：（1）建立坐标系建立坐标系(第二种第二种)a、机座坐标系、机座坐标系0 b、杆件坐标系、杆件坐标系i c、手部坐标系、手部坐标系h （与末端杆件坐（与末端杆件坐 标系标系n方向方向 一致）一致）l1l3l2x0y0y1x1y2x2y3x3yhxh2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（1）建立坐标系(第二种)l1l3l2x0y0y1x1yl1l3l2x0y0y1x1y2x2y3x3yhxh3212.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解解：（2）确定参数（第二种）确定参数（第二种）li-1相相邻坐坐标系系z轴之之间的距离；的距离；i-1相相邻坐坐标系系z轴之之间的的夹角；角；di相相邻坐坐标系系x轴之之间的距离；的距离；i相相邻坐坐标系系x轴之之间的的夹角。角。注意：注意：根据方向确定参数的正根据方向确定参数的正负！l1l3l2x0y0y1x1y2x2y3x3yhxh32解解：（2）确定参数确定参数(第二种）第二种）i li-1i-1 dii qi 1 0 0 011 2 l1 0 022 3 l2 0 033l1l3l2x0y0y1x1y2x2y3x3yhxh3212.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（2）确定参数(第二种）i li-1i-1 d解解：（3）相邻杆件位姿矩阵（第二种）相邻杆件位姿矩阵（第二种）l1l3l2x0y0y1x1y2x2y3x3yhxh3212.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（3）相邻杆件位姿矩阵（第二种）l1l3l2x0y0y1解解：（3）相邻杆件位姿矩阵（第二种）相邻杆件位姿矩阵（第二种）l1l3l2x0y0y1x1y2x2y3x3yhxh3212.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（3）相邻杆件位姿矩阵（第二种）l1l3l2x0y0y1解解：（3）相邻杆件位姿矩阵（第二种）相邻杆件位姿矩阵（第二种）l1l3l2x0y0y1x1y2x2y3x3yhxh3212.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（3）相邻杆件位姿矩阵（第二种）l1l3l2x0y0y1解解：（3）相邻杆件位姿矩阵（第二种）相邻杆件位姿矩阵（第二种）l1l3l2x0y0y1x1y2x2y3x3yhxh3212.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（3）相邻杆件位姿矩阵（第二种）l1l3l2x0y0y1解解：（4）建立方程（第二种）建立方程（第二种）将相邻杆件位姿矩阵依次相乘，则有：将相邻杆件位姿矩阵依次相乘，则有：2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（4）建立方程（第二种）2.3 机器人运动学方程2.3.解解：（4）建立方程（第二种）建立方程（第二种）若用矩阵形式表示，则为：若用矩阵形式表示，则为：2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（4）建立方程（第二种）2.3 机器人运动学方程2.3.解解：（4）建立方程（第二种）建立方程（第二种）若用方程组形式表示，则为：若用方程组形式表示，则为：2.3 2.3 机器人运机器人运动学方程学方程2.3.1 运动学方程建立步骤运动学方程建立步骤解：（4）建立方程（第二种）2.3 机器人运动学方程2.3.回回顾：运动学方程的模型：运动学方程的模型：M0h=f(qi)，i=1，n正问题：正问题：已知关节变量已知关节变量qi的值，求手在空间的位姿的值，求手在空间的位姿M0h。逆问题：逆问题：已知手在空间的位姿已知手在空间的位姿M0h，求关节变量，求关节变量qi的值。的值。2.3.2 运动学方程的解运动学方程的解2.3 2.3 机器人运机器人运动学方程学方程回顾：2.3.2 运动学方程的解2.3 机器人运动学方程1 1、运动学方程的正解、运动学方程的正解 正问题：正问题：已知关节变量已知关节变量qi的值，的值，求手在空间的位姿求手在空间的位姿M0h。正解特征：正解特征：唯一性。唯一性。用处：用处：检验、校准机器人。检验、校准机器人。2.3 2.3 机器人运机器人运动学方程学方程2.3.2 运动学方程的解运动学方程的解1、运动学方程的正解2.3 机器人运动学方程2.3.2 运2 2、运动学方程的逆解、运动学方程的逆解逆问题：逆问题：已知手在空间的位姿已知手在空间的位姿M0h，求关节变量，求关节变量qi的值。的值。逆解特征分三种情况：逆解特征分三种情况：多解、唯一解、无解。多解、唯一解、无解。多解的选择原则：多解的选择原则：最接近原则。最接近原则。计算方法：计算方法：递推逆变换法，即递推逆变换法，即2.3 2.3 机器人运机器人运动学方程学方程2.3.2 运动学方程的解运动学方程的解2、运动学方程的逆解2.3 机器人运动学方程2.3.2 运例：例：已知四轴平面关节已知四轴平面关节SCARASCARA机器机器人如图所示。人如图所示。试计算：试计算：（1 1）机器人的运动学方程；）机器人的运动学方程；（2 2）当关节变量取）当关节变量取qi=30=30，-60-60，120120，9090T T时，机器人手部的位置和姿态；时，机器人手部的位置和姿态；（3 3）机器人运动学逆解的数学）机器人运动学逆解的数学表达式。表达式。8004003002002.3 2.3 机器人运机器人运动学方程学方程2.3.2 运动学方程的解运动学方程的解例：已知四轴平面关节SCARA机器8004003002002解：解：（1 1）运动学方程）运动学方程a a、建立坐标系（第一种）、建立坐标系（第一种）机座坐标系机座坐标系0 杆件坐标系杆件坐标系i 手部坐标系手部坐标系hx0z0 x1z1x4hz4h800400300200 x2z2x3z32.3 2.3 机器人运机器人运动学方程学方程2.3.2 运动学方程的解运动学方程的解解：（1）运动学方程x0z0 x1z1x4hz4h800400解解：（1 1）运动学方程）运动学方程b b、确定参数（第一种）、确定参数（第一种）i dii lii qi 1 800 1400 01 2 02300 02 3 d3 0 0 0 d3 4-2004 0 04800400300200 x0z0 x1z1x2z2x3z3x4hz4h2.3 2.3 机器人运机器人运动学方程学方程2.3.2 运动学方程的解运动学方程的解解：（1）运动学方程 i dii lii qi 解解：（1 1）运动学方程）运动学方程c c、相邻杆件位姿矩阵（第一种）、相邻杆件位姿矩阵（第一种）2.3 2.3 机器人运机器人运动学方程学方程2.3.2 运动学方程的解运动学方程的解解：（1）运动学方程2.3 机器人运动学方程2.3.2 运解解：（1 1）运动学方程）运动学方程 c c、相邻杆件位姿矩阵（第一种）、相邻杆件位姿矩阵（第一种）2.3 2.3 机器人运机器人运动学方程学方程2.3.2 运动学方程的解运动学方程的解解：（1）运动学方程 2.3 机器人运动学方程2.3解解：（1 1）运动学方程）运动学方程 c c、相邻杆件位姿矩阵（第一种）、相邻杆件位姿矩阵（第一种）2.3 2.3 机器人运机器人运动学方程学方程2.3.2 运动学方程的解运动学方程的解解：（1）运动学方程 2.3 机器人运动学方程2.3解解：（1 1）运动学方程）运动学方程 c c、相邻杆件位姿矩阵（第一种）、相邻杆件位姿矩阵（第一种）2.3 2.3 机器人运机器人运动学方程学方程2.3.2 运动学方程的解运动学方程的解解：（1）运动学方程 2.3 机器人运动学方程2.3解解：（1 1）运动学方程）运动学方程d d、建立方程（第一种）、建立方程（第一种）2.3 2.3 机器人运机器人运动学方程学方程2.3.2 运动学方程的解运动学方程的解解：（1）运动学方程2.3 机器人运动学方程2.3.2 运解解：（2 2）已知）已知qi=30，-60，120，90T，代入（代入（1）中的运动学方程，则得：）中的运动学方程，则得：2.3 2.3 机器人运机器人运动学方程学方程2.3.2 运动学方程的解运动学方程的解解：（2）已知qi=30，-60，120，90T解解：（3 3）逆解数学表达式）逆解数学表达式已知运动学方程，用通式表示为：已知运动学方程，用通式表示为：2.3 2.3 机器人运机器人运动学方程学方程2.3.2 运动学方程的解运动学方程的解解：（3）逆解数学表达式2.3 机器人运动学方程2.3.2 解解：（3 3）逆解数学表达式）逆解数学表达式 联立方程联立方程：2.3 2.3 机器人运机器人运动学方程学方程2.3.2 运动学方程的解运动学方程的解解：（3）逆解数学表达式 联立方程：2.3 机器人解解：（3 3）逆解数学表达式）逆解数学表达式由上面（由上面（a a）、（）、（b b）两式可得）两式可得