机器人模型与控制-5动力学模型ppt课件

1、5. 动力学模型动力学模型 动力学是研究物体的运动和作用力之间的关系。 操作臂是一个复杂的动力学系统，由于系统由多个关节和连杆组成，且位形、速度和加速度都是耦合的，因此系统具有严重的非线性。 操作臂动力学反映的是多输入、多输出、多变量的耦合非线性系统动态特性。 动力学模型将力与位置、速度和加速度联系起来，实际上可看作是动力学方程，且是一个非线性微分方程组。 由力求位置、速度、加速度等运动学特征称为动力学原正问题，一般情况下根本不可能求解； 已知运动学特征求驱动力称为动力学逆问题。 研究机器人动力学的目的：研究机器人动力学的目的： (1)为了实时控制：为了实时控制： 在控制算法中包含动力学信息反

2、馈线性化、前在控制算法中包含动力学信息反馈线性化、前馈），以期达到更好的动态特性；馈），以期达到更好的动态特性； 但是动力学信息的完整性与计算实时性之间存在但是动力学信息的完整性与计算实时性之间存在矛盾，因此需要做适当的假设简化，随着计算机技术的飞矛盾，因此需要做适当的假设简化，随着计算机技术的飞速发展，这一矛盾可以在某种程度上得到解决；速发展，这一矛盾可以在某种程度上得到解决； 可以设计包含简化动力学信息的控制规律，甚至可以设计包含简化动力学信息的控制规律，甚至不包含动力学信息的控制规律，在详细动力学模型上进行不包含动力学信息的控制规律，在详细动力学模型上进行控制仿真。控制仿真。 (2)动态

3、设计与仿真：动态设计与仿真： 设计人员可以根据连杆的质量、负载大小、传动设计人员可以根据连杆的质量、负载大小、传动机构特征、驱动能力进行动态仿真，以选择适当的参数，机构特征、驱动能力进行动态仿真，以选择适当的参数，改进设计，获得动态性能更好的操作臂。改进设计，获得动态性能更好的操作臂。 获得机器人动力学模型的方法很多：拉格朗日算法、牛顿欧拉法、凯恩方程算法等 (1)拉格朗日方法： 一种能量方法，能以最简单的形式求得非常复杂的系统动力学方程，而且具有显示结构，可用于定性分析。 (2)牛顿欧拉法： 一种递推方法，基于运动坐标系和达朗伯原理，从操作臂末端开始，把驱动力作为位置、速度和加速度的函数精确

4、地计算出来，没有多余信息，计算速度快，可用于控制时的实时计算。5.1 拉格朗日动力学 对于任何机械系统，拉格朗日函数定义为系统的动能和势能之差pkEEL 系统的动能和势能可以用任意坐标系来表示，比如广义坐标 ，不限于笛卡儿坐标。 iq 第二类拉格朗日方程iiiqLqLdtd式中： 为广义坐标， 为广义速度， 为广义力假设 是直线坐标，那么 是力；假设 是角度坐标，那么 是力矩）。iqiq iiqiiqi 由于势能中不含显式的广义速度，因此拉格朗日方程还可写成ipikikiqEqEqEdtd例：图示例：图示RP机械手有机械手有2个关节组成，连杆质量均为集中个关节组成，连杆质量均为集中质量分别为质

5、量分别为m1和和m2，质心位置分别为，质心位置分别为r1和和r，广义坐标，广义坐标为为和和r。（1求质心速度 连杆1的质心速度sincos1111ryrxcossin1111ryrx221212121ryxv 连杆2的质心速度 sincos22ryrxcossinsincos22rryrrx222222222rryxv（2求系统的动能和势能 连杆1的动能 连杆2的动能2211121rmEk2222221rrmEk 系统总动能222222211212121rmrmrmEk 连杆2的势能 系统总势能 连杆1的势能sin111grmEpsin22grmEpsinsin211grmgrmEp（2求系统

6、的动力学方程 拉格朗日函数 拉格朗日方程 写成矩阵形式 sinsin212121 211222222211grmgrmrmrmrmEELpkcos2 211222211grmrmr rmrmrmLLdtd sin2222gmrmrmrLrLdtdr 重力项向心力项哥氏力项惯性力项sincos2002211222222211gmgrmrmrmr rmrmrmrmr5.2 惯性张量和惯性矩阵 前面例中将各连杆质量集中到一点，实际上各连杆质量是连续分布的。因此这里需要引入惯性张量和惯性矩阵的概念，用于描述连杆的质量分布。 张量 (Tensor) 是 n 维空间内，有 nr个分量的一种量， 其中每个分

7、量都是坐标的函数， 而在坐标变换时，这些分量也依照某些规则作线性变换。 r 称为该张量的阶 (Rank)，零阶张量 (r = 0) 为标量 (Scale)，一阶张量 (r = 1) 为向量 (Vector)， 二阶张量 (r = 2) 则成为矩阵 (Matrix)。数学定义：由若干坐标系改变时满足一定坐标转化关系的有序数组成的集合为张量。 刚体移动时，涉及到质量参数；刚体转动时，涉及到惯性矩。 刚体的质量分布由惯性矩和惯性积共同描述，惯性矩和惯性积共同组成惯性张量。 惯性矩 (moment of inertia)：物体相对于某坐标轴而言的，质量微元dm和此微元到某坐标轴的最短距离的平方之积，再

8、对整个物体质量进行积分。 惯性积(product of inertia)：物体相对于一组相互正交的两平面而言的，质量微元dm和此微元到各平面的最短距离的乘积，再对整个物体质量进行积分。mVzzmVyymVxxdmyxdVyxIdmxzdVxzIdmzydVzyI222222222222mVzxmVyzmVxyzxdmdVzxIyzdmdVyzIxydmdVxyI 惯性张量：贯性矩与惯性积的组合惯性张量：贯性矩与惯性积的组合zzyzzxyzyyxyzxxyxxAIIIIIIIIII 惯性张量与坐标系的选取有关，如果选取的坐标系方位使得各惯性积均为零，此时惯性张量为对角型，此坐标系的各轴称为刚体的

9、惯性主轴，相应的惯性矩称为主惯性矩。 mAdmyxyzxzyzzxxyxzxyzy222222I 惯性张量的元素是刚体相对于某坐标系的质量分布的二阶矩。 在机器人学中通常以齐次坐标 来描述点的位置，因此在下面推导动力学模型过程中要用到另外一种质量分布表达，称为伪惯性矩阵。 包含质量和一阶矩项T1zyxrmmVdmzyxzzyzxzyyzyxyxxzxyxdmdV1222TTrrrrJmVmVmVVzdmdVzzmydmdVyymxdmdVxxmdVm 表示刚体的质心位置 zyx, 定义相对于原点的惯性矩为 ，则伪惯性矩阵可表示为 。 zzyyxxoIIIImzmymxmzmIIIIymIIII

10、xmIIIIzzoyzxzyzyyoxyxzxyxxo222J 伪惯性矩阵也与坐标系的选取有关，如果选取坐标系原点在质心上，同时选取坐标轴方向使各惯性积为零，则此坐标系称为刚体主坐标系；相对于主坐标系而言，刚体的伪惯性矩阵为对角型的。 如果将现有坐标系A平移到质心C处，则根据平行移轴定理，刚体相对于两坐标系的贯性矩和惯性积存在以下关系 表示成矢量形式 222222CCzzCzzACCyyCyyACCxxCxxAyxmIIzxmIIzymIIT3TCCCCCAmPPIPPIICCzxCzxACCyzCyzACCxyCxyAxmzIIzmyIIymxII 式中： I3为的单位矩阵； 为质心相对于坐

11、标系A的位置矢量。 TCCCCzyxP例：求图示密度为例：求图示密度为的均匀长方体的的均匀长方体的惯性张量和伪惯性矩阵。惯性张量和伪惯性矩阵。 质量 lwhm质心坐标222hzlywx惯性矩220002222000222200022333lwmdzdydxyxIwhmdzdydxxzIhlmdzdydxzyIhlwzzhlwyyhlwxx 惯性积hwmdzdydxzxIlhmdzdydxyzIwlmdzdydxxyIhlwzxhlwyzhlwxy444000000000 惯性张量惯性张量 伪惯性矩阵伪惯性矩阵 如果将坐标系移到长方体中心处，惯性张量和伪惯性矩阵都为对角阵：22222234443

12、4443lwmhwmlhmhwmwhmwlmlhmwlmhlmI22232hlwmIIIIzzyyxxomhmlmwmhmhmhwmlhmlmhwmlmwlmwmlhmwlmwm222234424342443222J222222120001200012lwmwhmhlmCImhmlmwmC000012000012000012222J5.3 操作臂的拉格朗日方程 利用拉格朗日方法推导操作臂的动力学模型的步骤： 计算连杆各点的速度； 计算系统的动能； 计算系统的势能； 构造拉格朗日函数； 推导动力学方程拉格朗日方程）。 下面详细讨论上述步骤一、连杆各点的速度 假定连杆i上的一点对坐标系i的齐次坐标

13、为ir，对基坐标系0的齐次坐标为0r，则有X0 Y0 Z0 Xi Yi Zi ir 0r rTrii00该点的速度 rTrriijjjiqqdtd1000速度的平方可以用求矩阵迹来代替矢量点乘，矩阵对角元素之和称为该矩阵的迹trace）,记作tr(A)，变换乘法和求和的顺序） ijikkjkiiijiiikkkiiijjjiqqqqqqqq11T0T0T1010T000T0Tr TrTrTrrTrTrTrrrr二、系统的动能 在连杆i的ir处，质量为dm的质点的动能为 X0 Y0 Z0 Xi Yi Zi ir 0r 连杆i的动能为ijikkjkiiijiijikkjkiiijikiqqqmqq

14、qqqmmE11T0T011T0T00T0dTr21 Trd21 d21dTrrTTrrTrrijikkjkiijiijikkjkiiiiijiikikiqqqqqqqmqmEEi11T0011T0link T00T0link Tr21 dTr21d21d TJTTrrTrrJ伪惯性矩阵的连杆整个操作臂n个连杆的总动能为X0 Y0 Z0 Xi Yi Zi ir 0r 除各连杆的动能之外，关节的动能主要指传动机构动能，驱动器动能通常在控制器中考虑也不能忽视，可表示为 niijikkjkiijinikikqqqqEE111T001Tr21TJT221iaikaiqIE式中 为广义等效惯量，对于移动

15、关节是等效质量，对于转动关节是等效惯性矩。 aiI 把连杆动能与关节动能相加，并交换求迹运算与求和运算的顺序得到操作臂的系统动能 niiaiijikkjkiijinikikqIqqqqEE1211T001Tr21TJT三、系统的势能 连杆i的势能相对于基坐标系为重力做功的负为 X0 Y0 Z0 Xi Yi Zi ir 0r 操作臂的总势能为 iCiiiiCipimmErTgrg00式中：mi为连杆i的质量， 为重力行矢量， 、 分别为连杆i在坐标系i和0中的质心位置矢量。 0zyxggggiCiriCr0niiCiiipmE10rTg四、拉格朗日函数 niiCiiiniiaiijikkjkii

16、jipkmqIqqqqEEL101211T00Tr21rTgTJT五、操作臂的动力学方程 利用拉格朗日方程可以得到关节i的广义驱动力i nijjCjijjnijjkjmmkmkjjijiainijjkkkjjijiiiqmqqqqqqIqqqqLqLdtdrTgTJTTJT011T0201T00Tr Tr 上式可写成 inknmmkikmnkkikiGqqhqD111 式中： kikiIqqDikikainkijkjjijik,0,1,Tr,maxT00TJTnmkijmkjjijikmqqqh,maxT020TrTJTnijjCjijjiqmGrTg0整个操作臂的动力学方程写成矩阵形式 关节

17、驱动力矢量 式中： tttttqGtqqhqqD , Tntttt21 Tntqtqtqt21q关节位置矢量 关节速度矢量 Tntqtqtqt21q Tntqtqtqt 21q 为操作臂质量矩阵，nn对称，元素为 qDikDTnhhh21,qqh为非线性哥氏力和离心力矢量，元素为 nknmmkikmiqqhh11(km时为离心力；km时为哥氏力) TnGGG21qG为重力矢量，元素为Gi 的物理意义： 、iG、ikD、ikmh5.4 牛顿欧拉递推动力学公式 牛顿欧拉法基于达朗贝尔原理；惯性载荷由两部分组成：（刚体运动随质心移动+绕质心转动） 牛顿第二定律力平衡方程） 欧拉方程力矩平衡方程）“对

18、于任何物体，外加力和运动阻力惯性力在任何方向上的代数和为零。”iiiCiCiCmdtmdvvfiiCiiiCiiCCdtdiIIImgmiiCiiiiiiiiffRf111gmiiCiCiCiiiiiiiiiiiiiiiifrmfRPmRm11111110111gmiiiiiiiifRfiCif01111111gmiiCiiiiiiiiiiiiiirfRPmRmiiiCiCiCifrm解得 利用牛顿欧拉方法递推求操作臂的动力学模型的算法由三部分组成： 首先由连杆1到连杆n递推计算各连杆的速度和加速度； 再由牛顿欧拉公式算出各连杆的惯性力和惯性力矩； 最后根据达朗伯原理从连杆n到连杆1递推计算各

19、连杆内部相互作用力和力矩，以及关节驱动力和力矩。 操作臂连杆间的运动传递： （1旋转关节的速度和加速度传递 连杆i+1的角速度10010iiiiiR其中1111111100iTiiiiiiiiiRzR将角速度对时间求导1001001010010iiiiiiiiiiiiiiiiSRRRR其中11110111111111iiiiiiiiiiiiiiiii zRzRzR因此，连杆i+1的角加速度10011101010iiiiiiiiiiSRzR 10010iiiiiPRPP连杆i+1坐标原点的位置100010010iiiiiiiiiiSPRPPRPP对上式求导，得连杆i+1坐标原点的速度100010

20、iiiiiiSPRvv继续求导，得连杆i+1坐标原点的加速度 10001000100100010 iiiiiiiiiiiiiiiiiiiiSSSSSPRPRvPRPRvv（2移动关节的速度和加速度传递移动关节的速度和加速度传递 连杆连杆i+1的角速度的角速度ii010其中将角速度对时间求导，得到角加速度ii01010010iiiiiPRPP连杆i+1坐标原点的位置1010010iiiiiiiiPRPRPP对上式求导，得连杆i+1坐标原点的速度继续求导，得连杆i+1坐标原点的加速度1111111100iTiiiiiiiiiddRzRP整理得到连杆i+1坐标原点的速度11101100010iiii

21、iiiiiidSzRPRvv 1110111101010001000102iiiiiiiiiiiiiiiiiiiiddSSSS zRzRPRPRvv（3质心的加速度质心的加速度 连杆连杆i的质心位置的质心位置CiiiiCiPRPP000CiiiiiCiiiiCiSPRPPRPP000000对上式求导，得连杆i的质心速度CiiiiiCiSPRvv0000继续求导，得连杆i的质心加速度 CiiiiiCiiiiiCiiiiCiiiiiCiSSSSSPRPRvPRPRvv000000000000 （4递推公式递推公式 符号说明：符号说明： jui 表示坐标系表示坐标系i相对于相对于j的运动学量的运动学

22、量在在j中的表达；中的表达； 表示坐标系表示坐标系i相对于相对于j的运动学的运动学量在量在k中的表达。中的表达。)( )( 01111011010110移动关节旋转关节iiiiiiiiiiiiiiiRzRR进一步得到)( )( 0111101111011010110移动关节旋转关节iiiiiiiiiiiiiiiiiiiiiiRzRzRR )( 2 )( 111111110100100110010011010110移动关节旋转关节iiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiidd zzPPvRPPvRvRvCiiiiiiCiiiiiiCiiiCiPPvvRv00

23、00000kiju 牛顿欧拉法递推过程具体步骤如下： （1向外递推计算连杆速度和加速度i：1n） CiiiCiim vfiiCiiiiiiCiiCiiIIm000（2计算关节驱动力或力矩（牛顿第二定律）（欧拉方程）)( )( 11011101000移动关节旋转关节iiiiiiiiiiiiiiiRzRR)( )( 110111011101000移动关节旋转关节iiiiiiiiiiiiiiiiiiiiiiRzRzRR )( 2 )( 0111011011101101111011011101101000移动关节旋转关节iiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiid

24、d zzPPvRPPvRvRvCiiiiiiCiiiiiiCiiiCiPPvvRv0000000 （3向内递推计算力和力矩i：n1） gffRfiiCiiiiiiiim111gfrmfRPmRmiiCiiCiiCiiiiiiiiiiiiiim1111111关节驱动力或力矩)( )( TT移动关节转动关节iiiiiiiiiZfZn 阐明： 做速度和加速度递推时，当i=1，有000000110000110000110vviiiiii做力和力矩递推时，当i=n，有RRRmmmfffnTnniiTTnniiTTnnii1111111111表示末端手抓对环境的作用力表示末端手抓对环境的作用力矩表示手抓相

25、对于末端连杆的姿态5.5 不同空间的动力学模型 关节空间的状态方程 qGqqhqqD , 形位空间的动力学方程 qGqqCqqqBqqD2 将与速度有关的项分成两部分 2,qqCqqqBqqh 系数矩阵都只是形位的函数； 为哥氏力系数矩阵， 维； qB21nnn 为离心力系数矩阵 ， 维； qCnn 为关节速度积矢量 ， 维；T)1(3121nnqqqqqqqq21nn 操作空间动力学方程 利用 和 ，得到 假设 可逆，则存在 qPqqUXqVF , FqJT qGqqhqqD , qJqVqJqDT qqJqVqqUqJqqh,T qPqJqGT qJ qJFT- qJqDqJqV1T- q

26、qJqVqqhqJqqU,T- qGqJqPT-5.6 基于雅可比矩阵的拉格朗日方程 对于n连杆机械臂，连杆i的速度和角速度可表示为 整个机械臂的动能为整个机械臂的动能为 qqJviii，展开 qqJqqJviivii连杆i的质量为mi；相对于坐标系i的惯性张量为Ii ，将其坐标轴方向变换到参考坐标系中为 qRIqRTiii qqDqqqJqRIqRqJqJqJqTniiTiiiTiviTviiTkmE21 211定义惯性矩阵 niiTiiiTiviTviim1qJqRIqRqJqJqJqD惯性矩阵D(q)与构型有关，为对称正定矩阵，元素以dij表示。对称性显而易见；正定性可由动能总是非负，及

27、当且仅当速度为零时动能为零得出来 。有利于控制系统定性分析各关节速度产生的连杆线速度,3n各关节速度产生的连杆角速度,3nviJiJ 整个机械臂的势能整个机械臂的势能 在刚体动力学的情况下，势能的唯一来源就是重力。第在刚体动力学的情况下，势能的唯一来源就是重力。第i个连个连杆的势能可以通过假定其所有的质量集中在它的质量中心来计算杆的势能可以通过假定其所有的质量集中在它的质量中心来计算 CiTiPimErgg是在惯性坐标系下重力向量，是在惯性坐标系下重力向量，rCi是第个连杆质量中心的坐标向量是第个连杆质量中心的坐标向量 如果机器人有弹性，势能还包括弹性势能。留意：势能仅仅是广义坐标的函数，而不是广义坐标微分变量的函数。 整个机械臂的势能niCiTiniPiPmEE11rg 拉格朗日函数拉格朗日函数以关节k为例 qPijjiijPkEqqdEEL21jjkjkqdqLijjiikjjjkjjjkjjjkjkqqqdqdqddtdqdqLdtd kPijjikijkqEqqqdqL21代入拉格朗日方程kkkqLqLdtd 运动方