操作臂的雅可比

1、1第4章 机器人的雅可比公式 4.1 4.1 雅可比矩阵的定义雅可比矩阵的定义 4.2 4.2 机器人的微分运动和广义速度机器人的微分运动和广义速度 4.3 4.3 雅可比矩阵的构造雅可比矩阵的构造 4.4 4.4 机器人雅可比矩阵计算实例机器人雅可比矩阵计算实例 4.5 4.5 力雅可比力雅可比 4.6 4.6 奇异性和灵巧度奇异性和灵巧度 4.7 4.7 刚度和变形刚度和变形 4.8 4.8 误差标定和补偿误差标定和补偿 4.9 4.9 小结小结2 前面我们建立了操作臂的运动学方程，实际前面我们建立了操作臂的运动学方程，实际上是建立了机器人操作臂在上是建立了机器人操作臂在操作操作与与关节关

2、节空间中的空间中的位移关系位移关系。通过求解运动方程的反解，建立了两。通过求解运动方程的反解，建立了两个空间的个空间的映射关系映射关系。 本章在位移分析的基础上，进行本章在位移分析的基础上，进行速度分析速度分析，研究操作空间与关节空间的研究操作空间与关节空间的速度速度间的线性间的线性映射关映射关系系雅可比矩阵雅可比矩阵 雅可比雅可比(Jacobian)(Jacobian)。 雅可比不仅表示了两个空间之间的雅可比不仅表示了两个空间之间的速度速度线性线性映射关系，也表示两空间的映射关系，也表示两空间的力力的传递关系。的传递关系。 为进行为进行速度分析速度分析，要利用到微分运动的概念。，要利用到微分

3、运动的概念。3 机器人的操作与控制，常涉及到机械手位姿的微小机器人的操作与控制，常涉及到机械手位姿的微小变化。这些变化可由描述机械手位姿的变化。这些变化可由描述机械手位姿的T 的微小变化的微小变化来来表示。在数学上，这种微小变化可用表示。在数学上，这种微小变化可用微分变化微分变化来表达。来表达。 机械手运动过程中的微分关系是很重要的。如用摄机械手运动过程中的微分关系是很重要的。如用摄像机观察机械手的末端执行装置时，需把一个坐标系的像机观察机械手的末端执行装置时，需把一个坐标系的微分变化变换为对另一坐标系的微分变化，如把摄像机微分变化变换为对另一坐标系的微分变化，如把摄像机的坐标系建立在的坐标系

4、建立在T6上。上。 应用微分变化的另一情况是当已知对应用微分变化的另一情况是当已知对T6的微分变化的微分变化时，要求出对各关节坐标的相应变化。微分关系对于研时，要求出对各关节坐标的相应变化。微分关系对于研究机械手的动力学问题，也是十分重要的。究机械手的动力学问题，也是十分重要的。44.1 雅可比矩阵的定义雅可比矩阵的定义 操作臂的雅可比矩阵定义为其操作速度与关节速度操作臂的雅可比矩阵定义为其操作速度与关节速度的的线性变换线性变换，可看成是从关节空间向操作空间运动速度，可看成是从关节空间向操作空间运动速度的传动比。操作臂的运动方程的传动比。操作臂的运动方程XX(q) (4.1)代表操作空间代表操

5、作空间x与关节空间与关节空间 q 之间的位移关系。将式之间的位移关系。将式(4.1)两边对时间两边对时间 t 求导，即得出求导，即得出 q 与与 X 之间的微分关系之间的微分关系 XJ(q)q (4.2)式中，式中，X 称为末端在操作空间的广义速度，简称操作速称为末端在操作空间的广义速度，简称操作速度，度，q 为关节速度；为关节速度；J(q)是是6n的偏导数矩阵的偏导数矩阵操作臂的操作臂的雅可比矩阵雅可比矩阵。它的第。它的第 i 行第行第 j 列元素为列元素为5 Jij(q) Xi(q)/ qj， i1, 2, , 6；j1, 2, , n (4.3)式式(4.2)表示，对给定的表示，对给定的

6、q Rn，雅可比，雅可比J(q)是从关节空间速是从关节空间速度度q 向操作空间速度向操作空间速度x 映射的线性交换。映射的线性交换。6 例例4.1 对图示平面对图示平面2R机械手，其运动学方程如下：机械手，其运动学方程如下： x l1c1l2c12 ；y l1s1l2 s12 让运动学方程两端分别对时间让运动学方程两端分别对时间 t 求导，求导， x l1s1 1 l2 s12( 1 2) y l1c1 1 l2c12( 1 2)写成矩阵的形式：写成矩阵的形式： XJ(q)q式中，式中，X x y， q 1 2， J(q) (4.4)式式(4.4)中的中的J(q)是是22的方阵。的方阵。122

7、1221112212211clclclslslsl7当当 20 (或或180 )时。时。 J(q) 0，矩阵的秩为，矩阵的秩为1，处于奇异，处于奇异状态。从几何上看，机械手完全伸直状态。从几何上看，机械手完全伸直( 20 )，或完全缩，或完全缩回回( 2180 )时，机械手末端时，机械手末端丧失了径向自由度丧失了径向自由度，仅能沿，仅能沿切向运动。在奇异形位时，机械手在操作空间的自由度将切向运动。在奇异形位时，机械手在操作空间的自由度将减少。减少。 对关节空间的某些形位对关节空间的某些形位q，J(q)的秩减少，这些形位称为操的秩减少，这些形位称为操作臂的作臂的奇异形位奇异形位。可用。可用 J(

8、q) 判别奇异形位：判别奇异形位： J(q) l1l2s28满秩的，关节速度就可解出满秩的，关节速度就可解出qJ1(q)X 对平面对平面2R机械手，机械手，J1(q)可由式可由式(4.4)求得求得J1(q) (4.5) 例例4.2 如图如图41，为实现平面，为实现平面2R机械手末端沿机械手末端沿x0轴以轴以1m/s的速的速度运动，求相应的关节速度度运动，求相应的关节速度q 1, 2。 由式由式(4.2)可见，只要可见，只要J(q)是是12211122111221222211slslclclslclsl l图图41 平面平面2R机械手的速度反解机械手的速度反解 9得到对应于末端速度得到对应于末端

9、速度X1, 0的关节速度反解为的关节速度反解为 1c12/l1s2 ； 2c1/l2s2c12/l2s2 当当 20 (或或180 )时。机械手时。机械手奇异形位，相应的奇异形位，相应的q。 当操作臂有当操作臂有较多较多自由度时，用上述定义计算雅可比自由度时，用上述定义计算雅可比较为复杂，但有两种构造性方法较为复杂，但有两种构造性方法(矢量积法和微分变换法矢量积法和微分变换法)相对较为简单。下面介绍利用操作空间与关节空间的相对较为简单。下面介绍利用操作空间与关节空间的微微分运动关系分运动关系构造雅可比矩阵的构造雅可比矩阵的微分变换法微分变换法和和矢量叉积构矢量叉积构造法造法。10 4.2 4.

10、2 机器人的微分运动和广义速度机器人的微分运动和广义速度 若已知一个变换的元素是某个变量的函数，那么对若已知一个变换的元素是某个变量的函数，那么对这个变量的这个变量的微分变换微分变换就是其元素为就是其元素为原变换元素的导数原变换元素的导数。 本节推导出一种方法，使得对本节推导出一种方法，使得对T的微分变换等价于的微分变换等价于对基系的变换。此方法可推广至任何两个坐标系，使它对基系的变换。此方法可推广至任何两个坐标系，使它们的微分运动能联系起来。们的微分运动能联系起来。 机械手的变换包括平移变换、旋转变换、比例变换机械手的变换包括平移变换、旋转变换、比例变换和投影变换等。这里的讨论限于和投影变换

11、等。这里的讨论限于平移平移和和旋转旋转变换。变换。可可把其导数项表示为把其导数项表示为微分平移微分平移和和微分旋转微分旋转。11 1. 微分平移和微分旋转微分平移和微分旋转（补充内容）（补充内容） 用给定的坐标系用给定的坐标系(也可用基系也可用基系)来表示来表示微分平移和旋转微分平移和旋转。已知已知T，可表示，可表示TdT为为(相对基系相对基系)TdTTrans(dx, dy, dz)Rot( f, d )T式中，式中，Trans(dx, dy, dz)是基系中是基系中微分平移微分平移变换；变换；Rot( f, d )是基系中绕矢量是基系中绕矢量 f 的的微分旋转微分旋转变换。由上式得变换。由

12、上式得dT为为dTTrans(dx, dy, dz)Rot( f, d )IT (B.1)同样，可给出对同样，可给出对T的微分平移和旋转的微分变换的微分平移和旋转的微分变换TdTTTrans(Tdx, Tdy, Tdz)Rot( f, dT )式中，式中，Trans(Tdx, Tdy, Tdz)为对为对T的微分平移变换；的微分平移变换；Rot( f, dT )是绕是绕T中矢量中矢量 f 的微分旋转。这时有的微分旋转。这时有12dTTTrans(Tdx, Tdy, Tdz)Rot( f, dT ) I (B.2)式中式中(B.1)和和(B.2)都有都有相似相似项项Trans(dx, dy, dz

13、)Rot( f, d )I。当微分运动是相当微分运动是相对基系对基系时，记为时，记为 ；当运动是；当运动是相对相对T时，时，记为记为T 。即，当对基系有微分变化时，。即，当对基系有微分变化时，dT T；而当对；而当对T有微分变化时，有微分变化时，dTTT 。 表示微分平移和表示微分平移和T的齐次变换分别为的齐次变换分别为 Trans(dx, dy, dz)T B.3 1000100010001zyxddd1000zzzzyyyyxxxxpaonpaonpaon13上式中上式中Trans的变量是由微分变化的变量是由微分变化dx idy jdz k表示的微分表示的微分矢量矢量d。利用前面的通用旋转

14、变换式。利用前面的通用旋转变换式(参考式参考式(2.45)： Rot( f, )10000versversvers0versversvers0versversverscffsfffsfffsfffcffsfffsfffsfffcffzzxzyyxxxyzyyzyxyxzzxyxx 对微分变化对微分变化d ，其相应的正、余弦函数和正交函数为，其相应的正、余弦函数和正交函数为代入到上式，可把微分旋转齐次变换表示为代入到上式，可把微分旋转齐次变换表示为Rot( f, d ) B.40dverslim, 1dcoslim,ddsinlim0d0d0d100001dd0d1d0dd1xyxzyfffff

15、fz14将式将式(B.3)和和(B.4)代入代入 Trans(dx, dy, dz)Rot( f, d )I，可得可得 1000010000100001100001dd0d1d0dd11000100010001xyxzyzzyxffffffddd化简得化简得 (B.5)绕矢量绕矢量 f 的微分旋转的微分旋转d 等价于分别绕三个轴等价于分别绕三个轴x、y和和z的微的微分旋转分旋转 x, y和和 z，即，即 fxd x，fyd y，fzd z。代入。代入上式得上式得00000ddd0ddd0zxyyxzxyzdffdffdff三三.微分平移和微分旋转微分平移和微分旋转 由于微分旋转由于微分旋转0

16、，所以所以sind，cos1，Vers0，将将它们代入旋转变换通式中得微分旋转表达式它们代入旋转变换通式中得微分旋转表达式: 微分平移变换与一般平移微分平移变换与一般平移变换一样，其变换矩阵为变换一样，其变换矩阵为:于是得于是得1000100010001),(dzdydxdzdydxTrans1000010101),(dkdkdkdkdkdkdkRotxyxzyz44),(),(IdkRotdddTranszyx0000000dzdkdkdydkdkdxdkdkxyxzyz四四. 微分旋转的无序性微分旋转的无序性 当当0 时，有时，有sind，cos1若令若令x=dx，y=dy，z=dz，则绕

17、三个坐标轴的微分旋转矩阵分别为则绕三个坐标轴的微分旋转矩阵分别为10000100100001),(xxxxRot10000100010001),(yyyyRot10000100001001),(zzzzRot10000101001),(),(xyxyxyyyRotxxRot100001010001xyxy10000101001),(),(xyxyyxxxRotyyRot100001010001xyxy略去高略去高阶无穷阶无穷小量小量两者结果相同，可见这里两者结果相同，可见这里。同理可得同理可得 1000010101),(),(),(xyxzyzzzRotyyRotxxRot 若若Rot（x，y

18、，z） 和和Rot（x，y，z） 表示两表示两个不同的微分旋转，则两次连续转动的结果为：个不同的微分旋转，则两次连续转动的结果为：任意两个微分旋转的结果为绕每个轴转动的元素的代任意两个微分旋转的结果为绕每个轴转动的元素的代数和，即微分旋转是可加的。数和，即微分旋转是可加的。100001) (0) (10) (1) , , (),(xxyyxxzzyyzzzyxRotzyxRotkxd=x， kyd=y ， kzd=z所以有所以有由等效转轴和等效转角与由等效转轴和等效转角与 等效，有等效，有),(),(),(zzRotyyRotxxRot),(dkRot),(),(),(zzRotyyRotxx

19、Rot即即1000010101dkdkdkdkdkdkxyxzyz1000010101xyxzyz20 (B.6)类似，可得类似，可得T 为为T (B.7)0000000zxyyxzxyzddd0000000TTTTTTTTTzxyyxzxyzddd 于是，可把于是，可把 看成是由微分看成是由微分平移矢量平移矢量d 和微分和微分旋转矢旋转矢量量 构成的，且有：构成的，且有：ddxidy jdzk (4.6) xi y j zk (4.7)21用列矢量用列矢量D来包含上述两矢量，并称为刚体或坐标系的来包含上述两矢量，并称为刚体或坐标系的微微分运动矢量分运动矢量Ddx dy dz x y z 或或

20、 Dd (4.8)同理，相对同理，相对T有下列各式有下列各式 TdTdxiTdy jTdzk T T xiT y jT zk (B.8)TDTdx Tdy Tdz T x T y T z 或或 TDTd T (B.9)22 例例4.3 已知已知A和对基系的微分平移与微分旋转为和对基系的微分平移与微分旋转为 A ，d1 i0 j0.5k， 0 i0.1 j0k试求微分变换试求微分变换dA。解：解：首先据式首先据式(B.6)可得下式可得下式 在按照在按照dT T，有：，有：dA A，即，即 dAA的这一微分变化如图的这一微分变化如图B1。1000001050011010000005 . 0001

21、. 0000011 . 00000005 . 01 . 0000000101 . 001000001050011010000005 . 0001 . 0000011 . 000图图B123 2微分运动的等价变换微分运动的等价变换 要求机械手的雅可比矩阵，需要把一个坐标系内的位要求机械手的雅可比矩阵，需要把一个坐标系内的位姿的微小变化，变换为另一坐标系内的等效表达式。姿的微小变化，变换为另一坐标系内的等效表达式。 dT T和和dTTT ，当两坐标系等价时，当两坐标系等价时， TTT ，变换后得，变换后得T 1 TT (B.10)由式由式(B.3)和和(B.6)有有 T (B.11)0000000

22、zxyyxzxyzddd1000zzzzyyyyxxxxpaonpaonpaon0000zyxxyyxxyyxxyyxzyyzxxzzxxzzxxzzxxzxzyyzzyyzzyyzzyyzdppaaoonndppaaoonndppaaoonn24(B.11)式与下式等价式与下式等价 T用用T 1左乘上式得左乘上式得 T 1 T 0000)()()()()()()()()()()()(zzzzyyyyxxxxdpaondpaondpaon1000apopnpzyxzyxzyxaaaooonnn 0000)()()()()()()()()()()()(zzzzyyyyxxxxdpaondpaon

23、dpaon 0000)()()()()()()()()()()()(dpaaaoanadpoaooonodpnanonnn25 0000)()()()()()()()()()()()(dpaaaoanadpoaooonodpnanonnn应用三矢量相乘的两个性质应用三矢量相乘的两个性质a(bc)b(ca)及及a(ac)0，由式，由式(B.10)可把上式写为可把上式写为 T 0000)(0)()()()(0)()()()(0adapaonaodopaoonndnpnaon化简得化简得 0000)(0)(0)(0adapnoodopnandnpoaT (B.12)26 T 已被式已被式(B.7)所

24、定义，所定义，令式令式(B.7)与与(B.12)各元分各元分别相等别相等，并利用三矢量相乘的性质：，并利用三矢量相乘的性质：a(bc)c(ab)等，等，可求得下列各式可求得下列各式Tdx (pn)+dnn( p)+dTdy (po)+doo( p)+d (4.11)Tdz (pa)+daa( p)dT x nn ； T y o o T z a a (4.12)式中，式中，n, o, a和和p分别为微分坐标变换分别为微分坐标变换 T 的列矢量。用上两的列矢量。用上两式，能够十分方便地把对基系的微分变化变换为对式，能够十分方便地把对基系的微分变化变换为对T的的微分变化。从上列两式可得微分运动微分变

25、化。从上列两式可得微分运动TD和和D的关系如下：的关系如下：27 zyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxdddaaaooonnnaaaooonnnddd000000000)()()()()()()()()(TTTTTTapapapopopopnpnpnp(4.13)上式可简写为上式可简写为 (4.14)式中，式中，R 是旋转矩阵，是旋转矩阵， R (4.15) 对任何对任何3维矢量维矢量 ppx, py, pzT，其反对称矩阵，其反对称矩阵S(p)定义定义为为dRpSRRdTTTTT0)(zzzyyyxxxaonaonaon28S(p) (4.16)它具

26、有以下性质它具有以下性质000 xyxzyzpppppp(1) S(p) p ， S(p) p ；(2) TS(p) (p )， TS(p) (p )(3) RTS(p) zyxzyxzyx)()()()()()()()()(apapapopopopnpnpnp29 若定义刚体或坐标系的广义速度是由线速度和角速若定义刚体或坐标系的广义速度是由线速度和角速度组成的度组成的 6 维列矢量，即：维列矢量，即：V (4.9)则由式则由式(4.14)可得相应的广义速度可得相应的广义速度V的坐标变换为的坐标变换为(4.17)任意两坐标系间的广义速度的坐标变换为任意两坐标系间的广义速度的坐标变换为(4.18

27、) vRpSRRvTTTTT0)(vRpRSRvAABABABABABB0)(0tt1lim030 例例4.4 已知已知A对基系的微分平移对基系的微分平移d 和旋转和旋转 ，同，同例例4.3。试求对试求对A的等价微分平移和微分旋转。的等价微分平移和微分旋转。 解：因为解：因为n0 i1j0 k ； o0 i0 j1 k a1 i0 j0 k ； p10 i5j0 k以及以及 p 0 i0 j 1 k加上加上d后有：后有： pd1 i0 j 0.5 k。又据式又据式(4.11)和和(4.12)可可求得求得等价微分平移和微分旋转为等价微分平移和微分旋转为Ad0 i 0.5 j1 k A 0.1 i

28、0 j0 k 051001 . 00kji31由式由式dTTT ，计算，计算dAAA 计算，以检验所得微分运动计算，以检验所得微分运动是否正确。据式是否正确。据式(B.7)有有 A dA 所得结果与例所得结果与例4.3一致。可见所求得的对一致。可见所求得的对A的微分平移和微的微分平移和微分旋转是正确无误的。分旋转是正确无误的。0000101 . 005 . 01 . 00000001000001050011010000005 . 01 . 0000000001 . 000000101 . 005 . 01 . 000000032 3. 变换式中的微分关系变换式中的微分关系 式式(4.11)(4

29、.13)可用于可用于变换任何两坐标系间的微分运变换任何两坐标系间的微分运动动。由式。由式(4.11)和和(4.12)，可根据微分坐标变换，可根据微分坐标变换T 和微分旋和微分旋转变换转变换 求求T 。若要从。若要从T 的各微分矢量来求微分矢量的各微分矢量来求微分矢量 ，可从式可从式(B.10)左乘左乘T和和右乘右乘T 1，得：，得： T T T 1或者变换为或者变换为 (T 1)1T T 1 (B.13) 设设A和和B，B相对于相对于A定义。则定义。则A、B都可以都可以用来表示微分运动。左图用来表示微分运动。左图表示了这一情况。表示了这一情况。33 AB B B1A1或变换为或变换为 (B1A

30、1)1 B (B1A1) (B.14)此式表明了此式表明了B内与基系内的微分运动间的关系，它具有内与基系内的微分运动间的关系，它具有式式(B.13)的一般形式；而的一般形式；而(B1A1)则对应式则对应式(B.10)和和(B.11)中中的的T。 图图B2表明了表明了A、B间的微分关系，且有间的微分关系，且有 ABABB 对对 求解得求解得34 同样，由图同样，由图B2还可得：还可得：AA BABB 或或 A BBB 对对A 求解得求解得A BB B1 A (B1)1B B1 (B.15)此式表示此式表示A内与内与B内的微分运动的关系。其中，内的微分运动的关系。其中，B1对对应于式应于式(B.1

31、0)中的中的T，但，但T已不是坐标系矩阵，而是微分坐已不是坐标系矩阵，而是微分坐标变换矩阵。标变换矩阵。它可从图它可从图B2直接求得，即从已知的微分变直接求得，即从已知的微分变化变换图箭头起，回溯到待求的等价微分变化止，所经过化变换图箭头起，回溯到待求的等价微分变化止，所经过的路径。的路径。35 对上述第一种情况，从对上述第一种情况，从B 之箭头至之箭头至 之箭头间所经路之箭头间所经路径即为径即为B1A1；而对于第二种情况，从；而对于第二种情况，从B 至至A ，所经路径，所经路径为为B1。36 例例4.5 有一摄像机装在机械手的连杆有一摄像机装在机械手的连杆5上。连接由下式上。连接由下式确定：

32、确定： C机械手的连杆机械手的连杆6的位置描述如下的位置描述如下 A6被观察的目标物体为被观察的目标物体为CO。要把机械手的末端引向目标物体，。要把机械手的末端引向目标物体，需要知道的坐标系需要知道的坐标系C内的微分变化为：内的微分变化为：Cd1 i0 j0k ， C 0 i0 j0.1 k试求在坐标系试求在坐标系T6内所需要的微分变化。内所需要的微分变化。10008100000100101000100010010510037解：解：上述情况可由上述情况可由图图B3(a)及下列方程描述及下列方程描述T5A6EXT5CO式中，式中，T5描述连杆描述连杆5与基系与基系的关系；的关系；A6是是以以5

33、描述连描述连杆杆6；E为描述为描述物体对末端物体对末端的未知变换；的未知变换；O是用摄像是用摄像机坐标系描述物体。机坐标系描述物体。 变换图见变换图见图图B3(b)。T5CTT6T5A6图图B3 例例4.5的位姿及微分变换图的位姿及微分变换图38 从图可得相对从图可得相对T6 至至C 的微分变换的微分变换T为：为： TC1T51T5A6C1A6 C1 可得此微分坐标变换可得此微分坐标变换 T又因为又因为 p 0 i0.2 j0 k最后据式最后据式(4.11)和和(4.12)可得坐标系可得坐标系T6内的微分变化如下内的微分变化如下T6d0.2 i0 j1k ， T6 0 i0.1 j0 k100

34、0500100101010010005010000121001000810000010010100050010010101005021 . 000kji39 4.3 4.3 机器人的雅可比矩阵的构造法机器人的雅可比矩阵的构造法 上面分析了机械手的微分运动。下面将研究机器人上面分析了机械手的微分运动。下面将研究机器人操操作空间作空间速度与速度与关节空间关节空间速度之间的速度之间的线性映射关系线性映射关系，即，即雅可雅可比矩阵比矩阵。 1J(q)矩阵的分解矩阵的分解 刚体或坐标系的广义速度刚体或坐标系的广义速度 X 是由是由线线速度速度 v 和和角角速度速度 组成的组成的 6 维列矢量维列矢量V

35、X v d (4.9 ) J(q)阵既是从关节阵既是从关节操作空间的速度传递的操作空间的速度传递的线性线性关系，关系，又是微分运动转换的线性关系，由式又是微分运动转换的线性关系，由式(4.9 )和式和式(4.2)有：有：tt1lim040V J(q)q (4.19)D d X t J(q)q t即即 DJ(q)dq (4.20)对对n关节的机器人，关节的机器人，J(q)J(q)6n，每列代表相应的关节，每列代表相应的关节速度速度 qi 对手爪线速度和角速度的变换。对手爪线速度和角速度的变换。J(q)可分块为可分块为v q1 q2 qn (4.21)由上式可把由上式可把v 和和 表示成表示成 q

36、i 的线性函数的线性函数 vJl1q1Jl2q2Jlnqn Ja1q1Ja2q2Janqn (4.22)式中，式中，Jli和和Jai分别表示关节分别表示关节 i 的的单位单位关节关节速度速度引起手爪的引起手爪的线速度和角速度。线速度和角速度。0limt0limtanaalnllJJJJJJ212141 2J(q)矩阵的求法矩阵的求法 式式(4.8)、(B.9)、(4.13)、(4.14)和和(4.20)等是计算等是计算J(q)的的基本公式，但这些公式计算基本公式，但这些公式计算J(q)的过程较复杂。下面介绍的过程较复杂。下面介绍两种直接构造两种直接构造J(q)的方法。的方法。 (1) 矢量积法

37、：矢量积法：求机求机器人器人J(q)的矢量积法是建的矢量积法是建立在运动坐标系的概念立在运动坐标系的概念上的。上的。是关节速度是关节速度的传递情况，末端手爪的传递情况，末端手爪T的的v和和 与与qi有关。有关。 关节速度的传递关节速度的传递42 对对移动关节移动关节i，有，有 v zi 0qi； Jizi 0 (4.23) 对对转动关节转动关节i，有，有 v ziipn0 zi qi ； Jiziipn0 zizi(i0Ripn ) zi (4.24)式中，式中，ipn0表示表示T的原点相对的原点相对i的的位矢在位矢在0中的表示中的表示，即，即ipn0i0Ripn (4.25)而而zi 是是i

38、的的z轴轴单位矢量单位矢量在在0中的表示。中的表示。 43 (2) 微分变换法微分变换法(前面已介绍过推导方法前面已介绍过推导方法) 旋转关节旋转关节i，连杆，连杆 i 相对相对 i1绕绕 i 的的 zi 轴作微分转动轴作微分转动d i ，其微分运动矢量为，其微分运动矢量为d 0 0 0di ， 0 0 1d i (*)由式由式(4.13)可得手爪系可得手爪系T相应的微分运动矢量为相应的微分运动矢量为 Tdx Tdy Tdz T x T y T z(pn)z (po)z (pa)z nz oz azd i (4.26) 可得可得J(q)的的第第 i 列列如下：如下：TJl i(pn)z (po

39、)z (pa)z TJainz oz az (4.28a)44 移动关节移动关节i，连杆，连杆i 相对相对i1沿沿zi 轴作微分移动轴作微分移动ddi，其微分运动矢量为其微分运动矢量为 d 0 0 1ddi ， 0 0 0d i (*) 同样，由式同样，由式(4.13)可得手爪的微分运动矢量为可得手爪的微分运动矢量为 Tdx Tdy Tdz T x T y T znz oz az 0 0 0ddi (4.27)则则 TJl inz oz az ；TJai0 0 0 (4.28b)式中，式中，n, o, a和和 p 是是 iTT的的 4个列矢量。个列矢量。 上述求上述求TJq的方法是的方法是构造

40、性构造性的，的，只要知道只要知道i1Ti，就，就可自可自动生成动生成 J(q) ，而不需求解方程等手续。步骤如下：，而不需求解方程等手续。步骤如下：45 计算计算i1Ti ： 0T1，1T2，n1Tn(, nTT)。 计算计算iTn (或或iTT)(见见图图43)： n1Tn， n2Tnn2Tn1n1Tn，i1Tni1Ti iTn， ， 0Tn0T11Tn (，0TT0T11TT) 计算计算TJ(q)的各列元素，的各列元素，第第 i 列列TJi由由iTn决定。由式决定。由式(4.28)计算计算TJl i和和TJai。 从上述两种构造法得出的从上述两种构造法得出的J(q)和和JT(q)间存在有关

41、系：间存在有关系： Tji和和iTn之间的关系之间的关系JT(q) J(q) 或或 J(q) JT(q) (4.29)T0T000RRnnRR0000nn464.4 机器人雅可比矩阵计算实例机器人雅可比矩阵计算实例 下面举例说明计算具体机器人微分运动和雅可比下面举例说明计算具体机器人微分运动和雅可比矩阵的方法。矩阵的方法。 先计算先计算PUMA 560机器人的机器人的Jacobian矩阵，再计算矩阵，再计算西博特奇西博特奇(Cybotech) V80机器人的机器人的Jacobian矩阵。矩阵。 1. PUMA 560 机器人的雅可比矩阵机器人的雅可比矩阵 PUMA 560有有6个旋转关节，其雅

42、可比矩阵有个旋转关节，其雅可比矩阵有6列。列。由式由式(4.28)可计算各列元素。现分别用两种方法计算。可计算各列元素。现分别用两种方法计算。47 (1) 微分变换法求微分变换法求TJ(q) TJ(q)的第的第1列列TJ1(q)对应着对应着1T6，式，式(3.63)给出了给出了1T6的各的各元素，由式元素，由式(4.26)得得 TJ1(q)TJ1x TJ1y TJ1z s23(c4c5c6 s4s6) c23s5c6 s23(c4c5c6s4c6)c23s5s6 s23c4s5 c23s5式中，式中， TJ1xd2c23(c4c5c6 s4s6) s23s5c6 (a2c2a3c23 d4 s

43、23)(s4c5c6c4s6) TJ1yd2c23(c4c5c6s4c6)s23s5s6 (a2c2a3c23 d4s23)(s4c5s6 c4c6) TJ1zd2(c23c4c5 s23c5)(a2c2a3c23 d4 s23)s4s5教材中是教材中是式式(3.14)48同理，利用变换矩阵同理，利用变换矩阵2T6得出得出TJ(q)的第的第2列列 TJ2(q)TJ2x TJ2y TJ2z s4c5c6 c4s6 s4c5s6 c4c6 s4s5式中，式中，TJ2xa3s5c6 d4(c4c5c6 s4s6) a2s3(c4c5c6 s4s6)c3s5c6 TJ2ya3s5s6 d4(c4c5c

44、6 s4c6) a2s3(c4c5c6 s4s6)c3s5s6 TJ2za3c6d4c4s5a2(s3c4s5c3c6)同样可得，同样可得， TJ3(q)d4(c4c5c6 s4s6)a3s5c6 d4(c4c5c6s4c6) a3s5s6d4c4s5a3c6 s4c5c6c4s6 s4c5s6c4c6 s4s5 TJ4(q)0 0 0 s5c6 s5c6 c5； TJ5(q)0 0 0 s6 c6 0； TJ6(q)0 0 0 0 0 149 (2) 矢量积法求矢量积法求J(q) 对都是转动关节的对都是转动关节的PUMA 560，其，其J(q)矩阵为矩阵为 J(q) 由图由图37及各连杆的及

45、各连杆的0T1, 1T2, , 5T6，可计算出各中间项，可计算出各中间项，再求再求J(q)的各列，即的各列，即J1(q), J2(q), , J6(q)J(q)。利用第。利用第3章的章的公式，可计算上式中各量，过程如下：公式，可计算上式中各量，过程如下： 10R，21R，32R，43R，54R和和65R； z1，z2，z3，z4，z5和和z6； 1p6，2p6，3p6，4p6，5p6和和6p6； 用第用第3章的公式章的公式 1p60，2p60，3p60，4p60，5p60和和6p60； 用式用式(4.25) J(q) 中的各列：中的各列： J1(q), J2(q), , J6(q)。 用式用

46、式(4.24) 621066606220611zzzpzpzpz教材中是教材中是p40的式的式(3.09)50 2V80 机器人的雅可比矩阵机器人的雅可比矩阵 图图B4是法国西博特奇公司生产的是法国西博特奇公司生产的V80工业机器人的工业机器人的外形图和停止位置。在外形图和停止位置。在停止位置停止位置：悬臂与基系的：悬臂与基系的x轴平行，轴平行，末端夹手垂直向上。连杆和关节参数见表末端夹手垂直向上。连杆和关节参数见表41，其，其6个关节个关节都是旋转运动的。都是旋转运动的。 在建立在建立V80操操作机器人的雅可比作机器人的雅可比矩阵时，应用了图矩阵时，应用了图43的变换图。的变换图。图图B4

47、V80机械手外机械手外形图及停止位置形图及停止位置51表表 41 V80 的连杆和关节参数的连杆和关节参数 连杆连杆 变量变量 d a cos sin 1 1 90 0 0 1 2 2 0 0 a2 1 0 3 3 90 0 a3 0 1 4 4 90 d4 0 1 5 5 90 0 0 1 6 6 0 0 1 0 52 J(q)阵的阵的第第6列列把关节把关节6的运动变换为的运动变换为T6坐标，而关节坐标，而关节6也作用于也作用于5。通过跟踪变换图，从。通过跟踪变换图，从5至至T6坐标，可得微分坐标，可得微分坐标变换矩阵坐标变换矩阵5T6。据。据(*)式可得式可得 T6d60 i0 j0k ，

48、 T6 60 i0 j1 k这两矢量构成了这两矢量构成了J(q)阵的第阵的第6列。列。 对对第第5列列，其微分变换为，其微分变换为5T6。将其元素代入。将其元素代入(*)式，得式，得T6d50 i0 j0k ， T6 5s6 ic6 j0 k 对对第第4列列，采用矩阵，采用矩阵4T6。则有。则有 T6d40 i0 j0k ， T6 4s5 c6 is5s6 jc5k53 对对V80机械手机械手J(q)阵的阵的第第3列列，其微分坐标变换矩阵为，其微分坐标变换矩阵为3T6。是旋转关节，是旋转关节，用用(*)式求本列各元素式求本列各元素 T6d3xnx pyny pxc3(c4c5c6 s4s6)

49、s3s5c6(a3s3 d4c3) s3(c4c5c6 s4s6)c3s5c6(a3c3d4s3) T6d3yox pyoy px c3(c4c5s6s4c6)s3s5s6(a3s3 d4c3) s3(c4c5s6s4c6) c3s5s6(a3c3d4s3) T6d3zax pyay px (c3c4c5s3c5)(a3s3 d4c3)(s3c4c5 c3c5)(a3c3d4s3)化简得：化简得： T6d3xd4(c4c5c6 s4s6)a3s5c6 T6d3yd4(c4c5c6s4c6) a3s5s6 T6d3zd4c4s5 a3c554 第第3列列后三个元素后三个元素为：为：T6 3xnz

50、s4c5c6c4s6 T6 3yozs4c5c6c4c6 T6 3zazs4s6 用同样方法可以得到本机械手用同样方法可以得到本机械手J(q)阵的阵的第第2列列和和第第1列列，结果如下：结果如下： T6/ 2 (a2s3d4)(c4c5c6 s4s6)(a2c3a3) s5c6 (a2s3d4)(c4c5c6s4c6) (a2c3a3) s5s6 d4c4s5 a2c3c6a2s3c4s5 a3c5 s4c5c6c4s6 s4c5s6c4c6 s4s5 55 T6/ 1(s4c5c6c4s6)(a2c2d4s23) (s4c5c6 c4s6)(a2c2d4s23) s4s5(a2c2d4s23

51、) s23(c4c5c6 s4s6)c23s5s6 s23(c4c5c6s4s6) c23s5s6 s23c4c5c23c5 于是用于是用6所表示的所表示的V80机械手的机械手的 TJ(q)如下如下 100)()(0)(000)(000)()()()(000)()()()(5545452354236656465464654652364654236656465464654652364654235354453543263245442322546536465446533264654432423226465465364654465332646544324232264654cssssccscscsscc

52、scsccscscsccssccsscsscccsscccssscsscccscascdcascsaccadscdscasscsasscccdssacacscccdsadscaccscscsasscccdcsacasscccdsadscascccs564.5 力雅可比力雅可比 机器人与外界环境相互作用时，在接触处要产生力机器人与外界环境相互作用时，在接触处要产生力 f 和力矩和力矩n，统称为，统称为末端广义末端广义(操作操作)力力矢量。记为矢量。记为 F f n (4.30)如：如：手臂提取重物手臂提取重物时承受的外力和力矩；时承受的外力和力矩；手爪抓物体手爪抓物体的作的作用力和力矩；用力和力

53、矩；步行机构与地面步行机构与地面的作用力和力矩。的作用力和力矩。 在静止状态，在静止状态，F应与各关节的驱动力应与各关节的驱动力(或力矩或力矩)相平衡。相平衡。n个关节的驱动力个关节的驱动力(或力矩或力矩)组成的组成的n维矢量维矢量 1, 2, , nT (4.31)称为称为关节力矢量关节力矢量。令各关节的虚位移为。令各关节的虚位移为 qi，末端执行器相，末端执行器相应的虚位移为应的虚位移为D。用虚功原理就可导出。用虚功原理就可导出 F 的关系。的关系。57 各关节所作虚功的总和各关节所作虚功的总和W1 T q 1 1n i qi应与末端执应与末端执行器所作虚功的行器所作虚功的W2FTDf T

54、dnT 相等相等(总虚功为零总虚功为零)，即即 T qFTD (4.32) 将式将式(4.20)代入上式代入上式(4.32)可得出可得出 JT(q)F (4.33)式中，式中，JT(q)称为操作臂的称为操作臂的力雅可比力雅可比，表示在静平衡时，表示在静平衡时，F向向 映射的线性关系。映射的线性关系。可以看出可以看出：力雅可比运动雅可比的：力雅可比运动雅可比的转置，即操作臂的静力传递关系与速度有关。式转置，即操作臂的静力传递关系与速度有关。式( (4.19)和式和式(4.33) )具有对偶性。图具有对偶性。图4 44 4画出了画出了关节关节与与操作操作空间的空间的速度速度及及静力静力映射的线性关

55、系。映射的线性关系。 DT qTJT(q) qT DTF qT qTJT(q)F58 图图中，中，n是关节数，是关节数，m是操作空间维数。是操作空间维数。J(q)J(q)mn。对给定的对给定的q，J(q)的值域空间的值域空间R(J(q)代表关节运动能够产生代表关节运动能够产生的全部操作速度的集合。当的全部操作速度的集合。当J(q)退化时，操作臂处于奇异退化时，操作臂处于奇异形位。形位。图图44 速度和静力的线性映射速度和静力的线性映射 另一方面另一方面J(q)的零空的零空间间N(J(q)表示不产生操作表示不产生操作速度的关节速度的集合。速度的关节速度的集合。若若N(J(q)不只含有不只含有0，

56、则对，则对给定的操作速度，关节速给定的操作速度，关节速度的反解有无限多。度的反解有无限多。 59 静力映射是从静力映射是从m维操作空间向维操作空间向n维关节空间的映射，其维关节空间的映射，其零空间零空间N(JT(q)代表不需要任何关节驱动力代表不需要任何关节驱动力(矩矩)，就能承受就能承受的所有操作力的集合。即：末端操作力完全由机构本身承的所有操作力的集合。即：末端操作力完全由机构本身承受。受。 值域空间值域空间R(JT(q)则是则是F F 能平衡的所有能平衡的所有 矢量的集合。矢量的集合。 60 由线性代数可知，零空间由线性代数可知，零空间N(J(q)是值空间是值空间R(JT(q)在在n维关

57、节空间的维关节空间的正交补正交补，即对非零的，即对非零的q N(J(q)，则有，则有q R(JT(q)，反之亦然。，反之亦然。 物理含义物理含义：在不产生：在不产生操作速度的那些关节速度操作速度的那些关节速度方向上，关节力矩不能被方向上，关节力矩不能被操作力所操作力所平衡平衡。为使操作。为使操作臂保持静止不动，在零空臂保持静止不动，在零空间间N(J(q)的关节力矢量必的关节力矢量必须为零。须为零。 61 在在m维操作空间中存在着相似的对偶关系，维操作空间中存在着相似的对偶关系，R(J(q)是是N(JT(q)在操作空间的正交补。在操作空间的正交补。不能由关节运动产生的不能由关节运动产生的那些操作

58、运动的方向恰恰是不需要那些操作运动的方向恰恰是不需要 来平衡的来平衡的F 的方向。的方向。反之，若外力作用的方向是末端执行器能够运动的方向，反之，若外力作用的方向是末端执行器能够运动的方向，则外力就由则外力就由 来平衡。来平衡。 当当J(q)退化时，操作臂处于奇异形位，零空间退化时，操作臂处于奇异形位，零空间N(JT(q)不只包含不只包含0，因而外力可能承受在操作臂机构本身上。，因而外力可能承受在操作臂机构本身上。 利用瞬时运动和静力的对偶关系、可从瞬时运动关系利用瞬时运动和静力的对偶关系、可从瞬时运动关系导出相应的静力关系。由式导出相应的静力关系。由式(4.18)可导出可导出A和和B间广义操

59、间广义操作力的坐标变换作力的坐标变换关系关系nfRRpRnfAABABAABBABB)(00s624.6 奇异性和灵巧度奇异性和灵巧度* 一、一、 J(q)的奇异性的奇异性 操作臂操作臂J(q)依赖于形位依赖于形位q，与奇异形位，与奇异形位q对应的对应的J(q)6n是不满秩的，此时有是不满秩的，此时有 Rank(J(q)min(6, n) (4.33)对应操作空间中的点对应操作空间中的点XX(q)为工作空间的奇异点。在奇为工作空间的奇异点。在奇异形位处，操作臂丧失操作自由度。机器人的奇异形位有异形位处，操作臂丧失操作自由度。机器人的奇异形位有两类：两类：63 (1)边界奇异形位。平面边界奇异形

60、位。平面2R机械手机械手(例例4.1)当当s20时，时， 20 和和 2180 处于工作空间的边界，只有一个操作自处于工作空间的边界，只有一个操作自由度。这种奇异点容易避免由度。这种奇异点容易避免, ,不至于带来很大麻烦。不至于带来很大麻烦。 (2)内部奇异形位。由两内部奇异形位。由两( (或多或多) )个关节轴线重合造成个关节轴线重合造成的，操作臂各关节的，操作臂各关节运动相抵消运动相抵消，不产生操作运动。，不产生操作运动。 如如PUMA 560机械手：机械手： 在在 390 附近，手臂伸直，处于附近，手臂伸直，处于边界边界奇异状态；奇异状态； 当当 50 时，关节时，关节4和和6的轴线重合