第3章机器人动力学

1、机器人引论第3章 机器人动力学第3章 机器人动力学o 3.1 动力学分析基础o 3.2 机器人的静力分析o 3.3 机器人动力学方程3.1 动力学分析基础动力学分析基础3.1.1 机器人的坐标系机器人的坐标系图3-1 机器人的坐标系3.1.2 工具的定位工具的定位1SBBWTSWTTTT T(3-1)方程（3-1）在某些机器人系统中称为WHERE函数，用它可计算手臂的位置。对于图3-1中情况，WHERE的输出是轴销相对于工作台顶角处的位姿。3.1.3 惯性张量和惯性矩阵惯性张量和惯性矩阵绕轴x、y和z的质量惯性矩分别为222222222222()()()()()()xxvmyyvmzzvmIy

2、zdvyz dmIzxdvzx dmIxydvxy dm混合矩(称为惯性积)：xyvmyzvmzxvmIxydvxydmIyzdvyzdmIzxdvzxdmxxxyxzAxyyyyzxzyzzzIIIIIIIII I3.1.4 连杆运动的传递连杆运动的传递0AAABBBPPRP0AAAABABPBBPBBvvR vSR P 02AAAAAABABABAABPBBPBPBBBBBBvvR vSR vSR PSSR P加速度：速度：简化为：0()AAAABPBBBvvSR P()() ()AAAABAAABPBOBBBBBvvSR PSSR P0AABB AAABPBOBPvvR v简化为：AAA

3、BPBOBPvvR v()AABAABPBPBBvR vSR P2()()AAAABABAABAABPBPBPBBBPBBvR vSR vSR PSSR PAAABCBBCR AAAABABCBBCBCBR SR 微分得：下面利用Denavit-Hartenberg的连杆参数表示方法，依次递推出机器人操作臂或者步行机器人运动腿各连杆的速度和加速度。相邻两连杆速度的传递o 1 旋转关节的速度传递旋转关节的速度传递o 2 移动关节的速度传递移动关节的速度传递11111iiiiiiiiiRZ111111iiiiiiiiiR Z11iiiiiiiivvP1111iiiiiiiiiivRvP111iii

4、iiiR 1111111iiiiiiiiiiiiivRvPdZo 3 旋转关节的加速度传递旋转关节的加速度传递o 4 移动关节加速度的传递移动关节加速度的传递1111111111iiiiiiiiiiiiiiiiR R ZZ11111iiiiiiiiiiiiiiiivRvPP111iiiiiiR 11111111111112iiiiiiiiiiiiiiiiiiiiiiiivRvPPdZdZo 5 质心的加速度质心的加速度iiiiciiicivvPiiiiiiiciiiciiiciPP3.1.5 牛顿牛顿欧拉动力学方程欧拉动力学方程o 刚体的运动可以分解为刚体质心的移动和刚体绕质心的转动。应用牛顿

5、-欧拉方程来建立机器人机构的动力学方程，是指相对质心的移动用牛顿方程，相对于质心的转动用欧拉方程。o 在移动和转动的刚体S上任选固定在刚体上的一点O，将基准坐标系 的原点移至点O上成为随行坐标系 ，随行坐标系 随S移动，但不随S转动，以便观察S相对坐标系 的转动运动。动量矩()()()()()()AxAzAyAyAxAzAzAyAxHymvzzmvyHzmvxxmvzHxmvyymvx根据动量矩定理推出：()()gdmdt NI I v（I为刚体的惯性张量）() NI I 0yzzxxyIII()()mmgdmdtNIIv000000 xxmyyzzIIII()mmNII3.1.6 拉格朗日方

6、程拉格朗日方程()()( )KPLEEq,qq,qq1122TiT ciikiiciciiiiEmIv v1nkkiiEE 1( , )( )2TkEDq qqq q00TpiiciEm gp1nPPiiEE dLLdtqqKKPEEEddtqqq3.2 机器人的静力分析机器人的静力分析3.2.1 等效关节力和力雅可比等效关节力和力雅可比fnF称为终端广义力矢量将各个关节驱动力(或力矩)组成的n维矢量，称为关节力(矩)矢量。12.Tn若将关节力(矩)矢量看成是驱动装置的输入，在末端产生的广义力作为输出，可以建立两者之间的关系。 各关节所作的虚功之和为：1122.nnwqqq T q末端操作器所

7、作的虚功为：xyzxxyyzzwf dxf dyf dznnnTF D关节空间虚位移产生的虚功等于操作空间虚位移产生的虚功：TT q = F DD = J(q)q整理有：( )TJq F若不考虑关节之间的摩擦力，在外力F的作用下，操作臂或者运动腿保持平衡的条件是关节驱动力矩满足上式 v = xJ(q)q 我们将雅可比矩阵写成如下型式：111212122211.nnm nmmmnJJJJJJJJJJ11mm nnx= Jq (3-68)TTJ x = J Jq 1TTq = J JJ x3.2.2 连杆的静力学分析连杆的静力学分析连杆的静力平衡当连杆处于平衡状态时，力的平衡方程：10iiiiii

8、mffg力矩平衡方程：1110iiiiiiiiiiciimnnPfrg对于旋转关节i，若不考虑关节中的摩擦，则除了绕轴的扭矩之外，其余各方向的力和力矩分量都由机构构件承受。为了保持连杆平衡，关节驱动力矩平衡力矩的Z向分量应该等于 ：iTiiiinziTiiiifZ3.3 机器人动力学方程机器人动力学方程3.3.1牛顿牛顿欧拉递推动力学方程欧拉递推动力学方程将机器人的连杆看成刚体，其质心加速度、总质量、角速度、角加速度、惯性张量与作用力矩满足如下关系： 牛顿第二定律 （力平衡方程）/ciiciicid mdtmfvv 欧拉方程 （力矩平衡方程）/cccciiiiiddtiiinI I I o 1

9、 力和力矩的递推算式力和力矩的递推算式连杆i在运动情况下，作用在上面的合力为零，得力平衡方程式（暂时不考虑重力）：111iiiiciiiiffRf作用在连杆i上的合力矩等于零，得力矩平衡方程式：1111111iiiiiiiiiciiiiciciiiinnRnrfPRf将上式写成从末端连杆向内迭代的形式：111iiiiiiicifRff1111111iiiiiiiiiiiiciciciiiinRnnrfPRf利用这些公式可以从末端连杆n开始，顺次向内递推直至到操作臂的基座。对于旋转关节，各关节上所需的扭矩等于连杆作用在它相邻连杆的力矩的Z轴分量iT iiiinZiT iiiifZo 2 递推的牛

10、顿递推的牛顿欧拉动力学算法欧拉动力学算法综上所述，将递推计算过程的相关公式归纳如下：11111111(1)iiiiiiiiiiiiiiiR ZR 对于转动关节对于移动关节1111111111iiiiiiiiiiiiiiiiiiiR R ZZR （转动关节）（移动关节）11111111111111112iiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiRvPPvRvPPdZdZ（转动关节）（移动关节）11111111111111iiiiiiiciiiciiicivvrr11111iiciicimf 111111111111iciiiciiciiiiiinIIo 3 计及

11、重力的动力学算法计及重力的动力学算法111iiiiiiicifR ff1111111iiiiiiiiiiiiciciciiiinR nnrfPR f()iT iiiiiT iiinZfZ转动关节（移动关节）o 4 封闭形式动力学方程的建立封闭形式动力学方程的建立以2R机械手为例说明建立封闭形式的动力学方程的一般方法和步骤。2R平面机械手的质量分布22212 2122 1 221212112 1 222()(2)()m lm l l cmm lm l l s2 1 22122 21212112()m l l sm l gcmm l gc 2222 1 22 12 1 2 212 2122 212

12、()m ll cm ll sm l gcm l3.3.2 关节空间与操作空间动力学关节空间与操作空间动力学o 1 关节空间的状态方程关节空间的状态方程12T12Tq则( )( ,)( )q qqqqDhG222221 22 2112221 22 222221 22 2222()( )l mll m clmml mll m cql mll m cl mD(3-99)22 1 2 222 1 2 21222 1 2 212( , )m l l sm l l sq qm l l sh 2 21212112 212()( )m l gcmm l gcqm l gcGo 2 形位空间方程形位空间方程将（

13、3-99）式中与速度有关的项分成两部分：2( , )( )( )q qq qqq qhBc 因而动力学方程可以写成另一种形式：2( )( )( )( )q qq q qq qqDBCG o 3 操作空间动力学方程操作空间动力学方程两个空间中的位移关系：两个空间中的速度关系：XX(q)X = J(q)q两个空间中的加速度关系：rX = J(q)q+a (q,q)式中ra (q, q) =J(q)q在操作空间中，动力学方程式可以写成：F = V(q)X+U(q, q)+P(q)T = J (q)F关节空间动力学方程与操作空间动力学方程具有以下关系：TTTrTD(q) = J (q)V(q)J(q)

14、h(q, q) = J (q)U(q, q)+J (q)V(q)a (q, q)G(q) = J (q)P(q)(3-108)-T-1V(q) = J (q)D(q)J (q)-TrU(q,q) = J (q)h(q, q)-V(q)a (q,q)-TP(q) = J (q)G(q)o 4 操作运动操作运动关节力矩方程关节力矩方程操作运动关节力矩之间的动力学方程：T = J (q)(V(q)X+U(q, q)+P(q)可以改写成如下的形式：T2xx = J (q)V(q)X+B (q)q q+c (q) q+G(q)3.3.3 拉格朗日方程的应用拉格朗日方程的应用以RP机械手为例，说明采用拉格

15、朗日方程建立机器人动力学方程的方法。221 1221 122cos()m rm rm r rgm rm r2222sinrFm rm rm g将动力学方程写成更加一般的形式(将关节称关节l，关节r称关节2)1 11 2221 1 11 2 21 1 21 2 112 12 2222 1 12 2 22 1 22 2 12()()rDDrDDrDrDrDDDrDDrDrDrD惯性力项 （向心力项） （哥氏力项） （重力项）（7 - 6 ）F惯性力项 （向心力项） （哥氏力项） （重力项）（7 - 7 ）1 11 2221 1 11 2 21 1 21 2 112 12 2222 1 12 2 2

16、2 1 22 2 12()()rDDrDDrDrDrDDDrDDrDrDrD惯性力项 （向心力项） （哥氏力项） （重力项）（7 - 6 ）F惯性力项 （向心力项） （哥氏力项） （重力项）（7 - 7 ）221 11121 21 1 11 1 21 1 221 2 121112000c o s()DmrmrDDDDmrDmrDgmrmr2 12 222 1 122 2 22 1 22 2 1220000s i nDDmDmrDDDDmg3.3.4 多足步行机器人的动力学模型多足步行机器人的动力学模型lnWWZZ 其中12TllTlTlTlWWWW12TnnTnTnTlWWWW12diag()nZZZZ1diag()n ii000 000zyzxyx 3 3iiimI0Z0Jo 1 完整约束方程完整约束方程o 2 关节空间运动变换关节空间运动变换系统运动学约束关系为： 0 方程的秩为：16liirlf以拉格朗日乘子矢量函数形式的广义约束力矢量为：ltW T TI I