1
Lab 3:
Quanser Hardware and Proportional Control
ME 134/EE 128
Mandy Huo, Aldrich Ong, Deepak Talwar
Group #4
Lab performed on: 9/24/13
Lab due on: 10/1/13
2
Purpose
The objective of this lab is to lear how to use Quanser’s QuaRC tools and the Q4 Data
Acquisition board. We explored the dynamics of a cart driven by a DC motor on a track with the
ultimate goal of balancing an inverted pendulum with the cart. In the Pre-lab we derived a model
for the dynamics of the cart, which was tested with an actual cart system in the lab. We
calibrated our proportional controller and generated a step response on the actual hardware.
Prelab
Part A – Equations governing the cart dynamics
Figure 1: Free body diagram of the cart (neglecting friction)
For the cart system of Figure 1 we derive the equation of motion relating the input voltage V at
the motor to the motion of the cart.
First we apply Newton’s second law to the free body diagram in Figure 1 to get (ignoring
friction)
𝐹𝑎 = 𝑚𝑐�̈� (1)
where 𝐹𝑎 is force applied to the cart and 𝑚𝑐 is the mass of the cart.
Next we look at the motor dynamics to derive the relation between the input voltage to the motor
and the applied force on the cart. The circuit diagram is shown below in Figure 2. In this
derivation we assume the motor resistance 𝑅𝑚 is much larger than the motor inductance 𝐿𝑚 so
we can neglect 𝐿𝑚. We also assume the motor and gearbox have perfect efficiency.
Figure 2: Circuit diagram of the DC motor.
3
The current 𝐼𝑚 flowing through the motor windings is directly proportional to the torque 𝑇𝑚
generated by the motor but the motor inertia 𝐽𝑚 causes a counter torque. The torque equation is:
𝑇𝑚 = 𝐾𝑡𝐼𝑚 − 𝐽𝑚�̈� (2)
where 𝐾𝑡 is the motor torque constant, and �̈� is the acceleration of the motor.
To find the relation of current flow to voltage input, we sum voltages on the circuit diagram of
Figure 2:
𝑉 = 𝐼𝑚𝑅𝑚 + 𝐸𝑒𝑚𝑓 = 𝐼𝑚𝑅𝑚 + 𝐾𝑚�̇� (3)
where 𝐾𝑚 is the back EMF constant and �̇� is the angular velocity of the motor.
The relation between the motor torque and the force applied to the cart is
𝐾𝑔𝑇𝑚 = 𝐹𝑎𝑟 (3)
where 𝐾𝑔 is the gear ratio of the gearbox and 𝑟 is the radius of the motor gear.
The angular velocity of the motor is related to the linear velocity of the cart by
𝐾𝑔�̇� = 𝑟�̇� (4)
Taking the derivative,
𝐾𝑔�̈� = 𝑟�̈� (5)
Solving (3) for 𝐹𝑎 and combining with (2) to eliminate 𝑇𝑚 we get
𝐹𝑎 =𝐾𝑔
𝑅(𝐾𝑡𝐼𝑚 − 𝐽𝑚�̈�)
Then we eliminate 𝐼𝑚 using (3), and eliminate �̇� and �̈� using (4) and (5) respectively to get
𝑚𝑐�̈� = 𝐹𝑎 =𝐾𝑔
𝑅(
𝐾𝑡
𝑅𝑚𝑉 −
𝐾𝑡𝐾𝑚𝐾𝑔
𝑅𝑚𝑟�̇� −
𝐽𝑚𝐾𝑔
𝑟�̈�)
Rearranging,
(𝑚𝑐𝑟2𝑅𝑚 + 𝐽𝑚𝑅𝑚𝐾𝑔2)�̈� + (𝐾𝑡𝐾𝑚𝐾𝑔
2)�̇� = (𝑟𝐾𝑡𝐾𝑔)𝑉 (6)
Note this model of the system is linear.
Part B – Deriving the system model
Transfer function: To find the transfer function from the input voltage V and the cart
position X, we take the Laplace transform of (6), assuming zero initial conditions:
𝐻(𝑠) =𝑋(𝑠)
𝑉(𝑠)=
𝑟𝐾𝑔𝐾𝑡
(𝑚𝑐𝑟2𝑅𝑚 + 𝐽𝑚𝑅𝑚𝐾𝑔2)𝑠2 + (𝐾𝑡𝐾𝑚𝐾𝑔
2)𝑠
(7)
4
State-space model: Letting 𝑥1 = 𝑥, 𝑥2 = �̇�, 𝑦 = 𝑥, and 𝑢 = 𝑉 we obtain the state-space
representation of the cart system:
[�̇�1
�̇�2] = [
0 1
0 −𝐾𝑡𝐾𝑚𝐾𝑔
2
𝑚𝑐𝑟2𝑅𝑚 + 𝐽𝑚𝑅𝑚𝐾𝑔2
] [𝑥1
𝑥2] + [
0𝑟𝐾𝑔𝐾𝑡
𝑚𝑐𝑟2𝑅𝑚 + 𝐽𝑚𝑅𝑚𝐾𝑔2
] 𝑢
𝑦 = [1 0] [𝑥1
𝑥2]
State-space to Transfer function: We find the transfer function of the system
using the state-space matrices from (7) and the transfer function matrix relating
the output vector Y(s) to the input vector U(s):
(8)
𝐺(𝑠) =𝑌(𝑠)
𝑈(𝑠)= 𝐶(𝑠𝐼 − 𝐴)−1𝐵 + 𝐷 =
𝑟𝐾𝑔𝐾𝑡
(𝑚𝑐𝑟2𝑅𝑚 + 𝐽𝑚𝑅𝑚𝐾𝑔2)𝑠2 + (𝐾𝑡𝐾𝑚𝐾𝑔
2)𝑠
(9)
Note that this is the same result as derived in (7) since 𝑌(𝑠) = 𝑋(𝑠) and 𝑈(𝑠) = 𝑉(𝑠).
Part C – MATLAB Step Response
The values of all parameters used in this lab are listed below in Table 1.
Parameter Description Value
𝑚𝑐 cart mass 0.94 kg
𝑟 radius of motor gears 6.36 × 10-3 m
𝑅𝑚 resistance of motor windings 2.6 Ω
𝐾𝑡 motor torque constant 7.67 × 10-3 N⋅m/A
𝐾𝑚 back EMF constant 7.67 × 10-3 V⋅s/rad
𝐾𝑔 gearbox ratio 3.71
𝐽𝑚 moment of inertia of the motor 3.9 × 10-7 kg⋅m2
Table 1: Cart system parameters.
Using these values we created a Simulink block diagram of a simple proportional negative
feedback controller for the cart system with controller gain K. the Simulink model is shown
below in Figure 3.
5
Figure 3: cart_Kcontrol.mdl - negative feedback proportional control of cart system.
We searched for a value of K for which the step response has a maximum percent overshoot <
4.5% and rise time 𝑡𝑟 < 0.5 s. First we tested the system with several values of K. As K
decreases, the overshoot and rise time decreases as well. We wrote a MATLAB routine (see code
in Appendix A) to search for the largest value of K which satisfies the design conditions. The
result was K = 16.2. The plot of the step response (input of x = 1 m) is plotted below in Figure 4
for verification.
Figure 4: Step response of cart for controller gain K = 16.2 and input distance x = 1 m.
6
Lab
Part A - Using the Actual Hardware: Find Encoder-Distance Conversion
In this section of the lab we will interface the computer with the Quanser Hardware so that we
can collect data from it. The encoders on the cart output data in encoder count units. We would
need to convert these units into centimeters using a conversion factor which we will calculate in
this section of the lab.
First, we build our Simulink block diagram which will only be used for reading position encoder
values. This block diagram is shown in Figure 5.
Figure 5: Simulink block diagram used for observing distance encoder values.
This block diagram uses the HIL Initialize block, which is used to interface and establish a
connection between Simulink and the Quanser board. We set the board type value to q4, which is
the type of boards available in the lab. The Quanser board provides multiple input and output
channels, however, for the purpose of this section, we would only need to observe input values
from channel 0, which is connected to the reading position encoder on the cart. For this purpose,
we use a HIL Read Encoder block with only the channels #0 and #1 selected, and connect the
channel 0 to “simout” and the “Scope” blocks. The “simout” block was used to store data in the
form of arrays for further manipulation. We used the “Scope” block to see the real time change
in encoder values when the cart was moved.
We built the code for the block diagram by clicking on the QuaRC/ Build command. This
command converts the block diagram into C code which the Quanser board can understand.
Thus, we need to rebuild the code for the block diagram every time we make a change to it.
For observing the output through the scope, we set the Simulation mode to External and the final
time in Simulink to infinity “inf”. After rebuilding the code, and running the simulation (which
calls on the HIL Initialize block and sets up a connection between Simulink and Quanser board)
we could observe the encoder values in encoder count units on the Scope.
7
Since scoping wouldn’t give us accurate starting and end values, we decided to use a simout
block instead and plot the graph for encoder values v/s time for a specific length moved. Using
the length of a letter sized sheet (11 in) as reference, we moved the cart in a simulation of 5
seconds twice and recorded the output values as a vector. The plot of one of the trials is shown in
Figure 6.
We used 10,000 data points in the “Limit data points to last” field in order to get a high
resolution result. After performing two trials, we averaged the difference between starting
position and ending position of the encoder, and converted the value into the correct units
(counts/m) as shown below:
1. We calculated the difference between the starting and ending positions using y(end) –
y(1) in MATLAB, where y is one of the trials.
2. We averaged the differences over the number of trials to get a single value counts.
3. This value is then converted from units (counts/11 inches) to (counts/m) by dividing
counts by (11*0.0254) and is bound to the variable, c_m.
4. Since the radius of the position pinion is known to us (r_pp = 0.01482975 m), we
convert c_m to c_rev (counts/revolution) by multiplying c_m with 2*pi*r_pp.
Figure 6: Encoder values for moving the cart for 11 inches (0.2794 m)
8
After performing all the calculations described above, we got c_rev as the following:
c_rev = 4.0716e+003 counts/revolution
c_cm = 436.9721 counts/cm
As we can see, our c_rev value is really close (less than 1% error) to the actual encoder
resolution of 4096 counts/revolution as reported by the Quanser manual. Complete MATLAB
Code for these calculations and the plot are given in the Appendix.
9
Part B - Using the Actual Hardware: Cart Response
The proportional gain constant K=16.2 determined in Pre-lab section was for a step input
of 1m. If step function is change to a height of 0.15, which corresponds to 15cm, then the
value of K does not change. The response of the system do not depend on the value of the
step input but it does depend on the plant dynamics and the controller gain.
In Figure 7, a modified version of the Simulink model in the Pre-lab is shown. The
transfer function is replaced with the HIL Write Analog block, which will basically be
the plant of the system. The output voltage will be sent to the motor of the cart. In
between the controller and the HIL Write Analog block is a saturation block which is set
to 6V. The feedback of the system comes from the HIL Read Encoder. As seen in the
first part of the lab, the HIL Read Encoder outputs the displacement of the cart in counts,
which need to be converted to meters. The gain 1/c_m is the conversion factor. The
output of this gain is then subtracted to the reference input, hence the negative feedback.
Figure 7: cart_neg_feedback.mdl – Simulink model to implement negative feedback for the
physical cart.
10
The plot shown in Figure 8 shows for the response of the cart with K=16.2 for an input reference
of 0.15m. The steady state value of the cart is 0.1421m, and the steady state error is 0.0079m.
Since the physical system is not ideal, such that there is unaccounted friction and backlash, a
steady state error exist. Theoretically, if we created a very large gain, the steady state error would
approach zero. Since the controller is limited to an output of 6V, a large gain would be
ineffective.
Using the function stepinfo in MATLAB, the maximum overshoot and the rise time of the
actual response was determined and presented in the table below. The characteristics of the
theoretical response of the cart are also shown for comparison.
Cart Response for K=16.2
Actual Theoretical
Rise Time (s) 0.4028 0.4195
Overshoot (%) 0.0241 4.4401
Clearly there is a large discrepancy in the maximum overshoot percentage. The actual response
barely had any overshoot. In the theoretical model, we ignored damping and friction effects on
the motor. In reality, these effects can influence the response as shown in the actual response of
the system.
Figure 8: Plot of the cart response for K=16.2. Input reference of 0.15m is
shown to clarify the steady state error.
Table 2: Comparing the actual and theoretical response of the cart for a proportional gain K = 16.2
11
To obtain a percent maximum overshoot of < 4.5%, such that the overshoot is as close to 4.5, we
increased the gain. For K = 29.3, the response of the system is shown in Figure 9.
The maximum overshoot percentage for a proportional gain of K = 29.3 is 3.0549% of its steady
state output. A larger K value will output a larger voltage to the motor, and the motor will shoot
past the input reference and as a result have certain amount of overshoot. In this case the control
tries to bring the cart back to the input reference but due to unaccounted factors listed above
there is still steady state error.
From our theoretical model, increasing the value of K will decrease the damping ratio and as a
result have more oscillations. It is shown in equation 10 that the transfer function for the negative
feedback loop is.
𝐺(𝑠) =𝑌(𝑠)
𝑈(𝑠)=
𝐾 ∗ 𝑟𝐾𝑔𝐾𝑡
(𝑚𝑐𝑟2𝑅𝑚 + 𝐽𝑚𝑅𝑚𝐾𝑔2)𝑠2 + (𝐾𝑡𝐾𝑚𝐾𝑔
2)𝑠 + 𝐾 ∗ 𝑟𝐾𝑔𝐾𝑡
(10)
Figure 9: Actual cart response for K=29.3. Input reference is plotted to clarify the steady state
error.
12
From the transfer function, the damping ratio can be determined.
𝜁 =𝐾𝑡𝐾𝑚𝐾𝑔
2
2√𝐾 ∗ 𝑟𝐾𝑔𝐾𝑡
(11)
Equation 11 shows that if we want the system to overshoot, the proportional gain constant needs
to increase so that the damping ratio decrease and the system becomes under damped. Even
though the theoretical model doesn’t match the actual system, it is still reasonable to use the
model to figure out how the parameters will affect the system.
13
Appendix
A: findK.m – Finds value of K for which the step response to the cart system (without actual
hardware) has maximum overshoot less than 4.5% and rise time less than 0.5 s, then plots
response with this K. Uses cart_Kcontrol.mdl.
% Parameters
m = 0.94; % mass of cart [kg]
r = 6.36e-3; % radius of motor gear [m]
R = 2.6; % resistance of motor [ohms]
Kt = 7.67e-3; % motor torque constant [Nm/A]
Km = 7.67e-3; % back EMF constant [Vs/rad]
Kg = 3.71; % gearbox gear ratio
J = 3.9e-7; % moment of inertia of motor [kgm^2]
% test system for single K
% K = 20; % gain
% tspan = [0 5];
% sim('cart_neg_feedback',tspan)
% x = xout.Data;
% t = xout.Time;
% plot(t,x)
% Find K for which max overshoot < 4.5% and rise time < 0.5 s
k = 1:.1:20; % K values to test
k_rev = fliplr(k); % start from K = 20
for i = 1:length(k)
K = k_rev(i);
tspan = [0 5];
sim('cart_Kcontrol',tspan)
x = xout.Data;
t = xout.Time;
% Calculate max overshoot
e = max(x - 1);
% Calculate rise time
index1 = find(x>=0.1,1); % index at x ~ 0.1
index2 = find(x>=0.9,1); % index at x ~ 0.9
t_r = t(index2) - t(index1); % rise time
% Verify if performance criteria satisfied. If so, stop and print
K.
if(e < 0.045 && t_r < 0.5)
K;
break
end
end
% Plot step response
plot(t, x,'k')
xlabel('time [sec]')
ylabel('x [m]')
title(sprintf('Position of cart over time, K = %g',K))
14
% annotations
hold on
x_ss = 1;
x_os = 1.045;
plot([0 5],[x_os x_os],'--b', [0 5],[x_ss x_ss],':g')
legend('Step response','Overshoot criteria','Steady-state')
t1 = t(index1); % time at 0.1% of steady state
t2 = t(index2); % time at 0.9% of steady state
x1 = x(index1); % position at 0.1% of steady state
x2 = x(index2); % position at 0.9% of steady state
plot([0 t1],[x1 x1],':r',[t1 t1],[0 x1],':r')
plot([0 t2],[x2 x2],':r',[t2 t2],[0 x2],':r')
hold off
annotation('doublearrow',[0.145 0.21],[0.15 0.15],'Head1Width',7,...
'Head1Length',4,'Head2Width',7,'Head2Length',4)
tbox1 = annotation('textbox',[.135 .15 .2 .05],'LineStyle','none');
set(tbox1, 'String','Rise time');
tbox2 = annotation('textbox',[.45 .4 .4 .10]);
set(tbox2, 'String', sprintf(['Rise time = %g s',...
'\nMaximum overshoot = %g m'],t_r, e));
% Vector with encoder position values. Performed twice. y_1 = simout.signals.values;
%making the difference a positive value
y_1 = -y1; y_2 = -y2; % Plotting encoder counts v/s time t = linspace(0,5,length(y1)); hold on plot(t, y_1, 'LineWidth', 2) plot(t, y_1(end), '--') hold off title('Encoder counts v/s Time') xlabel('Time(s)') ylabel('Encoder counts') text(3.5, 13e3,sprintf('Final Value: %1.0f', y_1(end)));
% Calculating the conversion factor (counts/rev) r_pp = 0.01482975; counts =(abs(y_1(end))+ abs(y_2(end)))/2; % counts (average) per 11 inches c_m = counts/(11*0.0254); % counts/meter c_cm = counts/(11*2.54); % counts/cm c_rev = c_m*pi*2*r_pp; % counts/rev
B: part1.m - Calculation of the encoder conversion factor
15
C: part2.m – Plotting the response for the proportional gain constant K=16.2 and for K=29.3.
System response was determined by observing the response plot and using stepinfo function
in Matlab.
% Part 2- Using the Actual Hardware: Cart Response load('partb_data.mat') % load workspace for data obtained in lab
% Plotting response for K = 16.2 t = t_16_2; y = y_16_2;
U = 0.15*ones(size(t));
figure(1) plot(t,y,t,U,'r--') legend('Output response','Input Response','location','SouthEast') xlabel('Time (s)') ylabel('Position (m)') title('Cart response for K=16.2')
stepinfo(y,t)
% Plot response for K = 29.3 t = t_29_3; y = y_29_3;
U = 0.15*ones(size(t));
figure(2) plot(t,y,t,U,'r--') legend('Output response','Input Response','location','SouthEast') xlabel('Time (s)') ylabel('Position (m)') title('Cart response for K=29.3')
stepinfo(y,t)
1
Lab 4
Model-Based Position Control of a Cart
ME 134/EE 128
Lab Group #4 Tuesday 4-6pm
Mandy Huo, Aldrich Ong, and Deepak Talwar
Lab performed on: 10/7/13
Lab due on: 10/15/13
2
1. Purpose
The goal of this lab is to help understand the methodology to design a controller using the given
plant (cart) dynamics. This is done by using position control on the cart by developing various
controllers and then comparing their performance against various inputs. In the Pre-Lab, we set
out by defining our performance specifications, and then tried achieving those specifications
using two different kinds of controllers: proportional and PD.
We will be dealing with the same plant as derived in Lab 3. The ODE defining the plant is given
by Equation 1:
(𝑚𝑐𝑟2𝑅𝑚 + 𝑅𝑚𝐾𝑔2𝐽𝑚)�̈� + (𝐾𝑡𝐾𝑚𝐾𝑔
2 )�̇� = (𝑟𝐾𝑡𝐾𝑔)𝑉 (1)
Where the parameters are described in Figure 1:
For a generalized second order system, the maximum overshoot can be determined analytically
and is given by Equation 2.
𝑀𝑝 = exp(−𝜋𝜁
√1 − 𝜁2) (2)
There is no explicit formula for the rise time as a function of parameter ωn and ξ. We can use the
third order approximation given in Equation 3.
ωntr ≈ 1.76 ξ 3 – 0.417 ξ 2 + 1.039 ξ + 1 (3)
Parameter Description Value
𝑚𝑐 cart mass 0.94 kg
𝑟 radius of motor gears 6.36 × 10-3 m
𝑅𝑚 resistance of motor windings 2.6 Ω
𝐾𝑡 motor torque constant 7.67 × 10-3 N⋅m/A
𝐾𝑚 back EMF constant 7.67 × 10-3 V⋅s/rad
𝐾𝑔 gearbox ratio 3.71
𝐽𝑚 moment of inertia of the motor 3.9 × 10-7 kg⋅m2
Figure 1- Parameters of the cart system
3
2. Pre-lab
2.1 Position Controller Design
Here, we will define our performance objectives to be achieved by the feedback system for the
cart position control for a step amplitude of 15 cm:
1. Rise time tr ≤ 0.20s
2. Maximum overshoot Mp ≤ 5%
We need to achieve these performance objectives using a feedback control system.
Question: Will the amplitude of the input affect rise time and overshoot? Explain.
Answer: No the amplitude of the input will not affect rise time and overshoot of the response. As
given in the formulae (3) and (4), we can see that the rise time and overshoot solely depend on
ωn and ξ. These two values are intrinsic properties of the system. These do not change depending
on the input to the system. The amplitude of the input only affects the steady state value of the
step response.
2.1.1 Plant Model
The state space representation for the system is given in Equation (4). Letting 𝑥1 = 𝑥, 𝑥2 = �̇�,
𝑦 = 𝑥, and 𝑢 = 𝑉 we obtain the state-space representation of the cart system:
[�̇�1
�̇�2] = [
0 1
0 −𝐾𝑡𝐾𝑚𝐾𝑔
2
𝑚𝑐𝑟2𝑅𝑚 + 𝐽𝑚𝑅𝑚𝐾𝑔2
] [𝑥1
𝑥2] + [
0𝑟𝐾𝑔𝐾𝑡
𝑚𝑐𝑟2𝑅𝑚 + 𝐽𝑚𝑅𝑚𝐾𝑔2
] 𝑢
𝑦 = [1 0] [𝑥1
𝑥2]
(4)
This state space representation of the system is then loaded in MATLAB and the poles of the
system are found using the pole function. Since one of the poles of the system is 0, which lies
on the imaginary axis, the system is marginally stable.
Using Equation (2) and the given condition, we know,
Mp ≤ 0.05
ln (0.05) ≤ −ᴨ𝜉
√1−𝜉2
On solving, we get,
ξ ≥ 0.6901 (5)
Using Equations (3) and (5) and the given condition, we get,
(0.2)ωn ≈ 1.76 (0.6901)3 – 0.417 (0.6901)2 + 1.039 (0.6901) + 1
4
On solving, we get,
ωn ≥ 10.4843 (6)
The MATLAB code for these calculations is given in the appendix under Pre-lab 2.1.1.
2.1.2 Proportional Control
This is the first type of controller that we will use to stabilize the system. The Simulink diagram
for the system is given in Figure 2. Here the system output is in meters and the system input is
the reference.
The transfer function used in the plant is derived in Lab 2 and given in Equation 7.
𝐻(𝑠) =𝑋(𝑠)
𝑉(𝑠)=
𝑟𝐾𝑔𝐾𝑡
(𝑚𝑐𝑟2𝑅𝑚 + 𝐽𝑚𝑅𝑚𝐾𝑔2)𝑠2 + (𝐾𝑡𝐾𝑚𝐾𝑔
2)𝑠 (7)
Figure 2- “cart_model_tf.mdl”- Simulink Block diagram for the proportional control feedback
system
5
Now, to study the response of the proportional feedback system, we simulate the block diagram
with different values of K. Using K = 10, 20, 30, 40 and 50, we simulate the model to a step
input of u = 0.15 m and plot the step responses. This plot is given in Figure 3.
We can clearly see from Figure 3, that as K increases, the overshoot increases but the rise time
decreases. Figure 4 includes the values of overshoots and rise times corresponding to the step
responses shown in Figure 3.
From Figure 3 and values in Figure 4, it is
safe to say that with a proportional
controller only, both specifications cannot
be met simultaneously. We can see that
with K = 10, overshoot percentage is well
under 5% and that with K=40 and K=50,
the rise time is under 0.20 seconds.
However, there exists no such value of K
where both conditions are met.
Figure 4: Rise time and Overshoot values for
given K values
K Rise Time (seconds) Overshoot (%)
10 0.7153 0.1754
20 0.3422 7.6421
30 0.2410 14.9785
40 0.1926 20.7217
50 0.1635 25.3188
Figure 3: Proportional Control with K values between 10 and 50
6
Now, we will find the smallest integer value of K for with the rise time performance
specification is met. Looking at values in the second column of Figure 4, we can say that the
value of K will be somewhere between 30 and 40. Thus, we run the simulations with integer
values of K between 30 and 40 and look for the smallest value for with the rise time will be less
than 0.20 seconds. The MATLAB code for doing this is given in the appendix under Pre-lab
2.1.2. The result of the simulation is K = 39. We then simulate the block diagram with K=39; the
resulting plot is given in Figure 5.
Clearly, from Figure 5, we can see that for K = 39, only the rise time performance objective is
met. The overshoot is well above the required 5% objective. With all the analysis, it is safe to say
that with a proportional control alone, it is not possible to achieve both performance objectives.
The same analysis can be done by observing the root locus for the system. The root locus of the
plant can be plot using the rlocus function on the plant transfer function or the state space
representation. The root locus of the state space representation of the system given in Equation 4
is given by Figure 6.
Figure 5: Proportional Control with K = 39, minimum value of K required to meet rise time
specification
7
From the root locus plot, we can see that as K increases, the radial distance, which is measured
from the origin, increases. The angle θ, which is measured from the vertical axis to the root locus
remains constant for all the poles on the real axis. However, for complex poles, as K increases,
the angle θ decreases.
We know that the radial distance from the origin to the poles is equal to the magnitude of the
complex number that defines that pole, which in turn is equal to ωn, the critical frequency. Thus,
we can say that ωn continues to increase as K increases.
We know that the angle θ = sin-1 ξ, that is, ξ = sin(θ). Thus, we can see that for all poles on the
real axis, θ = 90o, so the system is either critically damped or over damped. However, at complex
poles (or higher values of K), θ varies from 90o to 0o, thus the value of sin(θ) (and ξ) goes from 1
to 0 which correspond to the under-damped cases. This is in direct correlation with our findings
in the previous analysis and as shown in Figure 3. To summarize, we can say that as K increases,
the critical frequency ωn increases; the damping ratio, ξ remains constant initially and then starts
to decrease as the underdamped cases are reached.
Figure 6: Root locus plot for the plant (Equation 4)
8
2.1.3 PD Controller
Implementing a PD controller will allow the desired specifications to be achieved. A PD
controller can be obtained by adding a zero at 𝑠 = −𝐾𝑝
𝐾𝐷 into the plant dynamics. Introducing a
pure zero, on the other hand, such that 𝑠 = 0 is a zero, is a bad idea due to noise becoming more
dominate as a signal to the plant. Since noise signals have high frequency, the derivative of it
will be large and the system will act as a high pass filter.
Now we add a PD controller by adding a pole at 𝑠 = −200 and zero at 𝑠 = −𝐾𝑝
𝐾𝐷. The closed
loop PD control transfer function for the plant shown in Equation 7 is shown in Equation 8.
Let
𝐴 = 𝑟𝐾𝑔𝐾𝑡
𝐵 = 𝑚𝑐𝑟2𝑅𝑚 + 𝐽𝑚𝑅𝑚𝐾𝑔2
𝐶 = 𝐾𝑡𝐾𝑚𝐾𝑔2
𝑎 =𝐾𝑃
𝐾𝐷
𝐾 = 𝐾𝐷
𝑇(𝑠) =𝑋(𝑠)
𝑋𝑟𝑒𝑓(𝑠)=
𝐴𝐾𝑠 + 𝐾𝐴𝑎
𝐵200 𝑠3 + (𝐵 +
𝐶200) 𝑠 + (𝐶 + 𝐾𝐴)𝑠 + 𝐾𝐴𝑎
(8)
The open loop transfer function is shown in Equation 9.
𝐺(𝑠) =𝑋(𝑠)
𝑋𝑟𝑒𝑓(𝑠)=
𝐾𝐴(𝑠 + 𝑎)
(𝑠
200 + 1)(𝐵𝑠2 + 𝐶𝑠)
(9)
Next we obtain the root locus of the “modified” plant, the transfer function shown in Equation 9
which contains the PD controller. We let 𝑎 =𝐾𝑃
𝐾𝐷= 12. The root locus is shown in Figure 7. A
zoomed in plot around the zero is shown in Figure 8.
9
Figure 7 – Root locus of the open loop transfer function shown in Equation 9.
Comparing the root locus in Figure 7 with the proportional gain controller shown in Figure 6, we
observe two things:
1) A zero in the left half plane attracts the root locus towards it, hence the oval shaped
branches in Figure 8.
2) For the root locus in Figure 7, 𝜔𝑛 and 𝜁 both increase as 𝐾𝐷 increases in the top left
portion of the oval branch, which does not occur in the root locus of Figure 6.
Observation # 2 will help us obtain the desired behavior, since we’ll be able to get a faster rise
time and a smaller overshoot.
As discussed in section 2.1.1, in order the meet the specifications the damping ratio 𝜁 ≥ 0.6901
and the natural frequency𝜔𝑛 ≥ 10.4843. We use the root locus in Figure 8 to find the gain
𝐾𝐷that meets these specifications.
𝐾𝐷 = 5.62
As seen in Figure 8, this gain satisfies the desired specifications. Next we use Simulink and
model the actually system to see how the “modified” plant behaves to a step input.
10
Figure 8 – Zoomed in version of the root locus shown in Figure 7. The tag shows at a gain of
5.62 the desired specs are met.
The Simulink model is shown below.
Figure 9 – “cart_model_tf_PD.mdl” Simulink block diagram that implements a PD controller in
the cart system.
11
The system response for 𝐾𝐷 = 5.62 is plotted in Figure 10.
Figure 10 – Cart response to a step input with a PD controller.
Using Matlab’s function, stepinfo, we calculate the characteristics of the response and find
the rise time 𝑇𝑟 = 0.1415 and %𝑂𝑆 = 5.3068, which corresponds to 𝜁 = 0.689 and 𝜔𝑛 = 14.8.
These specifications do not agree with the values predicted by the root locus in Figure 8 because
the calculations of root locus specs were created by assuming there was only two roots from a
second order system. Since we added a third pole at 𝑠 = −12, the system actually has 3 poles,
therefore the system’s response deviates slightly from a second order system.
We find a range of 𝐾𝐷values for which the system will meet the desired specs. The range is
calculated using Matlab, the code can be found in the appendix under Section 2.1.3.
3.80 ≤ 𝐾𝐷 ≤ 5.05
Four of these 𝐾𝐷 values are plotted in Figure 11.
12
Figure 11 – Cart response for varying 𝐾𝐷values.
The table below shows the rise time and the % overshoot for each of the 𝐾𝐷values.
𝑲𝑫 𝑻𝑹 % 𝑶𝑺
3.8 0.199 3.9969
4.3 0.1793 4.4629
4.7 0.1658 4.7703
5.0 0.1570 4.9699
Figure 12– Rise time and % overshoot for varying 𝐾𝐷values.
Figure 12 shows that for the four 𝐾𝐷values the specifications are met.
Now what happens if we move the zero to the left by making 𝑠 = −15? We use Matlab again, to
find the range of 𝐾𝐷values such that the desired specs are met. But instead of 𝐾𝐷(𝑆 + 12) in the
numerator, there will be 𝐾𝐷(𝑆 + 15). By running the Matlab code in the appendix, we find that
there is no value of 𝐾𝐷that satisfies the desired specs.
13
3. Lab
3.1 Proportional Control
We tested the performance of our proportional controller on the actual hardware. The Simulink
block diagram is shown below.
Figure 13 - “Pcontrol.slx” – Block diagram for implementing our proportional
controller on the hardware.
With this controller the rise time and overshoot criteria cannot be satisfied simultaneously. To
find the minimal K for which the rise time is less than 0.20 s and the maximal K for which the
overshoot is less than 5%, we tested some values of K on the hardware in the ranges predicted in
the Pre-lab using the Simulink model.
We found that the rise time specification is satisfied for K ≥ 54 while the overshoot specification
is satisfied for K ≤ 34. (Note that these are approximate values since the system response on the
hardware varies each time it is run even when all parameters remain unchanged.) Thus for any
intermediate gains the response does not satisfy either of the performance specifications. The
hardware responses are plotted below.
14
Figure 14: Response on hardware for step input u = 15 cm and K = 54, 39, and
34. Notice that the rise time and maximum overshoot performance specifications
cannot be met simultaneously for any K.
From Figure 14 we see that on the hardware the system response has a non-zero steady-state
error. We also see that the error decreases as the proportional controller gain increases. This is
due to nonlinearities in the actual system which we did not taking into account on our linear
model.
We also compared the system response on the hardware and on the Simulink model from the
Pre-lab at controller gain K = 100. The responses are plotted below in Figure 15. Examining this
figure we see that the rise time is longer for the actual system. One of the causes is the saturation
block which was added to protect the motor. Initially a large voltage is necessary to move to the
cart to the reference position but the saturation block cuts the voltage causing the rise time on the
actual system to be longer. Unmodeled nonlinearities in the motor also contribute to the longer
rise time.
15
Figure 15: Comparison of response for proportional control on hardware and on
Simulink model with step input u = 15 cm and K = 100.
16
3.2 – PD Control
We tested the PD controller from the Pre-lab on the hardware. The Simulink model with
hardware interface is shown below.
Figure 16- “PDcontrol.slx” - Block diagram for implementing our PD controller on
the hardware.
In the Pre-lab we found that the performance specifications are satisfied for gains 𝐾𝑑 ϵ [3.80,
5.05]. However, when we tested two values in this range (𝐾𝑑 = 4 and 5), we found that while the
response had no overshoot, the rise time was greater than 0.20 s. The responses for 𝐾𝑑 = 4 and 5
are plotted below.
17
Figure 17- Hardware response for PD control with step input u = 15 cm and
gain 𝐾𝑑 = 4 and 5.
We tuned our PD controller by varying the gain and the location of the zero and analyzing the
hardware response. As we increase 𝐾𝑑 the rise time decreased. We also found that as we moved
the zero farther to the left, the rise time decreased while the overshoot increased. The rise time
decreases since moving the zero farther left also “pulls” the root locus to the left. This increases
the radial distance of the complex poles, which means 𝜔𝑛 is increased. We found that the
performance specifications are met when 𝐾𝑑 = 4.7 and the zero is located at s = -25. The result is
plotted below.
18
Figure 18- Hardware response for our tuned PD controller with step input u =
15 cm. The controller gain is 𝐾𝑑 = 4.7 and the zero is set at s = -25.
19
3.3 Performance for Different Input signals
After tuning the controller in part 5.2, we implemented different input signals into the cart
system. The following list shows the 4 input signals used. Figures 19-22 shows the modified
models for different inputs.
Pulse generator (50% pulse width, amplitude = 0.075m, period = 3sec)
Sine Wave (amplitude = 0.1m, frequency = 1Hz)
Saw/Triangle Wave (amplitude = 0.05, frequency = 0.5Hz)
Exponential-decaying Sine Wave ( 0.15𝑒−0.5𝑡sin (4𝑡))
Figure 19 – “PDcontrol_pulsegen.mdl” Simulink/QuaRC model for a Pulse generator input.
Figure 20 - “PD_control_sinewav.mdl” Simulink/QuaRC model for a sine wave input
20
Figure 21- “PD_control_sawwav.mdl” Simulink/QuaRC model for a saw/triangle wave input.
Figure 22- “PD_control_expsinewav.mdl” Simulink/QuaRC model for an Exponential-decaying
sine wave.
21
The response for each input is plotted in Figure 23 – 26. Each response will be discussed
separately below.
Pulse Generator Input
The plot of the response to a pulse generator, is shown in Figure 23. We observe the response of
the cart varies slightly every oscillation. The overshoot, rise time, and steady state error are not
the same with each oscillation. Focusing on the first rise of the square wave or the first half of
the oscillation, we notice that the response is similar to a step response. The overshoot is within
bounds of the desired specs.
Figure 23 – Cart system to pulse generator input. Plots of response and input are shown.
22
Sinusoidal Wave Input
The response for the sinusoidal wave input in plotted in Figure 24. The response has the same
frequency as the input, but slightly shifted. Also the amplitude of the response is larger than the
input amplitude of 0.1. The specification for percent overshoot is not appropriate for the sine
wave input, because the input continuously changes.
Figure 24 – Cart response for a sine wave input. Plots of response and input are shown.
23
Saw/Triangle Wave
The response to the saw triangle wave is shown in Figure 25. When the input increases sharply
from negative amplitude to positive amplitude (e.g. at t =2s), the cart cannot effectively track the
input signal, hence the shifted peak in the top portions of the plot. But as the input gradually
decreases to its negative amplitude (e.g. going towards t = 4s), the cart response tracks the input
well. The negative peak of the input at 4secs matches the peak of the response which is also at
4secs.
Figure 25 – Cart response to a saw/triangle wave input. Plot of response and input are shown.
24
Exponential-Decaying Sine Wave
The cart response to an exponential decaying sine wave is shown in Figure 26. The oscillatory
response of the exponential decaying sine wave is slightly shifted, which is similar to the sine
wave response. After about t = 6 secs the response decays to zero and does not effectively track
the input. This is probably due to the backlash in the gears. The backlash can occur in the motor
pinion, the position pinion, or both.
Figure 26 – Cart response to an exponential decaying sine wave. Plot of response and input are
shown.
The cart can track both the exponential decaying sine wave and the sine wave signals easily.
Other than a small phase shift and amplitude variation, the two signals were tracked by the
system fairly well. Compared to the pulse generator and the saw/triangle wave these two signals
were continuous throughout.
25
Appendix
Pre Lab 2.1 – Position Controller Design
%% 2.1 Position Controller design % Setting up the given values m = 0.94; r = 6.3610e-3; Rm = 2.6; Kt = 7.6710e-3; Km = 7.6710e-3; Kg = 3.71; Jm = 3.910e-7;
% Setting up input V = 0.15; % Setting up the Transfer function a_0 = (r^2*m + Jm*Kg^2)/Kg; a_1 = Kt*Km*Kg/Rm; b_0 = Kt*r/Rm;
sys_tf = tf([b_0], [a_0, a_1, 0]);
% Rise time and ,aximum overshoot do not depend on the amplitude of the % input as in the transfer function, we can see that b_0 defines the % amplitude, and according to the formulae given in the lab, both the rise % time and the overshoot do not depend on b_0.
26
Pre Lab 2.1.1 – Plant Model
%% 2.1.1 Plant Model % For poles, we would need the state space representation of the system.
A = [0 1; 0 -a_1/a_0]; B = [0; b_0/a_0]; C = [1 0]; D = 0;
sys_ss = ss(A,B,C,D);
% Poles are given by: sys_poles = pole(sys_ss)
% The system is marginally stable as one of the poles lies on the
imaginary % axis.
% Desired values of w_n and zeta
% Using equation 4 and the given condition, we get: % Mp <= 0.05
zeta = -log(0.05)/sqrt(pi^2 + (log(0.05))^2)
% Therefore, zeta >= 0.6901
% Using equation 5 and the given condition, we get: % w_n >= 8.8*zeta^3 - 2.085*zeta^2 + 5.195*zeta + 5
w_n = (1/0.2)*(1.76*zeta^3 - 0.417*zeta^2 + 1.039*zeta + 1)
27
Pre Lab 2.1.2 – Proportional Controller
%% 2.1.2 Proportional Controller figure(1) linecolor = {'-b', '-g', '-r', ':', '-.'}; rise_time = []; overshoot = []; for K = 10:10:50 sim('cart_model_tf', [0 3]) response = stepinfo(stepout.signals.values, stepout.time); rise_time = [rise_time response.RiseTime]; overshoot = [overshoot response.Overshoot]; plot(stepout.time, stepout.signals.values, linecolor{K/10}) hold on end legend('K = 10', 'K = 20', 'K = 30', 'K = 40', 'K = 50', 'Location',
'SouthEast') title('Proportional Control with increasing K') xlabel('Time(sec)') ylabel('Position(m)')
disp('Overshoots in order of increasing value of K') overshoot
disp('Rise Tines in order of increasing value of K') rise_time
% From the plot (figure(1)), it seems that the proportional control is
able % to achieve one condition at a time, but not both of them simultaneously.
% To find the smallest value of K to get rise time less than 0.2 seconds for K = 30:40 sim('cart_model_tf', [0 3]) response = stepinfo(stepout.signals.values, stepout.time); if (response.RiseTime <= 0.2) break end end
% Therefore, smallest value of K to satisfy rise_time <= 0.2 is K % Risetime for K = 39 is response.RiseTime % Overshoot for K = 39 is response.Overshoot
% Clearly this does not meet the Overshoot requirement.
28
figure(2) plot(stepout.time, stepout.signals.values) xlabel('Time(sec)') ylabel('Position(m)') title('Proportional Control with K = 39') text(1,0.025, sprintf('Overshoot = %1.2f%%, Rise Time = %1.2f sec',
response.Overshoot, response.RiseTime))
figure(3) rlocus(sys_ss) title('Proportional Control Root Locus')
% On increasing K, the radial distance increases and the angle remains % constant. However, after a value, the angle starts decreasing while the % radial distance continues to increase.
% w_n increases with increasing K % zeta remains constant with increasing K initially, but then decreases.
29
Pre Lab 2.1.3 – PD Controller
%% 2.1.3 PD Controller % Initialize parameters m = 0.94; % cart mass (kg) r = 6.36e-3; % radius of motor (m) Rm = 2.6; % resistance of motor (ohms) Kt = 7.67e-3; % motor torque constant (Nm/A) Km = 7.67e-3; % back emf constant (Vs/rad) Kg = 3.71; % gearbox ratio Jm = 3.9e-7; % moment of inertia of motor (kgm^2)
% Initialize input U = 0.15; % step input tspan = 2; options = simset('FixedStep',0.01,'Solver','ode3'); Ustep = U*ones(1,100); tt = linspace(0,tspan);
%% 2.1.3 Root Locus PD Controller A = r*Kg*Kt; B = m*r^2*Rm+Rm*Kg^2*Jm; C = Kt*Km*Kg^2;
G = tf([1, 12],[1/200, 1]) * tf([A],[B, C, 0]); figure('Color',[1 1 1]) axes('FontSize',15) rlocus(G)
30
%% 2.1.3 Simulink model of PD controller for Kd = 5.62 Kd = 5.62; sim('cart_model_tf_pd',tspan,options);
oshoot_max = 1.05*Ustep;
index_10 = find(stepout.signals.values > 0.1*U,1); index_90 = find(stepout.signals.values > 0.9*U,1); t10 = stepout.time(index_10)*ones(1,100); t90 = stepout.time(index_90)*ones(1,100); y10 = linspace(0,stepout.signals.values(index_10)); y90 = linspace(0,stepout.signals.values(index_90)); figure('Color',[1 1 1]) axes('FontSize',15) plot(stepout.time,stepout.signals.values,... tt,Ustep,'r',... tt,oshoot_max,'g--') xlabel('Time (s)') ylabel('Position (m)') title(['Cart response to a step input, with a PD controller K = '
num2str(Kd)]) legend('Response','Input','Max Over shoot','Location','SouthEast') tr = stepout.time(index_90)-stepout.time(index_10) stepinfo(stepout.signals.values,stepout.time) %% 2.1.3 Simulink model of PD controller % Find a range of Kd values that will make the sys meet the desired specs a = 12; Kd = 5.62; k = []; while Kd>2 sim('cart_model_tf_pd',tspan,options); f = stepinfo(stepout.signals.values,stepout.time); if f.RiseTime<=0.2 && f.Overshoot <= 5 k = [k Kd]; end Kd = Kd - 0.01; end
range = [min(k) max(k)];
31
%% 2.1.3 Plot four Kd values that meet the desired specs a = 12; % a = K_P/K_D kd = [3.8 4.3 4.7 5]; % Initialize Kd values prop = zeros(numel(kd),3); % Plot Properties figure('Color',[1 1 1]) axes('FontSize',15) color = ['b' 'r' 'c' 'm' 'g' 'y' 'k']; leg = cell(1,numel(kd)); hold on
for i = 1:numel(kd) Kd = kd(i); sim('cart_model_tf_pd',tspan,options); plot(stepout.time,stepout.signals.values,color(i)) leg{i} = ['K_D = ' num2str(kd(i))]; f = stepinfo(stepout.signals.values,stepout.time); prop(i,:) = [Kd,f.RiseTime,f.Overshoot]; end
title('Cart response for varying K_D values') xlabel('Time (s)') ylabel('Distance (m)') legend(leg) hold off
%% 2.1.3 Simulink model of PD controller for zero s=-15 % Find a range of Kd values that will make the sys meet the desired specs a = 15; % a = K_P/K_D Kd = 50; % Initialize max gain. k = []; while Kd>0 sim('cart_model_tf_pd',tspan,options); f = stepinfo(stepout.signals.values,stepout.time); if f.RiseTime<=0.2 && f.Overshoot <= 5 k = [k Kd]; end Kd = Kd - 1; end
range = [min(k) max(k)];
32
Lab 3.1 – Proportional Controller
%% Plots for Proportional Controller close all; clear; clc; load('lab4_5_1data.mat')
figure(1) plot(t_K54,x_K54,t_K39,x_K39, t_K34,x_K34) title('Hardware response for step input u = 15 cm') xlabel('t [sec]') ylabel('x [m]') legend('K = 54', 'K = 39', 'K = 34') hold on plot([0 3],[0.15 0.15],'k:') hold off
% annotations info1 = stepinfo(x_K54,t_K54); t_r1 = info1.RiseTime; os1 = info1.Overshoot;
info2 = stepinfo(x_K39,t_K39); t_r2 = info2.RiseTime; os2 = info2.Overshoot;
info3 = stepinfo(x_K34,t_K34); t_r3 = info3.RiseTime; os3 = info3.Overshoot;
str1 = sprintf(' K = 54 K = 39 K = 34'); str2(1) = {sprintf('Rise time: %1.2f s %1.2f s %1.2f s',... t_r1, t_r2, t_r3)}; str2(2) = {sprintf('Overshoot: %1.2f%% %1.2f%% %1.2f%%',... os1, os2, os3)}; text(1.5,0.09,str1) text(1,0.075,str2)
% Compare Simulink and hardware response for K = 100 m = 0.94; % mass of cart [kg] r = 6.36e-3; % radius of motor gear [m] R = 2.6; % resistance of motor [ohms] Kt = 7.67e-3; % motor torque constant [Nm/A] Km = 7.67e-3; % back EMF constant [Vs/rad] Kg = 3.71; % gearbox gear ratio J = 3.9e-7; % moment of inertia of motor [kgm^2] u = 0.15; % step input [m] K = 100; % gain tspan = [0 5]; sim('cart_Kcontrol',tspan) x = xout.Data; t = xout.Time; figure(2) plot(t_K100, x_K100,t,x)
33
title('Comparing Simulink model and hardware at K = 100') xlabel('t [sec]') ylabel('x [m]') legend('Hardware','Model')
% annotations info4 = stepinfo(x_K100,t_K100); t_r1 = info4.RiseTime; os1 = info4.Overshoot;
info5 = stepinfo(x,t); t_r2 = info5.RiseTime; os2 = info5.Overshoot;
str1 = sprintf('Hardware Model'); str2(1) = {sprintf('Rise time: %1.2f s %1.2f s', t_r1, t_r2)}; str2(2) = {sprintf('Overshoot: %1.2f%% %1.2f%%',os1, os2)}; text(2.75,.12,str1) text(2,.1,str2)
34
Lab 3.2 PD Control
%% Testing PD controller on hardware close all; clear; clc; load('PDcontroltune.mat')
figure(1) plot(t_Kd4,x_Kd4,t_Kd5,x_Kd5) title('Testing hardware with PD control (zero at s = -12)') xlabel('t [sec]') ylabel('x [m]') legend('K_d = 4','K_d = 5')
% annotations info1 = stepinfo(x_Kd4,t_Kd4); t_r1 = info1.RiseTime; os1 = info1.Overshoot;
info2 = stepinfo(x_Kd5,t_Kd5); t_r2 = info2.RiseTime; os2 = info2.Overshoot;
str1 = sprintf('K_d = 4 K_d = 5'); str2(1) = {sprintf('Rise time: %1.2f s %1.2f s', t_r1, t_r2)}; str2(2) = {sprintf('Overshoot: %1.2f%% %1.2f%%',os1, os2)}; text(1.75,.095,str1) text(1.25,.08,str2)
% Tuned PD controller
figure(2) plot(t_Kd4_7_a25,x_Kd4_7_a25) title('Tuned PD control (K_d = 4.7, zero at s = -25)') xlabel('t [sec]') ylabel('x [m]')
35
Lab 3.3 Performance For Different Input Signals
%% 3.3 Performance for different input signals
%% Input = Pulse generator tt = linspace(0,t_pgen(end)); oshoot_x = [1.05*ones(100,1) -0.05*ones(100,1)]*0.075;
figure('Color', [1 1 1]) axes('FontSize',15) pgen = plot(t_pgen,x_pgen,tin_pgen,xin_pgen,tt,oshoot_x,'r--'); xlabel('Time (s)') ylabel('Distance (m)') title('Cart response to Pulse generator Input for K_P/K_D = 25 and K_D =
4.7') legend('Reponse','Input','5% overshoot')
%% Input = Sine Wave tt = linspace(0,t_sine(end)); oshoot_x = [1.05*ones(100,1) -1.05*ones(100,1)]*0.1; figure('Color', [1 1 1]) axes('FontSize',15) pgen = plot(t_sine,x_sine,tin_sine,xin_sine,tt,oshoot_x,'r--'); xlabel('Time (s)') ylabel('Distance (m)') title('Cart response to Sine Wave Input for K_P/K_D = 25 and K_D = 4.7') legend('Reponse','Input','5% overshoot')
%% Input = Saw/triangle Wave tt = linspace(0,t_saw(end)); oshoot_x = [1.05*ones(100,1) -1.05*ones(100,1)]*0.05; figure('Color', [1 1 1]) axes('FontSize',15) pgen = plot(t_saw,x_saw,tin_saw,xin_saw,tt,oshoot_x,'r--'); xlabel('Time (s)') ylabel('Distance (m)') title('Cart response to Saw/Triangle Wave Input for K_P/K_D = 25 and K_D =
4.7') legend('Reponse','Input','5% overshoot')
%% Input = Exponential-decaying sine Wave
figure('Color', [1 1 1]) axes('FontSize',15) pgen = plot(t_exp,x_exp,tin_exp,xin_exp); xlabel('Time (s)') ylabel('Distance (m)') title('Cart Response to Exponential-Decaying Sine Wave Input for K_P/K_D =
25 and K_D = 4.7') legend('Reponse','Input','5% overshoot')
1
Lab 6a: Pole Placement for the Inverted Pendulum
ME C134 / EE C128
Aldrich Ong, Mandy Huo, Deepak Talwar
2
Pre-lab
Equations of Motion of the Mechanical System
To effectively control the inverted pendulum system, we must understand the dynamics of the
cart and pendulum. We will first find the equations of motion of the system and then create a
controller suitable for the desired performance.
In finding the equations of motion, we make two important assumptions. First, we assume that
there is no friction or damping, and the second is that 𝜃 is small enough such that sin 𝜃 ≈ 𝜃 and
cos 𝜃 ≈ 1. After creating a force balance of inverted pendulum shown in Figure 1, two equations
of motion were formulated.
(𝑀 + 𝑚)�̈� + 𝑚𝐿𝑝�̈� = 𝐹𝑎 [1]
𝑚𝐿𝑝�̈� +4𝑚𝐿𝑝
2
3�̈� − 𝑚𝑔𝐿𝑝𝜃 = 0 [2]
The values of the constants are listed in Table 1.
Table 1: Parameters of the inverted pendulum system.
Figure 1: Free body diagram of the inverted pendulum
3
Full System Dynamics of Linearized System
The inverted pendulum system is a SIMO. Its only input is the voltage applied across the motor.
The two outputs are the cart position and the angle of the pendulum. From Lab 5, we know that
the applied force, 𝐹𝑎 can be found using Equation 3.
𝐹𝑎 = 𝑓(𝑉, 𝑥, �̇�, �̈�) =𝐾𝑔𝐾𝑡
𝑅𝑚𝑟𝑉 −
𝐾𝑔2𝐾𝑡𝐾𝑚
𝑟2𝑅𝑚−
𝐾𝑔2𝐽𝑚
𝑟2�̈� [3]
Substituting Equation 3 into Equations 1 and 2, and solving for the �̈� and �̈� we can obtain the
state-space model of the inverted pendulum systems shown in Equation 4.
�̇� = 𝐴𝒙 + 𝐵𝑢 [4]
�̇� [
�̇��̈��̇��̈�
] 𝒙 = [
𝑥�̇�𝜃�̇�
] 𝐴 = [
0 𝑎12 0 00 𝑎22 𝑎23 00 0 0 𝑎34
0 𝑎42 𝑎43 0
] 𝐵 = [
0𝑏2
0𝑏4
] 𝑢 = 𝑉
The elements of 𝐴 and 𝐵 are defined below.
𝑎12 = 𝑎34 = 1
[5]
𝑎22 = −1
𝑀 +𝑚4 +
𝐾𝑔2𝐽𝑚
𝑟2
𝐾𝑔
2𝐾𝑡𝐾𝑚
𝑟2𝑅𝑚
𝑎23 = −3
4
𝑚𝑔
𝑀 +𝑚4 +
𝐾𝑔2𝐽𝑚
𝑟2
𝑎42 =3
4𝐿𝑝
1
𝑀 +𝑚4
+𝐾𝑔
2𝐽𝑚
𝑟2
𝐾𝑔
2𝐾𝑡𝐾𝑚
𝑟2𝑅𝑚
𝑎43 = (1 +3
4
𝑚
𝑀 +𝑚4 +
𝐾𝑔2𝐽𝑚
𝑟2
)3𝑔
4𝐿𝑝
4
Analysis and Controller Design
Using MATLAB, we can find the eigenvalues of matrix A to be 𝜆1,2,3,4 =
0, −7.5515, −4.1290, 4.8682. The poles of the system are the eigenvalues. Since there is a
positive real pole, the system is unstable. Simulation of the system is shown in Figure 2. The plot
shows the output of the system for a step input. The outputs do not converge and so the system is
unstable.
There is a discrepancy between the simulation and what we would expect in reality. If the cart
was unstable, we would expect the pendulum to stay at a constant angle, but the simulation
shows that the pendulum angle will exponentially decrease. The discrepancy is due to the small
angle approximation made when deriving the equations of motion.
To remedy the stability of the system we implemented a full state-feedback controller, 𝑈 = 𝐾𝑥.
The given desired pole of the system are presented in Equation 6.
𝑠1,2 = −1.9 ± 10𝑗
𝑠3,4 = −1.6 ± 1.3𝑗 [6]
Figure 2: Plot of the system’s step response. The outputs 𝑥 and 𝜃 do not converge therefore
the system is unstable.
5
We found the values of 𝐾 = [𝐾1 𝐾2 𝐾3 𝐾4 ] that will give the characteristic polynomial the
desired poles. In order to find these values, we first determined the close-loop system state
matrix shown in Equation 7.
𝐴𝑘 = 𝐴 − 𝐵𝐾 [7]
Then we found the characteristic polynomial, 𝑃(𝐾𝑖 , 𝑠), of 𝐴𝑘 which should be a function of 𝐾𝑖 .
Next we equate the corresponding coefficients of 𝑃 with the coefficients of the characteristic
polynomial of the desired poles. The result were four independent equation which we solved to
find the values of 𝐾. The values of 𝐾that were obtained were
𝐾1 = −12.9796 𝐾2 = −14.7230 𝐾3 = −47.8456 𝐾4 = −6.5363 [8]
Next we implement the controller 𝑈 = 𝐾(𝑟 − 𝑥), where 𝑟 is the reference input. We find the
transfer function of the new state space model from the reference to the cart’s position. Then we
plot the bode plot of the transfer function as seen in Figure 3.
Figure 3: Bode plot of the inverted pendulum with full state-feedback controller.
6
Lab
In the lab, we will be implementing the controller designed in the Pre-lab on the actual hardware
and making observations.
4.1 Implementing Controller Hardware in Simulink
In this part of the lab, we will be implementing the state-feedback controller with the Quanser
I/O blocks in Simulink. We would interface with the hardware in the same way as done in the
Labs 3 and 4. The block diagram is given in Figure 4.
As given in Figure 4, we used the HIL Initialize block, with board type q4, to initialize the
connection with the Quanser board. Unlike Labs 3 and 4, where we only needed the encoder data
from for position, for the purpose of this lab, we would need encoder data from both #0 and #1
encoders. The #0 encoder gives us position values and #1 encoder gives us 𝜃 values. The HIL
Read Encoder block is used for this purpose.
The encoder values (counts) are then converted into the corrected units by dividing by the
conversion factors: 439.6 counts/cm for the position and -666.7 counts/rad for the 𝜃. Using the
derivative blocks, we find the numerical derivatives of these values. We use Mux and Demux
blocks to keep the block diagram clean. The values for 𝑥, 𝑥,̇ 𝜃, �̇� are then added into a single
array using mux and then matrix-multiplied by the gain vector K as calculated in the Pre-lab.
These are then made to go through a saturation block set to ±6𝑉 for gear protection and then
sent to the HIL WriteAnalog block which outputs to the motor in the cart. In order to record
values, we added To-Workspace blocks for each of the inputs.
Figure 4: Controller block diagram in Simulink
7
4.2 Running the Controller on the Hardware
1. For this part, we will set the reference, r vector to r = [0 0 0 0]T. We set the amplitude of the
sin wave block (in Figure x1) to 0 to get reference r. To make the pendulum balance, we hold the
pendulum exactly vertically before connecting to the controller. Since the controller sets the zero
position every time a connection is made, it is important to have the pendulum exactly vertical
every time we connect. Using this input, we see that the cart balances the pendulum really well.
If we connect to the hardware with the pendulum in the stable equilibrium position, then the
controller will reset the value of 𝜃 = 0° at the stable equilibrium position. Since this position is
stabilized by gravity, the cart will not move at all.
2. In this section, with the same reference vector r, we will apply small angle perturbations to
the pendulum and check the response. We will start the cart at the center of the track in order to
give it enough space to move and balance the pendulum.
(a) If we perturb the top of the pendulum slightly in a given direction, the cart accelerates in the
same direction in order to bring the pendulum back to its equilibrium position. For instance, if
the top of the pendulum is perturbed to the right (increasing 𝜃), then the cart also moves to the
right (increasing x) so that the pendulum moves back to the left at its equilibrium position.
Figure 5: Top: Cart Position v/s Time plot, Bottom: Pendulum Angle v/s Time Plot, with
added perturbations
8
(b) Figure 5 above give the plots between position of the cart (x) against time, and the angle of
the pendulum (𝜃) against time.
As seen from the plots in Figure 5, the controller is able to bring both the cart and the pendulum
very close to the equilibrium position. The small perturbations can be seen at times
𝑡 ≈ 3.1, 5.8, 8.9 𝑠𝑒𝑐𝑜𝑛𝑑𝑠. In the pendulum position plot we see sudden peaks where pendulum
was pushed to the right (theta becomes more positive). On the cart position plot we see that soon
after the perturbation the cart moves in the same direction, to the right, to correct the pendulum
position. It also seems like the controller is better at catching the pendulum than bringing the cart
back to its equilibrium position. After perturbations, the angle is quickly brought back to the
equilibrium position, however, it takes some time for the cart to get back to its equilibrium
position. We tried applying some large perturbations to the pendulum, however the cart could not
stabilize it.
Since our reference r is set as r = [0 0 0 0]T, the controller tries to get the cart and the pendulum
as close to their 0 positions as possible. As the pendulum is in unstable equilibrium, when the
cart moves to get to its equilibrium position, the pendulum loses stability and falls in the other
direction. This makes the cart to move and catch the pendulum, thus crossing the equilibrium
point again. As soon as the pendulum is in its equilibrium position again, the cart starts moving
back to its equilibrium position and the cycle continues.
9
3. Next we introduced sine wave reference inputs r = [𝑀 sin 𝜔𝑡 0 0 0]𝑇 with M = 0.1 m and ω =
1, 2, and 5 rad/s.
The resulting responses for cart position, cart velocity, and pendulum angle are plotted below.
Figure 6: Cart position for input amplitude 0.1 m and frequencies ω = 1, 2, and 5 rad/s.
11
Figure 8: Pendulum angle for input amplitude 0.1 m and frequencies ω = 1, 2, and 5 rad/s.
From Figure 6 we see that the responses are virtually sinusoids of with a phase shift and different
amplitude than the inputs. The gain and phase for each of the frequencies was estimated from the
data (see code in Appendix). These values are compared in the table below with the
corresponding values from the Bode plot of part 3.3.4 of the Pre-lab.
Frequency
[rad/s]
Actual gain Actual phase
[degrees]
Simulated gain Simulated phase
[degrees]
1 1.23 52.9 0.98 313.3
2 1.14 94.1 0.81 267.9
5 0.54 169.8 0.44 204.1
Table 2: Comparison of actual gain and phase with simulated values for each frequency.
The phase and gains do not match for any frequency. The actual gains are larger. On the actual
system the cart needs to move farther than predicted to correct the pendulum angle due to
nonlinearities. The phase also increases with frequency on the actual system rather than
decreasing with frequency as the Bode plot predicts. This is also caused by unrepresented
dynamics and nonlinearities which add terms to the system transfer function, thus affecting the
phase angle.
12
Next we experimented with the effects of changing the position of the desired closed-loop poles.
Using the place function we recalculated K for each set of new poles then ran the resulting
controller on the hardware with a sine wave reference input of amplitude 0.1 m and frequency 2
rad/s. We ran three tests. The resulting cart position, cart velocity, and pendulum angle plots are
shown below.
Figure 9: Cart position for various desired closed-loop poles and input sine wave with amplitude
0.1 m and frequency ω = 2 rad/s.
13
Figure 10: Cart velocity for various desired closed-loop poles and input sine wave with
amplitude 0.1 m and frequency ω = 2 rad/s.
14
Figure 11: Pendulum angle for various desired closed-loop poles and input sine wave with
amplitude 0.1 m and frequency ω = 2 rad/s.
First we tried increasing the imaginary part of the dominant pole. The new poles are: 𝑠1,2 =−1.9 ± 10𝑗 and 𝑠3,4 = −1.6 ± 3𝑗. From Figure 9, the resulting cart position seems to follow the
same sine wave as before, with an offset.
Next we moved a pole farther left along the real axis. The new poles were: 𝑠1,2 = −1.9 ± 10𝑗
and 𝑠3,4 = −3 ± 1.3𝑗. From Figure 9, the resulting cart position is (almost) a sine wave with
larger amplitude and smaller phase than with the original poles. The peaks and troughs of the
wave are distorted due to the saturation block which was included to protect the motor. From
Figure 10 and 11 we also see the cart reaches higher maximum velocities, which also cause the
pendulum angle to swing farther from θ = 0°.
Finally we moved a pole farther right along the real axis. The new poles were: 𝑠1,2 = −1.9 ±
10𝑗 and 𝑠3,4 = −0.5 ± 1.3𝑗. From Figure 9, the resulting cart position follows a sine wave of
larger amplitude and phase than with the original poles.
15
4.
Figure 12: Pendulum angular velocity for input amplitude 0.1 m and frequencies ω = 1, 2, and 5
rad/s.
The cart velocity is shown in Figure 7 and pendulum angular velocity is shown in Figure 12.
These plots were obtained by differentiating the signals x and θ. The resulting plots are very
fuzzy due to the resolution of the cart position and pendulum angle encoders. The cart position
and pendulum angle are registered with discrete counts so the resulting derivatives are not
smooth.
16
Appendix
1. Pre-Lab
%%% ME 134 Lab 6a Kt = 7.67e-3; % Nm/A Kg = 3.71; Rm = 2.6 ;% ohms r = 6.36e-3 ; % m Jm = 3.9e-7 ; % kgm^2 Km = 7.67e-3; % Vs/rad M = 0.94 ; % kg m = 0.230; % kg Lp = 0.3302; % m g = 9.81; % m/s a = 1/(M+.25*m+Kg^2*Jm/r^2); b = Kg^2*Kt*Km/(r^2*Rm); c = Kg*Kt/(r*Rm);
%% 3.3 Analysis and Controller Design % syms Kg Kt Km M m Lp Jm R r k1 k2 k3 k4 a = 1/(M+.25*m+Kg^2*Jm/r^2); b = Kg^2*Kt*Km/(r^2*Rm); c = Kg*Kt/(r*Rm);
A = [0 1 0 0 0 -a*b -0.75*m*g*a 0 0 0 0 1 0 0.75*a*b/Lp (1+0.75*a*m)*0.75*g/Lp 0]; B = [ 0 a*c 0 -0.75*a*c/Lp]; C = [1 0 0 0 0 0 1 0]; D = 0;
A12 = 1; A22 = -a*b; A23 = -0.75*m*g*a; A34 = 1; A42 = 0.75*a*b/Lp; A43 = (1+0.75*a*m)*0.75*g/Lp; B2 = a*c; B4 = -0.75*a*c/Lp;
%% 3.3 #1 eig(A) poles(A)
%% 3.3 #2 sys = ss(A,B,C,D) [y t]=step(sys); figure('Color', [1 1 1]) axes('FontSize',15) subplot(2,1,1)
17
plot(t,y(:,1)) title('Cart position for Step Input') xlabel('Time (s)') ylabel('x (m)'); subplot(2,1,2) plot(t,y(:,2)) title('Pendulum Angle for Step Input') xlabel('Time (s)') ylabel('\theta (rad)'); %% 3.3 #3a syms a12 a22 a23 a34 a42 a43 b2 b4
A = [ 0 a12 0 0 0 a22 a23 0 0 0 0 a34 0 a42 a43 0]; B = [0 b2 0 b4];
syms k1 k2 k3 k4 s K =[k1 k2 k3 k4]; Ak = A-B*K;
%% 3.3 #3b P = det(s*eye(4)-Ak)
%% 3.3 #3 C syms s s1 =-1.9+10j; s2 =-1.9-10j; s3 =-1.6+1.3j; s4 =-1.6-1.3j;
Pd = expand((s-s1)*(s-s2)*(s-s3)*(s-s4))
%% 3.3 #3 D A = [0 1 0 0 0 -a*b -0.75*m*g*a 0 0 0 0 1 0 0.75*a*b/Lp (1+0.75*a*m)*0.75*g/Lp 0]; B = [ 0 a*c 0 -0.75*a*c/Lp]; C = [1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0]; D = 0; p = [s1 s2 s3 s4]; K = acker(A,B,p)
%% 3.3 #4
18
Aprime = A-B*K; Bprime = B*K; Cprime = [1 0 0 0]; Dprime = [0 0 0 0]; [num,den] = ss2tf(Aprime,Bprime,Cprime,Dprime,1) figure('Color', [1 1 1]) axes('FontSize',15) bode(tf(num(1,:),den)) title('Bode Plot of Inverted Pendulum with Full State-Feedback Controller')
2. Lab
figure(1) subplot(2,1,1) plot(time_w0,x_w0) title('Cart position (x) with small pertubations v/s time') xlabel('Time (s)'); ylabel('Position (m)') subplot(2,1,2) plot(time_w0,theta_w0) title('Pendulum angle (\theta) with small pertubations v/s time') xlabel('Time (s)'); ylabel('\theta (rad)')
%% w = 1 [xmax, idx] = max(x_w1); xmin = min(x_w1); amp = (xmax - xmin)/2; gain = amp/0.1 period = 2*pi/1; t = time_w1(idx); phase = (t - period/4)*(360/period)
%% w = 2 [xmax, idx] = max(x_w2); xmin = min(x_w2(2000:end)); amp = (xmax - xmin)/2; gain = amp/0.1 period = 2*pi/2; t = time_w2(idx); phase = (t - (2 + 1/4)*period)*(360/period)
%% w = 5 [xmax, idx] = max(x_w5(3500:4000)); xmin = min(x_w5(3500:4000)); amp = (xmax - xmin)/2; gain = amp/0.1 period = 2*pi/5; t = time_w5(idx + 3499); phase = (t - (5 + 1/4)*period)*(360/period)
%% plots
figure(1) plot(time_w1,x_w1,time_w2,x_w2,time_w5,x_w5) legend('\omega = 1 rad/s','\omega = 2 rad/s','\omega = 5 rad/s')
19
xlabel('Time [sec]') ylabel('x [m]') title('Cart position for sine reference')
figure(2) plot(time_w1,xd_w1,time_w2,xd_w2,time_w5,xd_w5) legend('\omega = 1 rad/s','\omega = 2 rad/s','\omega = 5 rad/s') xlabel('Time [sec]') ylabel('velocity [m/s]') title('Cart velocity for sine reference')
figure(3) plot(time_w1,theta_w1,time_w2,theta_w2,time_w5,theta_w5) legend('\omega = 1 rad/s','\omega = 2 rad/s','\omega = 5 rad/s') xlabel('Time [sec]') ylabel('theta [rad]') title('Pendulum angle for sine reference')
figure(4) plot(time_w1,thetad_w1,time_w2,thetad_w2,time_w5,thetad_w5) legend('\omega = 1 rad/s','\omega = 2 rad/s','\omega = 5 rad/s') xlabel('Time [sec]') ylabel('velocity [m/s]') title('Pendulum angular velocity for sine reference')
%%% Lab 6a changing poles close all; clear; clc; load('workspace.mat')
figure(1) plot(time_K1w2,x_K1w2,time_K2w2,x_K2w2,time_K3w2,x_K3w2,time_w2,x_w2,':k') legend('s_{3,4} = -1.6 ± 3j','s_{3,4} = -3 ± 1.3j','s_{3,4} = -0.5 ±
1.3j','s_{3,4} = -1.6 ± 1.3j') xlabel('Time [sec]') ylabel('x [m]') title('Cart position with various poles')
figure(2) plot(time_K1w2,xd_K1w2,time_K2w2,xd_K2w2,time_K3w2,xd_K3w2,time_w2,xd_w2,':k') legend('s_{3,4} = -1.6 ± 3j','s_{3,4} = -3 ± 1.3j','s_{3,4} = -0.5 ±
1.3j','s_{3,4} = -1.6 ± 1.3j') xlabel('Time [sec]') ylabel('velocity [m/s]') title('Cart velocity with various poles')
figure(3) plot(time_K1w2,theta_K1w2,time_K2w2,theta_K2w2,time_K3w2,theta_K3w2,time_w2,th
eta_w2,':k') legend('s_{3,4} = -1.6 ± 3j','s_{3,4} = -3 ± 1.3j','s_{3,4} = -0.5 ±
1.3j','s_{3,4} = -1.6 ± 1.3j') xlabel('Time [sec]') ylabel('theta [rad]') title('Pendulum angle with various poles')
1
Lab 6B
Luenberger Observer Design for Inverted
Pendulum
ME 134/EE 128
Lab Group #4 Tuesday 4-6pm
Mandy Huo, Aldrich Ong, and Deepak Talwar (MAD-HOT Team)
Lab performed on: 11/19/13
Lab due on: 11/26/13
2
Purpose
In this lab we use an observer to estimate the state for full-state feedback control of the inverted
pendulum system. We estimate the cart velocity and pendulum angular velocity given just the
positions of the cart and pendulum.
Pre-lab
Controllability and Observability
We analyze the controllability and observability of the linearized open-loop system from Lab 6a.
The system in state-space form is given below and the parameter values are given in Table 1.
�̇� = 𝐴𝑥 + 𝐵𝑢
𝑦 = 𝐶𝑥
𝐴 =
[ 0 1 0 0
0 −𝐾𝑔
2𝐾𝑡𝐾𝑚
𝑅𝑚 [𝑟2 (𝑀 +𝑚4) + 𝐾𝑔
2𝐽𝑚]−
3
4⋅
𝑟2𝑚𝑔
[𝑟2 (𝑀 +𝑚4) + 𝐾𝑔
2𝐽𝑚]0
0 0 0 1
03
4𝐿𝑝⋅
𝐾𝑔2𝐾𝑡𝐾𝑚
𝑅𝑚 [𝑟2 (𝑀 +𝑚4) + 𝐾𝑔
2𝐽𝑚]
3𝑔
4𝐿𝑝⋅
𝑟2(𝑀 + 𝑚) + 𝐽𝑚𝐾𝑔2
[𝑟2 (𝑀 +𝑚4) + 𝐾𝑔
2𝐽𝑚]0
]
𝐵 =𝑟𝐾𝑔𝐾𝑡
𝑅𝑚 [𝑟2 (𝑀 +𝑚4) + 𝐾𝑔
2𝐽𝑚]
[
010
−3
4𝐿𝑝]
𝐶 = [1 00 0
0 01 0
]
(1)
3
Table 1: Parameters of the inverted pendulum system.
Using the MATLAB commands ctrb, obsv, and rank we find that both the controllability
and observability matrices have full rank so the system is both controllable and observable.
Observer Design
In Lab 6a we implemented full-state feedback control using derivative blocks to estimate the cart
velocity and the pendulum angular velocity. Taking derivatives yielded poor quality estimates
due to the amplification of noise. In this lab we use a Luenberger observer to provide a state
estimate, �̂�, for the state feedback. Using the same desired closed-loop poles as in Lab6a, we
have the same matrix K as before. Our new state feedback controller is:
𝑢(𝑡) = 𝐾(𝑟(𝑡) − �̂�(𝑡))
𝐾 = [−12.9795 −14.7230 −47.8456 −6.5363 ]
(2)
The state-space equation of the Luenberger observer is
�̇̂� = 𝐴�̂� + 𝐵𝑢 + 𝐿(𝑦 − �̂�)
�̂� = 𝐶�̂�
(3)
Subtracting (1) from (3) and letting �̃� = 𝑥 − �̂�, we have
�̇̃� = (𝐴 − 𝐿𝐶)�̃� (4)
Since 𝐴 ∈ ℝ4×4 and 𝐶 ∈ ℝ2×4, 𝐿 ∈ ℝ4×2 so that 𝐴 − 𝐿𝐶 is the same size as 𝐴.
4
We choose L such that the matrix 𝐴 − 𝐿𝐶 has eigenvalues at −10 ± 15𝑗 and −12 ± 17𝑗. Since
𝐴 − 𝐿𝐶 is a square matrix it has the same eigenvalues as its transpose (𝐴 − 𝐿𝐶)𝑇 = 𝐴𝑇 − 𝐶𝑇𝐿𝑇.
Using the place function we find 𝐿𝑇given the desired eigenvalues. We find that
𝐿 = [
16.1596 −2.3767254.9780 −5.483415.7136 21.0282180.7734 378.2196
]
(5)
Simulation
We implement the observer in MATLAB. The Simulink model is shown below in Figure P1.1.
Figure P1.1: observer.mdl: Simulink model of inverted pendulum system with
full-state feedback control using a Luenberger observer.
We simulate the system with an initial perturbation of 10 cm displacement of the cart and 5°
angular displacement of the pendulum. The observer estimate and actual state of the plant are
plotted in the figures below for comparison.
5
Figure P1.2: Position of cart for actual state and observer estimate.
Figure P1.3: Velocity of cart for actual state and observer estimate.
7
Figure P1.5: Pendulum angular velocity for actual state and observer estimate.
From Figures P1.2-P1.5 we see that the observer estimate catches up to the actual system soon
after the initial perturbation. The estimation error for each of the state variables is plotted below.
8
Figure P1.6: Estimation error for each state variable.
The initial errors of the positions of the cart and pendulum is nonzero due to the perturbation
while the cart velocity and pendulum angular velocity errors are initially zero but deviate a lot
before converging. The position errors converge to zero much quicker than the velocity errors,
but all errors converge to zero in less than one second.
9
Lab
1. In this portion of the lab, we will implement the observer designed in the pre-lab in order to
obtain the full state �̂� from the measured output 𝑦. This is done by inputting the output of the
plant into the observer, and using the full state output from the observer as the feedback into
the controller. The Simulink block diagram for the observer is given in Figure L1.1.
Now, we set the reference to zero, and compare the outputs of the plant, 𝑦 and the outputs
from the observer �̂�. The plots of the two outputs and the error between them is given in
Figure L1.2 and Figure L1.3
From Figure L1.2, we can see that the position estimated by the observer tracks the actual
position measured by the encoders on the cart very closely. From the second plot in Figure
L1.2, we can see that the error is miniscule, on the order of 10−3𝑚.
Figure L1.3 gives us the plots of the pendulum angle with time, as measured by the cart
encoder and as estimated by the observer. We can see that the tracking is still fairly good.
The error between the two is plotted in the second graph and is of the order 10−3 𝑟𝑎𝑑.
Figure L1.1 – Simulink Block Diagram of the Luenberger Observer implemented on the
inverted pendulum plant.
10
Figure L1.2 – Plots of the state variable of the cart position from the observer
and from the plant. The second plot shows the error between the observer and
the plant.
Figure L1.3 – Plots of the state variable of the pendulum angle from the observer
and from the plant. The second plot shows the error between the observer and
the plant.
11
2. For each of the three parts of this section we varied the reference and perturbations, while
comparing the performance of the controller with and without an observer. In part A, the
reference is zero for the cart position and no perturbation were applied. In part B, the
reference is also zero, but a perturbation was applied. In part C, the reference was changed to
sinusoidal input with no perturbation. In each of the parts, the cart position,𝑥, and the
pendulum angle, 𝜃, is plotted with time, to show a comparison between the system with the
observer and the one without an observer.
In all of the trials, we notice that the systems’ response with or without an observer are offset
from the reference input. The offset is due to the controller placing a larger importance in
setting the pendulum angle to its reference than setting the cart position to its reference. As a
result the controller won’t set the cart position exactly to zero, because that would cause the
pendulum angle to divert from its reference. A positive or negative offset is caused by the
initial position of the pendulum. For example if the pendulum was initially slightly tilted to
the left, then the cart’s initial response is to move left, therefore the offset will be negative.
Since systems with or without the observer both have offsets, we can conclude that the cause
of the offset is independent of having the observer in the system.
Comparing the state variable 𝑥 of the two systems, the one with and the other without the
observer, we conclude that there is no distinguishable difference in the output of the cart
position. For the state variable 𝜃 on the other hand, we noticed that the non-observer system
has numerous small clusters of fast changing angle values, while the observer system does
not.
Below we graph the response of the system for three different reference parameters and
compare the response of the two systems.
12
A) Zero reference with no perturbations
A plot for the cart position vs time is shown in Figure L2.1. Disregarding the offset, there
is no distinguishable difference between the two systems.
Figure L2.2 shows the pendulum angle over time for the two systems. Unlike the cart position
plots, the discretization of the angle values in the pendulum angle plot are more obvious. We also
notice the system without the observer has a clusters of fast changing angle values. One of the
clusters is shown in Figure L2.2 by the highlighted black circle. A zoom-in version is shown in
Figure L2.3. The system with the observer does not have these dense clusters.
Figure L2.1 – Plot of state value 𝑥 for a system with an observer and without an observer.
The system characteristic is zero input reference with no perturbations.
13
Figure L2.2 – Plot of state value 𝜃
for a system with an observer and
without an observer. The system
characteristic is zero input
reference with no perturbations.
The black circle shows one
occurrence of the unusual
oscillating angle values for the
non-observer system.
Figure L2.3 – Zoomed in plot of the
black circle in Figure L2.2.
14
B) Zero Reference with Perturbations
The plot cart position and pendulum angle for zero reference with perturbations is shown
in Figures L2.4 and L2.5. The perturbations for the non-observer system is not noticeable
(or we forgot to apply to it). But if we did apply the perturbation, I would expect there
would not be much difference between the two systems.
Figure L2.4 – Plot of state value 𝑥 for a system with an observer and without an observer.
The system characteristic is zero input reference with perturbations.
15
Figure L2.5 – Plot of state value 𝜃 for a system with an observer and without an observer.
The system characteristic is zero input reference with perturbations.
16
C) Sinusoidal Input Reference
Figure L2.6 and L2.7 shows the cart position and pendulum angle responses for a
sinusoidal input reference 𝑥𝑟 = 0.1 sin(2𝑡). The plot in Figure L2.6 shows no
distinguishable difference between the observer and non-observer systems on the cart
position.
The pendulum angle values for the both systems are also very similar. Other than the
non-observer system having more clusters of fast changing angle values, there is no
distinguishable difference.
Figure L2.6 – Plot of state value 𝑥 for a system with an observer and without an observer.
The system characteristic is a sinusoidal input reference.
17
3. In this part of the lab we will further investigate the differences in performances. We will
compare the estimates of carts and pendulum velocities as obtained from the observer with
those computed using derivative blocks in Lab 6a. We will then decide on which one
provides us better performance. Figure L3.1 gives us the velocities of the cart w.r.t time as
computed using derivative blocks and as estimated by the observer. The reference in these
cases is 𝑟(𝑡) = 0.1 sin(2𝑡).
Figure L2.7 – Plot of state value 𝜃 for a system with an observer and without an observer.
The system characteristic is sinusoidal input reference.
18
As we can see from Figure L3.1, the plot of the cart velocity, �̇� as computed by using
derivative blocks is a lot noisier than the velocity as estimated by the observer. This
reduction in noise results in a lot better feedback and silences the grinding motor noises.
Figure L3.2 gives the plots of pendulum velocities as computed by using derivative blocks
and as estimated by the observer. With just a simple glance at the plots, we can see that the
observer gives us much better and clean estimates of pendulum velocities. Again, the
observer results in reduction of noise and provides much better feedback.
These two schemes differ in the way the derivatives are computed. When we use derivative
blocks, we are just computing the numerical derivative of the sensor position values. Since
the sensor records values in discrete time, on calculating their derivative, we get spikes at the
times when the transition from one sample happens to another. This creates a lot of noise and
results in a much worse feedback.
Figure L3.1 – Plots of the cart velocity as computed using derivative blocks (top) and as
estimated by the observer (bottom)
19
4. The system with the observer gives a better system response. Even though both system were
able to regulate the pendulum and cart, the system with the observer produced a better
physical performance. During the testing of both systems, the one with no observer produced
a grinding sound, while the observer system did not.
Figure L3.1 – Plots of the pendulum velocity as computed using derivative blocks (top)
and as estimated by the observer (bottom)
20
Prelab Code (1/2)
%%% ME134/EE128 Prelab 6b close all; clear; clc;
%% Controllability and Observability M = 0.94; % mass of cart and motor [kg] m = 0.230; % mass of pendulum [kg] Lp = 0.3302; % pendulum: pivot to CM [m] Kt = 7.67e-3; % motor torque constant [Nm/A] Km = 7.67e-3; % motor back EMF constant [Vs/rad] Kg = 3.71; % motor gearbox ratio Rm = 2.6; % motor winding resistant [ohm] r = 6.36e-3; % motor gear radius [m] Jm = 3.9e-7; % motor moment of inertia [kg*m^2] g = 9.81; % [m/s^2]
a = 1/(Rm*(r^2*(M+m/4)+Jm*Kg^2));
a22 = -a*(Kt*Km*Kg^2); a23 = -a*Rm*3*m*g*r^2/4;
A = [0 1 0 0 0 a22 a23 0 0 0 0 1 0 (3/(4*Lp))*(-a22) (3/(4*Lp))*(-a23 + g) 0];
B = (r*Kt*Kg/(Rm*(r^2*(M+m/4)+Jm*Kg^2)))*[0 1 0 -3/(4*Lp)].';
C = [1 0 0 0 0 0 1 0];
rank_co = rank(ctrb(A,B)) rank_ob = rank(obsv(A,C))
%% Observer Design poles = [-10+15*1i -10-15*1i -12+17*1i -12-17*1i]; L_trans = place(A', C', poles); L = L_trans'
21
Pre-lab code (2/2)
%% Simulation K = [-12.9795 -14.7230 -47.8456 -6.5363]; x0_obs = [0 0 0 0]'; x0 = [0.1 0 5*pi/180 0]'; sim('observer') t = x_out.Time; x = x_out.Data(:,1); xd = x_out.Data(:,2); theta = x_out.Data(:,3); thetad = x_out.Data(:,4); x_obs = x_obs_out.Data(:,1); xd_obs = x_obs_out.Data(:,2); theta_obs = x_obs_out.Data(:,3); thetad_obs = x_obs_out.Data(:,4); figure(1) plot(t,x,':', t, x_obs) legend xlabel('Time [sec]') ylabel('x, x(obs) [m]') title('Cart position') legend('actual state','observer estimate') figure(2) plot(t,xd,':', t, xd_obs) legend xlabel('Time [sec]') ylabel('velocity [m/s]') title('Cart velocity') legend('actual state','observer estimate') figure(3) plot(t,theta,':', t, theta_obs) xlabel('Time [sec]') ylabel('theta, theta(obs) [rad]') title('Pendulum angle') legend('actual state','observer estimate') figure(4) plot(t,thetad,':', t, thetad_obs) xlabel('Time [sec]') ylabel('angular velocity [rad/s]') title('Angular velocity') legend('actual state','observer estimate') e = x_obs_out.Data - x_out.Data; figure(5) plot(t(1:150),e(1:150,1),t(1:150),e(1:150,2),t(1:150),e(1:150,3),... t(1:150),e(1:150,4)) legend('cart position error','velocity error','pendulum angle error',... 'angular velocity error') title('Estimation error') xlabel('Time [sec]') ylabel('Estimation errors')
--------------------
22
Lab Code (1/5)
load('lab6b_data.mat')
%% Lab 6B - Problem 1 xo = x_obs; tho = theta_obs;
x = x; th = theta; t; figure('Color', [1 1 1]) axes('FontSize',15) subplot(2,1,1) plot(t,xo,t,x) legend('observer','plant') xlabel('Time (s)') ylabel('Position (m)') title('Cart Position vs Time for the Plant and Observer')
subplot(2,1,2) plot(t,xo-x) xlabel('Time (s)') ylabel('error (m)') title('Cart Position Error vs Time')
figure('Color', [1 1 1]) axes('FontSize',15) subplot(2,1,1) plot(t,tho,t,th) legend('Observer','Plant') xlabel('Time (s)') ylabel('Angle (rad)') title('Pendulum Angle vs Time of Plant and Observer')
subplot(2,1,2) plot(t,tho-th) xlabel('Time (s)') ylabel('Error (rad)') title('Pendulum Angle Error vs Time')
23
Lab code (2/5)
%% Lab 6B - Problem 2 % Plot of position and pendulum angle vs Time for system with and without % an observer. %% Part 2A - Zero reference and no perturbation % Input reference t_in = linspace(0,9); x_in = zeros(1,100); % With an observer to = t_b2a; xo_zero = x_b2a; tho_zero = theta_b2a;
% Without an observer t = t_a2a; x_zero = x_a2a; th_zero = theta_a2a;
figure('Color', [1 1 1]) axes('FontSize',10) plot(t_in,x_in,to, xo_zero,t,x_zero) xlabel('Time (s)') ylabel('Position (m)') title({'Cart Position vs Time for System with and without an
Observer',... '(zero reference with no perturbation)'}) legend('Input Reference','With an Observer','Without an Observer')
figure('Color', [1 1 1]) axes('FontSize',10) plot(to, tho_zero,t,th_zero) xlabel('Time (s)') ylabel('Angle (rad)') title({'Pendulum Angle vs Time for System with and without an
Observer',... '(zero reference with no perturbation)'}) legend('With an Observer','Without an Observer')
24
Lab code (3/5)
%% Part 2B - Zero reference with pertubation % With an observer to = t_b2b; xo_zero = x_b2b; tho_zero = theta_b2b;
% Without an observer t = t_a2b; x_zero = x_a2b; th_zero = theta_a2b;
figure('Color', [1 1 1]) axes('FontSize',10) plot(t_in,x_in,to, xo_zero,t,x_zero) xlabel('Time (s)') ylabel('Position (m)') title({'Cart Position vs Time for System with and without an
Observer',... '(zero reference with pertubation)'}) legend('Input Reference','With an Observer','Without an Observer')
figure('Color', [1 1 1]) axes('FontSize',10) plot(t_in,x_in,to, tho_zero,t,th_zero) xlabel('Time (s)') ylabel('Angle (rad)') title({'Pendulum Angle vs Time for System with and without an
Observer',... '(zero reference with pertubation)'}) legend('Input Reference','With an Observer','Without an Observer')
25
Lab code (4/5)
%% Part 2C - Sinusoidal reference % Input Reference t_in = linspace(0,9); x_in = .1*sin(2*t_in); % With an observer to = t_b2c; xo_zero = x_b2c; tho_zero = theta_b2c;
% With an observer t = t_a2c; x_zero = x_a2c; th_zero = theta_a2c;
figure('Color', [1 1 1]) axes('FontSize',10) plot(t_in,x_in,to, xo_zero,t,x_zero) xlabel('Time (s)') ylabel('Postion') title({'Cart Position vs Time for System with and without an
Observer',... '(Sinusoidal Reference)'}) legend('Reference','With an Observer','Without an Observer')
figure('Color', [1 1 1]) axes('FontSize',10) plot(to, tho_zero,t,th_zero) xlabel('Time (s)') ylabel('Postion') title({'Cart Position vs Time for System with and without an
Observer',... '(Sinusoidal Reference)'}) legend('With an Observer','Without an Observer')
26
Lab code (5/6)
%% LAB 6B Problem 3 %% Cart Velocity v/s time plots subplot(2,1,1) plot(t_a2c, x_dot_a2c) % legend('using derivative blocks') xlabel('Time(s)'); ylabel('Velocity (m/s)') title('Cart Velocity v/s time using derivative blocks')
subplot(2,1,2) plot(t_b2c, x_dot_b2c) % legend('using observer estimates') xlabel('Time(s)'); ylabel('Velocity (m/s)') title('Cart Velocity v/s time using observer estimates')
%% Pendulum velocity v/s time plots subplot(2,1,1) plot(t_a2c, theta_dot_a2c) xlabel('Time(s)'); ylabel('Pendulum velocity (rad/s)') title('Pendulum velocity v/s time using derivative blocks')
subplot(2,1,2) plot(t_b2c, theta_dot_obs_b2c) xlabel('Time(s)'); ylabel('Pendulum velocity (rad/s)') title('Pendulum velocity v/s time using observer estimates')
1
Lab 6C
LQR Controller Design for Inverted
Pendulum
ME 134/EE 128
Lab Group #4 Tuesday 4-6pm
Mandy Huo, Aldrich Ong, and Deepak Talwar (MAD-HOT Team)
Lab performed on: 11/26/13
Lab due on: 12/3/13
2
Objective The objective of this lab is to design a full-state feedback controller using the Linear
Quadratic Regulator (LQR) design technique and to understand the effect of varying the
penalty matrices 𝑃 and 𝑄 in the cost function 𝐽 on the performance of the closed-loop system.
Pre-Lab We will be using the same model of the inverted pendulum system as used in Lab 6a and Lab
6b. The system state space is given below in Equation 1. The values of the system parameters
are given in Table P1.
�̇� = 𝐴𝑥 + 𝐵𝑢
𝑦 = 𝐶𝑥 where,
𝐴 =
[ 0 1 0 0
0 −𝐾𝑔
2𝐾𝑡𝐾𝑚
𝑅𝑚 [𝑟2 (𝑀 +𝑚4
) + 𝐾𝑔2𝐽𝑚]
−3
4⋅
𝑟2𝑚𝑔
[𝑟2 (𝑀 +𝑚4
) + 𝐾𝑔2𝐽𝑚]
0
0 0 0 1
03
4𝐿𝑝⋅
𝐾𝑔2𝐾𝑡𝐾𝑚
𝑅𝑚 [𝑟2 (𝑀 +𝑚4) + 𝐾𝑔
2𝐽𝑚]
3𝑔
4𝐿𝑝⋅
𝑟2(𝑀 + 𝑚) + 𝐽𝑚𝐾𝑔2
[𝑟2 (𝑀 +𝑚4) + 𝐾𝑔
2𝐽𝑚]0
]
𝐵 =𝑟𝐾𝑔𝐾𝑡
𝑅𝑚 [𝑟2 (𝑀 +𝑚4
) + 𝐾𝑔2𝐽𝑚]
[
010
−3
4𝐿𝑝]
𝐶 = [1 00 0
0 01 0
]
(1)
Table P1: Parameters of the inverted pendulum system.
3
The state vector 𝑥 contains of the following four states: 𝑥, 𝑥,̇ 𝜃, �̇�. The system has a single input:
motor voltage 𝑉. For the LQR Controller, the cost function 𝐽 is given by Equation 2, given
below:
𝐽 = ∫ (𝑥𝑇𝑄𝑥 + 𝑢𝑇𝑅𝑢) 𝑑𝑡∞
0
(2)
Here, 𝑄 and 𝑅 are the weighting matrices and the design parameters. As given by the sizes of 𝑥
and 𝑢, 𝑄 will be a 4×4 matrix and 𝑅 will be a 1×1 matrix. For the purpose of this lab, we will
assume that both 𝑄 and 𝑅 are diagonal matrices, such that the penalty on the states depends
solely on one diagonal element of matrix 𝑄 and that the penalty on the input depends solely on
the diagonal element of matrix 𝑅. Thus, the cost function now reduces to the following:
𝐽 = ∫ (∑𝑞𝑖𝑥𝑖2
𝑛
𝑖=1
+ ∑𝑟𝑗𝑢𝑗2
𝑚
𝑗=1
)∞
0
𝑑𝑡
(3)
Thus, since we don’t have inherent constrains on the velocity of the cart (�̇�) and the angular
velocity of the pendulum (�̇�), their weights, that is 𝑞2 and 𝑞4 will be set to 0. Thus, 𝑞1, 𝑞3 and 𝑟
will be our design variables.
For the pre-lab, we set the initial condition of the cart position 𝑥0 = 30 𝑐𝑚 = 0.3 𝑚 and the
initial condition of the pendulum angle at 𝜃0 = 0.05 𝑟𝑎𝑑 ≈ 2.5°. In order to use scalars 𝑞1, 𝑞3
and 𝑟 we will normalize them based on their initial conditions as follows:
𝑞1̅̅̅ =𝑞1
0.32 𝑞3̅̅ ̅ =
𝑞3
0.052 �̅� =
𝑟
82
Finding controller K using LQR for different values of the design variables
Case 1: Nominal Case
Using 𝑞1 = 1, 𝑞3 = 1, 𝑟 = 1, that is, giving equal weight to each term (nominal case), we will
determine the gain matrix 𝐾 and its associated closed loop poles. This is done using the lqr
command in MATLAB. Using this matrix 𝐾 as the controller and using 𝑥0 = 30 𝑐𝑚 = 0.3 𝑚
and 𝜃0 = 0.05 𝑟𝑎𝑑 ≈ 2.5° as initial conditions for the position of the cart and angle of the
pendulum respectively, we will simulate the system and plot its outputs and the control input.
We will use the same block diagram with the observer as used in Lab 6b. It is given in Figure P1
below. The observer matrix 𝐿 is the same as used in Lab 6b. It is given by:
𝐿 = [
16.1595 −2.3767254.9780 −5.483415.7136 21.0282
180.7734 378.2196
]
4
In this case, the cart position, the pendulum angle and the control input are all equally penalized.
Figure P2(i) shows the plots for the outputs and Figure P2(ii) shows the control input plot.
(a) (i)
Figure P1: Simulink Block diagram for inverted pendulum system with observer and LQR
Controller.
5
Table P2: Case 1: Nominal Case
𝐾 [−26.6667 − 37.1199 − 199.7557 − 24.9849]
Closed Loop Poles −17.4065 ± 15.9453𝑖, −0.9430 ± 0.8569𝑖
𝑥𝑚𝑎𝑥 0.3456 𝑚
𝜃𝑚𝑎𝑥 0.0513 𝑟𝑎𝑑
𝑢𝑚𝑎𝑥 17.9878 𝑉
Since the weights of the penalties are the same for all three parameters, we don’t see
significantly better performance on any one parameter compared to another.
Case 2: 𝒒𝟏 = 𝟏, 𝒒𝟑 = 𝟎. 𝟎𝟏, 𝒓 = 𝟏
Table P3: Case 2: 𝒒𝟏 = 𝟏, 𝒒𝟑 = 𝟎. 𝟎𝟏, 𝒓 = 𝟏
𝐾 [−26.6667 − 23.8002 − 72.4420 − 14.7525]
Closed Loop Poles −7.7553 ± 4.2670𝑖, −3.0411 ± 1.5161𝑖
𝑥𝑚𝑎𝑥 0.3790 𝑚
𝜃𝑚𝑎𝑥 0.1408 𝑟𝑎𝑑
𝑢𝑚𝑎𝑥 11.6221 𝑉
Figure P2: For nominal case. (i) Outputs: cart position and pendulum angle w.r.t time.
(ii) Control input w.r.t time
(ii)
Table P2: Case 1 response parameters.
Table P3: Case 2 response parameters.
6
(i)
(ii)
Figure P3: For Case 2. (i) Outputs: cart position and pendulum angle w.r.t time.
(ii) Control input w.r.t time
7
In this case, the cart position and the control input are equally penalized, but the pendulum angle
is relatively less penalized. Figure P3(i) shows the plots for the outputs and Figure P3(ii) shows
the control input plot. This is why the pendulum angle deviates a lot more from the steady state
value in this case.
Case 3: 𝒒𝟏 = 𝟏, 𝒒𝟑 = 𝟗𝟎, 𝒓 = 𝟏
Table P4: Case 2: 𝒒𝟏 = 𝟏, 𝒒𝟑 = 𝟗𝟎, 𝒓 = 𝟏
𝐾 [−0.0267 × 103 − 0.0963 × 103 − 1.5548 × 103 − 0.0704 × 103]
Closed Loop Poles −51.4715 ± 50.9944𝑖, −0.2950 ± 0.2920𝑖
𝑥𝑚𝑎𝑥 0.3262 𝑚
𝜃𝑚𝑎𝑥 0.0500 𝑟𝑎𝑑
𝑢𝑚𝑎𝑥 85.7401 𝑉
In this case, the cart position and the control input are equally penalized, but the pendulum angle
is relatively more penalized. Figure P4(i) shows the plots for the outputs and Figure P4(ii) shows
the control input plot.
Table P4: Case 3 response parameters.
(i)
8
Since in this case, the pendulum angle is highly penalized, we see that it converges to the steady
state value very quickly and without a lot of deviations. The maximum deviation for the
pendulum angle is seen at the initial condition.
Case 4: 𝒒𝟏 = 𝟏, 𝒒𝟑 = 𝟏, 𝒓 = 𝟎. 𝟎𝟏
Table P5: Case 4: 𝒒𝟏 = 𝟏, 𝒒𝟑 = 𝟏, 𝒓 = 𝟎. 𝟎𝟏
𝐾 [−0.2667 × 103 − 0.3156 × 103 − 1.7869 × 103 − 0.1681 × 103]
Closed Loop Poles −52.9016 ± 52.4387𝑖, −0.9355 ± 0.8692𝑖
𝑥𝑚𝑎𝑥 0.3467 𝑚
𝜃𝑚𝑎𝑥 0.0559 𝑟𝑎𝑑
𝑢𝑚𝑎𝑥 169.3467 𝑉
In this case, the cart position and the pendulum angle are equally penalized, but the input is
relatively less penalized. Figure P5(i) shows the plots for the outputs and Figure P5(ii) shows the
control input plot.
(ii)
Figure P4: For Case 3. (i) Outputs: cart position and pendulum angle w.r.t time.
(ii) Control input w.r.t time
Table P5: Case 4 response parameters.
9
(i)
(ii)
Figure P5: For Case 4. (i) Outputs: cart position and pendulum angle w.r.t time.
(ii) Control input w.r.t time
10
Since in this case, the control input is weakly penalized, we see that it deviates a lot from its
steady state value. From Table P5, we can see that 𝑢𝑚𝑎𝑥 = 169.34 V. This value is way larger
than the values of 𝑢𝑚𝑎𝑥 in previous cases.
Case 5: 𝒒𝟏 = 𝟏, 𝒒𝟑 = 𝟏, 𝒓 = 𝟗𝟎
Table P6: Case 5: 𝒒𝟏 = 𝟏, 𝒒𝟑 = 𝟏, 𝒓 = 𝟗𝟎
𝐾 [−2.8109 − 10.8146 − 42.6391 − 8.0663]
Closed Loop Poles −7.8336 ± 3.8126𝑖, −1.9203 ± 0.0000𝑖
𝑥𝑚𝑎𝑥 0.3481 𝑚
𝜃𝑚𝑎𝑥 0.0500 𝑟𝑎𝑑
𝑢𝑚𝑎𝑥 2.9752 𝑉
In this case, the cart position and the pendulum angle are equally penalized, but the input is
relatively heavily penalized. Figure P6(i) shows the plots for the outputs and Figure P6(ii) shows
the control input plot.
Table P6: Case 5 response parameters.
(i)
11
Since in this case, the control input is heavily penalized, we see that it does not deviate lot from
its steady state value. From Table P6, we can see that 𝑢𝑚𝑎𝑥 = 2.9752 V. This value is way
smaller than the values of 𝑢𝑚𝑎𝑥 in previous case, where it was lightly penalized.
We notice that in all cases the cart at first, moves away from the equilibrium position and then
converges to it. This is because we have a non-zero (positive) initial condition for theta, our
control system wants to quickly stabilize the system and thus moves the cart to the right (positive
direction) initially to stabilize it. When the pendulum is at its equilibrium position, the cart
moves back to its equilibrium position.
(ii)
Figure P6: For Case 5. (i) Outputs: cart position and pendulum angle w.r.t time.
(ii) Control input w.r.t time
12
Lab
We implemented the LQR controller on the hardware. The reference was a step input
ref = [0.3 0 0 0]T. The Simulink block diagram is shown below.
Figure L1: controller_observer.slx – Block diagram for controller for inverted pendulum, using
an observer.
We implemented the LQR controller for four cases:
1. nominal weights: q1 = q3 = r = 1
2. higher relative weight q1: q1 = 10, q3 = 1 and r = 1
3. higher relative weight q3: q1 = 1, q3 = 10 and r = 1
4. higher relative weight r: q1 = 1, q3 = 1 and r = 10
13
Case 1: nominal weights: q1 = q3 = 1 and r = 1
Figure L.2: Plot of cart and pendulum positions and control action for nominal
case, q1 = q3 = r = 1.
As with the prelab, the cart initially moves in the direction away frm the reference position. From
Figure L.2 above, we see that the cart first moves to the left before moving to the reference
position at +30 cm. The controller does this to move the pendulum to the right initially since
when the cart moves to the right to reach the reference position, the pendulum will be moved to
the left. The controller reaches its maximum absolute value umax = 6 V at the beginning to push
the cart quickly to the left. We also saw in the prelab that umax is reached at the beginning.
However, here umax is smaller due to the saturation block which protects the motor.
14
Case 2: higher relative weight q1: q1 = 10, q3 = 1 and r = 1
Figure L.3: Plot of cart and pendulum positions and control action for weights q1
= 10, q3 = 1 and r = 1.
In this case the penalty on cart position deviations is relatively high. This causes the controller to
try to move the cart to the reference position more quickly. We can see this by analyzing Figures
L.2 and L.3. Compared to the nominal case, in case 2 the cart position converges to steady state
quicker, with smaller deviations about the steady state in the last half of the simulation time. In
both cases the control input starts at -6 V. After this initial push, as the cart moves to the right,
the controller in case 2 reaches a higher maximum controller input (4.7 V for case 2 and 2.1 V
for case 1). This is due to the controller in case 2 attempting to bring the cart to the reference
position more quickly than in the nominal case.
15
Case 3: higher relative weight q3: q1 = 1, q3 = 10 and r = 1
Figure L.4: Plot of cart and pendulum positions and control action for q1 = 1, q3 =
10 and r = 1.
In this case the penalty on pendulum angle deviations is relatively high. This causes the
controller to keep the pendulum angle deviations low. From Figures L.2 and L.4 Compared to
the nominal case, in this case the pendulm angle deviations reach a smaller maxmimum value.
The deviations as the cart is moved to the reference position are also smaller in case 3. However
cart position takes a longer time to reach steady-state since the penalty on x deviations is
relatively smaller. The control input also reaches lower maximum absolute values (after the
initial -6 V input) in case 3 to keep the cart velocity low so as to perturb the pendulum less as the
cart reaches the reference position.
16
Case 4: higher relative weight q3: q1 = 1, q3 = 1 and r = 10
Figure L.5: Plot of cart and pendulum positions and control action for q1 = 1, q3 =
1 and r = 10.
In case 4, we increase the penalty for the input to 10 while maintaining the nominal weight for
the cart position and pendulum angle. Figure L.5 shows the cart position, pendulum angle, and
input for the controller with the listed penalties. As expected the maximum input deviation is
much smaller than in the nominal case 1. We notice that the initial input is about -3 volts, which
is lower than all the other cases. The cart position and pendulum angle deviations are larger and
the states do not converge to the desired reference.
In case 5 of the pre-lab the penalty for the input was set to 90, which is higher than the penalty
used in this case. The pre-lab case shows a smoother response than the actual case. All the states
and the input converge to the desired reference unlike in the actual case.
17
Pre-Lab Code
%% EE C128/ ME C134
%% Prelab 6c %% Dimensions of Q and R % Dimensions of Q: 4x4 % % Dimensions of R: 1x1
%% 1. % Setting up the system cx = 439.6*100; % counts/m ctheta = -666.7; % counts/rad
A = [ 0 1.0000 0 0 0 -6.8123 -1.4973 0 0 0 0 1.0000 0 15.4731 25.6828 0];
B = [0 1.5226 0 -3.4583].';
C = [1 0 0 0 0 0 1 0];
D = [0 0]'; cart_sys = ss(A,B,C,D);
% K = [-12.9795 -14.7230 -47.8456 -6.5363]; L = [16.1595 -2.3767 254.9780 -5.4834 15.7136 21.0282 180.7734 378.2196]; %% finding K q1 = 1; q3 = 1; r = 1; q1_bar = q1/(0.3^2); q3_bar = q3/(0.05^2); r_bar = r/(8^2);
Q = [q1_bar 0 0 0 0 0 0 0 0 0 q3_bar 0 0 0 0 0]; R = r_bar;
K6c = lqr(cart_sys, Q, R); observer_6C sim('observer_6C', 10)
18
Prelab (2/ subplot(2,1,1) plot(y_out.time, y_out.signals.values(:,1)) ylabel('Position (m)'); title('y v/s Time for nominal q1, q3, r') subplot(2,1,2) plot(y_out.time, y_out.signals.values(:,2)) xlabel('Time(s)'); ylabel('\theta (rad)');
figure plot(u_out.time, u_out.signals.values) title('Control input v/s time') xlabel('Time(s)'); ylabel('Voltage (V)')
%% 2. clear q1 q3 r q1_bar q3_bar r_bar Q R K6c %% case 1, q1 = 1, q3 = 0.01, r = 1 q1 = 1; q3 = 0.01; r = 1; q1_bar = q1/(0.3^2); q3_bar = q3/(0.05^2); r_bar = r/(8^2);
Q = [q1_bar 0 0 0 0 0 0 0 0 0 q3_bar 0 0 0 0 0]; R = r_bar;
%% (a) [K6c,~,pol] = lqr(cart_sys, Q, R)
sim('observer_6C', 10) %% (b) subplot(2,1,1) plot(y_out.time, y_out.signals.values(:,1)) ylabel('Position (m)'); title('y v/s Time for q1 = 1, q3 = 0.01, r = 1') subplot(2,1,2) plot(y_out.time, y_out.signals.values(:,2)) xlabel('Time(s)'); ylabel('\theta (rad)');
figure plot(u_out.time, u_out.signals.values) title('Control input v/s time') xlabel('Time(s)'); ylabel('Voltage (V)')
%% (c) x_max = max(abs(y_out.signals.values(:,1))) theta_max = max(abs(y_out.signals.values(:,2))) u_max = max(u_out.signals.values)
19
%% (d) % Decreasing q3 will penalize theta less, thus it will take longer for % theta to reach the steady state. %% case 2, q1 = 1, q3 = 90, r = 1 clear q1 q3 r q1_bar q3_bar r_bar Q R K6c q1 = 1; q3 = 90; r = 1; q1_bar = q1/(0.3^2); q3_bar = q3/(0.05^2); r_bar = r/(8^2);
Q = [q1_bar 0 0 0 0 0 0 0 0 0 q3_bar 0 0 0 0 0]; R = r_bar; %% (a) [K6c,~,pol] = lqr(cart_sys, Q, R)
sim('observer_6C', 10) %% (b) subplot(2,1,1) plot(y_out.time, y_out.signals.values(:,1)) ylabel('Position (m)'); title('y v/s Time for q1 = 1 q3 = 90, r = 1') subplot(2,1,2) plot(y_out.time, y_out.signals.values(:,2)) xlabel('Time(s)'); ylabel('\theta (rad)');
figure plot(u_out.time, u_out.signals.values) title('Control input v/s time') xlabel('Time(s)'); ylabel('Voltage (V)')
%% (c) x_max = max(abs(y_out.signals.values(:,1))) theta_max = max(abs(y_out.signals.values(:,2))) u_max = max(u_out.signals.values)
%% (d) % Increasing q3 will penalize theta more, thus making it go to steady state % much earlier. %% case 3, q1 = 1, q3 = 1, r = 0.01 clear q1 q3 r q1_bar q3_bar r_bar Q R K6c q1 = 1; q3 = 1; r = 0.01; q1_bar = q1/(0.3^2); q3_bar = q3/(0.05^2); r_bar = r/(8^2);
Q = [q1_bar 0 0 0 0 0 0 0 0 0 q3_bar 0 0 0 0 0]; R = r_bar;
20
%% (a) [K6c,~,pol] = lqr(cart_sys, Q, R)
sim('observer_6C', 10) %% (b) subplot(2,1,1) plot(y_out.time, y_out.signals.values(:,1)) ylabel('Position (m)'); title('y v/s Time for q1 = 1, q3 = 1, r = 0.01') subplot(2,1,2) plot(y_out.time, y_out.signals.values(:,2)) xlabel('Time(s)'); ylabel('\theta (rad)');
figure plot(u_out.time, u_out.signals.values) title('Control input v/s time') xlabel('Time(s)'); ylabel('Voltage (V)')
%% (c) x_max = max(abs(y_out.signals.values(:,1))) theta_max = max(abs(y_out.signals.values(:,2))) u_max = max(u_out.signals.values)
%% (d) % Decreasing r penalizes it less, thus letting it have a really big(or % really small) value, which in turn makes the system stabilize early. %% case 4, q1 = 1, q3 = 1, r = 90 clear q1 q3 r q1_bar q3_bar r_bar Q R K6c q1 = 1; q3 = 1; r = 90; q1_bar = q1/(0.3^2); q3_bar = q3/(0.05^2); r_bar = r/(8^2);
Q = [q1_bar 0 0 0 0 0 0 0 0 0 q3_bar 0 0 0 0 0]; R = r_bar; %% (a) [K6c,~,pol] = lqr(cart_sys, Q, R)
sim('observer_6C', 10)
21
%% (b) subplot(2,1,1) plot(y_out.time, y_out.signals.values(:,1)) ylabel('Position (m)'); title('y v/s Time for q1 = 1, q3 = 1, r = 90') subplot(2,1,2) plot(y_out.time, y_out.signals.values(:,2)) xlabel('Time(s)'); ylabel('\theta (rad)');
figure plot(u_out.time, u_out.signals.values) title('Control input v/s time') xlabel('Time(s)'); ylabel('Voltage (V)')
%% (c) x_max = max(abs(y_out.signals.values(:,1))) theta_max = max(abs(y_out.signals.values(:,2))) u_max = max(u_out.signals.values)
%% (d) % Increasing r penalizes it more, thus limiting its value, and in turn % making the system stabilize much slower.
%% case 5 nominal clear q1 q3 r q1_bar q3_bar r_bar Q R K6c q1 = 1; q3 = 1; r = 1; q1_bar = q1/(0.3^2); q3_bar = q3/(0.05^2); r_bar = r/(8^2);
Q = [q1_bar 0 0 0 0 0 0 0 0 0 q3_bar 0 0 0 0 0]; R = r_bar; %% (a) [K6c,~,pol] = lqr(cart_sys, Q, R)
sim('observer_6C', 10)
%% (b) subplot(2,1,1) plot(y_out.time, y_out.signals.values(:,1)) ylabel('Position (m)'); title('y v/s Time for q1 = 1, q3 = 1, r = 1') subplot(2,1,2) plot(y_out.time, y_out.signals.values(:,2)) xlabel('Time(s)'); ylabel('\theta (rad)');
figure plot(u_out.time, u_out.signals.values) title('Control input v/s time') xlabel('Time(s)'); ylabel('Voltage (V)')
22
%% (c) x_max = max(abs(y_out.signals.values(:,1))) theta_max = max(abs(y_out.signals.values(:,2))) u_max = max(u_out.signals.values)
%% 3. % Since we have a non-zero (positive) initial condition for theta, our
control system wants to % quickly stabilize the system and thus move the cart to the right % (positive direction) initially to stabilize it. This is why we see a % sharp positive gain in the cart position near t = 0s.
23
Lab Matlab code (1/4)
%%% Lab 6C - ME 134 C - Tues 4-6pm % Experiment conducted on 11/26/13 % MAD-HOT Team load('lab6c_data')
%% Initialize parameters % Conversion parameters cx = 439.6*100; % counts/m ctheta = -666.7; % counts/rad
% Model Parameters A = [ 0 1.0000 0 0 0 -6.8123 -1.4973 0 0 0 0 1.0000 0 15.4731 25.6828 0];
B = [0 1.5226 0 -3.4583].';
C = [1 0 0 0 0 0 1 0]; % Observer Luenberger L = [16.1595 -2.3767 254.9780 -5.4834 15.7136 21.0282 180.7734 378.2196]; %% Reference Parameters x_des = 0.3; th_des = 0; ref = [x_des 0 th_des 0]';
%% amp = [0 0 0.2 0]'; w = 6; %% % Initial input array for graphs t_in = linspace(0,9); x_in = x_des*ones(1,100); th_in = th_des*ones(1,100);
24
Lab Matlab code (2/4)
%% Case 1 - nominal weights q1 = 1; q3 = 1; r = 1; [K,e] = lqr_controller(q1,q3,r); %% t_1 = y_out.time; x_1 = y_out.signals.values(:,1); th_1 = y_out.signals.values(:,2); u_1 = u_out.signals.values; %% alot(t_in,x_in,th_in,t_1,x_1,th_1,u_1,q1,q3,r)
%% Case 2 - q1 >> 1 q1 = 10; q3 = 1; r = 1; [K,e] = lqr_controller(q1,q3,r); %% t_2 = y_out.time; x_2 = y_out.signals.values(:,1); th_2 = y_out.signals.values(:,2); u_2 = u_out.signals.values; %% figure alot(t_in,x_in,th_in,t_2,x_2,th_2,u_2,q1,q3,r)
%% Case 3 - q3 >> 1 q1 = 1; q3 = 15; r = 1; [K,e] = lqr_controller(q1,q3,r); %% t_3 = y_out.time; x_3 = y_out.signals.values(:,1); th_3 = y_out.signals.values(:,2); u_3 = u_out.signals.values; %% figure alot(t_in,x_in,th_in,t_3,x_3,th_3,u_3,q1,q3,r) %% Case 4 - r >> 1 q1 = 1; q3 = 1; r = 10; [K,e] = lqr_controller(q1,q3,r); %% t_4 = y_out.time; x_4 = y_out.signals.values(:,1); th_4 = y_out.signals.values(:,2); u_4 = u_out.signals.values; %% figure alot(t_in,x_in,th_in,t_4,x_4,th_4,u_4,q1,q3,r)
25
Lab Matlab code (3/4)
function [K, e] = lqr_controller(q1,q3,r_i) % determines the gain of the controller for the given penalties
% System parameters Kt = 7.67e-3; % Nm/A Kg = 3.71; Rm = 2.6 ;% ohms r = 6.36e-3 ; % m Jm = 3.9e-7 ; % kgm^2 Km = 7.67e-3; % Vs/rad M = 0.94 ; % kg m = 0.230; % kg Lp = 0.3302; % m g = 9.81; % m/s a = 1/(M+.25*m+Kg^2*Jm/r^2); b = Kg^2*Kt*Km/(r^2*Rm); c = Kg*Kt/(r*Rm);
A = [0 1 0 0 0 -a*b -0.75*m*g*a 0 0 0 0 1 0 0.75*a*b/Lp (1+0.75*a*m)*0.75*g/Lp 0]; B = [ 0 a*c 0 -0.75*a*c/Lp]; C = [1 0 0 0 0 0 1 0]; D = 0;
% p = [sc1 sc2 sc3 sc4];
% Initialize Q and R matrix q1_bar = q1/0.3^2; q3_bar = q3/0.05^2; r_bar = r_i/8^2; Q = [q1_bar 0 0 0 0 0 0 0 0 0 q3_bar 0 0 0 0 0]; R = r_bar; N = 0; [K,S,e] = lqr(A,B,Q,R,N); % K = controller gain % e = close loop poles
26
Lab Matlab code (4/4)
function [] = alot(t_in,x_in,th_in,t,x,th,u,q1,q3,r)
% plot’s the state variables and the input
subplot(2,2,1) plot(t_in,x_in,t,x) xlabel('Time (s)') ylabel('Position (m)') title(['Position vs time' ' q1 = ' num2str(q1) ' q3 = ' num2str(q3) ' r = '
num2str(r)]) legend('Reference Input','Output') subplot(2,2,3) plot(t_in,th_in,t,th) xlabel('Time (s)') ylabel('Pendulum Angle (rad)') title(['Pendulum angle vs time' ' q1 = ' num2str(q1) ' q3 = ' num2str(q3) '
r = ' num2str(r)]) legend('Reference Input','Output') subplot(1,2,2) plot(t,u) xlabel('Time (s)') ylabel('Input (V)') title(['Input vs time' ' q1 = ' num2str(q1) ' q3 = ' num2str(q3) ' r = '
num2str(r)])
1
Lab 6D
Self-Erecting Inverting Pendulum
ME 134/EE 128
Lab Group #4 Tuesday 4-6pm Mandy Huo, Aldrich Ong, and Deepak Talwar (MAD-HOT Team)
Lab performed on: 12/3/13
Lab due on: 12/8/13
2
Objective
The objective of this lab is to create a controller that can erect the inverted pendulum by itself. In
order to do this we will implement two controller which will switch at a specified angle. The first
controller will make the invert the pendulum to its erected position, while the second controller
will operate in the linear region and maintain the pendulum in its erected position.
Pre-Lab
Figure P1 – Free body diagram of inverted pendulum
Figure 1 shows the free body diagram of the inverted pendulum. To have the pendulum self-erect,
we will implement an energy analysis into our controller.
The total mechanical energy of the pendulum without the cart is shown in Equation 1.
𝐸 =1
2𝐽�̇�2 + 𝑚𝑔𝐿𝑝(cos 𝜃 − 1) (1)
Note that at 𝜃 = 0 and the pendulum is at rest, the total mechanical energy is zero.
When the pendulum is at rest in its stable equilibrium point, 𝜃 = 𝜋, the total energy is
𝐸(𝜃, �̇�) = −𝑚𝑔𝐿𝑝
We will be implementing a controller that will try to make the total energy go to zero. In order
for the controller to accomplish this as fast of possible, we need to explore the rate of the total
mechanical energy which was derived and shown in Equation 2.
𝑑𝐸(𝜃, �̇�)
𝑑𝑡= −𝑚𝐿𝑝�̈��̇� cos(𝜃) (2)
3
We notice from Equation 2 that to maximize the rate of total mechanical energy, we can vary �̈�
depending on what �̇� cos(𝜃) term is. Since �̈� is proportional to 𝑉, voltage input to the motor, we
have the controller apply the maximum voltage (𝑉𝑚𝑎𝑥 = 6𝑉) to get the maximum acceleration. Equation 3 shows what the voltage should be to bring the total energy to zero.
𝑉 = {
0𝑉𝑚𝑎𝑥
−𝑉𝑚𝑎𝑥
𝑤ℎ𝑒𝑛 𝐸(𝜃, �̇�) ≥ 0
𝑤ℎ𝑒𝑛 𝐸(𝜃, … 𝜃) < 0 𝑎𝑛𝑑 �̇� cos(𝜃) < 0
𝑤ℎ𝑒𝑛 𝐸(𝜃, … 𝜃) < 0 𝑎𝑛𝑑 �̇� cos(𝜃) > 0
(3)
Since the linear approximation is invalid when the angle is large, we cannot use the observer to
estimate �̇� and �̇�. In order to estimate these states we will use a transfer function to estimate the
derivate. The form of the transfer function is
𝐻 =1
𝑐𝑙𝑝𝑠 + 1
We want the phase shift to be small such that |𝜙(𝐻𝑙𝑝(𝑗𝑤0 ))| ≤ 5° for 𝑤0 = √𝑔
𝐿𝑝. We solve for
𝑐𝑙𝑝 for these parameters. We find that 𝑐𝑙𝑝 = 0.0161.
4
Lab
We implemented a hybrid controller which switches between the LQR controller designed in
Lab 6c, with higher relative weights on the regulatory term than the actuation term, and the
swing-up controller designed in the Pre-lab.
Implementing the swing-up controller
The full system
The full system block diagram is shown below. This contains subsystems which are described in
detail later.
Figure L1 – SEIP block diagram with subsystems
The inverted pendulum hardware
The inverted pendulum hardware block diagram is shown below. This subsystem takes the input
voltage and interfaces with the hardware. The output is the state of the system. The state estimate
derived in the Pre-lab is used.
5
Figure L2 – IP hardware subsystem
The mode selector
The mode selector block diagram is shown below. This block diagram output 1 when the
magnitude of the pendulum angle is less than or equal to a set threshold value. The output is -1
otherwise. This controls switch1 in the SEIP block diagram.
Figure L3 – Mode selector
The swing-up controller
The swing-up controller block diagram is shown below. This block diagram implements the
voltage switching condition in (2). The input is the state and the output is the input voltage to the
motor.
6
Figure L4 – Swing-up controller
The observer
The observer subsystem block diagram is shown below. The subsystem takes in the input, mode,
and y. Depending on the mode, this subsystem switches on the observer for LQR control (when θ
is small so the linearized system is a valid approximation).
Figure L5 – Observer subsystems
Testing and debugging the swing-up controller
When we tested the controller it was unable to swing the pendulum up to the unstable
equilibrium position. We calibrated the system by finding the actual 𝜃 value measured by the
7
encoder when the pendulum is at the equilibrium position. We then added a correction factor to
the encoder conversion factor so that the encoder would correctly output 𝜃 = 0° when the pendulum is vertical. It is important to make sure that the measured angle is correct as this
affects the calculated energy of the system as well as the switching condition and causes the
controller to stabilize the pendulum around a non-vertical angle. We also tweaked the system
parameters, setting the threshold angle to 𝜃 = 15° and Kpump = 50. We found that when
changing Kpump there was no noticeable difference in the performance of the controller since
the input voltage was always saturating at ± 6V while the swing-up controller was active.
Figure L6 and L7 give us the plots of the Cart Position and Pendulum Position respectively. The
threshold is set to 15° and is plotted on Figure L7. This is when the LQR controller kicks in to stabilize the pendulum.
Figure L6 – Cart Position v/s Time
8
Figure L7 – Pendulum angle v/s Time with the threshold angle.
Figure L8 and L9 give us the plots of the Control Input and System Energy respectively.
Figure L8 – Control Input v/s Time
9
Figure L9 – System Energy v/s Time
Figures L10 and L11 give us the plots of the states as estimated by the observer. Since the
observer only kicks in after the angle threshold is reached, its estimates are zero before that.
Figure L10 – Observer Estimates for cart position and velocities
10
Figure L11 – Pendulum Position and Velocity as estimated by the observer.
Video Link Please click on the following link to view our implementation of the inverted pendulum
http://youtu.be/0sC1oZYZxBI
11
Code
%%% Lab 6d
clear; clc;
m = 0.230; % kg Lp = 0.3302; % m g = 9.81; % m/s
J = (4/3)*m*Lp^2;
cx = 43959; ctheta = -666.7; c_lp = 0.01606;
% Model Parameters A = [ 0 1.0000 0 0 0 -6.8123 -1.4973 0 0 0 0 1.0000 0 15.4731 25.6828 0];
B = [0 1.5226 0 -3.4583].';
C = [1 0 0 0 0 0 1 0]; % Observer Luenberger L = [16.1595 -2.3767 254.9780 -5.4834 15.7136 21.0282 180.7734 378.2196]; %% delay = 0.5; Kpump = 50; thresh = 15*pi/180; ref = [.2 0 0 0]';
q1 = 3; q3 = 3; r = 1; [K,~] = lqr_controller(q1,q3,r);
theta_corr = pi/(pi-.08); %% plot(E.time,E.signals.values)
12
%% figure (1) theta = x.signals.values(:,3); plot(x.time,theta)
%% theta = xhat.signals.values(:,3); plot(xhat.time,theta) %% figure (2) plot(u.time,u.signals.values) %% xhat2 = xhat; x2 = x; u2 = u; E2 = E;
%% Plots figure(1) plot(u1.time,u1.signals.values) xlabel('Time(s)'); ylabel('Voltage(V)'); title('Control Input v/s Time')
figure(2) plot(E1.time,E1.signals.values) xlabel('Time(s)'); ylabel('Energy(Joules)'); title('Energy v/s Time')
figure(3) plot(x1.time,x1.signals.values(:,1)) xlabel('Time(s)'); ylabel('Position(m)'); title('Cart Position v/s Time')
figure(4) hold on plot(x1.time,x1.signals.values(:,3)) xlabel('Time(s)'); ylabel('Pendulum Angle(rad)'); title('Pendulum Angle v/s Time') plot(x1.time, 15*(pi/180)*ones(1,length(x1.time)), '--') legend('Pendulum Angle', 'Threshold = 15 deg') hold off
figure(5) subplot(2,1,1) plot(xhat.time, xhat.signals.values(:,1)) xlabel('Time(s)'); ylabel('Cart Position(m)'); title('Cart Position and Velocity as estimated by observer')
subplot(2,1,2) plot(xhat.time, xhat.signals.values(:,2)) xlabel('Time(s)'); ylabel('Cart Velocity(m/s)');