Professional Documents
Culture Documents
Wireless Controlled Two Wheel Balancing Robot: Charles Yong and Chiew Foong Kwong
Wireless Controlled Two Wheel Balancing Robot: Charles Yong and Chiew Foong Kwong
Wireless Controlled Two Wheel Balancing Robot: Charles Yong and Chiew Foong Kwong
Key words: IMU, Accelerometer, Gyroscope, Kalman Filter, PID Controller, Motor Back-EMF,
PWM, RF Controlled Robot, UART
88
22
Charles Yong
Chiew Foong Kwong
1 Introduction
A two-wheeled self balancing robot is a robot that can balance itself on its two wheels. Over the last decade,
research institutes, commercial companies, and independent hobbyists had been inspired to develop and implement
control system for the two-wheeled self balancing robot in different applications [7].
Control system for two-wheeled self balancing robot can be implemented in all sorts of platform, such as automobile,
wheelchair, robot, and even military transport. A two-wheeled self balancing robot offers following advantages over
three or more wheels platform:
2 Literature Review
2.1
2.1.1 Accelerometer
As stated by [10], an accelerometer is a device that measures inertial force that is in the opposite direction of
acceleration vector with respect to freefall in terms of g-force. This means, an accelerometer will indicates 0g force
during freefall where it accelerates constantly at 1g downwards due to gravity. When an accelerometer is at rest on
Earth surface, its casing will be supported by ground with an equivalent force of 1g upwards. Hence, it will indicate a
constant downward force of 1g perpendicular to ground at rest. This characteristic can be utilized to obtain tilt angle
using Pythagoras Theorem.
2.1.2 Gyroscope
A gyroscope is a device that measures angular rate around an axis. Tilt angle can be obtained by integrating angular
rate over sampled time.
2.2
Kalman Filter
The Kalman filter was introduced by Rudolf E. Kalman in his paper titled A New Approach to Linear Filtering and
Prediction Problems in year 1960 [8]. Kalman filter is a mathematical model that performs recursive computations to
estimate the state of a linear process with minimum squared error, as stated in [8]. The filter combines data from
multiple sources. Hence, filtered value tends to be closer to the true values than each individual noisy measured
value.
From [8], Kalman filter has two phases that alternate between each other, predict phase and update phase. Predict
phase use estimated values of previous state to project current state estimate. Update phase update the projected
estimated with actual measurement. The update phase is skipped if measurement is unavailable during a particular
state. The drawback of Kalman filter is that, it needs to be tuned properly before it can be implemented into a system
and there is no fixed tuning method available as it is widely applicable in numerous fields.
2.3
Motor speed is directly proportional to motor voltage. However, reducing motor voltage for lower motor speed
reduces motor torque as well.
89
22
Charles Yong
Chiew Foong Kwong
Pulse width modulation, in short PWM, can be use to counter the problem. PWM works by sending a square wave to
turn the motor on and off at a very high frequency in such a way that the motor never really turns off. The motor still
receives full supplied voltage momentarily which results in higher torque than directly reducing motor voltage for
speed control, as stated by [2]. In [9], PWM duty cycle is proportional to the motor average voltage. Thus, PWM duty
cycle is proportional to motor speed.
2.4
Motor converts electrical energy to mechanical energy in the form of rotation. On the contrary, generator converts
mechanical energy to electrical energy. DC motor can be generator at the same time by rotating the motor shaft. In
[1], Back-EMF is the voltage generated by a rotating motor which is directly proportional to the rotating speed.
According to [1], two alternating steps are required to sense the back-EMF. First, the motor is run for a period of time.
Then, the motor is stop for another period of time and measurement is taken within that time period.
2.5
PID controller is a closed loop feedback controller for a linear system. PID controller calculates the error between
measured values with its desired set point and attempts to correct the calculated error. PID controller stands for
proportional, integral and derivative controller.
Based on [5], proportional, integral and derivative control characteristics were summarized as follows:
Proportional control deals with present error. Proportional factor is the product of gain and measured error.
Hence, larger proportional gain has faster response time and smaller steady state error but causes
overshoots over the desired set point. Setting the proportional gain too high causes a system to oscillate
around the set point without settling.
Integral control deals with accumulation of past errors. When error is too small, proportional factor output
becomes negligible, which causes steady state error. Integral factor is the product of gain and summation of
past errors. Hence, it corrects even a very small error and eliminates the steady state error. Similar to
proportional controller, setting the integral gain too high causes overshoots over the set point.
Derivative control deals with prediction of future errors. Derivative factor is the product of gain and rate of
change of error. Therefore, it is use to reduce the overshoot caused by proportional and integral factor. The
downside is that, derivative gain amplifies noise as well, which can cause the system to become unstable if
the gain is too high.
The downside of PID control is that when there is a large change in set point, the integral factor will
accumulates a large error during response time and eventually overshoot. It will continue to increase over the
set point until the accumulated error is decreased by errors in other direction. This situation is called integral
windup.
90
22
Charles Yong
Chiew Foong Kwong
3 Design Methodology
3.1
Figure 3.1 shows the block diagram for the whole system design.
Charles Yong
Chiew Foong Kwong
3.2
Infrared Sensor:
Infrared sensors can be used to sense robot tilt by comparing the difference in distance from ground
between two sets of sensors on the robot chassis directing downwards with one attached in front and
another at the rear of the chassis. Infrared sensors are very cheap and simple to use but it can only
sense the direction of tilt and its approximate magnitude instead of the exact tilt angle. Its response is
slow compared to the IMU. Furthermore, it needs a smooth and flat floor surface to reflect properly.
Potentiometer:
Potentiometer can be used to sense robot tilt by attaching a rod to it that touches the ground at
another end. When the robot tilts, the rod will turn the knob of the potentiometer changing
resistances which changes voltage drop across it. Tilt angle can be sensed approximately by
o
comparing voltage acquire with voltage at 0 . It is very simple and very cheap but its response is very
slow. It is not very accurate as resistance changes with temperature. The rod may cause drag to the
robot movement.
Sensors
Method
Measurement
Resolution
Sensitivity
Cost
Problem
Potentiometer
Senses robot tilt through rod
with contact to the ground.
Magnitude representation of
tilt.
Low
Low
Very cheap. Less than RM2.
Resistance changes with
temperature.
Floor feeler may cause drag
to the robot.
After some considerations, IMU was chosen for sensing tilt angle. First, both infrared sensor and potentiometer
cannot sense actual tilt angle and their response is slow compared to IMU. To compensate for that, high torque high
speed motors are needed, which cost more than IMU alone. Second, IMU noisy measurements can be filtered by
o
combining both accelerometer and gyroscope measurements. Third, IMU has resolution of 0.1 , which is highest
among the three.
Complementary Filter
Complementary filter works by low-passing measurement from accelerometer and high-passing
measurements from gyroscope, which only involves basic averaging calculations [4]. It produces a
smooth but slow response due to the averaging, which causes delay. Furthermore, it neglects the
gyroscope bias in the calculation, which may cause severe offset in the output if the gyroscope bias
changes.
Kalman Filter
Kalman filter uses complex mathematical calculations for data filtering. Therefore, it consumes a lot
of processor speed. It response is fast and can recover from error very quickly.
92
22
Charles Yong
Chiew Foong Kwong
Filters
Algorithm
Reliability
Response
Problem
Both filters were roughly tested on their performance beforehand. Complementary filter produces a smooth graph
output due to the averaging but its response is slow. It produces a constant offset when gyroscope bias changes due
to its assumption of constant gyroscope bias. On the contrary, Kalman filter corrects the gyroscope bias by
comparing with accelerometer measurement. A high performance microcontroller can be used to counter the
processing problem. After testing, Kalman filter was chosen as it is clearly better even though it was roughly tuned.
Back-EMF
Back-EMF feedback method senses the back-EMF generated by the motor itself, which is
proportional to motor speed, by stopping the motor for a short duration. Therefore, it decreases the
motor maximum speed. This method is free of charge but it can only sense the magnitude
representation of velocity in the form of back-EMF.
Quadrature Encoder
Quadrature encoder considered senses motor speed by using two magnetic sensors to measure the
rotation speed of custom made wheel. Its output is in the form of pulses. It can sense actual motor
speed but it is very expensive and consumes a lot of processor speed.
Rotary Encoder
Rotary encoder senses motor speed using infrared sensor and rotary disc. The rotary disc needs to
be installed on the motor rotating part. It is very cheap and can sense actual motor speed.
Sensors
Method
Measurement
Cost
Disadvantages
Back-EMF feedback method was chosen because it is free and does not require any modification on motor and robot
chassis. Actual motor speed is insignificance to the control system. Magnitude and direction alone is sufficient. Its
problem in decreasing maximum motor speed does not matter as balancing is done in low speed.
3.2.6 Microcontroller
After serious considerations, dsPIC30F4013 was chosen as the microcontroller for the robot. First, it is a high
performance digital signal controller that can run up to maximum of 30 MIPs, which is more than sufficient for the
Kalman filter. Second, it has 12-bit ADC that is necessary for acquiring measurements from IMU with high resolution.
93
22
Charles Yong
Chiew Foong Kwong
Third, it has two addressable UART modules for communication with computer and for remote controlling. Fourth, it
has built-in PWM module for DC motor speed control.
Batteries
Rated Voltage
Mass
Cost
Based on the comparison, Lithium Polymer battery was chosen initially but during time of purchase, its charger is
temporarily out of stock. Sealed Lead Acid battery was chosen instead.
6V DC motors from Cytron Technologies were compared as follows:
Motors
Free-Run RPM
Stall Torque
Based on Table 3.5, 298:1 Micro Metal Gearmotor is too slow for balancing. 30:1 Micro Metal Gearmotor was
preferred with highest speed but its torque is insufficient for the 6V Sealed Lead Acid battery. Hence, 150:1 Micro
Metal Gearmotor was chosen instead.
3.3
The electronic part was designed based on modular design concept. The design for the robot was split into three
major parts, microcontroller board, motor driver board, and a main board which connects every part together through
ribbon cable. LED indicators were placed at each board to indicate power connections for easier troubleshooting.
Significance components such as IMU, RF receiver, motors, and battery were connected through connectors to
prevent soldering damage as well as easier replacement for damaged parts. To control the robot remotely, a RF
transmitter board was designed and built.
3.4
Before building the actual robot chassis, a prototype was made from cardboard paper for initial testing of the system.
The main purpose of the testing was to check whether the two motors can move under the weight of the entire
chassis. After testing, the actual chassis was constructed using acrylic by following the measurements from the
prototype. Acrylic was chosen due to its lightweight property.
3.5
Software Design
The program for the robot was written in C language and was compiled into hex file for microcontroller
dsPIC30F4013 using MikroC PRO for dsPIC software. Similarly, the program for RF controller was written in C
language and was compiled into hex file for microcontroller PIC16F627A using MikroC PRO for PIC software. Hex
file compiled was burned into microcontroller using PICkit 2 software through UIC00A programmer.
94
22
Charles Yong
Chiew Foong Kwong
3.5.1 UART
UART communication was made from the microcontroller to the computer serial port through RS232 Shifter Board
for data analyzing on the computer as well as real-time tuning through the computer. Program module for sending
and receiving integer and float data between microcontroller and the computer through UART was written and tested.
Accelerometer, ADXL335
ADXL335 triple-axis accelerometer has a maximum output of 3g per axis in the form of analog voltage. Its readings
were sampled through dsPIC30F4013 12-bit ADC module. Pythagoras Theorem was applied to obtain tilt angle from
the accelerometer by utilizing its characteristic of indicating a constant downward force of 1g perpendicular to ground
at rest. Readings from z-axis were used to obtain tilt angle due to the orientation of IMU on the robot structure.
Figure 3.3: Accelerometer Side View on Robot Structure with 0 Tilt Angle
o Tilt Angle
Calculations for obtaining tilt angle from accelerometer are shown as follows:
Analog voltage,
95
22
Charles Yong
Chiew Foong Kwong
(3.1)
where
= 12-bit ADC value
= Number of bits of ADC module
= Voltage reference for ADC module
The accelerometer z-axis analog voltage,
:
(3.2)
where
= Accelerometer z-axis output voltage at 0g
= Accelerometer sensitivity in mV/g
Both
and
Pythagoras Theorem was applied to calculate tilt angle in degree based on Figure 3.4 using the g-force,
in (3.2):
obtained
(3.3)
(3.4)
where
= Accelerometer tilt angle in radian
= Accelerometer tilt angle in degree
The ADXL335 accelerometer has a maximum output of 3g per axis. Mathematical error will occur at (3.4) if is
outside the range of 1g due to acceleration and mechanical noise. Therefore, readings were limited within the
range of 1g.
Gyroscope, IDG500
IDG500 dual-axis gyroscope measures angular rate around an axis in the form of analog voltage. It has two separate
outputs per axis, standard and high sensitivity. Similar to the accelerometer, its readings were sampled through
dsPIC30F4013 12-bit ADC module.
Readings from y-axis high sensitivity output were used to obtain tilt angle.
Calculations for obtaining tilt angle from gyroscope are shown as follows:
The sampled gyroscope y-axis analog voltage,
(3.5)
where
= Gyroscope y-axis zero rate output voltage
= Gyroscope sensitivity in mV//s
Discrete integration is the sum of all integrated discrete components. Hence, tilt angle,
accumulating integration of angular rate,
was obtained by
where
= Tilt angle from previous state
96
22
Charles Yong
Chiew Foong Kwong
The tilt angle obtained from gyroscope had smaller changes than the one obtained from accelerometer when the
o
IMU was rotated. Deduced that, the sensitivity of 9.1mV/ /s for IDG500 gyroscope stated in its datasheet is not
accurate. Thus the solution was that the IDG500 sensitivity was investigated and calculated manually using (3.7)
with accelerometer tilt rate as reference.
(3.7)
97
22
Charles Yong
Chiew Foong Kwong
(3.13)
where
= Tilt angle estimate based on gyroscope measurement
= Gyroscope angular rate bias estimate
= Tilt angle of previous iteration
= Angular rate bias of previous iteration
= Sampling time
= Gyroscope angular rate measurement
From (3.13),
. Based on (3.9):
(3.14)
:
(3.15)
(3.16)
As stated by [8],
. Based on (3.11),
(3.17)
(3.18)
where
= Accelerometer tilt angle measurement
= Tilt angle update
= Angular rate bias update
Based on (3.12),
(3.19)
(3.20)
Matrix calculations consume a lot of microcontroller processing time. Hence, all calculations were expressed in
algebraic form inside the program to minimize processing time needed by the Kalman filter. Process noise
covariance, Q and measurement noise covariance, R of the Kalman filter were tuned to minimize mechanical noise
from accelerometer and bias from gyroscope. The update frequency of the Kalman filter was set to 500Hz. Three
constants, , , and R, were tuned.
value determines the weight between the two sensors. Smaller value
means gyroscope is trusted more by the filter.
is the angular rate bias expected from the gyroscope. R is the
noise expected from accelerometer. , , and R values were set experimentally. Kalman filtered tilt angle and
accelerometer tilt angle were sent to the computer continuously in real-time for comparison. While the data was
sending, the robot was tilted back and forth gently for a few times and then vibrated roughly for at least 10 times. The
data acquired for the two actions were plotted on graph using Microsoft Excel. Based on the comparison on graph,
changes were made to , , and R values and the process was repeated until the Kalman filter produces an
acceptable tilt angle response.
Problems of Kalman Filter
Kalman filter response cannot match up with actual changes in robot tilt angle when more data were sent to
the computer during the same iteration as shown in Figure 4.4. Found out that, the UART transmission of
multiple data during the same iteration caused delay to the update time period. The solution to solve this
problema was to set the UART baud rate to maximum of 115200bps.
98
22
Charles Yong
Chiew Foong Kwong
Kalman filter angle started to change after 15min of continuously powering up the robot. Found out that, the
output voltage from 7805 voltage regulator, which was used as ADC reference voltage, dropped when the
battery level dropped due to discharging caused problem to the ADC module. The solution to this problema is
as follows:
First attempt: 1N4007 diode was replaced by 1N5817 diode with lower forward voltage and output
voltage from 3.3V voltage regulator was used as ADC reference voltage instead. However,
this only prevented the problem for half hour.
Final solution: The 7805 voltage regulator was replaced by LDO 5V voltage regulator, which produces
output voltage of 4.99V to 5.01V with minimum input voltage of 5.8V from battery.
99
22
Charles Yong
Chiew Foong Kwong
The robot moved out of balance when its tilt angle approaches set point angle. Each PID terms was sent to
computer and their values were observed when the robot moved out of balance. Found out that MikroC PRO
for dsPIC compiler generates error value for float data multiplication of 1.0 * 0. The solution was 1.0 was
changed to 1 instead.
The robot moved faster forward than backward with the same magnitude of offset angle. Deduced that, the
uneven weight distribution of the robot structure caused the robot to fell faster in forward direction. The
solution was the motor speed was used to limit the tilt angle offset.
Failed to balance the robot with 6V Sealed Lead Acid battery and 150:1 Micro Metal Gearmotor after
o
hundreds of trials. The motor cannot balance the robot after it tilt more than 1 . Deduced that, robot with
higher center of gravity has longer distance to fall before reaching tilt angle, than one with lower center of
gravity as illustrated in Figure 3.5 above. This allows more time for motor to balance the robot. Hence,
concluded that, 150:1 Micro Metal Gearmotor with 85RPM is insufficient to balance the robot with center of
gravity lower than 15 cm.The solution, changed to initial plan of using Lithium Polymer battery and 30:1 Micro
Metal Gearmotor after the battery charger was available. The 200g 6V Sealed Lead Acid battery cannot be
used with new motor due to its low torque of 0.3 kg-cm.
The robot cannot balance on one fixed position. It drifted from one position to another on smooth surface. The
solution was the robot was programmed to move back and forth repeatedly to minimize the drift.
The robot turn out of balance when offset was set to each motor PID controller. After numerous tests, the
robot was found to be more stable to turn by giving offset for 5 iterations and stop giving offset for 25
iterations for the robot to regain balance. The same process was repeated until there was no command
received.
100
22
Charles Yong
Chiew Foong Kwong
The robot randomly stopped although the controller was still transmitting. Found out that, the RF receiver
reads too much noise in between each data causing delay. Thus, the robot was programmed to move for at
least 500ms when no command was received from the transmitter to minimize the effect of the delay.
Gyroscope Sensitivity
Charles Yong
Chiew Foong Kwong
The values acquired in Figure 4.1 were substituted into (3.7) to calculate the gyroscope sensitivity. The gyroscope
o
sensitivity was calculated to be 3.6mV/ /s.
Figure 4.2: Graph of Gyroscope Tilt Angle with Sensitivity Set to 9.1mV/ /s
Figure 4.3: Graph of Gyroscope Tilt Angle with Sensitivity Set to 3.6mV/ /s
Figure 4.2 and Figure 4.3 above show the gyroscope tilt angle readings with accelerometer tilt angle readings as
o
reference. It is clear that gyroscope tilt angle readings were inaccurate when its sensitivity was set to 9.1mV/ /s from
o
its datasheet. More accurate readings were acquired by setting the gyroscope sensitivity to 3.6mV/ /s. The linear line
in Figure 4.3 shows the drift of gyroscope tilt angle caused by angular rate bias of unfiltered gyroscope readings.
4.2
102
22
Charles Yong
Chiew Foong Kwong
103
22
Charles Yong
Chiew Foong Kwong
4.3
Figure 4.9: Graph of Motor Back-EMF with Different PWM Duty Cycle
As shown in Figure 4.9, the motor back-EMF values are proportional to PWM duty cycle. Hence, it was verified that
motor back-EMF values are proportional to motor speed.
104
22
Charles Yong
Chiew Foong Kwong
105
22
Charles Yong
Chiew Foong Kwong
From the analysis above, the motor PID controller gain was set to
, and
106
22
Charles Yong
Chiew Foong Kwong
Figure 4.15: Graph of Left Motor Response with Different Speed Set Point
Figure 4.16: Graph of Right Motor Response with Different Speed Set Point
107
22
Charles Yong
Chiew Foong Kwong
Figure 4.15 and Figure 4.16 show the tuned motor PID controller response on left and right motor with different
speed set point. Negative sign indicates opposite direction of rotation. As shown, both motors produced similar
response in different speed.
4.4
No.
Table 4.1: Different Balance PID Gain Combinations Effect on Robot Response
Gains
Results
50
200
50
50
100
50
100
50
20
Table 4.1 shows some of the significance balance PID controller gain combinations effect on the robot response.
After countless of fine tuning, the robot was able to balance itself with the balance PID controller gain set to
,
, and
.
4.5
Overall, the robot can be moved and turned using the remote control. The robot control was not very smooth due to
signal corruption by noise causing delay to the commands onto the robot. This outcome was expected due to low
cost RF module used for the remote control.
5 Conclusion
Due to the time constraints, the robustness of the robot on different environment has yet to be tested. The robot still
suffered from a little drift on smooth surface. Finally, the balance PID controller response may still be improved with
current hardware. As the recommendations for Further Work, we proposed that the Back-EMF feedback for motor
speed control used in this project cannot acquire the actual distance the robot had travelled. Encoders can be used
on the motors to sense the exact position so that the robot can be programmed for autonomous functions. The PID
controllers used in this project need to be re-tuned whenever there was a change in the structure. Research can be
done on self tuning PID controller to increase the robustness of the control system on different structures as well as
different environments. The remote controlling can be improved by using module that is less susceptible to noise
such as Bluetooth. Extra mechanism can be added to the robot for recovery from fall. In essence, this research and
building of the robot opens the door of possibility to build cheaper and affordable tools for more practical use,
especially in the search and rescue operations.
108
22
Charles Yong
Chiew Foong Kwong
References
[1] Acroname
Robotics.
(2006).
Back-EMF
Motion
Feedback.
Available:
http://www.acroname.com/robotics/info/articles/back-emf/back-emf.html.
[2] F.
Cook.
(n.d.)
PWM
Motor
Speed
Controller
/
DC
Light
Dimmer.
Available:
http://www.solorb.com/elect/solarcirc/pwm1/.
[3] J. S. Hu, J. J. Wang, and G. C. Sun, Self-balancing control and manipulation of a glove puppet robot on a twowheel mobile platform, in Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems,
2009, pp. 424-425.
[4] R. A. Hyde, L. P. Ketteringham, S. A. Neild, and R. J. S. Jones, Estimation of upper-limb orientation based on
accelerometer and gyroscope measurements, IEEE Transactions on Biomedical Engineering, vol. 55, no. 2, pp.
746-754, 2008.
[5] M. A. Johnson, and M. H. Moradi, PID Control: New Identification and Design Methods. Springer, 2005.
[6] W. W. Kong, Robot head to toe, PID for Embedded Design, vol. 4, pp. 9-12, 2010.
[7] D. M. Ling, and W. W. Kong, Robot head to toe, Two Wheeled Balancing Robot, vol. 4, pp. 1-3, 2010.
[8] D. Simon, Kalman filtering with state constraints: A survey of linear and nonlinear algorithms, IET Proceediings
in Control Theory & Applications, vol. 4, no. 8, pp. 1303-1318, 2010.
[9] Society
of
Robots.
(n.d.)
SCHEMATICS
HOW
H-BRIDGES
WORK.
Available:
http://www.societyofrobots.com/schematics_h-bridgedes.shtml.
[10] A. Warnasch, and A. Killen, Low cost, high G, micro electro-mechanical systems (MEMS), inertial
measurements unit (IMU) program, IEEE Position Location and Navigation Symposium, 2002, pp. 299-305.
109
22
Charles Yong
Chiew Foong Kwong