¨ MALARDALENS UNIVERSITY School of Innovation, Design and Engineering V¨aster˚ as

MASTER THESIS CEL505

PLANAR ROBOT ARM MODELLING AND CONTROL

Francisco Hern´andez Gonz´alez Supervisor: Giacomo Spampinato June 2012

Acknowledgements

This work is the fruit of a long academic background. Therefore many people have intervened 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 V¨aste˚ as for all their help.

I

Abstract

The thesis objective is to model a one link robotic arm mounted on a sliding mobile platform 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 situations 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 DEFINITION OF THE PROBLEM

1

1.1

Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

1

1.2

Generic outline of inverted pendulum . . . . . . . . . . . . . . . . . . . . .

1

2 INVERTED PENDULUM ON LEVEL GROUND

4

2.1

Mathematical model of the system . . . . . . . . . . . . . . . . . . . . . .

4

2.2

State space representation . . . . . . . . . . . . . . . . . . . . . . . . . . .

7

2.3

Controllability and observability . . . . . . . . . . . . . . . . . . . . . . . .

8

2.4

LQR control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

9

2.5

Pole placement control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.6

2.7

2.5.1

Phase variables transformation . . . . . . . . . . . . . . . . . . . . 13

2.5.2

Calculation of K . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

2.5.3

SIMULINK simulation . . . . . . . . . . . . . . . . . . . . . . . . . 15

Design of an observer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18 2.6.1

Observable canonical form transformation . . . . . . . . . . . . . . 18

2.6.2

Calculation of parameters characterizing the observer . . . . . . . . 19

2.6.3

SIMULINK simulation . . . . . . . . . . . . . . . . . . . . . . . . . 20

Design of a reduced order observer . . . . . . . . . . . . . . . . . . . . . . 21 2.7.1

Model transformation . . . . . . . . . . . . . . . . . . . . . . . . . . 22

2.7.2

Calculation of characteristic parameters of the reduced order observer 23

2.7.3

SIMULINK simulation . . . . . . . . . . . . . . . . . . . . . . . . . 23

2.8

Track a reference input for the position . . . . . . . . . . . . . . . . . . . . 25

2.9

Study of the steady state error . . . . . . . . . . . . . . . . . . . . . . . . . 28

3 INVERTED PENDULUM ON SLOPING GROUND

32

3.1

Mathematical model of the system . . . . . . . . . . . . . . . . . . . . . . 32

3.2

State space representation . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

3.3

LQR control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

3.4

Observers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

3.5

Track a reference input for the position . . . . . . . . . . . . . . . . . . . . 38

3.6

Animation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

4 FINDINGS

43

4.1

Outcomes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

4.2

Methodology of work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

4.3

Other applications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

4.4

Future work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

A EXPLANATORY CALCULATIONS

46

A.1 Equations 2.7 and 2.8 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 A.2 Equations 2.14 y 2.15 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 A.3 Representation of a system by its phase variables . . . . . . . . . . . . . . 47 A.4 Representation of a system by observable canonical form . . . . . . . . . . 48 A.5 Steady state error to different kind of reference inputs . . . . . . . . . . . . 49 A.6 Steady state error to different kind of disturbance inputs . . . . . . . . . . 50 B SOURCE CODES MADE

51

B.1 Inverted pendulum on level ground . . . . . . . . . . . . . . . . . . . . . . 51 B.2 Inverted pendulum on sloping ground . . . . . . . . . . . . . . . . . . . . . 55 B.3 Animation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59 Bibliography.

63

IV

List of Figures

1.1

Inverted pendulum on sloping ground . . . . . . . . . . . . . . . . . . . . .

2

2.1

Inverted pendulum on flat ground . . . . . . . . . . . . . . . . . . . . . . .

5

2.2

System division into its component parts . . . . . . . . . . . . . . . . . . .

5

2.3

System’s response to different values of q1

. . . . . . . . . . . . . . . . . . 10

2.4

System’s response to different values of q3

. . . . . . . . . . . . . . . . . . 11

2.5

Respuesta final utilizando un control LQR . . . . . . . . . . . . . . . . . . 12

2.6

SIMULINK diagrams of inverted pendulum on level ground. . . . . . . . . 15

2.7

Pendulum response without control. . . . . . . . . . . . . . . . . . . . . . . 16

2.8

0.25 N step input . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 π rad θ¨0 = 0 rad . . . . . 17 Initial conditions: x0 = 0.5 m, x¨0 = 1 ms , θ0 = 20 180 s

2.9

2.10 Initial conditions + Step + Impulse . . . . . . . . . . . . . . . . . . . . . . 17 2.11 Observer diagram in SIMULINK . . . . . . . . . . . . . . . . . . . . . . . 20 2.12 Estimation errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21 2.13 Diagram of reduced order observer in SIMULINK . . . . . . . . . . . . . . 24 2.14 Estimate errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24 2.15 System for controlling the position . . . . . . . . . . . . . . . . . . . . . . 25 2.16 Ki adjust . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 2.17 Kr adjust . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27 2.18 Kd adjust . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27 2.19 Blocks diagram . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 2.20 Estimate errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30 2.21 Estimate errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30 2.22 Step source, step disturbance . . . . . . . . . . . . . . . . . . . . . . . . . 31 3.1

Inverted pendulum on sloping ground . . . . . . . . . . . . . . . . . . . . . 33

3.2

System division into its two parts . . . . . . . . . . . . . . . . . . . . . . . 33

3.3

Respond when m and l vary . . . . . . . . . . . . . . . . . . . . . . . . . . 38

3.4

SIMULINK diagram for the total control of the pendulum on sloping ground 39

3.5

Respond when it is acting a 2000 N step in t = 1 s and an impulse in t = 6 s 39

3.6

Variables used to fix the screen of the process . . . . . . . . . . . . . . . . 40

3.7

Variables used in animation programming . . . . . . . . . . . . . . . . . . 41

3.8

Inverted pendulum animation . . . . . . . . . . . . . . . . . . . . . . . . . 42

VI

CHAPTER

1

DEFINITION OF THE PROBLEM

1.1

Introduction

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 numerous 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, as well as its practical applications. From 70s, several projects about inverted pendulums have been made. A renowned researcher in this area is Professor Futura (look at [3]) of Tokyo Institute of Technology. Profesor Grasser of Swiss Federal Institute of Technology has also made relevant contributions in the research of inverted pendulums with two wheels on which are placed weights simulating the weight of a human [5]. This is precisely the case we are going to try to imitate in this thesis. Not being a new problem, we will study it as original as possible and dealing with each part in depth, as well as study the capacity of movement on sloping ground. The aim will be to get as close as possible to the control system of a Segway.

1.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 1.1. Where: 1

CHAPTER 1

Generic outline of inverted pendulum

Figure 1.1: Inverted pendulum on sloping ground • F~f ric 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 1.1 it is represented by a rectangle. This simplification is carried out 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: – F~disturbance . 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. – F~ref erence . 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 F~ref erence . In the case of a Segway, the person that 2

CHAPTER 1

Generic outline of inverted pendulum

controls it sends a signal from the handlebar ordering where he wants to go to. This signal is transformed into F~ref erence and influences on the driving system of the cart (the motor), acting on the basis of the pendulum as it was assumed. – F~control . The goal is to stabilize the pendulum and take it to the desired position. To achieve this, we have to change F~control in a proper way. Hence, F~cotrol 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. F~cotrol is generated by the motor obeying control commands, so it acts again on the basis of the pendulum and in the same direction than F~disturbance and F~ref erence . 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 F~control 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). • θ 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. ~ Y~ has been chosen as reference system, and its origin is located in some point on X, the ground surface.

3

CHAPTER

2

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.

2.1

Mathematical model of the system

Similarly as we described in Section 1.2, it is possible to outline an inverted pendulum on flat ground as Figure 2.1 shows. In this Section 1.2 we explain all the parameters involved in this figure. In order to know the mathematical equations governing the system, the inverted pendulum is divided in two components: pendulum and cart, both assumed as rigid bodies. This is illustrated in Figure 2.2. You can see that as a result of having a “basket” that ~x prevents movement but allows rotation of the pendulum, two reaction forces appear: R ~ y. and R ~ and Y~ , and for angular Applying Newton’s Second Law for both spatial dimensions X 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 F~f ric (t) = b~x(t); I is inertia momentum 1 , l is the length to pendulum center 1

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

4

CHAPTER 2

Mathematical model of the system

Figure 2.1: Inverted pendulum on flat ground

(a) Pendulum

(b) Cart

Figure 2.2: System division into its component parts of mass, and gc is the height of the cart. • Pendulum: P~ – Fx (t) = m~x¨(t)  d2~xp (t) d2  ~ = m ~ x (t) − l sin θ(t) ⇒ dt2 dt2 ~¨ cos θ(t) ~ − mlθ~˙2 (t) sin θ(t) ~ ~ x (t) = −m~x¨(t) + mlθ(t) R ~ x (t) = m −R

–

(2.1)

P~ Fy (t) = m~y¨(t)  d2 ~yp (t) d2  ~ = m g + l cos θ(t) ⇒ c dt2 dt2 ~¨ sin θ(t) ~ ~ + mlθ~˙2 (t) cos θ(t) ~ y (t) + m~g = mlθ(t) R

~ y (t) − m~g = m −R

–

(2.2)

P~ ~¨ T (t) = I θ(t). Siendo T~ (t) los momentos. ~¨ ~ −R ~ = I θ(t) ~ y (t)l sin θ(t) ~ x (t)l cos θ(t) −R

(2.3) 5

CHAPTER 2

Mathematical model of the system

• Cart: –

P~ Fx (t) = M ~x¨(t) 2 2 ~ x (t) = M d ~xc (t) = M d ~x(t) ⇒ F~ (t) − F~f ric (t) − R dt2 dt2 ~ x (t) = M ~x¨(t) F~ (t) − b~x(t) ˙ +R

–

–

(2.4)

P~ Fy (t) = M ~y¨(t)

P~ ~¨ T (t) = I θ(t)

2 ~ y (t) − M~g = M d ~yc (t) = 0 R dt2

(2.5)

gc ~ x (t) gc = I θ~¨c (t) = 0 − F~f ric (t) − R 2 2

(2.6)

~¨ cos θ(t) ~¨ sin θ(t) ~ of 2.1 equation and term mlθ(t) ~ It can be notice that both the term mlθ(t) of 2.2 equation, come from the tangential acceleration acting on the center of mass of the ~ of 2.1 equation and term mlθ~˙2 (t) cos θ(t) ~ of 2.2 pendulum. Likewise, term mlθ~˙2 (t) sin θ(t) 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 ~¨ × ~r(t) tangential acceleration inertia forces in the same system. Being θ(t)  are also acting  ~˙ × θ(t) ~˙ × ~r(t) centripetal acceleration, where ~r(t) is the vector from center of and θ(t) rotation to pendulum centre of mass. In this case, the modulus of ~r(t) will be l. So knowing that ~a(t)T otal = ~a(t) + ~ainertia (t) one can reconstruct forces sketch and achieve the same results. Substituting 2.1 and 2.2 in 2.3, and developing, equation 2.7 is obtained. In Appendix A (Explanatory Calculations) all intermediate steps are detailed.
~¨ ~ = ml~x¨(t) cos θ(t) ~ θ(t) I + ml2 − mgl sin θ(t)

(2.7)

Substituting 2.1 in 2.4 it is obtained: ~¨ cos θ(t) ~ − mlθ(t) ~ F~ (t) = (M + m)~x¨(t) + b~x(t) ˙ + mlθ~˙2 (t) sin θ(t)

(2.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. ~ ≈ θ(t), ~ ~ ≈ 0, and θ~˙2 (t) ≈ 0 because it is the Imposing this assumption, sin θ(t) cos θ(t) square of a magnitude ≈ 0. Applying this transformation to the equations 2.7 and 2.8, we obtain:
~¨ ~ = ml~x¨(t) I + ml2 − mglθ(t) θ(t) ~¨ F~ (t) = (M + m)~x¨(t) + b~x(t) ˙ − mlθ(t)

(2.9) (2.10)

6

CHAPTER 2

2.2

State space representation

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)

x˙ 1 (t) = x2 (t)

x˙ 3 (t) = x4 (t)

~¨ x4 (t) = θ(t)

Then, it is true that: (2.11)

Rewriting equations 2.9 and 2.10 we obtain:
x˙ 4 (t) I + ml2 − mgl x3 (t) = ml x˙ 2 (t)

(2.12)

F~ (t) = (M + m) x˙ 2 (t) + b x2 (t) − ml x˙ 4 (t)

(2.13)

In A Appendix, it is showed how manipulating conveniently 2.12 and 2.13, x˙ 2 (t) and x˙ 4 (t) can be solved: x˙ 2 (t) = −βb x2 (t) + mlgα x3 (t) + β F~ (t)

(2.14)

x˙ 4 (t) = −αb x2 (t) + g(M + m)α x3 (t) + α F~ (t)

(2.15)

Where: α=

ml I(M + m) + M ml2

β=

I + ml2 I(M + m) + M ml2

Equations 2.14, 2.15 and 2.11 can be written in matrix form:        x˙ 1 (t) 0 1 0 0 x1 (t) 0        x˙ 2 (t) 0 −βb     mlgα 0     x2 (t) + β  F~ (t) x˙ (t) = 0 0     0 1 x3 (t)  3    0 x˙ 4 (t) 0 −αb g(M + m)α 0 x4 (t) α

(2.16)

F~ (t) is system input, and bring together forces described in Section 1.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 2.17. This equation 2.17, with 2.16, form the state model of the inverted pendulum on level ground.   x1 (t) " #  " #  1 0 0 0  x (t)  2  + 0 F~ (t) ~y (t) = (2.17)  0 0 1 0  0 x3 (t) x4 (t) 7

CHAPTER 2

Controllability and observability

Thus, characteristics matrix of  0 1 0   mlgα ~ = 0 −βb A 0 0 0  0 −αb g(M + m)α

the model are:    0 0      0 ~ = β   B 0 1    0 α

" ~ = C

# 1 0 0 0 0 0 1 0

~ = D

" # 0 0

(2.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 2.18 are constants. Furthermore the system is lineal, 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.

2.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: h i Q = B|AB|A2 B| · · · |An−1 B has maximum range, that is, n. [2]. Analyzing matrix dimensions 2.18: n = 4. We have chosen standard values for constants: M = 0.75 kg, m = 0.25 kg, b = 2

and g = 9.8 sm2 . These values can be changed 0.075 N ms, l = 0.4 m, I = 0.005 kgsm 2 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.

8

CHAPTER 2

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 :





C

   CA     CA2  P =   .   ..    n−1 CA 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.

2.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: Z ∞
J(u) = y T Qy + uT Ru dt s.a. x˙ = A x + B F 0

Where J(u) is the energy of the system, y T Qy

2

is the energy contributed by each state

T

and u Ru is energy associated with control action. This function is minimized with a control law like u(t) = −Klqr y(t) [6]. 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 2

Do not confuse this Q with controllability matrix Q, different.

9

CHAPTER 2

LQR control

the desired response. Varying these matrices we are changing their relative importance in cost function. This selection depends on system behaviour [4]. In this case we will follow matrix 2.19 model.

Where qi =

1 , ∆xi max

  q1 0 0 0    0 q2 0 0   Q= 0 0 q 0 3   0 0 0 q4

(2.19)

being ∆xi max the maximum deviation of xi variable from its working

point [4]. 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 ∆xi max of states position (x1 ) and angle (x3 ) should be minimum and therefore their elements qi of 2.19 matrix have a high value. On the other states we do not impose restrictions on maximum variations (or equivalently: high maximum variations ⇒ ∆xi max ≈ 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 2.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 2.3 results 10

CHAPTER 2

LQR control

are obtained. Figure 2.3 (a) is the change experienced by the cart’s position to different values of q1 , and Figure 2.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 2.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 2.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 2.5. It 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 (2.20)

11

CHAPTER 2

Pole placement control

Figure 2.5: Respuesta final utilizando un control LQR

2.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 2.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 2.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 mathematical 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 12

CHAPTER 2

Pole placement control

with output, so choosing correctly K we could change Ar and achieve a feedback system with the desire dynamic. As noted above, this new control will place poles in the same place than LQR. Or equivalently, eigenvalues of matrix Ar must coincide with poles 2.20. The process is then developed to calculate Ar , so that it has these eigenvalues, and solve K to feedback the system.

2.5.1

Phase variables transformation

In order to resolve the ensuing system of equations and find K, it is convenient to transform 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 br structure in order to make the explanation livelier. Note that here A br is we only show A also represented by its phase variables (so it is written with a hat), because in this case b B b and K. b it has been built using A,   0 1 0 ··· 0    0  0 1 ··· 0    ..  .. .. .. . b . Ar =  . (2.21)  . . . .    0  0 0 ··· 1   b b b b k1 − b a0 k2 − b a1 k3 − b a2 · · · kn − b an−1 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  T e2   =  ..  . eTn

2. Construction of transformation matrix inverse:   eTn  T   en A  −1  TC =   ..  .   T n−1 en A 3. Transformed model: b = T −1 ATC A C

b = T −1 B B C

b = CTC C 13

CHAPTER 2

Pole placement control

b and B b are obtained: Program this process with MATLAB matrix A     0 1 0 0 0        0 1 0  b= 0 b = 0  A B 0  0 0 0 1      0.00 2.10 28.00 −0.09 1

(2.22)

It is check that matrix appear represented by their phase model, because the have same configuration.

2.5.2

Calculation of K

b of 2.22, it is possible to build A br keeping it as a function of Comparing 2.21 with A b components. As we are working with phase variables, K b also will be represented in K this way and then we will have to anti-transform it in order to restore to its original representation. 

0

1

0

0



  0  0 1 0 b   Ar =   0 0 1 0  b b b b k1 k2 + 2.10 k3 + 28 k4 − 0.09

(2.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 cr , because base transformation does not affect to be 2.20 poles. The same happens with A either its eigenvalues or its characteristic polynomial. br . We just have to Then, we can already make up two equivalent expressions of A b b match them and clear the components of K: k1 , b k2 , b k3 and b k4 . A more direct process is firstly calculate characteristic polynomial of 2.23, which will be function of b ki (2.24 equation). s4 + (0.09 − b k4 )s3 + (−28 − b k3 )s2 + (−2.10 − b k2 )s − b k1 = 0

(2.24)

br matrix. By definition, the roots of this 2.24 polynomial, will be the eigenvalues of A Moreover a polynomial in built using poles of 2.20 (2.25 equation). s4 + 26.5 s3 + 322.6 s2 + 1589.5 s + 2800

(2.25)

We just compare both polynomials 2.24 and 2.25 term by term, and clear values of b ki . h i h i b= b K (2.26) k1 b k2 b k3 b k4 = 2800 −1591.6 −350.6 −26.4 Last step is anti-transform in order to represent K in the original base of the system: h i −1 b K = KTC = 100 56.84 −167.70 −34.81 (2.27) 14

CHAPTER 2

Pole placement control

We can notice it is the same result than the other achieved with LQR control. MATLAB allows calculate directly K matrix using commands place(A,B,polos) or acker(A,B,polos) (with opposite sign, because the suppose negative feedback). Here we have shown the entire process because of theoretical interest, but we will make shorter the way using these commands in later problems.

2.5.3

SIMULINK simulation

Figure 2.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 2.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 2.7 Figure, where the system has been excited with a step input. If controlled system (Figure 2.6, (b)) is excited in t = 1 s with a 0.25 N step input, and starting with zero initial conditions, Figure 2.8 is obtained. Figure 2.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 t1 s the step is acting as a disturbance, but quickly the control start to work against the disturbance. Second part of Figure 2.8 shows this. Here it is showed the input and 3

And this error is not of concern because it is of order 10−3 .

15

CHAPTER 2

Pole placement control

Figure 2.7: Pendulum response without control.

(a) States

(b) Input and control

Figure 2.8: 0.25 N step input control action response. We can check that when initial unbalance is stabilized, control action keeps in −0.25 N in order to compensate the source. If the system starts with non-zero initial conditions (x0 = 0.5 m, x˙ 0 = 1 ms , θ0 = π 20 180 rad θ˙0 = 0 rad ) and without external disturbance, it reacts like Figure 2.9 shows. s These initial conditions have been configured setting the integrator block in SIMULINK’s diagram. Figure 2.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 = 0 m, not around initial position. This response is the desired one because the aim is to assign x = 0 m 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

16

CHAPTER 2

Pole placement control

(a) Output

(b) States

π rad θ¨0 = 0 rad Figure 2.9: Initial conditions: x0 = 0.5 m, x¨0 = 1 ms , θ0 = 20 180 s

continues falling just for a moment (θ raises) until it reacts. When response time pasts, it starts to stabilize. Figure 2.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 2.10: Initial conditions + Step + Impulse Figure 2.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 (5 N ). It is remembered from Section 1.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 17

CHAPTER 2

Design of an observer

acting in t = 6 s with value amplitud N , and second acting in t = 6 + duracion with value −amplitud N , where duracion = 10−5 s and amplitud =

1 duracion

N.

Figure 2.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 appear 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.

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

2.6.1

Observable canonical form transformation

As in K calculation, is worth to make the calculates of the observer design firstly transforming the system to its observable canonical form (Appendix A). The process is: 1. Inverse of observability matrix P h −1 P = e1 e2 · · ·

en

i

2. Construction of transformation matrix inverse: h i To = en Aen · · · An−1 en 3. Transformed model: b = T −1 ATo A o

b = T −1 B B o

b = CTo C

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: h i Cnew = 1 0 0 0

(2.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. 18

CHAPTER 2

Design of an observer

b and B b matrix represented in observed canonical form. Figure 2.29 shows A     0 0 0 0 −28     1 0 0 2.10   0  b b     A= B=   0 1 0 28 1.28     0 0 1 −0.09 0

(2.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 = TC−1 TO

2.6.2

(2.30)

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: x˙ e (t) = F xe (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 2.20, we choose these polos for the observer: p1 = −50

p2 = −51

p3 = −52

p4 = −53

(2.31)

With these poles characteristic polynomial is built: n X

fi si = s4 + 206s3 + 15911s2 + 546106s + 7027800 = 0

(2.32)

i=0

Using the coefficients of this polynomial, it is possible to construct matrix F according to expression 2.33. 

0 0 0 −f0



  1 0 0 −f1   F = 0 1 0 −f  2  0 0 1 −f3

(2.33)

19

CHAPTER 2

Design of an observer

Taking advantage of all matrices are expressed in observable canonical form, it is easy to know H clearing from expression 2.34. b − HC b F =A

(2.34)

b Last equation of the observer design dictates that G = B.

2.6.3

SIMULINK simulation

Observer is added to control diagram according to Figure 2.11.

Figure 2.11: Observer diagram in SIMULINK It achieves the same response than last section, therefore this observer estimates correctly the states. It is interesting to study the error in the estimate, in order to do that, we can subtract estimate value minus real value of each state. For instance, according to Figure 2.11, in order to represent estimate error in cart’s position we should run in MATLAB: plot(tiempo,salida(:,1)-estados1(:,1)). In Figure 2.12 this study is made supposing a step input of 0.25 N 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 2.11. Later, they b (other option is have to be past to controllable canonical form and be feedback with K to feedback them directly with K). It is proved in Figure 2.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).

20

CHAPTER 2

Design of a reduced order observer

(a) Position

(b) Speed

(c) Angle

(d) Angular speed

Figure 2.12: Estimation errors

2.7

Design of a reduced order observer

In the previous section all states have been estimated from information of input and output. 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 an observer more efficient than just estimate 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 take 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.

21

CHAPTER 2

2.7.1

Design of a reduced order observer

Model transformation

To make the design of a reduced order observer we must have a model in which the b will be like: accessible variables of the system are selected as outputs. Therefore matrix C h i b 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]: # " 0 0 C C 1 2 −1 TOR (2.35) = Iσ 0 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 −1 do the inverse of matrix TOR , this new model must generate a matrix TOR 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: T

T

O OR A −→ A0 −→ A00

A0 = TO−1 A TO 00

A =

−1 0 TOR A

TOR

) −1 −1 A00 = TOR TO A TO TOR = TT−1 ot A TT ot ⇒ TT ot = TO TOR

(2.36)

h i b Being C in observed canonical form: C = 0 0 0 1 , matrix TOR can be built following the structure of matrix 2.35. We check that it has rank 4 so it is invertible.   0 0 0 1   1 0 0 0  TOR =  (2.37) 0 1 0 0   0 0 1 0

22

CHAPTER 2

Design of a reduced order observer

Taking this result and knowing TO of last section, equation 2.36 is used to compose TT ot . Applying this transformation  −0.09 0 0   0 0 b= 0 A  2.1 1 0  28 0 1

to matrices A, B and C:    1 0    h i −28 0 b= b= 1 0 0 0  B  C  0  0    0 1.29

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

2.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: " # y˙

"

# " # " # b11 A b12 b1 A y B x˙ = = + + u b21 A b22 b2 w˙ A w B

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

(2.40)

The develop is analogous: these poles are used to form their characteristic polynomial and with its coefficients we build F . b22 −H2 A b12 it is clear H2 . Matrix A b12 and A b22 are easily deduced comparing From F = A the model with 2.39 structure. b22 − H2 A b12 H1 is calculated: H1 = H2 A b2 − H2 B b1 Finally G is obtained: G = B

2.7.3

SIMULINK simulation

In order to know this observer performance, diagram of Figure 2.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. 23

CHAPTER 2

Design of a reduced order observer

Figure 2.13: Diagram of reduced order observer in SIMULINK To do that, the system of Figure 2.13 is excited with a step source of amplitude 0.25 N starting in t = 1 s. Errors of Figure 2.14 are obtained. Figure 2.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 2.14: Estimate errors

24

CHAPTER 2

Track a reference input for the position

Firstly, graphic (a) shows a totally zero error because this state has not been estimated: 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.

2.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 2.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 slop 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 = 10 m.

Figure 2.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 2.5, so in Figure 2.15 it is represented into the System block. 25

CHAPTER 2

Track a reference input for the position

A PID is characterized by the transfer function: F DTP ID =

Kd s2 + Kr s + Ki = D d s + Kr + Ki s

According to this construction, in Figure 2.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 h i 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 2.16.

Figure 2.16: Ki adjust Examining Figure 2.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 2.17. Kr = −30 is chosen like optimal value, because both higher values and lower values mean higher overshoots. 26

CHAPTER 2

Track a reference input for the position

Figure 2.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 2.18).

Figure 2.18: Kd adjust Kd = 1 achieves the best answer as it is showed in Figure 2.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.

27

CHAPTER 2

2.9

Study of the steady state error

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 2.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 2.5), and a observer; finally Y (s) is the output.

Figure 2.19: Blocks diagram Therefore R(s) will be transfer function of PID: R(s) =

Kd s2 + Kr s + Ki s2 − 30s − 150 = s s

(2.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]: h i−1 P (s) = C sI − Ar B+D (2.42) h i 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 2.19: Y (s) =

P (s) G(s) V (s) + D(s) 1 + G(s) 1 − G(s)

(2.43)

The error E(s): G(s) P (s) V (s) − D(s) ⇒ 1 + G(s) 1 − G(s) 1 P (s) ⇒ E(s) = V (s) − D(s) 1 + G(s) 1 − G(s)

E(s) = V (s) − Y (s) = V (s) −

(2.44)

28

CHAPTER 2

Study of the steady state error

Calling: Kr ˆ G(s) sr Kw P (s) = w Pˆ (s) s G(s) =

(2.45) (2.46)

ˆ ˆ = 0) = 1 and Pˆ (s = 0) = 1. It is easy to Where G(s) and Pˆ (s) satisfy that: G(s 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 2.42) and comparing with equation 2.46, it is proved that w = 0 and Kw = −0.01. Knowing that G(s) = R(s)P (s), and comparing with 2.45, we achieved: r = 1 y 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 2.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 2.20 is obtained. It is verified that these results match 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 =

1 Kr

=

1 1.5

= 0.6667 29

CHAPTER 2

Study of the steady state error

(a) Step source, ramp disturbance

(b) Zoom of e

Figure 2.20: Estimate errors • Error caused by disturbances: eD = 0 And total error: e = eV + eD = 0.6667. Simulation with MATLAB is showed on Figure 2.21. Again data obtained in the simulation match exactly with mathematical results.

(a) Ramp source, step disturbance

(b) Zoom of e

Figure 2.21: Estimate errors

V (s) step and D(s) step By proceeding analogously: • Error caused by reference input: eV = 0 • Error caused by disturbances: eD = 0 30

CHAPTER 2

Study of the steady state error

Following this indications, total error must be zero. Looking at Figure 2.22 it is proved again that is the same than mathematical deduction: having a step for both reference input and disturbance input, the error in permanent regimen is zero.

Figure 2.22: Step source, step disturbance

31

CHAPTER

3

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 something 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 1.2. Both the procedure of calculations and conclusions of the results of the inverted pendulum 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.

3.1

Mathematical model of the system

As it was explained in Section 1.2, in a inverted pendulum on sloping ground act the forces showed in Figure 3.1. System is divided in its parts (pendulum and cart, Figure 3.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 ~ Y~ . The origin of this reference system X, ~ Y~ is located position respect reference axis X, at any point on the surface of the plane. ~x0 and ~x00 will be necessary below and are: the ~ = 0 to the ground, and form the ground to horizontal distance from reference point X the pendulums axle respectively. Therefore it is true that ~x(t) = ~x0 (t) + ~x00 . ~xc (t) and ~ Y~ . gc and lc are the ~yc (t) locate pendulum’s center of mass respect reference system X,

32

CHAPTER 3

Mathematical model of the system

Figure 3.1: Inverted pendulum on sloping ground thickness and the length of the cart respectively1 .

(a) Pendulum

(b) Cart

Figure 3.2: System division into its two parts Applying Newton’s Second Law to these systems we obtain the equations: • Pendulum: –

P~ Fx (t) = m~x¨(t)  d2~xp (t) d2  ~ = m ~ x (t) − l sin θ(t) ⇒ dt2 dt2 ~¨ cos θ(t) ~ − mlθ~˙2 (t) sin θ(t) ~ ~ x (t) = −m~x¨(t) + mlθ(t) R ~ x (t) = m −R

1

(3.1)

Firstly it is supposed that the pendulum is rectangular, later it will be generalized for other kind of

bodies.

33

CHAPTER 3 –

Mathematical model of the system

P~ Fy (t) = m~y¨(t)  d2  d2 ~yp (t) ~ ~ y (t) + l cos θ(t) ⇒ = m dt2 dt2 ~¨ sin θ(t) ~ + mlθ~˙2 (t) cos θ(t) ~ ~ y (t) + m~g + m~y¨ = mlθ(t) R

~ y (t) − m~g = m −R

–

(3.2)

P~ ~¨ T (t) = I θ(t). ~¨ ~ −R ~ = I θ(t) ~ y (t)l sin θ(t) ~ x (t)l cos θ(t) −R

(3.3)

• Cart: –

P~ Fx (t) = M ~x¨(t)  2 2  ~ x (t) = M d ~xc (t) = M d ~x(t) − gc sin γ ⇒ F~ (t) cos γ − F~F ric (t) cos γ + R dt2 dt2 2 ~ x (t) = M ~x¨(t) F~ (t) cos γ − b~x(t) ˙ cos γ + R (3.4)

–

P~ Fy (t) = M ~y¨(t) 2 ~ y (t) = M d ~yc (t) = M ~y¨(t) ⇒ −F~ (t) sin γ + F~F ric (t) sin γ + R dt2 ~ y (t) = M ~y¨(t) −F~ (t) sin γ + b~x(t) ˙ sin γ + R

–

(3.5)

P~ ~¨ T (t) = I θ(t) gc ~ gc
~ gc
−F~F ric (t)

+ R y (t) sin γ
− Rx (t) cos γ
= 0 ⇒
2
2
2 ~ ~ ~ bx(t) ˙ − Ry (t) sin γ + Rx (t) cos γ = 0

(3.6)

We can observe in equation 3.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 3.1 we take the information to make this change: sin γ =

gc gc ⇒ ~x00 = 00 ~x sin γ gc gc ⇒ ~x0 (t) = ~x(t) − sin γ sin γ

(3.8)

~y (t) ~y (t) gc = y (t) = ~x(t) tan γ − gc ⇒ ~ 0 ~x (t) ~x(t) − sin γ cos γ

(3.9)

~x(t) = ~x0 (t) + ~x00 = ~x0 (t) +

tan γ =

(3.7)

⇒ ~y(t) ˙ = ~x(t) ˙ tan γ ⇒ ~y¨(t) = ~x¨(t) tan γ

(3.10) 34

CHAPTER 3

Mathematical model of the system

Applying this change 3.10 to equations which included variable ~y¨(t) (equations 3.2 and 3.5): ~¨ sin θ(t) ~ + mlθ~˙2 (t) cos θ(t) ~ ~ y (t) + m~g + m~x¨(t) tan γ = mlθ(t) R

(3.11)

~ y (t) = M ~x¨(t) tan γ −F~ (t) sin γ + b~x(t) ˙ sin γ + R

(3.12)

Substituting 3.1 and 3.11 in 3.3, and doing an analogous development as it was done in pendulum on horizontal ground, it is obtained equation 3.13.   ~¨ I + ml2  − mgl sin θ(t) ~ = ~x¨(t)ml cos θ(t) ~ + tan γ sin θ(t) ~ θ(t)

(3.13)

Substituting 3.1 in 3.4 it is obtained 3.14. ~2 ~¨ cos θ(t) ~ F~ (t) cos γ = (M + m) ~x¨(t) + b~x(t) ˙ cos γ + ml ˙θ(t) sin γ − mlθ(t)

(3.14)

~ ≈ 0 to be As in pendulum on horizontal ground, it is necessary to linearise around θ(t) able to obtain the state model.   ~¨ I + ml2  − mglθ(t) ~ = ~x¨(t)ml 1 + tan γ θ(t) ~ θ(t) ~¨ F~ (t) cos γ = (M + m) ~x¨(t) + b~x(t) ˙ cos γ − mlθ(t)

(3.15) (3.16)

~¨ from equation 3.16: Solving θ(t) cos γ ~ ~¨ = M + m ~x¨(t) + b cos γ ~x(t) θ(t) ˙ − F (t) (3.17) ml ml ml Replacing 3.17 in 3.15, and ordering obtained expression, it is achieved equation 3.18.      I ~ tan γ − (M + m) ~x¨(t) ml 1 + θ(t) +l = ml     I I ~ ˙ − cos γ (3.18) + l ~x(t) + l F~ (t) − mglθ(t) = b cos γ ml ml It is appreciated that the first term of 3.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 ~ tan γ ≈ 1. Doing this we can be solved supposing that γ ≈ 0, then this term: 1 + θ(t) 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 α= +l β = ml − (M + m) α ml Applying this changes to equation 3.18: mgl ~ α cos γ ~ ~x¨(t) = αb cos γ ~x(t) ˙ − F (t) − θ(t) (3.19) β β β Finally, 3.19 is replaced in 3.17. Ordering terms, equation 3.20 is achieved.     ~θ(t) ¨ = M + m αb cos γ + b cos γ ~x˙ − (M + m) g θ(t) ~ − M + m α cos γ + cos γ F~ (t) ml β ml β ml β ml (3.20) 35

CHAPTER 3

3.2

State space representation

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)

x˙ 1 (t) = x2 (t)

x˙ 3 (t) = x4 (t)

~¨ x4 (t) = θ(t)

Thus, it is true that: (3.21)

Rewriting equations 3.19 and 3.20 it is obtained: x˙ 2 (t) =

 x˙ 4 (t) =

mgl α cos γ ~ αb cos γ x2 (t) − x3 (t) − F (t) β β β

M + m αb cos γ b cos γ + ml β ml

 x2 (t) −

(3.22)

(M + m) g x˙ 3 (t)+ β   M + m α cos γ cos γ ~ − F (t) (3.23) + ml β ml

It is possible to writhe equations 3.22, 3.23 and 3.21 in matrix form:        x1 (t) 0 0 1 0 0 x˙ 1 (t)        x˙ 2 (t) 0 a1 a2 0 x2 (t) a3    ~     x˙ (t) = 0 0 0 1 x (t) +  0  F (t)  3     3   a6 0 a4 a5 0 x4 (t) x˙ 4 (t)

(3.24)

Siendo: a1 = a2 = a3 =

αb cos γ β mgl − β γ − α cos β

a4 = a5 = a6 =

(M +m)b cos γ α γ + b cos mlβ ml − (M +m)g β (M +m) cos γ alpha γ − − cos mlβ ml

As in last chapter, cart’s position and pendulum’s angle are assumed as outputs. This information is resumed in equation 3.25. Both equation 3.25 and 3.24, make up the state model of the inverted pendulum on inclined ground.   x1 (t) " #  " #  1 0 0 0  x (t)  2  + 0 F~ (t) ~y (t) =  0 0 1 0 x3 (t) 0  x4 (t) So characteristic  0   ~ = 0 A 0  0

matrix of the model are:    1 0 0 0      a1 a2 0 ~ =  a3   B 0 0 0 1    a4 a5 0 a6

" ~ = C

# 1 0 0 0 0 0 1 0

(3.25)

~ = D

" # 0 0

(3.26)

36

CHAPTER 3

LQR control

So we already have the enough information to start designing the control of the pendulum and doing in parallel the m-file (Appendix B) that makes easy the calculates. In following sections will present this process, stressing the new points from the previous chapter and ignoring those already mentioned. ~ of 3.26 state The first observation we can notice is that if γ = 0, poles of matrix A model, are the same then poles of the state model of pendulum on horizontal ground ~ of 2.18), therefore the system has the same performance. This prove that (expression A the develop has been made correctly.

3.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.1 m (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 2.4, it can be seen that matrix 3.27 provide a satisfactory answer.  1000000   0 Q=  0  0

 0  0 0 0  0 100 0  0 0 0

0

0

R=1

(3.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 3.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 (3.3 (a)); and when it is used by a person of l = 1.1 m and other of l = 0.9 m (3.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 3.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 37

CHAPTER 3

Track a reference input for the position

(a) ∆m

(b) ∆l

Figure 3.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 2.9.

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

3.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 3.4 simulates system respond when it has to follow a ramp reference input and work against two hard disturbances: a step of 2000 N in t = 1 s and an impulse in t = 6 s2 . 2

The impulse has been modelled supposing two steps: one of amplitude 3 · 107 N in t = 6 s, and the

second in t = 6.00001 s with the same amplitude but opposite sign.

38

CHAPTER 3

Animation

Figure 3.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 2.13). Answer achieved is shown in Figure 3.5. We realise the prefect performance of control 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 3.5: Respond when it is acting a 2000 N step in t = 1 s and an impulse in t = 6 s

3.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 representing the evolution of the values of position and angle of the pendulum in each moment.

39

CHAPTER 3

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

Figure 3.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 3.10, because now we need to know values of y. In this case we can not just replace equation 3.10, because 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 3.7.

40

CHAPTER 3

Animation

Figure 3.7: Variables used in animation programming

x = x0 + x00 ⇒ x0 = x − x00 gc gc sin γ = 00 ⇒ x00 = 2x 2 sin γ −y tan γ = 0 ⇒ y = −x0 tan γ = − (x − x00 ) tan γ = x gc − x tan γ ⇒y= 2 cos γ

(3.28) (3.29) 

 gc − x tan γ 2 sin γ (3.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 3.7: Cart

Pendulum

• P : (x − gc sin γ, y − gc cos γ)

• Q : (x − lt sin θ, y + lt cos θ) • 1: x−

gt 2

cos θ, y −

gt 2

sin θ




• 5 : Px −

• 2 : Qx −

gt 2

cos θ, Qy −

gt 2

sin θ

• 3 : Qx +

gt 2

cos θ, Qy +

gt 2

sin θ

• 4: x+

gt 2

cos θ, y +

gt 2

sin θ




lc 2

cos γ, Py +

lc 2

sin γ




• 6: x−

lc 2

cos γ, y +

lc 2

sin γ







• 7: x+

lc 2

cos γ, y −

lc 2

sin γ




• 8 : Px +

lc 2

cos γ, Py −

lc 2

sin γ







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

CHAPTER 3

Animation

Some intermediate pictures of balance process are showed in Figure 3.8 in order to give an example of the animation.

(a) Pendulum on sloping ground γ = −5o

(b) Pendulum on sloping ground γ = 10o

Figure 3.8: Inverted pendulum animation

42

CHAPTER

4

FINDINGS

4.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 pushing, 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: mathematical 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 theme 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 pendulum’s process.

43

CHAPTER 4

4.2

Other applications

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 1. 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. Therefore, although the codes showed in Appendix B are specified for chosen parameters, the structure is generic, and then this control can be used for other similar situation.

4.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 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. • Educational applications (try several kinds of control techniques). This thesis has been focused to solve the problem explained in Chapter 1, 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. 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 Therefore from this study and making some mathematics modifications, it is possible to solve several problems similar to inverted pendulum. 44

CHAPTER 4

4.4

Future work

Future work

Once reached the dimensions of a Master Thesis, there are numerous and varied possibilities 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 2.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. • 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.

45

APPENDIX

A

EXPLANATORY CALCULATIONS

A.1

Equations 2.7 and 2.8

Taking equations 2.1, 2.2, 2.3 and 2.4, if 2.1 and 2.2 are replaced in 2.3, and developing, it is achieved equation 2.7: 

 ~¨ sin θ(t) ~ − m~g sin θ(t) ~ ~ + mlθ~˙2 (t) cos θ(t) mlθ(t) ~¨  I θ(t) ~ ~ 2 ˙ ~ ~ ¨ ~ ~ + −mx¨(t) + mlθ(t) cos θ(t) − mlθ (t) sin θ(t) cos θ(t) = − l (((( ( ( ~ ~ 2 2 ~ cos θ(t) ~ − m~g sin θ(t) ~ ¨ sin θ(t) ~ + ml(θ˙((t) (( sin(θ(t) ⇒ mlθ(t) 

(

~¨ (((( I θ(t) ~ ~ 2 (((~(( 2~ ~ ˙ ~ ¨ ~ ml(θ((t) sin θ(t) cos θ(t) = − − mx¨(t) cos θ(t) + mlθ(t) cos θ(t) − ( l    I ~ ¨ ~ + cos2 θ(t) ~ ~ = m~x¨(t) cos θ(t) ~ ml sin2 θ(t) + ⇒ θ(t) − m~g sin θ(t) l
~¨ ~ = ml~x¨(t) cos θ(t) ~ I + ml2 − m~g l sin θ(t) ⇒ θ(t) If 2.1 is replaced in 2.4, equation 2.8 is obtained: ~¨ cos θ(t) ~ − mlθ~˙2 sin θ(t) ~ = M ~x¨(t) F~ (t) − b~x(t) ˙ − m~x¨(t) + mlθ(t) ~¨ cos θ(t) ~ − mlθ(t) ~ ⇒ F~ (t) = (M + m)~x¨(t) + b~x(t) ˙ + mlθ~˙2 (t) sin θ(t)

A.2

Equations 2.14 y 2.15

Solving x2 (t) from equation 2.12:  x˙ 2 (t) =

I +l ml

 x˙ 4 (t) − g x3 (t)

(A.1) 46

CHAPTER A

Representation of a system by its phase variables

If A.1 is substituted in 2.13:    I F~ (t) = (M + m) + l x˙ 4 (t) − g x3 (t) + b x2 (t) − ml x˙ 4 (t) ml     I + l − ml = F~ (t) + g(M + m) x3 (t) − b xx (t) x˙ 4 (t) (M + m) ml b x2 (t) g(M + m) x3 (t) F~ (t)


x˙ 4 (t) = − + I I I (M + m) ml + l − ml (M + m) ml + l − ml (M + m) ml + l − ml This value of x˙ 4 (t) is substituted in equation A.1: "


# I I + l F~ (t) b ml + l x2 (t) g(M + m) ml +l


x˙ 2 (t) = − − g− x3 (t) I I I (M + m) ml + l − ml (M + m) ml + l − ml (M + m) ml + l − ml I ml

Calling: α=

1 I ml




=

ml I(M + m) + M ml2

(M + m) + l − ml   I I + ml2 β= +l α= ml I(M + m) + M ml2

Equations 2.14 and 2.15 are obtained: x˙ 2 (t) = −βb x2 (t) + mlgα x3 (t) + β F~ (t) x˙ 4 (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]:     0 1 0 ··· 0 0      0 0 0 1 ··· 0       ..  . .. .. . . . . x(t) ˙ = .  x(t) +  ..  u(t) . . . .      0 0 0 0 ··· 1      −a0 −a1 −a2 · · ·

h

y(t) = b0 b1 · · ·

−an−1

1

i

bn−2 bn−1 x(t)

Where bi and ai are coefficients of numerator and denominator respectively of transfer function of the system.

47

CHAPTER A

Representation of a system by observable canonical form

If it is feedback with a matrix K: Ar= A + BK =

  0      0   0 1 ··· 0    0 h  . .. .. ..   ..  .. =  .. +   k k k ··· . . . .    . 1 2 3  0   0 0 ··· 1    0 −a0 −a1 −a2 · · · −an−1 1   0 1 0 ··· 0    0  0 1 ··· 0    ..  .. .. .. . . = Ar =  .  . . . .    0  0 0 ··· 1   k1 − a0 k2 − a1 k3 − a2 · · · kn − an−1 0

1

0

···

0



i

kn =

And matrix C does not vary.

A.4

Representation of a system by observable canonical form

The model using this representation has the structure [2]:     0 0 · · · 0 −a0   b 0 − b n ao 1 0 · · · 0 −a1       b 1 − b n a1  0 1 · · · 0 −a    u(t) x(t) ˙ = 2  x(t) +  ..  . . .  .   ..   .. .. . . ... .   bn−1 − bn an−1 0 0 · · · 1 −an−1 h y(t) = 0 0 · · ·

i 0 1 x(t) + bn u

Doing A − HC it is obtained:

48

CHAPTER A

Steady state error to different kind of reference inputs

A−  HC = 0  1   = 0 .  ..  0

0 ···

0

0 ···

0

1 ··· 0 .. . . .. . . . 0 ···

1  0  1   = A − HC = 0 .  ..  0

A.5

−a0





h1



     −a1    h2  h   −a2   −  h3  0 0 · · ·  ..   ..   .   . −an−1 hn  0 · · · 0 −(a0 + h1 )  0 · · · 0 −(a1 + h2 )   1 · · · 0 −(a2 + h3 )    .. . . .. ..  . . . .  0 · · · 1 −(an−1 + hn )

i

0 1 =

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 2.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 2.44. Calling it EV (s): EV (s) =

1 V (s) 1 + G(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: 1 1 1 = = s→0 s→0 1 + G(s) s 1 + lims→0 G(s) 1 1 = = Kr ˆ 1 + lims→0 Ksrr 1 + lims→0 sr G(s)

eV = lim s EV (s) = lim s

(A.3)

Ramp A ramp source is V (s) =

1 . s2

49

CHAPTER A

Steady state error to different kind of disturbance inputs

And steady state error: 1 1 1 = = 2 s→0 1 + G(s) s s + lims→0 s G(s) 1 1 = = ˆ lims→0 s G(s) lims→0 s Ksrr G(s)

eV = lim s EV (s) = lim s s→0

A.6

(A.4)

Steady state error to different kind of disturbance inputs

Continuing with model of section 2.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 2.44, that describes disturbances effect. Calling it ED (s): ED (s) = −

P (s) D(s) 1 − G(s)

(A.5)

Step Therefore the disturbance is represented as D(s) =

1 s

And applying the Final value theorem it is possible to calculate the error in permanent regime: eD = lim s ED (s) = lim −s s→0

= lim − s→0

1

s→0

Kw sw − Ksrr

= lim − sw s→0

sr

Kw ˆ P (s) 1 w P (s) = lim − s K = ˆ 1 − G(s) s s→0 1 − srr G(s)

Kw Kw = lim − w−r r r s→0 s (s − Kr ) (s − Kr )

(A.6)

Ramp Now: D(s) =

1 s2

Developing analogously: P (s) 1 = s→0 s→0 1 − G(s) s2 Kw Kw sw
= lim − = lim − w+1−r r K r s→0 (s − Kr ) 1 − sr s s→0 s eD = lim s ED (s) = lim −s

(A.7)

50

APPENDIX

B

SOURCE CODES MADE

B.1 1

Inverted pendulum on level ground

% %% INVERTED PENDULUM %%%

2 3

% % SYSTEM PARAMETERS

4

M =0.75;

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;

% [ kg ] Cart mass

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

0 0 1 0]; D =[0 0] ’;

51

CHAPTER B

Inverted pendulum on level ground

26 27

% % CONTROLLABILITY

28 29

Q = ctrb (A , B ) ;

30

controlab = rank ( Q ) ;

31

if controlab