ISTANBUL TECHNICAL UNIVERSITY

MECHANICAL ENGINEERING DEPARTMENT

SYSTEM DYNAMICS & CONTROL

ADVANCED SYSTEM DYNAMICS & CONTROL

Term Project

The Control of Two-Wheels Balancing Robot

Team Members:

503101613 Barış Öz

503101605 İlkay Meşeli

503111606 Görkem Öztarlık

Instructor’s Name:

Prof. Dr. Ahmet Kuzucu

Delivery Date : January 20, 2012

ii

Table of Contents

1. Introduction ....................................................................................................................... 1

1.1. Control system and actuation equipment ..................................................................... 1

2. System Modeling ............................................................................................................... 2

2.1. Model of the DC motor ............................................................................................... 2

2.2. Dynamical model of the cart system ........................................................................... 3

3. Control System Design ..................................................................................................... 8

3.1. Pole Placement Control ............................................................................................... 9

3.2. Control Algorithm ..................................................................................................... 10

4. Original Nonlinear System ............................................................................................. 12

4.1. Actuator ..................................................................................................................... 13

4.2. Inclinometer ............................................................................................................... 13

4.3. Gyroscope .................................................................................................................. 13

4.4. Amplifier ................................................................................................................... 13

5. Finalizing The Control System Design .......................................................................... 14

5.1. Tuning Coefficients ................................................................................................... 14

5.2. Cost Analysis ............................................................................................................. 15

6. Conclusions ...................................................................................................................... 16

Appendix A : MATLAB Program ........................................................................................ 17

Appendix B – Simulink Model .............................................................................................. 19

References ............................................................................................................................... 20

1

1. Introduction

The balancing robot has become an interesting research area for robotic researchers around

the world. Its main concept is to have the ability to balance on its two wheels. As a naturally

unstable system it is a very good test bed for control theory experimentation and needs a

controller to keep it balanced all the time. It is basically based on the dynamics of an inverted

pendulum. The most popular balancing robot is Segway that is shown on Figure 1. On this

article, it will be shown that how a balancing robot can be designed and controlled with a pole

placement method.

Figure 1: Visual example of a segway robot.

1.1. Control system and actuation equipment

Control system development is vital to the balancing ability of the robot as it operates around

an unstable operating point. Basically two control strategies may be applied: linear control

and nonlinear control. Linear control systems linearises the system dynamics around an

operating point. This method is widely applied on inverted pendulum systems. Nonlinear

control may result in a more robust system but the complexity of the system makes it less

favorable. Structural diagram of the system is shown on Figure 2, it can be seen that the

control system is in regulator structure.

Figure 2: Structural diagram of the System

2

2. System Modeling

In order to control the system we need to obtain the differential equations of the system. The

differential equations obtained will be used to apply the state feedback control laws that will

balance the system.

2.1. Model of the DC motor

The robot is powered by a DC motor. The torque on the motor is controlled with the voltage

input. The visual representation of the motor is given in Figure 3.

Figure 3: Diagram of a DC motor

When the control voltage V is applied to the terminals of the motor a current i is generated in

the armatures. The torque produced τm is proportional to the current generated. The

mathematical representation is;

(1.1)

The motor can be modelled with a series inductor resistor pair and a voltage Vemf. The

electromotive force voltage can be expressed as;

(1.2)

Now we can acquire the linear differential equation for the dc motor's electrical circuit;

(1.3)

By approximating the friction as a linear function of the angular velocity of the shaft we can

find the acceleration of the shaft as a function of applied voltage;

∑ (1.4)

Substituting equation (1.1) and (1.2) into equations (1.3) and (1.4), and rearranging in terms

of the time derivatives, leads to the following two fundamental equations which governs the

motion of the motor.

(1.5)

(1.6)

If we further simplify by approximating the motor inductance and motor friction to zero;

3

(1.7)

(1.8)

(1.9)

2.2. Dynamical model of the cart system

The cart system with two wheels possesses similar dynamic behavior to the pendulum on a

cart problem.

Figure 4: Free body diagram of the wheels

If we sum the forces on the horizontal direction;

∑

(1.10)

Summing the forces around the center of the wheels gives

∑

(1.11)

From DC motor dynamics the motor torque is;

(1.12)

By rearranging the equation and substituting the parameters from the DC motor derivation

section, the output torque to the wheels is attained;

(1.13)

Therefore, equation (1.11) becomes

4

(1.14)

Thus;

(1.15)

Equation (1.13) is substituted into (1.10) to get the equation for the left and right wheels

For the left wheel;

(1.16)

For the right wheel;

(1.17)

Because the linear motion is acting on the centre of the wheel, the angular rotation can be

transformed into linear motion by simple transformation,

By the linear transformation, equation (1.18) and (1.19) becomes;

For the left wheel,

(1.18)

For the right wheel,

(1.19)

Adding equation (1.18) and (1.19) together yields,

(

)

( ) (1.20)

The robot’s chassis can be modeled as an inverted pendulum, Figure 5 shows the free body

diagram of the chassis.

5

Figure 5: Free body diagram of the chassis

Again, by using Newton’s law of motion, the sum of forces in the horizontal direction,

∑

( ) (1.21)

Thus,

( ) (1.22)

The sum of forces perpendicular to the pendulum,

∑

( ) ( ) (1.23)

Sum of moments around the centre of mass of pendulum,

∑

( ) ( ) ( ) (1.24)

The torque applied on the pendulum from the motor as defined in equation (1.13) and after

linear transformation,

Substituting this into equation (1.24) gives,

( ) ( ) (

)

thus,

( ) ( )

(1.25)

6

Multiply equation (1.23) by –l,

[ ( ) ( ) ] (1.26)

Substitute equation (1.25) in equation (1.26),

(1.27)

To eliminate (HL+HR) from the motor dynamics, equation (1.22) is substituted into equation

(1.20),

(

)

(1.28)

Rearranging equations (1.27) and (1.28) gives the non-linear equations of motion of the

system,

( )

(1.29)

(

)

(1.30)

The above two equations can be linearised by assuming θp =π + , where represents a

small angle from the vertical upward direction. This simplification was used to enable a linear

model to be obtained so linear state space controllers could be implemented.

Therefore,

and (

)

The linearised equation of motion is,

( )

(1.31)

(

)

(1.32)

In order to get the state space representation of the system, equations (1.31) and (1.32) are

rearranged,

( )

( )

( )

( ) (1.33)

( )

( )

( )

(1.34)

By substituting equation (1.33) into equation (1.32), substituting equation (1.34) into equation

(1.31) and after a series of algebraic manipulation the state space equation for the system is

obtained.

7

[

]

[

(

)

( )

]

[

]

[

(

)

( )

]

(1.35)

Where,

(

) [

(

)]

In the model above, it is assumed that the wheels of the vehicle will always stay in contact

with the ground and that there is no slip at the wheels. Cornering forces are also considered

negligible.

8

3. Control System Design

With given coefficients [2]

, state space matrices of the system can be written in numerical.

[

] ; [

]; [

] ; [ ]

As seen from system matrix A, the system has no sensitivity on first state variable which is ,

position of the balancing robot.

The balancing robot system has unstable system dynamics. It can be seen with analysing its

impulse response and pole/zero map. Figure 6 shows the simulation when an impulse input is

applied to the uncontrolled system and Figure 7 shows the pole-zero map of the uncontrolled

system. As known, ideally, all poles should be on the left-hand plane of the imaginary axis.

Figure 6: Uncontrolled System Behaviour

0 0.5 1 1.50

1

2

3

4

5

6x 10

4

Angle

Position

9

Figure 7: Pole-Zero Configuration of the Uncontrolled System

3.1. Pole Placement Control

The position of poles defines the stability of a system. According to linear control theory, the

poles of the system can be placed in the complex plane if the controllability matrix is of full

rank.

[ ] (2.1)

With a Matlab command, rank of the controllability matrix can be found.

rank(ctrb(A,B))

Rank of the controllability matrix is 4, which shows the system is completely controllable.

The control law for the pole placement controller is given as

(2.2)

Where u is the control voltage, x is the state parameters and K is the state feedback gain

matrix. Pole placement control gives designers ability of reshaping closed loop poles of the

system in order to translate it into a stable system. The most important part of this control

algorithm is placing the roots on optimum desired locations. So that, the best pole location

that gives thedesired response can be achieved by several simulation trials using MATLAB.

Stability of the system can be guaranteed as long as all the poles of the system are in the left

hand side of the imaginary axis at pole-zero configuration map. Because of structure of the

system, a fast response should be desired naturally, so that, the poles should be placed further

away from imaginary axis. It should not be forgotten that the further poles are placed at the

left hand plane, the system will require a faster or a stronger actuator to perform the task and

naturally construction cost of the system will become much greater. So, poles should be

chosen carefully.

-15 -10 -5 0 5 10 15-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

Pole-Zero Map

Real Axis

Imagin

ary

Axis

10

3.2. Control Algorithm

The control law of the state-feedback control is given as

( ) (2.3)

( ) (2.4)

The block diagram of the system with structure of the controller is shown on Figure 8.

Figure 8: Block Diagram of the System

The eigenvalues of ( ) matrix will be translated into desired values in order to control

the system. As long as the system matrix, A, has positive eigenvalues, the system is unstable.

With given coefficients, eigenvalues of the original system matrix, A, are;

Desired poles will be placed at .

Figure 9: Pole-Zero Configuration of the Controlled System

xDot x y

Kx

Scope

1

s

Integrator

K

Gain3

C

Gain2

A

Gain1

B

Gain

U

From

Workspace

-25 -20 -15 -10 -5 0-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

Pole-Zero Map

Real Axis

Imagin

ary

Axis

11

With a simple command in MATLAB, state feedback gain matrix, K can be found.

K=place(A,B,[-10 -15 -20 -25])

[ ]

Under these circumstances, simulation of the controlled system with an impulse function of

input can be done. System response with poles placed to desired places is shown on Figure

10.

Figure 10: Controlled System Response

To control the system, a torque must be obtained. This torque value can be extracted from

algorithm. The torque needed to control the system related on time has plotted on

Figure 11.

Figure 11: Control Torque

0 0.5 1 1.5-3

-2

-1

0

1

2

3

4

5x 10

-5

Angle

Position

0 0.5 1 1.5-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

Time [s]

Contr

ol u

12

4. Original Nonlinear System

A Simulink model has to be created to simulate original system with (1.29) and (1.30)

equations. Balancing Robot Nonlinear Model subsystem has given in Appendix B. Some

coefficients have assumed to simplify equations.

Figure 12 – Simulink Model

With using the controller coefficients that are found in Chapter 3.2, output values can be

plotted as Figure 13.

Figure 13 – Controlled System Response

t

To Workspace3

us

To Workspace2

ythetas

To Workspace1

yxs

To Workspace

Pulse

Generator

K*uvec

Gain

Clock

Va

xxdot

thetathetadot

Balancing Robot

Nonlinear Model

13

4.1. Actuator

The actuation to balance the robot is via a DC Servo Motor made by Faulhaber. 4490 048 BS

brushless DC servomotor is desired to be used. It has a back-e.m.f constant of , a

torque constant of , and a armature resistance of ohm. Datasheet has been

attached to the project CD. Fair and reasonable market value for this motor is 280 €.

4.2. Inclinometer

An Analog Devices ADIS16210 triaxial, digital inclinometer

system that provides a measurement range of degrees can be

convenient to use. It has a market value of 145 €.

4.3. Gyroscope

An Invensense ITG-3200-IC-Gyro-3Axis Gyroscope is

desired to be used in this project. It is a MEMS gyro and it

has three 16-bit ADCs for digitizing the gyro outputs. Also it

has a user-selectable internal low-pass filter bandwidth. It has

a supply voltage range of to V. Fair market value for

this gyroscope is 20 €. Its datasheets are given in project CD.

4.4. Amplifier

Since designing a full state observer, four operational amplifiers are needed. As seen on the

providers pages, some of definitions are appeared. The most important thing which existed

between them is gain-bandwidth (GBWP). If the GBWP of an operational amplifier is 1 MHz,

it means that the gain of the device falls to unity at 1 MHz. Hence, when the device is wired

for unity gain, it will work up to 1 MHz (GBWP = gain × bandwidth, therefore if

BW = 1 MHz, then gain = 1) without excessively distorting the signal. The same device when

wired for a gain of 10 will work only up to 100 kHz, in accordance with the GBW product

formula. Further, if the maximum frequency of operation is 1 Hz, then the maximum gain that

can be extracted from the device is . According to K matrix, it can be decided which

products are going to be selected. For this project, four LM101A amplifiers that made by

National Semiconductor are selected. Market price per product is 15€, so that, amplifiers cost

60 € for this project. LM101A datasheet is given in project CD.

Figure 14 - Inclinometer

Figure 15 - Gyroscope

14

5. Finalizing The Control System Design

In order to finalize the control system design, transfer functions of controllers, actuators and

measurement devices need to be add into original system. Actuator dynamics are already

included to the system. And actuator dynamics contains motor encoder transfer function too.

So that, there is no need to introduce new transfer functions for actuator and measurement of

variable . Also, because of using linear state feedback controller, there is no need to

introduce controller dynamics into system.

As measurement devices, an inclinometer and a gyroscope are used as discussed at the

previous chapter. Both of them is modeled as first-order systems and have these transfer

functions.

,

Figure 16 shows simulink model after entegrating measurement devices into the original

system.

Figure 16 – Simulink Model

Figure 17 – Inclinometer TF

Figure 18 – Gyroscope TF

5.1. Tuning Coefficients

As seen at Figure 13, the system went through steady-state condition at nearly 1 s. In order to

investigate the system better, desired closed loop poles need to be translated into righter at

real axis. As new desired closed-loop poles, -4 -6 -8 -10 are selected. Under these

circumstances, after measurement devices transfer functions are included to the simulink

model, controlled system response and necessary input voltage can be plotted.

t

To Workspace3

us

To Workspace2

ythetas

To Workspace1

yxs

To WorkspacePulse

Generator

In1 Out1

InclinometerIn1

Out1

Gyroscope

du/dt

Derivative

In1

In2

In3

In4

Out1

Controller

Clock

Va

x

theta

thetadot

Balancing Robot

Nonlinear Model

1

Out1

1

0.01s+1

Transfer Fcn

1

In1

1

Out1

s

s+0.5

Transfer Fcn

1

In1

15

Figure 19 – Controlled System Response and Input Voltage

5.2. Cost Analysis

With operational amplifiers, actuator and measurement devices, cost is approximately 400 €

without the original system cost. The total cost will be nearly 440 €.

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-5

-4

-3

-2

-1

0

1

2

3

4x 10

-4

Time [s]

Y (

Contr

olle

d)

x

theta

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-10

-5

0

5

10

15

20

16

6. Conclusions

In this project, linear full state feedback control has applied into a balancing robot system.

One important thing to discuss is that K, gain matrix which computed from linearized system,

is also usable for original nonlinear system.

This project can be improved with using two DC motors as actuators instead of one DC

servomotor. So that, two encoders can be introduced into the system to measure position and

velocity of the motors more accurate at the output instead of measuring them directly from

servomotor encoder.

This control process aims to balance the robot at zero position difference. In order to have

more realistic performance, with respect to industrial processes, it is better to analyse the

system under different input types.

Also, measurement devices are considered as ideal, that means no noise effects on

measurement devices. If a White-Gaussian noise is added into theinclinometer and gyroscope,

with modelling them explicitly in Simulink, more realistic model can be obtained.

17

Appendix A : MATLAB Program

clear clc % Coefficients g=9.81; r=0.051; Mw=0.03; Mp=1.13; Iw=0.000039; Ip=0.0041; l=0.07; %Motor Coefficients Km=0.000428; Ke=0.0004171; R=5.3; beta=(2*Mw+(2*Iw/r^2)+Mp); alpha=(Ip*beta+2*Mp*l^2*(Mw+Iw/r^2)); %Linear Model %Original System Matrices A=[0 1 0 0; 0 (2*Km*Ke*(Mp*l*r-Ip-Mp*l^2))/(R*r^2*alpha) (Mp^2*g*l^2)/alpha 0; 0 0 0 1; 0 (2*Km*Ke*(r*beta-Mp*l))/(R*r^2*alpha) (Mp*g*l*beta)/alpha 0]; B=[0; (2*Km*(Ip+Mp*l^2-Mp*l*r))/(R*r*alpha); 0; (2*Km*(Mp*l-r*beta)/(R*r*alpha))]; C=[0 0 1 0; 1 0 0 0] D=[0;0]; %%Simulation time step T=0:0.02:5; %%Impulse response input U=zeros(size(T)); U(1)=1; %%Original System [Yun,Xun]=lsim(A,B,C,D,U,T); %%Pole Placement Control Design disp('Eigenvalues of the original A matrix are') p=eig(A); disp(p) rank_=rank(ctrb(A,B)); K=place(A,B,[-4 -6 -8 -10]); disp('Eigenvalues of the controlled A matrix are') p_=eig(A-B*K); disp(p_) %%Simulate the system %%Controlled System Matrices Ac=[(A-B*K)]; Bc=[B]; Cc=[C]; Dc=[D]; %Obtaining the states and the output response [Y,X]=lsim(Ac,Bc,Cc,Dc,U,T); %Control Torque [n m]=size(X); for i=1:n Ucon(i)=-K*X(i,:)'; end

18

%Plotting subplot(2,3,1) plot(T,Yun), xlabel('Time [s]'),ylabel('Y (Uncontrolled)') subplot(2,3,2) pzmap(ss(A,B,C,D)) subplot(2,3,3) pzmap(ss(Ac,Bc,Cc,Dc)) subplot(2,3,4) plot(T,Y), xlabel('Time [s]'),ylabel('Y (Controlled)') subplot(2,3,5) title('Impulse Response of the system with Pole Placement control') plot(T,[X(:,1) X(:,2) X(:,3) X(:,4)]), xlabel('Time [s]'), legend('x','xDot','phi','phiDot') subplot(2,3,6) plot(T,Ucon), xlabel('Time [s]'),ylabel('Control u') %Nonlinear System p1=Ip+Mp*(l^2); p2=(2*Km*Ke)/(R*r); p3=(2*Km)/R; p4=Mp*g*l; p5=Mp*l; p6=(2*Km)/(R*r); p7=(2*Mw)+((2*Iw)/(r^2))+Mp; p8=(2*Km*Ke)/(R*(r^2)); p9=((p5^2)/(p1*p7)); sim('BalancingRobot'); figure subplot(1,2,1) plot(t,ythetas),xlabel('Time [s]'),ylabel('Y (Controlled)') hold all plot(t,yxs) legend('x','theta') subplot(1,2,2) plot(t,Ucon2)

19

Appendix B – Simulink Model

the

tad

ot

xd

ot

co

s(th

eta

p)

co

s2

(th

eta

p)

p9

co

s2

(th

eta

p)

1-p

9co

s2

(th

eta

p)

1/(

1-p

9co

s2

(th

eta

p))

sin

(th

eta

p)

4

the

tad

ot

3

the

ta

2

xd

ot

1 x

-K-

p8

/p7

-K-

p6

/p7

-K-

p5

p8

/p1

p7

-K-

p5

p6

/p1

p7

p9

p5

p6

/p1

p1

-K-

p5

p4

/p7

p1

-K-

p5

p3

/p1

p7

-K-

p5

p2

/p7

p1

-K-

p5

/p7

-K-

p4

/p1

-K-

p3

/p1

-K-

p2

/p1

co

s

Tri

go

no

me

tric

Fu

ncti

on

3

sin

Tri

go

no

me

tric

Fu

ncti

on

2

sin

Tri

go

no

me

tric

Fu

ncti

on

1

co

s

Tri

go

no

me

tric

Fu

ncti

on

Pro

du

ct9

Pro

du

ct8

Pro

du

ct7

Pro

du

ct6

Pro

du

ct5

Pro

du

ct4

Pro

du

ct3

Pro

du

ct2

Pro

du

ct1

1

Pro

du

ct1

0

Pro

du

ct1

Pro

du

ct

u2

Ma

th

Fu

ncti

on

1

u2

Ma

th

Fu

ncti

on

1 s

Inte

gra

tor3

1 s

Inte

gra

tor2

1 s

Inte

gra

tor1

1 s

Inte

gra

tor

p9

Ga

in

Div

ide

1

Co

nst

an

t

Ad

d

1 Va

20

References

[1] Dorf, Richard C.; Bishop, Robert H., Modern Control Systems 11th Edition, Pearson

International Education, 2008.

[2] Nasir, A.N.K.; Ahmad, M.A.; Raja Ismail, R.M.T., The Control of a Highly Nonliner

Two-Wheels Balancing Robot: A Comparative Assessment between LQR and PID-PID

Control Schemes, World Academy of Science, Engineering and Technology 70, 2010

[3] Abdulkadir, Herdawatie B., Modelling And Control of a Balancing Robot Using Digital

State Space Approach, Faculty of Electrical Engineering, Universiti Teknologi

Malaysia, 2005.

[4] Tewari A., Modern Control Design with MATLAB and Simulink, John Wiley & Sons

LTD, 2002.

[5] Anderson, D.P, nbot, a two wheel balancing robot (Online),

http://www.geology.smu.edu/~dpa-www/robo/nbot/ , 2003

[6] Mesa A., Zamora C., Preliminary Design Report with Diagrams : Omnibot, Advances

Self-Balancing Robot, University of Florida, Electrical & Computer Engineering, 2011

[7] National Semiconductor [Online], http://www.national.com/

[8] Invensense, [Online], http://www.invensense.com/

[9] Analog Devices,[Online], http://www.analog.com/en/index.html

[10] Craig,K.,Sensor Fusion, Rensselaer Polytechnic Institute, 2004.

[11] Callafon, R.A., Gyroscope Control Experiment, Department of MAE, 2012

[12] An Opamp Gain Bandwidth Product,[Online],

http://masteringelectronicsdesign.com/an-op-amp-gain-bandwidth-product/