机械电子学-动力学仿真-第二节

1、机械电子学机械电子学陈陈学学超超Email: 2正动力学：正动力学：已知各个关节的作用力或力矩，各个关节的位移、速度，求得关节的加速度多刚体系统动力学公式多刚体系统动力学公式逆动力学：逆动力学：已知各个关节的位移、速度、加速度，各个杆件受到的外力，求各个关节所需要的驱动力或力矩2( )( , )( )TextM q qD q qG qJ f( , , ,)extqfq q f ( , , ,)extf q q q fn牛顿欧拉法牛顿欧拉法n空间矢量法空间矢量法n拉格朗日法拉格朗日法n凯恩（凯恩（KaneKane）法）法n3n牛顿欧拉法牛顿欧拉法n空间矢量法空间矢量法n拉格朗日法拉格朗日法n凯恩

2、（凯恩（KaneKane）法）法n4n牛顿欧拉法牛顿欧拉法n空间矢量法空间矢量法n拉格朗日法拉格朗日法n凯恩（凯恩（KaneKane）法）法n5n牛顿欧拉法牛顿欧拉法l这种方法是最直观的方法，通过牛顿方程和欧拉方程求解多刚体系统动力学。l由于时间的限制，只介绍大体的计算流程，如果有疑问，可以自学具体的计算细节。6n基本公式基本公式牛顿方程：描述了刚体质心的平移运动欧拉方程：描述了刚体绕质心的旋转运动7ccNIIcFmvcI是当坐标系原点在质心上时刚体的惯量n向向外迭代外迭代计算速度和加速度计算速度和加速度在已知了各个关节的角度、角速度、角加速度后，可以依次向外迭代，计算出所有刚体质心的线加速度

3、，绕质心的角速度、角加速度在杆件坐标系中的表示8ccNIIcFmvn作用作用在刚体上的力和力矩在刚体上的力和力矩计算出所有刚体质心的线加速度，绕质心的角速度、角加速度后，通过牛顿-欧拉公式便可求出作用在刚体上的力和力矩9iicciiiiNIIiicFmvn向内迭代向内迭代计算关节的力或力矩计算关节的力或力矩在计算出来所有刚体所受的净外力和外力矩后，通过向内迭代，可以计算出相邻刚体间的相互作用力，通过计算作用力在关节轴向的分量，便可求出关节力矩或力。10iT iiiineiT iiiife转动关节移动关节n牛顿欧拉法牛顿欧拉法n空间矢量法空间矢量法n拉格朗日法拉格朗日法n凯恩（凯恩（KaneKa

4、ne）法）法n11参考文献：参考文献：nFeatherstone R.The acceleration vector of a rigid body J. IJRR, 2001, 20: 841nFeatherstone R. A Beginners Guide to 6-D Vectors (Part 1)J. IEEE Robotics & Automation Magazine, 2010, 17(3): 83-94.nFeatherstone R. A Beginners Guide to 6-D Vectors (Part 2)J. IEEE Robotics & A

5、utomation Magazine, 2010, 17(4): 88-99.12n主要分两个部分介绍空间向量第一部分：什么是空间向量？它们如何工作？如何使用它们？第二部分：如何将空间向量的运算方式转换成程序，求正逆动力学？13n主要分两个部分介绍空间向量第一部分：什么是空间向量？它们如何工作？如何使用它们？第二部分：如何将空间向量的运算方式转换成程序，解正逆动力学？14n既然一个刚体有6个自由度，那么为什么不用一个6维向量来表示它的运动与作用到它上的力呢？n优点：大大减少了代数运算量，只需更少的变量与更少的公式n如果将6维向量想成仅仅是3维向量的简单叠加，那么就没有理解其真正含义n6维向量法

6、其实是一种思考的方法，具有自有的物理含义与数学性质15n用3维向量解2个刚体系统的动力学16n用6维向量解2个刚体系统的动力学17n空间矢量法的两种变量空间矢量法的两种变量类型类型一种描述刚体的运动，表示为一种描述刚体受到的力，表示为变量上面的符号表示此变量为空间矢量，是一个6维向量，Plcker坐标系被用来描述空间矢量186mM6fFPlcker坐标系nPlckerPlcker坐标系坐标系首先需要建立一个笛卡尔坐标系，笛卡尔坐标系的位置与方位定义了Plcker坐标系，这两种坐标系所描述的向量集合是1:1的映射关系19n定义定义如下三种单位矢量如下三种单位矢量： 中的元素代表在x，y，z方向上

7、的单位欧几里德向量，这组单位向量定义了一个笛卡尔坐标系。 中的前三个元素分别表示绕ox，oy，oz轴的旋转，后三个元素分别表示沿x，y，z轴的平移。 中的前三个元素分别表示绕x，y，z轴的力矩，后三个元素分别表示沿ox，oy，oz方向的力。20Ci, j, koxoyozxyz6Dd ,d ,d ,d ,d ,dMxyzoxoyoz6e ,e ,e ,e ,e ,eFCD设一个刚体的运动在笛卡尔坐标系中的表示为那么此运动在Plcker坐标系中表示为：21xyzijkOOxOyOzvvvvijkxOxyOyzOzOxxOyyOzzvvvvdddddd设一个刚体受到的作用力在笛卡尔坐标系中表示为那

8、么此作用力在Plcker坐标系中的表示为：22xyzffffijkOxOyOznnnOnijkOxxOyyOzzxOxyOyzOznnnffffeeeeee如果采用坐标向量的方式表示，那么运动与力的空间向量表示简化为：或者为：23xyzOxOyOzvvvvOxOyOzxyznnnffffovvonff这种简单的表示方式非常方便，也比较常用，但是缺点是看上去好像是三维向量的简单叠加，容易使人误解为空间向量是三维向量的简单叠加，但是这种叠加其实只是书写形式的叠加，空间向量有其自己的物理含义。nPlckerPlcker坐标变换坐标变换在进行动力学计算时，往往需要将空间向量在不同的坐标系之间进行转换。

9、假设有两个坐标系A和B，E是从坐标系A到坐标系B的旋转矩阵，r是坐标系B的原点在坐标系A中的位置。24nPlckerPlcker坐标变换坐标变换设 ， ， ， 为空间向量，分别表示在A和B坐标系中的运动、受力，那么转换规则如下：其中 是将运动从坐标系A转到坐标系B的转换矩阵， 是将力从坐标系A转到坐标系B的转换矩阵，它们二者之间的关系为25AmBmAfBfBBAAmXm*BBAAfXfBAX*BAX*()BBTAAXXnPlckerPlcker坐标变换坐标变换 和 仅取决于坐标系B相对于坐标系A的相对位置其中：E是坐标系B在A中的姿态矩阵，r是坐标系B的原点在A中的位置。另外， 是斜对称矩阵，

10、26BAE010X0E-r1*()BBTAAE01-rXX0E01000 xzyyzxzyxrrrrrrrrr rBAX*BAXrn空间矢量的微分空间矢量的微分当对一个在移动的Plcker坐标系中定义的矢量进行微分时，有如下的式子：A既是Plcker坐标系的名字，也是定义这个坐标系的坐标框架的名字， 是坐标框架A的速度在坐标系A中的表示27AAv()AAAAAdmdmvmdtdt*()AAAAAdfdfvfdtdtn空间矢量的微分空间矢量的微分上面两式定义了两种运算符 和 定义如下28*()AAAAAdmdmvmdtdt*()AAAAAdfdfvfdtdt0oovvv *0oTovvvv n空

11、间矢量的微分空间矢量的微分一种比较特殊的情况（也是用得比较多的情况），即运动与力都固定在某个坐标系上，它们的改变仅仅是因为坐标系本身的运动，那么V是坐标系的速度。29mvm *fvf n空间矢量的加速度空间矢量的加速度欧式空间的速度定义为刚体上一个固定点O的速度，即 ，加速度定义为刚体上固定点O的加速度，即 。 空间矢量法定义的速度为在空间中一个固定点测量的刚体的速度，加速度为空间速度的变化。30oddavvrrdtdtr r 空间速度的参考点在刚体上不是固定的，而是一些列变化的点，这是线加速度多出一项的原因n空间矢量的加速度空间矢量的加速度与传统的加速度相比，空间加速度确实比较难以理解，但是

12、使用起来非常方便。假如两个刚体B1和B2由一个关节连接，那么它们的速度有如下关系式加速度有如下关系式s为关节的转轴方向矢量，q为关节角度3121vvsq 21aasqsqn刚体的空间惯量刚体的空间惯量空间惯量将速度 转成动量 空间惯量为如果有N个刚体被固连在一起，那么有32hIv vhTcTmmImm Ic ccc11NtotiiIIn刚体的空间惯量刚体的空间惯量空间惯量是与10个量相关，质量1个，质心位置3个，刚体惯量6个。注意：传统的惯量与位置无关，而空间惯量与质心位置相关。33TcTmmImm Ic ccc1n刚体的空间惯量刚体的空间惯量空间惯量遵循的运算法则：另外，刚体的机械能为34*

13、BBAAABIXI X*dIvIIvdt 12Ev Ivn运动方程运动方程一个速度为v，惯性为I的刚体的运动方程为： f 是作用在刚体上的合外力， a 是由于外力产生的加速度。35*()dfIvIavIvdt n运动约束运动约束相对速度：相对加速度：3621vvSq21aaSqSqn空间矢量运算法则总结空间矢量运算法则总结相对速度：如果刚体 与 的速度分别为 与 ，那么 相对于 的速度为合力：如果 和 作用于同样的刚体上，那么它们等效于一个合力 作用力与反作用力：如果刚体 施加了力 到刚体 上，那么 施加一个力 到 上，这是牛顿第三定律的空间矢量形式3721relvvv1B2B1v2v1B2B

14、1f2ftotf12totfffff1B2B2B1Bn空间矢量运算法则总结空间矢量运算法则总结数量积：如果力 作用于速度为 的刚体上，那么该力所产生的功率为微分：运动的微分还是运动，力的微分还是力。如果 和 固定在速度为 的刚体上，那么 ，加速度：空间加速度是空间速度的微分。如果 ，那么合惯量：如果刚体 和 的惯量分别为 和 ，当这两个刚体固连后组成的刚体的惯量为38f v fvmfvmv m *fvf 21relvvv21relaaa1B2B1I2I12totIIIn空间矢量运算法则总结空间矢量运算法则总结动量：速度为 ，惯量为 的刚体的动量为运动方程：作用于刚体上的合力为刚体动量的微分39

15、IvIv*()d IvfIavIvdt n主要分两个部分介绍空间向量第一部分：什么是空间向量？它们如何工作？如何使用它们？第二部分：如何将空间向量的运算方式转换成程序，解正动力学与逆动力学？40.au/roy/spatial/n空间矢量法的应用例子空间矢量法的应用例子逆动力学是计算给定加速度所需作用力的问题，是一个相对比较容易的问题，因此从这问题出发，理解空间矢量法在多刚体动力学求解中的应用41(mod, , , )IDel q q q n右侧为求解逆动力学的Matlab代码，采用了牛顿-欧拉递归算法n可以看出，程序非常简明，这是3维向量难以实现

16、的42n模型数据结构模型数据结构model.NB：指定杆件的个数model.parent：描述杆件之间的连接关系，指定当前的连杆连接到的母连杆的编号model.Xtree：描述相邻杆件之间的相对位置关系，与坐标系的建立、杆件的初始位置、杆件的几何形状相关model.pitch：表示关节的运动类型，如移动、转动等model.m：杆件的质量model.c：杆件质心的位置model.I：杆件绕其质心的惯性张量矩阵，是一个3x3的对称矩阵43n连接（连接（ model.parent ）连接图描述了所有杆件间的连接关系。一个固定基座的多刚体系统采用如下几步处理：固定基座编号为0，作为树形结构的根剩余的刚

17、体从1到N连续编号，每个刚体的编号大于其母节点关节从1到N进行编号，第i个关节将第i个刚体连接到其母节点44n连接（连接（ model.parent ）当给刚体和关节编号完成后，便可以通过一个矩阵 描述刚体的连接， 为第i个刚体的母节点的编号，这个矩阵有如下性质：注意：无论是编号还是母节点矩阵都不是唯一的45( ) i0( ) iin几何关系（几何关系（ model.Xtree ）模型的几何关系描述了每个关节在其刚体上的相对位置。为每个关节引入一对坐标系，分别固连与被这个关节连接的两个刚体，比如 和46( ),i iFiFn几何关系几何关系 的求取根据关节类型不同而不同，有纯旋转关节、纯平移关

18、节、螺旋关节47( )JXin几何关系几何关系（ model.Xtree ） 是从坐标系 到坐标系 的转换矩阵，其实是定义了两个关节间的相对位置，这个量存储于 中，即从坐标系 到 的转换矩阵为48( )TXi( ) iF( ),i iFmod.el Xtree imod.( )Tel Xtree iXiiF( ) iF( )iiX( )( )( )iiJTXXi Xi( )( ( )0TXixlt r iIrI n关节模型（关节模型（ model.pitch ）如果 和 是经过关节j的速度与力，那么关节的数学模型由两个量组成，转换矩阵 与运动子空间矩阵S，S与关节轴的方向相关能量平衡方程49(

19、 )JiiivvvJivJifJXJiiiTiiJivS qS fiiJiJiqfvn运算法则运算法则50( )iiiivvs q( )iiiiiiaas qvq*BiiiiiifI avI v( )JiBiJjjifffTiiJis fiBif 是作用在刚体 上的净力Jiif 是经过关节 的作用力( )ii是刚体 的子连杆上个关节的作用力+下个关节的反作用力=刚体的净力n雅克雅克比矩阵比矩阵第一种形式：第二种形式：51bbvJ q( )biii k bvs q( )brootk b 是刚体 到之间的所有刚体集合12J.eeNsss自学雅克比矩阵的求解程序自学雅克比矩阵的求解程序bbbaJ q

20、J q自学加速度中分量自学加速度中分量 的求解的求解过程过程bJ q52n二二连杆模型连杆模型总共有两个刚体，model.NBmodel.NB=2=2固定坐标系编号为0，第一个杆件编号为1，第二个杆件编号为2，model.parentmodel.parent = = 0 0 11 model.pitchmodel.pitch = = 1 1,1 1,1表示绕x轴旋转53n二二连杆模型连杆模型 model.Xtreei100000010000001000model.1000100000010000001Xtree100000010000001000model.201010010001000000

21、1XtreeLL54n二二连杆模型连杆模型 model.m=1.0 1.0 质心坐标0model.101 / 2cL0model.202 / 2cL55n二二连杆模型连杆模型 惯量（对称正定矩阵）10.0010.002model.10.0010.010.0030.0020.0030.01I10.0010.002model.20.0010.010.0030.0020.0030.01I56n二二连杆模型连杆模型利用现有程序对此模型求解正动力学与逆动力学Dynamics_test.mn牛顿-欧拉法与空间向量法均属于动态平衡法，需要求解加速度与力的关系。拉格朗日法是功能平衡法，只需求解速度，而不必求解

22、内力57n拉格朗日算子n拉格朗日方程（广义坐标系）58( , )( , )( )L q qT q qV q动能势能dLLdtqq外力n拉格朗日方程推导演示笛卡尔坐标系中的拉格朗日方程推导自学广义坐标系中的拉格朗日方程推导59在一个保守力场中有N个质点，第i个质点的坐标为 ，这个系统的动能为其中M是这个系统的自由度个数。当质点只在1维运动时，M=N。当质点在3维运动时，M=3N。60ix2112MiinTm x动能对速度的微分是质点的动量，即将动量对时间微分为61 iiTpxiiidpdTmxdtdtx在保守力场中，作用于质点的力是势能对位置的微分，即根据牛顿定律可得62 iiVFxiiidpF

23、dt ()iiidTVdtxxi 为作用于质点的外力根据动能与势能公式可得根据以上的结果可得630iTx 0iVx动能只与速度相关势能只与位置相关()()()iiidTVTVdtxx定义 ， L命名为拉格朗日算子，拉格朗日方程可写为64()iiidLLdtxxLTVn二连杆模型首先求出质点的位置65111 111 10 xyrszrc221 121221 12 120 xyrsr szrcrcn二连杆模型再求出质点的速度66111 1 111 1 10 xyrczrs221 1 12 121221 1 1212120()()xyrcrczrsr sn二连杆模型设杆件的惯量为671111000000 xyzIIII2222000000 xyzIIIIn二连杆模型系统动能682221111122222221211()2211()()22zzTm y