Linear control of inverted pendulum

Deep Ray, Ritesh Kumar, Praveen. C, Mythily Ramaswamy, J.-P. Raymond

IFCAM Summer School on Numerics and Control of PDE

22 July - 2 August 2013
IISc, Bangalore
http://praveen.cfdlab.net/teaching/control2013

1 Inverted pendulum
334 M. LANDRY, S. CAMPBELL, K. MORRIS, AND C. AGUILAR

l j
m i

θ l
P g = -g j

F(t) i M

x
O x(t)

Figure 1. Inverted pendulum system.

1
I = Moment of inertia of pendulum
Table 1 about its cg = ml2
Notation. 3
Lagrangian x(t) Displacement of the center of mass of the cart from point O
θ(t) Angle the pendulum makes with the top vertical
1 M 1Mass of the cart
2Mass of the pendulum 1 2 2 2
L = M ẋ + m(ẋ + lθ̇ cos θ) + (I + ml )θ̇ − mgl cos θ
m
2 L 2Length of the pendulum 2
l Distance from the pivot to the center of mass of the pendulum (l = L/2)
Euler-Lagrange equation P Pivot point of the pendulum
F (t) Force applied to the cart
   
d ∂L ∂L d ∂L ∂L
In our experimental setup,
−
the force
=
F
F,on the cart is
(t)
− =0
dt ∂ ẋ ∂x dt ∂ θ̇ ∂θ
gives (1.3) F (t) = αV (t) − β ẋ(t),

where V (t) (Mis the+voltage

m)ẍ + mlmotor
in the θ − ml
θ̈ cos driving theθ̇ 2cart,
sin and
θ +the
k ẋsecond
= term
F represents
electrical resistance in the motor. The voltage V (t) can be varied and is used to control the
2
system. Themlẍ valuescos θ+
of the (I + ml
parameters for ) θ̈ −
our mgl sin setup
experimental θ + c[21]
θ̇ are
=given
0 in Table 2.
The precise value of the viscous friction # is difficult to measure; however, we expect that it
will be of a similar order of magnitude as β. Our work will thus consider # in the range [0, 15].
A more convenient form of the equations 1 is found by solving for ẍ and θ̈ from equations
(1.1) and (1.2) and introducing the variables

y = (y1 , y2 , y3 , y4 )T
(1.4)
= (x, θ, ẋ, θ̇)T .
This is a coupled system

mlθ̇2 sin θ − k ẋ + F
    
M + m ml cos θ ẍ
=
ml cos θ I + ml2 θ̈ mgl sin θ − cθ̇

Define determinant
D = (M + m)(I + ml2 ) − m2 l2 cos2 θ
Solving for ẍ, θ̈

1 ml cos θ(cθ̇ − mgl sin θ) + (I + ml2 )(F − k ẋ + mlθ̇2 sin θ)

   
ẍ
=
θ̈ D (M + m)(−cθ̇ + mgl sin θ) − ml cos θ(F − k ẋ + mlθ̇2 sin θ)

Define
z = (z1 , z2 , z3 , z4 )> = (x, ẋ, θ, θ̇)>
Then
ż1 = z2 , ż3 = z4
we can write the first order system
   
ż1 z2
ż2   1 [ml cos z3 (cz4 − mgl sin z3 ) + (I + ml2 )(F − kz2 + mlz42 sin z3 )] 
 = D 
ż3   z4 
1 2
ż4 D
[(M + m)(−cz4 + mgl sin z3 ) − ml cos z3 (F − kz2 + mlz4 sin z3 )]

In the experimental setup of Landry et al. [1], the force F on the cart is

F (t) = αu(t) − β ẋ, α > 0, β>0

where u is the voltage in the motor driving the cart, and the second term represents the
electrical resistance in the motor. Let us write the non-linear pendulum model as
dz
= N (z, u)
dt

1.1 Excercises
The matlab programs are in the directory pendulum; go into this directory and start
matlab. E.g. if you are working on the Unix/Linux command line
$ cd control2013 / pendulum
$ matlab
If you have started matlab through some other way, then change the directory inside
matlab
>> cd <Full path to>/control2013 / pendulum

2
Or use the matlab gui to change the working directory.
The solution of the non-linear pendulum is implemented in nlp.m file and ths right
hand side function without any control (F = 0) is implemented in rhs nlp.m file. Study
these two programs.
The pendulum has two equilibrium positions, one upright (θ = 0) and another in the
down position (θ = π).

1. Using nlp.m solve the non-linear pendulum problem with initial condition close to
upright position
5π
 
z(0) = 0, 0, 180 ,0
and final time T = 100. Is the upright position stable ? What happens to the four
variables ? What is the time asymptotic value of x, ẋ, θ, θ̇ ?

2. Solve the non-linear pendulum problem with initial condition close to downward
position
z(0) = 0, 0, 170π
 
180
,0
and final time T = 100. Is the downward position stable ?

3. Now set the value of friction coefficients k and c to zero in parameters.m file and
run nlp.m again. How does the solution behave qualitatively ? Is it converging to
a stationary state ?

Note Set the value of k and c back to their initial non-zero values.

1.2 Linearised system

We linearize around z = (0, 0, 0, 0)
     
0 1 0 0

z1 z1 0
m2 l2 gv2 mlcv2   
d z2  0 −(k + β)v2 − I+ml2
    αv2 
I+ml2  z2  + 
= u
dt z3  0 0 0 1  z3 0 
  
z4 0 ml(k+β)v2
mlgv1 −cv1 z4 − αmlv
M +m
1
M +m

where
M +m I + ml2
v1 = , v2 =
I(M + m) + mM l2 I(M + m) + mM l2
then we have the linear model
dz
= Az + Bu
dt

3
1.3 Excercises
1. Generate matrices for linear system
>> parameters
>> [ A , B ] = get_system_matrices ( )

2. Compute eigenvalues
>> e = eig ( A )
>> plot ( real ( e ) , imag ( e ) , 'o' )

Is there an unstable eigenvalue ? Is there a zero eigenvalue ?

3. The linear pendulum system is controllable if rank of controllability matrix is four.

Check that (A, B) is controllable by computing rank of controllability matrix

Pc = B AB A2 B A3 B
 

>> Pc = [ B , A∗B , A ˆ2∗ B , A ˆ3∗ B ] ;

>> rank ( Pc )

4. Check the controllability using Hautus criterion. Compute all the eigenvectors and
eigenvalues of A> . For each eigenvalue λ and eigenvector V of A>

A> V = λV

check if
B > V 6= 0
If the above condition is true for each unstable eigenvalue, i.e., with real(λ) ≥ 0,
then the system is stabilizable. Is the linear pendulum stabilizable ?

5. Solve linear pendum model (see lp.m and rhs lp.m) with initial condition
5π
 
z(0) = 0, 0, 180 ,0

upto a final time of T = 5. Compare this solution with solution of non-linear model.

2 Matlab function: lqr

The solution X of algebraic Riccati equation

A> X + XA − XBR−1 B > X + Q = 0, Q = C >C (1)

can be computed in matlab using

4
>> [ K , X , E ] = lqr ( A , B , Q , R ) ;
Here K is the feedback gain and can be computed as

K = B>X

and E contains the eigenvalues of A − BK.

3 Minimal norm feedback control

The minimal norm control is given by

u(t) = −Kz(t) with K = B>X

where X is the maximal solution of Algebraic Bernoulli Equation (ABE)

A> X + XA − XBB > X = 0

For inverted pendulum, matrix A has a zero eigenvalue. We replace A with A+ωI, ω > 0,
for determining the control.

(A + ωI)> X + X(A + ωI) − XBB > X = 0, K = B>X

Model with feedback

dz
= (A − BK)z, z(0) = z0
dt

3.1 Excercises
1. Compute the minimal norm feedback matrix K using lqr function
>> Q = zeros ( 4 , 4 ) ;
>> R = 1;
>> om = 0 . 0 1 ;
>> As = A + om ∗ eye ( 4 ) ;
>> [ K , X , E ] = lqr ( As , B , Q , R ) ;

Compute feedback gain from solution X of Riccati equation and check that it is same
as K given by lqr function
>> K1 = B ' ∗ X
>> K−K1 % This s h o u l d g i v e you z e r o matrix

E contains the eigenvalues of As − BK. Compare eigenvalues of As and As-B*K

>> e = eig ( As ) ;
>> plot ( real ( e ) , imag ( e ) , 'o' , real ( E ) , imag ( E ) , '*' )

5
 Observe that unstable eigenvalues of As have been reflected symmetrically about
the imaginary axis.
2. Also check that A − BK is stable by computing its eigenvalues.
>> e1 = eig ( A ) ;
>> e2 = eig ( A−B∗K ) ;
>> plot ( real ( e1 ) , imag ( e1 ) , 'o' , real ( e2 ) , imag ( e2 ) , '*' )

4 Feedback control using LQR approach

Output
ym = Cz, e.g. C = I4
Performance measure
1 ∞ > 1 ∞ >
Z Z
J = y Qm ym dt + u Rudt
2 0 m 2 0
1 ∞ > 1 ∞ >
Z Z
= z Qzdt + u Rudt, Q = C > Qm C
2 0 2 0
Find feedback law
u = −Kz
which minimizes J. The feedback or gain matrix K is given by
K = R−1 B > X
where X is solution of algebraic Riccati equation (ARE)
A> X + XA − XBR−1 B > X + Q = 0

4.1 Excercises
1. We may measure different output quantities, e.g.,
 
  1 0 0 0
    1 0 0 0 0 1 0 0
C= 1 0 0 0 , C= 0 0 1 0 , C= , C= 
0 0 1 0 0 0 1 0
0 0 0 1
These correspond to controlling (x), (θ), (x, θ) and (x, ẋ, θ, θ̇) respectively. In each
case, check if (A, C) is observable by computing rank of observability matrix
 
C
 CA 
Po = 
CA2 
 Is rank(Po ) = 4 or is rank(Po ) < 4 ?
CA3

6
 For which C is the rank condition satisfied ? Which of the four state variables is
important ?

2. Compute feedback operator K using lqr function. We will control position and
angle.
>> C = [1 0 0 0; 0 0 1 0 ] ;
>> Q = C '∗C ;
>> R = 1;
>> [ K , X , E ] = lqr ( A , B , Q , R ) ;
>> plot ( real ( E ) , imag ( E ) , 'o' )

E contains the eigenvalues of A − BK. Compare eigenvalues of A and A − BK

>> e = eig ( A ) ;
>> plot ( real ( e ) , imag ( e ) , 'o' , real ( E ) , imag ( E ) , '*' )

Is A − BK stable ?

3. Solve with LQR feedback control. This is implemented in program pend lqr.m
which solves both linear and non-linear model. Compare the linear and non-linear
solutions. Try with R = 0.01 and R = 0.1 and initial condition

z(0) = 0 0 50π
 
180
0

Which is better, R = 0.01 or R = 0.1 ?

4. Run the program pend lqr.m with different initial angles

z(0) = 0 0 50π 58π

   
180
0 , z(0) = 0 0 180
0

and for R = 0.01 and R = 0.1. Which value of R gives faster stabilization ? What
is the maximum magnitude of control in each case ?

5. If the initial angle θ0 is too large, then it may not be possible to stabilize the non-
linear pendulum. The linear pendulum is always stabilized. Find angle beyond
which non-linear model cannot be stabilized. Do this for R = 0.01, R = 0.1 and
R = 0.5. Use final time of T = 3 and initial conditions of the form
 
z(0) = 0 0 θ0 0

Use the evolution of energy as an indicator for finding the threshold angle. If the
energy starts to increase monotonically, then conclude that it is not possible to
stabilize the system.

7
5 Linear system with noise and partial information
Consider the system with noise in the model and initial condition
dz
= Az + Bu + w, z(0) = z0 + η
dt
where w and η are error/noise terms. We may not have access to the full state but only
some partial information which is also corrupted by noise.
   
1 0 0 0 x
yo = Hz + v, e.g. H= =⇒ yo = +v
0 0 1 0 θ

which corresponds to observing the position of cart x and the position of pendulum θ
only.

Since we have partial information, we have to estimate to estimate the remaining state
variables somehow.

5.1 A simple “estimator”

Suppose we can observe only z1 = x and z3 = θ but this information is also contaminated
by some error. Then we can try to estimate ẋ and θ̇ by using finite difference in time.
x(tn ) − x(tn−1 ) θ(tn ) − θ(tn−1 )
ẋ(tn ) = , θ̇(tn ) =
∆t ∆t
Then we can try the following feedback control scheme using backward Euler time dis-
cretization

y n = Hz n + v n
y1n
 
 1 (y1n − y1n−1 )
un = −K  ∆t 
 y2n 
1 n−1
(y n − y2 )
∆t 2
z n+1 − z n
= Az n+1 + Bun + wn
∆t
This is implemented in lp noest.m.

5.2 Excercise
Run the program lp noest.m and observe that the state and control are very noisy and
hence this approach is not of practical utility.

8
5.3 Estimation problem
Linear estimator
dze
= Aze + Bu + L(yo − Hze )
dt
We determine the filtering gain L by minimizing
1 ∞ 1 ∞ > −1
Z Z
> −1
J= (yo − Hze ) Rv (yo − Hze )dt + w Rw wdt
2 0 2 0
The best L is given by
L = Xe H > Rv−1 (2)
where Xe is solution of algebraic Riccati equation

AXe + Xe A> − Xe H > Rv−1 HXe + Rw = 0

This equation is similar to equation (1) if we replace

A with A>
B with H >
Q with Rw
R with Rv

5.4 Excercises
1. Compute the filtering matrix L
>> Rw = ( 5 e−2) ˆ2 ∗ diag ( [ 1 , 1 , 1 , 1 ] ) ;
>> Rv = ( 5 e−2) ˆ2 ∗ diag ( [ 1 , 1 ] ) ;
>> [ L , Xe ] = lqr ( A ' , H ' , Rw , Rv ) ;
>> L = real ( L ' ) ;

2. Compute eigenvalues of A − LH and check that they are all stable.

>> eig ( A−L∗H )

3. Compute L from equation (2) and check that it is same as L obtained in step 1
above.
>> L1 = Xe ∗ H ' / Rv ;
>> L − L1

9
5.5 Linear system with linear estimator
The feedback is based on estimated solution u = −Kze
dz
= Az − BKze + w
dt
dze
= LHz + (A − BK − LH)ze + Lv
dt
or in matrix form
       
d z A −BK z I 0 w
= +
dt ze LH A − BK − LH ze 0 L v

The initial condition is given by

z(0) = z0 + η, ze (0) = z0

This is implemented in program lp est.m

5.6 Excercises
1. Run program lp est.m. Observe that initially z and ze differ but after some time,
ze approches z so that the estimation becomes more accurate. You may have to
zoom the plot near the initial time to see the difference.
Note that all the quantities including the control behave in a more regular manner
now as compared to the finite difference estimation used before.

5.7 Non-linear system with linear estimator

The feedback is based on estimated solution using the linear estimator which is used to
add the control into the non-linear model.
dz
= N (z, u)
dt
dze
= Aze + Bu + L(yo − Hze )
dt
yo = Hz
u = −Kze

with initial condition

z(0) = z0 + η, ze (0) = z0
Since we do not know the precise initial condition, the estimator contains an error in the
initial condition. This is implemented in program nlp est.m and rhs nlpest.m

10
5.8 Excercises
1. Run program nlp est.m. Zoom the plot near initial time and compare the non-
linear solution with the linear estimator. Note that ze approaches z for larger times.

2. Increase the initial angle and check if the feedback can be stabilize the non-linear
system. Have the threshold angles evaluated for R = 0.01, R = 0.1 and R = 0.5
changed? If yes, then what are the new threshold angles?

3. How does the control vary as R is changed? Is the behaviour the same as that
observed in the earlier cases?

4. How does the value of R influence the stabilization of the state variables?

5.9 Excercises
Here we will assume that we do not know the value of the friction coefficient k and c. So
the linear estimator will be constructed taking k = 0 and c = 0 whereas the non-linear
model will include the friction. We want to see if the estimator is still able to give a good
estimate of the real state and if the system is stabilized by the feedback.

1. In parameters.m set k and c to be zero.

2. Copy nlp est.m as nlp est2.m

3. Modify the initial part like this

parameters ;
[ A , B ] = get_system_matrices ( ) ;
k = 0.05;
c = 0.005;

4. Run the program

>> nlp_est2

Does the estimate and control still succeed ?

11
6 Inverted pendulum: Parameters
M Mass of cart 2.4
m Mass of pendulum 0.23
l Length of pendulum 0.36
k Friction in cart 0.05
c Friction in pendulum 0.005
α Coefficient of voltage 1.7189
β Friction in motor 7.682
a Acceleration due to gravity 9.81

7 List of Programs
1. parameters.m: Set parameters for pendulum

2. get system matrices.m: Computes matrices for linear model

3. hautus.m: Checks the stabilizability of the system using Hautus criterion

4. rhs nlpc.m: Computes right hand side of non-linear model, with feedback

5. rhs lp.m: Computes right hand side of linear model, without feedback

6. rhs nlp.m: Computes right hand side of non-linear model, without feedback

7. rhs lpc.m: Computes right hand side of linear model, with feedback

8. rhs nlpc.m: Computes right hand side of non-linear model, with feedback

9. rhs nlpest.m: Computes right hand side of non-linear model coupled with the
linear estimator and feedback

10. lp.m: Solves the linear model without feedback

11. nlp.m: Solves the linear model without feedback

12. pend lqr.m: Solves the linear and non-linear models with feedback and full state
information

13. lp est.m: Solves the linear model with noise and partial information, coupled esti-
mator and feedback stabilization

14. lp noest.m: Solves the linear model with noise and partial information, feedback
without estimation

12
15. nlp est.m: Solves the non-linear model with noise and partial information, coupled
estimator and feedback stabilization

References
[1] M. Landry, S. A. Campbell, L. Morris and C. O. Aguilar, “Dynamics of an inverted
pendulum with delayed feedback control”, SIAM J. Applied Dynamical Systems,
Vol. 4, No. 2, pp. 333-351, 2005.

13