PLANAR ROBOT ARM MODELLING AND CONTROL Francisco …537063/FULLTEXT01.pdf · PLANAR ROBOT ARM...

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


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


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


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


• θ 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


• 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


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


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


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


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


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


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


(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


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


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.


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


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


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


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


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


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


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


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


(a) Step source, ramp disturbance (b) Zoom of e


• 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


V (s) step and D(s) step

By proceeding analogously:

• Error caused by reference input: eV = 0

• Error caused by disturbances: eD = 0

36


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


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


–∑ ~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


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


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


(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


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


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


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


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


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


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


94 polos_obs =[-150 -151 -152 -153];

64


95 polinomio_obs=poly(polos_obs);

96

97 % Observer matrix:

98 F=[0 0 0 -polinomio_obs (5);



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


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

Date post:	01-Sep-2020
Category:	Documents
Upload:	others
View:	3 times
Download:	0 times

PLANAR ROBOT ARM MODELLING AND CONTROL Francisco …537063/FULLTEXT01.pdf · PLANAR ROBOT ARM...

Documents