CTM: Digital Control Example: Inverted Pendulum

Digital Control Example: Inverted Pendulum using State-Space method

Discrete state-space
Controllability and Observability
Control design via pole placement
Reference input
Observer design

In this digital control version of the inverted pendulum problem, we are going to use the state-space method to design the digital controller. If you refer to the Inverted Pendulum Modeling page, the state-space equations were derived as

where

M	mass of the cart	0.5 kg
m	mass of the pendulum	0.5 kg
b	friction of the cart	0.1 N/m/sec
l	length to pendulum center of mass	0.3 m
I	inertia of the pendulum	0.006 kg*m^2
u	step force applied to the cart
x	cart position coordinate
phi	pendulum angle from vertical

Output are the cart displacement (x in meters) and the pendulum deflection angle (phi in radians).

The design requirements are

Settling time for x and phi less than 5 seconds
Rise time for x of less than 1 second
Overshoot of phi less than 0.35 rad (20 deg)
Steady-state error of x and phi less than 2%

Discrete state-space

The first thing to do here is to convert the above continuous state-space equations to discrete state-space. To do this, we are going to use the Matlab function called c2dm. To use this c2dm, we need to specify six arguments: four state-space matrices (A, B, C, and D), sampling time (Ts in sec/sample), and the 'method'. You should already be familiar with how to enter A, B, C, and D matrices. The sampling time should be smaller than 1/(30*BW) sec, where BW is the closed-loop bandwidth frequency. The method we will use is the zero-order hold ('zoh').

Assuming that the closed-loop bandwidth frequencies are around 1 rad/sec for both the cart and the pendulum, let the sampling time be 1/100 sec/sample. Now we are ready to use c2dm. Enter the following commands to an m-file.


M = .5;
m = 0.2;
b = 0.1;
i = 0.006;
g = 9.8;
l = 0.3;

p = i*(M+m)+M*m*l^2; %denominator for the A and B matricies

A = [0      1              0           0;
     0 -(i+m*l^2)*b/p  (m^2*g*l^2)/p   0;
     0      0              0           1;
     0 -(m*l*b)/p       m*g*l*(M+m)/p  0];

B = [     0; 
    (i+m*l^2)/p;
          0;
      m*l/p];
       
C = [1 0 0 0;
     0 0 1 0];
       
D = [0;
     0];
     
Ts=1/100;

[F,G,H,J]=c2dm (A,B,C,D,Ts,'zoh')

Running this m-file in the Matlab command window gives you the following four matrices.


	F = 

      		1.0000   0.0100   0.0001   0.0000
           	     0   0.9982   0.0267   0.0001
	   	     0   0.0000   1.0016   0.0100
	   	     0  -0.0045   0.3119   1.0016
	   
	G =
 
       		0.0001
      		0.0182
      		0.0002
      		0.0454
      
	H = 

     	 	1    0    0    0
      		0    0    1    0
      
	J =   

      		0
      		0

Now we have obtained the discrete state-space model of the form

Controllability and Observability

The next step is to check the controllability and the observability of the system. For the system to be completely state controllable, the controllability matrix

must have the rank of n. The rank of the matrix is the number of independent rows (or columns). In the same token, for the system to be completely state observable, the observability matrix

must also have the rank of n.

Since our controllability matrix and observability matrix are '4x4', the rank of both matrices must be 4. The function rank can give you the rank of each matrix. In an new m-file, enter the following commands and run it in the command window.


F = [1.0000   0.0100   0.0001   0.0000;
          0   0.9982   0.0267   0.0001;
	  0   0.0000   1.0016   0.0100;
	  0  -0.0045   0.3119   1.0016];
	   
G =  [0.0001;
      0.0182;
      0.0002;
      0.0454];
      
H =  [1    0    0    0;
      0    0    1    0];
      
J =   [0;
      0];
      
co = ctrb (F,G);
ob = obsv (F,H);

Controllability = rank (co)
Observability = rank (ob)

In the command window, you should see

	Controllability = 

     	     4
     
	Observability = 
     
     	     4

This proves that our discrete system is both completely state controllable and completely state observable.

Control design via pole placement

The schematic of a full-state feedback system is shown below.

The next step is to assume that all four state are measurable, and find the control matrix (K). If you refer to the continuous Inverted Pendulum: State-Space page, the Linear Quadratic Regulator (LQR) method was used to find the control matrix (K). In this digital version, we will use the same LQR method. This method allows you to find the optimal control matrix that results in some balance between system errors and control effort. Please consult your control textbook for details. To use this LQR method, we need to find three parameters: Performance index matrix (R), state-cost matrix (Q), and weighting factors. For simplicity, we will choose the performance index matrix equals 1 (R=1), and the state-cost matrix (Q) equals to H' x H. The weighting factors will be chosen by trial and errors. The state-cost matrix (Q) has the following structure

	Q  =
		1	0	0	0
                0       0       0	0
                0       0       1	0
                0       0       0	0

The element in the 1,1 position will be used to weight the cart's position and the element in the 3,3 position will be used to weight the pendulum's angle. The weighting factors for the cart's position and the pendulum's angle will be chosen individually.

Now we are ready to find the control matrix (K) and see the response of the system. Enter the following commands to an new m-file and run it in the Matlab command window. You should see the following step response.


T=0:0.01:5;
U=0.2*ones(size(T));

F = [1.0000   0.0100   0.0001   0.0000;
          0   0.9982   0.0267   0.0001;
	  0   0.0000   1.0016   0.0100;
	  0  -0.0045   0.3119   1.0016];
	   
G =  [0.0001;
      0.0182;
      0.0002;
      0.0454];
      
H =  [1    0    0    0;
      0    0    1    0];
      
J =   [0;
      0];

x=1;	%weighting factor for the cart position
y=1;	%weighting factor for the pendulum angle

Q=[x 0 0 0;
   0 0 0 0;
   0 0 y 0;
   0 0 0 0];

R = 1;

K = dlqr(F,G,Q,R)

[Y,X]=dlsim(F-G*K,G,H,J,U);
stairs(T,Y)
legend('Cart (x)','Pendulum (phi)')

Note: The function dlsim is the discrete version of the lsim and has very similar characteristics as dstep.

The curve in green represents the pendulum's angle, in radians, and the curve in blue represents the cart's position in meters. The pendulum's and cart's overshoot appear fine, but their settling times need improvement and the cart's rise time needs to be decreased. Also the cart has, in fact, moved in the opposite direction. For now, we will concentrate on improving the settling times and the rise times, and fix the steady-state error later.

Let's increase the weighting factors (x and y) and see if both the settling and rise times decrease. Go back to your m-file and change the x and y to x=5000 and y=100. Running this m-file in the command window gives you the following new step response.

From this plot, we see that all design requirements are satisfied except the steady-state error of the cart position (x). We can easily correct this by introducing a feedforwarding scaling factor (Nbar).

Reference input

Unlike other design methods, the full-state feedback system does not compare the output to the reference; instead, it compares all states multiplied by the control matrix (K*x) to the reference (see the schematic shown above). Thus, we should not expect to see the output equals to the input. To obtain the desired output, we need to scale the reference input so that the output equals to the reference. This can be easily done by introducing a feedforwarding scaling factor called Nbar. The basic schematic with the Nbar is shown below.

Unfortunately, we can not use our user-defined function rscale to find Nbar. But certainly we can find it from trial and errors. After several trials, the Nbar equals to -61.55 provided the satisfactory response. Try the following m-file and obtain the step response shown below.


T=0:0.01:5;
U=0.2*ones(size(T));

F = [1.0000   0.0100   0.0001   0.0000;
          0   0.9982   0.0267   0.0001;
          0   0.0000   1.0016   0.0100;
          0  -0.0045   0.3119   1.0016];
                  
G =  [0.0001;
      0.0182;
      0.0002;
      0.0454];
             
H =  [1    0    0    0;
      0    0    1    0];
             
J =   [0;
       0];

x=5000;	%weighting factor for the cart position
y=100;	%weighting factor for the pendulum angle

Q=[x 0 0 0;
   0 0 0 0;
   0 0 y 0;
   0 0 0 0];

R = 1;

K = dlqr(F,G,Q,R)
       
Nbar=-61.55;
[Y,X]=dlsim(F-G*K,G*Nbar,H,J,U);
stairs(T,Y)
legend('Cart (x)','Pendulum (phi)')

Notice that the steady-state error of the cart's position have been eliminated. Now we have designed the system that satisfies all design requirements.

Observer design

The above response satisfies all design requirements; however, it was found assuming all states are measurable. This assumption may not be valid for all systems. In this section, we develop a technique for estimating the states of a plant from the information that is available concerning the plant. The system that estimates the states of another system is called an observer. Thus, in this section we will design a full-order state observer to estimate those states that are not measurable. For further explanation on how an observer works, please consult your control textbooks.

A basic schematic of the plant-observer system is shown below.

To design the observer, first, we need to find the L matrix. To find the L matrix, we need to find the poles of the system without the observer (the poles of F-G*K). Copy the following commands to an new m-file and run it in the Matlab command window.


F = [1.0000   0.0100   0.0001   0.0000;
          0   0.9982   0.0267   0.0001;
	  0   0.0000   1.0016   0.0100;
	  0  -0.0045   0.3119   1.0016];
	   
G =  [0.0001;
      0.0182;
      0.0002;
      0.0454];
      
H =  [1    0    0    0;
      0    0    1    0];
      
J =   [0;
      0];


x=5000;	%weighting factor for the cart position
y=100;	%weighting factor for the pendulum angle

Q=[x 0 0 0;
   0 0 0 0;
   0 0 y 0;
   0 0 0 0];

R = 1;

K = dlqr(F,G,Q,R);

poles = eig (F-G*K)

In the command window, you should see


	poles = 
	
	   0.9156+0.0729i
	   0.9156-0.0729i
	   0.9535+0.0079i
	   0.9535-0.0079i

We want to place observer poles so that the observer works a lot faster than the system without the observer. Let's place the observer poles far left of above poles, say, at [-0.3 -0.31 -0.32 -0.33]. These poles can be changed later, if necessary. We will use the Matlab function place to find the L matrix. Enter the following commands to an new m-file and run it.


F = [1.0000   0.0100   0.0001   0.0000;
          0   0.9982   0.0267   0.0001;
	  0   0.0000   1.0016   0.0100;
	  0  -0.0045   0.3119   1.0016];
	   
H =  [1    0    0    0;
      0    0    1    0];

P = [-0.3 -0.31 -0.32 -0.33];

L = place (F',H',P)'

You should see the following L matrix in the command window.

L = 
	
	2.6310    -0.0105
      172.8146    -1.3468
       -0.0129     2.6304
       -2.2954   173.2787

Now we will obtain the overall system response including the observer. Once again, create an new m-file and copy the following code.


T=0:0.01:5;
U=0.2*ones(size(T));

F = [1.0000   0.0100   0.0001   0.0000;
          0   0.9982   0.0267   0.0001;
          0   0.0000   1.0016   0.0100;
          0  -0.0045   0.3119   1.0016];
                  
G =  [0.0001;
      0.0182;
      0.0002;
      0.0454];
             
H =  [1    0    0    0;
      0    0    1    0];
             
J =   [0;
       0];

x=5000;	%weighting factor for the cart position
y=100;	%weighting factor for the pendulum angle

Q=[x 0 0 0;
   0 0 0 0;
   0 0 y 0;
   0 0 0 0];

R = 1;

K = dlqr(F,G,Q,R)

Nbar = -61.55;

L =   [2.6310    -0.0105;
      172.8146    -1.3468;
       -0.0129     2.6304;
       -2.2954   173.2787;

Fce = [F-G*K		G*K;
       zeros(size(F)) 	(F-L*H)];
       
Gce = [G*Nbar;
       zeros(size(G))];
       
Hce = [H zeros(size(H))];

Jce = [0;0];

[Y,X] = dlsim (Fce,Gce,Hce,Jce,U);

stairs (T,Y)
legend ('cart (x)','pendulum (phi)')

After running this m-file, you should get the following step response.

As you noticed, this response is about the same as before, and all of the design requirements have been satisfied.

User Feedback

Digital Control Examples: Cruise Control:RL | Motor Speed:PID | Motor Position:RL | Bus Suspension:SS | Inverted Pendulum:SS | Pitch Controller:SS | Ball & Beam:PID
Inverted Pendulum Examples: Modeling | PID | Root Locus | Frequency Response | State Space | Digital Control

Tutorials: Basics | Modeling | PID | Root Locus | Frequency Response | State Space | Digital Control | Examples

8/21/97 DK