Two Wheel Dynamic Balancing Robot
Figure 7. Restoring balance by applying a force.
applies to your robot; the true center of gravity might not be
level or parallel to the floor, but a bit or a lot off; this has to
be either calibrated or coded to the robot in a fixed or
dynamic way where the robot will learn where its CG (center
of gravity) is by measuring the torque required to keep it
from tilting. When the torque is zero, then the unit is in
The more practical way is to find this measure for
yourself and hard code it to the robot. This way, when the
unit is booted, the robot will slowly try to catch the angle
that will balance it. When you want it to move forward, then
set the offset of the real center of gravity proportionally off
to the speed desired. If the robot is not in its real center of
gravity, the motors will try to compensate and this is what
makes the robot drive forward.
There are some other factors involved. You can’t just set
the torque proportional to the angle and expect it to work.
It has to be damped somehow to avoid putting the unit into
resonance or not having enough torque to compensate. This
is done by means of “rate of change,” which is provided in a
The journal, videos, and all resources — including source code
and schematics — can be found on my site:
The robot I built uses a lot of the parts and algorithms
found on the scooter by:
Trevor Blackwell — http://tlb.org/scooter.html
Much of the research material I used on the subject
can be found at:
If you would like to learn more about the Kalman filter
I am implementing, go here:
If you want to contribute to the project, please Email me:
32 SERVO 07.2004
great form by the gyros. If we push the stick faster, then
it will need to react with more force than that applied in
order to compensate. Remember that with an action comes
a reaction and, in order to keep it in balance, the force
exerted has to be proportionally inverse to the one applied.
This will set the body in rest.
In other words, the variable that sets the torque to the
motors is based on a PID (Proportional Integral Derivative).
The PID provides a proportional value — an integral that
allows it to amplify that value and a derivative that allows you
to damp the response based on some history. PID is very
useful and can be complex and simple at the same time.
Algorithms are usually calibrated in different ways. The
types of constants we are going to use are usually for the
amplification of the gains. There is a way to actually calibrate
the constants without coding back and forth. Remember the
steering pot? This pot can serve well as a gain calibrator.
After you are comfortable with the gains, you can hard
code them in your source code or leave the pot for further
gain calibrations. You could also implement a fuzzy logic or
something that will auto calibrate your platform, but this will
be more complex than what is required for this app.
The algorithms I used for torque are based on the
following, simplified pseudo-code:
Torque = ((CurrentAngle * K1) * (Rate * K2)) * Gain
where CurrentAngle is the actual filtered angle and Rate is
the rate of change. Gain will be the feedback loop (usually
based on history to dampen or amplify the result) and K1, K2
represent the normalization constants for each sensor input.
There is more than one way of balancing a platform.
Some of them involve more complex mathematics. What I
found is that, when you are trying to balance a platform for
the first time, you don’t need complex mathematics; you just
output to the motors a value based on an actual angle times
the rate and you will have enough torque to counteract the
physics applied to your robot and, thus, keep it in balance to
some extent. From here, you can start working on making it
stiffer and more stable.
Now we’ll take things a bit further. In my experience,
making K1 change in proportion to the current angle makes
the platform a bit more stiff and resistant to change, but be
careful — too much and it can set the whole unit in
resonance. So, here is what I did:
GainAmp = Abs(CurrentAngle) * K3
Torque = [(CurrentAngle * (K1 + GainAmp)) *
(Rate * K2)] * Gain
Be careful to make sure that GainAmp does not reach
zero; you will have a multiplication by zero, causing
CurrentAngle to be zero, which is normally not true. In my
case, I prevented GainAmp from becoming less than 1.0 with
an IF statement: