ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 21 of 68
Alternate Strategy Considered
We experienced difficulty integrating the motor model into the control the pendulum system itself
could be controlled with a torque control signal. Two areas of concern: 1) The control output ultimately
needs to be in terms of a voltage (input to the motor). We tried creating a conversion block that
accepted a desired torque and the current shaft speed and calculated the motor voltage needed to
produce the desired torque. 2) The load torque (the force applied to the motor shaft by the pendulum
arms) is not accurately modeled.
We addressed both concerns at once by creating a state space model of the entire plant (motor and
pendulum arms). To do this, we also needed an equation that couples the motor equations to the
pendulum equations. In consultation, Dr. Cheok suggested considering a compliance factor that
relates the shaft angle at the motor to the measured (or computed) shaft angle. The difference in these
two quantities creates a load torque on the motor shaft.

(16)
Where

is a compliance factor that describes the twistability (torsional springiness) of the motor
shaft.
In the Simulink model, this torque is fed back into the motor subsystem as external torque.
The state variables for the entire system are

. Combining
equation (16) with the pendulum and motor equations, we obtain a seventh order state space
representation of the system (see Appendix B Derivation of the seventh order state space modelfor
the derivation):

0 0 1 0 0 0 0
0 0 0 1 0 0 0

0 0

0 0

0 0

0 0
0 0 0 0 0 1 0

0 0 0

0 0 0 0 0

0
0
0
0
0
0

(17)
Ultimately, we could not reconcile the compliance factor (we didnt have a value for it and had no way
to determine the value experimentally), so we looked for other methods of integrating the motor into
the system.
Simulating the system under control
Matlab/Simulink was used to simulate the proposed control system. The control system was initially
written to just control balance, but was eventually expanded to control both swing up and balance with
automatic switching between the two. The following Simulink diagram shows the overall control.
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun
Page 22 of 68

Figure 16 - Swing up and balance control blocks
The approach taken in defining physical constants was to use variables to define the constants in the
system and load those constants into the model as part of initialization using Matlab functions. This
approach allowed us to use estimated values before the physical model was actually built and then to
quickly and accurately switch to actual physical parameters when the Pendubot was actually built.
The following block shows the control for LQR Control.

Figure 17 - LQR control
1
u
Swi tch
elseif { }
theta1
theta2
theta1d
theta2d
theta1_pos
u
Parti al Li neari zati on Control
else { }
theta1
theta2
theta1d
theta2d
theta1_pos
theta2_pos
K
u
LQR Control
u1
if (u1 == 0)
elseif (u1 <= 4)
else
If
8
K
7
theta2_pos
6
theta1_pos
5
theta2d
4
theta1d
3
theta2
2
theta1
1
Control State
theta1
theta2
theta1d
theta2d
K
theta1_pos
<K>
<theta1>
<theta2>
<theta1d>
<theta2d>
<theta1_pos>
<theta1>
<theta2>
<theta1d>
<theta2d>
<theta1_pos>
theta2_pos
<theta2_pos>
1
u
-1
Gain1
Gain
el se { }
Acti on Port
7
K
6
theta2_pos
5
theta1_pos
4
theta2d
3
theta1d
2
theta2
1
theta1
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun
Page 23 of 68
Partial Linearization Control was used for swing up control during the simulation. This technique was
borrowed from Spongs paper on swing up control [1] Damon Lavrinc. (2009, September) Tokyo
Preview: Honda unveils U3-X personal mobility device - Autoblog. [Online].
http://www.autoblog.com/2009/09/24/tokyo-preview-honda-unveils-u3-x-personal-mobility-device
[2] E. J. Dionne Jr., "Trident Missile Explodes in Test; Failure Is 2d of 3 Sea Launchings," The New York
Times, August 16, 1989.
[3]. Spongs system used an Acrobot, but the equations are easily modified for a Pendubot.

Figure 18 - Partial linearization control
Successfully implementing swing-up control (even in simulation) is a trial and error process. The control
needs to provide enough energy to swing the pendulum to a position where the LQR control is in a
linear region and the control can switch to LQR control, but not too much energy or the LQR control isnt
stable. We were able to implement swing-up control successfully in simulation but we didnt have
enough time to be successful in the real world.
The overall control of the system was done by the Matlab m file SetControlState.m listed in Appendix C
Program Listings. This program is responsible for taking input from the user (in the form of switches)
and translating those into the desired control (swing-up, balance) and the desired position of the
pendulum (down-down, down-up and up-up).
Implement the physical system
Implementing the physical system was among the most frustrating parts of the project. The building of
the Pendubot was initially delayed due to sickness of the machinist. Then the original tube design of the
Pendubot did not meet our requirements. We finally were able to meet with the machinist and relayed
our requirements. Unfortunately, the first implementation of the design still would not work (the
encoder shaft was not fixed in relation to the upper arm). Finally, we received a Pendubot that would
work, although the joint wasnt as strong as it should be. In fact the joint did fall apart several times
during testing.
1
u
v1
phi
theta1
theta1d
phi
theta2
theta2d
A3*u(5)^2*si n(u(3)) - A2*B3/B1*u(2)^2*cos(u(3))*sin(u(3)) + A4*si n(u(1)) - A2*B4/B1*cos(u(3))*sin(u(4))
Li neari zi ng functi on
(A1*B1 - A2*B2*cos(u(2))^2)/B1*u(1)
Lineari zed function
200
Kp
10
Kd
Add
el sei f { }
Action Port
5
theta1_pos
4
theta2d
3
theta1d
2
theta2
1
theta1
theta2
v 1
theta2d
phi
theta1d
theta1
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun
Page 24 of 68
Our approach for determining what angle the arms were in relation to the upright position was to use
encoders. One encoder was supplied on the motor we were provided with. The other encoder was
purchased and attached to the Pendubot joint.
An H-Bridge for controlling the motor was provided to our team in kit form. We assembled the H-Bridge
and after a minor issue with the assembly (the surface mount current sensing chip was not making
contact with the board), we were able to successfully calibrate the board.
Another problem with the implementation was that the base initially wasnt as stable as it needed to be
either. We initially placed the motor on a plexiglass base, but this base didnt prove to be strong or
heavy enough to be stable when the Pendubot went out of control.
Yet a third problem with the physical system was that the H-Bridge we were using proved to have a
defective part. After replacing the H-Bridge with a good one, we were able to control the system.
Interfacing the motor and encoders with MATLAB
The motor is controlled with a command voltage and a digital direction line via the NIDAQ. The NIDAQ is
controlled by MATLAB library functions provided by National Instruments.
% SetHBridgeVoltage.m
% Author: John Eisenbraun
%
% Function: SetHBridgeVoltage
% Input: The voltage to set the H-Bridge to
% Output: None
%
% Input is a voltage and sign. The sign sets the H-Bridge digital output
% and the magnitude goes to the voltage (after being scaled).
%
function [i] = SetHBridgeVoltage(u)

global AO AI DIO;

% Output the value and sign

if u(2) == 0 % ControlState == 0
putsample(AO, 0);
else
Direction = u(1) >= 0;
putvalue(DIO, Direction);
output = abs(u(1)) * 5/24;
putsample(AO, output);
% i = getsample(AI);
end
i = 0;
end
Figure 19 function to set the motor command voltage and direction
Our initial design relied on the NIDAQ to decode the encoder quadrature signals. However, it quickly
became evident that MATLAB was not going to execute fast enough (in real time) to continuously
monitor and decode the signals. Our solution was to employ a small auxiliary processor with the sole
function of decoding quadrature signals and counting pulses. After a few false starts with a dsPIC and a
PSoC microcontroller, we successfully implemented the function on a TI TMS320 controlSTICK. The
processor counts edge transitions on the A/B encoder lines and communicates the current count when
queried via a Serial Communications Interface (routed through a virtual COM port on the PC). In
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun
Page 25 of 68
addition, the processor computes the present speed of each encoder (in counts per millisecond). A
source code listing can be found in Appendix C.
Although this solution worked, it introduced undesirable delays between the physical world and the
control due to Matlabs slowness in processing data from the serial port. This delay almost certainly
affected our ability to control the Pendubot in the upright position.
The physical system under control
After overcoming the difficulties detailed above, we were able to control the physical system in the
down-down position using our designed control. The Q values of the LQR control needed to be
modified for the physical system but after some tuning the control behaved very similarly to how it did
during simulation.
A video of the successful control is submitted as part of the project deliverables.
Unfortunately there was insufficient time to successfully show balance control in either (down-up or up-
up) positions. Control was attempted (once) in the down-up position, but we were unable to maintain
control. We did not attempt to do swing-up, although we did test that the basic swing-up control
(partial linearization) did work.

ECE 572 Term Project Al-Refai, Bowman, Eisenbraun
Page 26 of 68
References
[1] Damon Lavrinc. (2009, September) Tokyo Preview: Honda unveils U3-X personal mobility device -
Autoblog. [Online]. http://www.autoblog.com/2009/09/24/tokyo-preview-honda-unveils-u3-x-
personal-mobility-device
[2] E. J. Dionne Jr., "Trident Missile Explodes in Test; Failure Is 2d of 3 Sea Launchings," The New York
Times, August 16, 1989.
[3] Mark W. Spong, Partial Feedback Linearization of Underactuated Mechanical Systems
Bibliography
Bryan Kappa, State Space Control Design of a Dual Inverted Pendulum,
www.bryankappa.com/Resume/Kappa_MSME_Paper.pdf , retrieved 2/10/2011
Dufour Aurelie, et. al., Final Report Pendubot, www.e.kth.se/~aurelied/Finalreport.pdf, retrieved
2/11/2011
ece.ut.ac.ir/SMRL/members/Sharifi/Inverted%20Pendulum.pdf, retrieved 2/11/2011
Gene F. Franklin, J. David Powell, and Abbas Emami-Naeini, Feedback Control of Dynamic Systems, 3rd
ed.: Addison-Wesley Publishing Company, 1994.
Karl Johan Astrm and Richard M. Murray, Feedback Systems: An Introduction for Scientists and
Engineers, Electronic Edition ed.: Princeton University Press, 2009.


ECE 572 Term Project Al-Refai, Bowman, Eisenbraun
Page 27 of 68
Appendix A Derivation of non-linear equations of motion
Nomenclature

mass of link

moment of inertia of link

length of link

distance from pivot to center of mass for link

angle from vertical of link

external torque applied (by control)

A systems dynamics can be described by Langranges equations:

(A1)

nonconservative torques
(A2)
where

, 1. . is a convenient set of parameters, is the total kinetic energy of the system, and
is the total potential energy.

is called the Lagrangian of the system. For the pendubot, we

choose the parameters

to be

.
To find the Lagrangian, we will first determine the kinetic and potential energies.
Kinetic energy
The kinetic energy of each of the links is:

1
2

(A3)

1
2

1
2

(A4)
where

sin

sin

cos

cos

cos

cos

sin

sin

(A5)
is the translational velocity of the center of mass of link 2. Then,
|

cos

cos

cos

cos

sin

sin

sin

sin

cos

(A6)
The total kinetic energy is

1
2

1
2

1
2

cos

(A7)
Potential Energy
Potential energy, due to gravity, is:

cos

cos

cos

(A8)
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun
Page 28 of 68

cos

cos

Lagranges Equations
Subtracting the potential from the kinetic energy gives us the Lagrangian.


1
2

1
2

1
2

cos

cos

cos

(A9)

We now compute the derivatives according to equation (A2) to form the equations of motion for the
system.

(A10)

0
(A11)

cos

cos

sin

cos

sin

cos

sin

sin

(A12)

sin

sin

(A13)

cos

sin

sin

sin

sin

(A14)

cos

sin

sin

(A15)

cos

cos

sin

cos

sin

cos

sin

sin

(A16)

sin

sin

(A17)
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun
Page 29 of 68

cos

sin

sin

sin

sin

0
(A18)

cos

sin

sin

0 (A19)

To simplify equations (A15) and (A19), we introduce lumped parameters:

cos

sin

sin

(A20)

cos

sin

sin

0 (A21)
where

(A22)

To take advantage of a state space representation of the system, we can isolate

and

in each of the
equations (20) and (21); substitute and rearrange to obtain two functions of the four state variables

cos

sin

sin

(A23)

cos

sin

sin

(A24)
Substituting equation (A23) into (A20) we get:

cos

sin

sin

cos

sin

sin

cos

sincos

sin

cos

sin

sin

cos

sincos

sin

cos

sin

sin

cos

sin2

sin

cos

sin

sin

(A25)

Likewise, substituting equation (A22) into (A21):

cos

sin

sin

cos

sin

sin

0
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun
Page 30 of 68

cos

sincos

sin

cos

cos

sin

sin

cos

sin2

sin

cos

sin

sin

cos

sin2

sin

cos

sin

sin

(A26)

Equations (A20) & (A21) in matrix form:

cos

cos

sin

sin

sin

sin

(A27)

cos

cos

sin

sin

sin

sin

(A28)

cos

cos

cos

sin

sin

sin

sin

(A29)
Linearization
We choose state variables

and rewite (A29) in state space form:

(A30)
where

(A31)

0
0

(A32)

In general, a linear approximation of near a point can be obtained by an expansion of the first
term of a Taylor series:

(A33)
where