Rigid-Body Dynamics !Robert Stengel! Robotics and Intelligent Systems MAE 345, Princeton University, 2013"• Inertia and momentum"• Inertia matrix"• Equations of motion"Copyright 2013 by Robert Stengel. All rights reserved. For educational use only.!http://www.princeton.edu/~stengel/MAE345.html!Reference Frame "• Newtonian (Inertial) Frame of Reference"– Unaccelerated Cartesian frame "• Origin referenced to inertial (non-moving) frame"– Right-hand rule"– Origin can translate at constant linear velocity"– Frame cannot rotate with respect to inertial origin"• Translation = Linear motion"r =xyz⎡⎣⎢⎢⎢⎤⎦⎥⎥⎥• Position: 3 dimensions"– What is a non-moving frame?"Velocity and Momentum "• Velocity of a particle" v =drdt=r =xyz⎡⎣⎢⎢⎢⎤⎦⎥⎥⎥=vxvyvz⎡⎣⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥• Linear momentum of a particle"p = mv = mvxvyvz⎡⎣⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥Newtons Laws of Motion: !Dynamics of a Particle "• First Law"– If no force acts on a particle, "• it remains at rest "• or continues to move in straight line at constant velocity, "– Inertial reference frame "– Momentum is conserved"ddtmv( )= 0 ; mvt1= mvt2ddtmv( )= mdvdt= ma = ForceForce =fxfyfz⎡⎣⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥= force vector• Second Law"– Particle of fixed mass acted upon by force changes velocity with "• acceleration proportional to and in direction of force "– Inertial reference frame "– Ratio of force to acceleration is the mass of the particle:: F = m a"• Third Law"– For every action, there is an equal and opposite reaction"∴dvdt=1mForce =1mI3Force=1 / m 0 00 1 / m 00 0 1 / m⎡⎣⎢⎢⎢⎤⎦⎥⎥⎥fxfyfz⎡⎣⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥One-Degree-of-Freedom Example of Newtons Second Law"2nd-order, linear, time-invariant ordinary differential equation "" d2x(t)dt2x(t) =vx(t) =fx(t)mCorresponding set of 1st-order equations "" dx1t( )dtx1(t)  x2(t)  vx(t)dx2t( )dtx1(t) =x2(t) =vx(t) =fx(t)m Displacement : x1(t )  x(t )Rate : x2(t ) dx t( )dt  "Defined as"State-Space Model of the 1-DOF Example" x (t ) = F x(t ) + G u(t )x(t) =x1(t)x2(t)⎡⎣⎢⎢⎤⎦⎥⎥; u(t) = u t( )= fx(t); y(t) =x1(t)x2(t)⎡⎣⎢⎢⎤⎦⎥⎥F =0 10 0⎡⎣⎢⎤⎦⎥; G =01 / m⎡⎣⎢⎤⎦⎥State, control, and output vectors""Dynamic system coefficient matrices"Dynamic equation"State-Space Model of the 1-DOF Example"Output equation!y (t ) = Hxx (t ) + Huu(t )• Vectors"– State Vector: " " "x(t) (n x 1)!– Control Vector: " "u(t) (m x 1)"– Output Vector: " "y(t) (r x 1)!Hx=1 00 1⎡⎣⎢⎤⎦⎥; Hu=00⎡⎣⎢⎤⎦⎥Output coefficient matrices"• Matrices"– Stability Matrix: " " "F (n x n)"– Control-Effect Matrix; " "G (n x m)"– State-Output Matrix: " "Hx (r x n)"– Control-Output Matrix: "Hu (r x m)!State-Space Model of Three-Degree-of-Freedom (Point Mass) Dynamics" x(t) r(t )v(t )⎡⎣⎢⎢⎤⎦⎥⎥=dx(t)dt= F x(t) + G u(t)• System of 1st-order, ordinary differential equations"– Definition of velocity"– Newtons 2nd in 3 dimensions"Define" x(t ) r(t )v(t )⎡⎣⎢⎢⎤⎦⎥⎥State-Space Model of Three-Degree-of-Freedom Dynamics" r(t)v(t)⎡⎣⎢⎢⎤⎦⎥⎥=xyzvxvyvz⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥=vxvyvzfx/ mfy/ mfz/ m⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥r(t)v(t)⎡⎣⎢⎢⎤⎦⎥⎥=xyzvxvyvz⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥=0 0 0 1 0 00 0 0 0 1 00 0 0 0 0 10 0 0 0 0 00 0 0 0 0 00 0 0 0 0 0⎡⎣⎢⎢⎢⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥⎥⎥⎥xyzvxvyvz⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥+0 0 00 0 00 0 01 / m 0 00 1 / m 00 0 1 / m⎡⎣⎢⎢⎢⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥⎥⎥⎥fxfyfz⎡⎣⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥External Forces: Linear Springs"Scalar, linear spring"Scalar, nonlinear (cubic) spring"Uncoupled, linear vector spring"fx= −kΔx = −k x − xo( ); k = spring constantfx= −k1Δx − k3Δx3fS= −kxΔxkyΔykzΔz⎡⎣⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥= −kx0 00 ky00 0 kz⎡⎣⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥ΔxΔyΔz⎡⎣⎢⎢⎢⎤⎦⎥⎥⎥External Forces: Viscous Dampers"Scalar, linear damper"fx= −kΔv = −k v − vo( ); k = damping constantState-Space Model with Linear Spring and Damping Model" xyzvxvyvz⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥=0 0 0 1 0 00 0 0 0 1 00 0 0 0 0 10 0 0 0 0 00 0 0 0 0 00 0 0 0 0 0⎡⎣⎢⎢⎢⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥⎥⎥⎥xyzvxvyvz⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥+0 0 0 0 0 00 0 0 0 0 00 0 0 0 0 0−kxm0 0−kvxm0 00−kym0 0−kvym00 0−kzm0 0−kvzm⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥x − xo( )y − yo( )z − zo( )vx− vxo( )vy− vyo( )vz− vzo( )⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥Spring Effects"Damping Effects"State-Space Model with Linear Spring and Damping Model" xyzvxvyvz⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥=0 0 0 1 0 00 0 0 0 1 00 0 0 0 0 1−kxm0 0−kvxm0 00−kym0 0−kvym00 0−kzm0 0−kvzm⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥xyzvxvyvz⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥+0 0 0 0 0 00 0 0 0 0 00 0 0 0 0 0kxm0 0kvxm0 00kym0 0kvym00 0kzm0 0kvzm⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥xoyozovxovyovzo⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥Introduce spring and damping effects in the stability matrix" x(t) = F x(t ) + G u(t)External Forces: Gravity"• Flat-earth approximation"– g is gravitational acceleration"– mg is gravitational force"– Independent of position"– z measured up at right"• Round, rotating earth"– Inverse-square gravitation"– Centripetal acceleration "– Non-linear function of position"– µ = 3.986 x 1014 m/s2"– Ω = 7.29 x 10–5 rad/s"u = mgf= m00–go⎡⎣⎢⎢⎢⎤⎦⎥⎥⎥; go= 9.807 m / s2gr=gxgygz⎡⎣⎢⎢⎢⎢⎤⎦⎥⎥⎥⎥= ggravity[non − rotating frame]gr= ggravity+ grotation[rotating frame]= −µr3xyz⎡⎣⎢⎢⎢⎤⎦⎥⎥⎥+