Time Response of Dynamic Systems! Multi-Dimensional Trajectories Position, velocity, and acceleration are vectors

Transcription

1 Time Response of Dynamic Systems Robert Stengel Robotics and Intelligent Systems MAE 345, Princeton University, 217 Multi-dimensional trajectories Numerical integration Linear and nonlinear systems Linearization of nonlinear models LTI System Response Phase-plane plots Copyright 217 by Robert Stengel. All rights reserved. For educational use only. 1 Multi-Dimensional Trajectories Position, velocity, and acceleration are vectors x = x y ; v = v x v y ; a = a x a y ; j = j x j y ; s = s x s y ( t) ( t) x x t v x v x a x a x 1 x 1 t t 2 2 t 3 6 t 4 24 t 5 12 v x ( ) 1 a x ( ) = y1( ) t t 2 2 t 3 6 t 4 24 j x ( ) 1 y ( t) 1 s x ( ) 1 1 t t 2 2 t 3 6 t 4 24 t 5 12 t t 2 2 t 3 6 v y ( ) 1 c x ( ) v y ( t = ) 1 t t 2 2 t 3 6 t 4 24 a y ( ) 1 1 t t a y ( t) 2 2 t 3 6 y v y a y j y s y c y 2

2 Two-Dimensional Trajectory Solve for Cartesian components separately x Component j x s x c x 6 t 3 6 t 3 36 t 2 24 t 2 9 t 3 t = 36 t 4 36 t t t 3 36 t 2 24 t 2 72 t 5 72 t 5 36 t 4 36 t 4 6 t 3 6 t 3 ( t) ( t) x x t v x v x a x a x y Component j y s y c y 6 t 3 6 t 3 36 t 2 24 t 2 9 t 3 t = 36 t 4 36 t t t 3 36 t 2 24 t 2 72 t 5 72 t 5 36 t 4 36 t 4 6 t 3 6 t 3 ( t) ( t) y y t v y v y a y a y 3 Two-Dimensional Example Required acceleration vector is specified by a( t) = a( ) + j( )t + s( )t ct 3 6 = a control ( t) + a gravity ( t) + a disturbance ( t) + f gravity ( t) + f disturbance ( t) = f control t m t 4

3 Six-Degree-of-Freedom (Rigid Body) Equations of Motion r I = H B I v B v B = 1 m f B B v B = L I B B B = I 1 B ( m B B I B B ) Translational position and velocity r I = x y z ; v B = u v w Rotational position and velocity ( p * = *; + B = q * ) r ( * * * ) 5 nonlinear, and complex Rate of change of Translational Position x I = ( cos cos )u + ( cos sin + sin sin cos )v + ( sin sin + cos sin cos )w y I = ( cos sin )u + ( cos cos + sin sin sin )v + (sin cos + cos sin sin )w z I = (sin )u + ( sin cos )v + ( cos cos )w Rate of change of Translational Velocity u = X / m gsin + rv qw v = Y / m + gsin cos ru + pw w = Z / m + gcos cos + qu pv Rate of change of Angular Position = p + ( qsin + r cos )tan = qcos rsin = ( qsin + r cos )sec Rate of change of Angular Velocity 2 I xx I zz I xz p = I zz L + I xz N { I xz ( I yy I xx I zz ) p + I 2 xz + I zz ( I zz I yy ) r }q pr I xz ( p 2 r 2 ) q = M I xx I zz I yy r = I xz L + I xx N { I xz ( I yy I xx I zz )r + I 2 xz + I xx ( I xx I yy ) p }q 2 I xx I zz I xz 6

4 Multiple Rigid Links Lead to Multiple Constraints Each link is subject to the same 6-DOF rigidbody dynamic equations but each link is constrained to have a single degree of freedom w.r.t. proximal link 7 Newton-Euler Link Dynamics Link dynamics are coupled Proximal-link loads affected by distal-link positions and velocities Distal-link accelerations affected by proximallink motions Joints produce constraints on link motions Net forces and torques at each joint related to velocities and accelerations of the centroids of the links Equations of motion derived directly for each link, with constraints v B = 1 m f B B v B B = I 1 B ( m B B I B B ) x = f [ x,u,w,p,t ] 8

5 Lagrangian Link Dynamics Lagrange s equation derives from Newton s Laws Principle of virtual work D Alembert s principle Dynamic behavior described by work done and energy stored in the system Equations of motion derived from Lagrangian function and Lagrange s equation Generalized coordinate, q n, e.g., ( x, y,z,,,) Generalized force, F n, e.g., f x, f y, f z,m x,m y,m z d dt L( q n, q n ) KE PE dq n dl q n, q n ( L q, q n n = F n ( q n 9 x = f [ x,u,w,p,t ] Hamiltonian Link Dynamics Hamilton s Principle: Lagrange s equation is a necessary condition for an extremum (i.e., maximum or minimum) extremum I = t 2 t 1 L( q n, q n )dt Hamiltonian function H ( p,q) = q n p n Equations of motion derived from an optimization problem L q n, q n Hamilton s equations Generalized momentum, p n, q n = H ( p,q) p n p n = H p,q q n e.g., mv x,mv y,mv z, I xx p, I yy q, I zz r x = f [ x,u,w,p,t ] 1

6 Example: Two-Link Robot Equations of Motion State vector x 1 x 2 x 3 x 4 = Angleof1 st link, 1,rad Angular rateof1 st link, 1,rad/sec Angleof 2 nd link, 2,rad Angular rateof 2 nd link, 2,rad/sec Control vector u 1 u 2 st = 1, torque at 1 joint 2, torque at 2 nd joint McKerrow, When Possible, Simplify the Equations Two-link robot equations of motion Mass, m, located at end of Link 2 Inertias of Links 1 and 2 neglected x 1 = x 2 x 2 = x 3 = x 4 1 x cos 2 2 x 4 sin 2x 3 + u 1 x 3 ml 2 2 x 4 = g l 2 cos x 3 x 2 2 sin 2x 3 + u 2 ml 2 2 from Example 7.1, Lagrangian derivation, McKerrow, 1991, pp

7 Differential Equations Integrated to Produce Time Response x = f [ x,u,w,p,t ], x() given x = x() + t f [ x,u,w,p,t ]dt Numerical integration is an approximation 13 Rectangular and Trapezoidal Integration of Differential Equations Rectangular (Euler) Integration x(t k ) = x(t k1 ) + x(t k1,t k ) [ ] t, t = t k t k1 x(t k1 ) + f x(t k1 ),u(t k1 ),w(t k1 ) Trapezoidal (modified Euler) Integration (ode23) x(t k ) x(t k1 ) + 1 [ 2 x + x 1 2 ] where [ ] t { } t x 1 = f x(t k1 ),u(t k1 ),w(t k1 ) x 2 = f [ x(t k1 ) + x 1 ],u(t k ),w(t k ) ode23 varies step size, t, to reduce numerical error 14

8 Numerical Integration Examples Cosine Wave 1-sec Step How can approximation accuracy be improved? 15 More Complicated Algorithms (e.g., MATLAB) 16

9 1-D Example a net ( t) = ( ) +.6t.36t t Comparison of Exact and Numerically Integrated Trajectories x 1 t Calculate trajectory, given constants for t f = 1 x t v t a t 1 t t 2 2 t 3 6 t 4 24 t 5 12 = 1 t t 2 2 t 3 6 t t t 2 2 t Calculate trajectory by numerical integration = x 2 ( t) = v ( t) = a( t) =.6t.36t t 3 6 x 2 t 18

10 Comparison of Exact and Numerically Integrated Trajectories Polynomial, ode23, and ode45 19 Comparison of Exact and Numerically Integrated Trajectories (Zoom) Polynomial, ode23, and ode45 2

11 Generic Dynamic System Dynamic Process: Current state may depend on prior state x : state dim = (n x 1) u : input dim = (m x 1) w : disturbance dim = (s x 1) p : parameter dim = (l x 1) t : time (independent variable, 1 x 1) Observation Process: Measurement may contain error or be incomplete y : output (error-free) dim = (r x 1) n : measurement error dim = (r x 1) z : measurement dim = (r x 1) 21 Equations of the System x = dx dt Dynamic Equation = f[x,u,w,p,t] Output Equation y = h[x,u] Measurement Equation z = y + n 22

12 Dynamic System Example: Automotive Vehicle Dynamic Process x : dynamic state Position, velocity, angle, angular rate u : input Steering, throttle, brakes w : disturbance Road surface, wind p : parameter Weight, moments of inertia, drag coefficient, spring constants t : time (independent variable) Observation Process y : error-free output Speed, front-wheel angle, engine rpm, acceleration, yaw rate, throttle, brakes, GPS location n : measurement error Perturbations to y z : measurement Sum of y and n 23 Nonlinearity and Time Variation in Dynamic Systems Nonlinear, time-varying dynamics x = f x,u,w,p,t [ ] Nonlinear, time-invariant dynamics x = f x,u,w [ ] Linear, time-varying dynamics x = Fx + Gu + Lw Linear, time-invariant dynamics x = Fx + Gu + Lw NTV NTI LTV LTI 24

13 Nonlinearity and Time Variation in Dynamic Systems Nonlinear, time-varying dynamics Nonlinear, time-invariant dynamics Linear, time-varying dynamics Linear, time-invariant dynamics m, k, B 25 Solutions of Ordinary Differential Dynamic Equations Time-Domain Model (ODE) Solution by Numerical Integration Principle of Superposition Frequency-Domain Model. Nonlinear, timevarying (NTV) Yes No No. Nonlinear, timeinvariant (NTI) Yes No Yes (amplitudedependent, harmonics). Linear, timevarying (LTV) Yes Yes Approximate. Linear, timeinvariant (LTI) Yes Yes Yes 26

14 Comparison of Damped Linear and Nonlinear Systems x 1 = x 2 x 2 = 1x 1 x 2 Linear plus Stiffening Cubic Spring x 1 = x 2 x 2 = 1x 1 1x 1 3 x 2 Linear plus Weakening Cubic Spring x 1 = x 2 Linear Spring Spring Damper x 2 = 1x 1 +.8x 1 3 x 2 Spring Force vs. Displacement NTV or NTI? 27 MATLAB Simulation of Linear and Nonlinear Dynamic Systems MATLAB Main Program Nonlinear and Linear Examples clear tspan = [ 1]; xo = [, 1]; [t1,x1 = ode23(nonlin,tspan,xo); xo = [, 1]; [t2,x2] = ode23(nonlin,tspan,xo); xo = [, 1]; [t3,x3] = ode23(lin,tspan,xo); xo = [, 1]; [t4,x4] = ode23(lin,tspan,xo); subplot(2,1,1) plot(t1,x1(:,1),k,t2,x2(:,1),b,t3,x3(:,1),r,t4,x4(:,1),g) ylabel(position), grid subplot(2,1,2) plot(t1,x1(:,2),k,t2,x2(:,2),b,t3,x3(:,2),r,t4,x4(:,2),g) xlabel(time), ylabel(rate), grid Linear Spring x 1 = x 2 x 2 = 1x 1 x 2 function xdot = Lin(t,x) Linear Ordinary Differential Equation x(1) = Position x(2) = Rate xdot = [x(2) -1*x(1) - x(2)]; Weakening Spring x 1 = x 2 x 2 = 1x 1 +.8x 3 1 x 2 function xdot = NonLin(t,x) Nonlinear Ordinary Differential Equation x(1) = Position x(2) = Rate xdot = [x(2) -1*x(1) +.8*x(1)^3 - x(2)]; 28

15 Linear and Stiffening Cubic Springs: Small and Large Initial Rates x 1 () x 2 () = 1 and 1 Linear and nonlinear responses are indistinguishable with small initial condition 29 Linear and Weakening Cubic Springs: Small and Large Initial Rates x 1 () x 2 () = 1 and 1 3

16 Linearization of Nonlinear Equations Given Nominal (or reference) robot trajectory, control, and disturbance histories x N, u N, w N for t in [t o,t f ] Actual path and inputs x, u, w for t in [t o,t f ] = n 1 = m 1 = s 1 dim x dim u dim w perturbed by Initial condition variation Control variation Disturbance variation 31 State, Control, and Disturbance Perturbations Difference between nominal and actual paths: x(t o ) = x(t o ) x N (t o ) x = x x N Difference between nominal and actual inputs: u = u u N [ Control perturbation] w = w w N Disturbance perturbation [ ] 32

17 Linear Approximation of Perturbation Effects Nominal and actual paths both satisfy same dynamic equations Actual dynamics expressed as sum of nominal terms plus perturbations x = x N + x x N = f[x N,u N,w N,t] x = f[x,u,w,t] { } = f [x N + x],[u N + u],[w N + w,t] Exact f[x N,u N,w N,t]+ f ( x t)x + f ( u t)u + f ( w t)w Approx. Partial-derivative (Jacobian) matrices are evaluated along the nominal path 33 Linearized Equation Approximates Perturbation Dynamics Solve the nominal and perturbation parts separately Nominal (nonlinear) equation x N = f[x N,u N,w N,t], x N () given Perturbation (linear) equation x Fx + Gu + Lw, x() given Approximate total solution x x N + x 34

18 Jacobian Matrices Express Sensitivity to Small Perturbations Along Nominal Trajectory F = f x x=x N (t ) u=u N (t ) w=w N (t ) ; G = f u x=x N (t ) u=u N (t ) w=w N (t ) ; L = f w x=x N (t ) u=u N (t ) w=w N (t ) Sensitivity to state perturbations: stability matrix F = f = x x=x N (t ) u=u N (t ) w=w N (t ) f 1 x 1 f 1 x 2... f 1 x n f 2 f 2 f... 2 x 1 x 2 x n f n x 1 f n x 2... f n x n x=xn (t ) u=u N (t ) w=w N (t ) 35 Sensitivity to Small Control and Disturbance Perturbations Along Nominal Trajectory F = f x x=x N (t ) u=u N (t ) w=w N (t ) ; G = f u x=x N (t ) u=u N (t ) w=w N (t ) ; L = f w x=x N (t ) u=u N (t ) w=w N (t ) Control-effect matrix Disturbance-effect matrix G = f u x=x N (t ) u=u N (t ) w=w N (t ) = f 1 u m f n f... n u 1 u m f 1 u 1... x=xn (t ) u=u N (t ) w=w N (t ) L = f w x=x N (t ) u=u N (t ) w=w N (t ) = f 1 w 1... f 1 w s f n w 1... f n w s x=xn (t ) u=u N (t ) w=w N (t ) 36

19 Stiffening Cubic Spring Example Nonlinear equation, no inputs x 1 = f 1 = x 2 x 2 = f 2 = 1x 1 1x 3 1 x 2 Integrate nonlinear equation to produce nominal path x 1 () x 2 () t f ( f 1N f 2N dt x 1N x 2N in,t f 37 Stiffening Cubic Spring Example Evaluate partial derivatives along the path F( t) = f 1 x 1 = f 2 = 1 3x 2 1N x 1 f 1 x 2 = 1 f 2 x 2 = 1 ( ( ( ( ( G( t) = f 1 u = f 2 u = L( t) = f 1 w = f 2 w = 38

20 Nominal and Perturbation Dynamic Equations Nominal system, NTI x N = f[x N ], x N () given x 1N = x 2N x 2N = 1x 1N 1x 3 1N x 2N x 1N () x 2N () = 9 x 1 x 2 New initial condition: x = Fx, Perturbation system, LTV x() given = 1 ( 1 + 3x 2 1N (1 x 1 x 2 x 1 () x 2 () = 1 x 1 () x 2 () = 1 x 1N generated by nominal solution 39 Nominal and Perturbation Dynamic Solutions for Cubic Spring Example with x N () = If nominal solution remains at equilibrium x N = f[x N ], x N () =, x N = in [, ] Linearization is time-invariant x 1 x 2 = 1 x 1 (1 (1 x 2 = Linear, Time-Invariant (LTI) System 4

21 Comparison of Approximate and Exact Solutions Rate Initial Condition: x 2N () = 9 x 2 () = 1 x 2 = 1 x 2N + x 2 = 1 x N : Nominal x : Perturbation x : Actual x N + x : Approximation 41 Initial-Condition Response of a Linear, Time-Invariant (LTI) Model Doubling the initial condition doubles the output Stability, speed of response, and damping are independent of the initial condition Linear Model - Initial Condition F = [ ;.7814 ]; G = [1-1; 2]; Hx = [1 ; 1]; sys = ss(f, G, Hx,); xo = [1;]; [y1,t1,x1] = initial(sys, xo); xo = [2;]; [y2,t2,x2] = initial(sys, xo); plot(t1,y1,t2,y2) figure xo = [;1]; initial(sys, xo) F = G = 2 H x = I 2 ; H u = 42

22 Step Response of a Linear, Time-Invariant Model Doubling the step input doubles the output Stability, speed of response, and damping are independent of the input Linear Model - Step F = [ ;.7814 ]; G = [1-1; 2]; Hx = [1 ; 1]; sys = ss(f, G, Hx,); sys2 = ss(f, 2*G, Hx,); Step response step(sys, sys2) 43 Response to Combined Initial Condition and Step Input Linear system responses are additive Linear Model - Superposition F = [ ;.7814 ]; G = [1;]; Hx = [1 ; 1]; sys = ss(f, G, Hx,); xo = [1; ]; t = [:.2:2]; u = ones(1,length); [y1,t1,x1] = lsim(sys,u,t,xo); [y2,t2,x2] = lsim(sys,u,t); u = zeros(1,length); [y3,t3,x3] = lsim(sys,u,t,xo); plot(t1,y1,t2,y2,t3,y3) 44

23 Initial-Condition Responses of 1 st - Order LTI Systems are Exponentials x = Fx, x() given State vector is a scalar F = [ a] x = ax x() given 1 st -order system Unstable, a > Stable, a < t x = xdt = axdt = e at x() t 45 Initial-Condition Responses of 2 nd -Order LTI Systems Exponentials and Sinusoids x = Fx x() given F = f 11 f 12 f 21 f 22 x 1 = f 11 x 1 + f 12 x 2 + g 1 u x 2 = f 21 x 1 + f 22 x 2 + g 2 u Stable, Lightly Damped 2 nd -order system Sinusoid with exponential envelope x 1 = A 1 e nt cos n 1 2 t + 1 ( ) x 2 = A 2 e nt cos n 1 2 t + 2 ( ) 46

24 Equilibrium Response of Linear, Time-Invariant Models General equation x = Fx + Gu + Lw At equilibrium, Derivative goes to zero State is unchanging = Fx + Gu + Lw State at equilibrium ( Gu*+Lw *) x* = F 1 Gu*+Lw * = Adj F det F 47 State ( Phase )-Plane Plots Cross-plot of one component against another Time or frequency not shown explicitly in phase plane 2nd-Order Model - Initial Condition Response clear z =.1; Damping ratio wn = 6.28; Natural frequency, rad/s F = [ 1;-wn^2-2*z*wn]; G = [1-1; 2]; Hx = [1 ; 1]; sys = ss(f, G, Hx,); t = [:.1:1]; xo = [1;]; [y1,t1,x1] = initial(sys, xo, t); plot(t1,y1) grid on figure plot(y1(:,1),y1(:,2)) grid on 48

25 Effects of Damping Ratio on State-Plane Plots Damping ratio =.1 Damping ratio =.3 Damping ratio =.1 49 Response of Higher-Order LTI Systems Superposition of Sub-System Responses F System = F System 1 Effect of 1 on 2 Effect of 2 on 1 F System 2 Third-order system with uncoupled 1 st - and 2 nd - order sub-systems Coupling in first row and first column F = f 11 F = 1 f 32 f 33 f 11 f 12 f 13 f 21 1 f 31 f 32 f 33 x 1 x = x 2 x 3 5

26 Examples of Coupled and Uncoupled Third-Order Systems Third-order system with uncoupled 1 st - and 2 nd -order sub-systems F = x 1 x = x 2 x 3 Coupling in first row and first column Position Coupling, x 3 Rate Coupling,x 2 F = F = rd -Order LTI Systems with Coupled Response Initial Condition on x 1 Initial Condition on x 3 52

27 Next Time: Dynamic Effects of Feedback Control 53 Supplemental Material 54

28 Perturbed Initial Conditions Produce Perturbed Path Given Initial condition, control, and disturbance histories x(t ),u, w for t in [t o,t f ] Path (or trajectory) is approximated by executing a numerical algorithm Perturbing the initial condition produces a new path Both paths satisfy the same equations of motion x N = f[x N,u,w,t], x N (t ) given x N : Nominal path x: Perturbed path x = f[x,u,w,t], x(t ) given 55 Equilibrium Response of Second-Order LTI System System description F = f 11 f 12 ; G = g 1 ; L = l 1 f 21 f 22 g 2 l 2 State equilibrium depends on constant input values x 1 * x 2 * f 22 ( f 12 = ( ( f 21 f 11 ) ( f 11 f 22 ( f 12 f 21 ) * + g 1 g 2 / F = f 11 f 22 ( f 12 f 21, ) -. u *+ l 1 * + l 2, -. w * 56