Mae 342 Lecture 12
Mae 342 Lecture 12
Copyright 2016 by Robert Stengel. All rights reserved. For educational use only.
http://www.princeton.edu/~stengel/MAE342.html 1
Fortescue
2
1
2/12/20
2
2/12/20
Fortescue
5
3
2/12/20
More on Rotation
Matrices and Quaternions
4
2/12/20
§ Cosines of angles
between each I axis
and each B axis
§ Projections of vector
components in one
frame on the other
rB = H BI rI
= ( aT rI ) a + ⎡⎣ rI − ( aT rI ) a ⎤⎦ cos φ + sin φ ( rI × a )
= cos φ rI + (1− cos φ ) ( aT rI ) a − sin φ ( a × rI )
⎡ a1 ⎤
⎢ ⎥
a ! ⎢ a2 ⎥
⎢ a ⎥
⎣ 3 ⎦ 10
10
5
2/12/20
Euler’s Formula
rB = H BI rI = cos φ rI + (1− cos φ ) ( aT rI ) a − sin φ ( a × rI )
Identity
( a r ) a = ( aa ) r
T
I
T
I
Rotation matrix
11
11
⎡ ⎡ ⎛ a1 ⎞ ⎤
q1 ⎤ ⎢ ⎥
⎢ ⎥
⎢ q2 ⎥ ⎡ aφ ⎤ ⎢ sin (φ 2 ) ⎜ a2 ⎟ ⎥
⎥=⎢ ⎜ ⎟ ⎥
q=⎢ !⎢
q3 ⎥ ⎢ q4 ⎥⎦ ⎢ ⎜ a3
⎝
⎟
⎠ ⎥
⎢ ⎥ ⎣ ⎢ ⎥
⎢⎣ q4 ⎥
⎦ ⎢⎣ cos (φ 2 ) ⎥⎦
§ Not singular at θ = ±90°
§ 4-parameter representation of 3 parameters;
hence, a constraint must be satisfied
q T q = q12 + q2 2 + q32 + q4 2
= sin 2 (φ 2 ) + cos 2 (φ 2 ) = 1 12
12
6
2/12/20
( )
H BI = ⎡⎣ q4 2 − aφ T aφ ⎤⎦ I 3 + 2aφ aφ T − 2q4 a! φ
Rotation matrix from quaternion
H BI =
⎡ q2 − q2 − q2 + q2 2 ( q1q2 + q3q4 ) 2 ( q1q3 − q2 q4 ) ⎤
⎢ 1 2 3 4
⎥
⎢ 2 ( q1q2 − q3q4 ) −q12 + q22 − q32 + q42 2 ( q2 q3 + q1q4 ) ⎥
⎢ ⎥
⎢ 2 ( q1q3 + q2 q4 ) 2 ( q2 q3 − q1q4 ) −q12 − q22 + q32 + q42 ⎥
⎣ ⎦
13
13
⎡ q1 ⎤ ⎡ (h − h ) ⎤
1 ⎢ ⎥
23 32
⎢ ⎥
aφ = ⎢ q2 ⎥ = ⎢ ( h31 − h13 ) ⎥
⎢ q ⎥ 4q ⎢ ⎥
⎢ ( h12 − h21 )
4
⎣ 3
⎦ ⎥
⎣ ⎦
Pisacane, 2005 14
14
7
2/12/20
15
d ⎡ aφ ⎤ 1 ⎡ q4 ω B − ω" B aφ ⎤
q! = ⎢ ⎥= ⎢ ⎥
dt ⎢ q4
⎣ ⎥⎦ 2 ⎢ −ω BT aφ ⎥
⎣ ⎦
⎡ q!1 ⎤ ⎡ 0 ωz −ω y ω x ⎤ ⎡ q1 ⎤
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ q!2 ⎥ 1 ⎢ −ω z 0 ωx ωy ⎥ ⎢ q2 ⎥
⎢ = ⎢ ⎥
q! 3 ⎥ 2 ⎢ ω y −ω x 0 ωz ⎥ ⎢ q3 ⎥
⎢ ⎥ ⎢ ⎥
⎢⎣ q! 4 ⎥ ⎢ −ω −ω y −ω z 0 ⎥ ⎢⎣ q4 ⎥
⎦ ⎣ x
⎦B ⎦
Pisacane, 2005 16
16
8
2/12/20
dq (τ )
tk
q int ( t k ) = q ( t k−1 ) + ∫t dt dτ
k−1
17
B
⎡ h11 h12 h13 ⎤
⎢ ⎥
H I = ⎢ h21 h22
B
h23 ⎥
⎢ h h h33 ⎥
⎣ 31 32 ⎦I
⎡ cosθ cosψ cosθ sinψ − sin θ ⎤
⎢ ⎥
= ⎢ − cos φ sinψ + sin φ sin θ cosψ cos φ cosψ + sin φ sin θ sinψ sin φ cosθ ⎥
⎢ sin φ sinψ + cos φ sin θ cosψ − sin φ cosψ + cos φ sin θ sinψ cos φ cosθ ⎥
⎣ ⎦
• Apply equations on earlier slide to find q(0)
• Perform trigonometric inversions as indicated to generate
[Φ(tk), θ(tk),Ψ(tk)] from q(tk)
18
18
9
2/12/20
Yo-Yo De-Spin
19
19
20
20
10
2/12/20
Yo-Yo De-spin
Kaplan
21
21
Yo-Yo De-spin
Angular momentum
R = spacecraft radius
hz = I zzω z + mR 2 ⎡⎣ω z + φ 2 (ω z + φ! ) ⎤⎦ l = tether length
mR 2 + I zz
c=
Rotational energy mR 2
2
22
11
2/12/20
Continuously Variable
Torque Controllers
23
23
Overview of Control
24
24
12
2/12/20
Single-Axis “Classical”
Control of Non-Spinning
Spacecraft
Pitching motion (about the y axis) is to be controlled
⎡ p! ( t )
⎢
⎤ ⎡ M x ( t ) / I xx
⎥ ⎢ ⎥ ⎢
( )
⎤ ⎡ I zz − I yy q ( t ) r ( t ) / I xx ⎤
⎥
⎢ q! ( t ) ⎥ = ⎢ M y ( t ) / I yy ⎥ − ⎢ ( I xx − I zz ) p ( t ) r ( t ) / I yy ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢⎣ r! ( t ) ⎥⎦ ⎢⎣ M z ( t ) / I zz ( )
⎥⎦ ⎢ I yy − I xx p ( t ) q ( t ) / I zz
⎢⎣
⎥
⎥⎦
• For motion about the y axis q! ( t ) = M y ( t ) / I yy
only, this reduces to
• Pitch angle equation θ! ( t ) = q ( t )
25
25
26
13
2/12/20
27
27
⎡ −⎜ A ⎟ t ⎤
⎛ Cg ⎞
⎢ ⎥ ⎣ ⎦
⎣ ⎦
• where
– λ = –CgA/Iyy = eigenvalue or
root of the system (rad/s)
– τ = Iyy/CgA = time constant of
the response (s)
28
28
14
2/12/20
29
30
30
15
2/12/20
⎡ θ!(t) ⎤ ⎡ 0 1 ⎤ ⎡ θ (t) ⎤ ⎡ 0 ⎤
⎢ ⎥=⎢ ⎥⎢ ⎥+⎢ ⎥ C [θ c (t) − θ (t)]
!
⎢⎣ q(t) ⎥⎦ ⎣ 0 0 ⎦ ⎢⎣ q(t) ⎥⎦ ⎢⎣ gA / I yy ⎥⎦
⎡ θ!(t) ⎤ ⎡ 0 1 ⎤ ⎡ θ (t) ⎤ ⎡ 0 ⎤
⎢ ⎥=⎢ ⎥⎢ ⎥+⎢ ⎥θ c
!
⎢⎣ q(t) ⎥⎦ ⎢⎣ −CgA / I yy 0 ⎥ ⎢ q(t)
⎦⎣ ⎥⎦ ⎢⎣ CgA / I yy ⎥⎦
31
31
32
16
2/12/20
⎡ θ!(t) ⎤ ⎡ 0 1 ⎤ ⎡ θ (t) ⎤ ⎡ 0 ⎤
⎢ ⎥=⎢ ⎥⎢ ⎥+⎢ ⎥θ c
!
⎢⎣ q(t) ⎥⎦ ⎢⎣ −c1gA / I yy −c2 gA / I yy ⎥ ⎢ q(t) ⎥ ⎢ c1gA / I yy
⎦⎣ ⎦ ⎣ ⎥⎦
33
33
c1gA /Iyy = 1
c2gA /Iyy = 0, 1.414, 2.828
34
34
17
2/12/20
2nd-Order Dynamics
Oscillation and damping are induced by linear
feedback control
⎡ θ!(t) ⎤ ⎡ 0 1 ⎤ ⎡ θ (t) ⎤ ⎡ 0 ⎤
⎢ ⎥=⎢ ⎥⎢ ⎥+⎢ ⎥θ c
!
⎢⎣ q(t) ⎥⎦ ⎢⎣ −c1gA / I yy −c2 gA / I yy ⎥ ⎢ q(t)
⎦⎣ ⎥⎦ ⎢⎣ c1gA / I yy ⎥⎦
⎡ θ!(t) ⎤ ⎡ 0 1 ⎤ ⎡ θ (t) ⎤ ⎡ 0 ⎤
⎢ ⎥=⎢ ⎥⎢ ⎥+⎢ 2 ⎥θ c
⎥⎦ ⎢⎣ −ω n −2ζω n ⎥ ⎢ q(t) ⎥⎦ ⎢⎣ ω n ⎥⎦
2
!
⎢⎣ q(t) ⎦⎣
Natural frequency and damping ratio
ω n = c1gA / I yy
( )
ζ = c2 gA / I yy / 2ω n = c2 / 2 c 1 gA I yy
35
35
-0.4143 Overdamped
-2.4137
36
36
18
2/12/20
37
37
38
38
19
2/12/20
39
39
Adj ( sI − F )
x(s) = G u(s)
Δ(s)
Applied to the closed-loop system
⎡ c1gA I yy ⎤ ⎡ c1gA I yy ⎤
⎢ ⎥ ⎢ ⎥ Δu(s)
⎡ Δθ (s) ⎤ ⎢ sc1gA / I yy ⎥ ⎢ sc1gA / I yy ⎥
⎣ ⎦ Δu(s) = ⎣ ⎦
⎢ ⎥=
⎢⎣ Δq(s) ⎥
⎦ Δ(s) 2
( )
( s ) + c2 gA I yy ( s ) + c1gA I yy
40
40
20
2/12/20
Δθ ( jω ) ωn 2
=
Δu( jω ) ( jω ) 2 + 2ζω n ( jω ) + ω n 2
Δq( jω )
=
( jω ) ω n 2
Δu( jω ) ( jω ) + 2ζω n ( jω ) + ω n 2
2
• Bode plot
– 20 log(Amplitude Ratio) [dB] vs. log ω
– Phase angle (deg) vs. log ω
41
41
Proportional-Integral-
Derivative (PID) Controller
42
21
2/12/20
Proportional-Integral-
Derivative (PID) Controller
Control Law Transfer Function:
43
Proportional-Integral-Derivative
(PID) Controller
Forward-Loop Angle θ (s) ⎡ cI + cP s + cD s ⎤ ⎡ gA ⎤
2
= ⎥ ⎢ I s2 ⎥
Transfer Function: e(s) ⎢⎣ s ⎦ ⎣ yy ⎦
44
44
22
2/12/20
θ (s) ⎡ c I + cP s + c D s 2 ⎤
⎢ gA ⎥
θ (s) e(s) ⎣ I yy s 3 ⎦
= =
Closed-Loop Angle θ c (s) 1+ θ (s) ⎡ c I + cP s + c D s 2 ⎤
Transfer Function: e(s) 1+ ⎢ I yy s 3
gA ⎥
⎣ ⎦
c I + cP s + c D s 2
=
cI + cP s + cD s 2 + gA / I yy s 3 45
45
Closed-Loop Frequency
Response w/PID Control
θ (s) c I + cP s + c D s 2
=
θ c (s) cI + cP s + cD s 2 + gA / I yy s 3
As ω -> ∞
θ ( jω ) −cDω 2 c jc
→ g = D gA = − D gA High-frequency
θ c ( jω ) − j I yyω 3 A
j I yyω I yyω response “rolls off”
cD and lags input
AR → g ; φ → −90 deg
I yyω A 46
46
23
2/12/20
47
47
48
48
24
2/12/20
On/Off-Torque
Controllers
49
49
⎡ θ!(t) ⎤ ⎡ 0 1 ⎤ ⎡ θ (t) ⎤ ⎡ 0 ⎤ u (t ) =
⎢ ⎥=⎢ ⎥ ⎢ q(t) ⎥ + ⎢ g / I ⎥ u(t) +1, 0, or − 1
!
⎢⎣ q(t) ⎥⎦ ⎣ 0 0 ⎦ ⎢⎣ ⎥⎦ ⎣⎢ A yy ⎥⎦
What is the time evolution of the state while a thruster is
on [u(t) = 1]?
( )
q(t) = gA / I yy t + q(0)
( )
θ (t) = gA / I yy t 2 / 2 + q(0)t + θ (0)
Neglecting initial conditions, what does
the phase-plane plot look like? 50
50
25
2/12/20
Constant-Thrust
(Acceleration) Trajectories
For u = 1, For u = –1,
Acceleration = gA/Iyy Acceleration = –gA/Iyy
With zero thrust, what does the phase-plane plot look like?
51
51
52
26
2/12/20
Switching-Curve Control
Law for On-Off Thrusters
53
54
54
27
2/12/20
55
55
56
28
2/12/20
57
57
58
29
2/12/20
59
59
Next Time:
Sensors and Actuators
60
60
30
2/12/20
Supplemental Material
61
61
62
62
31