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

1、13. 微分运动与雅可比微分运动与雅可比（速度23.1 引例引例例例3-1 图示R-P平面机械手，有两个关节，一个旋转关节（），一个移动关节（r）。 3运动方程为方程两边对时间 t 求导 写成矩阵形式 sincosryrxsincoscossinrryrrxTyxXTr qsincoscossinrrJqJXrrryxsincoscossin4运动方程为方程两边对时间 t 求导 写成矩阵形式 sincosryrxsincoscossinrryrrxTyxXTr qsincoscossinrrJqJXrrryxsincoscossinrxcosrrx01cossin01cossinrJ5例例3-

2、2 图示2R平面机械手，有两个平行的转动关节(1, 2) 6运动方程为方程两边对时间 t 求导 写成矩阵形式 2121121211sinsincoscosllyllx22121212112212121211coscoscossinsinsinlllylllxqJX2122121121221211coscoscossinsinsinllllllJ212122121121221211coscoscossinsinsinllllllyx7 逆雅可比矩阵为 ，即20或时处于奇异状态，此时完全伸直或完全缩回。 当l1l2时，可达工作空间为两个同心圆中间的部分，半径分别为l1l2和l1-l2。 在边界上机

3、器人处于奇异形位（singular configuration），速度关系方程变为(若20) 退化为一个自由度，其末端只能沿圆切线方向运动。 0sin221llJ21211212112122122211sinsincoscossincossin1llllllllJ211212112121coscossinsinllllllyx12l1l2l1+ l2l1- l28 逆雅可比矩阵为 ，即20或时处于奇异状态，此时完全伸直或完全缩回。 当l1l2时，可达工作空间为两个同心圆中间的部分，半径分别为l1l2和l1-l2。 在边界上机器人处于奇异形位（singular configuration），速度

4、关系方程变为(若20) 退化为一个自由度，其末端只能沿圆切线方向运动。 0sin221llJ21211212112122122211sinsincoscossincossin1llllllllJ211212112121coscossinsinllllllyx9 逆雅可比矩阵为 ，即20或时处于奇异状态，此时完全伸直或完全缩回。 当l1l2时，可达工作空间为两个同心圆中间的部分，半径分别为l1l2和l1-l2。 在边界上机器人处于奇异形位（singular configuration），速度关系方程变为(若20) 退化为一个自由度，其末端只能沿圆切线方向运动。 0sin221llJ2121121

5、2112122122211sinsincoscossincossin1llllllllJ211212112121coscossinsinllllllyx12l1l2l1+ l2l1- l210 从例子可以看出：（1）将机器人的运动学方程对时间求导，即可得到它的雅可比矩阵和逆雅可比矩阵；（2）雅可比矩阵表示从关节空间运动到操作空间运动速度传递的广义传动比；（3）用雅可比矩阵可以判别机器人的奇异形位；（4）用雅可比矩阵可以分析机器人的运动特征和动力学特征。11 雅可比矩阵具有如下特点：（1）依赖于机器人形位q的线性变换矩阵；（2）不一定是方阵，可能是长矩阵（冗余驱动），也可能是高矩阵（欠驱动或少自

6、由度） ；（3）其行数等于机器人在操作空间的维数（平面3行，空间6行），列数等于关节数； 对于一般的6自由度机器人，其雅可比矩阵的计算比较复杂。操作空间与关节空间之间的速度具有如下形式 qqJv12 雅可比矩阵的含义：（1）空间操作臂雅可比矩阵的前3行代表对手爪线速度的传递，后3行代表对手爪角速度的传递，每一列代表相应的关节速度对手爪线速度和角速度的影响。（2）手爪的线速度和角速度为关节速度的线性函数（3）机器人的雅可比矩阵可写成分块的形式式中：JLi代表第i个关节的速度引起的手爪的线速度； JAi代表第i个关节的速度引起的手爪的角速度。 nnnqqqqJqJqJqJqJqJv21AA2A1L

7、L2L113 雅可比矩阵的确定通常采用两种构造性的方法：（1）矢量积法基于矢量的叉积推导机器人的雅可比，是相对于基坐标系表示的；（2）微分变换法利用操作空间与关节空间中的微分运动关系构造机器人的雅可比，是相对于运动坐标系（通常为末端坐标系）表示的。 在给出两种构造性方法之前，分别先介绍相关的理论基础。143.2 变换矩阵的导数3.2.1 反对称矩阵反对称矩阵 设S是一个 nn的矩阵，如果S满足 ，则称S为反对称矩阵反对称矩阵。 反对称矩阵反对称矩阵的对角线矩阵为0，只有n个独立元素； 定义33反对称矩阵空间so(3)， ，有如下形式0TSS000121323ssssssS 反对称矩阵与矢量有如

8、下关系 000 xyxzyzaaaaaaS a 3soS其中： ，S可看作是对矢量a的运算算子Tzyxaaaa15 反对称矩阵的性质反对称矩阵的性质（1）算子S的运算是线性的，即对于 ，和任意标量 ，有（2）反对称矩阵与矢量叉乘的关系：对于 ，有（3）对于旋转矩阵 和 ，有 3,Rba, babaSSS3,Rpa papaS3Ra RaRaRSSTR16 反对称矩阵的性质反对称矩阵的性质（1）算子S的运算是线性的，即对于 ，和任意标量 ，有（2）反对称矩阵与矢量叉乘的关系：对于 ，有（3）对于旋转矩阵 和 ，有 3,Rba, babaSSS3,Rpa papaS3Ra RaRaRSSTR旋转矩

9、阵 ，满足以下性质： 的各列（各行）是互相垂直的 的每一列（每一行）都是单位向量 )3(SOR)3(1SOTRRRR 1detR17 反对称矩阵的性质反对称矩阵的性质（1）算子S的运算是线性的，即对于 ，和任意标量 ，有（2）反对称矩阵与矢量叉乘的关系：对于 ，有（3）对于旋转矩阵 和 ，有 证明：利用 和性质（2），有（4）对于 ，有3,Rba, babaSSS3,Rpa papaS3Ra RaRaRSSTRbRabaR bRabRabRRRabRaRbRaRSSTTT nRnsoXS,0SXXTR183.2.2 旋转矩阵的导数旋转矩阵的导数 设旋转矩阵R是关于单变量的函数（旋转变换通式），

10、有 IRRT将上式两边对求导，得 0TTddddRRRR定义矩阵S为 TddRRS 因为 ，有 ，所以 TTddRRS0TSS 3soS将矩阵S 右乘旋转矩阵R ，得到 SRR dd19 用定义式求解S同理，当RRy,和 RRz,分别有 和 iSS010100000cossin0sincos0001sincos0cossin0000 对于RRx,，有 TddRRS jSS kSS 对于一般情况RRK,，利用旋转变换通式，有 000 xyxzyzkkkkkkS KS203.2.3 角速度角速度 设旋转矩阵R是关于单变量的函数，对时间求导有 tStSdtdtSdtdtddtRRKRKRR 其中 K

11、SkkkkkkSxyxzyzxyxzyz000000 对于多级旋转变换对于多级旋转变换RRR120102RRRRR1201120102RRRRR122, 110112011 , 00022, 00SSSRRRRRRR1201012, 110112011 , 00022, 00TSSSRRRRRR12012, 110112011 , 00022, 00SSSRRRR022, 1101021 , 00022, 00SSS212, 11011 , 002, 00RSSS利用 ，得到 babaSSS2, 11011 , 002, 00R进一步扩展，得到nnnnnnn, 102, 101 , 00, 1

12、1012, 11011 , 00, 00 RR2, 11011 , 002, 00R SS对于齐次变换矩阵10000nnnPRT 由其中的旋转矩阵 即可求得上述角速度关系式，它表示末端连杆角速度与各相邻连杆间角速度的关系。R0n223.2.4 线速度线速度 设末端手抓在坐标系n中的位置矢量为 ，通过齐次变换得到将 展开enPnennePPRP000ennePRP00nP0将其求导得到末端手抓的线速度0 2101101010210110100PRPRPRPPRPRPRPnnnennnnnenne21011 , 001011, 000, 0010210110100 PRPRPRPPRPRPRPSS

13、Snnnnennnnnnenne2, 101 , 00, 101, 202, 101 , 00,0, 102, 101 , 0010210110100 PPPPPRPRPRPSSSnnnnennnnnnenneennnnnennnennnnenneSSS,0, 103 , 20, 10,02, 102, 10, 10,01 , 0010210110100 PPPPPPPPPRPRPRPennneennneSSS,0, 10, 202, 10, 101 , 001012101100 PPPPRPRPP102101101PPRPRnnnnnnnPRP101010nP120210nnnnPRP20n

14、P10210120PPRP233.2.5 机械臂末端手爪的速度机械臂末端手爪的速度 末端手抓的角速度与连杆n的角速度一样； 末端手抓的线速度是指手爪上的点e的速度；(2) ,0, 10101, 202, 102101, 101 , 00100ennnnnneeeSSSPPRPPRPPP(1) , 11012, 11011 , 00, 00nnnnnRR 当关节i为移动关节时，上面 (1)式中 上面 (2)式中0, 1101iiiiR 当关节i为转动关节时，上面 (2)式中 0,0, 10eiiiSP0101iiiPR24(2) 100100100,0002, 20202201, 1010110

15、,00101, 2022021201, 101101010nennnneeennnnnnneeedddSdSdSdPZZPZZPZZPZRRPZRRPZRP(1) 100100100022011010121201101, 00nnnnnnnZZZRRRRR 当关节i为移动关节时 0i 当关节i为转动关节时0id25以关节i为研究对象；根据关节运动形式的不同：（1）移动关节移动关节i以速度 运动，末端手爪的线速度与Zi轴方向相同，角速度为零雅可比矩阵的第i列 （移动关节） Zi通过计算 并取其第3列前3个元素组成矢量来获得iq eiPiiq 0Zv0iiZJT0iiq 3.3 矢量积法求雅可比

16、1972年Whitney基于参考坐标系的概念提出来的。26（2）转动关节转动关节i以速度 运动，末端手爪产生的角速度为 ，产生的线速度为雅可比矩阵的第i列 (移动关节) 为末端手爪坐标原点相对坐标系i的位置矢量在0中的表示； 为末端手爪坐标原点相对坐标系i的位置矢量在i中的表示； 可通过取 中的位置矢量获得，进而通过公式 获得 Zi通过计算 并取其第3列前3个元素组成矢量来获得iq eiPiq iiq Z 0eiiiqPZvieiiiieiiiZPRZZPZJ000eiPeiPeiPTieeiieiPRP000eiPT0i27 矢量积法求雅可比矩阵的步骤： 建立连杆坐标系，求得 从关节0开始计

17、算 按照上面的方法对移动关节和转动关节分别求得 然后组成 矢量积法得到的雅可比矩阵是相对于参考坐标系的； 如果想获得动坐标系（手爪坐标系）的速度，可进一步做如下变换 、T01 qqJRRvRRveeeeee0010100000、T12、T1ii、T1nnTne、T0iTieiJ qJqJqJqJqJqJJnnAA2A1LL2L1283.4 微分转动与角速度 绕X轴、Y轴或Z轴转角的旋转变换矩阵分别是 cossin0sincos0001,R Xcos0sin010sin0cos,R Y1000cossin0sincos,RZ29 角度很小时，把它当成微量，称为微分转动。 绕X、Y、Z轴转动的微分

18、角度记为x、y、z。 根据下列性质 微分转动变换为 1cos, 1cos, 1cossin,sin,sinzyxzzyyxx1010001,RxxxX1001001,RyyyY1000101,RzzzZ30 微分转动变换可以看成是以上三个变换的复合作用，将三个矩阵相乘，并略去高阶（2）微量，得 根据旋转变换通式，得到微分转动变换的另一种形式 二者是等价的111,R,R,RxyxzyzzyxZYX111,RxyxzyzkkkkkkK31 对比两种微分转动变换矩阵，可以得到如下关系 微分旋转变换具有以下性质： （1）具有交换律（一般旋转变换不具有这一性质） （2）绕任一矢量的微分转动与绕X、Y和Z

19、轴的微分转动等价。 zzyyxxzyxzzyyxxkkkkkk,222xyyxXYYX,R,R,R,R32 利用微分转动推导旋转变换矩阵的导数 可看成是 经过微分旋转变换得到的 进一步 tttttzztyytxxtlimlimlimlim0000,zyxzyxkkk ttttttttRRRRlimlim00ttR tR tttRKR,R ttRR33 称为微分旋转算子 旋转矩阵的导数 角速度算子矩阵 如果已知旋转变换矩阵表达式，计算 ，并与 对应元素相等，可得刚体的角速度 000,R3xyxzyzkkkkkkIK tSttttttRRRRlimlim00 000 xyxzyzS1 RR Sxy

20、xyxyzzxzxzxyyzyzyzxaaoonnaaoonnaaoonn343.5 微分运动与广义速度 微分运动：微分移动和微分转动 对应广义速度（线速度和角速度） 操作臂由位姿T(t)经过微分转动和微分移动后到达T(t+t) 相对于参考系的微分运动为 微分运动算子 定义微分运动矢量 广义速度为 tdddttzyxTKT,Rot,Trans TTIKTtddddzyx4,Rot,Trans0000000zxyyxzxyzddddDTzyxzyxdddvDVT0limzyxzyxtvvvt35 相对于动坐标系，微分变换矩阵为 相应的微分运动为 微分运动算子 微分运动矢量 广义速度为 TTTTT

21、,Rot,TransKTTzyxdddttt T4TTTTT,Rot,TransTIKTTzyxdddtd0000000TTTTTTTTTTzxyyxzxyzddddDTTTTTTTTTTzyxzyxdddvDVTTTTTTTTTT0Tlimzyxzyxtvvvt36 齐次变换的导数 如果齐次变换矩阵已知， 由式 和 可以求得相对参考系和动系的广义速度 VTTVTTTTT00limlimSttSttdtttttt 0000000zxyyxzxyzvvvSV0000000TTTTTTTTTTzxyyxzxyzvvvSV 1TTVSTTV1TS37例例 手爪的位姿为相对于基坐标系的微分移动和微分转

22、动分别为1 0 0.5T和0.1 0 0T，求相对于基系微分运动。 10000010150015100T00005 . 001 . 0001 . 000100000002001 . 0001 . 001000TTd38如果相对于动坐标系的微分移动和微分转动分别为1 0 0.5T和0.1 0 0T，求相对于动系的微分运动。 00005 . 001 . 0001 . 0001000T000001 . 00010005 . 001 . 00TTTd393.6 微分运动的等价坐标变换 利用齐次变换矩阵求导来求广义速度的方法，需要求齐次变换矩阵的导数和逆，在实际中对于复杂模型难于应用。 微分变换法：利用

23、同一微分运动在不同坐标系下的描述的等价关系求广义速度。 TTTTTTTTdd 求解 ，实际上是对 做相似变换 1000013131TTTTTT1TPaon0d0PaaPooPnnTTS0000TdPaaaoanadPoaooonodPnanonnnT40 矢量三重混合积的性质 各方向矢量间的正交性和规一化条件 同一微分运动在动系和基系下的微分运动算子之间的关系可以化简为acbcabcba0caaaonnaoona0000000TadaPnoodoPnandnPoa41 根据前面定义 将两式右边对应元素相等可得到动系和基系下微分运动的等价坐标变换 0000000TTTTTTTTTTzxyyxzx

24、yzdddaaoonndaaPdooPdnnPzyxzyxdddTTTTTT42 写成矩阵形式 简写为 zyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxdddaaaooonnnaaaooonnnddd000000000TTTTTTaPaPaPoPoPoPnPnPnP dRPSRRdT33TTTT043 进一步推广，得到任意两坐标系A和B中微分运动矢量的等价变换 两个坐标系中的速度等价关系 dRPSRRdAATAB33OBATABTABBB0vRPSRRvAATAB33OBATABTABBB044例例 手爪的位姿为相对于基坐标系的微分移动和微分转动分别为 1 0 0

25、.5T和0.1 0 0T，求相对于动系的等价微分运动。 dRPSRRdT33TTTT010000010150015100TT201dPTTTT1 . 000120d RPdRPSdRdTTTTT453.7 微分变换法求雅克比 根据微分运动的等价坐标变换，速度在动系和基系之间存在如下等价变换关系 利用上述速度等价关系，我们来推导微分变换法求雅克比的算法。 根据关节运动形式不同，分为两种情况：zyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxvvvaaaooonnnaaaooonnnvvv000000000TTTTTTaPaPaPoPoPoPnPnPnP46（1）对于转

26、动关节i，若关节速度为 ，则连杆i的速度和角速度矢量在坐标系i为 根据速度等价关系，该关节速度产生的手爪的速度（在动系中 ）iT000viT00iiizzzzzzzyxzyxaonvvvaPoPnPTTTTTTizyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxaaaooonnnaaaooonnnvvv00000000000000TTTTTTaPaPaPoPoPoPnPnPnP47（2）对于移动关节i，若关节速度为 ，则连杆i的速度和角速度矢量在坐标系i为 根据速度等价关系，该关节速度产生的手爪的速度（在动系中 ） idT00iidvT000iizzzzyxzyxdaonvvv000TTTTTT00000000000000TTTTTTizyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxdaaaooonnnaaaooonnnvvvaPaPaPoPoPoPnPnPnP48 （1）和（2）中n,o,a,P的是 的4个列矢量 雅可比（相对于动系）的第i列为 转动关节： 移动关节： 组合起来 iiiATLTTJJJzzziaPoPnPJLTzzziaonATJiiiATLTTJJJzzziaonLTJ000ATiJnnAT2AT1ATLT