ENPM667: Control of Robotic Systems Final Project: University of Maryland, College Park
ENPM667: Control of Robotic Systems Final Project: University of Maryland, College Park
Final Project
By:
This report is a review of the work done for the final project of the course
ENPM667: Control of Robotic Systems. This work consists of two components, with the
first part being to design state space equation for a special case of the famous cart with
pendulum problem, where there are two pendulum attached to the cart. The first part also
discusses the controllability criterion of the system and also design of an LQR controller
to control few states. The second part discusses about the observability criterion and also
1
PROBLEM A:
We will use the Newton-Euler Method to write the non-linear model equation.
We will consider the forces on all the mass objects in the system. The free body diagram
The forces are balanced on the different mass objects of the system and the torque
generated by each mass is balanced separately. The equations that balance forces on the
… (1)
2
… (2)
… (3)
The equations to balance the torques at the focal point due to the forces on M1 and
… (4)
… (5)
Consider the origin frame to be at the lift side of the rail on which the cart is moving. The
position vectors, of the masses can be represented by , , for all the masses.
… (6)
… (7)
… (8)
… (9)
… (10)
… (11)
… (12)
… (13)
… (14)
3
The vector cross product terms for balancing the torque equations are as follows
… (15)
… (16)
… (17)
… (18)
After substituting the values of position vector and its derivatives along with the
cross product values in the torque balance equations, we get the following force balance
… (19)
… (20)
… (21)
… (22)
4
… (23)
Multiplying the first three equations in the above set of equations by , and the last two
… (24)
… (25)
… (26)
… (27)
… (28)
To eliminate the terms containing the tensions in the two links attaching the masses M1
and M2 to
the cart, we add the first three equations of the above equations set. Upon
… (29)
… (30)
… (31)
5
… (32)
PROBLEM B:
For linearization, the coefficients of the A matrix are found by computing the
jacobian of the non-linear state equations and substituting the equilibrium point condition
The linear time-invariant state model of the given system can be written as
… (33)
… (34)
… (34)
… (35)
6
… (36)
PROBLEM C:
For the linearized system to be controllable, the rank of the controllability matrix should
be equal to the order of the system, which means that the determinant of the
… (37)
… (38)
Hence, for the controllability condition, the numerator should not be zero.
… (39)
… (40)
7
PROBLEM D:
From part C, we see that the controllability of the system can be confirmed if the
… (41)
… (42)
… (43)
8
… (44)
Hence, the system is controllable for the given values of the masses and cable lengths.
… (45)
And the closed-loop gain matrix (K) is found by minimizing the cost function where
Following are the tuned Q and R matrices used to find the K matrix:
… (46)
… (47)
Each diagonal element in the Q matrix denotes how important the corresponding state
parameter is and thus a higher value corresponds to a greater penalization for poor system
performance. Similarly, R defines how much to penalize the system for its input efforts.
9
…
(48)
The graphs below show the time response characteristics of the six states
respectively . The graph below shows the time response of the different
increasing time. Looking at the graphs below, it can be seen that the system almost
Observing the poles of the closed-loop matrix, we can evaluate the stability of the
system.
10
… (49)
Looking at the values we can tell that the poles lie in the negative x-axis but not too far
from 0. Hence, we can be certain that the system converges within a reasonable amount
of time without tipping off the actuators with unreasonably large input values.
Code Snippets:
%% State-space equations
g = 9.8;
M1 = 100;
M2 = 100;
M = 1000;
l1 = 20;
l2 = 10;
a = -M1*g/M;
b = -M2*g/M;
c = -(M+M1)*g/(M*l1);
d = -M2*g/(M*l1);
e = -M1*g/(M*l2);
f = -(M+M2)*g/(M*l2);
%% LQR Controller
11
Q = [10^7 0 0 0 0 0; 0 10^3 0 0 0 0; 0 0 10^10 0 0 0; 0 0 0
10^9 0 0; 0 0 0 0 10^10 0; 0 0 0 0 0 1 0^9;];
R = 50;
Kr = lqr(A,B,Q,R);
%% Visualization
C = eye(6)
D = [0;0;0;0;0;0;]
x0 = [5;0;0;0;0;0;]
sys = ss((A - (B*Kr)), B, C, D);
[y,t,x] = initial(sys,x0)
PROBLEM E
The observability of the different states can be found by finding the rank of the
observability matrix. The formula of the observability matrix can be given by the
following equation
… (50)
The observability for different combination of the output matrix and the state matrix
12
observability matrices it can be said that the observers when having a state feedback of
the position from the system, can estimate the other states based on the position.
All the observability matrices. Also by finding the gramian of the observability matrices,
we get that the output vectors having a positive coefficient for $$ x(t)$$ are more
favourable for getting the inputs and hence the first state is selected for the design of
observer.
Code Snippet:
A state observer is a system that provides an estimate of the states of the real system
based on the measurements of one or more states of the real systems. Observers are
typically useful in situations where there is not enough states that could be measured with
real hardware. An observer would need a state feedback and the input action acted upon
the system. Based in the state feedback of one state, the observer predicts the other states
as given below.
… (51)
13
… (52)
Therefore in order to design the observer, we need to place the eigenvalues of the
Code Snippet:
% Initial step
x0 = [5;0;1;0;0.5;0;]
sys = ss((A - (L'*C)), B, C, D);
[y,t,x] = initial(sys,x0)
14
Each graph corresponds to the individual variable state in X showcasing the conversion
PROBLEM G
Following is the design of an output feedback controller for the control state . Hence
the matrix is .
(LQE) and Linear Quadratic Regulator (LQR) and they comply with the Separation
Principle, hence it is possible to consider LQE and LQR individually and then combine
15
… (53)
Hence the new and matrix for the LQG model would be:
… (54)
… (55)
Code Snippet:
%% LQE
[L,P,E] = lqe(A,Vd,C,Vd,Vn); % design estimator
sysKF = ss(A-L*C,[B L],eye(6),0*[B L]); % estimator
%% LQR Controller
Q = [10^7 0 0 0 0 0; 0 10^3 0 0 0 0; 0 0 10^10 0 0 0; 0 0 0
10^9 0 0; 0 0 0 0 10^10 0; 0 0 0 0 0 1 0^9;];
R = 50;
Kr = lqr(A,B,Q,R);
%% LQG
A_lqg = [(A - B*Kr) (B*Kr); zeros(6,6) (A - L*C);]
B_lqg = [eye(0) zeros(6,1); eye(0) -L;]
16
sysLQG = ss(A_lqg, B_lqg, eye(12), []); % LQG model
x0 = [5;0;1;0;0.5;0;0.1;0;0.1;0;0.2;0;]
[y,t,x] = initial(sysLQG,x0)
17