m x ¨ = f m\ddot x=f m x ¨ = f I θ ¨ = τ I\ddot \theta=\tau I θ ¨ = τ Box example
m x ¨ + c x ˙ + k x = f m\ddot x+c\dot x+kx=f m x ¨ + c x ˙ + k x = f
v ˙ = 1 m ( f − k x − c v ) \dot v=\frac{1}{m}(f-kx-cv) v ˙ = m 1 ( f − k x − c v )
x ¨ = 1 m ( f e x t − k x − c x ˙ ) \ddot x=\frac{1}{m}(f_{ext}-kx-c\dot x) x ¨ = m 1 ( f e x t − k x − c x ˙ )
=>
x ¨ + c m x ˙ + k m x = 1 m f \ddot x+\frac{c}{m}\dot x+\frac{k}{m}x=\frac{1}{m}f x ¨ + m c x ˙ + m k x = m 1 f
time constant system
v ˙ = 1 m ( f e x t − k x − c v − μ N ) \dot v=\frac{1}{m}(f_{ext}-kx-cv-\mu N) v ˙ = m 1 ( f e x t − k x − c v − μ N )
Steady state (x ˙ = x ¨ = 0 \dot x=\ddot x=0 x ˙ = x ¨ = 0 Transient response => how long does it take to get to steady state spring-damper system
Final:: Given a system of the form
m x ¨ = f m\ddot x=f m x ¨ = f
I θ ¨ = τ I\ddot\theta=\tau I θ ¨ = τ
How would you make it into a mass-spring-damper system with the desired dynamics through choice of f or τ \tau τ
virtual spring, virtual damper
Free Body Diagram
friction, f f r i c t i o n = μ N f_{friction}=\mu N f f r i c t i o n = μ N ext, $$ gravity, normal force spring damp Assume f s p r i n g = f e x t = 0 f_{spring}=f_{ext}=0 f s p r in g = f e x t = 0
m v ˙ = − c v m\dot v=-cv m v ˙ = − c v
Let v ( t ) = v 0 e − α t v(t)=v_0e^{-\alpha t} v ( t ) = v 0 e − α t α \alpha α
v ˙ = − α v 0 e − α t \dot v=-\alpha v_0e^{-\alpha t} v ˙ = − α v 0 e − α t
− α v 0 e − α t + c m v 0 e − α t = 0 -\alpha v_0e^{-\alpha t}+\frac{c}{m}v_0e^{-\alpha t}=0 − α v 0 e − α t + m c v 0 e − α t = 0
− α + c m = 0 ⇒ α = c m -\alpha+\frac{c}{m}=0\Rightarrow \alpha=\frac{c}{m} − α + m c = 0 ⇒ α = m c
v ( t ) = v 0 e − α t = v 0 e − c m t v(t)=v_0e^{-\alpha t}=v_0e^{-\frac{c}{m}t} v ( t ) = v 0 e − α t = v 0 e − m c t
e 0 = 1 e − 1 = 0.37 e − 2 = 0.13 e − 3 = 0.05 e − 4 = 0.02 e^0=1\\e^{-1}=0.37\\e^{-2}=0.13\\e^{-3}=0.05\\e^{-4}=0.02 e 0 = 1 e − 1 = 0.37 e − 2 = 0.13 e − 3 = 0.05 e − 4 = 0.02
choose t = 1 α t=\frac{1}{\alpha} t = α 1 t c t_c t c
v ( t ) = v 0 e − 1 = v 0 ( 0.37 ) v(t)=v_0e^{-1}=v_0(0.37) v ( t ) = v 0 e − 1 = v 0 ( 0.37 )
v ( t ) = v(t)= v ( t ) =
Use to design controller
t = 4 α = 4 t c t=\frac{4}{\alpha}=4t_c t = α 4 = 4 t c
v ( t ) = v 0 e − 4 = v 0 ( 0.02 ) v(t)=v_0e^{-4}=v_0(0.02) v ( t ) = v 0 e − 4 = v 0 ( 0.02 )
What are components of gravity in body axes?
In World:
translation: m a ⃗ = f m\vec a=f m a = f
rotation: I ω ˙ + ω × I ω = τ ⃗ I\dot \omega+\omega\times I\omega=\vec \tau I ω ˙ + ω × I ω = τ
In body coords:
In Body Coor:
Translation: m v ˙ + ω × v = f m\dot v+\omega\times v=f m v ˙ + ω × v = f
rotation: I ω + ω × I ω = τ I\omega+\omega\times I\omega=\tau I ω + ω × I ω = τ
In body coor, I is constant according to the object
To simulate:
Equas of motion
v ˙ B = 1 m ( f − ω × v ) \dot v^B=\frac{1}{m}(f-\omega\times v) v ˙ B = m 1 ( f − ω × v )
ω ˙ B = I − 1 ( τ − ω × I ω ) \dot \omega^B=I^{-1}(\tau-\omega\times I\omega) ω ˙ B = I − 1 ( τ − ω × I ω )
Given v ˙ B \dot v^B v ˙ B v ˙ 0 \dot v^0 v ˙ 0
v ˙ 0 = R B 0 v ˙ B \dot v^0=R^0_B\dot v^B v ˙ 0 = R B 0 v ˙ B
ω ˙ 0 = R B 0 ω ˙ B \dot\omega^0=R_B^0\dot\omega^B ω ˙ 0 = R B 0 ω ˙ B
v ˙ 0 → v → x \dot v^0\rightarrow v\rightarrow x v ˙ 0 → v → x
use numerical Integration
v ˙ B → v B \dot v^B\rightarrow v^B v ˙ B → v B
v B → x B v^B\rightarrow x^B v B → x B
This does not allow us to determine where the object is in the world
For rocket
Integrate v ˙ B \dot v^B v ˙ B v B v^B v B Dist Pinned in world around y-axis
Equs of motion
I ω + ω × I ω = ∑ τ I\omega+\omega\times I\omega=\sum\tau I ω + ω × I ω = ∑ τ
I=moment of Inertia
I = [ I x x − I x y − I x z I y y − I y z I z z ] I=\begin{bmatrix} I_{xx}&-I_{xy}&-I_{xz}\\ &I_{yy}&-I_{yz}\\ &&I_{zz} \end{bmatrix} I = I xx − I x y I y y − I x z − I y z I z z
I x x = I z z = 1 4 m r 2 I_{xx}=I_{zz}=\frac{1}{4}m r^2 I xx = I z z = 4 1 m r 2
I y y = 1 2 m r 2 I_{yy}=\frac{1}{2}mr^2 I y y = 2 1 m r 2
I x y = I y z = I x z = 0 I_{xy}=I_{yz}=I_{xz}=0 I x y = I y z = I x z = 0
what is ω × I ω \omega\times I\omega ω × I ω
Rotational mass-spring damper dynamics
I y y w ˙ y = − k θ − c θ ˙ + τ ^ e x t I_{yy}\dot w_y=-k\theta-c\dot\theta+\hat \tau_{ext} I y y w ˙ y = − k θ − c θ ˙ + τ ^ e x t
since ω ˙ = θ ¨ \dot \omega=\ddot\theta ω ˙ = θ ¨
(in world coordinates)
Applied forces and Torques
f t o t a l = ∑ f i f_{total}=\sum f_i f t o t a l = ∑ f i τ t o t a l = ∑ ( r i × f i ) + ∑ τ i \tau_{total}=\sum (r_i\times f_i)+\sum\tau_i τ t o t a l = ∑ ( r i × f i ) + ∑ τ i Forward Dynamics
v ˙ = x ¨ = 1 m f t o t a l \dot v=\ddot x=\frac{1}{m}f_{total} v ˙ = x ¨ = m 1 f t o t a l ω ˙ = ( I ) − 1 ( τ t o t a l − ω × I ⋅ ω ) \dot \omega=(I)^{-1}(\tau_{total}-\omega\times I\cdot\omega) ω ˙ = ( I ) − 1 ( τ t o t a l − ω × I ⋅ ω ) System Input: u = [ f t o t a l , u t o t a l ] u=[f_{total},u_{total}] u = [ f t o t a l , u t o t a l ]
System State: x = [ p , θ , v , ω ] x=[p,\theta,v,\omega] x = [ p , θ , v , ω ]
System Dynamics: x ˙ = f ( x , u ) \dot x=f(x,u) x ˙ = f ( x , u )
Objective
Compute input (u u u state (x x x a desired value (x d x_d x d
Controller Objective: drive e->0 in the steady state
把a（二次项）隔离开，这样可以算f=xxx
Type
Proportional Control PD PID Center of mass
Apply a t
Translation: v ˙ = 1 m f \dot v=\frac{1}{m}f v ˙ = m 1 f
Rotation: ω ˙ = I − 1 ( τ − ω × I ω ) \dot \omega=I^{-1}(\tau-\omega\times I\omega) ω ˙ = I − 1 ( τ − ω × I ω )
θ ¨ = 1 I y y τ \ddot\theta=\frac{1}{I_{yy}}\tau θ ¨ = I y y 1 τ
θ d = \theta_d= θ d =
like mass-spring damper system (virtual spring, virtual damper)
θ ¨ + a 1 θ ˙ + a 0 θ = a 0 θ d \ddot\theta+a_1\dot\theta+a_0\theta=a_0\theta_d θ ¨ + a 1 θ ˙ + a 0 θ = a 0 θ d
feedback controller
τ = \tau= τ =
f = f= f =
τ = I y y ( a 0 θ d − a 1 θ ˙ − a 0 θ ) \tau=I_{yy}(a_0\theta_d-a_1\dot\theta-a_0\theta) τ = I y y ( a 0 θ d − a 1 θ ˙ − a 0 θ )
θ ¨ + 2 ζ ω n θ ˙ + ( ω n ) 2 θ = ( ω n ) 2 θ d \ddot\theta+2\zeta\omega_n\dot\theta+(\omega_n)^2\theta=(\omega_n)^2\theta_d θ ¨ + 2 ζ ω n θ ˙ + ( ω n ) 2 θ = ( ω n ) 2 θ d
t c = 1 ζ ω n t_c=\frac{1}{\zeta\omega_n} t c = ζ ω n 1
t s e t t l e = 4 t c t_{settle}=4t_c t se ttl e = 4 t c
which is how long it takes to get to steady state
Given system of this form:
θ ¨ + 10 θ θ ˙ + 2 ( θ ˙ ) 2 + 2 z θ = b τ \ddot \theta+10\theta\dot\theta+2(\dot\theta)^2+2z\theta=b\tau θ ¨ + 10 θ θ ˙ + 2 ( θ ˙ ) 2 + 2 z θ = b τ
how would you make it respond like a system of the form
θ ¨ + a 1 θ ˙ + a 0 θ = a 0 θ d \ddot \theta+a_1\dot\theta+a_0\theta=a_0\theta_d θ ¨ + a 1 θ ˙ + a 0 θ = a 0 θ d
step1: clean house (leave θ ¨ \ddot \theta θ ¨
τ = 1 b ( 10 θ θ ˙ + 2 ( θ ) 2 + 2 z θ ) + 1 b ( a 0 θ d − a 1 θ ˙ − a 0 θ ) \tau=\frac{1}{b}(10\theta\dot\theta+2(\theta)^2+2z\theta)+\frac{1}{b}(a_0\theta_d-a_1\dot\theta-a_0\theta) τ = b 1 ( 10 θ θ ˙ + 2 ( θ ) 2 + 2 z θ ) + b 1 ( a 0 θ d − a 1 θ ˙ − a 0 θ )
Given Actual Dynamics:
m z ¨ + c z ˙ + k z = f m\ddot z+c\dot z+kz=f m z ¨ + c z ˙ + k z = f
Want desired 1st order dynamicsL V d = V_d= V d =
v ˙ + a V = a V d \dot v+aV=aV_d v ˙ + aV = a V d
choosing f = − K p z − K v V + K 1 V d = k z + ( c − m a ) v + m a v d f=-K_pz-K_vV+K_1V_d=kz+(c-ma)v+mav_d f = − K p z − K v V + K 1 V d = k z + ( c − ma ) v + ma v d
Given: m z ¨ + c z ˙ x x x x m\ddot z+c\dot zxxxx m z ¨ + c z ˙ xxxx
Want e = ( z − z d ) ⇒ 0 ; e ˙ = 1 ; e ¨ = 0 e=(z-z_d)\Rightarrow0;\dot e=1;\ddot e=0 e = ( z − z d ) ⇒ 0 ; e ˙ = 1 ; e ¨ = 0
With desired Dynamics
Proportional-Derivative (PD) Control
Computing the error.
Before the feedback controller can do anything, it must first find the difference between the current and desired system states (e = x d − x e = x_d - x e = x d − x
Proportional Control terms
Derivative (PD) Control terms
Integral (I) Control terms
There are three basic types of controllers that can used to minimize the error, denoted:
Frame of Reference for equation of motion
World frame
m x ˙ = f m\dot x=f m x ˙ = f I ω ˙ + ω × I ω = τ I\dot\omega+\omega \times I\omega=\tau I ω ˙ + ω × I ω = τ Body frame
T s e t t l e = 4 T c T_{settle}=4T_c T se ttl e = 4 T c
What is T c T_c T c
v ˙ + a v = a v d \dot v+av=av_d v ˙ + a v = a v d
for this system T c = 1 a T_c=\frac{1}{a} T c = a 1
T s e t t l e = 4 a T_{settle}=\frac{4}{a} T se ttl e = a 4 a = 4 T s e t t l e a=\frac{4}{T_{settle}} a = T se ttl e 4
f = k v ( v d − v ) f=k_v(v_d-v) f = k v ( v d − v ) k v k_v k v
v ˙ + k v v = k v v d \dot v + k_v v=k_vv_d v ˙ + k v v = k v v d k v = 4 T s e t t l e k_v=\frac{4}{T_{settle}} k v = T se ttl e 4
FKIK
Hopefully, see how these connect
If better, use final, worse ave
Actual: x ˙ = a x + b u \dot x=ax+bu x ˙ = a x + b u Desire: x ˙ = c ( x d − x ) \dot x=c(x_d-x) x ˙ = c ( x d − x ) T c = 1 c , T s e t t l e = 4 T c T_c=\frac{1}{c}, T_{settle}=4T_c T c = c 1 , T se ttl e = 4 T c
x ˙ = ( a − b K 0 K p ) x + b K 0 K 1 x d \dot x=(a-bK_0K_p)x+bK_0K_1x_d x ˙ = ( a − b K 0 K p ) x + b K 0 K 1 x d
Actual: x ¨ + a 1 x ˙ + a 0 x = b u \ddot x+a_1\dot x+a_0x=bu x ¨ + a 1 x ˙ + a 0 x = b u Desire: x ¨ + 2 ζ ω n x ˙ + ω n 2 x = ω n 2 x d \ddot x+2\zeta\omega_n\dot x+\omega_n^2x=\omega_n^2x_d x ¨ + 2 ζ ω n x ˙ + ω n 2 x = ω n 2 x d T s e t t l e = 4 T c = 4 ζ ω n T_{settle}=4T_c=\frac{4}{\zeta\omega_n} T se ttl e = 4 T c = ζ ω n 4
ζ = \zeta= ζ =
ω n = \omega_n= ω n =
ζ = 0 ⇒ \zeta=0\Rightarrow ζ = 0 ⇒
0 < ζ < 1 ⇒ 0<\zeta<1\Rightarrow 0 < ζ < 1 ⇒
ζ = 1 ⇒ \zeta=1\Rightarrow ζ = 1 ⇒
ζ > 1 ⇒ \zeta>1\Rightarrow ζ > 1 ⇒
