ENPM667: Control of Robotic Systems

Final Project

University of Maryland, College Park

By:

Satyarth Praveen (116752749)

Sandeep Kota Sai Pavan (116911603)

 Abstract

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

design of an observer followed by the design of an LQG controller.

1
PROBLEM A:

Fig.1 ​Cart with pendulum system with 2 pendulum

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

(FBD) of the masses is depicted as shown in ​Fig. ​2.

Fig. 2 ​Free body diagram of the forces acting on masses

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

masses M, M​1​, and M​2​ are given below in equations 1, 2 &3.​.

… (1)

2
 … (2)

… (3)

The equations to balance the torques at the focal point due to the forces on M​1 and

mass M​2​ is shown in the equations 4 & 5 below.

… (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.

These can be represented as

… (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

and torque balance equations.

… (19)

… (20)

… (21)

… (22)

4
 … (23)

Multiplying the first three equations in the above set of equations by , and the last two

equations with we get.

… (24)

… (25)

… (26)

… (27)

… (28)

To eliminate the terms containing the tensions in the two links attaching the masses M​1

and M​2 to
​ the cart, we add the first three equations of the above equations set. Upon

performing the addition we get the following non-linear state equations.

… (29)

Simplifying the other equations we get the non-linear state equations

… (30)

… (31)

The non-linear system in matrix form can be represented as following

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

in the jacobian. Similarly the coefficients of the B matrix can be computed.

The linear time-invariant state model of the given system can be written as

… (33)

… (34)

where represents the state variables

… (34)

… (35)

6
 ​ … (36)

PROBLEM C:

Conditions on , , , , for which the linearized system is controllable.

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

controllability matrix should not be zero.

… (37)

… (38)

We know the denominator cannot be zero because M (mass) is not zero.

Hence, for the controllability condition, the numerator should not be zero.

… (39)

Also, the gravitational constant cannot be zero, hence:

… (40)

7
PROBLEM D:

From part C, we see that the controllability of the system can be confirmed if the

determinant of the controllability matrix is not zero.

… (41)

For the given values of the masses and cable lengths,

… (42)

… (43)

8
 … (44)

Hence, the system is controllable for the given values of the masses and cable lengths.

From the values of and , we also see that .

For LQR Controller, the cost function is defined as:

… (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.

Following is the K vector obtained from above,

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

states of the system namely (top graph to bottom graph) with

increasing time. Looking at the graphs below, it can be seen that the system almost

converges to a stable state within 30 seconds.

Observing the poles of the closed-loop matrix, we can evaluate the stability of the

system.

10
 … (49)

Poles can be found by calculating the eigenvalues of .

For the above-defined , , , and , the poles of the system are:

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);

A = [​0​ ​1​ ​0​ ​0​ 0

​ ​ ​0​; ​0​ ​0​ a ​0​ b ​0​; ​0​ ​0​ ​0​ ​1​ ​0​ ​0​; ​0​ ​0​ c ​0​ d ​0​; ​0​ ​0​ ​0
0​ ​ ​
0 ​1​; ​0​ ​0​ e 0 ​ ​ f ​0​;];
B = [​0​; ​1​/M; 0 ​ ​; ​1​/(M*l1); ​0​; ​1​/(M*l2)];
C = ​eye​(​6​);
D = []

%% 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

has to be analyzed. The output matrix with the feedback states as is

, similarly for the combination , , the

combination will have and finally, for the combination

the vector would be .

By analyzing the rank of the observability matrices of the above combinations of

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:

%% Part E - Observable states

C_x = [​1​ ​0​ ​0​ ​0​ ​0​ ​0​]; ​% Observable
rank(obsv(A, C_x)) ​% 6
C_t1t2 = [​0​ ​0​ ​1​ ​0​ ​1​ ​0​]; ​% Not observable
rank(obsv(A, C_t1t2)) ​% 4
C_xt2 = [​1​ ​0​ ​0​ ​0​ ​1​ ​0​]; ​% Observable
rank(obsv(A, C_xt2)) ​% 6
C_xt1t2 = [​1​ ​0​ ​1​ ​0​ ​1​ ​0​]; ​% Observable
rank(obsv(A, C_xt1t2)) ​% 6
PROBLEM F

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

based on the input function ​U.​ T

​ he general form of a Luenberger equation can be written

as given below.

… (51)

and the observer error can be given as

13
 … (52)

Therefore in order to design the observer, we need to place the eigenvalues of the

observer system state at the eigenvalues at which our system operates on by

pole placement method.

Code Snippet:

%% Part F - Luenberger observer

C_xt1t2 = [​1​ ​0​ ​1​ ​0​ ​1​ ​0​];
C_x = [​1​ ​0​ ​0​ ​0​ ​0​ ​0​];

poles = [​-0.7068​ + ​1.1717​i; -​ 0.7068​ - ​1.1717​i; -​ 0.2687​ +

0.3420​i; ​-0.2687​ - 0
​ .3420​i; -​ 0.1273​ + 0
​ .7681​i; -​ 0.1273​ -
0.7681​i];

L = place(A', C_x', poles);

% 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

of the states to equilibrium.

PROBLEM G

Following is the design of an output feedback controller for the control state . Hence

the matrix is .

Since Linear Quadratic Gaussian (LQG) is a combination of Linear Quadratic Estimator

(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

them to get the LQG model.

15
 … (53)

Hence the new and matrix for the LQG model would be:

… (54)

… (55)

Code Snippet:

%% Augment system with disturbances and noise

Vd = [​0.01​ ​0​ ​0​ ​0​ ​0​ ​0​; ​0​ ​0.0005​ ​0​ ​0​ ​0​ ​0​; ​0​ ​0​ ​0.0001​ ​0​ ​0​ ​0​; ​0​ ​0​ ​0
0.0001​ ​0​ ​0​; ​0​ ​0​ ​0​ ​0​ ​0.0001​ ​0​; ​0​ ​0​ ​0​ ​0​ ​0​ ​0.0001​;]; ​% disturbance
covariance
Vn = ​10​; ​% noise covariance

BF = [B Vd ​0​*B]; ​% augment inputs to include disturbance and

noise

C = [​1​ ​0​ ​0​ ​0​ ​0​ ​0​];

sysC = ss(A,BF,C,[​0​ ​0​ ​0​ ​0​ 0
​ ​ ​0​ ​0​ Vn]); ​% build state space
system with single output x.

sysFullOutput = ss(A,BF,​eye​(​6​),​zeros​(​6​,​size​(BF,​2​))); ​% system

with full state output, disturbance, no noise

%% 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)

%% LQG Pole placement

poles = [​-0.7068​ + ​1.1717​i; ​-0.7068​ - ​1.1717​i; ​-0.2687​ + 0 ​ .3420​i;
-0.2687​ - ​0.3420​i; ​-0.1273​ + ​0.7681​i; ​-0.1273​ - ​0.7681​i; - ​ 0.7068
+ ​1.1717​i; ​-0.7068​ - ​1.1717​i; ​-0.2687​ + ​0.3420​i; ​-0.2687​ -
0.3420​i; ​-0.1273​ + ​0.7681​i; ​-0.1273​ - ​0.7681​i];

C = [​1​ ​0​ ​0​ ​0​ ​0​ ​0 1 0 0 0 0 0​];

L = place(A_lqg', C', poles);

17