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

5.动力学模型•动力学是研究物体的运动和作用力之间的关系。操作臂是一个复杂的动力学系统，由于系统由多个关节和连杆组成，且位形、速度和加速度都是耦合的，因此系统具有严重的非线性。操作臂动力学反映的是多输入、多输出、多变量的耦合非线性系统动态特性。•动力学模型将力与位置、速度和加速度联系起来，实际上可看作是动力学方程，且是一个非线性微分方程组。由力求位置、速度、加速度等运动学特征称为动力学原（正）问题，一般情况下根本不可能求解；已知运动学特征求驱动力称为动力学逆问题。•研究机器人动力学的目的：(1)为了实时控制：在控制算法中包含动力学信息（反馈线性化、前馈），以期达到更好的动态特性；但是动力学信息的完整性与计算实时性之间存在矛盾，因此需要做适当的假设简化，随着计算机技术的飞速发展，这一矛盾可以在某种程度上得到解决；可以设计包含简化动力学信息的控制规律，甚至不包含动力学信息的控制规律，在详细动力学模型上进行控制仿真。(2)动态设计与仿真：设计人员可以根据连杆的质量、负载大小、传动机构特征、驱动能力进行动态仿真，以选择适当的参数，改进设计，获得动态性能更好的操作臂。•获得机器人动力学模型的方法很多：拉格朗日算法、牛顿欧拉法、凯恩方程算法等(1)拉格朗日方法：一种能量方法，能以最简单的形式求得非常复杂的系统动力学方程，而且具有显示结构，可用于定性分析。(2)牛顿－欧拉法：一种递推方法，基于运动坐标系和达朗伯原理，从操作臂末端开始，把驱动力作为位置、速度和加速度的函数精确地计算出来，没有多余信息，计算速度快，可用于控制时的实时计算。5.1拉格朗日动力学•对于任何机械系统，拉格朗日函数定义为系统的动能和势能之差pkEEL•系统的动能和势能可以用任意坐标系来表示，比如广义坐标，不限于笛卡儿坐标。iq•第二类拉格朗日方程iiiqLqLdtd式中：为广义坐标，为广义速度，为广义力（若是直线坐标，则是力；若是角度坐标，则是力矩）。iqiqiiqiiqi•由于势能中不含显式的广义速度，因此拉格朗日方程还可写成ipikikiqEqEqEdtd例：图示R－P机械手有2个关节组成，连杆质量（均为集中质量）分别为m1和m2，质心位置分别为r1和r，广义坐标为θ和r。（1）求质心速度连杆1的质心速度sincos1111ryrxcossin1111ryrx221212121ryxv连杆2的质心速度sincos22ryrxcossinsincos22rryrrx222222222rryxv（2）求系统的动能和势能连杆1的动能连杆2的动能2211121rmEk2222221rrmEk系统总动能222222211212121rmrmrmEk连杆2的势能系统总势能连杆1的势能sin111grmEpsin22grmEpsinsin211grmgrmEp（2）求系统的动力学方程拉格朗日函数拉格朗日方程写成矩阵形式sinsin212121211222222211grmgrmrmrmrmEELpkcos2211222211grmrmrrmrmrmLLdtdsin2222gmrmrmrLrLdtdr重力项向心力项哥氏力项惯性力项sincos2002211222222211gmgrmrmrmrrmrmrmrmr5.2惯性张量和惯性矩阵•前面例中将各连杆质量集中到一点，实际上各连杆质量是连续分布的。因此这里需要引入惯性张量和惯性矩阵的概念，用于描述连杆的质量分布。•张量(Tensor)是n维空间内，有nr个分量的一种量，其中每个分量都是坐标的函数，而在坐标变换时，这些分量也依照某些规则作线性变换。r称为该张量的阶(Rank)，零阶张量(r=0)为标量(Scale)，一阶张量(r=1)为向量(Vector)，二阶张量(r=2)则成为矩阵(Matrix)。数学定义：由若干坐标系改变时满足一定坐标转化关系的有序数组成的集合为张量。•刚体移动时，涉及到质量参数；刚体转动时，涉及到惯性矩。•刚体的质量分布由惯性矩和惯性积共同描述，惯性矩和惯性积共同组成惯性张量。惯性矩(momentofinertia)：物体相对于某坐标轴而言的，质量微元dm和此微元到某坐标轴的最短距离的平方之积，再对整个物体质量进行积分。惯性积(productofinertia)：物体相对于一组相互正交的两平面而言的，质量微元dm和此微元到各平面的最短距离的乘积，再对整个物体质量进行积分。mVzzmVyymVxxdmyxdVyxIdmxzdVxzIdmzydVzyI222222222222mVzxmVyzmVxyzxdmdVzxIyzdmdVyzIxydmdVxyI惯性张量：贯性矩与惯性积的组合zzyzzxyzyyxyzxxyxxAIIIIIIIIII•惯性张量与坐标系的选取有关，如果选取的坐标系方位使得各惯性积均为零，此时惯性张量为对角型，此坐标系的各轴称为刚体的惯性主轴，相应的惯性矩称为主惯性矩。mAdmyxyzxzyzzxxyxzxyzy222222I•惯性张量的元素是刚体相对于某坐标系的质量分布的二阶矩。•在机器人学中通常以齐次坐标来描述点的位置，因此在下面推导动力学模型过程中要用到另外一种质量分布表达，称为伪惯性矩阵。包含质量和一阶矩项T1zyxrmmVdmzyxzzyzxzyyzyxyxxzxyxdmdV1222TTrrrrJmVmVmVVzdmdVzzmydmdVyymxdmdVxxmdVm表示刚体的质心位置zyx,,•定义相对于原点的惯性矩为，则伪惯性矩阵可表示为。zzyyxxoIIIImzmymxmzmIIIIymIIIIxmIIIIzzoyzxzyzyyoxyxzxyxxo222J•伪惯性矩阵也与坐标系的选取有关，如果选取坐标系原点在质心上，同时选取坐标轴方向使各惯性积为零，则此坐标系称为刚体主坐标系；相对于主坐标系而言，刚体的伪惯性矩阵为对角型的。•如果将现有坐标系{A}平移到质心C处，则根据平行移轴定理，刚体相对于两坐标系的贯性矩和惯性积存在以下关系表示成矢量形式222222CCzzCzzACCyyCyyACCxxCxxAyxmIIzxmIIzymIIT3TCCCCCAmPPIPPIICCzxCzxACCyzCyzACCxyCxyAxmzIIzmyIIymxII式中：I3为的单位矩阵；为质心相对于坐标系{A}的位置矢量。TCCCCzyxP例：求图示密度为ρ的均匀长方体的惯性张量和伪惯性矩阵。质量lwhm质心坐标222hzlywx惯性矩220002222000222200022333lwmdzdydxyxIwhmdzdydxxzIhlmdzdydxzyIhlwzzhlwyyhlwxx惯性积hwmdzdydxzxIlhmdzdydxyzIwlmdzdydxxyIhlwzxhlwyzhlwxy444000000000惯性张量伪惯性矩阵如果将坐标系移到长方体中心处，惯性张量和伪惯性矩阵都为对角阵：222222344434443lwmhwmlhmhwmwhmwlmlhmwlmhlmI22232hlwmIIIIzzyyxxomhmlmwmhmhmhwmlhmlmhwmlmwlmwmlhmwlmwm222234424342443222J222222120001200012lwmwhmhlmCImhmlmwmC000012000012000012222J5.3操作臂的拉格朗日方程•利用拉格朗日方法推导操作臂的动力学模型的步骤：计算连杆各点的速度；计算系统的动能；计算系统的势能；构造拉格朗日函数；推导动力学方程（拉格朗日方程）。下面详细讨论上述步骤一、连杆各点的速度假定连杆i上的一点对坐标系{i}的齐次坐标为ir，对基坐标系{0}的齐次坐标为0r，则有X0Y0Z0XiYiZiir0rrTrii00该点的速度rTrriijjjiqqdtd1000速度的平方（可以用求矩阵迹来代替矢量点乘，矩阵对角元素之和称为该矩阵的迹（trace）,记作tr(A)，变换乘法和求和的顺序）ijikkjkiiijiiikkkiiijjjiqqqqqqqq11T0T0T1010T000T0TrTrTrTrrTrTrTrrrr二、系统的动能在连杆i的ir处，质量为dm的质点的动能为X0Y0Z0XiYiZiir0r连杆i的动能为ijikkjkiiijiijikkjkiiijikiqqqmqqqqqmmE11T0T011T0T00T0dTr21Trd21d21dTrrTTrrTrrijikkjkiijiijikkjkiiiiijiikikiqqqqqqqmqmEEi11T0011T0linkT00T0linkTr21dTr21d21dTJTTrrTrrJ伪惯性矩阵的连杆整个操作臂（n个连杆）的总动能为X0Y0Z0XiYiZiir0r除各连杆的动能之外，关节的动能（主要指传动机构动能，驱动器动能通常在控制器中考虑）也不能忽视，可表示为niijikkjkiijinikikqqqqEE111T001Tr21TJT221iaikaiqIE式中为广义等效惯量，对于移动关节是等效质量，对于转动关节是等效惯性矩。aiI把连杆动能与关节动能相加，并交换求迹运算与求和运算的顺序得到操作臂的系统动能niiaiijikkjkiijinikikqIqqqqEE1211T001Tr21TJT三、系统的势能连杆i的势能（相对于基坐标系）为重力做功的负为X0Y0Z0XiYiZiir0r操作臂的总势能为iCiiiiCipimmErTgrg00式中：mi为连杆i的质量，为重力行矢量，、分别为连杆i在坐标系{i}和{0}中的质心位置矢量。0zyxggggiCiriCr0niiCiiipmE10rTg四、拉格朗日函数