机构学与机器人学chap6

第六章机器人的动力学分析机器人操作的动态数学模型，主要采用下列两种理论：（1）动力学基本理论，包括牛顿-欧拉方程。（2）拉格朗日力学，特别是二阶拉格朗日方程。拉格朗日函数L被定义为系统的动能K和位能P之差，即:PKL（4.1）其中，K和P可以用任何方便的坐标来表示。系统动力学方程式，即拉格朗日方程如下：niqLqLdtdFiii,2,,（4.2）式中，iq为表示动能和位能的坐标，iq速度，而为相应的Fi是力或是力矩，由qi为直线坐标或角坐标所决定。这些力、力矩和坐标称为广义力、广义力矩和广义坐标，n为连杆数目。Fi为作用在第i个坐标上的力或是力矩。4.1.1刚体的动能与位能在理论力学或物理力学部分，曾对如图4.1所示的一般物体平动时所具有的动能和位能进行过计算，其求法是大家所熟悉的，如下：012010011201200211)(21)(212121FxFxWxxcDgxMgxMxxkPxMxMK式中，表示物体所具有的动能K、位能P，所消耗的能量D和外力所做的功W；M0和M1为支架和运动物体的质量；x0和x1为运动坐标；g为重力加速度；k为弹簧虎克系数；c为摩擦系数；F为外施作用力。对于这一问题，存在两种情况。1、10,0xx为广义坐标11111xWxpxDxKxKdtd012010011201200211)(21)(212121FxFxWxxcDgxMgxMxxkPxMxMK其中，左式第一项为动能随速度（或角速度）和时间的变化；第二项为动能随位置（或角度）的变化；第三项为能耗随速度的变化；第四项为位能随位置的变化。右式为实际外加力或力矩。代入相应各项的表达式，并化简可得:FgMkxxcxMdtd111110)(表示为一般形式为:gMFkxxcxM11111即为所求x0=0时的动力学方程式。其中，左式三项分别表示物体的加速度、阻力和弹力，而右式两项分别表示外加作用力和重力。100,0xxx和2、均为广义坐标这时有下式:FgMxxkxxcxMFgMxxkxxcxM10101001010111)()()()(或用矩阵形式表示为:FFxxkkkkxxccccxxMM0101010100下面来考虑二连杆机械手（见图4.2）的动能和位能。这种运动机构具有开式运动链，与复摆运动有许多相似之处。图中，T1和T2为转矩，m1和m2为连杆1和连杆2的质量，且以连杆末端的点质量表示；d1和d2分别为两连杆的长度，θ1和θ2为广义坐标；g为重力加速度。先计算连杆1的动能K1和位能P1。因为:1111111112111cos,,,21dhghmPdmK，所以有:1111212111cos21gdmPdmK再求连杆2的动能K2和位能P2:222222,21mgyPmK式中:))(sin(cos))(cos(cos)cos(cos)sin(sin212121112212121112212112212112222222ddyddxddyddxyx于是可求得:)(cos2)2(212122122212122212122dddd以及:)(cos)(212121212212221222212122ddmdmdmK)cos(cos21221122gdmgdmP这样，二连杆机械手系统的总动能和总位能分别为:)(cos)(21)(212121221222122221212121ddmdmdmmKKK（4.3）)cos(cos)(2122112121gdmgdmmPPP（4.4）将相应各导数和偏导数代入(4.2)，即可求得力矩T1和T2的动力学方程式：重力项：在分析简单的二连杆机械手系统的基础上，我们分析由一组A变换描述的任何机械手，求出动力学方程。推导过程分五步进行。（1）计算任一连杆上任一点的速度；（2）计算各连杆的动能和机械手的总动能；（3）计算各连杆的位能和机械手的总位能；（4）建立机械手系统的拉格朗日函数；（5）对拉格朗日函数求导，以得到动力学方程式。图4.4表示一个四连杆机械手的结构。我们先从这个例子出发，求得此机械手某个连杆（例如连杆3）上某一点（如点P）的速度、质点和机械手的动能与位能、拉格朗日算子，再求系统的动力学方程式。然后，由特殊到一般，导出任何机械手的速度、动能、位能和动力学方程的一般表达式。4.2.1速度的计算图4.4中连杆3上点P的位置为：pprTr330式中，pr0为总（基）坐标系中的位置矢量；pr3为局部（相对关节O3）坐标系中的位置矢量；T3为变换矩阵，包括旋转变换和平移变换。对于任一连杆i上的一点，其位置为:rTrii0（4.14）点P的速度为:pppprTrTdtdrdtdv333300)()(式中，jjjqqTdtdTT31333，所以有pjjjprqqTv33130对于连杆i上任一点的速度为rqqTdtdrviijjji1P点的加速度pkjjkkjpjjjpkjjkkjpjjjpjjjppprqqqqTrqqTrqqqqTrqdtdqTrqqTdtdrTdtdvdtda331313233133313132331333133300)()(P点速度的平方3131333331313333000020))(()()()()()()()(jkkjkTTppjjkTpTkkpjjTpppppqqqTrrqTTracerqqTrqqTTracevvTracevvv对于任一机械手上一点的速度平方为ijikkjTkiTiijiijikTikkiijjiqqqTrrqTTracerqqTrqqTTracedtdrv111122式中，Trace表示矩阵的迹。对于n阶方程来说，其迹即为它的主对角线上各元素之和。4.2.2动能和位能的计算令连杆3上任一质点p的质量为dm，则其动能为31313333313133332321)(2121jkkjTkTppjjkkjTkTppjpqqqTrdmrqTTracedmqqqTrrqTTracedmvdK任一机械手连杆i上位置矢量ri其动能如下式表示：的质点，ijikkjkTiTiijiijikkjkTiTiijiiqqqTrrdmqTTracedmqqqTrrqTTracedK11112121对连杆3积分dK3，得连杆3的动能为31313333333321jkkjTkTppjqqqTdmrrqTTracedKK连杆连杆式中，积分dmrrTpp33称为连杆的伪惯量矩阵，并记为dmrrITpp3333连杆这样，3131333321jkkjTkjqqqTIqTTraceK任何机械手上任一连杆i动能为ijikkjkijiiiqqqTIqTTracedKK113321连杆（4.17）式中，Ii为伪惯量矩阵，其一般表达式为dmzdmydmxdmzdmdmzdmyzdmxydmzdmydmyydmxxdmzdmxydmxdmxdmrrdmrrIiiiiiiiiiiiiiiiiiiiiiiiTiiTiii222连杆根据理论力学或物理学可知，物体的转动惯量、矢量积以及一阶矩量为dmyxIdmzxIdmzyIzzyyxx)(,)(,)(222222yzdmIIxzdmIIxydmIIzyyzzxxzyxxy,,zdmmzydmmyxdmmx,,如果令2/)()(21)(21)(212222222zzyyxxIIIdmyxdmzxdmzydmx2)()(21)(21)(212222222zzyyxxIIIdmyxdmzxdmzydmy2/)()(21)(21)(212222222zzyyxxIIIdmyxdmzxdmzydmz于是可把Ii表示为iiiiiiiiiizziyyixxiyzixziiiyzizziyyixxixyiiixzixyizziyyixximzmymxmzmIIIIIymIIIIIxmIIIIII222（4.18）具有n个连杆的机械手总的动能为niijikkikTiikiniiqqqTIqTTraceKK111121（4.19）此外，连杆i的传动装置动能为221iaiaiqIK式中，Iai为传动装置的等效转动惯量，对于平动关节，Iai为等效质量；iq为关节i的速度。所有关节的传动装置总动能为niiaiaqIK1221于是得到机械手系统（包括传动装置）的总动能为niiaikjniijikkTiijiatqIqqqTIqTTraceKKK121112121（4.20）下面再来计算机械手的位能，一个在高度h处质量为m的物体，其位能为mghP连杆i上位置ri处的质点dm，其位能为rdmTgrdmgdPiiTTi0式中，]1,,,[zyxTggggiiiTiiiiiTiiiTiiiTiiirTgmrmTgrdmTgrdmTgdPP连杆连杆连杆其中，mi为连杆i的质量：iir关节坐标系的重心位置。为连杆i相对于其由于传动装置的重力作用Pai一般是很小的，可以略之不计，所以，机械手系统的总位能为nininiiiiTiiaiirTgmPPPP111)(（4.21）4.2.3动力学方程的推导据式（4.1）求拉格朗日函数,2,1,2121112111nrTgmqIqqqTIqTTracePKLniiiiTiniiaikjniijikkTiijit（4.22）再据式（4.2）求动力学方程。先求导数npqIqqTIqTTraceqqTIqTTraceqLpapjniijpTiijikniikkTiipip,2,121211111niqLqLdtdFiii,2,,据式（4.18）知，Ii为对称矩阵，即iTiII，所以下式成立papkniikpTiikipjTiikijTiTikik