第6章 机器人动力学

2020-03-01 180浏览

  • 1.第6章 机器人动力学 机器人学简明教程 第6章 机器人动力学 6.1 刚体定轴转动与惯性矩 6.2 刚体的惯性张量 6.3 刚体的牛顿欧拉方程 6.4 拉格朗日方程 6.5 双足机器人动力学
  • 2.第6章 机器人动力学 6.1 刚体定轴转动与惯性矩 在物理学中，刚体定轴转动微分方程: I    (6－1) 式中，I称为绕固定轴的惯性矩（也称为转动惯量），τ是作 用在固定轴上的合外力矩。对于一个质量为m的质点，其在 直线上运动的动力学问题可以用牛顿第二定律描述: mv  f 或 mx   f （6－2） 比较式（6－1）和式（6－2）可以发现，刚体定轴转动和质 点直线运动的动力学方程的形式是完全相同的。因此，I可以 看成刚体定轴转动的惯性质量。
  • 3.第6章 机器人动力学 下面以图6－1所示质量为M，半径为r的均匀圆盘绕过圆 心的Z轴的惯性矩计算问题给出惯性矩的定义: I   r 2dm V (6－3) 式（6－3）给出了任意刚体绕固定轴惯性矩的定义，其中 dm是微元体质量，r是微元体到转轴的距离，V是刚体的体 积，因此式（6－3）表示在整个体积上积分。
  • 4.第6章 机器人动力学 图6－1 圆盘绕过圆心轴惯性矩
  • 5.第6章 机器人动力学 对于图6－1所示均匀圆盘，面密度ρ=M/(πR2)，取极坐标微 元体，则 I   r 2 dm   V R 0  2 0 4 4 R M R 1 2 2 r  rdrd   2  2   MR 4  R2 4 2 (6－4) 例6－1 如图6－2所示匀质杆，质量为M，杆长为L，计 算绕质心的惯性矩。 图6－2 匀质杆绕质心惯性矩
  • 6.第6章 机器人动力学 解:匀质杆的线密度ρ=M/L，取微元体dx，则 I  L/2 L/2 x 2 dm  2  L/2 0 3 ( L / 2) x 2  dx  2  3 M L3 1 2   ML2 L 3  8 12 平行移轴定理:刚体绕任意平行于质心轴的惯性矩为 C I  I  Md 2 (6－5) 式中，CI表示刚体绕质心轴的惯性矩，M为刚体质量，d为两 轴之间的距离。若已知刚体绕质心轴的惯性矩，则刚体绕任 意平行轴的惯性矩可以非常方便地利用平行移轴定理式（6－ 5）进行计算。
  • 7.第6章 机器人动力学 例如，计算图6－2所示匀质杆绕杆端点的惯性矩，根据平行 移轴定理 1 L 2 1 2 2 I  I  Md  ML  M ( )  ML 12 2 3 C 2 (6－6) 可以验证，式（6－6）计算结果与采用积分方法相同。
  • 8.第6章 机器人动力学 6.2 刚体的惯性张量 对于在三维空间自由运动的刚体，存在无穷多个可能转 轴，计算绕所有转轴的惯性矩显然是不现实的。因此需要考 虑这样的问题:是否存在一个量，它能够表示刚体绕任意转 轴的惯性矩？答案是肯定的，该量就是刚体的惯性张量。惯 性张量描述了刚体的三维质量分布，若在某坐标系下表示出 来，它就是一个3阶对称矩阵。图6－3所示的一个刚体，其 上定义了固连的坐标系{A}。
  • 9.第6章 机器人动力学 图6－3 空间刚体的惯性张量
  • 10.第6章 机器人动力学 在坐标系{A}中惯性张量为  I xx  A I    I xy   I xz   I xy I yy  I yz  I xz    I yz  I zz  (6－7) 惯性张量是一个对称矩阵，各元素的值为 I xx    y 2  z 2  dv V I xy   xy dv V I xz   xz dv V I yz   yz dv I yy    x2  z 2  dv I zz    x2  y 2  dv V V V (6－8)
  • 11.第6章 机器人动力学 惯性张量中Ixx、Iyy和Izz称为惯性矩，交叉项Ixy、Ixz和Iyz 称为惯性积。显然，惯性张量中元素的数值与坐标系的选择 有关，一般存在某个坐标系，使得交叉项全为0，该坐标系 称为惯性主轴坐标系，坐标轴称为惯性主轴。对于质量均匀 分布的规则物体，惯性主轴就是物体的对称轴。
  • 12.第6章 机器人动力学 例6－2 如图6－4所示质量均匀分布的长方形刚体，密 度为ρ，质量为M，计算其惯性张量。 图6－4 质量均匀分布的长方形刚体
  • 13.第6章 机器人动力学 解:单元体dv=dx dy dz，根据式（6－8）得: I xx    y  z  dv    2 2 V  W  H 0  y L 0 2 z 2    y  z  dx dy dz  dy dz W  L / 3  Lz dz H L W 0 0 0 2 2 H 3 2 0  HL3 LH 3  M 2 2  W    ( L  H )  3  3  3 同理可以得到另外两个惯性矩 M M 2 2 I yy  (W  H ), I zz  (W 2  L2 ) 3 3
  • 14.第6章 机器人动力学 下面计算惯性积 I xy   xy dv    0 V  W 2 L2 H  H dz  L   0 W 0 xydxdydz    W 2 L2 H H 0  L 0 yW 2 / 2dydz M  WL 4 4 4 同理可以得到另外两个惯性积 M M I xz  WH , I yz  HL 4 4 至此，我们已经计算出了式（6－7）中的所有6个分量的值。 0 对于惯性张量的计算问题，平行移轴定理也是成立的，下面 给出其中两个表达式，其余的4个表达式与此类似: A A I zz  C I zz  M ( xc2  yc2 ) C I xy  I xy  M ( xc yc ) (6－9)
  • 15.第6章 机器人动力学 例6－3 如图6－4所示，求质量均匀分布的长方形刚体 在质心坐标系（原点位于质心，坐标系姿态与原坐标系姿态 相同）下表示的惯性张量。 解:根据平行移轴定理式（6－9）计算，其中 因此得  xc  W  y   1  L   c 2    zc   H  M 2 L W M 2 ( L  W 2 )  M [( ) 2  ( ) 2 ]  (L  W 2 ) 3 2 2 12 M L W C I xy  A I xy  M ( xc yc )  WL  M (  )  0 4 2 2 C I zz  A I zz  M ( xc2  yc2 ) 
  • 16.第6章 机器人动力学 其他4个值可以采用类似的方法获得。在质心坐标系{C} 下，刚体的惯性张量为  H 2  L2 0 M C 2 2 I 0 W  H 12   0 0  0   0  L2  W 2  (6－10) 结果是对角矩阵，此时坐标系{C}的坐标轴是刚体的惯性主轴。
  • 17.第6章 机器人动力学 6.3 刚体的牛顿－欧拉方程 下面介绍描述单个刚体动力学的牛顿－欧拉方程。如图 6－5所示，机械臂的一个连杆的动力学问题可以简化为单刚 体动力学问题。在动力学分析过程中，把刚体的运动分解为 质心的平移运动和绕质心的转动。一般将连体坐标系的原点 固定在刚体的质心，这样坐标原点的运动描述刚体的平移运 动，坐标系的转动描述刚体绕质心的旋转运动。
  • 18.第6章 机器人动力学 图6－5 单刚体动力学
  • 19.第6章 机器人动力学 刚体质心的平动用牛顿第二定律描述 F  M vc (6－11) . 式中，M表示刚体质量;F表示作用在刚体上的合外力矢量；vc 表示质心速度矢量。 N  CI ω  ω  CI ω (6－12) 式中，CI表示刚体在质心坐标系{C}下表示的惯性张量；N表示 . 作用在刚体上的合外力矩矢量；ω表示角速度矢量；ω表示角 加速度矢量。 式（6－11）和式（6－12）一起称为刚体的牛顿欧拉方 程。分析机械臂的动力学问题时，首先对每个连杆列出牛顿 欧拉方程，同时需要分析连杆间的速度、加速度传递关系以及 力的传递关系。上述分析过程比较复杂，有兴趣的读者可以参 考文献［1］。
  • 20.第6章 机器人动力学 6.4 拉格朗日方程 上节介绍牛顿欧拉方程是采用几何矢量方法建立每个 连杆的动力学方程，方程中会出现约束力项。拉格朗日方程 采用解析方法建立系统的动力学方程，在理想约束条件下， 动力学方程中不出现约束力项。 定义拉格朗日函数 L=K-P (6－13) 式中，K是系统动能；P是系统势能。 系统动力学方程为 d L L Fi  ( )  , i  1, 2, , n dt qi qi (6－14)
  • 21.第6章 机器人动力学 式中，qi是描述系统位置的坐标，称为广义坐标；Fi是作用 在qi上的广义力。分量形式的方程式(6－14)也可以写成矢量 形式  L  L d F    dt  q  q (6－15) 式（6－14）或式（6－15）称为第二类拉格朗日方程。
  • 22.第6章 机器人动力学 例6－4 如图6－6所示单摆由一根无质量杆末端连接一 集中质量m，杆长为l，其上作用力矩τ，试建立系统的动力 学方程。 图6－6 单摆
  • 23.第6章 机器人动力学 解:(1)牛顿欧拉方法。单摆运动可以简化为刚体的定 轴转动，其动力学方程为 I  N 转动惯量和合外力矩计算如下 I  ml 2 N    mgl sin  因此，系统的动力学为 ml 2  mgl sin   
  • 24.第6章 机器人动力学 (2)拉格朗日方程。选择θ为描述单摆位置的广义坐标， 先用广义坐标表示集中质量的位置，然后再对时间求导得到 速度 x  l sin  , y  l cos  x  l cos  , y  l sin  系统的动能为 1 m 2 m 2 2 1 K  mv 2  x  y 2  l  cos 2   l 2 2 sin 2   ml 2 2 2 2 2 2    取坐标原点为势能零点，则系统的势能为 P  mgy  mgl cos  
  • 25.第6章 机器人动力学 系统的拉格朗日函数为 L  K  P  1 ml 2 2  mgl cos  2 根据式(6－14)计算相应的导数 d  L  d 2  2   ml   ml    dt    dt L  mgl sin     代入到拉格朗日方程得系统的动力学方程   m    mg  sin  2
  • 26.第6章 机器人动力学 例6－5 如图6－7所示两连杆平面机械臂。连杆长分别 为L1和L2，连杆质量分别为m1和m2，质心到杆端点距离分别 为Lc1和Lc2，两连杆绕质心转动惯量分别为Ic1和Ic2，两个关 节上作用驱动力矩分别为τ1和τ2，试建立系统的动力学方程。 图6－7 两连杆平面机械臂
  • 27.第6章 机器人动力学 解：非定轴转动刚体的动能表示为质心平移动能和绕质 心转动动能之和 1 2 1 K  mvc  I c 2 2 2 其中，vc是质心速度的大小，ω是刚体的角速度。因此，两连 杆的动能分别为 1 1 2 K1  m1vc1  I c112 , 2 2 1 1 2 K 2  m2 vc 2  I c 222 2 2 选择θ1和θ2为描述连杆位置的广义坐标，先用广义坐标表示 质心的位置， xC1  Lc1c1 , yC1  LC1s1 xC 2  L1c1  LC 2 c12 , yC 2  L1s1  LC 2 s12
  • 28.第6章 机器人动力学 再对时间求导得到质心的速度 xC1   LC1s11 , y C1  LC1c11 xC 2   L1s11  LC 2 s12 1  2 ,   两连杆的转动角速度分别为 1  1 , 2  1  2 因此，两连杆的动能为  y C 2  L1c11  LC 2 c12 1  2 
  • 29.第6章 机器人动力学 1 1 2 1 2 2 K1  m1  xC 1  yC 1   IC 1 1   IC 1  m1 L2C 1 12 2 2 2 2 1 1 2 2   K 2  m2  xC 2  yC 2   IC 2  1  2 2 2 2 1  2 2 1 2        m2 L11  LC 2 1   2  2 L1 LC 2 c21 1   2  IC 2 1  2  2 2  1 1 2 2 2      I C 2  m2  L1  LC 2  2 L1 LC 2 c2   1   I C 2  mL2C 2  22 2 2   I C 2  m2 L2C 2  m2 L1 LC 2 c2 12        系统的总动能可以表示为 1 T 1    M11 K  K1  K 2  q Mq  1  2   2 2  M 21 M12  1     M 22   2   2
  • 30.第6章 式中， 机器人动力学 M11  I C1  I C 2  m1 L2C1  m2  L12  L2C 2  2 L1 LC 2c2  M 21  M12  I C 2  m2 ( L2C 2  L1 LC 2c2 ) M 22  I C 2  mL2C 2 取固定在基座处的坐标原点为势能零点，系统的总势能为 P  m1 gLC1s1  m2 g ( L1s1  LC 2 s12 ) 系统的拉格朗日函数为 LK P
  • 31.第6章 机器人动力学 直接代入到拉格朗日方程式(6－14)，即可得到系统的动力 学方程。当然，导数的计算过程是比较复杂的。下面分析方 程的结构。 d  L  L d  K  P K  q   q   q   q  q dt     dt      d P K   M  q  q    q dt  q 1 T M P   q  Mq  M  q q  q 2  q 拉格朗日方程可以表示为 M (q)q  C (q, q )  G  τ
  • 32.第6章 机器人动力学 式中，M是对称正定质量矩阵；C是离心力和柯氏力项；G 是重力项。C和G如下: 1 T M  C  Mq  q q q ,  2 G  P q 下面计算各量的具体值， K   1 q T M q  0 1 2 1 K  1     2m2 L1 LC 2 s2  2 2  1 2    m2 L1 LC 2 s2   m L L s  2    2 1 C2 2  1 1 2   m2 L1 LC 2 s2  1     0   2 
  • 33.第6章 机器人动力学    2 m L L s  2 1 C 2 2 2 M q     m L L s  2 1 C 2 2 2   m2 L1 LC 2 s22  1    0   2   2       m L L s 2        2 1  1 2 1 C2 2 1 2 2     m2 L1 LC 2 s2 2      1 0   2    m2 L1 LC 2 s212  2      2     1 T M 1 2 2      C  Mq  q q  m2 L1LC 2 s2 q 2  12 P  m1 LC1c1  m2 LC 2 c12  g  G   q   m2 LC 2 c12 g     
  • 34.第6章 机器人动力学 例6－6 如图6－8所示在竖直平面内运动的小车倒立摆 系统，假设小车质量为M，倒立摆连杆长度为L，末端集中 质量为m，重力加速度为g，忽略连杆质量和转动惯量。选 择图中给定的坐标系，用广义坐标(x和θ)表示小车中心和倒 立摆集中质量的位置，试计算系统的动能和势能，并采用拉 格朗日方法建立系统的动力学方程。
  • 35.第6章 机器人动力学 图6－8 小车倒立摆系统
  • 36.第6章 机器人动力学 解:先用广义坐标表示两个集中质量的位置，然后计算 其速度  xM  x   yM  0  xm  x  L sin    ym  L cos  系统的动能为  xM  x   y M  0  xm  x  L cos     y m   L sin  1 1  2 2 2    K  M ( x  y )  Mx M M M  2 2  1 1 2 2 2 2 2    K  m ( x  y )  m ( x  L   2 Lx cos  ) m m m  2 2   K  K M  K m  1 (M  m ) x 2  1 mL22  mLx cos   2 2
  • 37.第6章 机器人动力学 取y=0为势能零点，则P=mgLcosθ，将L=K-P代入拉格朗日方 程得 d  L  L F    dt  q  q ( M  m)  x  mL cos   mL 2 sin   F  2  mL   mL cos   x  mLg sin   0 
  • 38.第6章 机器人动力学 6.5 双足机器人动力学 日本的类人机器人ASIMO和HRP－2P代表了双足机器 人研究的最高水平。机器人一般采用零力矩点 （ZeroMomentPoint,ZMP）方法规划机器人关节的轨迹，然 后通过控制伺服电机使机器人的关节跟踪规划的轨迹。该方 法的主要缺点是计算非常复杂，同时和人类行走相比需要消 耗非常多的能量。
  • 39.第6章 机器人动力学 图6－9所示的被动行走（PassiveDynamicWalking， PDW）机器人不需要外部的能量输入，可以在重力的作用 下稳定地走下小的斜坡。在行走的每一步，机器人从重力势 能的变化中获取能量，脚与地面的冲击作用耗散机器人的能 量。如果初始条件和坡度的组合适当，每一步获取的能量和 耗散的能量恰好平衡，就可以得到稳定的双足被动行走步态。 双足被动行走步态设计通常采用牛顿迭代算法，对于给定的 坡度和机器人模型，确定机器人运动的初始条件。
  • 40.第6章 机器人动力学 图6－9 两连杆双足机器人
  • 41.第6章 机器人动力学 1.腿摆动阶段动力学模型 图6－10给出了双足机器人腿摆动阶段的简图，忽略转 动惯量的影响。支撑腿与铅垂线夹角为θ1，摆动腿与铅垂线 夹角为θ2，假设a=b=l/2，其中l为腿的长度。可以采用和前 面相同的方法建立双足机器人的动力学方程。
  • 42.第6章 机器人动力学 图6－10 腿摆动阶段动力学
  • 43.第6章 机器人动力学 集中质量的位置和速度为       x1   2 s1  x1   2 c1 1    y   c  y    s  1 1  1 2 1  1 2  x h  c11  xh  s1     y h  s11  yh  c1        x2  s1  2 s2  x 2  c1 1  2 c2 2     y  c   c  y  s    s  1 2 1 1 2 2  2  2 2 2
  • 44.第6章 机器人动力学 系统的动能 2  1 1 m  K1  m  x12  x12    2 2 4 2  1  Kh  1 1 mh  xh2  yh2  mh 212 2 2 2   1 1  K 2  m  x 22  y 22   m  212  22   2 cos   1  2 12  2 2  4  1  1   K  K1  K 2  Kh   1  1  M   2 2    5m  2 1   m cos 1  2     mh  4   2   M   1 2  1 2  m   2 m cos 1   2   4
  • 45.第6章 机器人动力学 取腿支撑点为势能零点，系统势能为 mg     3  m  P c1  mh g  c1  mg   c1  c 2    mh  m  g c1  gc2 2 2   2  2  计算对时间导数，  M  0  1   对称 1 2  m sin 1   2   2  0   0   M  对称 1    m 2 sin 1   2   M  0  2    2 0  对称  1 2 m sin 1   2  1  2 2 0      
  • 46.第6章 机器人动力学 1 2  2 m sin 1   2  1  M T C  M q  q q q   2   1 m 2 sin     1 2  2  1 2 2   m  sin       1 2 2   2   1  m 2 sin      2  1 2 1  2  1 2    m  sin       1 2 2 1 2 1 2  2   1 1  2 1    m 2 sin 1   2  12    2           3     mh  m  gs1   P 2   G   q  1   2 mgs2  最后得到腿摆动阶段的动力学方程： M (q)q  C (q, q )  G  0 (6－17)
  • 47.第6章 机器人动力学 2.地面瞬时冲击 假设地面冲击作用是瞬时的，采用下面假设 (1)冲击是完全塑性的； (2)冲击的同时完成支撑腿与摆动腿的转换； (3)冲击时腿与地面之间没有滑动。 根据动量矩守恒可以得到冲击前后的速度关系 Q  ( )θ   Q  ( )θ  (6－18) 式中，α=(θ－1－θ－2)/2；上标“-”和“+”分别代表冲击前和冲 击后的相应量。  ml 2  cos(2 )  2 Q  ( )   2  5ml 2 ml 2 (  m l cos(2 )  H )  4 2  ml 2  4  2 2  ml ml  cos(2 )  4 2  (6－19)
  • 48.第6章 机器人动力学  ml 2   4 Q  ( )   2  ml 2  (m  mH )l cos(2 )   4  0   2 ml    4  (6－20) 冲击结束后，两腿的角色互换，因此有 θ+1=θ－2,θ+2=θ－1。定义摆动腿末端到行走平面的距离 ytip  l[cos(1   )  cos( 2   )] (6－21) 对时间求导可以得到 ytip  l[2 cos( 2   )  1 cos(1   )] (6－22)
  • 49.第6章 机器人动力学 冲击时应该满足以下条件: (1)（θ2>θ1）,摆动腿在支撑腿之前。 (2)（ytip=0）,摆动腿与地面接触。 . (3)（ytip<0）,摆动腿的末端向下运动。
  • 50.第6章 机器人动力学 3.周期步态与极限环 定义状态变量 x  [θ , θ ]T ，考虑没有力矩输入的被动行走， 双足机器人的行走模型由式(6－17)~式(6－20)组成一个混合 系统:  x  f ( x ) swing   x (0)  x0 x   h( x  ) events
  • 51.第6章 机器人动力学 式中，“swing”表示腿摆动阶段；“events”表示腿与地面冲击； “-”和“+”分别代表冲击前和冲击后的状态变量。双足机器人 的周期步态对应系统的周期，解x(t)=x(t+T)中T是步态的周期。 系统式(6－23)的解可以表示为x(t)=φ(x0,t)，如果存在x*和时间T 使得x*=φ(x*,T)，则x(t)=φ(x*,t)是一个周期解。对于二维状态 空间，周期解对应于一个闭环，如果周期解是孤立的，称其为 极限环。若从极限环附近出发的解收敛到极限环，则称极限环 是稳定的。双足机器人周期被动行走步态对应于系统式(6－23) 的稳定极限环。
  • 52.第6章 机器人动力学 搜索极限环的一种方法是确定系统的初始状态x*和周期 T，使得x*=φ(x*,T)。确定周期解的常用方法是庞加莱映射， 具体方法是选择庞加莱截面Γ使系统式(6－23)的解曲线与之 相交。可以得到庞加莱映射（见图6－11） Xi+1=p(xi) (6－24) 极限环对应庞加莱映射的不动点 x i  p ( x i ) 。通过庞加莱映射 技术，极限环搜索转换成解非线性方程组式(6－24)问题。定 义 r ( x i )  p( x i )  x i ，可以使用牛顿（Newton）法求方程组(6 －24)的根 x i 1  x i  [ Dx r ( x i )]1 r ( x i ) (6－25)
  • 53.第6章 机器人动力学 图6－11 庞加莱映射示意图
  • 54.第6章 机器人动力学 图6－12和图6－13给出了斜坡角度￠为1°、3°和5°时， 支撑腿和摆动腿的相平面图。从图中可以看出，机器人行走 的步长和行走速度随着斜坡角度的增加而增加。然而，理 论和实践都已经表明，当斜坡角度增大到一定值时，机器 人不存在周期行走步态。
  • 55.第6章 机器人动力学 图6－12 非支撑腿极限环
  • 56.第6章 机器人动力学 图6－13 支撑腿极限环