机器人模型与控制-3运动学速度关系

3. 微分运动与雅可比 （ 速度 关系） 3.1 引例 例 3-1 图示 R-P平面机械手，有两个关节，一个旋转关节（ ），一个移 动关节（ r）。 运动方程为 方程两边对时间 t 求导 写成矩阵形式 s in c os ry rx s inc o s c o ss in rry rrx T y x X T r q s inc o s c o ss in r rJ qJX rr r y x s inc o s c o ss in 运动方程为 方程两边对时间 t 求导 写成矩阵形式 s in c os ry rx s inc o s c o ss in rry rrx T y x X T r q s inc o s c o ss in r rJ qJX rr r y x s inc o s c o ss in rx c o s r rx 01 c o ss in 01 c o ss in rJ 例 3-2 图示 2R平面机械手，有两个平行的转动关节 (1, 2) 运动方程为 方程两边对时间 t 求导 写成矩阵形式 21211 21211 s ins in c o sc o s lly llx 2212121211 2212121211 c o sc o sc o s s i ns i ns i n llly lllx qJX 21221211 21221211 c o sc o sc o s s i ns i ns i n lll lllJ 2 1 21221211 21221211 c o sc o sc o s s i ns i ns i n lll lll y x 逆雅可比矩阵为 ，即 2 0或 时处于奇异状态，此时完全 伸直或完全缩回。 当 l1l2时，可达工作空间为两个同心圆中间的部分，半径 分别为 l1 l2和 l1-l2。 在边界上机器人处于奇异形位（ singular configuration）， 速度关系方程变为 (若 2 0) 退化为一个自由度，其末端只能沿圆切线方向运动。 0s in 221 llJ 2121121211 212212 221 1 s i ns i nc o sc o s s i nc o s s i n 1 llll ll llJ 2 1 12121 12121 c o sc o s s ins in lll lll y x 1 2 l 1 l2 l 1+ l2 l1- l2 逆雅可比矩阵为 ，即 2 0或 时处于奇异状态，此时完全 伸直或完全缩回。 当 l1l2时，可达工作空间为两个同心圆中间的部分，半径 分别为 l1 l2和 l1-l2。 在边界上机器人处于奇异形位（ singular configuration）， 速度关系方程变为 (若 2 0) 退化为一个自由度，其末端只能沿圆切线方向运动。 0s in 221 llJ 2121121211 212212 221 1 s i ns i nc o sc o s s i nc o s s i n 1 llll ll llJ 2 1 12121 12121 c o sc o s s ins in lll lll y x 1 2 l 1 l 2 l 1 + l 2 l 1 - l 2 逆雅可比矩阵为 ，即 2 0或 时处于奇异状态，此时完全 伸直或完全缩回。 当 l1l2时，可达工作空间为两个同心圆中间的部分，半径 分别为 l1 l2和 l1-l2。 在边界上机器人处于奇异形位（ singular configuration）， 速度关系方程变为 (若 2 0) 退化为一个自由度，其末端只能沿圆切线方向运动。 0s in 221 llJ 2121121211 212212 221 1 s i ns i nc o sc o s s i nc o s s i n 1 llll ll llJ 2 1 12121 12121 c o sc o s s ins in lll lll y x 1 2 l 1 l2 l 1+ l2 l1- l2 从例子可以看出： （ 1）将机器人的运动学方程对时间求导，即可得到它的 雅可比矩阵和逆雅可比矩阵； （ 2）雅可比矩阵表示从关节空间运动到操作空间运动速 度传递的广义传动比； （ 3）用雅可比矩阵可以判别机器人的奇异形位； （ 4）用雅可比矩阵可以分析机器人的运动特征和动力学 特征。 雅可比矩阵具有如下特点： （ 1）依赖于机器人形位 q的线性变换矩阵； （ 2）不一定是方阵，可能是长矩阵（冗余驱动），也可能 是高矩阵（欠驱动或少自由度） ； （ 3）其行数等于机器人在操作空间的维数（平面 3行，空间 6行），列数等于关节数； 对于一般的 6自由度机器人，其雅可比矩阵的计算比较 复杂。操作空间与关节空间之间的速度具有如下形式 qqJv 雅可比矩阵的含义： （ 1）空间操作臂雅可比矩阵的前 3行代表对手爪线速度的传 递，后 3行代表对手爪角速度的传递，每一列代表相应的 关节速度对手爪线速度和角速度的影响。 （ 2）手爪的线速度和角速度为关节速度的线性函数 （ 3）机器人的雅可比矩阵可写成分块的形式 式中： JLi代表第 i个关节的速度引起的手爪的线速度； JAi代表第 i个关节的速度引起的手爪的角速度。 n n n q q q qJqJqJ qJqJqJ v 2 1 AA2A1 LL2L1 雅可比矩阵的确定通常采用两种构造性的方法： （ 1）矢量积法 基于矢量的叉积推导机器人的雅可比， 是相对于基坐标系表示的； （ 2）微分变换法 利用操作空间与关节空间中的微分运 动关系构造机器人的雅可比，是相对于运动坐标系（通常 为末端坐标系）表示的。 在给出两种构造性方法之前，分别先介绍相关的 理论基础。 3.2 变换矩阵的导数 3.2.1 反对称矩阵 设 S是一个 n n的矩阵，如果 S满足 ，则称 S为 反对称矩阵 。 反对称矩阵 的对角线矩阵为 0，只有 n个独立元素； 定义 3 3反对称矩阵空间 so(3)， ，有如下形式 0 TSS 0 0 0 12 13 23 ss ss ss S 反对称矩阵与矢量有如下关系 0 0 0 xy xz yz aa aa aa S a 3soS 其中： ， S可看作是对矢量 a的运算算子 T zyx aaaa 反对称矩阵的性质 （ 1）算子 S的运算是线性的，即对于 ，和任意标量 ，有 （ 2）反对称矩阵与矢量 叉乘 的关系：对于 ，有 （ 3）对于旋转矩阵 和 ，有 3, Rba , baba SSS 3, Rpa papa S 3Ra RaRaR SS T R 反对称矩阵的性质 （ 1）算子 S的运算是线性的，即对于 ，和任意标量 ，有 （ 2）反对称矩阵与矢量 叉乘 的关系：对于 ，有 （ 3）对于旋转矩阵 和 ，有 3, Rba , baba SSS 3, Rpa papa S 3Ra RaRaR SS T R 旋转矩阵 ，满足 以下性质： 的各列（各行）是 互相垂直的 的每一列（每一行） 都是单位向量 )3(SOR )3(1 SOT RR R R 1det R 反对称矩阵的性质 （ 1）算子 S的运算是线性的，即对于 ，和任意标量 ，有 （ 2）反对称矩阵与矢量 叉乘 的关系：对于 ，有 （ 3）对于旋转矩阵 和 ，有 证明：利用 和性质（ 2），有 （ 4）对于 ，有 3, Rba , baba SSS 3, Rpa papa S 3Ra RaRaR SS T RbRabaR bRa bRa bRRRa bRaRbRaR S S T TT nRnso XS , 0SXX T R 3.2.2 旋转矩阵的导数 设旋转矩阵 R是关于单变量 的函数 （旋转变换通式） ，有 IRR T 将上式两边对 求导，得 0 TT d dd d RRRR 定义矩阵 S为 Tdd RRS 因为 ，有 ，所以 TT dd RRS 0 TSS 3soS 将矩阵 S 右乘旋转矩阵 R ，得到 SRR dd 用定义式求解 S 同理，当 R Ry,和 R Rz,分别有 和 iS S 010 100 000 c o ss i n0 s i nc o s0 001 s i nc o s0 c o ss i n0 000 对于 R Rx,，有 Tdd RRS jS S kS S 对于一般情况 R RK,，利用旋转变换通式，有 0 0 0 xy xz yz kk kk kk S KS 3.2.3 角速度 设旋转矩阵 R是关于单变量 的函数，对时间求导有 tS tSdtdtSdtdtd dt R RKRKRR 其中 K S kk kk kk S xy xz yz xy xz yz 0 0 0 0 0 0 对于多级旋转变换 RRR 1 20102 RRRRR 1201120102 RRRRR 122,110112011,00022,00 SSS RRRRRRR 1201012,110112011,00022,00 TSSS RRRRRR 12012,110112011,00022,00 SSS RRRR 022,1101021,00022,00 SSS 2,11011,002,00 R SSS 利用 ，得到 baba SSS 2,11011,002,00 R 进一步扩展，得到 nn nn n nn ,1 0 2,1 0 1,0 0 ,1 10 12,1 10 11,0 0 ,0 0 RR 2,11011,002,00 R SS 对于齐次变换矩阵 10 00 0 nn n PRT 由其中的旋转矩阵 即可求得上述角速度关系式，它表示末端连 杆角速度与各相邻连杆间角速度的关系。 R0n 3.2.4 线速度 设末端手抓在坐标系 n中的位置矢量为 ，通过齐次变换得到 将 展开 enP nenne PPRP 000 enne PRP 00nP0 将其求导得到末端手抓的线速度 0 21011010 1 0 2 10 1 10 1 00 PRPRPR PPRPRPRP n n ne n n n n ne n ne 21011,001011,000,00 1 0 2 10 1 10 1 00 PRPRPR PPRPRPRP SSS nnnnennn n n ne n ne 2,101,00,101,202,101,00,0,102,101,00 1 0 2 10 1 10 1 00 PPP PPRPRPRP SSS nnnnennn n n ne n ne ennnnnennnen n n ne n ne SSS ,0,103,20,10,02,102,10,10,01,00 1 0 2 10 1 10 1 00 PPPPPPP PPRPRPRP ennnee n n ne SSS ,0,10,202,10,101,00 10 12 10 11 00 PPP PRPRPP 102101101 PPRPR nnn nnnn PRP 1010 10 nP 120210 nnnn PRP 20 P 10210120 PPRP 3.2.5 机械臂末端手爪的速度 末端手抓的角速度与连杆 n的角速度一样； 末端手抓的线速度是指手爪上的点 e的速度； ( 2 ) , 0 ,1 010 1 ,2 0 2,1 0 2 10 1 ,1 0 1,0 0 1 00 ennnn n n e ee S S S PPR PPR PPP ( 1 ) ,11012,11011,00,00 nnnnn RR 当关节 i为移动关节时，上面 (1)式中 上面 (2)式中 0,1101 iiii R 当关节 i为转动关节时，上面 (2)式中 0,0,10 eiiiS P 0101 iii PR ( 2) 1 0 0 1 0 0 1 0 0 , 000 2,2 0 2 0 22 0 1,1 0 1 0 11 0 , 0010 1,2 0 22 0 2 1 2 0 1,1 0 11 0 1 0 1 0 nennnnee ennnn n nneee ddd SdSdSd PZZPZZPZZ PZRRPZRRPZRP ( 1) 1 0 0 1 0 0 1 0 0 0 22 0 11 0 10 12 1 2 0 11 0 1,0 0 nn n n nnn ZZZ RRRRR 当关节 i为移动关节时 0i 当关节 i为转动关节时 0 id 以关节 i为研究对象； 根据关节运动形式的不同： （ 1） 移动关节 i以速度 运动，末 端手爪的线速度与 Zi轴方向相同， 角速度为零 雅可比矩阵的第 i列 （移动关节） Zi通过计算 并取其第 3列前 3个 元素组成矢量来获得 iq eiP i i q 0 Z v 0ii ZJ T0i iq 3.3 矢量积法求雅可比 1972年 Whitney基于参考坐标系的概念提出来的。 （ 2） 转动关节 i以速度 运动，末端 手爪 产生的角速度为 ， 产生的线速度为 雅可比矩阵的第 i列 (移动关节 ) 为末端手爪坐标原点相对坐标 系 i的位置矢量在 0中的表示； 为末端手爪坐标原点相对坐标 系 i的位置矢量在 i中的表示； 可通过取 中的位置矢量获得， 进而通过公式 获得 Zi通过计算 并取其第 3列前 3个 元素组成矢量来获得 iq eiP iq ii qZ 0eiii q PZv i e i ii i e i i i Z PRZ Z PZJ 00 0eiP eiP eiP T ie eiiei PRP 00 0eiP T0i 矢量积法求雅可比矩阵的步骤： 建立连杆坐标系，求得 从关节 0开始计算 按照上面的方法对移动关节和转动关节分别求得 然后组成 矢量积法得到的雅可比矩阵是相对于参考坐标系的； 如果想获得动坐标系（手爪坐标系）的速度，可进一步做 如下变换 、T01 qqJRRvRRv e e e e e e 0 0 10 10 0 0 0 0 、T12 、T1i i 、T1n n Tne 、T0i Tie iJ qJqJqJ qJqJqJJ n n AA2A1 LL2L1 3.4 微分转动与角速度 绕 X轴、 Y轴或 Z轴转 角的旋转变换矩阵分别是 c o ss in0 s inc o s0 001 ,R X c o s0s in 010 s in0c o s ,R Y 100 0c o ss in 0s inc o s ,R Z 角度 很小时，把它当成微量，称为微分转动。 绕 X、 Y、 Z轴转动的微分角度记为 x、 y、 z。 根据下列性质 微分转动变换为 1c o s,1c o s,1c o s s in,s in,s in zyx zzyyxx 10 10 001 ,R x xxX 10 010 01 ,R y y yY 100 01 01 ,R z z zZ 微分转动变换可以看成是以上三个变换的复合作用，将三 个矩阵相乘，并略去高阶（ 2）微量，得 根据旋转变换通式，得到微分转动变换的另一种形式 二者是等价的 1 1 1 ,R,R,R xy xz yz zyx ZYX 1 1 1 ,R xy xz yz kk kk kk K 对比两种微分转动变换矩阵，可以得到如下关系 微分旋转变换具有以下性质： （ 1）具有交换律（一般旋转变换不具有这一性质） （ 2）绕任一矢量的微分转动与绕 X、 Y和 Z轴的微分转动等 价。 zzyyxx zyx zzyyxx kkk kkk , , 222 xyyx XYYX ,R,R,R,R 利用微分转动推导旋转变换矩阵的导数 可看成是 经过微分旋转变换得到的 进一步 tttt tz z ty y tx x t limlimlimlim 0000 , z y x z y x k k k t t t ttt tt RRRR limlim 00 tt R tR ttt RKR ,R tt RR 称为微分旋转算子 旋转矩阵的导数 角速度算子矩阵 如果已知旋转变换矩阵表达式，计算 ，并与 对 应元素相等，可得刚体的角速度 0 0 0 ,R 3 xy xz yz kk kk kk IK tSt tt t tt RRRR l i ml i m 00 0 0 0 xy xz yz S 1RR S xyxyxyz zxzxzxy yzyzyzx aaoonn aaoonn aaoonn 3.5 微分运动与广义速度 微分运动：微分移动和微分转动 对应广义速度（线速度和角速度） 操作臂由位姿 T(t)经过微分转动和微分移动后到达 T(t+t) 相对于参考系的微分运动为 微分运动算子 定义微分运动矢量 广义速度为 tdddtt zyx TKT ,R o t,T r a n s TTIKT tdddd zyx 4,R o t,T r a n s 0000 0 0 0 zxy yxz xyz d d d dD Tzyxzyx ddd vDV T 0l i m zyxzyxt vvvt 相对于动坐标系，微分变换矩阵为 相应的微分运动为 微分运动算子 微分运动矢量 广义速度为 TTTTT ,R o t,T r a n s KTT zyx dddttt T4TTTTT ,R o t,T r a n s TIKTT zyx dddtd 0000 0 0 0 TTT TTT TTT T zxy yxz xyz d d d dD TTTTTTTTTT zyxzyx ddd vDV T TT TTTTTTT 0 T lim zyxzyx t vvvt 齐次变换的导数 如果齐次变换矩阵已知， 由式 和 可以求得相对参考系和动系的广义速度 VTTVTTTT T 00 limlim SttSt tdt ttt tt 0000 0 0 0 zxy yxz xyz v v v S V 0000 0 0 0 TTT TTT TTT T zxy yxz xyz v v v S V 1 TTV S TTV 1TS 例 手爪的位姿为 相对于基坐标系的微分移动和微分转动分别为 1 0 0.5T和 0.1 0 0T，求相对于基系微分运动。 1000 0010 15001 5100 T 0000 5.001.00 01.000 1000 0000 2001.0 001.00 1000 TTd 如果相对于动坐标系的微分移动和微分转动分别为 1 0 0.5T和 0.1 0 0T，求相对于动系的微分运动。 0000 5.001.00 01.000 1000 T 0000 01.000 1000 5.001.00 TTTd 3.6 微分运动的等价坐标变换 利用齐次变换矩阵求导来求广义速度的方法，需要求齐次 变换矩阵的导数和逆，在实际中对于复杂模型难于应用。 微分变换法：利用同一微分运动在不同坐标系下的描述的 等价关系求广义速度。 TT TTTT TTdd 求解 ，实际上是对 做相似变换 10000 1 31 31 TT TT TT 1T Paon 0 d 0 Paa Poo Pnn TT S 0000 T dPaaaoana dPoaooono dPnanonnn T 矢量三重混合积的性质 各方向矢量间的正交性和规一化条件 同一微分运动在动系和基系下的微分运动算子之间的关系 可以化简为 acbcabcba 0 caa aon nao ona 0000 0 0 0 T adaPno odoPna ndnPoa 根据前面定义 将两式右边对应元素相等可得到动系和基系下微分运动的 等价坐标变换 0000 0 0 0 TTT TTT TTT T zxy yxz xyz d d d aa oo nn daaP dooP dnnP z y x z y x d d d T T T T T T 写成矩阵形式 简写为 z y x z y x zyx zyx zyx zyxzyx zyxzyx zyxzyx z y x z y x d d d aaa ooo nnn aaa ooo nnn d d d 000 000 000 T T T T T T aPaPaP oPoPoP nPnPnP d R PSRR d T 33 TT T T 0 进一步推广，得到任意两坐标系 A和 B中微分运动矢量 的等价变换 两个坐标系中的速度等价关系 d R PSRR d A A TA B33 OB ATA B TA B B B 0 v R PSRR v A A TA B33 OB ATA B TA B B B 0 例 手爪的位姿为 相对于基坐标系的微分移动和微分转动分别为 1 0 0.5T和 0.1 0 0T， 求相对于动系的等价微分运动。 d R PSRR d T 33 TT T T 0 1000 0010 15001 5100 T T201 dP TT TT 1.000 120 d R PdRPSdRd TT TTT 3.7 微分变换法求雅克比 根据微分运动的等价坐标变换，速度在动系和基系之间存 在如下等价变换关系 利用上述速度等价关系，我们来推导微分变换法求雅克比 的算法。 根据关节运动形式不同，分为两种情况： z y x z y x zyx zyx zyx zyxzyx zyxzyx zyxzyx z y x z y x v v v aaa ooo nnn aaa ooo nnn v v v 000 000 000 T T T T T T aPaPaP oPoPoP nPnPnP （ 1）对于转动关节 i，若关节速度为 ，则连杆 i的速度和角 速度矢量在坐标系 i为 根据速度等价关系，该关节速度产生的手爪的速度（在动 系中 ） i T000vi T00 ii i z z z z z z z y x z y x a o n v v v aP oP nP T T T T T T izyx zyx zyx zyxzyx zyxzyx zyxzyx z y x z y x aaa ooo nnn aaa ooo nnn v v v 0 0 0 0 0 000 000 000 T T T T T T aPaPaP oPoPoP nPnPnP （ 2）对于移动关节 i，若关节速度为 ，则连杆 i的速度和角 速度矢量在坐标系 i为 根据速度等价关系，该关节速度产生的手爪的速度（在 动系中 ） id T00 ii dv T000i i z z z z y x z y x d a o n v v v 0 0 0 T T T T T T 0 0 0 0 0 000 000 000 T T T T T T i zyx zyx zyx zyxzyx zyxzyx zyxzyx z y x z y x d aaa ooo nnn aaa ooo nnn v v v aPaPaP oPoPoP nPnPnP （ 1）和（ 2）中 n,o,a,P的是 的 4个列矢量 雅可比（相对于动系）的第 i列为 转动关节： 移动关节： 组合起来 i i i A T L T T J JJ z z z i aP oP nP J LT z z z i a o n A T J i i i A T L T T J JJ z z z i a o n L T J 0 0 0 A T iJ n n A T 2A T 1A T L T 2L T 1L T T JJJ JJJJ Tie 这种计算雅可比的方法是构造性的，从 n到 1依次计算齐 次变换矩阵 ,使用其中的矢量按上面公式 可构造雅克比的各列，不需要求导或解方程。 qqJRRvRRv T0000 0 00 0 e e e e e e TTTT 121 , een ene T6eT56T45T34T23T12T01 T5e T4e T3e T2e T1e 6TJ 5TJ T6e 4TJ 3TJ 2TJ 1TJ 3.8 两个实例（自学） 一、 XHK5140换刀机械手的雅可比矩阵 二、 PUMA560机器人的的雅可比矩阵 3.9 逆雅可比矩阵和奇异性 矢量积法和微分变换法得到速度正解，是由关节速度求末 端手爪速度。 如果已知手爪速度求关节速度，称为速度逆解，即 速度逆解的性质取决于雅克比矩阵的性质。 下面介绍雅克比矩阵的性质。 XJq 1 一、雅克比的性质 操作空间与关节空间的速度关系反映的是 n维关节空间 向 m维操作空间的线性映射。 线性映射的域空间 R(J)是操作空间 Rm的子空间，代表机 器人所能达到的操作速度的集合。 线性映射的零空间 N(J)是关节空间 Rn的子空间，代表不 产生操作速度的关节速度的的集合。 域空间 R(J)和零空间 N(J)的维数之和 dimR(J)+dimN(J)=n （ 1）当 nm，且 J是行满秩时，机器人为冗余自由度， 冗余度为 N(J)的维数； （ 2）当 n m，且 J是满秩时，机器人为满自由度； （ 3）当 n1，条件数越小，各向同性越好， 灵巧度越高； （ 2）最小奇异值 反映所需关节速度上限，最小奇异值 越大，操作臂末端对关节运动的反应越快； （ 3）可操作度 ，等于各奇异值的乘积。 （ w值越大，灵活性越好；当 w=0时，发生奇异状态。 ） )de t( TJJw 冗余度机器人： 从运动学的观点是指完成某一 特定任务时，机器人具有多余的自由度。 冗余度机器人的 优点 ： 冗余度机器人增加灵活性 躲避障碍物 改善动力学性能的规划算法 优化 动力学控制算法 3.11 冗余度机器人 习题 图示 3R空间机械手具有 3个旋转关节。分别用矢量积法和 微分变换法求雅克比矩阵。