第3章 雅可比矩阵和动力学分析

1、第第3 3章章 雅可比矩阵雅可比矩阵和动力学分析和动力学分析上一章讨论了刚体的位姿描述、齐次变换，机器人各连杆间的位移关系，建立了机器人的运动学方程，研究了运动学逆解，建立了操作空间与关节空间的映射关系。本章将在位移分析的基础上，进行速度分析，研究操作空间速度与关节空间速度之间的线性映射关系雅可比矩阵(简称雅可比)。雅可比矩阵不仅用来表示操作空间与关节空间之间的速度线性映射关系，同时也用来表示两空间之间力的传递关系。3.1 机器人速度雅可比与速度分析一、机器人速度雅可比可写成：YF(X)将其微分，得：),(),(),(654321666543212265432111xxxxxxfyxxxxxx

2、fyxxxxxxfy也可简写成：666226116666222211226612211111ddddddddddddxxfxxfxxfyxxfxxfxxfyxxfxxfxxfyXXFYdd雅可比矩阵用J表示二自由度平面关节型机器人端点位置X、Y与关节1、2的关系为112 12112 12ccssXllYll1212( ,)( ,)XXYY 即12121212ddddddXXXYYY微分得121212ddddXXXYYY121212ddddXXXYYY写成矩阵形式为1212XXYYJ令简写为： dX=J d关节空间微小运动d与手部作业空间微小位移dX的关系。2R机器人的速度雅可比矩阵为：112

3、122 12112 122 12l sl sl sl cl cl cJ已知关节和角速度,可求出该机器人手部速度。若J1，J2分别为雅可比的第1列矢量和第2列矢量，则:1 122JJv右边第一项表示仅由第一个关节运动引起的端点速度；右边第二项表示仅由第二个关节运动引起的端点速度；总的端点速度为这两个速度矢量的合成。因此，机器人速度雅可比的每一列表示其他关节不动而某一关节运动产生的端点速度。 dX=J d( )XJ q qvn自由度机器人J 阵关节变量用广义关节变量q表示： q = q1, q2, , qnT当关节为转动关节时qi=i；当关节为移动关节时qi=di关节空间的微小运动： dq = d

4、q1，dq2, , dqnT机器人末端在操作空间的位姿X表示，它是关节变量的函数，X=X(q)，是一个6维列矢量。J（q）：反映了关节空间微小运动dq与手部作业空间微小运动dX之间的关系。121212T121212nnnXXXnYYYnZZZnXXXqqqYYYqqqZZZqqqqqqqqqqqqXqJ(q)dX = J(q) dqdX=dX，dY，dZ，X，Y，ZT反映了操作空间的微小运动，由机器人末端微小线位移和微小角位移(微小转动)组成。二、机器人速度分析 对dX = J d两边各除以dt得dd( )ddttXqJ q( )XJ q qv或表示为 式中：v为机器人末端在操作空间中的广义速

5、度； q为机器人关节在关节空间中的关节速度；与操作空间速度v之间关系的雅可比矩阵。 qJ(q)为确定关节空间速度反之，假如给定工业机器人手部速度，可解出相应的关节速度，即：VJq1式中：J-1称为工业机器人逆速度雅可比。当工业机器人手部在空间按规定的速度进行作业，用上式可以计算出沿路径上每一瞬时相应的关节速度。例1 如图示的二自由度机械手，手部沿固定坐标系X0轴正向以1.0 m/s的速度移动，杆长l1=l2=0.5 m。求当1=30，2=60时的关节速度。解 由推导知，二自由度机械手速度雅可比为112 122 12112 122 12ssscccllllllJ二自由度机械手手爪沿X0方向运动示

6、意图2 122 121112 12112 121 22cs1ccssslllllll lJ逆雅可比为1Jv12 122 12112 12112 121 22210cs1ccssslllllll l 且vX=1 m/s，vY=0，因此11221212cc4 rad/sssll在两关节的位置分别为1=30,2= 6012 rad s/24 rad/s速度分别为，手部瞬时速度为1 m/s。 rad/s)(2)(-60 sin5 . 0)60-03( cossin)( cos21211l三、雅可比矩阵的奇异性由此可见，当雅可比矩阵的行列式为0时，要使手爪运动，关节速度将趋于无穷大。当雅可比不是满秩矩阵

7、时，J的行列式为0。 qJqJqJ*1 VqJq1 qJ1则 0qJ若J矩阵的伴随阵 qJ*当雅可比不是满秩矩阵时，可能出现奇异解，机器人的奇异形位，相应操作空间的点为奇异点。机器人的奇异形位分为两类：(1) 边界奇异形位：当机器人臂全部伸展开或全部折回时，手部处于机器人工作空间的边界上或边界附近，逆雅可比奇异。相应的机器人形位叫做边界奇异形位。(2) 内部奇异形位：两个或两个以上关节轴线重合时，机器人各关节运动相互抵消，不产生操作运动。相应的机器人形位叫做内部奇异形位。当机器人处在奇异形位时会产生退化现象，丧失一个或更多的自由度。这意味着在工作空间的某个方向上，不管怎样选择机器人关节速度，手

8、部也不可能实现移动。当l1l2s20时无解，机器人逆速度雅可比J-1奇异。因l10，l20，所以，在20或2180时，机器人处于奇异形位。机器人二臂完全伸直，或完全折回，两杆重合。在奇异形位下，手部正好处在工作域的边界上，该瞬时手部只能沿着一个方向(与臂垂直的方向)运动，退化了一个自由度。如果希望机器人手部在空间按规定的速度进行作业，雅可比是满秩矩阵，可以计算出沿路径每一瞬时相应的关节速度。对空间机器人，J的行数为6。二维平面机器人，J的行数为3，列数则为机械手含有的关节数目。平面运动机器人手的广义位置向量x,y,T容易确定，且方位与角运动的形成顺序无关，可直接采用微分法求J 。对于空间机器人

9、,根据机器人运动学方程，可以获得直角坐标位置向量 x,y,zT 的显式方程，但找不到方位向量 的一般表达式。空间机器人雅可比矩阵J确定：不能用直接微分法，采用构造法构造法。Tzyx机器人关节速度向量定义为：手爪在基系中的广义速度向量为： Tnqqqq21TzyxzyxvVnzyxqqqJzyx21四、雅可比矩阵的构造法 qqJV*qqJ*)(n个关节机器人，J是6n矩阵。naaainiiqqqJJJJJJV2122121前三行称为位置雅可比矩阵，代表对手爪线速度 V 的传递比；后三行称为方位矩阵，代表相应的关节速度 对手爪角速度的传递比。将J分块为：矢量积法构造雅可比矩阵对于移动关节对于移动关

10、节i i: :0,0iiiizJqzwv对于转动关节对于转动关节i i: :iioniiqzpzwv zi是i坐标系z轴单位矢量在基系中的表示。手爪坐标原点在i系的位置矢量手爪坐标原点的位置矢量在基系的表示onipnip inioiiioniiizpRzzpzJ iziyixiziyixzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxaaaoonnapapapaaaopopopoonpnpnpnn 0 0 0 o 0 0 0 n 0 0 0)()()( )()()( o)()()(n666666微分变换法微分变换法坐标系之间的速度变换关系坐标系之间的速度变换关系矢量运算矢量运

11、算kajaiaazyx kbjbibzyx bzzyyxxbabababakbabajbabaibaba)( )( )(xyyxzxxzyzzy bazyxzyxbbbaaakji转动关节转动关节i：系系i只绕只绕zi轴以角速度轴以角速度 转动转动i .666666100000000000000)()()()()()(ooo)()()(nizyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxaaaooonnnapapapaaaopopopnpnpnpnnkbabajbabaibaba)( )( )(baxyyxzxxzyzzy1000zzzzyyyyxxxxpaonpaonpao

12、nzzzzzziaonapopnpJ)()()(（2）移动关节）移动关节i：系系i只沿只沿zi轴以速度轴以速度 移动移动i izyxzyxzyxzyxxxxzyxxxxzyxzyxzyxzyxaaaooonnnapapapaaaopopopnpnpnpnn 000100000000000)()()()()()(ooo)()()(n666666中的元素中的元素i6T中的元素中的元素i6TzzzzzziaonapopnpJ)()()(000zzziaonJ56T46T16T26T36T1)()()()()(0)()()()()(0)()()()()(0)()()()()(0)()()()()(0)

13、()()()()(564636261656463626165646362616565646463636262616165656464636362626161656564646363626261616zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzaaaaaooooonnnnnapapapapapopopopopopnpnpnpnpnpJ前置坐标系前置坐标系 T6 = A1 A2 A3 A4 A5 A6 A1-1 T6 = 1T6 ( ( 1T6 = = A2 A3 A4 A5 A6 ) ) A2-1 A1-1 T6 = 2T6 ( ( 2T6 = A3 A4 A5 A6 ) ) A

14、3-1A2-1 A1-1 T6 = 3T6 ( ( 3T6 = A4 A5 A6 ) ) A4-1 A3-1A2-1 A1-1 T6 = 4T6 ( ( 4T6 = A5 A6 ) ) A5-1 A4-1 A3-1A2-1 A1-1 T6 = 5T6 ( ( 5T6 = A6 ) ),0,0)Rot(x)Trans(ad,)Trans(0,0Rot(z,Aiiiii10000000T56565665656546ccssscsssccc555654 cs ss 0 0 0 cJPUMA560雅可比各列的计算实例雅可比各列的计算实例knpnp)(n)(pxyyxzzzzizzzaonapopnpJ

15、)( )( )( )()()(i6i66i6i6i6i6i6i6in x = c23（c4c5c6 s4s6） s23s5c6n y = s4c5c6 c4s6 n z = s23c4c5c6 s4s6 c23s5c6o x = c23c4c5c6+s4s6 + s23s5c6o y = s4c5c6 c4s6 o z = s23c4c5c6+s6s6 + c23s5s6a x = c23c5s5 s23c5a y = s4s5a z = s23c4s5 c23c5p x = a2c2 + a3c23 d4s23p y= d3p z = a3c23 a2s2 d4s23knpnp)(n)(px

16、yyxzJ11=(a2c2 + a3c23 d4s23)( s4c5c6 c4s6 ) d3c23(c4c5c6 s4s6) s23s5c616TzzzizzzaonapopnpJ)( )( )( )()()(i6i66i6i6i6i6i6i6i例2 如图示的二自由度机械手，手部沿固定坐标系X0轴正向以1.0 m/s的速度移动，杆长l1=l2=0.5 m。求： 1）当1=30，2=60时的机械手位姿。 2）机械手J 3) 当1=30，2=60时关节速度x2Y2x3Y3解：1）D-H坐标系建立 2）确定各连杆的D-H参数和关节变量连 杆变 量ad123x1Y13）求两杆之间的位姿矩阵Ai连 杆变

17、 量ad11l10022l20030000 x2Y2x3Y3x1Y110000100000011111csscA),()0 , 0 ,(), 0 , 0(),(iiiiixRotaTransdTranszRotAx2Y2x3Y3x1Y1100001000010001223lA000001000002212212cslscA)iiiii,0,0)Rot(x)Trans(ad,)Trans(0,0Rot(z,A x2Y2x3Y3x1Y14）当1=30，2=60时末杆的位姿5）若给定机械手位姿，求逆解100001000012211121212211121232103slslcsclclscAAAT已

18、知机械手末端杆的位姿：求：求：1 2100001000003yyyxxxponponT1112211122slslpclclpyx),(2tan),(2tan211221221xyxynnaclpslpa6）求）求 J=?knpnp)(n)(pxyyxzzzzizzzaonapopnpJ)( )( )( )()()(i6i66i6i6i6i6i6i6i1 0 0 0 221211lclslJ 21JJJ 求求J113T1000010000222222122231213slcscllscAAT1 0 0 0 0 22lJ23Tknpnp)(n)(pxyyxz1000010000100012232

19、3lATx2Y2x3Y3x1Y1求求J22R平面机器人坐标系如图所示。A阵和T矩阵分别为：1000010000111111111slcsclscA1000010000222222222slcsclscAx1Y1x2Y210000100001221112121221112122102slslcsclclscAAT后置坐标系后置坐标系02Tkbaba)(b)(axyyxz求求J110000100001221112121221112122102slslcsclclscAAT1 0 0 0 221211lclslJzzzizzzaonapopnpJ)( )( )( )()()(i6i66i6i6i6i

20、6i6i6i后置坐标系后置坐标系zzzizzzaonapopnpJ)( )( )( )()()(i6i66i6i6i6i6i6i6i1 0 0 0 0 22lJ12T100001000022222222212slcsclscATkbaba)(b)(axyyxz求求J2后置坐标系后置坐标系 3.2 机器人静力分析机器人在作业过程中，各关节产生相应的作用力。关节力由机器人各关节的驱动装置提供，通过连杆传递到手部，克服外界作用力。本节讨论操作臂在静力平衡关系。两类静力学问题：(1) 已知机器人手部作用力F ，求关节驱动力矩 。 （满足静力学平衡条件）(2) 已知关节驱动力矩，确定机器人手部的作用力F

21、或负荷的质量。定义：机器人末端力矢量机器人末端力矢量：力f和力矩n，记做：在静止状态下，F 应与各关节的驱动力或力矩平衡。关节力矢量关节力矢量：n个关节的驱动力矩组成n 维矢量：Tn21nfF FqJT假定关节无摩擦，忽略各杆件的重力，广义关节力矩与机器人末端力F的关系为： 力雅可比矩阵力雅可比力雅可比JT是工是工业机器人速度雅业机器人速度雅可比可比J的转置的转置。利用虚功原理证明。设各个关节的虚位移为qi，手部的虚位移为X。 手部及各关节的虚位移X0Y0O0iqi-nn，n+1-fn，n+1d ddx dy dz T， x y zT机器人关节虚位移矢量（关节空间）： qq1，q2qn TdX

22、机器人手部的虚位移和虚角位移（作业空间） 设各关节力矩为i(i1，2，n) 机器人手部的作用力和力矩为-fn，n+1和-nn，n+1 根据虚位移原理，各关节所作的虚功之和与末端执行器所作的虚功相等。 即：1q1+2q2+nqn= fn，n+1d + nn，n+1 简写成：Tq F TX 虚位移q和X符合杆件的几何约束条件。 有： XJdq， 代入：Tq F TX 有： JTF JT 称为机械手的力雅可比。 表示在静态平衡状态下，操作力向关节力映射的线性关系。 Y01FFxFy1=0X02=90l1l22(b)X011l122l2F=Fx，FyT(a)Y0例3 图示为二自由度平面关节型机械手，已

23、知手部端点力FFx，FyT，若关节无摩擦力存在，求力 F的等效关节力矩。另求当10，290时的等效关节力矩。解：由前面推导知，该机械手的速度雅可比为：)c()c(c)s()s(s2122121121221211llllllJ则该机械手的力雅可比为：)c()s()c(c)s(s2122122121121211llllllTJ根据 JTF，得： 1 = -l1sin1+ l2sin(1+2)Fx +l1cos1+ l2cos(1+2)Fy 2 = -l2sin(1+2)Fx+ l2 cos(1+2)Fy当10，290yxFFllllll212212212112121121css)ccss1=-l2

24、Fx+ l1Fy ， 2=- l2Fx机器人动力学研究各杆件的运动和作用力之间的关系，是机器人设计、运动仿真和动态实时控制的基础。机器人动力学问题有两类：动力学正问题已知关节的驱动力矩，求机器人系统相应的运动参数（包括关节位移、速度和加速度）。动力学逆问题已知运动轨迹点上的关节位移、速度和加速度，求出所需要的关节力矩。3.3 机器人动力学分析 机器人是由多个连杆和多个关节组成的复杂的动力学系统，具有多个输入和多个输出，存在着严重的非线性和耦合关系。采用方法： 拉格朗日(Lagrange)方法 牛顿欧拉方法(Newton-Euler)方法 高斯(Gauss)方法 凯恩(Kane)方法等。拉格朗日

25、方法以简单的形式求得系统动力学方程，而且具有显式结构，物理意义比较明确，对理解机器人动力学比较方便。因此，本节只介绍拉格朗日方法，并结合简单实例进行分析。机器人动力学问题的求解比较困难，而且需要较长的运算时间。因此，简化求解的过程，最大限度地减少机器人动力学在线计算的时间是持续研究的课题。 一、拉格朗日方程1. 拉格朗日函数定义：机械系统的动能Ek和势能Eq之差，即： LEk - Eq令 qi 广义关节变量， 是广义关节速度。系统动能Ek是 qi 和 的函数，系统势能Eq是qi的函数，因此L是 qi 和 的函数。 iq iq iq 2. 拉格朗日方程 关节i的广义驱动力为：iiiqLqLtFd

26、di1，2，nFi为关节i的广义驱动力。 移动关节：Fi为驱动力； 转动关节：Fi为驱动力矩。l1k1m2k2m121p2l2p1X0Y0二、二自由度平面关节机器人动力学方程推导1.关节变量及关节力矩选取图示笛卡尔坐标系。关节变量：1和2关节力矩：1和2。连杆1和连杆2杆长为ll和l2，质量分别是ml和m2质心分别在kl和k2处，离关节中心的距离分别为pl和p2。l1k1m2k2m121p2l2p1X0Y02112121pyx杆1质心kl的位置坐标为：x1p1sin1y1-p1cos1杆1质心kl的速度平方为： l1k1m2k2m121p2l2p1X0Y0杆2质心k2的位置坐标为：x2llsi

27、nl + p2sin(l +2)y2-llcosl - p2cos(l +2)22121212212221212222cos2plplyx212121112212121112)sin(sin)cos(cosplyplx杆2质心k2的速度平方为： 22121212221222212122112122121212221222212122212111cos2121cos212121plmpmlmpmEEplmpmlmEpmEikikkk2.系统动能 21221121121cos1cos1gpmglmpmEEipip3.系统势能1111cos1gpmEp21221122cos1cos1gpmglmEp

28、pkEEL4.拉格朗日函数 2122112112212121222122221212211cos1 cos1 cos 2121gpmglmpmplmpmlmpm拉格朗日方程 i1，2计算各关节上的力矩，得到系统动力学方程。 5.系统动力学方程iiiLLtdd111 LdtdLL222 LdtdLL)sin(sin2122112111gpmglmpmL dd111LLt6.计算关节1上的力矩1：2122222121212122111cos2pmplmlmpmL注意：这里只求显因变量的偏导数)(sinsinsin sin2cos cos2212211211222212212212222122221

29、2212212222211gpmglmpmplmplmplmpmplmlmpmpm 简写为： 122122211122121111DDDDD 其中：)(sinsinsinsin2cincos2212211211122121222212112221222212221221222221111gpmglmpmDplmDplmDplmpmDplmlmpmpmD21212212222cosplmpmL)(sinsinsincos dd2122212212212212212222212212222222gpmplmplmplmpmplmpmLLt 7.计算关节2上的力矩2：)(sinsin)(212222

30、1212122gpmplmL简写为： 221211212122221212DDDDD 其中： )(sinsin0sincon212222212211221221221222222221222221gpmDplmDplmplmDpmDplmpmD2112212212121211122221222211122111212221121121DDDDDDDDDDDDDD 力矩力矩 惯性力惯性力 向心力向心力 哥氏力哥氏力 重力重力上式表示了关节驱动力矩与关节位移、速度、加速度之间的关系，即力和运动之间的关系，称为二自由度工业机器人的动力学方程。有效惯量：D11、D22 D11、D22：关节1和关节2加

31、速度引起的惯性力矩；耦合惯量：D12、 D21 D12：关节2加速度对关节1的耦合惯性力矩； D21：关节1加速度对关节2的耦合惯性力矩。2112212212121211122221222211122111212221121121DDDDDDDDDDDDDD 力矩力矩 惯量惯量 向心加速度系数向心加速度系数 哥氏加速度系数哥氏加速度系数 重力重力力矩力矩 惯性力惯性力 向心力向心力 哥氏力哥氏力 重力重力向心加速度系数： D122：关节2速度引起的向心力对关节1的耦合力矩； D211：关节1速度引起的向心力对关节2的耦合力矩。哥氏加速度系数： D112：哥氏力对关节1的耦合力矩； D212：哥

32、氏力对关节2的耦合力矩。 哥氏力是由于牵连运动有转动成分造成的。哥氏力是由于牵连运动有转动成分造成的。 2112212212121211122221222211122111212221121121DDDDDDDDDDDDDD 力矩力矩 惯量惯量 向心加速度系数向心加速度系数 哥氏加速度系数哥氏加速度系数 重力重力重力项： D1：连杆1的质量对关节1引起的重力矩； D2：连杆2的质量对关节2引起的重力矩。简单的二自由度平面关节型机器人动力学方程非常复杂。多自由度机器人，动力学方程更复杂。通常的简化处理方法：(1) 当杆件质量不很大，重力矩项可以省略；(2) 当关节速度不很大，不是高速机器人，二次

33、项可以省略。2112212212121211122221222211122111212221121121DDDDDDDDDDDDDD 力矩力矩 惯量惯量 向心加速度系数向心加速度系数 哥氏加速度系数哥氏加速度系数 重力重力二杆机器人二杆机器人有效惯量系数：有效惯量系数： 2211121222122()2cos()Dmm dm dm d d22222Dm d耦合惯量系数：耦合惯量系数： 21221222122cos()DDm dm d d向心力项系数：向心力项系数： 0)sin()sin(22222122112212122111DddmDddmDDD哥氏力系数：哥氏力系数： 0)sin(2221

34、2122212121112DDddmDD重力项：重力项： 112112212()sin()sin()Dmmg dm g d22212sin()Dm g d二、关节空间和操作空间动力学 1关节空间的动力学方程关节空间的动力学方程写成矩阵形式,D q qH q qG q( )()( )1212q12 q12q操作臂在关节空间的动力学方程的一般结构形式反映了关节力矩与关节变量、速度、加速度之间的函数关系。 222211212122221222222122222cc( )cm pm lpl pmpl pmpl pm pD q()()()22 12222 1221222 1221s2s,sm l pm l pm l p H q q()112 1122122212sssm pm l gm p gm p gG q()( )操作臂的惯性矩阵：操作臂的惯性矩阵：对于n个关节的操作臂，D(q)是nn的正定对称矩阵，是q的函数。G(q)是n1的重力矢量，与操作臂的形位q有关。 H q q(), ,是n1的离心力和科氏力矢量。2操作空间动力学方程操作空间动力学方程 在笛卡儿操作空间中用末端操