Robot Dynamics

Hesheng Wang Dept. of Automation Shanghai Jiao Tong University What is Robot Dynamics?

• Robot dynamics studies the relation between robot motion and forces and moments acting on the robot.  2 l2

2 l 1 1 1 x Rotation about a Fixed Axis

The velocity v can be determined from

the cross product of  and rp . Here rp is a vector from any point on the axis of rotation to P.

v =  x rp =  x r

The direction of v is determined by the right-hand rule. Rotation about a Fixed Axis (continued) The acceleration of P can also be defined by differentiating the velocity.

a = dv/dt = d/dt x rP +  x drP/dt =  x rP +  x ( x rP) Tangent accel Normal/centripetal acceleration

It can be shown that this equation reduces to

2 a = a x r –wr = at + an 2 2 The magnitude of the acceleration vector is a = (at) + (an) Rotation of a Vector • Consider rotation of a vector about a axis. P r Point P is rotating about axis u. r is the position vector of point P. u  : the speed of rotation  The angular velcotiy : ω  u The rate of change of position vector r : d r  ωr dt Rotation of a Frame Frame A y B x • Consider a frame B rotating B about an unit vector u. u

i B , jB ,k B : unit directional vectors of the axes z  of frame B w.r.t. the reference frame A B d d d i  ωi , j  ω j , k  ωk dt B B dt B B dt B B The derivative of the rotation matrix of frame B : d R  i j k  ω i ω j ωk  dt B B B B B B  R  ω R General Motion

• A general motion can be considered a combination of a translation with a z1 point and motion about the point. B A O  xo yo zo : A fixed reference frame z y1 A  x y z : A frame that translate with A 0 1 1 1 x1 O The velocity relation: y0 x0 VB  VA  ωr

The acceleration relation:

aB  a A  α r  ω(ωr) Introduction to Dynamics Newton’s Laws of Motion

First Law: A particle originally at rest, or moving in a straight line at constant velocity, will remain in this state if the resultant force acting on the particle is zero. F Second Law: If the resultant force on the particle is not zero, the a particle experiences an acceleration in the same direction as the resultant F  ma force. This acceleration has a m : the mass magnitude proportional to the F : the net force a : the acceleration resultant force.

Third Law: Mutual forces of action and reaction between two particles F F’ are equal, opposite, and collinear. Example m Find the accelerations of the ball 1 Frictionless and the wedge. surface m2  Solution: (1) Draw the free-body diagram of the particles (2) Apply Newton’s 2nd Law N For the ball R m1xb  N sin (1)

m1yb  N cos  m1g (2)

For the wedge m2g m1g y N m2 xw  N sin (3) x Example (continued)

• Consider acceleration relationship between the ball and the wedge

ab ab  aw  ab/ w

ab / w : relative velocity of the ball a w.r.t. the wedge w Note that the relative velocity is along the surface of the wedge ab/w From the diagram, we have

xb  xw  ab/W cos (4)

yb  ab/ w sin (5)

From eqs. (1)-(5), we can solve the five unknowns, i.e. the acceleration Linear Momentum

• Linear momentum: product of mass and velocity: L  mV mV – It is a vector, in the same direction as velocity - SI Unit : Kg  m/s V From Newton's 2nd Law : d F  ma  m V  L dt

• Principle of Linear Momentum: The rate of change of the linear momentum of a particle is equal to the result force acting on the particle Angular Momentum

• r: the position vector of a particle w.r.t. a V reference point O.

• V: the velocity of the particle r • m: the mass of the particle • The angular momentum of the particle O about reference O:

Ho  r  mV

Ho is a vector perpendicular to both V and r. Its direction is determined by the right- hand rule Its unit is Kgm2 / s Principle of Angular Momentum

Consider a force F acting on the particle V

Differentiating the angular momentum F r H  r  mV  r  mV  r F o  O

 H o  r F  Mo

Principle of angular momentum: The rate of change of the angular momentum of particle about a fixed point O is equal to the resultant moment of forces acting on the particle about point O Dynamics of a System of Particles Consider a system of n particles. f1 fi fi: the external force exerted on particle i m1 eij: the internal force exerted on particle i mi by particle j m2 eij mi: mass of particle i. r : position vector of particle i eji i f d 2 m Linear momentum: L  m V  m r j  i i  i i fj dt Total mass mr The center of mass : r   i  L  m r  MV  c  i c C  mi Velocity of The linear momentum of a system of particles is center of mass equal to the product of the total mass and the velocity of the center of mass Dynamics of a System of Particles Differentiating the linear momentum 2 d f1 fi L  m r  (f  e ) m1  i dt 2 i i ij i ij mi

 eij  0  L  fi m2 ei ij i The rate of change the linear momentum of a ej f2 system of particles is equal to the resultant of all mj EXTERNAL forces acting on the particles fj

ac: acceleration of center of mass L  MVC  Mac  fi i Equation of motion of Center of mass The center of mass of the system moves as if all the forces and masses are concentrated at the center of mass Angular Momentum of a System of Particles Similarly, by differentiating the f f 1 m i angular momentum 1 mi

d m2 ei H o   ri  mVi  MO i dt ej f 2 m The resultant moment of all j fj EXTERNAL forces acting on the system about O O

The reference point must be a fixed point. However, the center of mass of the system can be the reference point even when it is moving Example: m2 Ry Example: Calculate the angular acceleration l2 of the massless link  Rx l O Solution 1 Consider the two particles and the link as a system m2g m (1) Analyze the external forces 1 (2) Consider the angular momentum about O m g  1 0  0      d  H o  Mo  0   0  dt  2 2   m gl cos  m gl cos  (m1l1  m2l2 )   2 2 1 1 

(m1l1 m2l2 )cos   2 2 m1l1 m2l2 Linear and Angular Momentums for Rigid Body • Since a rigid body can be considered as a system with infinite number of VC particles, the linear momentum L  mV C Center of mass •The angular momentum about the center of mass ω

HC  Iω I : Inertia tensor matrix about C. ω : angular velocity of the body Newton’s Equation and Euler’s Equation

• A general motion can be considered a f1 combination of a translation with the n z center of mass and motion about the i 1 center of mass C O  xo yo zo : A fixed reference frame y z0 ri 1 C  x1 y1z1 : A frame that translates with C x1 f O i The equation of translation: y0 x ma  F  f 0 c  i Newton’s equation The equation of motion about C: Euler’s equation d A A A HC  MC  Iω  ω( Iω)  ri fi  ni dt   Newton’s Equation and Euler’s Equation (Cont’)

• The angular velocity is with respect to the translating frame. • The inertia tensor matrix is with respect to the translating frame, so it will change its value with rotation of the body. • The force is the resultant of the EXTERNAL force • The moment is the resultant moment of the EXTERNAL forces and moments. Example

• Derive the dynamic equation of the 2 DOF manipulator. Here, the masses of links 1 and 2 are m1 and m2 respectively. Assume that the mass is uniformly distributed over the link.  2 Solution: l2

(1) Analyzing forces acting on the links 2 l  1 1 2 Rx  O 1 1 x0 Ry Nx

Rx Ry m g m2g Ny 1 Example (continued)

R (2) Dynamics of link 1. As x 1 link 1 is rotating about O, Nx 1 Ry m g IM  Ny 1  Oo1  Moment of Resultant moment about O inertia about O    l I    m g 1 cos  R l cos  R l sin (1) O 1 1 1 2 1 y 1 1 x 1 1 Example (3) Dynamics of link 2.  2 As link 2 is in a general plane motion C2 m a  F Ry 2 C2 C C I ω  ω  I ω  M Rx 2  2 2 2 2 C2 B Acceleration of C2: a  a  a  a m g C2 B C / Bt C / Bn 2  l 2c   l  s  a a  a  a   1 1 1    1 1 1   ... Bt B Bn Bt  2   l1 1 s1  l11c1  l  2  c l  a  2 (  )  12  2  s12   C / Bn 1 2  s   aC / Bt  ( 1 2 )  1 2  12  2 c12   l  l   a  l (  s  2c )  2 (   )s  2 (    )2 c  Bn 1 1 1 1 1 2 1 2 12 2 1 2 12 aC    2  2 l2 l2  2   l1( 1c1  1 s1)  ( 1  2 )c12  ( 1 2 ) s12  a  2 2  C / Bt 1 2 Newton’s m a  R (2) 2 c2 x x equation: m a  R  m g (3) 2 c2 y y 2 aC / Bn Example (continued)

Consider Euler’s equation. As the mass in uniformly distributed and the link is  symmetric, the inertia tensor matrix is diagonal.     I 0 0      I 0 0     2xx 0 0 2xx 0  0   0 I 0 0  0   0 I 0 0  0  2 yy      2 yy       0 0 I       0 0 I    l l  2zz  1 2   1 2   2zz  1 2    2 R s  2 R c   2 2 x 12 2 y 12     l l I (    )    2 R s  2 R c (4) 2zz 1 2 2 2 x 12 2 y 12

From (2) and (3), we can solve Rx and Ry.

Substituting Rx and Ry in eqs. (1) and (4) leads to the dynamic equation of the robot arm. Formulation of Robot Dynamics Recursive Newton-Euler Formulation We consider manipulators with revolute joints only. z y z i-1 Link i-1 i i Joint i

O x Joint i-1 i i r C : center si-1 i i yi-1 of mass  ai-1 i xi-1 i1 Oi-1 Relative angular velocity if link I Angular velocity relation ωi ωi1 ωi/i1 to link i-1 between link i-1 and i:  ωi ωi1 zii (1 ) Differentiating ω ω z  ω  z (2 ) (1)  i  i1 i i i1 i i Recursive Newton-Euler Equation (Cont’)

• Consider velocity and acceleration of Oi. Vi : velocity of Oi V  V  V Vi1 : velocity of Oi-1 i i1 i /i1 Vi /i1 : realtive velocity of Oi to Oi-1

As the relative motion of Oi w.r.t. Oi-1 is a motion about Oi-1, V  ω s i /i1 i1 i1  Vi  Vi1  ωi1 si1 (3)

Acceleration ai  ai1 a Rt aRn at Oi:  ai1  ω i1 si1  ωi1 (ωi1 si1) (4)

aC  ai aC /O Acceleration at the i i i center of mass:  ai  ω i ri  ωi (ωi ri ) (5) Forward Equations

ωi  ωi1  zii (1)

ω i ω i1 zi i  ωi1 zii (2 ) ai  ai1  ω i1 si1  ωi1 (ωi1 si1) (3) i=0 a  a  ω r  ω (ω r ) (4) Ci i  i i1 i i i1 Calculate  from (1) From (1), (2), (4) and (5), we i+1 can recursively calculate . the angular velocity and Calculate i+1 from (2) acceleration of the links, and the acceleration at the Calculate ai+1 from (3) center of mass. The initial conditions: Calculate a from (4) When i  0, ci+1 ω0  0, ω 0  0 V  0, a  0 No YES 0 0 i=i+1 i=n-1 END Dynamic equation of robot link

Derive the dynamics by applying Newton-Euler equations to link i. -ni+1 (1) Draw the free-body diagram of link O i. Assume that the links are rigidly i+1 connected. Cut link i from the arm: si fi: the force acting on link i by link i-1 Ci ni O ni: the moment applied on link i by link i-1 i ri -fi+1 Applying Newton’s Law mig 0  fi m a  f  f  0  i ci i i1    mi g  0   f  m a  f  0  (7) i i ci i1   mi g  Dynamics of robot link (Cont’)

Applying the Euler’s equation

Iiω i  ωi Iiωi  ni  ni1  ri fi  (si  ri )fi1  ni  Iiω i  ωi Iiωi  ni1  ri fi  (si  ri )fi1 (8) -ni+1 Eqs. (7) and (8) give the recursive backward equation for calculating the Oi+1

interaction force and moment si

Relation between ni and joint torque Ci ni O T i ri -fi+1  i  zi ni as actuator produces torque about the joint axis only mig fi Backward Calculation

Initial conditions: When i  k (for the last link) : fk 1  0, nk1  0 k : the number of links

i=K

Calculate fi from (7)

Calculate ni from (8)

i=i-1

NO YES i=0 END Lagrange Formulation of Robot Dynamics

• Lagrange formulation is an analytical method for deriving the robot dynamics. It is based on the energy and work principle • Energy of Mechanical Systems – Kinetic energy: energy due to motion of a particle or body – Potential energy: due to gravitational forces, deformation of mechanical systems, etc. Kinetic Energy

• A particle (body) has kinetic energy when it moves. Kinetic energy is always greater than zero 1 V For a particle : K  mV 2 2 1 For a system of particles : K  m V 2  2 i i ω

1 T For a rigid body : K  r rdm V  2 c V C 1 1  mV 2  ωT Iω 2 c 2 I: inertia tensor Potential Energy

• We here consider the gravitational potential energy only. For a particle : U  mgz

z: the height of the particle w.r.t. a reference level

For a system of particles : U  mi gzi z

For a rigid body : U  mgzc

The height of the center of mass Potential energy is a value relative to the reference. It could be positive, zero and negative Work

When a particle underwent a displacement r under a constant force f, the work done by the force on the particle is W  f T r r f – Work is a scalar – It could be positive, zero and negative – SI unit: Nm Work done by time-varying force

Consider the work done by a

time-varying force on a particle V2 that moved from one position to another. 2 V1 Position 2 W  f T (t)dr 1 f(t) 2 2 dV  m dr  mVdV  dt  Position 1 1 1 1 1  mV 2  mV 2 2 2 2 1 Work-energy principle: The work done  K by a force acting on particle is equal to the change of its kinetic energy Conservative force and Non- Conservative force • The force associated to the potential energy is called conservative force. A force that is not associated with the potential energy is called non-conservative force. • The work done by conservative force (gravity z2 force)

Wg  mg(z2  z1)  U

• The work done by non-conservative forces mg z1

Wn Wg  K  Wn  K  U  (K U ) The work done by non-conservative forces is equal to the change of total energy Conservation of Energy

• If no non-conservative force acting on a system (a particle, or a system of particles, or a rigid body), does not do any work, the total energy of the system is conserved.

K U  constant Lagrange Equation

• Generalized coordinates q: A set of parameters for representing the configuration (position & orientation) of a system. – q must specify the configuration uniquely – Once the values of q are fixed, the system cannot move. – The choice of q is not unique. • Degree of freedom (DOF): The dimension of the generalized coordinates vector q is called degrees of freedom of the system Examples

 2 l2

2  1  l1 q    DOF =2 1  2  1

x0

 x     q  y    DOF=4      (x,y) Generalized Force Consider the work done by non-conservative forces under a differential displacement of the system

ri : differential displacement at the action point of fi due to a differential change q of the generalized coordinates q

The differenti al work W : f T i W  fi ri The generalized force F of the system is given by  F   (W) q: generalized coordinates vector ( q) Example

Calculate the generalized force of f the 2 DOF arm  2  l Generalized Coordinate s : q   1  2     2  2  T l For differential motion q   1,2  1 1  The differential work  1 T  x  W  1 1  2 2  f x  0 x : the differential motion of the end point      T x  J (q)q  W  1 1  2 2  f J (q) q   q  (J (q)f )T q 当前无法显示此图像。 1 2

Generalized Force: Lagrange Equation q: the generalized coordinates of a system K: Kinetic energy of the system U: the potential energy of the system F: the generalized forces of the system Define L=K-U: Called Lagrangian The dynamics of the system is given by d L L   F dt q q Derivation of Robot Dynamics using Lagrange Equation 1) Choose the generalized coordinates q (usually use the kinematics parameters defined by the D-H method) 2) Identify the non-conservative forces that are exerted at the system and do work 3) Calculate the kinetic energy K and the potential energy U, and then L=K-U 4) Calculate the partial derivatives 5) Calculate the generalized force F. 6) Apply the Lagrange equation. Example 1 Center of mass Example: Denote the mass of link i by mi. The mass is uniformly distributed over the link. Derive the dynamics of the 2 DOF arm.  2 l  y 2  1  c Solution (1) Generalized coordiantes : q    2  2  2 l (2) Non - conservative forces that do work : 1,  2 1 1 (3) The kinetic energy: 1 1 2  link 1 rotates about O,  K1  Io1 x 2 1 2 1 2  link 2 in general plane motion,  K 2  m2Vc  Ic 2 2 2 2 2    l2 Position of xc  l1c1  c12 2   2 2 2 2 2 2 2 l2 2  the Center   Vc  xc  yc  l1 1  ( 1  2 )  l1l2c2 1( 1 2 ) l2 2 2 2 4 Of mass y  l s  s  c2 1 1 2 12 Example 1 (continued)

The total kinetic energy 11 K K K() I  ml22  mllc ()   1222o 211 2122112  1122  (ml22 Ic )( 1  2 )  822 The potential energy (assume y=0 is the reference l l U  m gy  m gy   m g 1 s  m g(l s  2 s ) 1 c1 2 c2 1 2 1 2 1 1 2 12 (4) Calculate the partial derivatives L  1 1   (I  m l 2 )   m l l c (    )  ( m l 2  I )(   ) 0 2 1 1 2 1 2 2 1 2 2 2 c2 1 2  1 2 4 L 1 1   m l l c   ( m l 2  I )(   )   2 2 1 2 2 1 4 2 2 c2 1 2 2 L l1 l2  m1g c1  m2 g(l1c1  c12 ) 1 2 2  L l2 1   m2 g c12  m2l1l2s2 1( 1 2 )  2 2 2 Example 1 (continued)

5) The generalized forces:

 1  W  11  22  F     2  d L L 6) Apply the Lagrange equation:   F   dt q q   I  m l 2  0.25m l 2  I  m l l c 0.25m l 2  I  0.5m l l c     o1 2 1 2 2 c2 2 1 2 2 2 2 c2 2 1 2 2 1   2 2    0.25m2l2  Ic2  0.5m2l1l2c2 0.25m2l2  Ic2  2   m l l s (   0.5  )   (0.5m  m )l gc  0.5m l gc     2 1 2 2 1 2 2 1 2 1 1 2 2 12    1   2     0.5m2l1l2s2 1  0.5m2l2 gc12   2  Example 2

Example 2: The moment of inertia of the first link is I1. The mass of link 2 is m2. The mass is concentrated at the endpoint. An external force f acts at the endpoint. Derive the dynamics of the arm. y 3 x3 Solution: (1) Generalized coordinates z1 y l2 y 1 Use the D-H method to assign 2 x2 z x 3 frames and select the joint angles as 1 f the generalized coordinates z2

i i1 ai1 di i 1 0 0 0  1 z  0 y 2 90 0 0 2 0 3 0 l2 0 0 x   0 Generalized coordinates: q   1  2  Example 2 (continued) (2) No-conservative forces that do work:

joint inputs : 1, 2 , and the external force f (3) Kinetic energy, potential energy and Lagrangian Kinetic energy: 1 1 2 2 Link 1: K  I  Link 2 : K2  m2V 1 2 1 1 2 To find the velocity V of the endpoint, we need to solve the forward kinematics

1 0 0 l2  c  s 0 0  c2  s2 0 0    1 1    2 0 1 0 0 0 s c 0 0 1 0 0 1 0 T3    T   1 1  T2    0 0 1 0 1 0 0 1 0 s2 c2 0 0     0 0 0 1  0 0 0 1   0 0 0 1    Example 2 (continued)

Forward kinematics: c c  c s s l c c c1c2  c1s2 s1 0   1 2 1 2 1 2 1 2    0 0 2  s c  s s  c l s c  0T 0T 1T  s1c2  s1s2  c1 0 T  T T  1 2 1 2 1 2 1 2 2 1 2  s c 0 0 3 2 3  s c 0 l s   2 2   2 2 2 2   0 0 0 1   0 0 0 1 

d l2c1c2   V  l s c   V 2  l 2c2 2  l 22 dt  2 1 2  2 2 1 2 2  l2s2 

The potential energy (Assuming that U=0 when z0=0).

U  m1gh1  m2 gl2s2 Lagrangian:  1 1 L  K U  (I  m l 2c2 ) 2  m l 22  m gh  m gl s 2 1 2 2 2 1 2 2 2 2 1 1 2 2 2 Example 2 (continued)

(4) Calculate the partial derivatives  L 2 2  (I1  m2l2 c2 )1  1

L 2  m2l22  2 L  0 1 L 1 2  2   m2l2 sin 2 21  m2 gl2c2  2 2    Example  2 (continued) (5) Generalized forces:  T  1  T W  1 1  2 2  f x  F     J (q)f  2  x: the position of the endpoint

l2c1c2  x  l2c1c2   l2s1c2  l2c1s2  x  l s c  J(q)   l s c    l c c  l s s   2 1 2  q q  2 1 2   2 1 2 2 1 2   l2s2   l2s2   0 l2c2 

(6) Applying the Lagrange equation leads to the dynamics:

 I  m l 2c2 0    0.5m l 2 sin 2    0   1 2 2 2  1    2 2 2 1 2     0 m l   2 2  m gl c  2 2  2  0.5m2l2 sin 2 2 1   2 2 2    T   1   J (q)f   2      Structure of Robot Dynamics  • The dynamics of the 2 DOF manipulator:   2 2 2  Io1  m2l1  0.25m2l2  Ic2  m2l1l2c2 0.25m2l2  Ic2  0.5m2l1l2c2  1   2 2    0.25m2l2  Ic2  0.5m2l1l2c2 0.25m2l2  Ic2  2    H (q)  m l l s (   0.5  )   (0.5m  m )l gc  0.5m l gc    2 1 2 2 1 2 2    1 2 1 1 2 2 12    1   2   0.5m l gc     0.5m2l1l2s2 1   2 2 12   2      C(q,q ) G(q)

H (q) : Intertia matrix of the manipulator, H T (q)  H (q)  symmetric matrix depending on the joint position C(q, q) : the centrifugal and Coriolis forces Centrigual force : Terms that depend on the square of the joint velocity of a joint. Coriolis force :Terms that depend on the product of the joint velocities of two joints. G(q) : The gravity force    Structure of Robot Dynamics   • The centrifugal and Coriolis term can be re-written as

 m2l1l2s2 ( 1  0.5 2 ) 2   C(q,q)   2   0.5m2l1l2s2 1         1   m l l s   0.5m l l s   0  0.5m l l s (   0.5  )      2 1 2 2 2 2 1 2 2 2    2 1 2 2 1 2  1  2  0.5m2l1l2s2 2 0  0.5m2l1l2s2 ( 1  0.5 2 ) 0  2        1 S(q,q)  H (q)   2 

S(q, q ) is a skew - symmetric matrix, i.e. T S (q, q )  S(q, q ) T For any x, x S(q, q )x  0 Structure of Robot Dynamics

• In general, the dynamics of a robot manipulator has the following form:

1 H(q)q  ( H (q)  S(q,q))q  G(q)  τ 2 Inertial force Centrifugal and Coriolis forces Gravity Joint inputs H(q) : Symmetric and positive - definite inertial matrix.

1 T q H(q)q is the kinetic energy  0 2 S(q,q) : a skew -symmetric matrix T n  x S(q,q)x  0, x  R Linear Parameterization of Robot Dynamics    The dynamics of the 2 DOF arm:  2 2 2  Io1  m2l1  0.25m2l2  Ic2  m2l1l2c2 0.25m2l2  Ic2  0.5m2l1l2c2  1   2 2    0.25m2l2  Ic2  0.5m2l1l2c2 0.25m2l2  Ic2  2   m l l s (   0.5  )   (0.5m  m )l gc  0.5m l gc    2 1 2 2 1 2 2    1 2 1 1 2 2 12    1   2   0.5m l gc     0.5m2l1l2s2 1   2 2 12   2 

 2 2 The parameters depend 1  Io1  m2l1  0.25m2l2  Ic2  2 Define 2  0.25m2l2  Ic2 on mass, length, moment  3  0.5m2l1l2 of inertia. They are the  4  (0.5m1  m2 )l physical parameters 5  0.5m2l2    1    θ θ c (2θ  θ )  2s (θ  0.5θ )θ gc gc  2   1 2 2 1 2 2 1 2 2 1 12     1   2  3    0 θ1  θ2 c2θ1  s1θ1 0 gc12   2     4  Y(q,q ,q ,q)  5  The result can be generalized to n DOF robot manipulator β Parameter vector