1、机械工程与汽车学院1 动力学是机器人控制的基础，本章主要从控制的角度来研究机械手的动力学问题。机械手通常是一种开链式多关节机构，是一种复杂的动力学系统，需要采用系统的分析方法来研究它的动态特性。本章我们运用拉格朗日力学原理来分析机械手的动力学问题，因为拉格朗日方法能以最简单的形式求得非常复杂的系统的动力学方程。本章的主要内容如下：l 运用拉格朗日力学原理分析和求取两自由度机械手的动力学方程； l 介绍六自由度机械手动力学方程的求取方法和步骤； l 推导出完整的动力学方程，然后根据有效性分析来简化这些方程。 6.1 引言引言 ( Introduction )机械工程与汽车学院2 拉格朗日算子拉格
2、朗日算子 L 定义为系统的动能定义为系统的动能 K 与势能与势能 P 的差的差 L = K P (6.1) 系统的动能和势能可以用任何能使问题简化的坐标系统来表系统的动能和势能可以用任何能使问题简化的坐标系统来表示，并不一定要使用笛卡尔坐标。示，并不一定要使用笛卡尔坐标。 动力学方程通常表述为动力学方程通常表述为 其中，其中，qi是表示动能和势能的坐标值，是表示动能和势能的坐标值， 是速度，而是速度，而Fi是对应的力或是对应的力或力矩，力矩，Fi是力还是力矩，这取决于是力还是力矩，这取决于qi是直线坐标还是角度坐标。这是直线坐标还是角度坐标。这些力、力矩和坐标分别称为广义力、广义力矩和广义坐标
3、。些力、力矩和坐标分别称为广义力、广义力矩和广义坐标。iiiqLqLdtdF(6.2) 6.2 拉格朗日力学拉格朗日力学 一个简例一个简例( Lagrangian Mechanics A Simple Example )iq 机械工程与汽车学院3 为了说明问题，我们看一个具为了说明问题，我们看一个具体例子，假定有如图体例子，假定有如图6.16.1所示的两所示的两连杆的机械手，两个连杆的质量分连杆的机械手，两个连杆的质量分别为别为m m1 1、m m2 2，由连杆的端部质量代由连杆的端部质量代表，两个连杆的长度分别为表，两个连杆的长度分别为d d1 1、d d2 2，机械手直接悬挂在加速度为机械
4、手直接悬挂在加速度为g g的重的重力场中，广义坐标为力场中，广义坐标为1 1和和2 2。m2d1d2m1xy21图图6.1 6.1 两连杆的机械手两连杆的机械手机械工程与汽车学院4 动能的一般表达式为动能的一般表达式为 ，质量，质量m1的动能可直接写出的动能可直接写出 势能与质量的垂直高度有关，高度用势能与质量的垂直高度有关，高度用y坐标表示，于是势能可直接写坐标表示，于是势能可直接写出出 对于质量对于质量m2，由图由图6.1，我们先写出直角坐标位置表达式，然后求微，我们先写出直角坐标位置表达式，然后求微分，以便得到速度分，以便得到速度221mvK )(1111Cosgdmp(6.4)(6.4
5、)21211121dmK(6.3)(6.3)()(212112SindSindx(6.5)(6.5)()(212112CosdCosdy(6.6)(6.6)6. 2. 1 动能和势能动能和势能 ( The Kinetic and Potential Energy )机械工程与汽车学院5速度的直角坐标分量为速度的直角坐标分量为 速度平方的值为速度平方的值为)()(212121112SindSindy(6.8)(6.8)()(212121112CosdCosdx(6.7)(6.7)2(22212122212122ddV)()(2212121121SinSindd)(2)2(212122122212
6、1222121Cosdddd)()(2212121121CosCosdd(6.9)(6.9)机械工程与汽车学院6从而动能为从而动能为)2(2121222121222212122dmdmK)(21212212Cosddm(6.10)(6.10)质量的高度由式质量的高度由式(6.6)(6.6)表示，从而势能就是表示，从而势能就是)()(21221122CosgdmCosgdmp(6.11)(6.11)机械工程与汽车学院7拉格朗日算子拉格朗日算子 L = K P 可根据式可根据式(6.3)、(6.4)、(6.10)和和(6.11)求求得得)2(21)(21222121222212121dmdmmL)
7、(21212212Cosddm)()()(21221121CosgdmCosgdmm(6.12)(6.12)6. 2. 2 拉格朗日算子拉格朗日算子 ( The Lagrangian )机械工程与汽车学院8为了求得动力学方程，我们现在根据式为了求得动力学方程，我们现在根据式(6.2)(6.2)对拉格朗日算子进行微分对拉格朗日算子进行微分)()(21222121211dmdmmL2221212212)()(2CosddmCosddm（6.136.13）（6.146.14）1221222221211)(2)( CosddmdmdmmLdtd22212222)( Cosddmdm2222122122
8、12)()(2SinddmSinddm)()()(212211211SingdmSingdmmL（6.156.15） 6. 2. 3 动力学方程动力学方程 ( The Dynamics Equations )机械工程与汽车学院9根据式根据式(6.2)(6.2)，把式，把式(6.14)(6.14)与与(6.15)(6.15)相减就得到关节相减就得到关节1 1的力矩的力矩1221222221211)(2)( CosddmdmdmmT22212222)( Cosddmdm222212212212)()(2SinddmSinddm)()()(21221121SingdmSingdmm（6.166.16
9、）机械工程与汽车学院1012212222212222)(CosddmdmdmL(6.17)12212222212222)( CosddmdmdmLdtd212212)(Sinddm(6.18)()(2122212122122SingdmSinddmL(6.19)用拉格朗日算子对 求偏微分，进而得到关节2的力矩方程 22和机械工程与汽车学院11于是关节2的力矩为2222122122222)( dmCosddmdmT212212)sin(ddm)sin(2122gdm （6.20）将式(6.16)和(6.20）重写为如下形式 （6.21）1121212111222122211112121111DD
10、DDDDDT 2122212121222222212112221122DDDDDDDT （6.22）机械工程与汽车学院12 在方程（6.21）和（6.22）中各项系数 D 的含义如下：Dii 关节 i 的等效惯量（Effective inertia）， 关节 i 的加速度使关节 i 产生的力矩Dij 关节 i 与关节 j 之间的耦合惯量（Coupling inertia） 关节 i 或关节 j 的加速度分别使关节 j 或 i 产生的力矩 和Dijj 由关节 j 的速度产生的作用在关节 i 上的向心力 系数 （Centripetal force）Dijk 作用在关节 i 上的复合向心力（哥氏力
11、Coriolis force）的组合项 系数，这是关节 j 和关节 k 的速度产生的结果Di 作用在关节 i 上的重力（Gravity） iiiD jijiijDD 2jijjDjkijkkjijkDD机械工程与汽车学院13 把方程(6.16)、(6.20)与(6.21)、(6.22)比较，我们就得到各项系数的值： 等效惯量 D11 = (m1 + m2)d12 + m2d22 + 2m2d1d2cos(2 ) (6.23) D22 = m2d22 (6.24) 耦合惯量 D12 = m2d22 + m2d1d2cos(2 ) (6.25) 向心加速度系数D111 = 0 (6.26) D12
12、2 = - m2d1d2sin(2 ) (6.27) D211 = m2d1d2sin(2 ) (6.28)D222 = 0 (6.29)机械工程与汽车学院14 哥氏加速度系数D112 = D121 = - m2d1d2sin(2) (6.30)D212 = D221 = 0 (6.31) 重力项为D1 = (m1 + m2)gd1Sin(1) + m2gd2Sin(1 + 2 ) (6.32)D2 = m2gd2Sin(1 + 2 ) (6.33)机械工程与汽车学院15 下面给两连杆机械手赋予具体数值，并且对于静止状态（下面给两连杆机械手赋予具体数值，并且对于静止状态（ ）和在无重力环境中的
13、机械手求解方程和在无重力环境中的机械手求解方程(6.21)和和(6.22)。求解在下列两种条件下。求解在下列两种条件下进行进行: 关节关节2处于锁定状态（处于锁定状态（ ）；关节）；关节2处于自由状态处于自由状态( ( T2 = 0 ) )。在第在第一种条件下一种条件下, 方程方程(6.21)和和(6.22)简化为简化为 在第二种条件下，在第二种条件下，T2 = 0 ，我们可以由方程我们可以由方程(6.22)解出解出 ，再把它代入，再把它代入方程方程(6.21)，得到，得到T102 02221122 DDT122122 DD于是于是代入方程代入方程(6.21)(6.21)有有 12212211
14、1 DDDT(6.36) (6.36) 1111 DT1122 DT(6.35)(6.35)(6.34)(6.34)0212 机械工程与汽车学院16 现在，取定现在，取定 d1 = d2 = 1 ，m1 = 2，而对于三个不同的而对于三个不同的 m2 值，分别求出各值，分别求出各个系数：个系数： m2 = 1，表示机械手无负载情况；表示机械手无负载情况；m2 = 4 ，表示有负载；表示有负载；m2 = 100 ，表示位于外太空表示位于外太空( 无重力环境无重力环境 )的机械手的负载。在外太空，没有重力负载，允的机械手的负载。在外太空，没有重力负载，允许非常大的工作负载。根据求得的系数以及方程许
15、非常大的工作负载。根据求得的系数以及方程(6.34)和和(6.35)，分别对应关节，分别对应关节2的四种不同的锁定状态的四种不同的锁定状态 IL和自由状态和自由状态 If ，计算关节计算关节1的惯量如下表所示（表中的惯量如下表所示（表中 IL 表示锁定状态，表示锁定状态，If 表示自由状态）。表示自由状态）。表表6.1 6.1 m m1 1 = 2 , m= 2 , m2 2= 1 , d= 1 , d1 1 = 1 , d = 1 , d2 2 = 1 = 1D D1111 D D1212 D D2222 I IL L I If f CosCos2 201 6 2 1 6 21 6 2 1
16、6 290 0 4 1 1 4 30 4 1 1 4 3180-1 2 0 1 2 2-1 2 0 1 2 2270 0 4 1 1 4 30 4 1 1 4 32机械工程与汽车学院17表表6.2 6.2 m m1 1 = 2 , m= 2 , m2 2=4 , d=4 , d1 1 = 1 , d = 1 , d2 2 = 1 = 1D D1111 D D1212 D D2222 I IL L I If f CosCos2 201 18 8 4 18 21 18 8 4 18 290 0 10 4 4 10 60 10 4 4 10 6180-1 2 0 4 2 2-1 2 0 4 2 22
17、70 0 10 4 4 10 60 10 4 4 10 6表表6.3 6.3 m m1 1 = 2 , m= 2 , m2 2=100 , d=100 , d1 1 = 1 , d = 1 , d2 2 = 1 = 1D D1111 D D1212 D D2222 I IL L I If f CosCos2 201 402 200 100 402 21 402 200 100 402 290 0 202 100 100 202 1020 202 100 100 202 102180-1 2 0 100 2 2-1 2 0 100 2 2270 0 202 100 100 202 1020 20
18、2 100 100 202 10222机械工程与汽车学院18 上面三个表格中，靠右两列表明关节上面三个表格中，靠右两列表明关节1 1的等效惯量。表的等效惯量。表6.16.1说明，说明，对于无负载的机械手来说，对于无负载的机械手来说，2 2 从从 0 0变为变为 180180，在，在锁定状态锁定状态情况情况下，等效惯量下，等效惯量I IL L的变化为的变化为 3:13:1。同时，在。同时，在2 20 0时，锁定状态时，锁定状态（ I IL L ）和自由状态（）和自由状态（ I If f ）等效惯量的变化也为）等效惯量的变化也为 3:13:1。 从表从表6.26.2可以看出，对于加载机械手，可以看
19、出，对于加载机械手，2 2从从 0 0变为变为 180180，在，在 锁定状态情况下，等效惯量锁定状态情况下，等效惯量I IL L的变化为的变化为 9 9:1:1。而自由状态等效惯量。而自由状态等效惯量I If f的变化为的变化为 3 3:1:1。 对于表对于表6.36.3所示的负载为所示的负载为100100的外太空机械手，在不同状态下惯的外太空机械手，在不同状态下惯量的变化竟为量的变化竟为 201 201:1:1。这些关联的变化情况对于机械手的控制问题将。这些关联的变化情况对于机械手的控制问题将有重要的影响。有重要的影响。 机械工程与汽车学院196. 3 机械手动力学方程机械手动力学方程 (
20、 The Manipulator Dynamics Equation ) 推导机械手的动力学方程可按下述五个步骤进行推导机械手的动力学方程可按下述五个步骤进行l 首先计算机械手任意连杆上任意一点的速度首先计算机械手任意连杆上任意一点的速度 ；l 再计算它的动能再计算它的动能 K ；l 然后推导势能然后推导势能 P ；l 形成拉格朗日算子形成拉格朗日算子 L = K P ；l 对拉格朗日算子进行微分得到动力学方程对拉格朗日算子进行微分得到动力学方程 。iq iiiqLqLdtdF机械工程与汽车学院20假定机械手的连杆假定机械手的连杆 i 上有一个点上有一个点 ir，它在基坐标中的位置为，它在基坐
21、标中的位置为于是，它的速度就是于是，它的速度就是速度的平方速度的平方rrdtdr 2)(或者用矩阵形式表为或者用矩阵形式表为rTrii（6.376.37）)()(2TrrTracedtdr （6.396.39）rqqTdtdriijjji)(1（6.386.38）6. 3. 1 机械手上一点的速度机械手上一点的速度 ( The Velocity of a Point on the Manipulator )z0ziyxiryixiTi机械工程与汽车学院21 根据方程根据方程(6.38)(6.38)可得可得ikTikkiiijjjirqqTrqqTTracedtdr112ijikkjkTiTii
22、jiqqqTrrqTTrace11（6.406.40）机械工程与汽车学院22 在连杆在连杆i上上 ir 处，质量为处，质量为 dm 的质点动能是的质点动能是 于是，连杆于是，连杆i的动能就是的动能就是ijikkjkTilinkTiijilinkiiqqqTdmrrqTTracedKKii11)(21(6.42)(6.42)dmqqqTrrqTTracedKijikkjkTiTiijii1121 ijikkjkTiTiijiqqqTrrdmqTTrace11)(21(6.41)(6.41)6. 3. 2 动能动能 ( The Kinetic Energy )机械工程与汽车学院23式式(6.42)
23、(6.42)中的积分称为伪惯量矩阵，可由下式确定中的积分称为伪惯量矩阵，可由下式确定 （6.436.43） iiiiiiiiiiiiiiiiilinklinklinkilinkilinkilinkilinkilinkiilinkiilinkilinkiilinkilinkiilinkilinkiilinkiilinkiTiiidmzdmydmxdmzdmdmzzdmyzdmxydmzdmydmyydmxxdmzdmxydmxdmxdmrrJ222机械工程与汽车学院24回顾一下转动惯量，惯量叉积和物体的一阶动量的定义为回顾一下转动惯量，惯量叉积和物体的一阶动量的定义为 dmzyIxx)(22dm
24、zxIyy)(22dmxyIzz)(22xydmIxyxzdmIxzyzdmIyzxdmxmydmymzdmzm机械工程与汽车学院25从而从而 dmxydmzxdmzydmz)(21)(21)(2122222222/ )(zzyyxxIII(6.46)(6.46) dmxydmzxdmzydmx)(21)(21)(2122222222/ )(zzyyxxIII(6.44)(6.44) dmxydmzxdmzydmy)(21)(21)(2122222222/ )(zzyyxxIII(6.45)(6.45) 机械工程与汽车学院26 于是，于是，Ji 就能表示为就能表示为 iiiiiiiiiizzi
25、yyixxiyzixziiiyzizziyyixxizyiiixzizyizziyyixximzmymxmzmIIIIIymIIIIIxmIIIIIJ222（6.476.47）机械手的总动能就是机械手的总动能就是ijikkjkTiiiiiiqqqTJqTTraceKKj11616121（6.486.48）机械工程与汽车学院27 上面这个方程表示了机械手结构的动能，然而，动能还有另外一个重要上面这个方程表示了机械手结构的动能，然而，动能还有另外一个重要组成部分，即各个关节的传动机构的动能（对非直接驱动机械手而言）。我组成部分，即各个关节的传动机构的动能（对非直接驱动机械手而言）。我们通过传动机构
26、的惯量以及有关的关节速度表示这部分动能们通过传动机构的惯量以及有关的关节速度表示这部分动能221iaactuatorqIKii 把把TraceTrace运算和求和运算相互交换一下，再加上传动机构的动能部分，运算和求和运算相互交换一下，再加上传动机构的动能部分，最后得到机械手的总动能为最后得到机械手的总动能为在棱柱形滑动关节的情况下，在棱柱形滑动关节的情况下，I Ia a成为一个等价质量。成为一个等价质量。 611612121)(21iijiiakTiiiikqIqqqTJqTTraceKikjj(6.49)(6.49)机械工程与汽车学院28 在重力场在重力场g中，一个物体的质量为中，一个物体的
27、质量为m，位于某个参考零点之上的高度位于某个参考零点之上的高度为为h，它的势能为它的势能为 P = m g h (6.50) 如果由重力引起的加速度表示为矢量如果由重力引起的加速度表示为矢量g，物体质心的位置表为矢量物体质心的位置表为矢量 ，那么式那么式(6.50)就变为就变为 例如，在重力场中，例如，在重力场中，g = 0i + 0j 32.2k， = 10i + 20j + 30k ，位于，位于r处处的质量的质量m就有势能就有势能 966 nm。rrmgp(6.51)6. 3. 3 势能势能 ( The Potential Energy )r机械工程与汽车学院29 如果连杆如果连杆i的质心
28、用矢量的质心用矢量 表示，它相对于坐标系表示，它相对于坐标系 Ti 的势能为的势能为ir其中其中从而，机械手的总势能就是从而，机械手的总势能就是iiiTiirTgmP(6.52)(6.52)61iiiiTirTgmP (6.54) (6.54)0zyxgggg(6.53)(6.53)机械工程与汽车学院30由式由式(6.49)(6.49)和和(6.54)(6.54)得到的得到的 K K 和和 P P，可计算拉格朗日算子可计算拉格朗日算子 L L = = K - P K - P应用欧拉应用欧拉拉格朗日方程拉格朗日方程 我们就可求得动力学方程。我们就可求得动力学方程。 61161612121)(21
29、iijiiiiTiiiakTiiiikrTgmqIqqqTJqTTraceLikjj(6.55)(6.55)iiiqLqLdtdF)(6.56)(6.56) 6. 3. 4 拉格朗日算子拉格朗日算子 ( The Lagrangian )机械工程与汽车学院31 先求方程先求方程(6.56)(6.56)第一项中的偏微分第一项中的偏微分 把上式第二项中的脚标把上式第二项中的脚标 j 变为变为 k ，把第一项中把第一项中Trace运算换成运算换成pajpTiijiijikkTiipiikipqIqqTJqTTraceqqTJqTTraceqLp)(21)(21161161(6.57)(6.57)()(
30、)(pTiikiTkTiipikTiipiqTJqTTraceqTJqTTraceqTJqTTrace我们就得到我们就得到pakpTiikiikipqIqqTJqTTraceqLp)(161(6.58)(6.58)6. 3. 5 动力学方程动力学方程 ( The Dynamics Equations )机械工程与汽车学院32 由于 ( p i 时)，最后得到0piqT现在求式(6.59)对于时间t 的微分 pakpTiijiiikpqIqqTJqTTraceqLp )(611(6.59)pakpTiikipiikpqIqqTJqTTraceqLdtdp )(61mkpTiimkipiikimq
31、qqTJqqTTrace)(2611 (6.60)mkkTiimpipiikimqqqTJqqTTrace)(2611 机械工程与汽车学院33欧拉拉格朗日方程的第二项是 把式(6.61)第二项中求和运算的脚标 j 换成 k ，再把第二项与第一项合并，就得到 6121)(21piijkjkTiipiikpqqqTJqqTTraceqLj 61621)(21piijpiiipiTikjjTiipkiikrqTgmqqqTJqqTTrace(6.61) 6121)(piijkjkTiijpiikpqqqTJqqTTraceqL6piiipiTirqTgm(6.62)机械工程与汽车学院34 按照方程(
32、6.56)，把式(6.60)减去(6.62)，再把式(6.62)中求和运算的脚标j换成m，这样，式(6.62)的第一项就与式(6.60)中的第三项抵消，可得到最后，把求和运算的脚标 p 和 i 换成 i 和 j ，就得到动力学方程 61)(pipakpTiikiikppqIqqTJqTTraceqLqLdtdp 61621)(piikpiiipiTimkpTiimkiimrqTgmqqqTJqqTTrace(6.63)611)(jiakiTjjkjjkiqIqqTJqTTraceFi 621)(ijjjijTjmkiTjjmkjjmrqTgmqqqTJqqTTrace61pjjk(6.64)机
33、械工程与汽车学院35这些方程与求和的次序是无关的，所以可以把方程(6.64)重写为其中 616161jkikjijkiajjijiDqqDqIqDFi (6.65)6ipppipTpirqTgmD6),max()(jipiTppjpijqTJqTTraceD(6.66)6),max(2)(kjipiTppkjpijkqTJqqTTraceD(6.67)(6.68)机械工程与汽车学院36 上面这些式子与在第上面这些式子与在第6.2.3小节中得到的那些式子形式完全一样。小节中得到的那些式子形式完全一样。其中形式为其中形式为 Di 的项表示关节的项表示关节 i 的的等效惯量等效惯量；形式为；形式为
34、Dij 的项表示关节的项表示关节i 和关节和关节 j 之间的之间的耦合惯量耦合惯量；形式为；形式为 Dijj 的项表示由于关节的项表示由于关节 j 的速度的速度所产生的作用在关节所产生的作用在关节 i 上的上的向心力向心力；形式为；形式为 Dijk 的项表示由于关节的项表示由于关节 j 和关节和关节 k 的速度所产生的作用在关节的速度所产生的作用在关节 i 上的上的哥氏向心力哥氏向心力；最后，形；最后，形式为式为 Di 的项表示关节的项表示关节 i 上的上的重力负载重力负载。 惯量项和重力项在机械手的控制中特别重要，因为它们影响到惯量项和重力项在机械手的控制中特别重要，因为它们影响到伺服稳定性和位置精度。向心力和哥氏向心力仅当机械手高速运动伺服稳定性和位置精度。向心力和哥氏向心力仅当机械手高速运动时才比较重要，通常情况下，由它们造成的误差比较小。比较而言，时才比较重要，通常情况下，由它们造成的误差比较小。比较而言，转动机构的惯量转动机构的惯量 Iai 往往很大，因而应尽量减小等效惯量和耦合惯量往往很大，因而应尽量减小等效惯量和耦合惯量与结构的相关性。与结构的相关性。机械工程与汽车学院精品课件精品课件！机械工程与汽车学院精品课件精品课件！机械工程与汽车学院39习题 用拉格朗日法推导如图所示两自由度机器人手臂的运动方程。连杆质心位于连杆中心，其转动惯量分别为I1和I2。
