1、 与动力学有关的两个问题:已知一个轨迹点, , 希望 求出期望的关节力矩矢量 .计算在施加一组关节力矩的情况下机构如何运动., 在任一瞬时，对刚体的线速度和角速度进行求导，可分别得到线加速度和角加速度: 同速度一样，当微分的参考坐标系为世界坐标系U时，可用下列符号表示刚体的速度，即: UAAORGUAAvV00()( )lim()( )lim BBQQBBQQtAAAABBBBtVttVtdVVdtttttddtt 1. 线加速度 描述了坐标系 A下的速度矢量，当坐标系A和坐标系B的原点重合时: 因为两个坐标系的原点重合，因此可以将上式改写为: 对 求微分 , 得到 相对于A的加速度： 因为:
2、() ABABAABBBQBBdR QR VR Qdt AABAABQBQBBVR VR QBQAQV()() AABAABAABQBQBBBBddVR VR QR Qdtdt() ABABAABBQBQBBQdR VR VR VdtBQ 所以有 当两个坐标系原点不重合时: ()()()2() AABAABAABQBQBBBBABAABAABAABAABBQBBQBBBBQBBABAABAABAAABBQBBQBBBBBddVR VR QR QdtdtR VR VR QR VR QR VR VR QR Q2() AAABAABQBORGBQBBQAABAAABBBBBBVVR VR VR QR
3、 Q 当 是常数， 加速度的推导公式化简为: 对于旋转关节的操作臂，上式为操作臂连杆的线加速度. 对于移动关节，常用 (*)式.BQ0BBQQVV() AAAABAAABQBORGBBBBBVVR QR Q 2. 角加速度 假设 B相对于A以 转动，同时 C 相对于 B 以 转动.求 在A中进行矢量叠加: 求导， 得到: 因为有 于是得到操作臂连杆的角加速度.ABAAABCBBCR AAABAABCBBCBBCRR BCAC()AAABCBBCdRdt ()ABABAABBCBCBBCdRRRdt 惯性张量可以在任意坐标系中定义，但一般在固连在刚体上的坐标系中定义惯性张量. 坐标系 A 中的惯
4、性张量可用 33 矩阵表示: 矩阵中的各元素如下:xxxyxzAxyyyyzxzyzzzIIIIIIIIII 222222()()()xxVyyVzzVIyzdvIxzdvIxydvxyVxzVyzVIxy dvIxz dvIyz dv 式子中刚体由单元体 组成 单元体的密度为 . 每个单元体的位置由矢量 确定. -The elements are called the mass moments of inertia. We are integrating the mass elements, , times the squares of the perpendicular distances
5、 from the corresponding axis. -The elements with mixed indices are called the mass products of inertia.TAPx y zdvdv,xxyyzzIII 例: 求图中坐标系中长方体的惯量张量. 已知长方体密度均匀，其大小 解：计算 : Permuting the terms:22220 0 00 02332220()()()()()3333h l wh lxxhIyzdxdydzyzw dydzlhl wh lwmz l w dzlh2222()3()3yyzzmIwhmIlwdvdxdydzxx
6、I 图示物体的惯性张量阵: 惯性张量是坐标系位姿的函数. 222222()344()434()443AmmmlhwlhwmmmIwlwhhlmmmhwhllw20 0 00 02 20244h l wh lxyhwIxy dxdydzy dydzw lmdzwl44xzyzmIhwmIhl 平行移轴定理 : 矢量 表示刚体质心在坐标系A中的位置。矢量矩阵形式：22()ACzzzzccACxyxyccIIm xyIImx y3ACTTccccIIm P P IP P,TccccPx y z 例: 当坐标系原点在刚体质心时，求图中刚体的惯性张量. 运动平行移轴定理: Next, we find:
7、22(),012CCzzxymIwlI12cccxwylhz 其它参量由对称性得出： 222222()00120()01200()12CmlhmIwhmlw 惯性张量的其他性质: -如果由坐标系的两个坐标轴构成的平面为刚体质量分布的对称平面，则正交于这个对称平面的坐标轴与另一个坐标轴的惯量积为0. -惯量距永远是正值，而惯量积可能正，可能负. -三个惯量距的和保持不变. -惯性张量的特征值为刚体的主惯量距，相应的特征矢量为主轴。. 大多数操作臂连杆的几何形状及结构比较复杂，一般使用测量装置来测量。 大多数操作臂连杆的几何形状及结构比较复杂，一般使用测量装置来测量。 承物台承物台遮光细棒遮光细棒
8、塔轮塔轮光电门光电门滑滑轮轮砝砝码码xJJJ0刚体的转动惯量的测量刚体的转动惯量的测量转动体系由承物台和塔轮组成，空承物台转动转动体系由承物台和塔轮组成，空承物台转动时，体系对转轴的转动惯量为时，体系对转轴的转动惯量为J0，另有待测物，另有待测物放在承物台上时，总转动惯量为：放在承物台上时，总转动惯量为：若分别测出若分别测出J和和J0，则待测物体的转动惯量，则待测物体的转动惯量Jx为：为：0JJJxJMMT刚体系受刚体系受外力矩外力矩有：绳子的张力作用力矩有：绳子的张力作用力矩MT和摩擦力矩和摩擦力矩M 。由转动定律知：由转动定律知： 为角加速度。为角加速度。即：即：JMmgr可见，测量转动惯
9、量可见，测量转动惯量J的的关键是测量角加速度和关键是测量角加速度和摩擦力矩！摩擦力矩！J 是转动体系的转动惯量，是转动体系的转动惯量， 是角加速度，是角加速度，m 是下落砝码的质量，是下落砝码的质量，r 是绕线轮的半径，是绕线轮的半径，M 是摩擦力矩是摩擦力矩。 1. 牛顿欧拉方程 要使连杆运动，必须对连杆进行加速和减速运动，连杆运动所需的力是关于连杆期望加速度及其质量分布的函数。牛顿方程以及描述旋转运动的欧拉方程描述了力、惯量和加速度之间的关系。. 牛顿方程: 欧拉方程 CFmvCCNII 2. 向外迭代 为了计算作用在连杆上的惯性力，需要计算操作臂每个连杆在某一时刻的角速度、线加速度和角加
10、速度. 可应用迭代方法完成这些计算。首先对连杆1进行计算，接着计算下一个连杆，这样一直向外迭代到连杆n 计算出每个连杆质心的线加速度和角加速度之后，运动牛顿欧拉公式计算出作用在连杆质心上的惯性力和力矩 . 角速度在连杆之间的“传递问题”: 连杆之间的角加速度变换方程: 当第 i+1个关节是移动关节, 上式简化为: 1111111111iiiiiiiiiiiiiiiiRRZZ111iiiiiiR111111iiiiiiiiiRZAAABAABCBBCBBCRR 每个连杆坐标系原点的线加速度: 对于i+1是移动关节时:11111()iiiiiiiiiiiiiiiivRPPv() AAAABAAAB
11、QBORGBBBBBVVR QR Q1111111111111() 2iiiiiiiiiiiiiiiiiiiiiiiivRPPvdZdZ2() AAABAABAABAAABQBORGBQBBQBBBBBVVR VR VR QR Q 每个连杆质心的线加速度: 假设坐标系 Ci固连于连杆i上,坐标系原点位于连杆质心，且各坐标轴方位与原连杆坐标系i方位相同。 注意，第1个连杆的方程非常简单，因为 .00000()iiiiiiiiiiCiCiiCivPPv 计算出每个连杆质心的线加速度和角加速度之后，运动牛顿欧拉公式计算出作用在连杆质心上的惯性力和力矩 : iiiiCCCiiiiFmvNII 3. 向
12、内迭代法 列出力平衡和力矩平衡方程. 每个连杆都受到相邻连杆的作用力和力矩以及附加的惯性力和力矩. 计算出每个连杆上的力和力矩之后，计算关节力矩. 将所有作用在连杆i上的力相加，得到力平衡方程: 将所有作用在质心上的力矩相加，并且令它们的和为零，得到力平衡方程: 最后重新排列力和力矩方程，形成相邻连杆从高序号向低序号排列的迭代关系：111iiiiiiiifRfF1111111iiiiiiiiiiiiiiCiiiinNRnPFPRf111iiiiiiiiFfRf111()()iiiiiiiiiiiiiCiiCiNnnPfPPf 在静力学中，可通过计算一个连杆施加于相邻连杆的力矩在 方向的分量求得
13、关节力矩: 注意对一个在自由空间中运动的机器人来说, 和 等于零. ZiT iiT iiiiiiinZorfZ11NNf11NNn 牛顿-欧拉迭代动力学算法 由关节运动计算关节力矩的完整算法由两部分组成: -对每个连杆应用牛顿-欧拉方程，从连杆1到连杆n向外迭代计算连杆的速度和加速度. -从连杆n到连杆1向内迭代计算连杆间的相互作用力和力矩以及关节驱动力矩. 对于转动关节，该算法归纳如下: -外推: i: 0 5111111111111111111111111111111111111()()iiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiC
14、iCiiCiiRZRRZZvRPPvvPPvF1111111111111111iiiiiiCCCiiiiiiiiiimvNII -内推: i: 6 1考虑重力：令 1111111111iiiiiiiiiiiiiiiiiiiiiiCiiiiiT iiiifRfFnNRnPFPRfnZ00vGmg 例: 计算二连杆操作臂的动力学方程. 假设质量分布非常简单：每个连杆的质量都集中在连杆的末端，设其质量分别为 和 . 首先，确定牛顿欧拉迭代公式中各参量的值: 12111222,ccpl xpl x000vgY330,0fn000,012120,0ccII2m1m22122200001csRsc1111
15、11111111000,0ZZ111111 0011 000 100gscsvscggc 旋转矩阵: 对连杆 1 向外迭代:11011100001csRsc2310001 000 1R2111111111111111000000 00 0000cgslgsllvgcgcl 2111 11111111111 1110,000cm gsm lFm vm gcm lN 000vgY 对连杆 2 向外迭代:22221212000,022111121112221111211121222 022 021200100gslgslclscsvscgclgclslc 2212112112212222212121
16、121120()()0000cgslclslvlgclslc222122 1122 11 22 21222222122 1122 1122 2122()0(),000m gsm lcm lsm lFm gcm lsm lcm lN 对连杆 2 向内迭代: 对连杆 1 向内迭代:222222222 1 2212 1 2212 2122 2120,0()fFnm l l cm l l sm l gcm l112222 1 2212 2122 1 2212 2121 111 11222 112 1 22122 1 22122 12122 12 120000()00()()nm l l cm lm l
17、 l sm l gcm lm l gcm lm l l cm l l sm l gc cm l gs s2222 1 212 1212 2122121 111122121222 1212 1 212 2122121 1111()00()00 100m l sm l cm lm gsm lm gscsfscm l cm l sm lm gcm lm gc 取 中的 方向分量, 得关节力矩: 将驱动力矩表示为关于关节位置、速度和加速度的函数.Z2212 2122 1 2212121122 1 2222 1 22122 21212112222 1 2212 1 2212 2122 212()(2)(
18、)2()()m lm l l cmm lm l l sm l l sm l gcmm l gcm l l cm l l sm l gcm l iin 1. 迭代形式与封闭形式的动力学方程 迭代形式的动力学方程有两个作用: -进行数值计算. -作为一种分析方法用于符号方程的推导. 我们经常需要对方称的结构进行研究。 2. 状态空间方程 当用牛顿-欧拉方程对操作臂进行分析时，动力学方程可以写成如下形式:这里 是 nn 操作臂的质量矩阵, 是n1的离心力和哥氏力矢量， 是n1重力矢量. ( )( , )( )MVG ( )M ( ,)V ( )G 包含了所有与关节速度有关的项. 例: 2212 21
19、22 1 2212121122 1 2222 1 22122 2121211()(2)()2()m lm l l cmm lm l l sm l l sm l gcmm l gc 2222 1 2212 1 2212 2122 212()m l l cm l l sm l gcm l2222 22 1 221212 22 1 22222 22 1 222 22()2( )2m lm l l cmm lm lm l l cMm lm l l cm l 22 1 2222 1 221222 1 2212( ,)m l l sm l l sVm l l s 2 21212112 212()( )m
20、l gcmm l gcGm l gc ( ,)V : n1 Coriolis 项. 包含了所有与关节速度有关的项 是与离心力有关的项，因为它是速度的平方. 是与哥氏力有关的项，它总是包含两个不同关节速度的乘积. : n1 与重力加速度有关的项, 只与 有关，与它的导数无关 : nn 质量矩阵, 的函数.22 1 222m l l s2 1 22122m l l s ( ,)V ( )G ( )M 3. 位形空间方程 将速度项写成另一种形式: -nn(n-1)/2 哥氏力系数矩阵. -nn 离心力系数矩阵. 动力学方程随着操作臂的运动不断更新.2( )( )( )( )MBCG ( )C 121
21、31 ,Tnn222212Tn( )B 2 1 222 1 222 1 2202( ),( )00m l l sm l l sBCm l l s 4. Inclusion of nonrigid body effects It is important to realize that the dynamic equations we have derived do not encompass all the effects acting on a manipulator. They include only these forces which arise from rigid body me
22、chanism. The important source of forces that are not included is friction. The forces due to friction can actually be quite largeperhaps equaling 25% of the torque required to move the manipulator in typical situations. Viscous friction: is proportional to the velocity of joint motion: Coulomb frict
23、ion: is constant except for a sign dependence on the joint velocity:sgn( )frictioncfrictionv A reasonable model is to include both: Friction also diaplays a dependence on the joint position. A major cause of this effect might be gears that are not perfectly roundtheir eccentricity would cause fricti
24、on. So a fairly complex friction model: So the more complete manipulators model: We dont consider bending effects (which give rise to resonances), it is extremely difficult to model. ( , )frictionf ( )( ,)( )( ,)MVGF sgn( )frictioncv ()iiiLLFtxxLKPL L是拉格朗日函数，是拉格朗日函数，K K是系统动能，是系统动能，P P是系统势能是系统势能()iii
25、LLTt 操作臂的动能表达式: 整个操作臂的动能是各个连杆动能之和: 操作臂的动能可以描述为关节位置和速度的标量函数： 操作臂的质量矩阵一定是正定矩阵1122iiiCTiTiiiCCiiikm vvI1niikk1( ,)( )2TkM The potential energy of the ith link can be expressed as: where is the 31 gravity vector, is the vector locating the center of mass of the link, and is a constant chosen so that the
26、 minimum value is zero. The total potential energy stored in the manipulator is the sum of the potential energy in the individual links: We see that the potential energy of a manipulator can be described by a scalar formula as a function of joint position.00iiTiiCrefumgPu irefu0g0iCP1niiuu The Lagra
27、ngian dynamic formulation provides a means of deriving the equations of motion from a scalar function called the Lagrangian: The equations of the motion for the manipulator are then given by: where is the n1 vector of actuator torques. In the case of a manipulator, this equation becomes: dLLdt( ,)(
28、,)( )Lku dkkudt Example: The links of an RP manipulator have inertia tensors: and total mass and . The center of mass of link1 is located at a distance from the joint-1 axis, and the center of mass of link2 is at the variable distance from the joint-1 axis. Use Lagrangian dynamics to determine the e
29、quation of the motion for this manipulator. 2d11111000000 xxCyyzzIIII1l2m1m22222000000 xxCyyzzIIII Write the kinemic energy of link 1 as: the kinetic energy of link 2 as: Hence, the totle kinetic energy is given by: Write the potential energy of link 1 as: 11111222111111 111111112222CTTCCzzkm vvIm l
30、I222222222222222212211111()2222CTTCCzzkm vvIm ddI22221 1122212211( ,)()22zzzzkm lIIm dm d 100111 111 1siniTCrefum gPum l gm l g Potential energy of link 2 as: Total potential energy is given by: Then2002222122maxsiniTCrefumgPum d gm dg 1 12211 122max( )()sinug m lm dm l gm dg 221 11222122()zzzzm lII
31、m dkm d 22210km d1 122121()cossing m lm dugm Finally:22211 11222122121 1221222222121()2() cossinzzzzm lIIm dm ddm lm dgm dm dm g221 1122220( )0zzzzm lIIm dMm 221222212( ,)m ddVm d 1 122121() cos( )sinm lm dgGm g We developed dynamic equations in joint space because we could use the serial-link natur
32、e of the mechanism to advantage in deriving the equations. In this section, we discuss the formulation of the dynamic equations that relate acceleration of the end-effector expressed in Cartesian space to Cartesian forces and moments acting at the end-effector. 1. 笛卡尔状态空间方程 应用笛卡尔变量的一般形式建立操作臂的动力学方程:
33、-F 作用于机器人末端的力和力矩矢量 - 能够恰当表达末端执行器位姿的笛卡尔矢量. - 笛卡尔质量矩阵. - 笛卡尔空间的速度项矢量. - 笛卡尔空间的重力项矢量. ( )( , )( )FMVG( )M ( , )V( )G F用关节驱动力表示: 11JJJJJ J( )TJF ( )( , )( )( )( , )( )TTTTTTTJJMJVJGFJMJVJG Substituting: The expressions for the terms in Cartesian dynamics: Note that, the Jacobian is written in the same f
34、rames as , the choice of this frame is arbitrary. When the manipulator approaches a singularity, certain quantities in the Cartesian dynamics become infinite. 11( )( )( , )( )TTTTFJMJJMJ JJVJG 11( )( )( )( )( ,)( )( ( ,)( )( )( )( ) ( )TTTMJMJVJVMJJGJG,F 例子： 两连杆平面机械臂的笛卡尔空间形式的动力学方程. 2222 22 1 221212
35、22 1 22222 22 1 222 22()2( )2m lm l l cmm lm lm l l cMm lm l l cm l 22 1 2222 1 221222 1 2212( ,)m l l sm l l sVm l l s 2 21212112 212()( )m l gcmm l gcGm l gc 12211222122121 22001( ),( )l slJJl clll cll sl l s We can get: When , the manipulator is in a singular position. For example, when , the eff
36、ective Cartesian mass of the end-effector becomes infinite in the direction of the link-2 tip frame. In general, at a singular configuration there is a certain direction, the singular direction in which motion is impossible, but general motion in the subspace “orthgonal” to this direction is possibl
37、e. 2X2020s 122220( )0mmsMm 2222 122 212 222 22 121 1122222 1212 1212()(2)( ,)cm l cm lm lm lm l cm lsVm l sm l s 112122212( )cm gm gssGm gc 2. 笛卡尔位形空间的力矩方程 利用笛卡尔空间动力学方程写出等价的关节力矩: 改写为: where is a matrix of Coriolis coefficients. is a matrix of centrifugal coefficients. Note that, is the same as in the joint-space equation, but in general:( )B( )C ( )( )( )( )BBandCC ( )( )( ,)( )TJMVG( )G 2( )( )( )( )( )TJMBCG用拉格朗日法推导两自由度机器人手臂的运动方程，连杆的质心位于连杆的中心，其质量分别为 和 ，转动惯量分别为 和 . 2m1m1I2I
