Basic Gimbal Control Using Matlab Steve Rogers 28 June 2012
1
Gimbal Control Design • • • • •
Gimbal model Feedforward (mathworks) Integral control using root locus (mathworks) LQR (mathworks) PIDtune & sisotool verification for PI & PID
2
Denavit Hartenburg Gimbal Model
Θ1 & θ2 are the pan & tilt angles respectively. Θ2 = cos-1((z-d1)/d3) Θ1 = tan-1(y/x)
3
differential equations defining a permanent magnet DC motor
4
Open-Loop DC Motor Step Rate Response Step Response 0.25 System: untitled1 Settling time (seconds): 1.07 System: untitled1 Rise time (seconds): 0.598
0.2
Amplitude
0.15
0.1
0.05
0
0
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
Time (seconds)
5
Motor Step Rate Response With Disturbance Setpoint tracking and disturbance rejection 1.2 cl_ff 1
Disturbance
0.6
To: w
Amplitude
0.8
0.4
disturbance T = -0.1Nm d
0.2
0
-0.2
0
5
10
15
Time (seconds)
6
Root Locus Integral design for Disturbance Rejection Root Locus 15
Setpoint tracking and disturbance rejection 1.6 feedforward feedback w/ rlocus
1.4 System: untitled1 Gain: 4.99 Pole: -1.66 - 1.33i Damping: 0.782 Overshoot (%): 1.95 Frequency (rad/s): 2.13
0
1.2 1
-5
To: w
5
Amplitude
Imaginary Axis (seconds-1)
10
0.8 0.6 0.4
-10
0.2 -15 -15
0 -10
-5 Real Axis (seconds -1)
0
5
-0.2
0
5
10
15
Time (seconds)
Integrator design
K = 5; C = tf(K,[1 0]);
% compensator K/s 7
LQR Design for Disturbance Rejection Setpoint tracking and disturbance rejection 1.6
• LQR Control Design To further improve performance, a linear quadratic regulator (LQR) for the feedback structure is designed. • In addition to the integral of error, the LQR scheme also uses the state vector x=(i,w) to synthesize the driving voltage Va. • The resulting voltage is of the form Va = K1 * w + K2 * w/s + K3 * i where i is the armature current. • For better disturbance rejection, use a cost function that penalizes large integral error.
feedforward feedback (rlocus)
1.4
feedback (LQR) 1.2
To: w
Amplitude
1 0.8 0.6 0.4 0.2 0 -0.2
0
5
10
15
Time (seconds)
8
PID Design for Disturbance Rejection Using pidtune Root Locus Editor for Open Loop 1(OL1)
Open-Loop Bode Editor for Open Loop 1(OL1) 20
80 0.85
0.74
0.62 0.48 0.320.16
• pidtune tunes the parameters of the PID controller C to balance performance (response time) and robustness (stability margins). • Phase Margin shows stability. • Rise time is much faster now. • Overshoot is probably excessive @ ~20%.
0
60 0.93
-20 40 0.98
-40
20
140 120 100 0
80
60
40
-60 G.M.: inf Freq: NaN Stable loop -80 0
20
System: Closed Loop r to y I/O: r to y Step Response Peak amplitude: 0.596 Overshoot (%): 19.3 At time (seconds): 0.284
0.7
0.6
-20 -45
0.98
0.5
-40
-60 0.93
-135 0.85
-80 -150
Amplitude
-90
0.74
P.M.: 120 deg Freq: 6.4 rad/s
0.62 0.48 0.320.16 -50
Real Axis
0
-2
10
0.3
0.2
-180 -100
System: Closed Loop r to y I/O: r to y Rise time (seconds): 0.129
0.4
0
2
10 10 Frequency (rad/s)
4
10
0.1
0
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
Time (seconds)
9
1
PID Design for Disturbance Rejection Using pidtune Setpoint tracking and disturbance rejection
• pidtune tunes the parameters of the PI & PID controllers C to balance performance (response time) and robustness (stability margins). • Comparison with other controllers shows less magnitude impact and quicker recovery from disturbance.
1.6 feedforward feedback (rlocus)
1.4
feedback (LQR) pidtune (PI) (PID)
1.2
To: w
Amplitude
1 0.8 0.6 0.4 0.2 0 -0.2
0
5
10
15
Time (seconds)
10
Bode Plots Bode Diagram From: w ref
From: Td
50
To: w
feedforward feedback (rlocus)
-50
feedback (LQR) pidtune (PI) (PID)
-100
-150 90 0
To: w
Magnitude (dB) ; Phase (deg)
0
-90 -180 -270 -2 10
0
10
2
10
-2
10
0
10
2
10
Frequency (rad/s)
11