Final REPORT Pendubot - Personliga hemsidor på KTHaurelied/Finalreport.pdf · Final REPORT...

Final REPORT

Pendubot

Dufour Aurelie820825-A325

Mairesse Benjamin840426-A276

Blanquer Eric830706-A138

Henriksson Erik820505-7451

Riback Jacob810813-0231

2006-05-10

Contents

1 Introduction 4

1.1 Description of the problem . . . . . . . . . . . . . . . . . . . . . . . . 41.2 Description of the Pendubot . . . . . . . . . . . . . . . . . . . . . . . 41.3 Structure of the report . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2 Model of the Pendubot 7

2.1 Modelling strategy . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.2 Derivation of the parameters . . . . . . . . . . . . . . . . . . . . . . . 7

2.2.1 Inner link . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.2.2 Outer link . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2.3 Derivation of the nonlinear model . . . . . . . . . . . . . . . . . . . . 102.3.1 Dynamic equations . . . . . . . . . . . . . . . . . . . . . . . . 102.3.2 State space equations . . . . . . . . . . . . . . . . . . . . . . . 142.3.3 Validation of the model: comparison with the real system . . . 16

2.4 Linearization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 172.4.1 Theoretical approach . . . . . . . . . . . . . . . . . . . . . . . 172.4.2 Calculations . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

3 Control design 19

3.1 Balancing control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 193.1.1 Tuning the balancing controllers . . . . . . . . . . . . . . . . . 203.1.2 Robustness of stabilizing controllers . . . . . . . . . . . . . . . 20

3.2 Swing-Up control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 223.2.1 Partial linearization . . . . . . . . . . . . . . . . . . . . . . . . 223.2.2 Swing-Up: Up-Up . . . . . . . . . . . . . . . . . . . . . . . . . 243.2.3 Swing-Up: Down-Up. . . . . . . . . . . . . . . . . . . . . . . . 24

3.3 Safety net . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 253.4 Switched system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

1

Automatic control Project Course2E1242

Final Report2006-05-10

4 Implementation 31

4.1 The Pendubot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 314.2 Program and DAQ . . . . . . . . . . . . . . . . . . . . . . . . . . . . 314.3 Implementation problems . . . . . . . . . . . . . . . . . . . . . . . . . 31

4.3.1 Sampling frequency and execution time . . . . . . . . . . . . . 314.3.2 Optimizing the control program . . . . . . . . . . . . . . . . . 32

4.4 Hardware problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . 324.4.1 Measurement errors . . . . . . . . . . . . . . . . . . . . . . . . 334.4.2 Output errors . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

4.5 Program structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . 344.5.1 General structure . . . . . . . . . . . . . . . . . . . . . . . . . 344.5.2 Swing-Up block . . . . . . . . . . . . . . . . . . . . . . . . . . 354.5.3 Balancing block . . . . . . . . . . . . . . . . . . . . . . . . . . 354.5.4 Safety net . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

2

List of Figures

1.1 Down-Up and Up-Up Position . . . . . . . . . . . . . . . . . . . . . . 51.2 Pendubot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2.1 Pendubot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.2 Behavior of the outer link when the inner link is fixed . . . . . . . . . 162.3 Behavior when the both arms are falling freely . . . . . . . . . . . . . 17

3.1 Feedback control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 203.2 Placement of the controllers . . . . . . . . . . . . . . . . . . . . . . . 213.3 Controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 283.4 Swing-Up control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 293.5 Reference signal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 293.6 Switching limits . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

4.1 General structure of the program . . . . . . . . . . . . . . . . . . . . 344.2 Swing-Up structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . 354.3 Balancing structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . 364.4 Safety net structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

3

Chapter 1

Introduction

1.1 Description of the problem

The system that will be controlled in this project is the Pendubot. The controlsystem should have the following features:

• Swing up the outer link of the Pendubot to the upright position:

– when the inner link is pointing downwards, as shown in Figure1.1(a): thiswill be called the ”Down-Up” control

– when the inner link is pointing upwards, as shown in Figure1.1(b): thiswill be called the ”Up-Up” control

• Stabilize the links in these configurations, while moving the inner link slowlyfrom one peripheral position to the other.

• Implement a software safety net, such that when the system enters an unsafestate or gets disturbed both links should be controlled to their hanging downposition. This should also be the procedure at a system shut down.

1.2 Description of the Pendubot

The Pendubot is a two-link robot as shown in Figure 1.2. It consists of a motorwhich is connected to an arm (arm 1); to this arm another arm (arm 2) is connected.By applying a voltage the motor will give out a torque acting on arm 1. The torqueapplied on arm 1 will also affect arm 2 via some non-linear relation.

4



(a) Down-Up position

(b) Up-Upposition

Figure 1.1: Down-Up and Up-Up Position

Figure 1.2: Pendubot

The system can be described as a 2ndorder mechanical system as shown in Figure1.2. One can use some control theory to achieve impressive performances on thissystem.

5



1.3 Structure of the report

After a short introduction which describes the project, the derivation of the physicalmodel will be explained in chapter 2. More specifically it will be seen how to useLagrange’s approach in order to derive a state space model of the Pendubot.

Then the chapter 3 will focus on the control design including the balancing con-troller and the Swing-Up control. After that the emergency procedure, namely thesafety net, will be explained. Finally, all those strategies will be merged in a switchedsystem.

The last chapter will describe how those solutions will be implemented withLabview.

6

Chapter 2

Model of the Pendubot

2.1 Modelling strategy

In mechanics two approaches are possible to model a system:

• Lagrange’s approach based on a energetic description of the system.

• Newton’s approach based on the fundamental principle of dynamics.

In this case, Lagrange’s approach seems easier to apply since only the kineticand potential energies of the system are necessary to describe it. This theory statesthat one system can entirely be described with a convenient set of parameters. Here,the knowledge of the angles of the inner and the upper link is enough to describecompletely the system in every positions. The derivation of the physical model willthus be completed in 3 steps:

1. Derivation of all the physical parameters of the Pendubot

2. Derivation of the nonlinear equations using Lagrange’s theory

3. Linearization of the nonlinear model

2.2 Derivation of the parameters

The general notations of the Pendubot are described in Figure 2.1.

7



Figure 2.1: Pendubot

2.2.1 Inner link

The following parameters are needed for the description of the system:Those values were obtained by simple weighting and measurements after taking

apart each piece of the link.

The moment of inertia of the inner link can now be calculated. It can be computedby adding the moments of every parts of the arm:

J1 = JInner + JWheel + JSensor (2.1)

where:

• JInner = 112

Minner (l21 + w2) = 0, 51.10−3 kg.m2

• JWheel = 12MWheelR

2Wheel + MWheell

2Wheel = 0, 338.10−3 kg.m2

• JSensor = MSensorl2Sensor = 7, 19.10−3 kg.m2

The total moment of inertia of the inner link is thus:

J1 = 8, 03.10−3 kg.m2

8



Total length of the arm l1 = 0, 21 mDistance from the axis of rotation to the center of gravity lg1 = 0, 116 m

Width of the link w = 0, 005 mWeight of the link Minner = 138, 75 g

Total weight of the arm M1 = 482, 5 gWeight of the wheel MWheel = 51, 9 gWeight of the sensor MSensor = 291, 85 gRadius of the wheel RWheel = 0, 015 m

Distance from the axis of rotation to the wheel lWheel = 0, 08 m

Table 2.1: Parameters of the inner link

2.2.2 Outer link

The following parameters are needed for the description of the system:Those values were obtained by simple weighting and measurements after taking

apart each piece of the link.

The moment of inertia of the outer link can now be calculated. It can be computedby adding the moments of every parts of the arm:

J2 = JOuter + JWheel1 + JWheel2 (2.2)

where:

• JOuter = 112

MOuter (l22 + w2) = 0, 529.10−3 kg.m2

• JWheel1 = 12MWheelR

2Wheel + MWheell

2Wheel1 = 0, 716.10−3 kg.m2

• JWheel2 = 12MWheelR

2Wheel + MWheell

2Wheel2 = 0, 567.10−3 kg.m2

The total moment of inertia of the outer link is thus:

J2 = 1, 812.10−3 kg.m2

9



Total length of the arm l2 = 0, 233 mDistance from the axis of rotation to the center of gravity lg2 = 0, 134 m

Width of the link w = 0, 005 mWeight of the link MOuter = 117 g

Total weight of the arm M2 = 220, 8 gWeight of the wheels MWheel = 51, 9 gRadius of the wheels RWheel = 0, 015 m

Distance from the axis of rotation to the 1st wheel lWheel1 = 0, 117 mDistance from the axis of rotation to the 2nd wheel lWheel2 = 0, 221 m

Table 2.2: Parameters of the outer link

2.3 Derivation of the nonlinear model

2.3.1 Dynamic equations

According to Lagrange’s theory, a system can be totally described by using aconvenient set of parameters qi(t), i = 1..n. Then, the studied system will be ruledby the following set of Lagrange’s equations:

L(qi, qi) = Ek − Ep , i = 1..n (2.3)

d

dt

(

∂L

∂qi

)

−∂L

∂qi

=∑

Non conservative torques (2.4)

where L(qi, qi) is called the Lagrangian of the system.

In this case, a convenient set of parameters is {θ1(t); θ2(t)} as shown in Figure 2.1.Indeed, no more parameter is needed to know exactly the position of the Pendubotat any time.

It is now necessary to calculate the energy of both arms. In the following, thenotation {θ1; θ2} will be used instead of {θ1(t); θ2(t)}.

10



Kinetic energy of the inner link

The inner link only has a movement of rotation around one axis. Its kinetic energycan thus be simply computed as follows:

Ek1 =1

2J1θ1

2(2.5)

where θ1 is its angular velocity.

Kinetic energy of the outer link

The movement of the outer link is composed by one movement of rotation andone movement of translation. Thus, in order to calculate its total kinetic energythe energy from both movements must be added. Koenig’s relation thus has to beapplied as follows:

Ek2 =1

2J2θ2

2+

1

2M2

∣

∣

∣

~VG2

∣

∣

∣

2

(2.6)

where θ2 is the angular velocity of the link and VG2 is the velocity of its center ofgravity. It can be calculated using the classical laws of kinematics and the geometryof the system:

~VG2 =d ~OG2

dt(2.7)

with ~OG2 = (l1cos(θ1) + lG2cos(θ2)) ~x + (l1sin(θ1) + lG2sin(θ2)) ~y

This leads to the following result:

~VG2 =(

−θ1l1sin(θ1) − θ2lG2sin(θ2))

~x +(

θ1l1cos(θ1) + θ2lG2cos(θ2))

~y (2.8)

Thus,

∣

∣

∣

~VG2

∣

∣

∣

2

=θ12l1

2sin(θ1)2 + θ2

2lG2

2sin(θ2)2 + θ1θ2l1lG2sin(θ1)sin(θ2)

+ θ12l1

2cos(θ1)2 + θ2

2lG2

2cos(θ2)2 + θ1θ2l1lG2cos(θ1)cos(θ2)

+ θ12l1

2 + θ22lG2

2 + 2θ1θ2l1lG2cos(θ1 − θ2)

(2.9)

Finally, the kinetic energy of the outer link is :

EK2 =1

2J2θ

22 +

1

2M2

(

θ21l

21 + θ2

2l2G2 + +2l1lG2 cos(θ1 − θ2)θ1θ2

)

(2.10)

11



Global kinetic energy of the system

Now that the kinetic energy of both arms have been calculated, it is necessary toadd them to get the global kinetic energy of the system:

EK = EK1 + EK2

=1

2J1θ1

2+

1

2J2θ

22 +

1

2M2

(

θ21l

21 + θ2

2l2G2 + +2l1lG2 cos(θ1 − θ2)θ1θ2

) (2.11)

Potential energy

The potential energy of the systems only comes from the action of the gravity. Itcan be easily calculated using the geometry of the system:

EP1 = M1lG1gsinθ1 (2.12)

EP2 = M2g (l1 sin θ1 + lG2 sin θ2) (2.13)

where g = 9, 81 m.s−2 is the constant of gravity.

Then, the global potential energy of the system is simply the sum of both poten-tial energies:

EP = EP1 + EP2

= M1lG1gsinθ1 + M2g (l1 sin θ1 + lG2 sin θ2)(2.14)

Derivation of the equations

Now that all the energies are known it is possible to calculate the Lagrangian ofthe system as follows:

L(θ1, θ2, θ1, θ2) = Ek − Ep

= θ21

[

12J1 + 1

2M2l

21

]

+ θ22

[

12J2 + 1

2m2l

2G2

]

+ M2l1lG2 cos (θ1 − θ2) θ1θ2

− (M1lG1 + M2l1) g sin θ1 − M2glG2 sin θ2

(2.15)

Then, all the necessary calculations are made in several steps :

12



• Partial derivative with respect to the angular velocities :

∂L

∂θ1

= θ1

[

J1 + M2l21

]

+ M2l1lG2 cos (θ1 − θ2) θ2 = A(

θ1, θ2, θ1, θ2

)

(2.16)

∂L

∂θ2

= θ2

[

J2 + l2G2M2

]

+ M2l1lG2 cos (θ1 − θ2) θ1 = B(

θ1, θ2, θ1, θ2

)

(2.17)

• Partial derivative of the previous equations with respect to the time :

d

dtA

(

θ1, θ2, θ1, θ2

)

=∂A

∂θ1

θ1 +∂A

∂θ2

θ2 +∂A

∂θ1

θ1 +∂A

∂θ2

θ2

= − M2l1lG2 sin (θ1 − θ2) θ1θ2 + M2l1lG2θ22 sin (θ1 − θ2)

+ θ1

(

J1 + M2l21

)

+ θ2M2l1lG2 cos (θ1 − θ2)(2.18)

d

dtB

(

θ1, θ2, θ1, θ2

)

=∂B

∂θ1

θ1 +∂B

∂θ2

θ2 +∂B

∂θ1

θ1 +∂B

∂θ2

θ2

= − θ21M2l1lG2 sin (θ1 − θ2) + θ1θ2M2l1lG2 sin (θ1 − θ2)

+ θ1M2l1lG2 cos (θ1 − θ2) + θ2

(

J2 + M2l2G2

)

(2.19)

• Partial derivative with respect to the angles:

∂L

∂θ1

= −M2l1lG2 sin (θ1 − θ2) θ1θ2 − [lG1M1 + M2l1] g cos θ1 (2.20)

∂L

∂θ2

= M2l1lG2 sin (θ1 − θ2) θ1θ2 − M2lG2g cos θ2 (2.21)

The final relations of the model can then be derived:

τ =d

dt

(

∂L

∂θ1

)

−∂L

∂θ1

(2.22)

where τ is the input torque of the system.

0 =d

dt

(

∂L

∂θ2

)

−∂L

∂θ2

(2.23)

13



which gives :

τ = θ1

(

J1 + M2l21

)

+ θ2M2l1lG2 cos(θ1 − θ2) + M2l1lG2θ22 sin(θ1 − θ2)

+ (M1lG1 + M2l1) g cos θ1

(2.24)

0 =θ1M2l1lG2 cos(θ1 − θ2) + θ2

(

J2 + M2l2G2

)

− M2l1lG2θ21 sin(θ1 − θ2)

+ M2glG2 cos θ2

(2.25)

2.3.2 State space equations

The previous equations are the general relations that describe the model. Tosimplify them, new constants will be introduced as follows :

τ = A1θ1 + A2θ2 cos(θ1 − θ2) + A3θ22 sin(θ1 − θ2) + A4 cos θ1 (2.26)

0 = B1θ1 cos(θ1 − θ2) + B2θ2 − B3θ21 sin(θ1 − θ2) + B4 cos θ2 (2.27)

where the following constants have been introduced:

A1 = J1 + M2l21

A2 = M2l1lG2

A3 = M2l1lG2

A4 = (M1lG1 + M2l1) g

B1 = M2l1lG2

B2 = J2 + M2l2G2

B3 = M2l1lG2

B4 = M2glG2

The two variables θ1 and θ2 must now be decoupled. Thanks to the equation(2.27), θ2 can be expressed as a function of all the other variables:

θ2 =1

B2

[

−B1θ1 cos(θ1 − θ2) + B3θ21 sin(θ1 − θ2) − B4 cos θ2

]

(2.28)

Then, this relation can be put into the equation (2.26) :

τ =θ1A1 +A2

B2

cos(θ1 − θ2)[

−B1θ1 cos(θ1 − θ2) + B3θ21 sin(θ1 − θ2) − B4 cos θ2

]

+ A3 sin(θ1 − θ2)θ22 + A4 cos θ1

(2.29)A first relation involving θ2 but not θ2 is thus obtained :

14



θ1 = 1B1A2

B2cos2(θ1−θ2)−A1

[A2B3

2B2

sin (2(θ1 − θ2)) θ21 + A3 sin(θ1 − θ2)θ

22

−A2B4

B2

cos θ2 cos(θ1 − θ2) + A4 cos θ1 − τ ](2.30)

In a similar way, θ1 can be expressed as a function of all the other variables andput into the equation (2.27):

0 =B1

A1

cos(θ1 − θ2)[

−θ2A2 cos(θ1 − θ2) − A3θ22 sin(θ1 − θ2) − A4 cos θ1 + τ

]

+ B2θ2 − B3θ21 sin(θ1 − θ2) + B4 cos θ2

(2.31)

A second relation involving θ2 but not θ1 is thus obtained :

θ2 = 1B1A2

A1cos2(θ1−θ2)−B2

[−B3θ21 sin(θ1 − θ2) − B1

A3

2A1

sin (2(θ1 − θ2)) θ22

−B1A4

A1

cos θ1 cos(θ1 − θ2) + B4 cos θ2 + B1

A1

cos(θ1 − θ2)τ ](2.32)

Thus, the system is now in a state-space form. Let X =[

θ1, θ2, θ1, θ2

]T

denote

the state vector containing the four state variables, the following relation is obtained:

X = F(

θ1, θ2, θ1, θ2

)

+ G (τ) (2.33)

where

F(

θ1, θ2, θ1, θ2

)

=

0 0 1 00 0 0 1

f1

(

θ1, θ2, θ1, θ2

)

f2

(

θ1, θ2, θ1, θ2

)

(2.34)

G (τ) =

00τ0

(2.35)

f1

(

θ1, θ2, θ1, θ2

)

denotes equation (2.25) and f2

(

θ1, θ2, θ1, θ2

)

denotes equation

(2.27).

15



2.3.3 Validation of the model: comparison with the real sys-

tem

Thanks to the Data Acquisition function of Labview, it is possible to observe thebehavior of the system. For these validations, one parameter corresponding to slidingfriction has been added on each arm. This parameter has been set proportional tothe velocity of the angles.

Two tests have been run separately:

• In the first one the inner link is fixed. The studied movement is the free fall ofthe outer link as illustrated in Figure 2.2.

Figure 2.2: Behavior of the outer link when the inner link is fixed

These two behaviors are similar. One can observe a very slight difference be-tween the frequency of oscillation in both cases: the real system oscillates witha frequency of 1Hz whereas the simulated model has a frequency of 1.1Hz. This10% difference can be neglected. Indeed, it is only due to small imprecisionsthat cannot be avoided in the estimation of the parameters of the Pendubot.

• In the second simulation both arms are falling freely, as shown in Figures2.3(a)and 2.3(b).

16



(a) Behavior of the inner link when both armsare falling freely

(b) Behavior of the outer link when both armsare falling freely

Figure 2.3: Behavior when the both arms are falling freely

The global behaviors are similar but differences remain. These can easily beexplained since it is really hard to get a very accurate model for the frictions.The influence of the cable when the system is oscillating could also be takeninto account.

Nevertheless, the results seem acceptable and the model seems good enoughto work with.

2.4 Linearization

2.4.1 Theoretical approach

The previous model is not directly exploitable. It is necessary to linearize it in

order to control it in an easier way. If[

θ1eq, θ2eq, θ1eq, θ2eq

]T

denotes the values of the

state variables in an equilibrium point, the liberalization around this point will bederived by using Taylor’s expansion:

X = Xeq + A.∆X + B.∆U (2.36)

where A is the jacobian of the matrix F defined in equation (2.34) and B is the

17



Jacobian of the matrix G defined in equation (2.35).

A and B can be calculated as follows:

A =

0 0 1 00 0 0 1

∂f1

∂θ1

∂f1

∂θ2

∂f1

∂θ1

∂f1

∂θ2

∂f2

∂θ1

∂f2

∂θ2

∂f2

∂θ1

∂f2

∂θ2

[θ1,θ2,θ1,θ2]T

=[θ1eq ,θ2eq ,θ1eq ,θ2eq]T

(2.37)

B =

00∂τ∂τ

0

=

0010

(2.38)

Finally, the general relation that will be used in the project is:

∆X = A∆X + B∆U (2.39)

2.4.2 Calculations

Regarding the complexity of the equations, all the calculations of the partial deriva-tives are performed with Maple.

The matrices A and B must be computed for each studied point. Those pointswill be defined in the next chapter, when choosing the controllers.

18

Chapter 3

Control design

The control of the Pendubot is divided into two main problems, Swing-Up andbalancing. These two problems are solved with different approaches.

3.1 Balancing control

To stabilize the Pendubot in position, state-feedback with LQR-control is used [2]. LQR is used since it gives an optimal stabilizing controller. The LQ-parameters arederived using the linearized systems (2.39) evaluated around the different equilibria.This gives a feedback torque as

τ = τ0 − K(x − x0) (3.1)

where K is the 1 × 4 vector containing the state feedback gains. K is calculatedby minimizing equation 3.2. This is done by solving the Algebraic Riccati Equation,equation 3.3, then K is given by equation 3.4.

min

∫

∆xT Q∆x + ∆τT R∆τ (3.2)

AT P + PA + Q = PBR−1BT P (3.3)

K = R−1BT P (3.4)

19



Figure 3.1: Feedback control

3.1.1 Tuning the balancing controllers

As mentioned in Section 3.1 LQR is optimal, however it is important to rememberthat it is optimal with respect to the weightings Q and R. Hence the controller canbe tuned by changing the values of these parameters. They have to be set for eachcontroller in order to achieve good performances.

3.1.2 Robustness of stabilizing controllers

Since the controllers are calculated based on the linearized system they need to berobust so that they work on the real system. The controllers derived with LQR arerobust and work in a neighborhood around the linearization points. Since this neigh-borhood only covers a part of a revolution, several controllers are needed. Testing hasshown that one revolution needs to be divided into 6 sectors with one balancing con-troller in each one, as showed in Figure 3.2. Those controllers will be placed in the fol-lowing positions of the inner link: θ1 = (3π/4 ; π/2 ; π/4 ; −π/4 ; −π/2 ; −3π/4)

It can be verified thanks to Simulink, that those 6 controllers will be sufficient tocontrol the motion of the Pendubot. The domains in which each controller is ableto control the system are presented in Figures 3.3(a) and 3.3(b). These values were

20



Figure 3.2: Placement of the controllers

obtained by simulating a freefall from an offset angle on the inner link around thecontroller’s working point, while the outer link was vertical. By observing whetherthe controllers were able to catch and stabilize the system or not, it was possible todetermine the sectors in which the controllers were working.

One could think that the controllers situated in sectors 2, 3, 5 and 6 are sufficientto control the whole movement. Nevertheless, the two other controllers are necessaryto improve the robustness and the stability of the system.

As for the Q and R matrices, the following values have been chosen after severalexperiments:

• For sectors 2, 3, 5 and 6 :

Q =

10 0 0 00 65 0 00 0 5 00 0 0 5

R = 40

• For sectors 1 and 4 :

21



Q =

10 0 0 00 10 0 00 0 1 00 0 0 1

R = 50

3.2 Swing-Up control

The Swing-Up control design has been done in two steps:

• A partial linearization of the model

• The design of the PD controller

3.2.1 Partial linearization

The Pendubot consists in two arms linked by a free join. One can control thissystem (which has two degrees of freedom) via a DC-motor applying a torque onthe inner arm. Then, one have to deal with an underactuated system since we havefewer control inputs than degrees of freedom. In this case, it is not possible to usethe direct feedback linearization. Thus a weaker form, namely the partial feedback

linearization, has been chosen.

The idea is to feed the system with a modified torque in order to cancel thenonlinearities. This can be achieved via an inner-loop within the system and gives anew control input, namely v1 (see Figure 3.4).

The following shows the steps to get a linear relation between the control input(the torque τ) and θ1:

The initial equations are :

τ = θ1A1 + θ2A2 cos(θ1 − θ2) + A2θ22 sin(θ1 − θ2) + A4 cos(θ1) (3.5)

0 = θ1B1 cos(θ1 − θ2) + θ2B2 − A2θ21 sin(θ1 − θ2) + B4 cos θ2 (3.6)

From equation (3.6) the expression of θ2 can be obtained, as showed in the pre-vious chapter:

22



θ2 =1

B2

[

θ21A2 sin(θ1 − θ2) − B4 cos(θ1 − θ2) − θ1A2 cos(θ1 − θ2))

]

(3.7)

A new equation for the torque can be derivated by replacing θ2 in equation (3.5)

τ = θ1A1 + A2θ22 sin(θ1 − θ2) + A4 cos(θ1) + A2

B2

cos(θ1 − θ2)[θ21A2 sin(θ1 − θ2)

−B4 cos(θ1 − θ2) − θ1A2 cos(θ1 − θ2))]

(3.8)

which can be rewritten:

τ =θ1

[

A1 −A2

2

B2

cos2 (θ1 − θ2)

]

+ θ22A2 sin (θ1 − θ2) + θ2

1

A22

B2

cos (θ1 − θ2) sin (θ1 − θ2)

+ A4 cos(θ1) − B4A2

B2

cos(θ2) cos (θ1 − θ2)

(3.9)So far, a new control input can be defined, namely v1, using the two following

notations α(θi) and β(θi):

α(

θ1, θ2, θ1, θ2

)

= θ22A2 sin (θ1 − θ2) + θ2

1A2

2

B2

cos (θ1 − θ2) sin (θ1 − θ2)

+A4 cos(θ1) − B4A2

B2

cos(θ2) cos (θ1 − θ2)(3.10)

β(θ1, θ2, θ1, θ2) = A1 −A2

2

B2

cos2 (θ1 − θ2)

v1 is defined by the following relation and is designed in order to suppress thenonlinear terms:

τ = v1β(θi) + α(θi) (3.11)

Using this feedback for the partial linearization leads to a new set of state equa-tions:

θ1 = v1 (3.12)

θ2 =1

B2

[

θ21A2 sin(θ1 − θ2) − B4 cos(θ1 − θ2) − v1A2 cos(θ1 − θ2))

]

(3.13)

23



The system is now linear with respect to θ1. Furthermore, as it is a second orderrelation, a PD controller can be implemented. The realizable control law that is usedhere is:

v1 = Kp(θ1ref − θ1) − Kdθ1 , where Kp and Kd are positive gains. (3.14)

Finally, the global corrected system will be double-looped as shown in Figure 3.4.The inner loop partially linearizes the system and the outer loop is used to controlthis modified system.

Now that the system has been put in a handier form, one have to figure out thetwo strategies to reach the Up-Up position and the Down-Up position. This can beachieved choosing a suitable reference signal as explained in the following.

3.2.2 Swing-Up: Up-Up

This motion is the simplest to implement since a static reference signal is enough toswing the system from its Down-Down position to its Up-Up position. Thanks to thepartial feedback linearization, it is possible to set the angular position of θ1. Thus, itis not hard to reach the up-position for the inner arm: this has been achieved settingthe reference signal to π/2. However, one want to be able to catch the Pendubot inits Up-Up position by switching to the balancing controller.

Consequently, the PD-controller (Kp, Kd) has been tuned in order to reach theUp-Up position with an angular speed of the outer arm (θ2) as small as possible.Firstly, the P-part has been tuned to get the maximum response from the systemwhile avoiding saturation. Secondly, the motion of the outer arm has been smoothedusing the D-part. Finally, the following set of parameters are used for this Up-Upstrategy:

Kp = 164

Kd = 11.85

θ1ref = π/2

3.2.3 Swing-Up: Down-Up.

This second strategy is more subtle than the previous one. A static reference signalis not enough since the inner arm starts in its down-position and will end up in this

24



position as well. The goal is to Swing-Up the outer arm in a smart way and be ableto catch the system by switching to the balancing controller.

Intuitively, one can easily find out that one have to store up energy balancing theouter arm in order to swing it up. Different balancing strategies have been experi-mented. In the end, it has been shown that the most ’basic’ strategy was the mostefficient.

Thus, one worked out that a suitable sinusoid can achieve the goal. Trying out tobalance the Pendubot manually, it was found out that a sinusoid frequency around1 Hz could work. Then, both increasing step by step the amplitude and tuning thePD-parameters, it was finally possible to derive the appropriate sinusoid sequence(see Figure 3.5) which enables the Pendubot to reach its Down-Up position. Finally,the following set of parameters are used for this Down-Up strategy:

Kp = 140

Kd = 10.25

θ1ref = θ1ref (t)

3.3 Safety net

The safety net is an emergency procedure which is used to ”stop” the processwhen it is unsafe. Thus, one can never imagine to stop the system and let it free-fallsince the two arms will have chaotic motions. Then, whatever the configuration ofthe Pendubot, it has been decided that if the supply voltage is higher than

9.9 Volts (which corresponds to 99 Volts after the servo-amplifier), the system is

considered as unsafe. The safety net is engaged and a two steps procedure starts.

Umax = 9.9V

First, the partial linearization feedback is used to make the system point down-wards with θ1ref = -π/2. This can be seen as a sufficient procedure to stop thePendubot. However, in order to reach the stable ”Down-Down” equilibrium faster, aDown-Down LQ-controller has been used. Indeed, using only the partial linearizationfeedback makes the outer link spin since the motion of the outer arm is an internaldynamic in this case. Then, when the Pendubot´s arms enter a sufficiently small an-gular sector around −π/2 and the corresponding angular velocities are small enough

25



(see Figures 3.6(a) and 3.6(b)), the system switches to a Down-Down LQ-controllerwhich will stabilize the whole Pendubot.

Switching constraints:

Inner link :∣

∣θ1 ± π/2∣

∣ < 1 rad and∣

∣θ1

∣

∣ < 16.5 rad/s (3.15)

Outer link :∣

∣θ2 ± π/2∣

∣ < 1.2 rad and∣

∣θ2

∣

∣ < 16.5 rad/s (3.16)

3.4 Switched system

All of the strategies and controllers described above have been merged in a switchedsystem.

Two central LQ-controllers have been derived for the sectors 1 and 4 which cor-respond to the two equilibrium points.

To be able to move from one equilibrium point to the other, the revolution wasdivided in different sectors as shown in Figure 3.2. Then, four more LQ-controllershave been computed for the medium positions (π/4; 3π/4; 5π/4; 7π/4). This setof controllers enables the system to get closer to the two uncontrollable positions,namely: θ1 = 0 and θ1 = π.

To be able to understand what one call switched system, an example of how toreach the Up-Up position from the Down-Up position is given here.

• The system is swung-up from its Down-Down position to its Down-Up positionusing the corresponding Swing-Up strategy.

• The Pendubot is catched by the balancing controller in sector 1.

• Changing the reference signal, the Pendubot starts to move upwards (anti-clockwise direction).

• After the inner arm has crossed the value -π/4, the second balancing controllerstabilizes the system in sector 2.

• When the process has risen up enough (close to the uncontrollable position),the partial feedback linearization is used to jump over this position.

26



• After that, balancing controllers 3 and 4 are able to catch again the Pendubotuntil it reaches its Up-Up position.

27



(a) Controllers in (π/2;-π/2)

(b) Controllers in (π/4; -π/4)

Figure 3.3: Controllers

28



Figure 3.4: Swing-Up control

Figure 3.5: Reference signal

29



(a) Inner Link

(b) Outer Link

Figure 3.6: Switching limits

30

Chapter 4

Implementation

4.1 The Pendubot

The Pendubot has two encoders that give the angle of the inner and the outerlink, namely θ1 and θ2. The encoders have a resolution of 1250 points per revolution,which means that it is possible to detect an angular deviation of 5 · 10−3 rad on eachlink. To apply a torque to the inner link the Pendubot uses a 90 volts DC motor.Since the power is taken from a wall socket, the Pendubot is equipped with an AC-DC converter. This converter provides a control unit with DC power. The controlunit also receives control signals from the user’s control program and amplifies themby a factor 10 before sending them out to the DC motor.

4.2 Program and DAQ

To control the Pendubot a PC with LabView 7.1 is used. The PC is also equippedwith a Data Acquisition Card (DAQ card), that constitutes the interface between thePendubot and the PC. The DAQ card used in this project is a National InstrumentsPCI-6221.

4.3 Implementation problems

4.3.1 Sampling frequency and execution time

To be able to control the Pendubot it is important that the sampling rate is highenough. With a too low sampling rate the system will not be able to react as fast as

31



required and the Pendubot will go out of control. However, there is also an upperlimit for the sampling frequency. This limit is set by the execution time of the controlprogram, Te. In one sampling interval the control program must have enough timeto collect the angular measurements, do all the calculations and send out the newoutput signal. It is also important to give the system time to react to the new controlsignal before collecting the new measurement samples. This gives the sampling time,Ts, a constrain

Ts > Te.

The resulting time the system has in order to react thus is

Tr = Ts − Te.

The important thing to get a stable system is to choose Ts so that

Tr >> Te.

Therefore it is important to have a fast control program to fulfill those constrainsand still have a sampling frequency that is high enough to control the Pendubot. Itis very hard to calculate a suitable sampling frequency in advance. Therefore thesampling frequency used to control the Pendubot was determined by testing, andfinally set to 100 Hz.

Fsampling = 100Hz

4.3.2 Optimizing the control program

The first versions of the control program proved to be far too slow to be ableto control the Pendubot. The program managed to stabilize the Pendubot but thebehavior was very shaky. Hence, it was necessary to optimize the control program toexecute faster. It was found that the built-in sub VI:s such as the DAQ-assistants andother pre-defined functions were very slow. Therefore all the built-in sub VI:s wereremoved and new ones were made. It was noticed that it was very time consuming touse dynamic data wires. Thus, when the new sub VI:s were constructed, only wiresthat send doubles were used. These changes made the program 15 times faster, whichwas enough to control the Pendubot in a satisfactory way.

4.4 Hardware problems

During the project three major hardware problems occurred. Two of these prob-lems were related to the measurement of θ2 and one was related to the output.

32



4.4.1 Measurement errors

There are two major problems with the measurement of θ2. The first one is dueto the fact that the mounting between the angular encoder and the axis holding theouter link gets loose after running the process several times. This causes the angularmeasurements to drift and thereby become wrong. The solution to this problem isto tighten the screws holding the mounting. This needs to be done in quite closeintervals.

The more serious problem is that even though the sensor is well tightened someangular drifting still occurs sometimes. The angular drift is quite random with largevariance. It has also been shown that the errors are not repeatable.

Further investigation has lead to some facts about the drift which are:

• it is increased when the motor gets warmer

• it is always positive

• it only occurs at high voltage

• it is increased if the sensor cable is winded around the motor

Since the angular measurements are based on counting pulses from the angularencoder, a short inductive pulse in the sensor cable will make the program count upthe angle. If a lot of these inductive pulses occurred, an angular drift would appearin the program.

How was this problem solved?

It has been shown that the drifting problem came from the induction. When themotor gets hot, the hysteresis is changed thus increasing magnetic field leakages.Indeed, high voltages into the motor will lead to high currents thus increasing themagnetic field. Then, when winded around the motor, the cable is exposed to moremagnetic flux. The induction inside the cable is thus higher.

To get these suspicions confirmed Professor Hans-Peter Nee from the departmentof ”Electrical Machines and Power Electronics” was consulted. According to Nee,this is a quite common problem which is difficult to handle.His advice was to use awell shielded cable. This helped lowering the problem, but not solving it completely.

33



4.4.2 Output errors

When balancing the Pendubot with both arms pointing upwards, an offset of about+0.05 rad could be observed on the inner link . This offset did not appear in thesimulations so it was believed to be hardware related.

To investigate this further measurements were made on the servo amplifier insidethe Pendubot. They showed that the servo amplifier amplified positive voltages morethan negative ones.

Since the controllers first calculate the required torque and then translate thisinto a voltage sent out to the servo amplifier, the amplification must be uniform forthe system to work perfectly. The nonlinearities in the servo amplifier result in thefact that the actual torque applied on the process becomes too large if the desiredtorque is positive and too small if the desired torque is negative. This is believed tocause the above stated offset.

4.5 Program structure

4.5.1 General structure

The program is built up in three major building blocks. The first one is the Swing-Up block, the second is the balancing block and the third one is the safety net block,see Figure 4.1 below. The program always runs in this sequence and all blocks mustbe passed during one iteration. Inside each of these blocks the program can read andprocess data as well as write data to and from the Pendubot. Each block is in facta program that can be run separately.

Figure 4.1: General structure of the program

34



4.5.2 Swing-Up block

This is the first block the program enters in each iteration. This block is built upas shown in Figure 4.2. The Data proc block measures the angles from the DAQcard and computes the states. The monitor listens to user input and monitors theSwing-Up process.

When the program enters the Swing-Up block the user has the alternative ofwaiting or swinging the system to the Up-Up or Down-Up equilibrium. The swingingis done by pushing one of the two buttons. When this is done the program swingsup the system to the desired equilibrium. When the Pendubot has reached a statewhere the balancing block can catch it, the swing monitor stops the swinging andhands over the control to the balancing block.

Figure 4.2: Swing-Up structure

4.5.3 Balancing block

The balancing block is the block that has most functionality and that is run mostof the time. The block contains two major alternative sequences, see Figure 4.3.

The upper path is used when the Pendubot is being balanced around an equilib-rium or recovering from disturbances. Here the sector block determines which sectorthe Pendubot is in and thereby determines which of the balancing LQ-controllers inthe balancer to use.

35



The lower path is used for peripheral movement of the Pendubot. The switchto this path is done by the user pressing one of the two buttons in the GUI. Whenthis path is activated the Launch monitor moves the Pendubot close to the uncon-trollable point and then activates the launcher which launches the system passedthe uncontrollable point. When the launch is done, the Launch monitor hands thesystem over to the balancing path.

The last block inside the Balancing block is the safety check, through which alloutputs must pass. Inside the safety check the computed output from the upper andlower path is monitored. If it is found to be unsafe the balancing block is closed andthe safety net is engaged.

Figure 4.3: Balancing structure

4.5.4 Safety net

The program can enter the safety net for three reasons. The first one is that theuser has stopped the balancing controller, the second one is that a disturbance hasmade the system unsafe and the third one is that the user has wished to stop theprogram. The safety net works in the same way whatever the reason is.

First the supervisor monitors the system to determine which of the safety netcontrollers to use. Usually it first uses the take-down block to move the inner linkdownwards. When this is done the supervisor waits for the outer arm to be in a givensector. Then, the Down-Down controller is engaged until the supervisor finds thesystem is safe again (see Figure 4.4). When the system is found safe the supervisorcloses the Safety net.

36



As mentioned in the first paragraph the system can enter the safety net for threereasons. If it has entered because of reasons one or two the program will start overin the Swing-Up block. If the safety net was entered because the user wished to stopthe program, the program will stop.

Figure 4.4: Safety net structure

37

Bibliography

[1] Bo Wahlberg Lecture notes in Nonlinear Control

[2] Ulf Jonsson, Claes Trygger and Petter Ogren Lecture notes in Optimal

Control

[3] Mark W.Spong Underactuated Mechanical Systems

[4] Mark W.Spong Swing up control of the Acrobot

38

Final REPORT Pendubot - Personliga hemsidor på KTHaurelied/Finalreport.pdf · Final REPORT...

Documents

Transcript of Final REPORT Pendubot - Personliga hemsidor på KTHaurelied/Finalreport.pdf · Final REPORT...