There are many examples on the internet that show how people have built their own self-balancing robots. Many blindingly accept the approaches of others with little explanation. Most include source code and very few include explanations of how the many magic numbers were determined or how to apply their approach to other projects. This series is intended to not only explain the workings of my approach but the lessons I have learned that hopefully you can apply to your self-balancing robot project. I plan to create a first generation prototype and then use what I learn to build ever more improved versions. The current prototype is a bit of Frankenstein. It combines components from Lego Mindstorms v1.0, an Arduino Uno, motor control shield, IMU breakout board, NiCd battery from an old HandyBoard and chassis boards from a Tamiya Universal Plate Set from Japan. Do you like the pipe insulation bumpers?
This posting covers the very heart of the system – the inertial measurement unit or IMU.
Not a Funny Looking Bird
The job of the IMU is to quickly and accurately report the angle of your robot. The angle is used to decide to move your robot forward or backward in an attempt to keep it upright. I elected to use the Adafruit 9 degree-of-freedom IMU Breakout. This tiny board combines a L3GD20H gyroscope with a LSM303 accelerometer/compass with a convenient Arduino library. It’s called 9 degrees-of-freedom (or DOF) because it measures the x, y and z values for all three measurements (angular velocity, linear acceleration and direction).
Much of the internal workings of an IMU implementation depends on the physical installation and orientation of the hardware. The following diagram shows how mine is installed.
The IMU is installed so that the “front” of the robot points in the positive z direction. When the robot tilts forward, it corresponds to a positive rotation about the x-axis and a downward (negative) acceleration of the z-axis. The tilt angle can be determined based on a combination of the accelerometer and gyroscope readings. The accelerometer readings return the linear acceleration for each of the three axis. What which one to use? Have a look at the following diagram.
As the robot tilts forward by an angle of θ, gravity exerts a force on the y and z aspects of the accelerometer. After projecting the force of gravity along those axis, we can express the z-axis acceleration Az as:
sin(θ) = Az/g
For angles less than 30 degrees (our intended control range for the robot), the angle in radians can be approximated within 1% or less by using the the sine of the angle as a close approximation of the actual angle:
θ(t) = Az/g (for a point-in-time from the accelerometer)
The tilt angle from the accelerometer is a point-in-time measurement and is not constant over any period of time.
As the robot tilts forward by an angle of θ over a period of time, it acquires an angular velocity. The angular velocity is measured by a gyroscope as the number of radians per second and is often designated ω. In this implementation I decided to make leaning forward be designated as a positive tilt angle. Using the right-hand rule, the above example will have a positive angular velocity. In order to make an angle determination, a time element must be considered in conjunction with the angular velocity – the value must be integrated. The gyroscope based tilt angle change can be estimated to be:
Δθ(t) = ω * Δt (for a short time period Δt from the gyroscope)
The tilt angle from the gyroscope is an average over a short period of time, typically 10 milliseconds or so for a balancing robot.
Given the volatility of the accelerometer and gyroscope measurements, how can we determine the current tilt angle? We can start by using time discrete integration of the angle like this:
θ(t) = θ(t – 1) + ω * Δt
In other words, accumulate the angle value by continuously taking the previous measurement and adding the change based on the angular velocity and the time difference between the last measurement and the current measurement. Theoretically, this can be done with the gyroscope measurement alone. However, gyroscopes have several sources of error: constant bias, bias stability and angle random walk.
Constant bias is the average output of the gyroscope (ω) when it is still. Since ω is integrated through the multiplication with Δt, this error can accumulate quite substantially. Before we begin using the robot, we need to measure the average of ω over a period of time and subtract this bias from every use of ω. This makes the determination of the tilt angle:
θ(t) = θ(t – 1) + (ω – bias)* Δt
However, the constant bias is not really constant. The bias varies slowly over time and is typically modeled as white noise or a “random walk”. This is measured by something called the “bias stability”. While this value is typically on the gyroscope data sheet, I was unable to find a value for the ST Microelectronics LSM303DLHC. MEMS gyroscopes like this exhibit very high frequency noise that is caused by thermo-mechanical events internal to the chip. This cumulative white noise is typically called drift. I was curious about this noise level and recorded the following data during a 30 second period while the robot was perfectly still.
What is the effect of the gyroscope noise?
The effect of this noise can be experimentally determined. The Angle Random Walk (ARW) value can be thought of as the variation (or standard deviation) of the result of integrating the output of a stationary gyro over time due to the noise. I did some experimenting and found that the ARW for my ST Microelectronics LSM303DLHC is about 1.377 degrees per square root hour (with 100 millisecond intervals measured over a 30 second period). In other words, if the robot ran for an hour, the inherent gyroscope noise would contribute to a variation of ±1.377 degrees from the ideal vertical position. This sounded far too good to be true. In order to measure bias stability, I used a much longer measurement period of 30 minutes, measuring once per second. The results are below:
After looking at the graph above, I thought that 0.0014 radians/second drift over 30 minutes didn’t sound too bad. Then I realized that this is in fact very bad because each reading would be integrated and accumulated on top of the previous error. After integrating with the Δt and converting to degrees, the bias drift error became very obvious:
So while the gyroscope readings have relatively small short-time errors, left unchecked the errors can become quite large over longer periods of time. While this experiment sampled the gyroscope once per second, a self-balancing robot is likely to check far more frequently (e.g. once every 10 to 15 milliseconds). At what point does this drift cause significant error? My next experiment was to measure a perfectly still robot at 15 millisecond intervals to see where the accumulated error reached 1 degree. It took 98.5 seconds.
There is nothing that can be done to eliminate the drift effects on the gyroscope output. However, the effective tilt angle can be corrected against these effects through sensor fusion.
Sensor Fusion Confusion
We now know that the accelerometer can provide a point-in-time tilt angle and the gyroscope can provide an angular velocity that can be used to calculate a tilt angle change. The gyroscope will drift as demonstrated above and need a constant correction. Since the accelerometer can provide an instantaneous tilt angle, it could be used to correct the drift. But why not just use the accelerometer by itself? The accelerometer is subject to vibration from the motors and sudden stops and starts as the robot stays balanced – in other words it is too noisy by itself. When I first started my research on how to approach sensor fusion, I found that there were two schools of thought: Kalman filter and complementary filter. The clearest explanation of why a Kalman filter is overkill for a balancing robot is in Kerry D. Wong’s write up. The complementary filter essentially combines the gyroscope and accelerometer readings to take advantage of their respective strengths and filters out their weaknesses. The values are combined through a weighting that heavily favors the gyroscope over the accelerometer through a single variable α. This approach makes the tilt angle calculation:
θ(t) = α * (θ(t – 1) + (ω – bias)* Δt) + (1 – α)*Az/g
However, I have not found an explanation of how to tune the complementary filter variable α other than it should be greater than 0.9. In the absence of a mathematical foundation for tuning, I turned to experimentation. What value of α enables the accelerometer reading to correct the gyroscope drift? I iterated on several values and plotted a polynomial trend line on the filter output when the robot was perfectly vertical and still.
At least for my IMU, a value of α = 0.94 nearly eliminated the gyroscope drift over a 15 minute measurement period. I deemed this value good enough.
Now that we have an effective means to measure a tilt angle, how do we apply that to the control of the robot? That is covered next in this posting.