Introduction The furuta pendulum is a device widely used in the development of different types of controllers as well as a learning tool

in academic settings to show and teach students in universities how to design and implement controllers. The furuta pendulum is a device that incorporates two important elements, an arm and a simple pendulum. The arm is driven by a servomotor that allows rotation in the horizontal plane, while the pendulum is attached to the other end of the arm and it consist of a simple mass suspended at the end of a mass-less rigid string. The pendulum can rotate in the vertical plane and its main characteristic is that it has a fixed period, meaning that it always takes the same amount of time to complete a single revolution about its rest position when a small displacement is given. This is called the time period. As already mentioned before this type of pendulum is used to develop different types of controllers which goals are to bring the pendulum to the inverted position and to maintain it fixed at such position using two degrees of freedom and one actuator. One example can be seen in figure 1. During the course of this Lab we will design and implement a controller based in the energy concept, which will allow us to bring the pendulum to an upward position. Later an LQR controller and observer will be employed for the task of keeping the pendulum in the inverted position. Finally a pole cross fader will be implemented in order to improve the catching of the system.

Figure 1: Inverted Pendulum

Description of the Furuta pendulum As stated above, the furuta pendulum consist of one actuator. The actuation is done by the help of a servo motor. The motor gets its signals from a board coupled to a computer using a special board. A simplified diagram of the pendulum is shown below in figure 2. The degrees of freedom are the and . They represent the rotation of the arms. However both of

Figure 2: Simplified Model

them are not actuated. Only is controlled, by the use of a motor. The motion of is simply due to inertia. The masses and the lengths are as shown in the diagram. The Motor is not shown in the picture. Motor applies a torque and as a result we have a change in . changes due to the inertia of the motion caused in the arm. For simplification we have considered the masses of the elements as point masses, as shown in figure 3. This helps in the modelling.

Figure 3: Simplification as used in Model

Obtaining the Model The following considerations have been taken into account for the modeling of the furuta pendulum: The zero for has been chosen at the upright position. That makes the controller design an easier task; 2 otherwise the controller would be switching at the stable position between and -At the same time it . allows have a better visualization in the graphs.

The coordinate axes have been chosen so that they match with a Denavit Hartenberg representation.

The arms have been modeled as a thin rod such that, the coordinate axes of the thin rod are the principal axes of inertia, hence the inertia tensor is in its diagonal form. Furthermore, due to the geometric characteristics of the rod, the moment of inertia associated to the axis of rotation can be neglected.

The coordinate axes have been chosen as are shown in the figure. It also shows the zero angular position (=0, =0). 1 2 To derive the equations of motion of the system a Lagrangian formulation [x] is to be used, hence energies involved in the system need to be computed, which in turn involves the computation of velocities and positions. We need to know centers of mass velocities; first we calculate the velocities of the joints: Angular velocity of joint 1: 1 = [0 0 1 ] 1 = [0 0 0]

Linear velocity of joint 1:

Rotation matrix of joint 2 relative to joint 1:

1 2

Position of joint 2 relative to joint 1:

sin (2 ) cos (2 ) 0 0 1 = 0 cos (2 ) sin (2 ) 0

1 2

= [0 1

0]

Velocities of joint 2 can be derived with the formulas [x]:

Angular velocity of joint 2: 2 = ( 1)1 2 1 + [0 0 2 ]

Linear velocity of joint 2:

Position of joint 3 relative to joint 2:

sin (2 )1 1 2 = ( 1)1 (1 + 1 1) = cos(2 ) 1 1 2 2 0

2 3

cos (2 )1 = sin (2 )1 2

Angular velocity of joint 3:

3 = 2 + [0 0 0] = 2 Linear velocity of joint 3:

= [2

0 0]

3 2

is the identity matrix sin (2 )1 1 = cos(2 ) 1 1 + 2 2 sin (2 )1 2 0]

3 = 2 + 2

2 3

Position of center of mass 1 relative to joint 1:

Liner velocity of center of mass 1:

Angular velocity of center of mass 1:

1 1 1 1 = 1 + 1 1 = 0 0

1 1

= [0 1

Position of center of mass 2 relative to joint 2:

2 2

1 = 1 = [2

0 0]

Liner velocity of center of mass 2: 2 = 2 + 2 Angular velocity of center of mass 2:

2 2

To calculate kinetic energy inertia tensors are needed: Inertia tensor of arm 1: 1 1 = 0 0 0 1 0

2 = 2

sin (2 )1 1 = cos(2 ) 1 1 + 2 2 sin (2 )1 2

Inertia tensor of arm 2:

Now energies can be computed. Kinetic energy of arm 1:

2 2 = 0 0

0 2 0

0 0 0 0 0 2 0 0 2

0 1 0 0 0 1

0 0 0 0 0 1

0 0 2

Potential energy of arm 1: 1 = 0

1 1 1 2 1 = 1 1 1 + 1 1 1 = 1 (1 1 2 + 1 ) 2 2 2

Kinetic energy of arm 2:

Position of center of mass 2 relative to origin:

0 2

1 2 1 2 1 2 1 2 + sin(2 )2 (2 2 2 + 2 ) + 2 2 2 2 + 2 1 2 2 cos (2 )1 2 2 2 2 cos(1 ) sin(2 ) 1 sin (1 ) = 2 sin(1 ) sin(2 ) + 1 cos (1 ) 2 cos (2 )

1 1 2 = 2 2 2 + 2 2 2 2 2

Potential energy of arm 2:

0 2 = 2 2 + 2 2 = 2 2 (1 + cos (2 ))

The total kinetic energy is:

The total potential energy is:

1 2 1 2 1 1 1 2 + 1 + 2 1 2 + sin(2 )2 (2 2 2 + 2 ) + 2 2 2 2 + 2 1 2 2 cos (2 )1 2 2 2 = 1 + 2 = 2 2 (1 + cos (2 )) = =

= 1 + 2 =

The Lagrangian is:

1 2 1 2 1 1 1 2 + 1 + 2 1 2 + sin(2 )2 (2 2 2 + 2 ) + 2 2 2 2 + 2 1 2 2 cos(2 ) 1 2 2 2 2 2 (1 + cos (2 )) The equation of motion of joint 1 is: + 1 1 = 1 1 1

This yields:

1 (1 1 2 + 1 + 2 1 2 + sin(2 )2 (2 2 2 + 2 )) 2 (2 cos(2 ) 1 2 ) 2 + 2 2 sin(2 )1 2 + 1 2 sin (22 )(2 2 2 + 2 ) + 1 1 = 1 + 2 2 = 2 2 2

The equation of motion of joint 2 is:

This yields:

Making the following substitutions (parallel axis theorem):

1 (2 cos(2 ) 1 2 ) + 2 2 2 2 + 2

1 2 sin(22 ) (2 2 2 + 2 ) 2 2 sin(2 ) + 2 2 = 2 2 1

 2 = 2 2 2 + 2 The simplified equations of motion are:

2 1 0 + 2 sin(2 )2 2 2 cos(2 ) 1 2 + 2 2 sin(2 )1 2 + 1 2 sin(22 ) 2 + 1 1 = 1

0 = 1 + 2 1 2

1 = 1 1 2 + 1

Separating 1 and 2 and rearranging terms yields:

1 2 cos(2 ) 1 2 + 2 2

1 2 sin(22 )2 2 2 sin(2 ) + 2 2 = 2 2 1

1 2 1 2 cos(2 ) 1 2 0 + 2 sin(2 )2 2 2 1 2 cos(2 ) 1 2 2 2 1 2 cos(2 ) sin(22 ) 1 2 + 0 + 2 sin(2 )2 2 1 2 2 2 2 sin(2 )0 + 2 sin(2 )2 2 2 sin(22 )0 + 2 sin(2 ) 1 2 1 2 2 2 2 2 2 1 sin(22 ) 2 2 = 2 2 2 2 1 2 cos(2 )2 + 0 + 2 sin(2 )2 2

1 2 1 2 2 1 2 cos(2 ) 2 2 1 2 2 sin(22 ) 2 1 2 cos(2 ) 2 1 2 + 1 1 2 2 1 2 cos(2 ) sin(22 ) 1 2 2 2 2 2 1 sin(22 ) 2 2 2 2 sin(2 ) 2 2 1 2 1 = 2 2 2 2 1 2 cos(2 )2 0 + 2 sin(2 )2 2

Linearization of the nonlinear model The last equations are nonlinear, they are correct but they are not suitable for the controller design, because the controller deals with linear equations, hence a linearization is needed. A Taylor linearization is employed and is about the fixed point = 0. At that point the system must be 2 stabilized (after the swing up). The basic procedure is to use a linear equation derived from the linear terms in a Taylor series expansion (high order terms are neglected) about the fixed point [x]: ( ) ( ) + | ( )

Where,

 is the vector of variables is the xed point

Applying the previous formula to our nonlinear equations and considering 2 additional states (, ) yields 1 2 to the following state space representation: 0 0 1 0 0 2 = 0 32 1 0 42
2

Where,

33 = 34 = 42 =

32 =

0 2 2 2 2 2 1 2 2 2 2 1 2 2 2 2 2 1 2 0 2 2 0 2 0

2 2 2 2 1 0 2 2 2 2 2 1 2 1 2

1 0 33 43

0 1 0 1 2 0 + 34 1 31 44 2 41

0 0 1 32 2 42

43 = 44 =

0 2 2 2 2 2 1 2 0 2 2 2 2 2 1 2 2 1 2 2 1 0 2 2 2 2 2 1 2

32 = 41 = 42 =

31 =

0 2 2 2 2 2 1 2 2 2 1 2 2 2 2 2 1 2 0 0 2 2 1 0 2 2 2 2 2 1 2

0 2 2 2 2 2 1 2

http://www.ece.jhu.edu/~pi/Courses/454/NotesA.pdf

An accurate model of the furuta pendulum has yet to be agreed upon. Even though the subject has been under study for a considerable time, yet due to the non-linear nature of the pendulum the system dynamics have been a problem. The model used here has been developed at the University of Adelaide, Australia. Some changes were made n the model, as will be discussed later. The model simulates the system dynamics of the pendulum with a complete inertia tensor by Lagrangian formulation. From that governing equations of the pendulum are derived. The lengths and mass of the arms were measured from the pendulum. In order to make the model as in figure 2, the masses were lumped. This was done using the moments of inertia concepts. The calculations are done in the matlab file named ParamsReal_edited.

Figure 4: Furuta Pendulum

Free fall Test: The model is found with viscous damping caused by the friction of the bearings. However we do not have the values of this damping. In order to have a model we are required to find a method to find these values. The free fall tests are a simple and effective means of finding these values. Tests of this nature can only be validated if they are reproducible, so we needed an effective reference point from where we could start the free motion of the pendulum. Inverting the pendulum to its topmost position is an effective strategy. It also induces a motion on the horizontal arm so we can use it to find the damping of both arms. A test response was taken by recording readings of the free fall. This response was then imitated by varying values of damping for both arms and simulating the model in Matlab. This gave us the required values.

It must be noted however that the time period of these vibrations will vary and this will depend on the length of the pendulum and the masses attached. This fact will be useful at the end of the report.

Model Normalization: The model uses some simple assumptions for its formulation. One of these assumptions is that the masses are considered to lumped masses. In order to accomplish this we need to normalize the model according to the real model. This is done by equalizing the real system as shown in Fig. 1 to that of the system as in Fig. 2.

Fig. 2 Fig. 1

To accomplish this we have to find the corresponding masses and lengths of Fig.1 from the given values of Fig.1. We also need to find the moment of inertia of the arms with these lumped masses and corresponding lengths.

The Moments of inertias are calculated according to their respective axis. However we need them to be about their pivot points. We use the parallel axis theorem to find this value. The parallel axis theorem states that if we have the moment of inertia I of a body of mass M about some axis, the moment of inertia of the body about another parallel axis, separated by a distance d is given by = +

Using this theorem we find the expression of the moments of each arm at the pivots as a function of the lumped masses m, distances separating the axes l and moments at original axes J. Thus we have the moment of arm 1 and 2 as
2 1 = 1 + 1 1

2 2 = 2 + 2 2

And the total moment that is acting on the origin as in the Fig. 2 is as follows
2 0 = 1 + 1 1 + 2 2 1

This is important as we want to have the mass as a variable and not fixed value. We will need this because we want the system to be able to adjust itself to a variation in the masses and lengths of the pendulum. Thus increasing the robustness of the system.

Procedure We used the nonlinear model from the said paper. To check if the model is working properly we made some checks using small displacements for both angles. At first the system showed an unstable behaviour. Upon 2 deriving the equations a change of sign was found in the model with respect to 1 Changing the sign made the system stable. After the implementation of the model we needed to apply the controller. The controller could only be applied at the linear range, so we had to choose a smaller operating range for it. The objective of the pendulum is to stay in the inverted position, so the fixed point was made to be at 180 from the rest position. An energy based controller was made to swing the pendulum and increase the

amplitude. Once the pendulum got close to the top the LQR controller was switched on. This was used to fine tune the control input and steady the pendulum at the top position.

Controllers As mentioned before the goal is to keep the furuta pendulum in the upward position. In order to achieve this, two controllers were implemented, one to swing the arm of the pendulum until it reached a position close enough to the inverted position and the second to keep it in the upward position in spite of perturbations. For the first an energy controller was implemented, while for the latter an LQR controller was chosen. Energy Controller Before stabilizing the pendulum, it must be in the stabilizing region. The energy controller drives the pendulum from the downward (initial) position to close to the upright position and with a sufficiently small velocity, conditions preferred to be able to switch to the stabilizing controller, in order to have a more accurate performance, thus the pendulum can be stabilized. How does the energy controller works?

The energy controller does not work in the entire domain of (the work region 2 can be selected), the objective is to let the energy controller dominate in the non linear region (green region), and accomplish stabilization by the LQR controller (blue region, small ). 2 The energy controller makes the pendulum oscillate (in the green region) until a desired level of energy is reached, given only by the potential energy (we desire a zero velocity) of the upright position. When the pendulum is about to enter the blue region, the energy controller switches off automatically, letting the stabilizing controller works. If a disturbance forces the pendulum again to the green region, the energy controller switches on and it will drive the pendulum again to the blue region
Figure 5: Working region of the controller

The controller scheme is shown in figure 4: As we can see, the input data are the state variables ( is not involved because potential energy of Arm 1 is 1 0), an Energy reference value which is the desired value of energy to achieve, and a constant gain K, which just multiply the output in order to have a stronger/weaker control signal. Inside the Treshold block, we can specify the region in which the energy controller will be switched on/off.

Figure 6: Controller scheme

Inside the Energy controller block we find

Figure 7: Energy Controller block The control law is given by: = ( ) 2 cos (2 )

The control law it is composed by 3 important parts:

A constant gain k, which just amplifies the control signal to make it sufficiently strong, this value is tunable. The direction of the controller is given by the sign of an error signal. At each sample instant the energy of the system is computed, then the error signal is calculated and the sign is extracted. Generally this sign is positive (because most of the time the pendulum is below the desired energy level).

A variable gain given by 2 cos (2 ), which tells the controller when to swing, i.e. the sign of the product of the cosine and the velocity tells if the controller has to push or pull to make the pendulum swing. The change between pulling and pushing occurs when the velocity goes from negative to positive or viceversa, or when the pendulum moves from the lower region to the upper region or viceversa (change in the sign of the cosine).

In figure 8 we can see the energy behavior, the blue line is the desired value to achieve, and the red one represents the actual energy of the system. We can notice that at the beginning, the energy is swinging (as pendulum does) and little by little it reaches de desired level, but it has an overshoot (due to the high velocity of ), but it recovers itself and drives the 2 pendulum again to the desired value without overshoot and now the LQR controller can be switched on.

Figure 8: Energy Behavior

Stabilizer Controller Once the energy controller has brought the arm near the upright position the next step is to activate a second controller which will maintain the pendulum in such position. For this purpose an LQR controller was implemented.

Linear Quadratic Regulator (LQR) The LQR is an optimal control technique which objective is to find a control input which brings the system back to its equilibrium as quickly as possible but with reasonable control effort. One way to achieve these requirements is evaluating a cost function with the following form. Where refers to the state variables of the system, the control input, Q is a symmetric, positive semidefinite matrix which determines the weight of each of the state variables and R is the penalization of the systems control effort. The values of the Q and R matrices should be chosen such that they meet the energy, velocity and final equilibrium position requirements. The result of minimizing the cost function is a full state feedback gain and the control input takes the form Where refers to a static optimal state feedback gain. () = () = ( + ) 0

(X)

Some advantages of this technique are that the controller is easy to implement, the gains are easy to calculate using a numerical computational program such as MATLAB and robustness is similar to a PID controller. On the other hand one of its disadvantages is that it assumes that all the states are available for feedback, which is usually not the case due to lack of sensors. To overcome this physical limitation an observer will be later designed.

LQR Design As already mentioned before the state feedback gains is obtained by minimizing (XX). For this purpose we need the linearized model of the pendulum (see chapter XX).

The next step is to define the effort weighting matrix and the control input penalization. Once these two matrices are defined we can minimize the cost function either by solving the Ricatti Equation or using a numerical computing environment such as MATLAB. For this case MATLAB was used and both matrices were defined as follows: 500 0 = 0 0 0 1000000000 0 0 0 0 85000 0 0 0 0 40000000

With both matrices and the linearized model loaded in the MATLAB workspace we can use the lqr command as follows K=LQR(A,B,Q,R); In this case this yields the following result. = [0.2236 367.3225 3.4294 67.7390]

= 10000

Finally a simulation was performed to validate the performance of the controller. The following image shows the response of the controller when applied an initial condition giving an acceptable response of the controller.

Figure 9: Simulation of the LQR controller

Problems encountered

During the LQR design some minor problems were faced. This method provides an easy and straightforward way to compute the gains of the feedback loop; nevertheless the selection of the weighting matrix Q and the control input penalization R werent that straightforward to choose because the matrices dont directly reflect how much they actually affect the dynamics of the system, contrary to other methods, like pole placement for example. To solve this, the matrices were initially proposed on an initial guess and with the help of the initial command in MATLAB the response of the system was simulated. Based on the simulated results we were able to tune the matrices in order to achieve a better performance for the controller. State Observer As discussed before due to the lack of velocity sensors an observer has to be implemented. An observer is basically a model of the real system that provides an estimation of the internal states given the input and the output of the system. A typical example of a system diagram for an observer is shown below.

Figure 10: Observer-based state feedback [XX] Where the elements inside the dashed rectangle belong to the plant while the rest of the elements belong to the observer.

State Observer Design To design the observer for the furuta pendulum the first step is to define the system matrices for the observer. The matrices were defined as follows = = 100

In the case of , it was defined as such because only two states out of the four were required. In this case we are interested in the positions so we can subtract them from the measured ones. The result is the error between the actual plant values and the calculated from the observer, giving a correcting term to the controller. Now that the matrices are defined the next step is to calculate the observer gain, but for that we need to make the observer dynamics faster than the plant dynamics, therefore we first compute the real poles of the system with the state feedback implemented and from those poles we can place the observer poles farther to the left in order to have an aggressive controller and assure that the observer can react correctly to disturbances. For such task we can use the following MATLAB command P=min(real(eig(Ao+Bo(:,1)*k))); Next we compute the desired observer poles from the smaller system pole. To assure that the observer will be faster than the plant we place the observer poles two times farther with the following command Po=2*P*ones(1,4)-[0 1 2 3]*1; Finally with the aid of pole placement we compute the observer gain and we get the following results L=-place(A0,Co,Po); 0.1502 0.0106 = 5.5518 0.7937 0.0095 0.1472 103 0.6484 5.4322

In this case is defined as B/100 because the DS1104 interface amplifies the output 10 times and also because we want to make the observer faster than the plant itself.

0 1 0 0 = 1 0 0 0

The observer along with the observer gains were implemented in Simulink as follows.

Figure 11: Simulink model of the observer