Professional Documents
Culture Documents
Systems!
Robert Stengel!
Robotics and Intelligent Systems MAE
345, Princeton University, 2017
Multi-dimensional trajectories
Numerical integration
Linear and nonlinear systems
Linearization of nonlinear models
LTI System Response
Phase-plane plots
Copyright 2017 by Robert Stengel. All rights reserved. For educational use only.
http://www.princeton.edu/~stengel/MAE345.html 1
Multi-Dimensional Trajectories
Position, velocity, and acceleration are vectors
! x $ ! vx $ ! ax $ ! jx $ ! sx $
x=# & ; v = # & ; a = # &; j= # &; s = # &
#" y &% #" vy &% #" ay &% #" jy &% #" sy &%
! x (0) $ ! 1 0 0 0 0 0
!
$ # x (0) &
$
# &
x (t ) & # &
# 1 t t 2 2 t 3 6 t 4 24 t 5 120 & # vx ( 0 ) &
# & # # &
# vx ( 0 ) & # 0 1 0 0 0 0 & # ax ( 0 ) &
=# !0 y1( 0 ) t$ t 2 2 t 3 6 t 4 24 & #
# vx ( t ) & # & #0 jx ( 0 ) 0&
& ! y(0) $
# & # & ! 1 0 0 0 $# &
# ax ( 0 ) & #
# #0 y0( t ) 1& # 10 t t02 2 t 3 06 t &4 #24s ( 0t 5) 120
& &# vy ( 0 ) &
# & #0 0 & # &# x & &# &
1 t t 02
2
t0 6 & 0
3
#" ax ( t ) & #" # vy ( 0 ) & # 0 1 % # cx ( 0 ) 0& &# ay ( 0 ) &
% # v t &=# 0 1 " % &# &
( ) # t t 2 2 t 3 6 t 4 24 &# jy ( 0 ) &
# y &
# a (0) & # 0 0 1 0 0 0 &#
&# sy ( 0 ) &
# y & # 0 0 1 t t 22
t 6
3 &
# ay ( t ) & #" &% # cy ( 0 ) &
" % " %
2
Two-Dimensional Trajectory
Solve for Cartesian components separately
x Component
! x (0) $
# &
# x (t ) &
! j ( 0 ) $ ! ' 60 t 3 60 t 3 ' 36 t 2 ' 24 t 2 '9 t 3t $# &
# x & # &# vx ( 0 ) &
# sx ( 0 ) & = # 360 t 4 ' 360 t 4
192 t 3
168 t 3
36 t 2
' 24 t 2 &#
# & # &# vx ( t ) &
#" cx ( 0 ) &% #" ' 720 t
5
720 t 5 ' 360 t 4 ' 360 t 4 ' 60 t 3 60 t 3 &
&% #
ax ( 0 ) &
# &
#" ax ( t ) &
%
y Component
! y(0) $
# &
# y (t ) &
! j ( 0 ) $ ! ' 60 t 3 60 t 3 ' 36 t 2 ' 24 t 2 '9 t 3t $# &
# y & # &# vy ( 0 ) &
# sy ( 0 ) & = # 360 t 4 ' 360 t 4
192 t 3
168 t 3
36 t 2
' 24 t 2 &#
# & # &# vy ( t ) &&
# cy ( 0 ) & #" ' 720 t 720 t 5 ' 360 t 4 ' 360 t 4 ' 60 t 3 60 t 3
5
&% #
" % ay ( 0 ) &
# &
# ay ( t ) &
" %
3
Two-Dimensional Example
Required acceleration vector is specified by
a ( t ) = a ( 0 ) + j ( 0 ) t + s ( 0 ) t 2 2 + ct 3 6
= a control ( t ) + a gravity ( t ) + a disturbance ( t )
= !" fcontrol ( t ) + fgravity ( t ) + fdisturbance ( t ) #$ m ( t )
4
Six-Degree-of-Freedom (Rigid Body)
Equations of Motion
Translational position
r!I = H IB v B and velocity
1 ! x $ ! u $
v! B = fB ! "" B v B # & #
rI = # y & ; v B = # v &
&
( { ( ) (
p! = I zz L + I xz N ! I xz I yy ! I xx ! I zz p + "# I xz2 + I zz I zz ! I yy $% r q ) }) (I I ! I xz2 )
xx zz
q! = "# M ! ( I xx ! I zz ) pr ! I xz ( p 2 ! r 2 )$% 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 (I I ! I xz2 )
xx zz 6
Multiple Rigid Links Lead to
Multiple Constraints
•! Each link is subject to the same 6-DOF rigid-
body dynamic equations
•! … but each link is constrained to have a
single degree of freedom w.r.t. proximal link
Newton-Euler
Link Dynamics
•! Link dynamics are coupled
–! Proximal-link loads affected by distal-link
positions and velocities
–! Distal-link accelerations affected by proximal-
link motions
–! Joints produce constraints on link motions
1
v! B = fB ! "" B v B
m x! (t) = f [ x(t), u(t), w(t), p(t),t ]
"! B = I B !1 ( m B ! "" B I B " B )
8
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, Fn , e.g., fx , fy , fz , mx , my , mz )
L ( q n , q! n ) " KE ! PE
d ! dL ( q n , q! n ) $ ( L ( q n , q! n )
&% ' = Fn
dt #" d!q n ( qn
9
x! (t) = f [ x(t), u(t), w(t), p(t),t ]
•! Hamiltonian function
H ( p,q ) = ! q!n pn " L ( qn , q!n )
•! Hamilton
s equations Generalized momentum, p n ,
(
e.g., mvx , mvy , mvz , I xx p, I yy q, I zz r )
! H ( p,q )
q!n =
! pn x! (t) = f [ x(t), u(t), w(t), p(t),t ]
! H ( p,q )
p! n = "
! qn 10
Example: Two-Link Robot
Equations of Motion
State vector
! x1 $ ! Angle of1st link,'1 ,rad $
# & # &
# x2 & # Angular rate of1st link,'!1 ,rad/sec &
=# &
# x3 & # Angle of 2 nd link,' 2 ,rad &
# &
# x4 & # Angular rate of 2 nd link,'! ,rad/sec &
" % #" 2 &%
Control vector
11
McKerrow, 1991
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 = x2
1 ! u1 $
x!2 = x x sin 2x +
cos 2 x3 #" ml2 2 &%
2 4 3
x! 3 = x4
g x u
x! 4 = ' cos x3 ' 2 sin 2x3 + 2 2
l2 2 ml2
from Example 7.1, Lagrangian derivation, McKerrow, 1991, pp. 388-390.
12
Differential Equations Integrated to
Produce Time Response
x! (t) = f [ x(t),u(t),w(t),p(t),t ], x(0) given
t
13
15
16
1-D Example
x!1 ( t ) = x2 ( t )
x!2 ( t ) = v! ( t ) = a ( t ) = 0.6t ! 0.36t 2 2 + 0.072t 3 6
18
Comparison of Exact and Numerically
Integrated Trajectories
19
20
Generic Dynamic System
Equations of
the System
Dynamic Equation
dx(t)
x˙ (t) = = f[x(t),u(t),w(t),p(t),t]
dt
Output Equation
y(t) = h[x(t),u(t)]
Measurement Equation
Nonlinearity and
Time Variation in
Dynamic Systems
25
Solutions of Ordinary
Differential Dynamic Equations
Solution by
Time-Domain Numerical Principle of Frequency-Domain
Model (ODE) Integration Superposition Model
. Nonlinear, time-
varying (NTV) Yes No No
. Nonlinear, time- Yes (amplitude-
invariant (NTI) Yes No dependent, harmonics)
. Linear, time-
varying (LTV) Yes Yes Approximate
. Linear, time-
invariant (LTI) Yes Yes Yes
26
Comparison of Damped Linear
and Nonlinear Systems
Linear Spring Spring Force vs.
Displacement
! x1 (0) $ ! 0 $ ! 0 $
# &=# & and # &
#" x2 (0) &% " 10 % " 1 %
Linear and nonlinear responses are indistinguishable with small initial condition
29
! x1 (0) $ ! 0 $ ! 0 $
# &=# & and # &
#" x2 (0) &% " 10 % " 1 %
30
Linearization of Nonlinear Equations
•! Given
–! Nominal (or reference) robot
trajectory, control, and disturbance
histories
Linearized Equation
Approximates
Perturbation Dynamics
Solve the nominal and perturbation parts separately
Nominal (nonlinear) equation
" ! f1 ! f1 % " ! f1 ! f1 %
$ ... ' $ ... '
$ ! u1 ! um ' $ ! w1 ! ws '
!f !f
G(t) = =$ ... ... ... ' L(t) = =$ ... ... ... '
! u u=uN (t ) $
x=x N (t )
' ! w u=uN (t ) $
x=x N (t )
'
$ ! fn ! fn ' $ ! fn ! fn '
w=w N (t )
... w=w N (t )
...
$ ! u1 ! um ' x=x N (t ) $ ! w1 ! ws ' x=x N (t )
# & u=u N (t ) # & u=u N (t )
w=w N (t ) w=w N (t )
36
Stiffening Cubic Spring Example
Nonlinear equation, no inputs
37
# ! f1 ! f1 &
% =0 =1 (
! x ! x (
F (t ) = %
1 2
% ! f2 ! f2 (
% = "10 " 30x1N
2
(t) = "1 (
%$ ! x1 ! x 2 ('
" ! f1 % " ! f1 %
$ =0 ' $ =0 '
!u !w
G (t ) = $ ' L (t ) = $ '
$ ! f2 ' $ ! f2 '
$ =0 ' $ =0 '
# !u & # !w &
38
Nominal and Perturbation
Dynamic Equations
Nominal system, NTI
x! N (t) = f[x N (t)], x N (0) given
! x1 (0) $ !
&=# 0 &
$
x!1N (t) = x2 N (t) # N
# x2 N (0) & " 9 %
" %
x!2 N (t) = !10x1N (t) ! 10x1N 3 (t) ! x2 N (t)
! x1 (0) $ ! 0 $
New initial condition: # &=# &
#" x2 (0) &% " 10 %
Perturbation system, LTV
!!x(t) = F(t)!x(t), !x(0) given
" !x1 (0) % " 0 %
" !!x1 (t) % " 0 1 % " !x1 (t) % $ '=$ '
$ '=$ '$ ' $# !x2 (0) '& # 1 &
(
$# !!x2 (t) '& $# ( 10 + 30x1N (t)
2
) (1 ' $ !x2 (t) '
&# &
x1N (t) generated by nominal solution 39
Linearization is time-invariant
x N (t) : Nominal
!x(t) : Perturbation
x(t) : Actual
x N (t) + !x(t) : 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
xo = [1;0];
[y1,t1,x1] = initial(sys, xo);
xo = [2;0];
[y2,t2,x2] = initial(sys, xo);
plot(t1,y1,t2,y2)
" !0.5572 !0.7814 %
F=$ '
figure # 0.7814 0 &
xo = [0;1]; " 1 !1 %
G=$ '
initial(sys, xo) # 0 2 &
Hx = I2 ; Hu = 0
42
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
% Step response
step(sys, sys2)
43
xo = [1; 0];
t = [0:0.2:20];
u = ones(1,length(t));
[y1,t1,x1] = lsim(sys,u,t,xo);
[y2,t2,x2] = lsim(sys,u,t);
u = zeros(1,length(t));
[y3,t3,x3] = lsim(sys,u,t,xo);
plot(t1,y1,t2,y2,t3,y3)
44
Initial-Condition Responses of 1st-
Order LTI Systems are Exponentials
"x˙ (t) = F"x(t), "x(0) given
State vector is a scalar
1st-order system
F = [ a]
!!x(t) = a!x(t)
Unstable, a > 0
t t
= e !x(0)
at
45
Initial-Condition Responses of
2nd-Order LTI Systems
Exponentials and Sinusoids
!!x(t) = F!x(t) Stable, Lightly Damped
2nd-order system
!x(0) given
"f f12 %
F = $ 11 '
# f 21 f 22 &
plot(t1,y1)
grid on
figure
plot(y1(:,1),y1(:,2))
grid on
48
Effects of Damping Ratio
on State-Plane Plots
•! Damping ratio = 0.1 •! Damping ratio = 0.3 •! Damping ratio = –0.1
49
52
Next Time:!
Dynamic Effects of
Feedback Control!
53
Supplemental Material
54
Perturbed Initial Conditions
Produce Perturbed Path
•! Given
–! Initial condition, control, and
disturbance histories
Equilibrium Response of
Second-Order LTI System
System description
"f f12 % " g1 % " l1 %
F = $ 11 ' ; G = $ ' ; L = $ '
# f 21 f 22 & #g2 & #l2 &