第3章 机器人运动学

1、1第三章第三章 机器人运动学机器人运动学 第一节第一节 概述概述第二节第二节 机器人的运动学基本问题机器人的运动学基本问题第三节第三节 机器人的雅可比矩阵机器人的雅可比矩阵 2第一节第一节 概述概述 常见的机器人运动学问题可归纳如下：常见的机器人运动学问题可归纳如下：1 1对一给定的机器人，已知杆件几何参数和关对一给定的机器人，已知杆件几何参数和关节角矢量求机器人末端执行器相对于参考坐标节角矢量求机器人末端执行器相对于参考坐标系的位置和姿态。系的位置和姿态。2 2已知机器人杆件的几何参数，给定机器人末已知机器人杆件的几何参数，给定机器人末端执行器相对于参考坐标系的期望位置和姿态端执行器相对于参

2、考坐标系的期望位置和姿态 ( (位姿位姿) )，机器人能否使其末端执行器达到这个，机器人能否使其末端执行器达到这个预期的位姿？如能达到，那么机器人有几种不预期的位姿？如能达到，那么机器人有几种不同形态可满足同样的条件同形态可满足同样的条件? ? 3 第一个问题常称为运动学正问题第一个问题常称为运动学正问题( (直接问题直接问题););第二个问题常称为运动学逆问题第二个问题常称为运动学逆问题( (解臂形问题解臂形问题) )。这两个问题是机器人运动学中的基本问题。这两个问题是机器人运动学中的基本问题。 4第二节第二节 机器人运动学的基本问题机器人运动学的基本问题 一、运动学基本问题一、运动学基本问

3、题 图图3 31 1所示为所示为2 2自由度机器人手部的连杆机构自由度机器人手部的连杆机构。 5 图中的连杆机构是两杆件通过转动副联接的关节结构，图中的连杆机构是两杆件通过转动副联接的关节结构，通过确定连杆长度，以及关节角，可以定义该连杆机构。通过确定连杆长度，以及关节角，可以定义该连杆机构。在分析机器人的末端手爪的运动时，若把作业看作主要依靠在分析机器人的末端手爪的运动时，若把作业看作主要依靠机器人手爪来实现的，则应考虑手爪的位置（图中点的位机器人手爪来实现的，则应考虑手爪的位置（图中点的位置）。一般场合中，手爪置）。一般场合中，手爪姿势也表示手指位置。从姿势也表示手指位置。从几何学的观点来

4、处理这个几何学的观点来处理这个手指位置与关节变量的关手指位置与关节变量的关系称为运动学（系称为运动学（KinematicsKinematics）。）。 6引入向量分别表示手爪位置和关节变量，引入向量分别表示手爪位置和关节变量， 因此，利用上述两个向量来描述一下因此，利用上述两个向量来描述一下这个这个2 2自由度机器人的运动学问题。自由度机器人的运动学问题。手爪位置的各分量，按几何学可表示为：手爪位置的各分量，按几何学可表示为：xry 1211212coscos()xLL11212sinsin()yLL(3-1)(3-2)7用向量表示这个关系式，其一般可表示为用向量表示这个关系式，其一般可表示为

5、 式中式中 表示向量函数。已知机器人的关节变量表示向量函数。已知机器人的关节变量 ，求其手爪位置的运动学问题称为正运动学（求其手爪位置的运动学问题称为正运动学（direct direct kinematicskinematics）。该公式被称为运动方程式。如果，）。该公式被称为运动方程式。如果，给定机器人的手爪位置，求为了到达这个预定的位给定机器人的手爪位置，求为了到达这个预定的位置，机器人的关节变量的运动学问题称为逆运动学置，机器人的关节变量的运动学问题称为逆运动学（inverse kinematicsinverse kinematics）。其运动方程式可以通过）。其运动方程式可以通过以下分

6、析得到。以下分析得到。 ( )rff(3-3)8如图所示，根据图中描述的几何学关系，可得如图所示，根据图中描述的几何学关系，可得 2221122sinarctan()arctan()cosLyxLL22221212()arccos2xyLLL L式中式中(3-4)(3-5)(3-6)9同样，如果用向量表示上述关系式，其一般可表同样，如果用向量表示上述关系式，其一般可表示为示为 1( )fr 如图所示，机器人到达给定的手爪位置如图所示，机器人到达给定的手爪位置 r有两个姿态满足要求，即图中的有两个姿态满足要求，即图中的 也是也是其解。其解。这时这时 和和 变成为另外的值。变成为另外的值。即逆运动

7、学的解不是惟一的，即逆运动学的解不是惟一的，可以有多个解。可以有多个解。 12(3-7)10二、机器人位置与关节变量的关系二、机器人位置与关节变量的关系 1 1表示方法表示方法 以手爪位置与关节变量之间的关系为例，以手爪位置与关节变量之间的关系为例，要想正确表示机器人的手爪位置和姿态，就要要想正确表示机器人的手爪位置和姿态，就要首先建立坐标系，如图首先建立坐标系，如图3 33 3所示，应分别定义所示，应分别定义固定机器人的基座和手爪的坐标系，这样才能固定机器人的基座和手爪的坐标系，这样才能很好地描述它们之间的位置和姿态之间的关系。很好地描述它们之间的位置和姿态之间的关系。 11图图3 33 3

8、 基准坐标系和手爪坐标系基准坐标系和手爪坐标系 基准坐标系基准坐标系,固定在基座上固定在基座上 BE手爪坐标系手爪坐标系 ，固定在手爪上，固定在手爪上 122姿态的变换矩阵姿态的变换矩阵 如图如图3 34 4所示，给出原点重合的两坐标系所示，给出原点重合的两坐标系 ()AAAAOX Y()BBBBOX Y则假设点则假设点 P 的位置的位置p 向量的向量的分量在两坐标系中分别表示分量在两坐标系中分别表示为为 AxAAyppp BxBByppp 13则从则从 向向 的变换为：的变换为：ABAT AATxxxBABAABAT AATyyypepeppRppepeApBpATxBAATyeRe 其中：

9、其中：它是从它是从 坐标向坐标进行位置向坐标向坐标进行位置向 量姿态变换量姿态变换的矩阵，称为姿态变换矩阵（或旋转矩阵）。的矩阵，称为姿态变换矩阵（或旋转矩阵）。 B14为了加深印象，现在分析如图为了加深印象，现在分析如图3 35 5所示坐标所示坐标系系 ，它是将，它是将 围绕围绕 Z Z轴沿正方向旋轴沿正方向旋转角转角 后构成的坐标系。后构成的坐标系。pp211000cossin0sincos222ZYXO 111ZYXO 图图35 两个坐标系的旋转坐标变换两个坐标系的旋转坐标变换因此，在坐标系因此，在坐标系 上表示的坐标上表示的坐标 与在将坐标系与在将坐标系 绕绕Z Z轴沿正方向旋转角轴沿

10、正方向旋转角得到的坐标系得到的坐标系 上表示的上表示的坐标坐标 之间，存在下列关系式：之间，存在下列关系式： p1111ZYXO 222ZYXO p2pp211000cossin0sincos15由上面知从由上面知从 坐标系向坐标系坐标系向坐标系 的坐标变换矩阵为：的坐标变换矩阵为： 111ZYXO 222ZYXO 1000cossin0sincos21R16 因为上述变换是把某一坐标系上表示的坐标，因为上述变换是把某一坐标系上表示的坐标，表示到另一坐标系中，因此有时也称它为坐标变表示到另一坐标系中，因此有时也称它为坐标变换。在该例子中是从换。在该例子中是从 坐标系向坐标系坐标系向坐标系 的坐

11、标变换，由于坐标系的坐标变换，由于坐标系 是是 围绕围绕Z轴旋转轴旋转角后构成的坐标系，则该角后构成的坐标系，则该坐标变换矩阵也可用坐标变换矩阵也可用 来表示来表示111ZYXO 222ZYXO 222ZYXO 111ZYXO )(zR1000cossin0sincos)(21RRz17 同理，上述例子中，当考虑围绕着同理，上述例子中，当考虑围绕着 x轴旋转时（设轴旋转时（设其旋转量其旋转量为），可得到如下关系式：为），可得到如下关系式： ppRpx221cossin0sincos0001)(另外，当围绕着轴另外，当围绕着轴y y旋转时（设其旋转量旋转时（设其旋转量为），可表示为），可表示为如

12、下关系式为如下关系式 ppRpy221cos0sin010sin0cos)(18可以验证可以验证 该矩阵为单位矩阵式中该矩阵为单位矩阵式中* *表示表示 x x、y y 、z z中的任中的任何一个。所以有下列等式成立何一个。所以有下列等式成立 在分析机器人运动时，当只用围绕一个轴旋转在分析机器人运动时，当只用围绕一个轴旋转不能表示时，可以通过围绕几个轴同时旋转的不能表示时，可以通过围绕几个轴同时旋转的组合方式进行表示。组合方式进行表示。 )(xR)(yR)(zR均满足均满足 100010001)()(TRRxTRR)()(1193 3齐次变换齐次变换 前面讨论了机器人在进行旋转运动前面讨论了机

13、器人在进行旋转运动时的坐标变换，一般来说，机器人的运时的坐标变换，一般来说，机器人的运动不仅是旋转运动，有时要做平行移动，动不仅是旋转运动，有时要做平行移动，或以上两种运动的合成，因此也应考虑或以上两种运动的合成，因此也应考虑平移运动时的坐标变换，即齐次变换。平移运动时的坐标变换，即齐次变换。20现在来看下图的两个坐标系，现在来看下图的两个坐标系，坐标系坐标系 是将坐标系是将坐标系 单独地平行移动单独地平行移动 后，再后，再进行适当地旋转得到的坐标系。进行适当地旋转得到的坐标系。2222ZYXO 1111ZYXO 0p21 这时，某一点这时，某一点P其在坐标系其在坐标系 和和 上的坐标分别为上

14、的坐标分别为p 1 、p2 ，可以认为，可以认为 p 1 是由是由p2旋转而进行坐标变换后，即乘以旋转坐标变换旋转而进行坐标变换后，即乘以旋转坐标变换 R，在加上表示平移的向量在加上表示平移的向量 p0而得到的，因此可写出而得到的，因此可写出下列表达式：下列表达式：1111ZYXO 2222ZYXO 021pRpp22 因旋转而进行的坐标变换，与因平移而进因旋转而进行的坐标变换，与因平移而进行的坐标变换，可以用一个坐标变换矩阵来表行的坐标变换，可以用一个坐标变换矩阵来表示，记为示，记为A ，称这个矩阵，称这个矩阵 A为齐次坐标变换矩为齐次坐标变换矩阵，或简称为坐标变换矩阵，表示为：阵，或简称为

15、坐标变换矩阵，表示为：100pRA23三、机器人的运动学的一般表示三、机器人的运动学的一般表示 前面所介绍的是任意两个坐标系之间的坐标前面所介绍的是任意两个坐标系之间的坐标变换，我们知道，机器人一般是有多个关节组成变换，我们知道，机器人一般是有多个关节组成的，各关节之间的坐标变换可以通过坐标变换相的，各关节之间的坐标变换可以通过坐标变换相乘后，结合在一起进行求解。如前所述，可以把乘后，结合在一起进行求解。如前所述，可以把机器人的运动模型看作是一系列由关节连接起来机器人的运动模型看作是一系列由关节连接起来的连杆机构。一般机器人具有个自由度，为了分的连杆机构。一般机器人具有个自由度，为了分析其运动

16、，可将上述方法扩展一下。析其运动，可将上述方法扩展一下。 24通常把描述一个连杆与下一个连杆间相对关系通常把描述一个连杆与下一个连杆间相对关系的齐次变换称为的齐次变换称为A矩阵。一个矩阵。一个A矩阵就是一个矩阵就是一个描述连杆坐标系间相对平移和旋转的齐次变换。描述连杆坐标系间相对平移和旋转的齐次变换。如果用如果用 表示第一个连杆在基系的位置和姿表示第一个连杆在基系的位置和姿态，态， 表示第二个连杆相对第一个连杆的位置表示第二个连杆相对第一个连杆的位置和姿态，那么第二个连杆在基系的位置和姿态和姿态，那么第二个连杆在基系的位置和姿态可由下列矩阵的乘积求得可由下列矩阵的乘积求得 01A12A1201

17、2AAT 25同理，若同理，若 表示第三个连杆相对第二个连杆表示第三个连杆相对第二个连杆的位置和姿态，那么第三个连杆在基系的位置的位置和姿态，那么第三个连杆在基系的位置和姿态可由下列矩阵的乘积求得和姿态可由下列矩阵的乘积求得 23A2312013AAAT 26于是，对于六连杆的机器人，有下列矩阵于是，对于六连杆的机器人，有下列矩阵 成立成立一般，每个连杆有一个自由度，则六连杆组成的一般，每个连杆有一个自由度，则六连杆组成的机器人具有六个自由度，并能在其运动范围内任机器人具有六个自由度，并能在其运动范围内任意定位与定向。其中，三个自由度用于规定位置，意定位与定向。其中，三个自由度用于规定位置，另

18、外三个自由度用来规定姿态。所以，表示了机另外三个自由度用来规定姿态。所以，表示了机器人的位置和姿态。器人的位置和姿态。 5645342312016AAAAAAT 27对于具有对于具有n个关节的机器人，若设坐标系个关节的机器人，若设坐标系 为固定在指尖上的坐标系时，则从坐标系为固定在指尖上的坐标系时，则从坐标系 到基准坐标系到基准坐标系 的坐标变换矩阵的坐标变换矩阵T可由下可由下式给出：式给出： T 不仅是从不仅是从 坐标系到坐标系坐标系到坐标系 的坐标变换，而且同时还可以解释为在基准坐标的坐标变换，而且同时还可以解释为在基准坐标系系 上看到的表示指尖位置和方向的矩上看到的表示指尖位置和方向的矩

19、阵。阵。 nnnnzyxO nnnnzyxO 0000zyxO 1231201 nnnAAAATnnnnzyxO 0000zyxO 0000zyxO 28四、机器人运动问题的示例四、机器人运动问题的示例 1机器人正运动学问题机器人正运动学问题 机器人正运动学问题就是求机器人运动学机器人正运动学问题就是求机器人运动学的正解（的正解（forward kinematics），即在给定），即在给定组成运动副的相邻连杆的相对位置情况下，确组成运动副的相邻连杆的相对位置情况下，确定机器人末端执行器的位置和姿态。通过上述定机器人末端执行器的位置和姿态。通过上述分析可知，运动学正解可用一个反映此相对关分析可知

20、，运动学正解可用一个反映此相对关系的变换矩阵来表示，这里一般是指开式链的系的变换矩阵来表示，这里一般是指开式链的机器人结构。机器人结构。 29以一个以一个6自由度的机器人为例，如图所示，自由度的机器人为例，如图所示，在该机器人中，除第在该机器人中，除第3个关节为平移关节外，个关节为平移关节外，其余均为旋转关节。其余均为旋转关节。 30对于这个机器人，根据图中表示的坐标系对于这个机器人，根据图中表示的坐标系 为基准坐标系，正运动学问题就是求该机器人末端为基准坐标系，正运动学问题就是求该机器人末端手指关节手指关节6的位置和姿态，也就是在基准坐标系上看的位置和姿态，也就是在基准坐标系上看关节关节6，

21、因此找出由，因此找出由 到到 的坐的坐标变换矩阵标变换矩阵T即可。即可。也就是表示这个机器人的末也就是表示这个机器人的末端指尖的位置和方向，可以由下式给出：端指尖的位置和方向，可以由下式给出： 0000zyxO 6666zyxO 0000zyxO )()()()()()(656545434323212101AAAdAAAT 31其中其中10000100cos0sin0sin0cos)(01111101lA10000100cos0sin0sin0cos)(12222212lA32100010000100001)(3323ddA100000100cos0sin0sin0cos)(4444434A1

22、00000100cos0sin0sin0cos)(5555545A100000100cos0sin0sin0cos)(4444434A33100000100cos0sin0sin0cos)(5555545A100010000cossin00sincos)(26666656lA34上式即为该上式即为该6自由度机器人的运动学正解。对于自由度机器人的运动学正解。对于不同类型的机器人，其坐标变换矩阵的形式不同，不同类型的机器人，其坐标变换矩阵的形式不同，要根据实际结构求得。要根据实际结构求得。 352 2机器人逆运动学机器人逆运动学 机器人的逆解问题比较复杂，为了说明问题，下面机器人的逆解问题比较复杂

23、，为了说明问题，下面先以先以2自由度的机器人为例。自由度的机器人为例。如图所示，已知机器人末端的坐标值如图所示，已知机器人末端的坐标值（x,y） ，试利用，试利用 x,y表示表示 2 2 36根据图中的几何关系可知： )cos(cos21211llx)sin(sin21211lly （338） （339） 37 联立求解上述两方程，可分别求出联立求解上述两方程，可分别求出 的表达式。的表达式。21,221222122cos2llllyx因此可进一步得到：因此可进一步得到： )2(cos2122212212llllyx将该式代入前面的几何表达式就可求出的将该式代入前面的几何表达式就可求出的 表达

24、式。表达式。 138 从机器人的手爪末端位置姿态出发，从机器人的手爪末端位置姿态出发，可以求出机器人对应的各关节的角度。可以求出机器人对应的各关节的角度。该例的机器人是属于平面多关节机器人，该例的机器人是属于平面多关节机器人，对于一般的机械手来讲，其求解过程比对于一般的机械手来讲，其求解过程比较复杂，往往其解不是唯一的。请有兴较复杂，往往其解不是唯一的。请有兴趣的爱好者参考相关的文献书籍。趣的爱好者参考相关的文献书籍。 39第三节第三节 机器人的雅可比矩阵机器人的雅可比矩阵 一、雅可比矩阵的定义一、雅可比矩阵的定义 前面讨论了机器人的指尖位置和方向与前面讨论了机器人的指尖位置和方向与各关节的变

25、化位置之间的关系。在本节各关节的变化位置之间的关系。在本节将进一步讨论指尖的速度与各关节的速将进一步讨论指尖的速度与各关节的速度（转动或平移）之间的关系。度（转动或平移）之间的关系。考虑机械手的手爪位置考虑机械手的手爪位置r和关节变量和关节变量的的关系用正运动学方程表示如下：关系用正运动学方程表示如下： 40假定这里考虑的是假定这里考虑的是 )(fr 121, mTmRrrrr121, nTmR的一般情况，并设手爪位置包含表示姿态的变量，的一般情况，并设手爪位置包含表示姿态的变量，以及关节变量由回转角和平移组合而成的情况。以及关节变量由回转角和平移组合而成的情况。（355）41若用每个分量表示

26、，则变为若用每个分量表示，则变为 在在 的情况下，将变为手爪位置的关节变量的情况下，将变为手爪位置的关节变量有无限个解的冗余机器人。而工业上常用的多有无限个解的冗余机器人。而工业上常用的多关节机器人手臂，通常用于作业的所需手爪应关节机器人手臂，通常用于作业的所需手爪应有有3个位置变量和个位置变量和3个姿态变量，总计个姿态变量，总计6个变量。个变量。而且由于不采用冗余机器人结构，所以而且由于不采用冗余机器人结构，所以),(21njjfr ), 2 , 1(mj mn6 mn42将式（355）的两边对时间微分，可得到下式 Jr （357）其中 nmnmmnTRfffffJ 1111)(（358）4

27、3称称 为雅可比矩阵（为雅可比矩阵（Jacobian matrix）。若）。若在式（在式（357）的两边乘以微小时间）的两边乘以微小时间 ，则可，则可得到得到 JdtJddr （359）该式是用雅可比矩阵表示微小位移间关系的关系式。该式是用雅可比矩阵表示微小位移间关系的关系式。 44二、与平移速度相关的雅可比矩阵二、与平移速度相关的雅可比矩阵 现在设基准坐标系为现在设基准坐标系为 ，固定于指尖，固定于指尖的坐标系为的坐标系为 ，在，在 上表示的上表示的的坐标为的坐标为 ，则，则 可以表示如下：可以表示如下： 0000zyxO eeeezyxO 0000zyxO eOePeP)(1000qfTP

28、e（360） 45这时，指尖的平移速度可以写成：这时，指尖的平移速度可以写成： qJdtdqJdtdqdqdfdtdPvLLe（361） 式中，式中， ，其中，其中 是关节的数目。这是关节的数目。这里的里的 称为与平移速度相关的雅可比矩阵。称为与平移速度相关的雅可比矩阵。 Tnqqq),(1 nLJ46下面以下面以2自由度机械手为例，如前面图自由度机械手为例，如前面图32所所示的示的2自由度机械手的雅可比矩阵。前面已推自由度机械手的雅可比矩阵。前面已推导过，该机器人的指尖位置可以表示为导过，该机器人的指尖位置可以表示为 )cos(cos21211llx)sin(sin21211lly47则与这

29、个机器人的平移速度相关的雅可比矩阵，则与这个机器人的平移速度相关的雅可比矩阵，可以下列形式给出：可以下列形式给出： )cos()cos(cos)sin()sin(sin21221211212212112121llllllyyxxJL（363） 48现在，我们来讨论一下现在，我们来讨论一下 的各列向量的几何学意的各列向量的几何学意义，即在义，即在 时，考虑时，考虑 ， 的几何学的几何学意义。根据式（意义。根据式（363），）， 是在时是在时 ，也就，也就是第是第2关节固定时，仅在第关节固定时，仅在第1关节转动的情况下，关节转动的情况下，指尖平移速度在基准坐标系上表示出的向量。指尖平移速度在基准坐

30、标系上表示出的向量。同样，同样， 是第是第1关节固定时，仅在第关节固定时，仅在第2关节转动关节转动的情况下，指尖平移速度在基准坐标系上表示出的情况下，指尖平移速度在基准坐标系上表示出的向量。的向量。 LJ),(21LLLJJJ1LJ2LJ1LJ022LJ49因此，当用图表示因此，当用图表示 和和 时，就变成了时，就变成了如图所示的情况。如图所示的情况。 11LJ22LJ 图图39 和和 的几何学说明的几何学说明 11LJ22LJ50三、与旋转速度相关的雅可比矩阵三、与旋转速度相关的雅可比矩阵 一般来讲，指尖的旋转速度表示方法，有以下一般来讲，指尖的旋转速度表示方法，有以下两种类型：两种类型：

31、1考虑由表示指尖方向的三变量组合（例如为考虑由表示指尖方向的三变量组合（例如为欧拉角）构成向量欧拉角）构成向量 ，然后由它对时间的微分，然后由它对时间的微分 进行表示的一种方法。进行表示的一种方法。2以基准坐标系的各坐标轴作为旋转轴，以分以基准坐标系的各坐标轴作为旋转轴，以分别围绕各旋转轴的角速度作为分量构成向量别围绕各旋转轴的角速度作为分量构成向量 ，然后用然后用 进行表示的方法。进行表示的方法。 51在第二种表示方法中，可以把在第二种表示方法中，可以把 解释为在基准解释为在基准坐标系上，围绕坐标系上，围绕x轴，轴，y轴和轴和 z轴的旋转速度的轴的旋转速度的合成，因为物理意义明确。这时，公式

32、合成，因为物理意义明确。这时，公式 qJA（364） 其中矩阵其中矩阵JA称为与旋转速度相关的雅可比矩阵。称为与旋转速度相关的雅可比矩阵。 52四、雅可比矩阵的计算方法四、雅可比矩阵的计算方法 考虑一般情况，如六维向量，它可以指尖的平考虑一般情况，如六维向量，它可以指尖的平移速度和旋转速度作为其向量的分量，即移速度和旋转速度作为其向量的分量，即 vp （365） 这时，若采用这时，若采用 和和 表示机器人的雅可比表示机器人的雅可比矩阵，则表示矩阵，则表示 LJAJqJJqJpAL（366） 53这里，为了计算雅可比矩阵中的各分量，需对进这里，为了计算雅可比矩阵中的各分量，需对进一步作下列分割一步作下列分割 式中，式中，n为机器人的关节数，为机器人的关节数， 和和 分别表示分别表示 和和 的第个列向量。而的第个列向量。而 和和 则分别表则分别表示只有第示只有第i个关节以速度个关节以速度 运行，其他的关节运行，其他的关节都固定时的指尖平移速度向量和旋转速度向