Design and Implementation of Real Time Controller

for Self Balancing Robot

Muhammad Usman Asad1, Umar Farooq2, Athar Hanif3, Mahmood ul Hasan4
1
Student Member IEEE
[email protected]

Abstract—In this paper modeling and control of a two wheeled and rotate according to signals sent. The prototype used in this
balancing autonomous robot is reported. The main focus is on work relies on a dead-reckoning multi-sensor system, which will
developing efficient control algorithms required to enable the robot consist of i) two optical incremental encoders mounted on the
to perceive and act in real time for a dynamically changing world. motor shafts, to sense the relative angular positions and angular
The prototype relies on a multi-sensor system consisting of i) two velocities of the wheels and ii) a 3-Axis accelerometer tilt sensor
optical incremental encoders and ii) a three axis accelerometer
to measure the inclination and angular velocity. The code is
which is used as a tilt sensor. The information from the sensory
system is provided to the robot controller. The performance and written in C and compiled for the Atmel AT89C52
reliability of controller is then simulated in MATLAB® as a microcontroller, which is interfaced with the sensors and
platform. Finally, the solution is verified on a real physical model motors. The main goal of the microcontroller is to fuse the
controlled by means of a single chip AT89C52 microcontroller. wheel encoder and accelerometer sensors to estimate the
behaviour of the platform and then to use this information to
Keywords— Simulation, dynamics, inverted pendulum mobile drive the wheel in the direction to maintain an upright and
robot, state space control balanced position.

I. INTRODUCTION II. ROBOT DESIGN

Effective and efficient control system designs provide the The physical layout of the robot is shown in Fig 1 and the
robot with the ability to control itself and operate autonomously. assembled robot is shown in Fig 2.. The robot’s frame consists
Two wheeled robots are one variation of robot that has become of two platforms that allow room for the placement of the
a standard topic of research and exploration for young engineers electronic equipment. It is important to consider the overall
and robotic enthusiasts. They offer the opportunity to develop weight of the robot because base and the wheels might bend
control systems that are capable of maintaining stability of an outward due to the overall weight of the components. The entire
otherwise unstable system. This type of system is also known as component contributed in the overall weight is tabulated in the
an inverted pendulum [1]. following Table 1.
Two wheeled balancing robot is a classic engineering problem TABLE 1
based on inverted pendulum and is much like trying to balance a PARAMETER OF ROBOT
broom on the tip of your finger. The word balance means the Item Quantity kg
inverted pendulum is in equilibrium state, which its position is
Lead Acid batteries 3 1.19
like standing upright 90 degrees. However, the system itself is Nylon material (sheets) 2 0.65
not balance, which means it keeps falling off, away from the Printed circuit board 1 0.35
vertical axis. Therefore, a sensor is needed to provide the angle Motors, Bearings & couplings 2 1.5
position of the inverted pendulum or robot base and input into
the microcontroller, which the program in itself is a balancing Material that is used for building the chassis is polestar which is
algorithm. The microcontroller will then provide a type of a strong, light and affordable. This setup is taken to ensure the
feedback signal through PWM control to the H-bridge circuit to portability of the robot [3].
turn the motor clockwise or anticlockwise, thus balancing the
robot [2].
III. SYSTEM MODELLING
This project implements a self-sustaining, two-wheeled,
balancing robot capable of balancing on level surfaces. The One of the very first steps for a control system engineer to
robot maintains its balance by implementing a Proportional able to control a system successfully is to understand the system
Integral Derivative (PID) algorithm, which determines the first. An engineer can understand the system better through
amount of wheel rotation required to keep the robot upright. The modelling the system in a free body diagram. In this balancing
robot is able to remain balanced indefinitely regardless of small wheeled robot the modelling is divided into two parts. The first
outside disturbances, and is able to move forwards, backwards, is the wheel model and the second is the chassis model.
 Summing up the moments around the centre of the wheel,
∑ M = Jα
.. (2)
C r -H fr × r = J wh θ
Rearranging equation (2),
..
C r -J wh θ wh
H fr = (3)
r
For the right wheel substitute equation (2) into (1),
..
C r -J wh θ wh
M wh ɺɺx= -H r (4)
r
For the left wheel,
..
Cl -J wh θ wh
Fig.1 Physical Layout of the robot M wh ɺɺx= -H l (5)
r
Because the linear motion is acting on the centre of the wheel,
angular motion can be transformed into linear motion by simple
transformation.
.. x
ɺɺ
θ wh × r=x, ɺɺ θ.. = (6)
wh r
. . . x.
θ wh × r=x , θ wh = (7)
r
For the right wheel,
C J
M wh ɺɺx= r wh x-H r
ɺɺ (8)
r r2
For the left wheel,
C J
x= l wh
M wh ɺɺ x-H l
ɺɺ (9)
r r2
Fig.2 Assembled robot
Summing up equation (8) and (9),
A. Wheel Modelling  
  C C
2  M wh+ J wh x= l + r - ( Hl +H r )
 ɺɺ (10)


r2 

r r

B. Chassis Modelling
Chassis modelling parameters is shown in Fig 4. Sum of
forces perpendicular to the chassis or pendulum,
∑ Fperpendicular =M rc ɺɺ
xcosθ rc

( Pr +Pl ) sinθrc + ( H r +Hl ) cosθrc (11)

..
-M rclθ rc -M rc gsinθ rc =M rc ɺɺ
xcosθ rc
Fig.3 Free body diagram of the wheel
Sum of the forces in the horizontal direction
The forces acting on wheels are depicted in Fig 3. Using the ∑ Fhorizontal =M rc ɺɺ
x
Newton’s law of motion, sum of the forces on the x-directions,
∑ Fx = Ma ( H r +Hl ) + M rclθ.2rcsinθrc − M rclθ..rc cos θrc = M rcɺɺx
(1) (12)
H fr − H r = M wh ɺɺ
x
 Let θ rc = π + δ , where δ , is the small angle from the vertical
2
d θ rc
upward direction. Therefore, cos θ rc = −1 and 2 = 0.
dt
The linearising equation representing the whole system is,
..
( ) 2 ..
J rcδ rc + C r+ Cl − M rc glδ + M rcl δ rc (18)
 
  C C ..
x= l + r + M rclδ rc
2  M wh+ 2J wh + M rc  ɺɺ (19)
 r 2  r r
 
Rearranging the state space representation,

..
δ rc =
M rcgl
δ+
M rcl
x−
ɺɺ
(Cr+Cl )
J rc + M rcl J rc + M rcl
2
2 2
J rc + M rcl
Fig.4 Inverted pendulum free body diagram M rc l .. ( Cr +Cl )
x=
ɺɺ δ + (20)
   
   
Sum of moments around the centre mass of the pendulum
 2 M wh +
2 J wh + M rc  r  2 M wh + 2 J wh + M rc 
..    2 
∑ M=Jα where α=θ rc r 2

r

 
(13)
-( P +P )lsinθ -( H +H )lcosθ -( C C )=J θ..  The state space equation is obtained as below after substituting
 r l rc r l rc r+ l rc rc 
equation (19) into (18) and equation (20) into (17) in the form
Rearranging equation (13), of:
( ) ( ) ..
(
- Pr +Pl lsinθ rc - H r +H l lcosθ rc =J rcθ rc + C r+ Cl (14) )
Multiplying equation (11) by –ell,

-( Pr +Pl )sinθ rc -( H r +Hl )cosθ rc  l +M rc glsinθ rc +M rcl 2 θ..rc

0 1 0 0
=-M rclɺɺ
xcosθ rc  
(15)
 M 2rcgl 
 .  0 0 0
 
Substitute equation (14) into above equation to eliminate (Pr+Pl) x   M2 l
rc  x 
φ−
and (Hr+Hl) term and rearranging the same term, x  
 ɺɺ
J rc + M rcl 2
  .
 =  x 
δ .  0 
1 δ 
..
( ) 2 ..
J rc θ rc + C r+ Cl + M rc glsinθ rc + M rcl θ rc
(16)
  
δ ..  
0
2
0
M rcgl 2
 
 δ . 
  0 0 0
= -M rclɺɺ
xcosθ rc  M2 l 2 
 J rc − rc + M rcl 2  (21)
To eliminate (Hr+Hl) term from the second equation, equation  φ 
(12) is inserted into equation (10) and rearranging the same
terms,  
 
   0 0 
  C C ..  
2J x= l + r + M rclθ rc cos θ rc
2  M wh+ wh + M wh  ɺɺ 1 M rcl 1 M rcl  Cr 
 2  + − −
r J + M l 2  Cl 
r r r (17)
 J rc + M rcl
  r 2
rc rc 
.2  M rcl M rcl 
+ M rc lθ rc sinθ rc  −1 −1 
 rφ rφ 
Equation (16) and (17) are non-linear equations, and to get
2 J wh
linear equations, some linearising or assumption are taken. where φ = 2M wh + + M rc and output of
r2
 x  Mwh Mass of the robot wheels [0.145kg]
  Mrc Mass of the robot chassis [3.694kg]
1 0 0 0   x.  l Distance between the centre of the wheels and
y=   (22) robot center of gravity [0.165m]
0 0 1 0  δ  r Radius of the wheel [0.053m]
 .
δ 
Putting the above calculated parameter in equation 21 and the
The C matrix is oriented this way because the position of the updated matrix is as follow
wheel base and the robot chassis are interested.
Applying the feedback equation:
 .   
 x1   x  0 1 0 0  x 
  x  0
 ɺɺ   .  0 0 
x2  =
0 2.27 0  x   Cr 
u = r − e f g h    (23) δ .  0 0 0
  + 14.68
1  δ  
14.68   
C
(24)
 
x3
    1.786 1.786   l 
 x 4  δ ..  0 0 65.61 0  δ . 
   
Whereby x1, x2, x3, and x4 are the states of the state space
equation. IV. CONTROLLER DESIGNING
The feedback equation is the equation that is about to inserted After conducting the linearization, the mathematical model is
into the main system, with coefficient e, f, g and h are the applied to develop the control scheme.
arbitrary chosen values to be experimented (by trial and error) in
order to get the robot to balance properly.
Finally the overall balancing robot control system is shown in
the Fig 5 [3].

Fig.6 Block diagram of the entire model control system

Fig.5 Balancing robot control system As shown in the Fig 6, the tilt sensor and encoders measured
The robot parameters are as tabulated in Table 2. four variables. All the variables is feedback to the controller.
The controller computes the state space variables and produces
TABLE 2 the control inputs to stabilize the robot. Thus the controller
PARAMETER OF ROBOT computes the voltage which is then decoupled and modified to
Symbol Parameter Value [unit] the actual voltage to be applied to the right and left wheels [4].

x Linear Displacement [m] A. PID Controller Design

x Linear velocity [ m/sec]
Proportional –integral-derivative (PID) control and the phase
x.. Linear acceleration [m/sec2]
lead-lag compensation have been not only widely used in
θ rc Rotational angle of robot chassis [rad] industrial control systems for achieving setpoint (or
. trajectory) tracking and regulation, but also extensively
θ Angular velocity of robot chassis [m/sec]
rc
taught in feedback control courses [5], [6], due to their
.. simple structure, robustness against plant variation, and easy
θ rc
Angular acceleration of robot chassis [m/sec2]
parameter tuning. PID control generates control signals that
.
θ wh
Angular velocity of the wheel [m/sec] are proportional to the error between the reference signal and
the actual output (proportional action), to the integral of the
..
θ wh
Angular acceleration of the wheel [m/sec2] error (integral action), thereby providing the desired control
Cl Applied torque from motor to the left wheel performance if the three-term parameters (proportional,
Cr Applied torque from motor to the right wheel integral and derivative gains) are well designed or tuned.
Pl, Pr Reaction forces between the wheel and chassis Many classical and advance methods [7]-[9] have been
Hl, Hr
Hfl, Hfr Friction forces between the wheels and the ground proposed to design or tune these parameters in order to
g Gravitational constant 9.8 [m/s2] achieve the desired transient and steady-state performance
Jrc Moment of inertia of robot chassis [0.0458kg/m2] specification.
Jwh Moment of inertia of robot wheels [0.00020365kg/m2]
 The first thing is to do when using PID control in Matlab® is
to find the transfer function of the system. The interest is
lying in to control the robot position, which should return to
the vertical after the initial disturbance, the reference signal
should be zero. The force applied by the motors can be
added as an impulse disturbance [10]. The closed loop
transfer function is designed to make the settling time less
than 2 and steady state error to the input reference should be
zero [11]. The schematic of this problem is shown in Fig 7.

Fig.7 Block diagram of PID control system

Fig.10 Impulse response with PID controller k=100, kd=1, ki=1

The closed loop transfer function is modeled in Matlab and the

impulse response plot with PID controller is shown in Fig 9. The
response is still not stable. Now by increasing the proportional
control to the system, increase the k to note the effect on the
response as shown in Fig 10. The settling time is acceptable at
about 2 seconds. By increasing the derivative control,
satisfactory results are obtained as shown in Fig 11.

Fig.8 Block diagram of PID self balancing control system

Fig.11 Impulse response with PID control k=100, kd=80, ki=45

The complete block diagram of self balancing control system is

shown in Fig. 8. It is interesting to note what happen to the robot
position when the controller for the robot’s angle is in place.
The impulse response of the robot position with PID control is
shown in Fig 12. This shows that the robot moves in the
negative direction with a variable velocity to stabilize itself.
The root locus sketch with k=100, kd=80, ki=45 is shown in
Fig. 13. The PID controller is implemented in real time by using
the discrete time representation with sampling time Ts 50msec
[1].
t
∫ error (t ) dt ≈ Ts ∑ error ( k )
0 k =1

derror (t ) error ( k ) − error ( k − 1)

≈
dt Ts
Fig.9 Impulse response with PID controller k=1, kd=1, ki=1
   A. Motor Interface Board
Ts T
u ( k ) ≈ K p  error ( k )+ ∑ error ( k )+ D (error ( k )−error ( k −1))  The motor interface circuit contains two L298 H-bridges
 TI k =1 Ts
  each of which is connected in a parallel manner to enhance
where the current rating. Blocking diodes is used to block the
TpTs reverse current or the leakage current. 4N25 optocouplers is
KI = being used to provide the isolation between the controller
TI
side and to the motor drive section. The circuit uses three
K pTD inputs from the controller.
KD =
Ts The motor interface circuit is shown in Fig 15 and
u ( k ) = K p error ( k ) microcontroller along with motor interface circuit is shown
in Fig 16.
+ K I ∑ error ( k ) + K D (error ( k ) − error ( k − 1)) TABLE 3
K =1 MOTOR INTERFACE CIRCUIT OPERATION
Inputs Status Motor Status
IN1 IN2 En

0 0 0 Stop
0 1 1 Forward
1 0 1 Backward
1 1 1 Stop

B. Accelerometer
The Hitachi H48C Tri-Axis Accelerometer is an integrated
module that can sense gravitational (g) force of ±3g on three
axes (X, Y, and Z). The module contains an onboard regulator to
provide 3.3-volt power to the H48C, analog signal conditioning,
and an MCP3204 (four channel, 12-bit) analog-to-digital
converter to read the H48C voltage outputs. All components are
mounted on a 0.7 by 0.8 inch module. Acquiring measurements
from the module is simplified through a synchronous serial
interface. With the BASIC Stamp® series, for example, this is
Fig.12 Impulse response of the robot position with PID control easily handled with the SHIFTOUT and SHIFTIN commands.
Features: Measure ±3 g on any axis Uses MEMS (Micro
Electro-Mechanical System) technology Onboard regulator and
high-resolution ADC for simple connection to microcontroller
host SPI communications compatible with BASIC Stamp 2
series SHIFTOUT and SHIFTIN commands Free-fall output
indicates simultaneous 0g an all axes Small, breadboard-friendly
package Key Specifications: Power Requirements: 5 VDC
Communication: Serial SPI Dimensions: 0.7 x 0.8 x .45 in (17.8
x 20.3 x 11.4 mm) Operating Temperature: -13 to +167 °F (-25
to +75 °C) [12].
The accelerometer board is shown in Fig. 14.

Fig.13 Root Locus sketch with PID control k=100, kd=80, ki=45

V. HARDWARE DESIGN
Fig. 14 Accelerometer Board
The hardware of the robot consists of motor interface board,
accelerometer and microcontroller board
 itself properly. The implementation of the robots hardware was
easily achieved providing the system integration and
functionality. Being based on low cost components, the project
is an ideal demonstration of control theory for a classroom.

ACKNOWLEDGMENT
The author wish to acknowledge the assistance of Mr. Umar
Farooq on the development and the implementation of control
theory of this project.

REFERENCES
[1] Mr Peter Miler, “Buliding a Two Wheeled Balancing Robot”, University
of Southern Queensland, October, 2008.
[2] Soumit Kumar Biswal, “Development of Self Balanced Robot and its
Controller”, National Institute of Technology, Rourkela, 2009.
[3] Ho-Khoon Chye Randel, “Balancing Wheel Robot”, University of
Southern Queensland, October 2005.
[4] M.H. Knudsen, “Experimental modeling of dynamic systems: An
educational approach”, IEEE Trans. Educ., vol. 41, pp. 146-158, May
1998.
[5] R. Kelly and J. Moreno, “Learning PID structures in an introductory
courses of automatic control”, IEEE Trans. Educ., vol. 44, pp. 373-376,
Nov. 2001.
[6] J.C. Basilio and S.R. Matos, “Design of PI and PID controllers with
transient performance specification”, IEEE Tans. Educ., vol. 45, pp. 364-
370, Nov. 2002.
[7] R.C. Dorf and R.H. Bishop, “Modern Control Systems”, 10th ed.
Englewood Cliffs, NJ: Prentice-Hall, 2005.
[8] B.C. Kuo and F. Golnaraghi, “Automatic control System”, 8th ed. New
York: Wiley, 2003.
[9] G.F. franklin, J. D. Powell, and A. Emami-Naeini, “Feedback Control of
Dynamic Systems”, 4th ed. Englewood Cliffs, NJ: Prentice-Hall, 2002.
[10] Shui-Chun Lin and Ching-Chih Tsai, “Development of a self-balancing
human transportation vehicle for the teaching of feedback control”, IEEE
Fig. 15 Motor interface circuit Trans. Educ, vol. 52, No. 1, February 2009.
[11] Nawawi S. W, Ahmad. M. N, Osman J.H.S, Husain A. R and Abdollah
A.F, “Controller design for two-wheels inverted pendulum mobile robot
using PISMC", 4th Student Conference on Research and Development
(SCOReD 2006), Shah Alam, Selangor, Malaysia 27-28 June, 2006.
[12] [Online] visit: Toptronic (PTY) Ltd T A AP ELECTRONICS - HITACHI
H48C TRI-AXIS ACCELEROMETER MODULE.

Fig. 16 Motor Controller Board

VI. CONCLUSION
In this paper, an effective and pragmatic approach to the
modeling and control of two wheeled balancing robot using low
cost components is presented. Sensory system of the robot
consists of accelerometer and encoders which gives the tilt and
the displacement information respectively. A linear PID
controller has been developed that helps the robot to balance