MALARDALENS UNIVERSITY
School of Innovation, Design and EngineeringVasteras
MASTER THESISCEL505
PLANAR ROBOT ARM MODELLINGAND CONTROL
Francisco Hernandez Gonzalez
Supervisor:Giacomo Spampinato
June 2012
Acknowledgements
This work is the fruit of a long academic background. Therefore many people have inter-
vened directly in my learning or have offered their support, encouragement and motivation
to make my studies possible. I’m very grateful for all their work and company.
Specially I would like to express my gratitude to my family, to Giacomo Spampinato,
Pedro Roncero and my friends from Ciudad Real, Toledo and Vasteas for all their help.
I
Abstract
The thesis objective is to model a one link robotic arm mounted on a sliding mobile plat-
form and to investigate different control strategies under the effect of gravity and external
force disturbance. For simplicity the robotic set up can be modelled and controlled as an
inverted pendulum moving on a non constant sloping surface.
Firstly the control is done on level ground. This lower mathematical complexity will
be taken as an advantage to approach the analysis on aspects more related with control
theory: several control techniques and observers, steady state error study, etcetera.
Afterwards the control is generalized for sloping grounds. This chapter will seek sit-
uations closer to reality, the purpose is to design something with practical interests, like
model a Segway. The chapter ends explaining an animation.
Finally, some new projects that might arise from this are suggested.
II
Contents
List of Figures. V
1 Introduction 1
1.1 History and state of the art . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Contents . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3 Innovative researches . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2 DEFINITION OF THE PROBLEM 6
2.1 Description of the problem . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.2 Generic outline of inverted pendulum . . . . . . . . . . . . . . . . . . . . . 7
3 INVERTED PENDULUM ON LEVEL GROUND 10
3.1 Mathematical model of the system . . . . . . . . . . . . . . . . . . . . . . 10
3.2 State space representation . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.3 Controllability and observability . . . . . . . . . . . . . . . . . . . . . . . . 14
3.4 LQR control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.5 Pole placement control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.5.1 Phase variables transformation . . . . . . . . . . . . . . . . . . . . 19
3.5.2 Calculation of K . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.5.3 SIMULINK simulation . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.6 Design of an observer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.6.1 Observable canonical form transformation . . . . . . . . . . . . . . 24
3.6.2 Calculation of parameters characterizing the observer . . . . . . . . 25
3.6.3 SIMULINK simulation . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.7 Design of a reduced order observer . . . . . . . . . . . . . . . . . . . . . . 27
3.7.1 Model transformation . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3.7.2 Calculation of characteristic parameters of the reduced order observer 29
3.7.3 SIMULINK simulation . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.8 Track a reference input for the position . . . . . . . . . . . . . . . . . . . . 31
3.9 Study of the steady state error . . . . . . . . . . . . . . . . . . . . . . . . . 34
4 INVERTED PENDULUM ON SLOPING GROUND 38
4.1 Mathematical model of the system . . . . . . . . . . . . . . . . . . . . . . 38
4.2 State space representation . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
4.3 LQR control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
4.4 Observers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
4.5 Track a reference input for the position . . . . . . . . . . . . . . . . . . . . 44
4.6 Animation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
5 FINDINGS 49
5.1 Outcomes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
5.2 Methodology of work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
5.3 Other applications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
5.4 Future work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
A EXPLANATORY CALCULATIONS 53
A.1 Equations 3.7 and 3.8 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
A.2 Equations 3.14 y 3.15 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
A.3 Representation of a system by its phase variables . . . . . . . . . . . . . . 54
A.4 Representation of a system by observable canonical form . . . . . . . . . . 55
A.5 Steady state error to different kind of reference inputs . . . . . . . . . . . . 56
A.6 Steady state error to different kind of disturbance inputs . . . . . . . . . . 57
B SOURCE CODES MADE 58
B.1 Inverted pendulum on level ground . . . . . . . . . . . . . . . . . . . . . . 58
B.2 Inverted pendulum on sloping ground . . . . . . . . . . . . . . . . . . . . . 62
B.3 Animation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
Bibliography. 70
IV
List of Figures
2.1 Inverted pendulum on sloping ground . . . . . . . . . . . . . . . . . . . . . 7
3.1 Inverted pendulum on flat ground . . . . . . . . . . . . . . . . . . . . . . . 11
3.2 System division into its component parts . . . . . . . . . . . . . . . . . . . 11
3.3 System’s response to different values of q1 . . . . . . . . . . . . . . . . . . 16
3.4 System’s response to different values of q3 . . . . . . . . . . . . . . . . . . 17
3.5 Respuesta final utilizando un control LQR . . . . . . . . . . . . . . . . . . 18
3.6 SIMULINK diagrams of inverted pendulum on level ground . . . . . . . . . 21
3.7 Pendulum response without control . . . . . . . . . . . . . . . . . . . . . . 22
3.8 0.25N step input . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.9 Initial conditions: x0 = 0.5m, x0 = 1 ms
, θ0 = 20 π180
rad θ0 = 0 rads
. . . . . 23
3.10 Initial conditions + Step + Impulse . . . . . . . . . . . . . . . . . . . . . . 23
3.11 Observer diagram in SIMULINK . . . . . . . . . . . . . . . . . . . . . . . 26
3.12 Estimation errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
3.13 Diagram of reduced order observer in SIMULINK . . . . . . . . . . . . . . 30
3.14 Estimate errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
3.15 System for controlling the position . . . . . . . . . . . . . . . . . . . . . . 31
3.16 Ki adjust . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.17 Kr adjust . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.18 Kd adjust . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.19 Blocks diagram . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
3.20 Estimate errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
3.21 Estimate errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
3.22 Step source, step disturbance . . . . . . . . . . . . . . . . . . . . . . . . . 37
4.1 Inverted pendulum on sloping ground . . . . . . . . . . . . . . . . . . . . . 39
4.2 System division into its two parts . . . . . . . . . . . . . . . . . . . . . . . 39
4.3 Respond when m and l vary . . . . . . . . . . . . . . . . . . . . . . . . . . 44
4.4 SIMULINK diagram for the total control of the pendulum on sloping ground 45
4.5 Respond when it is acting a 2000N step in t = 1 s and an impulse in t = 6 s 45
4.6 Variables used to fix the screen of the process . . . . . . . . . . . . . . . . 46
4.7 Variables used in animation programming . . . . . . . . . . . . . . . . . . 47
4.8 Inverted pendulum animation . . . . . . . . . . . . . . . . . . . . . . . . . 48
VI
CHAPTER 1
Introduction
1.1 History and state of the art
The use of the simple pendulum has been linked to our history from far off times. There
is evidence that it was already used in Pharaonic Egypt. Since then this valuable tool
has been present in most civilizations.
Pendulum’s problem began to be studied mathematically in the 16th century, when
Galileo carried out the first researches. Then the problem has been raised on numer-
ous occasions because of its many applications. Important taxpayers to this topic were
Newton, Huygens, Foucault, Pohl and some studies during Second World War.
Inverted pendulum has also been intensively studied, due to its academic interest
because it is an appropriate system on which to apply several control techniques [6], as
well as its practical applications.
It was A. Stephenson in 1908, who first described the problem of inverted pendulum.
Until the fifties there wasn’t any explanation of this phenomenon. Then, in 1951, Pyotr
Kapitza carried out some studies which described the causes of pendulum’s stabilization.
His work opened a new field in Physics called Vibrational Mechanics.
In the sixties, R. E. Kalman and other investigators introduced powerful theoretical
tools which revolutionize the Control Theory. This contributions culminated in Modern
Control Theory, based on time domain, which using a space state model allows working
with systems with multiples inputs and outputs (MIMO systems).
The used of these new tools made the studies in this field begin to be successful in the
late seventies. A renowned researcher into inverted pendulum was Professor Futura (look
at [5]) of Tokyo Institute of Technology.
It has also been studied the way to carry the pendulum from the stable equilibrium
in the inferior position to the unstable equilibrium in the top position (Q. Wei, [16]).
In 1996, Fradkov proposed a control strategy for non linear oscillations [4]. His method
1
CHAPTER 1 Contents
was also able to be applied to stabilize the inverted pendulum.
This same year Shiroma presented Cooperative Behaviour of a Wheeled Inverted Pen-
dulum for Object Transportation, in which it is studied the performance of the pendulum
against some disturbances.
Mazenc and Praly utilized a structure based on integrators to stabilize the pendulum
in its unstable position [10].
In 2002, Profesor Grasser of Swiss Federal Institute of Technology also made relevant
contributions in the research of inverted pendulums with two wheels on which are placed
weights simulating the weight of a human [8].
Taking advantage of this studies, S. Hassenplug built using LEGO Mindstorms a robot
called Legway that, on two wheels, was able to keep the equilibrium.
Also in 2002, Fantoni and Lozano proposed a strategy based on the total energy of
the system in order to make zero the total displacement of the cart [3].
In the other hand, the rapid increase of aged population in Japan, encourages re-
searches like done by Takahashi for the develop of robotic wheelchairs based on inverted
pendulums control.
Not being a new problem, we will study it as original as possible and dealing with
each part in depth.
Chapter 5 enumerates several applications that are used as incentive for continue
studying this kind of projects.
The aim will be to get as close as possible to the control system of a Segway. This
system was invented by Dean Kamen in 2001, and it is explained more in detail in the
next chapter, because it is one of the main objectives to pursue in this work.
1.2 Contents
This report is structured as follows:
• In the first chapter it is done a concise description of the inverted pendulum problem
along the history and other important researches already done. Then it is said in
which points this work is innovative. In this way we can put this work in context
and see the contributions made in this field.
• In Chapter 2 it is thoroughly described the problem that we want to solve with
this work. It is specified which are the objectives that are expected to achieve,
it is explained what a Segway is and introduced the outline that will be used as
mathematical model for this specific problem, in which each parameter is carefully
described.
2
CHAPTER 1 Innovative researches
• In Chapter 3 it is exposed the theoretical study of the problem. It is introduced
the mathematical develop in depth and the model of the system. Two structures
of Modern Control are compared in order to balance disturbances. Two kinds of
observers are designed. It is added the capacity to follow a reference input using a
PID regulator. Finally it is done a study of the steady state errors. All these results
achieved analytically are programmed in line in a m-file in MATLAB and simulated
with SIMULINK, then we can compare both analytic results and simulated ones.
• In Chapter 4, furthermore generalize making the theoretic study valid also for slop-
ing grounds (what means a more mathematical complexity), it is changed the point
of view trying to apply all the study to the reality. To do that, we purpose the aim
of mimic a Segway. Again, a program in MATLAB is developed in line. With this
program is possible to imitate the performance of the vehicle when it is driven by
different people. Finally it is programmed an animation of all the process.
• Chapter 5 starts talking about results obtained, discussing the merits achieved in
the project. It is mentioned the methodology followed along the work. It is also
enumerated other applications that might use this work to solve similar problems
related with inverted pendulum. The chapter ends proposing new projects that can
born from this one, limitations of the model used and possible improvements that
could be done.
• Appendix A shows all the mathematical manipulations done to achieve the equations
used in the project.
• Appendix B consists of all the programmed codes in MATLAB to automatize and
make general the process.
1.3 Innovative researches
As it was said, the problem of the inverted pendulum has been thoroughly studied in
the last years. Therefore it is inevitably to share many points with previous researches
(concept of balancing disturbances, theory of control structures for non linear systems,
equations used to build observers, the idea of a vehicle able to be driven by people,
etcetera). In this cases we will try to make the study as depth as possible, trying to
comment in detail any aspect that has been obviated in other investigations. Anyway,
all the calculates have been made starting from scratch, taking a general system and
just helping using mathematical manipulations, so although they have been previously
studied, the way to deal with them is particular of this work and the approach is personal
of the author.
3
CHAPTER 1 Innovative researches
On the other hand, this work also tries to innovate and make some contributions,
focusing the study on new aspects which have not been sawed before or have not made
public. These are the different and strong points in this report:
• The mathematical model used is rigorous and detailed. Generic equations are ap-
plied to the system, specifying in each step which is the aim to follow in the math-
ematical develop, making just the necessary simplifications and commenting why
they are used. Other ways are also proposed to reach the same results, as in Section
3.1. Most of previous researches began from non generic equations, with develops
not enough clear, and with more restrictions in their use. These limitations were un-
known in most of the cases because they began from non generic equations, so later
some problems could appear. In fact, in Appendix A are showed all the intermediate
calculates made to achieve the equations that are particular for this work.
In this sense, a point specially powerful and innovative of this report is the mathe-
matical study on sloping ground. I have not found any previous work which had
develop this issue. The complex of the mathematical develop rises considerably,
because all the equations become very unlinear and the variables depending on the
time rises, therefore the derivatives are more complex. Thus, the process is not
trivial and we have to make it carefully.
Done the develop as generic as possible, it is also commented how we have evaded
several kinds of problems such as indeterminates, matrix dimensions troubles, non
linearities, base transformations, etcetera.
• Comparison between two control structures: LQR Vs Pole placement control.
Both techniques of Modern Control, used to balance disturbances, are deeply ex-
plained and compared in detail, commenting which are the advantages and disad-
vantages of each one. Theoretically it is cleared the concept of both techniques, how
each parameter influences on the performance and how it is possible to dominate the
behaviour of the system doing the suitable calculates. In previous projects marked
on the bibliography, only one of these techniques was used (almost always LQR). In
this case we add the Pole placement control and moreover it is used to imitate the
other control structure, so it is possible to compare them.
• Design and comparison between two observers. It is taken advantage of the fact
that we have an observable system, to design two kinds of observers. Firstly it is
done a complete order observer and then other more efficient which just estimates
the unknown states. These researches had already done in previous works. In this
case we also add an analysis of the errors in the estimation of each method. The
diagram built in SIMULINK is slightly different from the most of the other proposed
solutions.
4
CHAPTER 1 Innovative researches
It is important to notice how was the mathematical problem solved in the design
of the second observer. The special mathematical structure of this system caused
a lot of problems with matrix dimensions. In Subsection 3.7.1 it was created an
original method to solve this problem. This method neither was sawed in other
sources.
• Track a reference input. The control does not end with the balance of distur-
bances, but it has also added the ability to follow a reference input for the position.
To do that, it is done other control: PID.
• Study of the steady state errors. It was curious to observe the error that ap-
peared in the steady state for the position. Therefore we decided to develop the
equations of the error in function of the different kind of inputs of the system, in or-
der to justify why there was this error and calculate which should be its magnitude.
After all this process it was gratifying to check that both ways, experimentally and
mathematically, converge achieving exactly the same result.
• Animation. This is not an innovative section from other researches, but it was
carried out without any previous information an animation to see the performance
in a more real point of view.
• The system was designed as generic as possible. Both equations and programmed
code (which is showed in Appendix B) are done executing the minimum simplifi-
cations. Even in some cases, like in the simplification of Equation 4.6, previous
hypothesis culminate in a system even more generic. The model is also able to
work against some simultaneous forces which don’t need to act in the basis, as it is
commented in the next chapter. In fact, it will be checked that the model can be
used in a very large rank of values of all its parameters.
• It was also important to provide this previous theoretic work with useful applica-
tions. Thus, all the process has been carried out with the aim of model a Segway.
In fact, in Section 4.3 we try with several magnitudes of people who drive the vehicle.
5
CHAPTER 2
DEFINITION OF THE PROBLEM
2.1 Description of the problem
The problem of a planar robot arm or of an inverted pendulum can be raised from many
different approaches and be applied to several situations. In this case it was chosen to
imitate the performance of a Segway. Therefore it will be chosen a model in which the
forces and other parameters are localized and have similar dimensions to those of the
Segway.
A Segway is a transport vehicle with two wheels, produced by Segway Inc. Both wheels
are in a cart, on which the driver is situated. From this cart comes out a bar used by the
person driving the vehicle. It utilizes batteries and two electrical motors, which allows to
reach speeds until 20 kmh
. It also have two sensors for inclination and five gyroscopes used
to balance the system.
It is designed for urban use, and after its first presentation by Dean Kamen in 2001,
it was very positive forecasts. Nowadays it is utilized in many situations such as public
transport and other excursions, airports, warehouses in big areas, crop fields, golf courses,
police, disabilities, etcetera. There is a sport called Segway Polo, similar to Horse Polo but
driving a Segway, and curiously the last world championship was carried out in Stockholm
in 2012.
In this work it is not expected to built exactly the complex control structure of a real
Segway, but imitate the main characteristics. For example, we are not going to take into
account the possibility of drive the cart to the left and right, just forward and back.
In short, we have a pendulum (in the case of a Segway it would be a person), joined
to a cart by a “basket” (which prevents movements but allows free rotation in the plane).
This system, characterized by the dimensions, masses, inertia torques of the pendulum
and the cart, is moved on a sloping ground with a specific angle and friction. It is desired
to provide the system with the ability to balance different types of disturbances in the
6
CHAPTER 2 Generic outline of inverted pendulum
most effective way, keeping the vertical position against pushing, potholes, changes of
inclination, driver movements, non zero initial conditions and other disturbances. It has
to be also able to go to any desired point in a line, following driver instructions. The
objectives to solve this problem are:
• To make a thoroughly study of the theoretical part related with the mathematical
analysis and control of a inverted pendulum, focussing on innovative points from
other previous researches. This objective will be worked in Chapter 3.
• Design a system to face up to all the situations described above, with the purpose of
imitate a Segway. It has to be as general as possible, being able to act on a large rank
of values for the ground friction, surface inclination, driver characteristics (weigh
and height), and different points of the center of mass of the system. Chapter 4
works in this aspect.
2.2 Generic outline of inverted pendulum
It is possible to outline a Segway and any inverted pendulum in which forces of disturbance,
reference and control act on the basis, as it is shown in Figure 2.1.
Figure 2.1: Inverted pendulum on sloping ground
Where:
• ~Ffric represents the frictional force of the base of the pendulum (the cart) against
the ground. This friction usually occurs between the wheels and the ground, but
in the Figure 2.1 it is represented by a rectangle. This simplification is carried out
7
CHAPTER 2 Generic outline of inverted pendulum
without loss of generality because subsequently we will introduce the real value of
the friction wheels on MATLAB program.
• M~g and m~g are the forces that occur as a result of gravity and they act on the center
of gravity of the cart and the pendulum respectively. M and m are the masses of
the cart and pendulum respectively. g is gravity acceleration.
• ~F is the sum of all the other forces acting on the pendulum. They are grouped as
~F because it is assumed that all of them act in the same direction. This direction is
carts’ longitudinal line passing through its center of mass. These are the following
forces:
– ~Fdisturbance. The system will be affected by disturbances over which we have no
control. In the case of a Segway they can be the ground roughness, movements
of the person, etc... that tend to destabilize the pendulum. For other systems
that can be modelled by an inverted pendulum, these disturbances could be
earthquakes, pushing, etcetera. It is supposed all of them act in pendulum
basis.
– ~Freference. The aim is to be able to move the system wherever we want. This
is achieved doing a control in which the cart follows a reference input. This
reference input will be ~Freference. In the case of a Segway, the person that
controls it sends a signal from the handlebar ordering where he wants to go to.
This signal is transformed into ~Freference and influences on the driving system
of the cart (the motor), acting on the basis of the pendulum as it was assumed.
– ~Fcontrol. The goal is to stabilize the pendulum and take it to the desired posi-
tion. To achieve this, we have to change ~Fcontrol in a proper way. Hence, ~Fcotrol
is the responsible for neutralizing the effect of disturbances or non-zero initial
conditions, and in addition to make the system to follow the reference signal.
~Fcotrol is generated by the motor obeying control commands, so it acts again
on the basis of the pendulum and in the same direction than ~Fdisturbance and
~Freference. Also this can be generalized to every inverted pendulum in which
the control action acts on the basis.
It is possible that other disturbances non applied in the base of the cart, appear
during the movement. For example, in the case of a Segway, the person might
lose balance or start from non-zero initial conditions. These new disturbances
can be modelled as a change of angle to which the control system have to
respond with ~Fcontrol to keep it in balance. Therefore it is possible to generalize
this model to face up to other types of disturbances non acting on the basis
(provided that they act causing an instantaneous variation of the angle).
8
CHAPTER 2 Generic outline of inverted pendulum
• θ is pendulum instantaneous angle with the vertical. Counterclockwise is considered
positive.
• γ is the inclination of the ground. Surfaces with negative inclination will have a
γ > 0 and surfaces with positive inclination γ < 0.
~X, ~Y has been chosen as reference system, and its origin is located in some point on
the ground surface.
9
CHAPTER 3
INVERTED PENDULUM ON LEVEL GROUND
Before doing inverted pendulum study on sloping ground, we will proceed with the analysis
on horizontal ground. It is interesting to include this first stage because the mathematical
model of the inverted pendulum on level ground is simpler, therefore we will be able to
play more easily with its system parameters in order to make a more theoretical study.
Next chapter (Inverted Pendulum on Inclined Floor) will be more practical and applied
to reality, all time comparing with a Segway and other applications of the pendulum.
However, in this chapter we will use this lower mathematical complexity to perform a
thorough analysis of all factors involved in control implementation.
3.1 Mathematical model of the system
Similarly as we described in Section 2.2, it is possible to outline an inverted pendulum on
flat ground as Figure 3.1 shows. In this Section 2.2 we explain all the parameters involved
in this figure.
In order to know the mathematical equations governing the system, the inverted pen-
dulum is divided in two components: pendulum and cart, both assumed as rigid bodies.
This is illustrated in Figure 3.2. You can see that as a result of having a “basket” that
prevents movement but allows rotation of the pendulum, two reaction forces appear: ~Rx
and ~Ry.
Applying Newton’s Second Law for both spatial dimensions ~X and ~Y , and for angular
momentum, we obtain the following equations, where ~xp(t), ~yp(t) are the coordinates ~x(t),
~y(t) of pendulum center of mass at any instant, ~xc(t), ~yc(t) are the coordinates of cart’s
com, b is the damping ratio of mechanical system, proportional to the friction force by the
expression ~Ffric(t) = b~x(t); I is inertia momentum 1, l is the length to pendulum center
1In order to keep generality, we do not replace I by inertia momentum expression of any known
element. We will insert the value of body’s inertia momentum directly in MATLAB.
10
CHAPTER 3 Mathematical model of the system
Figure 3.1: Inverted pendulum on flat ground
(a) Pendulum (b) Cart
Figure 3.2: System division into its component parts
of mass, and gc is the height of the cart.
• Pendulum:
–∑ ~Fx(t) = m~x(t)
−~Rx(t) = md2~xp(t)
dt2= m
d2
dt2
(~x(t)− l sin ~θ(t)
)⇒
~Rx(t) = −m~x(t) +ml~θ(t) cos ~θ(t)−ml~θ2(t) sin ~θ(t) (3.1)
–∑ ~Fy(t) = m~y(t)
−~Ry(t)−m~g = md2~yp(t)
dt2= m
d2
dt2
(gc + l cos ~θ(t)
)⇒
~Ry(t) +m~g = ml~θ(t) sin ~θ(t) +ml~θ2(t) cos ~θ(t) (3.2)
–∑ ~T (t) = I~θ(t). Being ~T (t) the momentums.
− ~Ry(t)l sin ~θ(t)− ~Rx(t)l cos ~θ(t) = I~θ(t) (3.3)
11
CHAPTER 3 Mathematical model of the system
• Cart:
–∑ ~Fx(t) = M~x(t)
~F (t)− ~Ffric(t)− ~Rx(t) = Md2~xc(t)
dt2= M
d2~x(t)
dt2⇒
~F (t)− b~x(t) + ~Rx(t) = M~x(t) (3.4)
–∑ ~Fy(t) = M~y(t)
~Ry(t)−M~g = Md2~yc(t)
dt2= 0 (3.5)
–∑ ~T (t) = I~θ(t)
− ~Ffric(t)gc2− ~Rx(t)
gc2
= I~θc(t) = 0 (3.6)
It can be notice that both the term ml~θ(t) cos ~θ(t) of 3.1 equation and term ml~θ(t) sin ~θ(t)
of 3.2 equation, come from the tangential acceleration acting on the center of mass of the
pendulum. Likewise, term ml~θ2(t) sin ~θ(t) of 3.1 equation and term ml~θ2(t) cos ~θ(t) of 3.2
equation come from normal or centripetal acceleration (which also acts on the center of
mass).
Therefore, we could have achieved the same results if we had considered that these
inertia forces are also acting in the same system. Being ~θ(t)×~r(t) tangential acceleration
and ~θ(t) ×(~θ(t)× ~r(t)
)centripetal acceleration, where ~r(t) is the vector from center of
rotation to pendulum centre of mass. In this case, the modulus of ~r(t) will be l. So
knowing that ~a(t)Total = ~a(t) + ~ainertia(t) one can reconstruct forces sketch and achieve
the same results.
Substituting 3.1 and 3.2 in 3.3, and developing, equation 3.7 is obtained. In Appendix
A (Explanatory Calculations) all intermediate steps are detailed.
~θ(t)(I +ml2
)−mgl sin ~θ(t) = ml~x(t) cos ~θ(t) (3.7)
Substituting 3.1 in 3.4 it is obtained:
~F (t) = (M +m)~x(t) + b~x(t) +ml~θ2(t) sin ~θ(t)−ml~θ(t) cos ~θ(t) (3.8)
In order to represent the system using matrix dependent on state variables, we must
have a linear system. This will be achieved assuming that θ ≈ 0, which is not a bad
approximation because the pendulum will swing always small angles around the vertical.
Imposing this assumption, sin ~θ(t) ≈ ~θ(t), cos ~θ(t) ≈ 0, and ~θ2(t) ≈ 0 because it is the
square of a magnitude ≈ 0. Applying this transformation to the equations 3.7 and 3.8,
we obtain:
~θ(t)(I +ml2
)−mgl~θ(t) = ml~x(t) (3.9)
~F (t) = (M +m)~x(t) + b~x(t)−ml~θ(t) (3.10)
12
CHAPTER 3 State space representation
3.2 State space representation
It is possible to use different methodologies to select the state variables of a system and to
represent its state model. In this case we choose Status Variables as physical magnitudes
of the system [2, Section 1.7]. The idea of this technique is to choose as state variables
the items which are able to collect energy.
In this case it will be selected:
x1(t) = ~x(t) x2(t) = ~x(t) x3(t) = ~θ(t) x4(t) = ~θ(t)
Then, it is true that:
x1(t) = x2(t) x3(t) = x4(t) (3.11)
Rewriting equations 3.9 and 3.10 we obtain:
x4(t)(I +ml2
)−mgl x3(t) = ml x2(t) (3.12)
~F (t) = (M +m) x2(t) + b x2(t)−ml x4(t) (3.13)
In A Appendix, it is showed how manipulating conveniently 3.12 and 3.13, x2(t) and x4(t)
can be solved:
x2(t) = −βb x2(t) +mlgα x3(t) + β ~F (t) (3.14)
x4(t) = −αb x2(t) + g(M +m)αx3(t) + α ~F (t) (3.15)
Where:
α =ml
I(M +m) +Mml2β =
I +ml2
I(M +m) +Mml2
Equations 3.14, 3.15 and 3.11 can be written in matrix form:x1(t)
x2(t)
x3(t)
x4(t)
=
0 1 0 0
0 −βb mlgα 0
0 0 0 1
0 −αb g(M +m)α 0
x1(t)
x2(t)
x3(t)
x4(t)
+
0
β
0
α
~F (t) (3.16)
~F (t) is system input, and bring together forces described in Section 2.2. The objective is
to direct the system wherever we want without destabilizing the pendulum. Therefore it
is interesting that variables assumed as output of system space are the space (x1(t)) and
pendulum angle (x3(t)). This information is given in equation 3.17. This equation 3.17,
with 3.16, form the state model of the inverted pendulum on level ground.
~y(t) =
[1 0 0 0
0 0 1 0
]x1(t)
x2(t)
x3(t)
x4(t)
+
[0
0
]~F (t) (3.17)
13
CHAPTER 3 Controllability and observability
Thus, characteristics matrix of the model are:
~A =
0 1 0 0
0 −βb mlgα 0
0 0 0 1
0 −αb g(M +m)α 0
~B =
0
β
0
α
~C =
[1 0 0 0
0 0 1 0
]~D =
[0
0
](3.18)
This matrix dimensions are: A [n× n], B [n×m], C [p× n] and D [p×m].
With this information we will start to make a m-file in MATLAB in which all the
control process will be developed. Parallel in this chapter, theoretical explanations are
showed and results are commented. The contents of this m-file are showed in Appendix
B.
Before starting the process is worth noting that we are working with a invariant system.
This is so, because if we start from the same initial conditions, the answer will be the same
for any instant of time in which input is applied. The consequence is that all elements in
matrix 3.18 are constants. Furthermore the system is linear, because all the equations
making up the state model are formed by linear combinations of the variables. These two
features are exploited in this work because using them we can simplify mathematics of
control.
3.3 Controllability and observability
Controllability A system is controllable if it can reach any point in state space
selecting correctly its inputs.
A lineal and invariant system is controllable if and only if its controllability
matrix Q:
Q =[B|AB|A2B| · · · |An−1B
]has maximum range, that is, n. [2].
Analyzing matrix dimensions 3.18: n = 4.
We have chosen standard values for constants: M = 0.75 kg, m = 0.25 kg, b =
0.075Nms, l = 0.4m, I = 0.005 kgm2
s2and g = 9.8 m
s2. These values can be changed
later because the control was performed in a generic way, although these are used as an
example to illustrate the behaviour of the control system.
Entering these data in the m-file, matrix Q can be calculated. We can immediately
check that its range is 4, so the system is controllable and it will be possible to achieve
the desired response by acting appropriately on the entry.
14
CHAPTER 3 LQR control
Observability A system is observable if, having information on the evolution of
input and output, it is possible to know the value of any internal state.
A lineal and invariant system is observable if and only if its controllability
matrix P :
P =
C
CA
CA2
...
CAn−1
has maximum range, that is, n. [2].
Checked with the program in MATLAB that P is of maximum rank 4, so the system
is observable. This implies that if we had not got any measuring instrument to determine
the value of any state, it would be possible to estimate the unknown state(s) only knowing
the evolution of input and output.
3.4 LQR control
This section carries out a LQR control (Linear Quadratic Regulator) using state feedback
with goal of make pendulum-cart behave as we desire. At the end of this control, we will
imitate its dynamic with another state feedback control in which we will specify the poles
we want to determine the characteristics of its response.
At first it would seem unnecessary to make other control in order to obtain the same
answer, but it is interesting to compare these two techniques, because LQR control uses
trial and error techniques to set the proper value of its parameters; however we can
determine exactly the dynamic we want to achieve choosing the poles of the system. We
will studied this in next section (more accurate method).
Using LQR control we try to minimize the the quadratic cost function:
J(u) =
∫ ∞0
(yTQy + uTRu
)dt s.a. x = Ax+B F
Where J(u) is the energy of the system, yTQy 2 is the energy contributed by each state
and uTRu is energy associated with control action. This function is minimized with a
control law like u(t) = −Klqr y(t) [11]. Where Klqr is a matrix gain multiplying output
before system feedback (y(t) and u(t) are output and input respectively).
The exact value of J(u) is not relevant, we only want in this problem the value of Klqr
which makes J(u) minimum. To achieve this, we have to select Q and R matrix to have
2Do not confuse this Q with controllability matrix Q, different.
15
CHAPTER 3 LQR control
the desired response. Varying these matrices we are changing their relative importance in
cost function. This selection depends on system behaviour [7]. In this case we will follow
matrix 3.19 model.
Q =
q1 0 0 0
0 q2 0 0
0 0 q3 0
0 0 0 q4
(3.19)
Where qi = 1∆ximax
, being ∆ximax the maximum deviation of xi variable from its working
point [7].
The first control action aim is to balance the pendulum angle to keep it around 0o and
to match initial position with final one. Later we will make a second control to carry the
position of the cart where we want, but in this first problem it is supposed that all inputs
are disturbances (not reference inputs).
For this reason ∆ximax of states position (x1) and angle (x3) should be minimum and
therefore their elements qi of 3.19 matrix have a high value. On the other states we do not
impose restrictions on maximum variations (or equivalently: high maximum variations⇒∆ximax ≈ 0).
For simplicity we choose R = 1 and try varying q1 and q3.
This information has been introduced in the m-file (Appendix B). MATLAB function
lqr calculates feedback matrix Klqr. This function has for inputs matrix A and B, which
characterized the system; and Q and R, which characterized LQR control. Using function
lsim we can obtain the simulation of the system to a unit step disturbance. To do this,
A and Klqr are grouped in this form: Alqr = A−BKlqr [2, Section 5.2].
(a) Position variation due to q1 (b) Angle variation due to q1
Figure 3.3: System’s response to different values of q1
Then, q1 and q3 need to be set in order to achieve a satisfactory control. First fixing
q3 = 10 and varying q1: q1 = 10, q1 = 100, q1 = 1000 and q1 = 10000, Figure 3.3 results
16
CHAPTER 3 LQR control
are obtained. Figure 3.3 (a) is the change experienced by the cart’s position to different
values of q1, and Figure 3.3 (b) is the variation of the angle for different values of the
same parameter.
As previously guessed, it is proved that result is all the more satisfying the higher the
value of q1. In fact, giving even greater values of q1 response is improved, but we decide
to set q1 = 10000 because the response is good enough.
Similar process is performed to calculate the value of q3. Fixing q1 = 10000, and trying
with q3 = 10, q3 = 100, q3 = 1000 and q3 = 10000, results of Figure 3.4 are obtained,
where again we study how the change of q3 affects both output separately.
(a) Position variation due to q3 (b) Angle variation due to q3
Figure 3.4: System’s response to different values of q3
Results agree again with prediction made initially that the higher the value of q3,
the minor the variation from operating point. However, in this case is not selected the
highest value of q3 because, although it varies less than other values of q3, its parking time
is higher. So q3 = 1000 is selected.
Final result with q1 = 10000 and q3 = 1000 is showed in Figure 3.5. It is adopted
because its features of rise and establishment times, overshoot, and steady-state error are
acceptable values. Position steady-state error, although is acceptable, is not null. Further
study will justify mathematically why this error can not be reduced and has this value.
As discussed in the beginning of the section, we will implement other state feedback
control that mimics the dynamics achieved. For that, it will be set using the same poles
obtained in this LQR model. These poles are the same than eigenvalues of matrix Alqr
[2, Section 1.6]. To find them, m-file is run. We observe the system is characterized by
two conjugate pole pairs:
p1 = −9.21+8.45 i p2 = −9.21−8.45 i p3 = −4.03+1.29 i p4 = −4.03−1.29 i (3.20)
17
CHAPTER 3 Pole placement control
Figure 3.5: Respuesta final utilizando un control LQR
3.5 Pole placement control
The power of this new control structure is evident because you can specify exactly the
desired behaviour forcing the feedback system to take the poles where you prefer. This
advantage makes it a model which can achieve behaviour closer to our desire than LQR,
which is a try and error method.
In this case, final result will be the same than using LQR, but we could have calculated
other more suitable poles, subject to more ambitious restrictions than those imposed of
rise time, establishment time, overshoot, and so on. But in this case we have enough with
our control: Figure 3.5 shows a very acceptable result.
This control structure can only be applied to both controllable and observable part of
the system. In this case there is no limit to its application because our system is totally
controllable and observable (as demonstrated in Section 3.3).
The property of observability is necessary to implement the control, but in this section
we are not going to use this property because it is assumed that we have prior information
of every states, all of them are directly measurable. These concepts are not equivalent,
because even having a totally observable system, it would be possible not to have the
necessary measuring instruments to know directly the value of any state. In following
sections we will study the case in which is not possible to access to this information, so we
would have to take advantage of having a fully observable system to apply the equations
needed to estimate the unknown variables.
Also we will take advantage of having an invariant linear system using simpler math-
ematical structures.
If states are feedback with with K matrix, block diagram can be restructured to obtain
a new matrix A, called Ar, composed like: Ar = A+BK [2, Section 5.2]. Ar relates states
18
CHAPTER 3 Pole placement control
with output, so choosing correctly K we could change Ar and achieve a feedback system
with the desire dynamic.
As noted above, this new control will place poles in the same place than LQR. Or
equivalently, eigenvalues of matrix Ar must coincide with poles 3.20.
The process is then developed to calculate Ar, so that it has these eigenvalues, and
solve K to feedback the system.
3.5.1 Phase variables transformation
In order to resolve the ensuing system of equations and find K, it is convenient to trans-
form the current state model and represent it by its phase variables. This will simplify
calculations. Appendix A shows model structure represented by its phase variables. Here
we only show Ar structure in order to make the explanation livelier. Note that here Ar is
also represented by its phase variables (so it is written with a hat), because in this case
it has been built using A, B and K.
Ar =
0 1 0 · · · 0
0 0 1 · · · 0...
......
. . ....
0 0 0 · · · 1
k1 − a0 k2 − a1 k3 − a2 · · · kn − an−1
(3.21)
Transformation matrix to go from any representation to phase variables is obtained
using the following procedure:
1. Inverse of controllability matrix Q:
Q−1 =
eT1
eT2...
eTn
2. Construction of transformation matrix inverse:
T−1C =
eTn
eTnA...
eTnAn−1
3. Transformed model:
A = T−1C ATC B = T−1
C B C = CTC
19
CHAPTER 3 Pole placement control
Program this process with MATLAB matrix A and B are obtained:
A =
0 1 0 0
0 0 1 0
0 0 0 1
0.00 2.10 28.00 −0.09
B =
0
0
0
1
(3.22)
It is check that matrix appear represented by their phase model, because they have
same configuration.
3.5.2 Calculation of K
Comparing 3.21 with A of 3.22, it is possible to build Ar keeping it as a function of
K components. As we are working with phase variables, K also will be represented in
this way and then we will have to anti-transform it in order to restore to its original
representation.
Ar =
0 1 0 0
0 0 1 0
0 0 0 1
k1 k2 + 2.10 k3 + 28 k4 − 0.09
(3.23)
Moreover, as discussed above, being Ar matrix A in closed loop of feedback system,
will determine the behaviour of controlled system and therefore its eigenvalues will have
to be 3.20 poles. The same happens with Ar, because base transformation does not affect
either its eigenvalues or its characteristic polynomial.
Then, we can already make up two equivalent expressions of Ar. We just have to
match them and clear the components of K: k1, k2, k3 and k4.
A more direct process is firstly calculate characteristic polynomial of 3.23, which will
be function of ki (3.24 equation).
s4 + (0.09− k4)s3 + (−28− k3)s2 + (−2.10− k2)s− k1 = 0 (3.24)
By definition, the roots of this 3.24 polynomial, will be the eigenvalues of Ar matrix.
Moreover a polynomial is built using poles of 3.20 (3.25 equation).
s4 + 26.5 s3 + 322.6 s2 + 1589.5 s+ 2800 (3.25)
We just compare both polynomials 3.24 and 3.25 term by term, and clear values of ki.
K =[k1 k2 k3 k4
]=[2800 −1591.6 −350.6 −26.4
](3.26)
Last step is anti-transform in order to represent K in the original base of the system:
K = KT−1C =
[100 56.84 −167.70 −34.81
](3.27)
20
CHAPTER 3 Pole placement control
We can notice it is the same result than the other achieved with LQR control.
MATLAB allows calculate directly K matrix using commands place(A,B,polos) or
acker(A,B,polos) (with opposite sign, because the suppose negative feedback).
Here we have shown the entire process because of theoretical interest, but we will make
shorter the way using these commands in later problems.
3.5.3 SIMULINK simulation
Figure 3.6 diagrams are built in SIMULINK using information calculate doing the control.
The first represents the physical system of the inverted pendulum without any kind of
control. The second one adds the feedback state control by K.
(a) Inverted pendulum (b) Inverted pendulum controlled
Figure 3.6: SIMULINK diagrams of inverted pendulum on level ground
The physical system without control has a totally unstable response as is expected: if
the pendulum is subjected to a stimulus it falls. In the figure it appears as if the angle and
space increased exponentially. It is an illogical result that occurs because we hypothesized
the angles were small, so when it goes away from this operating point, the model is not
reliable. This is noticed in 3.7 Figure, where the system has been excited with a step
input.
If controlled system (Figure 3.6, (b)) is excited in t = 1 s with a 0.25N step input,
and starting with zero initial conditions, Figure 3.8 is obtained.
Figure 3.8 (a) shows states evolution. First and third are system output (position
and angle states). We can notice this output is exactly like the other obtained with LQR
model, because it is used the same feedback matrix. This picture also shows that the only
state that has error in permanent regimen is the position3. This will be mathematically
justify later, where this error will also be related trying with other kind of inputs such as
impulses and ramps.
In t = 1 s the step is acting as a disturbance, but quickly the control starts to work
against the disturbance. Second part of Figure 3.8 shows this. Here it is showed the
3And this error is not of concern because it is of order 10−3.
21
CHAPTER 3 Pole placement control
Figure 3.7: Pendulum response without control
(a) States (b) Input and control
Figure 3.8: 0.25N step input
input and control action response. We can check that when initial unbalance is stabilized,
control action keeps in −0.25N in order to compensate the source.
If the system starts with non-zero initial conditions (x0 = 0.5m, x0 = 1 ms
, θ0 =
20 π180
rad θ0 = 0 rads
) and without external disturbance, it reacts like Figure 3.9 shows.
These initial conditions have been configured setting the integrator block in SIMULINK’s
diagram. Figure 3.9 (a) shows the output evolution as a result of these non-zero initial
conditions. Firstly we can perceive that now the position has not got error in permanent
regimen (as we said, we will justify this later), and moreover it is stabilized around
x = 0m, not around initial position. This response is the desired one because the aim is
to assign x = 0m as operating point on which the system should be balanced. Later we
will do other kind of control in order to vary this operating point and be able to drive
the cart where we want. Moreover, control force acts instantaneously, but pendulum
22
CHAPTER 3 Pole placement control
(a) Output (b) States
Figure 3.9: Initial conditions: x0 = 0.5m, x0 = 1 ms
, θ0 = 20 π180
rad θ0 = 0 rads
continues falling just for a moment (θ raises) until it reacts. When response time pasts,
it starts to stabilize.
Figure 3.9 (b) is a zoom of the first part of the period. We can notice that all the states
start from initial conditions specified, continue their trends going away from equilibrium
point 0 during a short period of time, and then rectify stabilizing themselves because of
feedback.
Figure 3.10: Initial conditions + Step + Impulse
Figure 3.10 represents a more complex case. Firstly the system start with the same
non-zero initial conditions than last case. In t = 3 s it undergoes a step source 20
times bigger than first case (5N). It is remembered from Section 2.2 that this force must
act on the cart. Finally, in t = 6 s a impulse disturbance acts (it could be a push or
a pothole in the street). This impulse has been modelled using two step sources: first
23
CHAPTER 3 Design of an observer
acting in t = 6 s with value amplitudN , and second acting in t = 6+duracion with value
−amplitudN , where duracion = 10−5 s and amplitud = 1duracion
N .
Figure 3.10 is a summary of last explanation. First, the system reacts to a non-zero
initial conditions, then acts against a step, moment in which appears a residual steady
state error, and finally it is also balanced the effect of a impulse. Therefore it is proved
the success of control system.
3.6 Design of an observer
Being our system observable, it is possible to know the value of all states just having
information about input and output. This is useful, because we don’t need to use mea-
suring instruments to know this information: we can calculate it. We need to know all
this final values because they are used in feedback multiplying K. This section deals with
this problem.
3.6.1 Observable canonical form transformation
As in K calculation, is worth to make the calculates of the observer design firstly trans-
forming the system to its observable canonical form (Appendix A). The process is:
1. Inverse of observability matrix P
P−1 =[e1 e2 · · · en
]2. Construction of transformation matrix inverse:
To =[en Aen · · · An−1en
]3. Transformed model:
A = T−1o ATo B = T−1
o B C = CTo
As C has [8 × 4] dimensions, is not possible to calculate P inverse. That is not a
problem because we will consider that unique solution is position, then:
Cnew =[1 0 0 0
](3.28)
There is no inconvenient doing it because new matrix P which generates Cnew continues
having rank 4, therefore the system continues being totally observable. It would not be
the same if the angle was considered as unique output. In this case we would have P rank
equal to 3 and we could not estimate the states just having information about angle and
input.
24
CHAPTER 3 Design of an observer
Figure 3.29 shows A and B matrix represented in observed canonical form.
A =
0 0 0 0
1 0 0 2.10
0 1 0 28
0 0 1 −0.09
B =
−28
0
1.28
0
(3.29)
In this problem we will be playing with three different representations: the original,
the controllable canonical form (or phase variables) and the observable canonical form.
We will have to move from one to another several times, so it is interesting to know matrix
TCO, which relates both controllable and observable canonical forms. This matrix TCO
will be a combination of the two transformation matrix:
TCO = T−1C TO (3.30)
3.6.2 Calculation of parameters characterizing the observer
The observer will have as inputs the inputs and outputs of the system and as output the
estimated states, therefore remain a dynamic defined by the equations:
xe(t) = Fxe(t) +Gu(t) +Hy(t)
ye(t) = Cxe(t)
Imposing to this model the necessary conditions to be an observer, we will achieve the
equations used to its design [2, Section 6.2].
First we must determine the poles of F . F has to be stable in order to make the error
tends to zero, therefore all its poles must be in negative half-plane. Additionally, the
dynamic of the observer has to be significantly faster than the observed system, because
it has to estimate the value of the states faster than they vary. Therefore, poles of F will
have to be negatives and their real part must have an absolute value much bigger than
real part of Ar poles. Knowing that poles of Ar are 3.20, we choose these polos for the
observer:
p1 = −50 p2 = −51 p3 = −52 p4 = −53 (3.31)
With these poles characteristic polynomial is built:
n∑i=0
fi si = s4 + 206s3 + 15911s2 + 546106s+ 7027800 = 0 (3.32)
Using the coefficients of this polynomial, it is possible to construct matrix F according
to expression 3.33.
F =
0 0 0 −f0
1 0 0 −f1
0 1 0 −f2
0 0 1 −f3
(3.33)
25
CHAPTER 3 Design of an observer
Taking advantage of all matrices are expressed in observable canonical form, it is easy to
know H clearing from expression 3.34.
F = A−HC (3.34)
Last equation of the observer design dictates that G = B.
3.6.3 SIMULINK simulation
Observer is added to control diagram according to Figure 3.11.
Figure 3.11: Observer diagram in SIMULINK
It achieves the same response than last section, therefore this observer estimates cor-
rectly the states.
It is interesting to study the error in the estimate, in order to do that, we can sub-
tract estimate value minus real value of each state. For instance, according to Figure
3.11, in order to represent estimate error in cart’s position we should run in MATLAB:
plot(tiempo,salida(:,1)-estados1(:,1)). In Figure 3.12 this study is made suppos-
ing a step input of 0.25N starting in t = 1 s.
It should be noted that the estimated states are not those which leave the observer,
because there they are represented in observable canonical form. To know their value in
original base, it is necessary multiply them by TO, as done in Figure 3.11. Later, they
have to be past to controllable canonical form and be feedback with K (other option is
to feedback them directly with K).
It is proved in Figure 3.12 that the observer estimates successfully (errors of position,
speed, angle and angular speed are of order 10−11, 10−9, 10−7 and 10−6 respectively).
26
CHAPTER 3 Design of a reduced order observer
(a) Position (b) Speed
(c) Angle (d) Angular speed
Figure 3.12: Estimation errors
3.7 Design of a reduced order observer
In the previous section all states have been estimated from information of input and out-
put. However it may be not necessary to estimate all if we have already some information
about some of them. It is worth to take advantage of this information to design a more
efficient observer that just estimates the unknown states.
However in this case we are going to assume we just have information about position
for two reasons:
• To compare results with the complete observer.
• Because the reduced-order observers takes information from the output and the
derivative of the output. If we assume that it is possible to measure both position
and angle, and the observer also takes information of its derivatives (velocity and
angular velocity), we are actually accessing to information about all states of the
system (because the other two are just their derivatives). Thus the utility of the
observer for this system would disappear.
27
CHAPTER 3 Design of a reduced order observer
3.7.1 Model transformation
To make the design of a reduced order observer we must have a model in which the
accessible variables of the system are selected as outputs. Therefore matrix C will be like:
C =[Ij | 0
]Where j is the number of accessible variables of the system.
The transformation matrix used to achieve this model will be [2, Section 6.6]:
T−1OR =
[C ′1 C ′2
Iσ 0
](3.35)
In this case it would be possible to consider that it is not necessary transformation
matrix, because the state considered as accessible (position) is already placed first in the
output matrix. Therefore matrices would be already placed in an appropriate way to
proceed with observer construction.
However if we continue with original matrix, we will have a lot of problems about
matrix dimensions. To avoid this we will do an intermediate transformation and then we
will transform again using TOR, keeping all the matrix in the correct form to be able to
solve all the equations without dimension problems. The question is: which intermediate
model should be chosen to do the first transformation? Considering that we will have to
do the inverse of matrix TOR, this new model must generate a matrix T−1OR invertible. A
possible intermediate state model that allows to do this is the observable canonical form.
It also has the advantage that we can use the calculations made for the full order observer.
In short: we will first convert actual system to observable canonical form, and second we
will transform this result to reduced order observer canonical form.
These two steps can be combined as a single composing the transformation matrices:
ATO−→ A′
TOR−→ A′′
A′ = T−1O ATO
A′′ = T−1ORA
′ TOR
}A′′ = T−1
ORT−1O ATO TOR = T−1
TotATTot ⇒ TTot = TO TOR (3.36)
Being C in observed canonical form: C =[0 0 0 1
], matrix TOR can be built
following the structure of matrix 3.35. We check that it has rank 4 so it is invertible.
TOR =
0 0 0 1
1 0 0 0
0 1 0 0
0 0 1 0
(3.37)
28
CHAPTER 3 Design of a reduced order observer
Taking this result and knowing TO of last section, equation 3.36 is used to compose
TTot. Applying this transformation to matrices A, B and C:
A =
−0.09 0 0 1
0 0 0 0
2.1 1 0 0
28 0 1 0
B =
0
−28
0
1.29
C =[1 0 0 0
](3.38)
A new model, different from original, has been reached (matrix A and B are different),
but in which C continues providing the observable state as output of the system. In fact,
with this new system we will not have any problem solving equations
3.7.2 Calculation of characteristic parameters of the reduced
order observer
Grouping accessible states as y and non accessible as w, it is possible to divide the model
following this construction:
x =
[y
w
]=
[A11 A12
A21 A22
][y
w
]+
[B1
B2
]u (3.39)
Imposing the necessary restrictions to achieve an observer behaviour, we obtain the
equations used in its design [2].
As in the complete order observer, we should choose poles of the reduced order observer
sufficiently far from dominant poles of pendulum system to make observer dynamics faster.
In this case:
p1 = −50 p2 = −51 p3 = −52 (3.40)
The develop is analogous: these poles are used to form their characteristic polynomial
and with its coefficients we build F .
From F = A22−H2A12 it is clear H2. Matrix A12 and A22 are easily deduced comparing
the model with 3.39 structure.
H1 is calculated: H1 = H2A22 −H2A12
Finally G is obtained: G = B2 −H2B1
3.7.3 SIMULINK simulation
In order to know this observer performance, diagram of Figure 3.13 is built.
Response is the same than having information of all the states, therefore the observer
has been designed correctly.
Again we examine the errors between this estimation and the exact state value to
study more in detail the performance of this observer.
29
CHAPTER 3 Design of a reduced order observer
Figure 3.13: Diagram of reduced order observer in SIMULINK
To do that, the system of Figure 3.13 is excited with a step source of amplitude 0.25N
starting in t = 1 s. Errors of Figure 3.14 are obtained.
Figure 3.14 shows that reduced order observer has better performance than complete
order observer in every way.
(a) Position (b) Speed
(c) Angle (d) Angular speed
Figure 3.14: Estimate errors
30
CHAPTER 3 Track a reference input for the position
Firstly, graphic (a) shows a totally zero error because this state has not been esti-
mated: it has been added directly after the observer. This will also have the advantage
of subtracting operating load to the observer and therefore to be more efficient.
The error is also reduced in the rest of the graphics. Using complete order observer
the error was practically zero, but with this new observer the estimation is even better
because the error is one order of magnitude shorter in each state. Also, this error does
not oscillate during all the performance: it tends to zero.
3.8 Track a reference input for the position
So far, the aim was to oppose to disturbances effect. The system always balances itself
taking all states to “zero” values. However, it is interesting to add to the system the
ability to follow a path. This feature is clearly necessary if the application is to build a
Segway, because the objective is not just neutralize vibrations: we also want to be able
to direct it wherever we want.
To achieve this, reference signal will be model as a new source that “position” state
(x1) has to follow. This input is different from disturbances.
We will do a PID control for following reference input, then it will be added to the
previous control of disturbances. Figure 3.15 shows this. A reference signal in ramp form,
and a disturbance in the form of unit step acting in t = 1 s are taken as an example. The
ramp has a positive slope of 1 Ns
, therefore the aim is to move the cart in a constant speed
of 1 ms
in the positive x-axis. Thus, final position will be x = 10m.
Figure 3.15: System for controlling the position
PID regulators are SISO systems, so they can only work with one input and one
output. In fact, it is not possible to make the previous control using just a PID because
against a disturbance, it could not control simultaneously the two outputs (position and
angle).
In this case it is used the pole placement control for the disturbances. This control
has already been explained thoroughly in Section 3.5, so in Figure 3.15 it is represented
into the System block.
31
CHAPTER 3 Track a reference input for the position
A PID is characterized by the transfer function:
FDTPID =Kds
2 +Krs+Ki
s= Dds+Kr +Ki
According to this construction, in Figure 3.11 PID is showed divided in its three
components. It is placed before the disturbance and acts correcting the error between
output (position) and the reference input.
Having to operate the PID with a single input and output, in pole placement control,
matrix C has been changed in order to have only one output. This output will be the
position: C =[1 0 0 0
]. This is not a drawback, because this system will continue
acting balancing disturbances, although its only output is the position.
PID regulator will be configured experimentally by try with its parameters.
Below are showed the tests performed to achieve values that get a satisfactory answer.
Firstly, we fix Kr = 1 and Kd = 1, and vary Ki = 10, 1, −1, −10, −30, −70, −150 and
−200. This study is about integral action of the PID, that works minimizing the steady
state error checking the variation between the output and the set-point input. Results
are showed in Figure 3.16.
Figure 3.16: Ki adjust
Examining Figure 3.16, we realize that the most appropriate value is Ki = −150.
Higher values (lower in absolute value) have higher steady state error, and lower values
(higher in absolute value) cause the system starts to oscillate to become unstable.
Fixing Ki = −150 and maintaining Kd = 1, we vary Kr = 10, 1, −1, −10, −30, −50 y
−70. This is the proportional action of PID, that works together with integral action
minimizing steady state error and also overshoots. The study is done in Figure 3.17.
Kr = −30 is chosen like optimal value, because both higher values and lower values
mean higher overshoots.
32
CHAPTER 3 Track a reference input for the position
Figure 3.17: Kr adjust
Finally we vary Kd = 10, 1, −1, −10 keeping Ki and Kr on their optimal values. This
calculation is the derivative action of the PID, and it is related to the rapidity with
which the control responds to errors (Figure 3.18).
Figure 3.18: Kd adjust
Kd = 1 achieves the best answer as it is showed in Figure 3.18.
Using this control we can carry the cart with the inverted pendulum to the desired
point and also balanced disturbances or non-zero initial conditions.
The system is also valid with other kind of sources, as discussed in the following
section.
33
CHAPTER 3 Study of the steady state error
3.9 Study of the steady state error
It has been observed all over this work that there is a steady state error associated with
each system response. This section justify mathematically this error and its magnitude.
Figure 3.19 is the blocks diagram of the complete system. V (s) is the reference input
that the position should follow; R(s) is the PID regulator designed to follow V (s); D(s)
are the disturbances; P (s) is our system plant, which includes the inverted pendulum
model, the disturbance control (section 3.5), and a observer; finally Y (s) is the output.
Figure 3.19: Blocks diagram
Therefore R(s) will be transfer function of PID:
R(s) =Kd s
2 +Kr s+Ki
s=s2 − 30s− 150
s(3.41)
P (s) groups: transfer function of the inverted pendulum, the state feedback control
and the observer. The observer does not affect to system dynamic, because its poles are
much farther from real axis. It is easy to obtain the transfer function of the system and
the state feedback control: from their characteristic matrix of the state model [2, Section
1.6]:
P (s) = C[sI − Ar
]−1
B +D (3.42)
Where we have to choose C =[1 0 0 0
]as explained in the previous section.
Calling G(s) = R(s)P (s) and doing superposition of inputs in Figure 3.19:
Y (s) =G(s)
1 +G(s)V (s) +
P (s)
1−G(s)D(s) (3.43)
The error E(s):
E(s) = V (s)− Y (s) = V (s)− G(s)
1 +G(s)V (s)− P (s)
1−G(s)D(s)⇒
⇒ E(s) =1
1 +G(s)V (s)− P (s)
1−G(s)D(s) (3.44)
34
CHAPTER 3 Study of the steady state error
Calling:
G(s) =Kr
srG(s) (3.45)
P (s) =Kw
swP (s) (3.46)
Where G(s) and P (s) satisfy that: G(s = 0) = 1 and P (s = 0) = 1. It is easy to
realise that r and w are the number of poles in the origin of G(s) and P (s) respectively.
Calculating with MATLAB transfer function P (s) (equation 3.42) and comparing with
equation 3.46, it is proved that w = 0 and Kw = −0.01.
Knowing that G(s) = R(s)P (s), and comparing with 3.45, we achieved: r = 1 and
Kr = 1.5.
The total error in permanent regimen will be the superposition of two errors: one
caused by reference input and the second error caused by disturbances, as is showed in
equation 3.44.
In Appendix A, it is calculated analytically what should be the magnitude of these
errors for step and ramp sources.
We can make a lot of combinations with these calculates. Below three examples
are studied: step reference input and ramp disturbance; ramp reference input and step
disturbance; and step input and step disturbance.
V (s) step and D(s) ramp
If values of r = 1 and w = 0 are replaced in equations A.3 and A.7 (calculates in Appendix
A), we can achieve the values of errors:
• Error caused by reference input: eV = 0
• Error caused by disturbances: eD = Kw
Kr= −0.01
1.5= −0.0067
Therefore, total error e will be −0.0067.
In the other hand it is simulated in MATLAB this case, in order to prove if we achieve
with SIMULINK the same results calculated theoretically. Figure 3.20 is obtained. It is
verified that these results match up exactly with the others intuited mathematically
V (s) ramp and D(s) step
In this example we want to carry the cart with constant speed and to balance a step
disturbance in t = 0 s.
If values of r = 1 and w = 0 are replaced in equations A.4 and A.6, we can achieve
the values of errors:
• Error caused by reference input: eV = 1Kr
= 11.5
= 0.6667
35
CHAPTER 3 Study of the steady state error
(a) Step source, ramp disturbance (b) Zoom of e
Figure 3.20: Estimate errors
• Error caused by disturbances: eD = 0
And total error: e = eV + eD = 0.6667.
Simulation with MATLAB is showed on Figure 3.21. Again data obtained in the
simulation match exactly with mathematical results.
(a) Ramp source, step disturbance (b) Zoom of e
Figure 3.21: Estimate errors
V (s) step and D(s) step
By proceeding analogously:
• Error caused by reference input: eV = 0
• Error caused by disturbances: eD = 0
36
CHAPTER 3 Study of the steady state error
Following this indications, total error must be zero.
Looking at Figure 3.22 it is proved again that is the same than mathematical deduc-
tion: having a step for both reference input and disturbance input, the error in permanent
regimen is zero.
Figure 3.22: Step source, step disturbance
37
CHAPTER 4
INVERTED PENDULUM ON SLOPING GROUND
The last chapter was focused to perform a theoretical study of all factors involved in the
control structure.
Now, we will work with situations closer to reality, where the goal is to design some-
thing with practical interest. The mathematical model is more general, because it includes
the possibility of having inclined floor. The study has carried out with the intention of
imitate a Segway, but it is also valid for other real system, as it was explained in Section
2.2.
Both the procedure of calculations and conclusions of the results of the inverted pen-
dulum on sloping ground, are similar to those of the pendulum on level ground. Therefore
we will not repeat either obvious steps or interpretations already mentioned about control
structure and intermediate calculates. In this cases just the different between last chapter
will be commented.
4.1 Mathematical model of the system
As it was explained in Section 2.2, in a inverted pendulum on sloping ground act the
forces showed in Figure 4.1.
System is divided in its parts (pendulum and cart, Figure 4.2) in order to know
its equations. In these figures are shown all magnitudes that will be involved in the
development of the equations. The new parameters are: ~x(t) and ~y(t) determine pendulum
position respect reference axis ~X, ~Y . The origin of this reference system ~X, ~Y is located
at any point on the surface of the plane. ~x′ and ~x′′ will be necessary below and are: the
horizontal distance from reference point ~X = 0 to the ground, and form the ground to
the pendulums axle respectively. Therefore it is true that ~x(t) = ~x′(t) + ~x′′. ~xc(t) and
~yc(t) locate pendulum’s center of mass respect reference system ~X, ~Y . gc and lc are the
38
CHAPTER 4 Mathematical model of the system
Figure 4.1: Inverted pendulum on sloping ground
thickness and the length of the cart respectively1.
(a) Pendulum (b) Cart
Figure 4.2: System division into its two parts
Applying Newton’s Second Law to these systems we obtain the equations:
• Pendulum:
–∑ ~Fx(t) = m~x(t)
−~Rx(t) = md2~xp(t)
dt2= m
d2
dt2
(~x(t)− l sin ~θ(t)
)⇒
~Rx(t) = −m~x(t) +ml~θ(t) cos ~θ(t)−ml~θ2(t) sin ~θ(t) (4.1)
1Firstly it is supposed that the pendulum is rectangular, later it will be generalized for other kind of
bodies.
39
CHAPTER 4 Mathematical model of the system
–∑ ~Fy(t) = m~y(t)
−~Ry(t)−m~g = md2~yp(t)
dt2= m
d2
dt2
(~y(t) + l cos ~θ(t)
)⇒
~Ry(t) +m~g +m~y = ml~θ(t) sin ~θ(t) +ml~θ2(t) cos ~θ(t) (4.2)
–∑ ~T (t) = I~θ(t).
− ~Ry(t)l sin ~θ(t)− ~Rx(t)l cos ~θ(t) = I~θ(t) (4.3)
• Cart:
–∑ ~Fx(t) = M~x(t)
~F (t) cos γ − ~FFric(t) cos γ + ~Rx(t) = Md2~xc(t)
dt2= M
d2
dt2
(~x(t)− gc
2sin γ
)⇒
~F (t) cos γ − b~x(t) cos γ + ~Rx(t) = M~x(t) (4.4)
–∑ ~Fy(t) = M~y(t)
−~F (t) sin γ + ~FFric(t) sin γ + ~Ry(t) = Md2~yc(t)
dt2= M~y(t)⇒
−~F (t) sin γ + b~x(t) sin γ + ~Ry(t) = M~y(t) (4.5)
–∑ ~T (t) = I~θ(t)
−~FFric(t)���gc2
+ ~Ry(t) sin γ���gc2− ~Rx(t) cos γ
���gc2
= 0⇒
b~x(t)− ~Ry(t) sin γ + ~Rx(t) cos γ = 0 (4.6)
We can observe in equation 4.6, that gc is cancelled and disappears, therefore we will
be able to apply this model to all kind of carts which are symmetric about its longitudinal
and transversal axis (not only to rectangles, as it was supposed).
Variables x and y depend mutually because the pendulum is always in contact with
ground surface. Thus, we should delete one of the two variables in order to work with a
simpler system. To do that it will be make a change of variable. Looking Figure 4.1 we
take the information to make this change:
sin γ =gc~x′′⇒ ~x′′ =
gcsin γ
(4.7)
~x(t) = ~x′(t) + ~x′′ = ~x′(t) +gc
sin γ⇒ ~x′(t) = ~x(t)− gc
sin γ(4.8)
tan γ =~y(t)
~x′(t)=
~y(t)
~x(t)− gcsin γ
⇒ ~y(t) = ~x(t) tan γ − gccos γ
(4.9)
⇒ ~y(t) = ~x(t) tan γ ⇒ ~y(t) = ~x(t) tan γ (4.10)
40
CHAPTER 4 Mathematical model of the system
Applying this change 4.10 to equations which included variable ~y(t) (equations 4.2 and
4.5):
~Ry(t) +m~g +m~x(t) tan γ = ml~θ(t) sin ~θ(t) +ml~θ2(t) cos ~θ(t) (4.11)
−~F (t) sin γ + b~x(t) sin γ + ~Ry(t) = M~x(t) tan γ (4.12)
Substituting 4.1 and 4.11 in 4.3, and doing an analogous development as it was done in
pendulum on horizontal ground, it is obtained equation 4.13.
~θ(t)[I +ml2
]−mgl sin ~θ(t) = ~x(t)ml
(cos ~θ(t) + tan γ sin ~θ(t)
)(4.13)
Substituting 4.1 in 4.4 it is obtained 4.14.
~F (t) cos γ = (M +m) ~x(t) + b~x(t) cos γ +ml~2θ(t) sin γ −ml~θ(t) cos ~θ(t) (4.14)
As in pendulum on horizontal ground, it is necessary to linearise around ~θ(t) ≈ 0 to be
able to obtain the state model.
~θ(t)[I +ml2
]−mgl~θ(t) = ~x(t)ml
(1 + tan γ ~θ(t)
)(4.15)
~F (t) cos γ = (M +m) ~x(t) + b~x(t) cos γ −ml~θ(t) (4.16)
Solving ~θ(t) from equation 4.16:
~θ(t) =M +m
ml~x(t) +
b cos γ
ml~x(t)− cos γ
ml~F (t) (4.17)
Replacing 4.17 in 4.15, and ordering obtained expression, it is achieved equation 4.18.
~x(t)
[ml(
1 + ~θ(t) tan γ)− (M +m)
(I
ml+ l
)]=
= b cos γ
(I
ml+ l
)~x(t)− cos γ
(I
ml+ l
)~F (t)−mgl~θ(t) (4.18)
It is appreciated that the first term of 4.18 is non linear, therefore it will not be possible
to obtain a model in the space state to apply the same techniques as in last chapter. This
can be solved supposing that γ ≈ 0, then this term: 1 + ~θ(t) tan γ ≈ 1. Doing this we
are already working with equations which all their variables are linear. Later we will see
that this simplification is not very restrictive, because the control will be able to answer
correctly on relatively high surface inclinations.
α and β are created to group constants:
α =I
ml+ l β = ml − (M +m)α
Applying this changes to equation 4.18:
~x(t) =αb cos γ
β~x(t)− α cos γ
β~F (t)− mgl
β~θ(t) (4.19)
Finally, 4.19 is replaced in 4.17. Ordering terms, equation 4.20 is achieved.
~θ(t) =
[M +m
ml
αb cos γ
β+b cos γ
ml
]~x− (M +m) g
β~θ(t)−
(M +m
ml
α cos γ
β+
cos γ
ml
)~F (t)
(4.20)
41
CHAPTER 4 State space representation
4.2 State space representation
They are selected the same state variables than in pendulum on level ground:
x1(t) = ~x(t) x2(t) = ~x(t) x3(t) = ~θ(t) x4(t) = ~θ(t)
Thus, it is true that:
x1(t) = x2(t) x3(t) = x4(t) (4.21)
Rewriting equations 4.19 and 4.20 it is obtained:
x2(t) =αb cos γ
βx2(t)− mgl
βx3(t)− α cos γ
β~F (t) (4.22)
x4(t) =
(M +m
ml
αb cos γ
β+b cos γ
ml
)x2(t)− (M +m) g
βx3(t)+
−(M +m
ml
α cos γ
β+
cos γ
ml
)~F (t) (4.23)
It is possible to write equations 4.22, 4.23 and 4.21 in matrix form:x1(t)
x2(t)
x3(t)
x4(t)
=
0 1 0 0
0 a1 a2 0
0 0 0 1
0 a4 a5 0
x1(t)
x2(t)
x3(t)
x4(t)
+
0
a3
0
a6
~F (t) (4.24)
Siendo:a1 = αb cos γ
βa4 = (M+m)b cos γ α
mlβ+ b cos γ
ml
a2 = −mglβ
a5 = − (M+m)gβ
a3 = −α cos γβ
a6 = − (M+m) cos γ alphamlβ
− cos γml
As in last chapter, cart’s position and pendulum’s angle are assumed as outputs. This
information is resumed in equation 4.25. Both equation 4.25 and 4.24, make up the state
model of the inverted pendulum on inclined ground.
~y(t) =
[1 0 0 0
0 0 1 0
]x1(t)
x2(t)
x3(t)
x4(t)
+
[0
0
]~F (t) (4.25)
So characteristic matrix of the model are:
~A =
0 1 0 0
0 a1 a2 0
0 0 0 1
0 a4 a5 0
~B =
0
a3
0
a6
~C =
[1 0 0 0
0 0 1 0
]~D =
[0
0
](4.26)
42
CHAPTER 4 LQR control
So we already have the enough information to start designing the control of the pendulum
and doing in parallel the m-file (Appendix B) that makes easy the calculates. In following
sections will present this process, stressing the new points from the previous chapter and
ignoring those already mentioned.
The first observation we can notice is that if γ = 0, poles of matrix ~A of 4.26 state
model, are the same than poles of the state model of pendulum on horizontal ground
(expression ~A of 3.18), therefore the system has the same performance. This prove that
the develop has been made correctly.
4.3 LQR control
Once it has been proven we are working with a controllable and observable system (see
m-file Appendix B), it is possible to start doing a LQR control to balance the disturbances.
In order to simulate a situation closer to Segway, constants values will be modified:
M = 50 kg (mass of the Segway), m = 70 kg and l = 1.1m (mass and distance to the
center of gravity of the person riding on the Segway), γ = 10o (ground inclination). Rest
parameters: b, I and g (friction coefficient of the cart, rotation inertia of the person and
acceleration of gravity) are suppose the same than in horizontal pendulum.
Being guided by theory explained in Section 3.4, it can be seen that matrix 4.27
provide a satisfactory answer.
Q =
1000000 0 0 0
0 0 0 0
0 0 100 0
0 0 0 0
R = 1 (4.27)
It is possible to try changing constants values of the pendulum and see how they affect
to the respond. In a Segway cart’s mass M does not vary, however pendulum mass m
does vary and also the length to the center of mass l, depending which person uses the
Segway. Figure 4.3 shows how the balance of the system varies when a unitary step is
acting as a disturbance and the Segway is used by a person of 70 kg and other of 110 kg
(4.3 (a)); and when it is used by a person of l = 1.1m and other of l = 0.9m (4.3 (b)).
The results are expected: the same permanent regimen is achieved in both cases, but it
spends slightly more time to balance when person’s weigh is higher and when the length
to his/her center of mass is lower. In both cases the result is successful. It is achieve a
good control because variations between people induce changes of the parameters in very
small ranges and therefore does not significantly alter the response.
The control is performed for the constants values specified above. However Figure 4.3
shows that if some parameters are modified, the control is also acceptable: the transitory
regimen varies very slightly and the permanent regimen is identical. Moreover, variations
43
CHAPTER 4 Track a reference input for the position
(a) ∆m (b) ∆l
Figure 4.3: Respond when m and l vary
on the ground angle and on the friction cart’s coefficient induce changes in transitory
regimen even lower than the changed induced by person mass m and length to his/her
center of mass l. This is very important because it indicates that this control can be used
for different ground states and different people who ride on the Segway.
The steady state error that is appreciated was already justified mathematically in
Section 3.9.
4.4 Observers
It is advisable to design an observer to estimate system states that are not accessible.
The procedure is identical to that explained in the previous chapter.
In Section B it is detailed the code used in the design of a complete order observer
and a reduced order observer.
4.5 Track a reference input for the position
In this section a PID regulator will be added to be able to drive the cart anywhere. In
this case constants values of PID which optimize the respond are: Kr = −500, Ki = −700
and Kd = −1.
Figure 4.4 simulates system respond when it has to follow a ramp reference input and
work against two hard disturbances: a step of 2000N in t = 1 s and an impulse in t = 6 s2.
2The impulse has been modelled supposing two steps: one of amplitude 3 · 107N in t = 6 s, and the
second in t = 6.00001 s with the same amplitude but opposite sign.
44
CHAPTER 4 Animation
Figure 4.4: SIMULINK diagram for the total control of the pendulum on sloping ground
Inside the subsystem of SIMULINK it is the system of the inverted pendulum, the
control of the disturbances, and a reduced order observer (the same as in Figure 3.13).
Answer achieved is shown in Figure 4.5. We realise the prefect performance of con-
trol realised: cart’s position follows reference input and both disturbances are balanced
quickly. Therefore control system is be able to work as a simplify Segway.
Figure 4.5: Respond when it is acting a 2000N step in t = 1 s and an impulse in t = 6 s
4.6 Animation
It has been made in MATLAB a program that shows graphically the performance of the
inverted pendulum. The aim is to see the respond in a more intuitive and closer to reality
way.
Program’s inputs are: ground inclination, time vector, and other two vectors represent-
ing the evolution of the values of position and angle of the pendulum in each moment.
45
CHAPTER 4 Animation
Output of the program is an animation which have all this information. The code is
showed in Appendix B.
Animation dimensions used are justify by graphical reasons and they needn’t be the
same than real pendulum involved in simulation. Anyway, they can be configured in the
m-file.
It is supposed in the animation that the pendulum is a uniform density rod. Therefore
total length of pendulum is: lt = 2l m.
Program distinguishes between two cases: γ > 0 and γ < 0, because some of the
utilized parameters change their sign from one case to another. Above it is just described
γ > 0 case, because the develop with γ < 0 is the same.
Firstly we should fix dimensions of the screen in which animation is showed, in order
to focus correctly the process. Parameters utilized in the process to set the screen are
shown in Figure 4.6.
Figure 4.6: Variables used to fix the screen of the process
Once the screen is fixed, input vectors are covered. Position of cart and pendulum
will be drawn for each moment.
It is interesting to undo the change of variable used applying equation 4.10, because
now we need to know values of y. In this case we can not just replace equation 4.10, be-
cause it was obtained supposing the cart moves on ground surface (as a Segway), however
in this animation we intend to create another effect. New change is achieved following
the procedure showed bellow. Variables used in this procedure are showed in Figure 4.7.
46
CHAPTER 4 Animation
Figure 4.7: Variables used in animation programming
x = x′ + x′′ ⇒ x′ = x− x′′ (4.28)
sin γ =gc
2x′′⇒ x′′ =
gc2 sin γ
(4.29)
tan γ =−yx′⇒ y = −x′ tan γ = − (x− x′′) tan γ =
(gc
2 sin γ− x)
tan γ
⇒ y =gc
2 cos γ− x tan γ (4.30)
Knowing this information, and values of x and θ provided by program inputs, it is
already possible draw al the elements of the animation (rail, pendulum and cart). These
are characterized by marked points in Figure 4.7:
Pendulum
• Q : (x− lt sin θ, y + lt cos θ)
• 1 :(x− gt
2cos θ, y − gt
2sin θ
)• 2 :
(Qx − gt
2cos θ, Qy − gt
2sin θ
)• 3 :
(Qx + gt
2cos θ, Qy + gt
2sin θ
)• 4 :
(x+ gt
2cos θ, y + gt
2sin θ
)
Cart
• P : (x− gc sin γ, y − gc cos γ)
• 5 :(Px − lc
2cos γ, Py + lc
2sin γ
)• 6 :
(x− lc
2cos γ, y + lc
2sin γ
)• 7 :
(x+ lc
2cos γ, y − lc
2sin γ
)• 8 :
(Px + lc
2cos γ, Py − lc
2sin γ
)In these case it is easy to make calculates directly. However if we had a more complex
situation it would be convenient to use rotation matrix.
47
CHAPTER 4 Animation
Some intermediate pictures of balance process are showed in Figure 4.8 in order to
give an example of the animation.
(a) Pendulum on sloping ground γ = −5o (b) Pendulum on sloping ground γ = 10o
Figure 4.8: Inverted pendulum animation
48
CHAPTER 5
FINDINGS
5.1 Outcomes
We have achieved a successful control of the inverted pendulum on sloping ground. The
system responds appropriately balancing disturbances that can come in the form of push-
ing, potholes, changes of inclination, driver movements (in the case of a Segway), non zero
initial conditions, etcetera, and it is also able to follow a reference input in order to direct
it to the desire place.
We have also made an exhaustive study of the theoretical part of this project: math-
ematical model of a inverted pendulum and its simplifications to obtain a state space
representation, how each parameter influences in control structure, comparison between
several control methods (LQR, Pole Placement Control, PID), theoretical justification of
errors in steady state, and so on.
This mathematical development has always been proved with simulations made with
SIMULINK that are able to appear in reality.
It is possible either to don’t have the enough measure instruments to estimate all the
states, or to be physically impossible to measure some of them. To solve these problems
we have developed some observers which estimate these non accessible states. The most
effective of them is the Reduced Order Observer, as shown above. These observers calculate
mathematically these states, therefore we can also save money in measure instruments
and have a less complex system.
Finally an animation was made which simulated graphically the performance of pen-
dulum’s process.
49
CHAPTER 5 Other applications
5.2 Methodology of work
The first step was to develop by hand the mathematical model as generic as possible in
order to solve de problem proposed in Chapter 2. Then some few simplifications were
added in order to consider it as linear and temporally invariant and then be able to apply
control techniques.
Later all this information were programmed in a m-file to automate all the process
made by hand and to be able to test it with SIMULINK. Without any external imposition,
they were chosen MATLAB and SIMULINK as computer support of the problem because
they are powerful and comfortable tools for this kind of problems.
Therefore, although the codes showed in Appendix B are specified for chosen parame-
ters, the structure is generic, and then this control can be used for other similar situation.
5.3 Other applications
In the nature there are a lot of systems which behave like a pendulum. In many of theme
it would be interesting to add a control to make them balanced as a inverted pendulum
and achieve practical results and applicable to engineering.
Some examples are:
• Buildings analysis against balances caused by earthquakes.
• Vibrations controller in platforms for launching rockets.
• A satellite positioning with respect to earth. It can be modelled as a pendulum
in which fixed part is located in the earth, the axis are the earth’s antennas which
communicate with the satellite, and mobile part is the satellite. The satellite should
not move so much because otherwise, it would go out of the rank of communication
with the earth.
• Stabilization of cranes and other robotic arms.
• Humanoid Robots: ability to walk without swinging. Thus, the robot could work
in hazardous environments to humans. Also robots that mimic animal movements,
like [15].
• Educational applications (try several kinds of control techniques).
This thesis has been focused to solve the problem explained in Chapter 2, because the
aim was to implement a control capable to be applied to a Segway, but the work can also
be applied to other systems which can be schematized by same form.
50
CHAPTER 5 Future work
Even if forces distribution in the system are not the same (as in some of the cases cited
above), the equations will slightly vary, but the process to design the control is analogous.
Therefore from this study and making some mathematics modifications, it is possible to
solve several problems similar to inverted pendulum.
5.4 Future work
Once reached the dimensions of a Master Thesis, there are numerous and varied possibil-
ities for further research. Other projects can start from this work. It is also possible to
make it better and do it more efficient. These are some ideas of projects and improvements
which are suggested as future work:
• Add the capacity of changing from one inclined ground to another with different
angle. We suggest two possibilities to carry it out:
– Measure continuously the slope of the the ground and recalculate for each
moment the control of the pendulum. This option can be implemented easily
using SIMULINK.
– Calculate the physic equations of the system respect any point of the space,
instead of suppose that the reference origin is on ground’s surface (as it was
made above).
• When the pole placement control is done, as well as imitate LQR dynamic, also add
a pole in the origin in order to make zero the steady state error (according to the
explanations of Section 3.9).
• Design the PID analytically, not by try and error. In this work we have obtained
very successful results using try and error, in fact, they are very acceptable in a
large rank of combinations of weighs, angles, etcetera. However analytically it could
be possible to program the process and then to make the control totally general to
any value (and for example make it able, not only to work as a Segway, but also to
be used in some of the applications mentioned in last section).
• Add a third dimension in the control. When equations were calculated, it was
supposed that the union between pendulum and cart was a “basket”, therefore it
just allows oscillations in a plane. We could make the model more multipurpose
supposing that the union is a ball-and-socket joint, then the pendulum can oscillate
in the space.
• Use this theoretical study made by computer to build a real system. We would have
take care about the drivers, add sensors, filters to reduce the noise in measurements
and bear in mind the physical limitations in the equipment.
51
CHAPTER 5 Future work
• Add other link in the end of the pendulum and make the control of both links. This
could be a beginning to imitate the performance of a human arm.
52
APPENDIX A
EXPLANATORY CALCULATIONS
A.1 Equations 3.7 and 3.8
Taking equations 3.1, 3.2, 3.3 and 3.4, if 3.1 and 3.2 are replaced in 3.3, and developing,
it is achieved equation 3.7:(ml~θ(t) sin ~θ(t) +ml~θ2(t) cos ~θ(t)−m~g
)sin ~θ(t)
+(−m~x(t) +ml~θ(t) cos ~θ(t)−ml~θ2(t) sin ~θ(t)
)cos ~θ(t) = −I
~θ(t)
l
⇒ ml~θ(t) sin2 ~θ(t) +((((((((
((((
ml~θ2(t) sin ~θ(t) cos ~θ(t)−m~g sin ~θ(t)
−m~x(t) cos ~θ(t) +ml~θ(t) cos2 ~θ(t)−(((((((((
(((
ml~θ2(t) sin ~θ(t) cos ~θ(t) = −I~θ(t)
l
⇒ ~θ(t)
[ml(
sin2 ~θ(t) + cos2 ~θ(t))
+I
l
]−m~g sin ~θ(t) = m~x(t) cos ~θ(t)
⇒ ~θ(t)(I +ml2
)−m~gl sin ~θ(t) = ml~x(t) cos ~θ(t)
If 3.1 is replaced in 3.4, equation 3.8 is obtained:
~F (t)− b~x(t)−m~x(t) +ml~θ(t) cos ~θ(t)−ml~θ2 sin ~θ(t) = M~x(t)
⇒ ~F (t) = (M +m)~x(t) + b~x(t) +ml~θ2(t) sin ~θ(t)−ml~θ(t) cos ~θ(t)
A.2 Equations 3.14 y 3.15
Solving x2(t) from equation 3.12:
x2(t) =
(I
ml+ l
)x4(t)− g x3(t) (A.1)
53
CHAPTER A Representation of a system by its phase variables
If A.1 is substituted in 3.13:
~F (t) = (M +m)
[(I
ml+ l
)x4(t)− g x3(t)
]+ b x2(t)−ml x4(t)
x4(t)
[(M +m)
(I
ml+ l
)−ml
]= ~F (t) + g(M +m)x3(t)− b xx(t)
x4(t) =~F (t)
(M +m)(Iml
+ l)−ml
− b x2(t)
(M +m)(Iml
+ l)−ml
+g(M +m)x3(t)
(M +m)(Iml
+ l)−ml
This value of x4(t) is substituted in equation A.1:
x2(t) =
(Iml + l
)~F (t)
(M +m)(Iml + l
)−ml
−b(Iml + l
)x2(t)
(M +m)(Iml + l
)−ml
−
[g −
g(M +m)(Iml + l
)(M +m)
(Iml + l
)−ml
]x3(t)
Calling:
α =1
(M +m)(Iml
+ l)−ml
=ml
I(M +m) +Mml2
β =
(I
ml+ l
)α =
I +ml2
I(M +m) +Mml2
Equations 3.14 and 3.15 are obtained:
x2(t) = −βb x2(t) +mlgα x3(t) + β ~F (t)
x4(t) = −αb x2(t) + g(M +m)αx3(t) + α ~F (t)
A.3 Representation of a system by its phase variables
Final structure of state model by phase variables is [2]:
x(t) =
0 1 0 · · · 0
0 0 1 · · · 0...
......
. . ....
0 0 0 · · · 1
−a0 −a1 −a2 · · · −an−1
x(t) +
0
0...
0
1
u(t)
y(t) =[b0 b1 · · · bn−2 bn−1
]x(t)
Where bi and ai are coefficients of numerator and denominator respectively of transfer
function of the system.
54
CHAPTER A Representation of a system by observable canonical form
If it is feedback with a matrix K:
Ar = A+BK =
=
0 1 0 · · · 0
0 0 1 · · · 0...
......
. . ....
0 0 0 · · · 1
−a0 −a1 −a2 · · · −an−1
+
0
0...
0
1
[k1 k2 k3 · · · kn
]=
= Ar =
0 1 0 · · · 0
0 0 1 · · · 0...
......
. . ....
0 0 0 · · · 1
k1 − a0 k2 − a1 k3 − a2 · · · kn − an−1
And matrix C does not vary.
A.4 Representation of a system by observable canon-
ical form
The model using this representation has the structure [2]:
x(t) =
0 0 · · · 0 −a0
1 0 · · · 0 −a1
0 1 · · · 0 −a2
......
. . ....
...
0 0 · · · 1 −an−1
x(t) +
b0 − bnaob1 − bna1
...
bn−1 − bnan−1
u(t)
y(t) =[0 0 · · · 0 1
]x(t) + bnu
Doing A−HC it is obtained:
55
CHAPTER A Steady state error to different kind of reference inputs
A−HC =
=
0 0 · · · 0 −a0
1 0 · · · 0 −a1
0 1 · · · 0 −a2
......
. . ....
...
0 0 · · · 1 −an−1
−
h1
h2
h3
...
hn
[0 0 · · · 0 1
]=
= A−HC =
0 0 · · · 0 −(a0 + h1)
1 0 · · · 0 −(a1 + h2)
0 1 · · · 0 −(a2 + h3)...
.... . .
......
0 0 · · · 1 −(an−1 + hn)
A.5 Steady state error to different kind of reference
inputs
Total error will be the superposition of the error caused by reference inputs and error
caused by disturbances. In this section it will be only calculated the effect of reference
inputs.
Model of section 3.9 will be used, and it will be studied the error associated to a step
input and a ramp input.
In order to keep just the effect of reference input, it is taken the first term of equation
3.44. Calling it EV (s):
EV (s) =1
1 +G(s)V (s) (A.2)
It will be used the Final value theorem to calculate the error in permanent regime.
Step
A step is represented as V (s) = 1s.
Error in permanent regime will be:
eV = lims→0
sEV (s) = lims→0
s1
1 +G(s)
1
s=
1
1 + lims→0G(s)=
=1
1 + lims→0Kr
srG(s)
=1
1 + lims→0Kr
sr
(A.3)
Ramp
A ramp source is V (s) = 1s2
.
56
CHAPTER A Steady state error to different kind of disturbance inputs
And steady state error:
eV = lims→0
sEV (s) = lims→0
s1
1 +G(s)
1
s2=
1
s+ lims→0 sG(s)=
=1
lims→0 sG(s)=
1
lims→0 sKr
srG(s)
(A.4)
A.6 Steady state error to different kind of distur-
bance inputs
Continuing with model of section 3.9, it will be studied the steady state error when some
disturbances like step and ramp are acting.
In order to do that, it is used the second term of equation 3.44, that describes distur-
bances effect. Calling it ED(s):
ED(s) = − P (s)
1−G(s)D(s) (A.5)
Step
Therefore the disturbance is represented as D(s) = 1s
And applying the Final value theorem it is possible to calculate the error in permanent
regime:
eD = lims→0
sED(s) = lims→0−s P (s)
1−G(s)
1
s= lim
s→0−
Kw
swP (s)
1− Kr
srG(s)
=
= lims→0−
Kw
sw
1− Kr
sr
= lims→0− Kw
sw
sr(sr −Kr)
= lims→0− Kw
sw−r (sr −Kr)(A.6)
Ramp
Now: D(s) = 1s2
Developing analogously:
eD = lims→0
sED(s) = lims→0−s P (s)
1−G(s)
1
s2=
= lims→0−
Kw
sw(1− Kr
sr
)s
= lims→0− Kw
sw+1−r (sr −Kr)(A.7)
57
APPENDIX B
SOURCE CODES MADE
B.1 Inverted pendulum on level ground
1 %%% INVERTED PENDULUM %%%
2
3 %% SYSTEM PARAMETERS
4 M=0.75; % [kg] Cart mass
5 b=0.075; % [N/m/s] Cart friction
6 l=0.4; % [m] Distance from cart axis to pendulum center of mass
7 m=0.25; % [kg] Pendulum mass
8 I=0.005; % [kg*m^2/s^2] Pendulum inertia
9 g=9.8; % [m/s^2] Gravity acceleration
10
11 %% SYSTEM MATRIX
12
13 % In orther to simplicity , these constants are created:
14 alpha=m*l/(I*(M+m)+M*m*l^2);
15 beta=(I+m*l^2)/(I*(M+m)+M*m*l^2);
16
17 % Matrix:
18 A=[0 1 0 0;
19 0 -b*beta m*l*g*alpha 0;
20 0 0 0 1;
21 0 -b*alpha g*(M+m)*alpha 0];
22 B=[0 beta 0 alpha]’;
23 C=[1 0 0 0;
24 0 0 1 0];
25 D=[0 0]’;
58
CHAPTER B Inverted pendulum on level ground
26
27 %% CONTROLLABILITY
28
29 Q=ctrb(A,B);
30 controlab=rank(Q);
31 if controlab <4
32 input(’Non controlable system ’)
33 end
34
35 %% OBSERVABILITY
36
37 P=obsv(A,C);
38 observb=rank(P);
39 if observb <4
40 input(’Non observable system ’)
41 end
42
43 %% LQR CONTROL
44
45 Qlqr=[ 10000 0 0 0;
46 0 0 0 0;
47 0 0 1000 0;
48 0 0 0 0];
49 Rlqr =1;
50 Klqr=lqr(A,B,Qlqr ,Rlqr);
51 Alqr=A-B*Klqr;
52 Blqr=B;
53 Clqr=C;
54 Dlqr=D;
55
56 T_sim =0:0.1:10;
57 Imputs =0.25* ones(size(T_sim));
58 Output=lsim(Alqr ,Blqr ,Clqr ,Dlqr ,Imputs ,T_sim);
59 plot(T_sim ,Output)
60 legend(’Position (m)’,’Angle (rad)’)
61 xlabel(’t (s)’)
62
63 % Final poles
64 polos_finales=eig(Alqr);
65
66 %% POLES ASIGNATION CONTROL
59
CHAPTER B Inverted pendulum on level ground
67
68 % Transformation matrix to phase variables:
69 Qinv=inv(Q);
70 Tcinv =[Qinv (4,:);Qinv (4,:)*A;Qinv (4,:)*A^2; Qinv (4,:)*A^3];
71 Tc=inv(Tcinv);
72
73 % Transforming:
74 Ac=Tcinv*A*Tc;
75 Bc=Tcinv*B;
76 Cc=C*Tc;
77
78 % Matrix we are looking for (Ar):
79 syms k0 k1 k2 k3 k4 s
80 Ar=[0 1 0 0;
81 0 0 1 0;
82 0 0 0 1;
83 k1+Ac(4,1) k2+Ac(4,2) k3+Ac(4,3) k4+Ac(4,4)];
84
85 % Characteristics polynomials
86 polinomio_polos_finales=poly(polos_finales);
87 pol_Ar=poly(Ar);
88
89 % Comparing:
90 Kc1 = -2800; Kc2 = -1591.6; Kc3 = -350.6; Kc4 = -26.4;
91 Kc=[Kc1 Kc2 Kc3 Kc4];
92
93 % Anti -tranforming:
94 K=Kc*Tcinv;
95
96 % Other options are: place(A,B,polos_finales) or acker(A,B,
polos_finales)
97
98 %% OBSERVER
99
100 % New observability
101 Pnueva =[P(1,:);P(3,:);P(5,:);P(7,:)];
102 if rank(Pnueva)<4
103 input(’It is impossible to make the observer doing this
simplification ’)
104 end
105 Pnueva_inv=inv(Pnueva);
60
CHAPTER B Inverted pendulum on level ground
106 To=[ Pnueva_inv (:,4) A*Pnueva_inv (:,4) A^2* Pnueva_inv (:,4) A^3*
Pnueva_inv (:,4)];
107 Toinv=inv(To);
108
109 % Transforming:
110 Ao=Toinv*A*To;
111 Bo=Toinv*B;
112 Co=C(1,:)*To;
113
114 % Transformation between observer variables and phase variables:
115 Tco=Tcinv*To;
116 Tcoinv=inv(Tco);
117
118 % Observer poles sufficiently remotes:
119 polos_obs =[-50 -51 -52 -53];
120
121 % Observer matrix:
122 polinomio_obs=poly(polos_obs);
123
124 F=[0 0 0 -polinomio_obs (5);
125 1 0 0 -polinomio_obs (4);
126 0 1 0 -polinomio_obs (3);
127 0 0 1 -polinomio_obs (2)];
128 HoCo=Ao -F;
129 for z=1:4
130 H(z,1)=HoCo(z,4);
131 end
132
133 %% REDUCED ORDER OBSERVER
134
135 % Transformation matrix: observer ->reduced order observer
136 Tred_inv =[0 0 0 1; 1 0 0 0; 0 1 0 0; 0 0 1 0];
137 Tred=inv(Tred_inv);
138
139 % Transforming:
140 Ared=Tred_inv*Ao*Tred;
141 Bred=Tred_inv*Bo;
142 Cred=Co*Tred;
143
144 % Direct transformation:
145 T_reducido=To*Tred;
61
CHAPTER B Inverted pendulum on sloping ground
146 T_reducido_inv=inv(T_reducido);
147
148 % Matrix of the reduced order observer:
149 A11=[Ared (1,1)];
150 A12=[Ared (1 ,2:4)];
151 A21=[Ared (2:4 ,1)];
152 A22=[Ared (2:4 ,2:4)];
153 B1=[Bred (1)];
154 B2=[Bred (2:4)];
155
156 % Observer poles sufficiently remotes:
157 polos_obs_r =[-50 -51 -52];
158
159 % Construction:
160 polinomio_obs_r=poly(polos_obs_r);
161 Fr=[0 0 -polinomio_obs_r (4);
162 1 0 -polinomio_obs_r (3);
163 0 1 -polinomio_obs_r (2)];
164 H2m=(A22 -Fr);
165 H2=H2m(:,3);
166 H1=A21 -H2*A11;
167 G=B2-H2*B1;
B.2 Inverted pendulum on sloping ground
1 %%% INVERTED PENDULUM ON INCLINED FLOOR %%%
2
3 %% SYSTEM PARAMETERS
4 M=50; % [kg] Cart mass
5 b=0.075; % [N/m/s] Cart friction
6 l=1.1; % [m] Distance from cart axis to pendulum center of mass
7 m=70; % [kg] Pendulum mass
8 I=0.005; % [kg*m^2/s^2] Pendulum inertia
9 g=9.8; % [m/s^2] Gravity acceleration
10 gamma =10; % [o] Angle of floor ’s inclination
11 gamma=gamma*pi/180;
12
13 %% SYSTEM MATRIX
14
62
CHAPTER B Inverted pendulum on sloping ground
15 % In orther to simplicity , these constants are created:
16 alpha=I/(m*l)+l;
17 beta=m*l-(M+m)*alpha;
18
19 % Matrix coefficients:
20 a1=alpha*b*cos(gamma)/beta;
21 a2=-m*g*l/beta;
22 a3=-alpha*cos(gamma)/beta;
23 a4=(M+m)*b*cos(gamma)*alpha/(m*l*beta)+b*cos(gamma)/(m*l);
24 a5=-(M+m)*g/beta;
25 a6=-(M+m)*cos(gamma)*alpha/(m*l*beta)-cos(gamma)/(m*l);
26
27 % Matrix:
28 % States: x1=pos_x; x2=vel_x; x3=theta; x4=vel_theta
29 A=[0 1 0 0;
30 0 a1 a2 0;
31 0 0 0 1;
32 0 a4 a5 0];
33 B=[0 a3 0 a6]’;
34 C=[1 0 0 0;
35 0 0 1 0];
36 D=[0 0]’;
37
38 %% CONTROLLABILITY
39
40 Q=ctrb(A,B);
41 controlab=rank(Q);
42 if controlab <4
43 input(’Non controlable system ’)
44 end
45
46 %% OBSERVABILITY
47
48 P=obsv(A,C);
49 observb=rank(P);
50 if observb <4
51 input(’Non observable system ’)
52 end
53
54 %% LQR CONTROL
55
63
CHAPTER B Inverted pendulum on sloping ground
56 Qlqr =[1000000 0 0 0;
57 0 0 0 0;
58 0 0 100 0;
59 0 0 0 0];
60 Rlqr =1;
61 Klqr=lqr(A,B,Qlqr ,Rlqr);
62 Alqr=A-B*Klqr;
63 Blqr=B;
64 Clqr=C;
65 Dlqr=D;
66
67 T_sim =0:0.1:10;
68 Imputs =1* ones(size(T_sim));
69 Output_lqr=lsim(Alqr ,Blqr ,Clqr ,Dlqr ,Imputs ,T_sim);
70 plot(T_sim ,Output_lqr)
71 legend(’Cart (m)’,’Pendulum (rad)’)
72 xlabel(’time (s)’)
73
74 % Final poles
75 polos_finales=eig(Alqr);
76
77 %% OBSERVER
78
79 % New observability:
80 Pnueva =[P(1,:);P(3,:);P(5,:);P(7,:)];
81 if rank(Pnueva)<4
82 input(’It is impossible to make the observer doing this
simplification ’)
83 end
84 Pnueva_inv=inv(Pnueva);
85 To=[ Pnueva_inv (:,4) A*Pnueva_inv (:,4) A^2* Pnueva_inv (:,4) A^3*
Pnueva_inv (:,4)];
86 Toinv=inv(To);
87
88 % Transforming:
89 Ao=Toinv*A*To;
90 Bo=Toinv*B;
91 Co=C(1,:)*To;
92
93 % Observer poles sufficiently remotes:
94 polos_obs =[-150 -151 -152 -153];
64
CHAPTER B Inverted pendulum on sloping ground
95 polinomio_obs=poly(polos_obs);
96
97 % Observer matrix:
98 F=[0 0 0 -polinomio_obs (5);
99 1 0 0 -polinomio_obs (4);
100 0 1 0 -polinomio_obs (3);
101 0 0 1 -polinomio_obs (2)];
102 HoCo=Ao -F;
103 for z=1:4
104 H(z,1)=HoCo(z,4);
105 end
106
107 %% REDUCED ORDER OBSERVER
108
109 % Transformation matrix: observer ->reduced order observer
110 Tred_inv =[0 0 0 1; 1 0 0 0; 0 1 0 0; 0 0 1 0];
111 Tred=inv(Tred_inv);
112
113 % Transforming:
114 Ared=Tred_inv*Ao*Tred;
115 Bred=Tred_inv*Bo;
116 Cred=Co*Tred;
117
118 % Direct transform:
119 T_reducido=To*Tred;
120 T_reducido_inv=inv(T_reducido);
121
122 % Matrix of the Reduced Order Observer:
123 A11=[Ared (1,1)];
124 A12=[Ared (1 ,2:4)];
125 A21=[Ared (2:4 ,1)];
126 A22=[Ared (2:4 ,2:4)];
127 B1=[Bred (1)];
128 B2=[Bred (2:4)];
129
130 % Observer poles sufficiently remotes:
131 polos_obs_r =[ -150 -151 -152];
132
133 % Construction:
134 polinomio_obs_r=poly(polos_obs_r);
135 Fr=[0 0 -polinomio_obs_r (4);
65
CHAPTER B Animation
136 1 0 -polinomio_obs_r (3);
137 0 1 -polinomio_obs_r (2)];
138 H2m=(A22 -Fr);
139 H2=H2m(:,3);
140 H1=A21 -H2*A11;
141 G=B2-H2*B1;
B.3 Animation
1 %% INVERTED PENDULUM ANIMATION
2
3 % gamma source must come in degrees (floor inclination)
4
5 function []= animacion_pendulo_inclinado(posicion ,angulo ,tiempo ,
gamma)
6
7 l=0.4; % Distance from cart axis to pendulum center of mass
8 gt =0.1; % Pendulum ’s thickness
9 lc =0.65; % Cart ’s length
10 gc =0.06; % Cart ’s thickness
11 gl =0.016; % Thinkness of the line that support the cart
12
13 lt=2*l;
14
15 gamma=gamma*pi/180;
16
17 tiemp=tiempo;
18
19 sal=[ posicion angulo ];
20
21 xmin=min(posicion);
22 xmax=max(posicion);
23 xmi=xmin -0.5;
24 xma=xmax +0.5;
25
26 if gamma <-pi/2 | gamma >pi/2
27 input(’gamma must be between -90 and 90 degrees ’)
28 end
29
66
CHAPTER B Animation
30 if gamma <0
31 y_max=abs(xmax)*abs(tan(gamma))+gc/(2* cos(gamma));
32 y_min=-abs(xmin)*abs(tan(gamma)-gc/(2* cos(gamma)));
33 yma=y_max+lt +0.2;
34 ymi=y_min -0.3;
35
36 figure;
37 iteraciones =0;
38 for w=1: size(tiemp ,1)
39 y=gc/(2* cos(gamma))-sal(w,1)*tan(gamma);
40 % Axis
41 axis([xmi xma ymi yma]);
42 % R ans S points
43 R=[xmax +0.4 -(xmax +0.4)*tan(gamma)];
44 S=[xmin -0.4 -(xmin -0.4)*tan(gamma)];
45 % Rail:
46 patch ([S(1) S(1) R(1) R(1)],[S(2)-gl*1.35 S(2)-gl*0.35 R
(2)-gl *0.35 R(2)-gl*1.35] ,[7 8 8 7])
47 % -1.35 and -0.35 are used to move the cart but
respecting its thinkness gl
48 % P point
49 P=[sal(w,1)-gc*sin(gamma),y-gc*cos(gamma)];
50 % Q point
51 Q=[sal(w,1)-lt*sin(sal(w,2)),y+lt*cos(sal(w,2))];
52 % Cart
53 patch ([P(1)-lc/2*cos(gamma) sal(w,1)-lc/2*cos(gamma) sal(
w,1)+lc/2* cos(gamma) P(1)+lc/2* cos(gamma)],[P(2)+lc/2*
sin(gamma) y+lc/2*sin(gamma) y-lc/2*sin(gamma) P(2)-lc
/2* sin(gamma)],[1 2 1 2])
54 % Pendulum
55 patch ([sal(w,1)-gt/2*cos(sal(w,2)) Q(1)-gt/2*cos(sal(w,2)
) Q(1)+gt/2*cos(sal(w,2)) sal(w,1)+gt/2*cos(sal(w,2))
],[y-gt/2* sin(sal(w,2)) Q(2)-gt/2* sin(sal(w,2)) Q(2)+gt
/2* sin(sal(w,2)) y+gt/2*sin(sal(w,2))],[4 3 3 4])
56 hold on
57 pause (0.03);
58
59 iteraciones=iteraciones +1;
60 if w<size(tiemp ,1)
61 clf % Last picture is frozen , the rest are deleted in
order to continue drawing.
67
CHAPTER B Animation
62 else
63 break
64 end
65 end
66
67 else
68 y_min=-abs(xmax)*abs(tan(gamma))-gc*sin(gamma)/2;
69 y_max=abs(xmin)*abs(tan(gamma))+gc*sin(gamma)/2;
70 yma=y_max+lt +0.2;
71 ymi=y_min -0.3;
72
73 figure;
74 iteraciones =0;
75 for w=1: size(tiemp ,1)
76 y=gc/(2* cos(gamma))-sal(w,1)*tan(gamma);
77 % Axis
78 axis([xmi xma ymi yma]);
79 % Rail:
80 patch ([xmin -0.4 xmin -0.4 xmax +0.4 xmax +0.4] ,[ abs(xmin
-0.4)*abs(tan(gamma))-gl*1.35 abs(xmin -0.4)*abs(tan(
gamma))-gl*0.35 -abs(xmax +0.4)*abs(tan(gamma))-gl*0.35
-abs(xmax +0.4)*abs(tan(gamma))-gl*1.35] ,[7 8 8 7])
81 % -1.35 and -0.35 are used to move the cart but
respecting its thinkness gl
82 % P point:
83 P=[sal(w,1)-gc*sin(gamma),y-gc*cos(gamma)];
84 % Q point:
85 Q=[sal(w,1)-lt*sin(sal(w,2)),y+lt*cos(sal(w,2))];
86 % Cart:
87 patch ([P(1)-lc/2*cos(gamma) sal(w,1)-lc/2*cos(gamma) sal(
w,1)+lc/2* cos(gamma) P(1)+lc/2* cos(gamma)],[P(2)+lc/2*
sin(gamma) y+lc/2*sin(gamma) y-lc/2*sin(gamma) P(2)-lc
/2* sin(gamma)],[1 2 1 2])
88 % Pendulum:
89 patch ([sal(w,1)-gt/2*cos(sal(w,2)) Q(1)-gt/2*cos(sal(w,2)
) Q(1)+gt/2*cos(sal(w,2)) sal(w,1)+gt/2*cos(sal(w,2))
],[y-gt/2* sin(sal(w,2)) Q(2)-gt/2* sin(sal(w,2)) Q(2)+gt
/2* sin(sal(w,2)) y+gt/2*sin(sal(w,2))],[4 3 3 4])
90 hold on
91 pause (0.03);
92
68
CHAPTER B Animation
93 iteraciones=iteraciones +1;
94 if w<size(tiemp ,1)
95 clf % Last picture is frozen , the rest are deleted in
order to continue drawing.
96 else
97 break
98 end
99 end
100 end
69
Bibliography
[1] R. H. Bishop. Modern control systems analysis and design using MATLAB. Addison
Wesley, 1993.
[2] S. Domınguez, P. Campoy, J. M. Sebastian, A. Jimenez. Control en el Espacio de
Estado. Prentice Hall, 2002.
[3] I. Fantoni, R. Lozano. Nonlinear Control for the Underactuated Mechanical Systems.
Springer-Verlag, 2002.
[4] A. L. Fradkov. Swinging Control of Nonlinear oscillations, 1996.
[5] K. Furuta, H. Nishihara, S. Mori. Control of Unstable Mechanical Systems: Control
of Pendulum. International Journal of Control, vol. 23, 1976.
[6] S. Garcıa Nieto, M. Martınez, A. Llosa, J. Sanchis. Estrategias de Ensenanza a Dis-
tancia sobre Control No-Lineal aplicada al Pendulo Invertido. Informacion Tecnologica,
2007.
[7] D. Giraldo, E. Giraldo. Quadratic Linear Control Application using a Minimum Order
Observer in a Crane. Scientia et Technica year XIII, 2007.
[8] F. Grasser, A. D’Arrigo, S. Colombi and A. Rufer. Joe: A Mobile, Inverted Pendulum.
Swiss Federal Institute of Technology, 2002.
[9] O. O. Gutierrez Frıas. Diseno de Controladores para Sistemas Subactuados del Tipo
Pendulo Invertido. Instituto Politecnico Nacional, 2009.
[10] F. Mazenc, L. Praly. Adding Integrations, Saturated Controls, and Stabilization for
feedforward systems. IEEE Transactions on Automatic Control, 1996.
[11] J. Moore, B. Anderson. Optimal Control: Linear quadratic methods. Prentice Hall,
1989.
[12] K. Ogata. Ingenierıa de Control Moderna. Prentice Hall, 2003.
70
CHAPTER B
[13] . C. Ooi. Balancing a Two-Wheeled Autonomous Robot. The University of Western
Australia, 2003.
[14] E. A. Puente. Regulacion Automatica I. Universidad Politecnica de Madrid, Seccion
Publicaciones, 1997.
[15] M. H. Raibert. Legged Robots. Communications of the ACM, 1986.
[16] Q. Wei, W. P. Dayawansa, W. S. Levine. Nonlinear Controller for an Inverted Pen-
dulum having Restricted Travel. Automatica, 1995.
71