Gyroscopes, Accelerometers and the Complementary Filter

How Balance Works:-

In teaching a robot how to stand straight and not fall over, we need to understand how balance really works. Balance is something we take for granted in our daily lives, most of us are actually pretty good at it, until we have had a little too much to drink.

Here is how I think balance (writ easy) works:

1. We need a sensor that tells us how 'upright' we are standing. In humans, this sensor is located in the inner ear. In a robot, we could use a sensor like the MPU6050 which can be used to provide tilt in two axes just by directly reading from its registers.

MPU 6050 Accelerometer and Gyroscope

2. Measure the deviation from the 'upright' position. Obviously as this deviation increases, it would eventually cause our centre of gravity to move outside the limits of our baseline, causing us to tip over.

If the COG shifts outside the baseline, the object tips over

3. To prevent tipping over... depending on the deviation observed, move the baseline to quickly get directly under the new position of the COG. In humans, while walking, we do this by moving our legs forward to arrest falling over forwards. In a wheeled balancing robot, we could measure the deviation from the 'upright' position and use motors to brings the wheels directly under this position.

So the first step is to extract angular deviation from the 'upright' position. Angular deviation in three axes is called ROLL, PITCH and YAW. To keep things simple, I have worked only with one axis, PITCH.

The MPU6050 communicates to the Arduino through the I2C protocol. Here is the circuit I used to extract raw data from the sensor. I made this circuit using Fritzing.

The circuit I used to read data from the MPU 6050

The MPU6050 provides us with the following data:-

1. Acceleration measured in G forces, in three axes

2. Angular Velocity (rate of rotation) in deg/sec in three axes

The code for extracting raw data from the MPU 6050 has been written by "krodal" and can be downloaded from here. This code provides us data in RAW format (16 bit integer values) and we need to convert these into units we can use.

The MPU6050 works in various scales, details can be found in the datasheet, but in simple terms,

if the accelerometer scale is +/- 2G, then ANGLE IN DEG = raw value/ 182.

if the gyroscope scale is set to +/- 250 deg/sec then ROTATION RATE in DEG/SEC = raw value/ 131. We need to integrate this value over time to compute ANGLE from ROTATION RATE. 

This screen-shot shows data from all three axes graphed on my PC. The upper graph shows data from the accelerometers and the lower graph shows gyroscope data. I used Qt to program the graphing and my ArduinoTalker class to transfer data from the Arduino over the serial interface.

Raw data from the MPU 6050.

Now comes the tricky part, filtering the data to get the best value for PITCH.

(a) In the short term, the accelerometer output is terribly prone to noise. Therefore we need to use the accelerometer values only in the long term determination of PITCH.

(b) The Gyro values are much less noisy in the short term but will drift away from the true value of PITCH in the longer term.
So if we can apply a filter to (a) and (b), using the accelerometer readings in the long term and the gyro readings in the short term, we can get a steady, accurate value for the angle of PITCH. Such a filter is called a complementary filter, the formula is given below:-

The complementary filter

[EDIT 31 Jan 2014] I was prompted to re-examine my code by Ka, who commented on this article and brought up a very interesting point. In short, and because of the way accelerometers work, the data I have used to run this filter came from two mutually perpendicular axes. Look at the code below to see what I mean.....the details are in my reply to Ka on 31 Jan 2014 (to whom I am very grateful for pointing this out to me).

[code language="cpp"]

float a = accel_t_gyro.value.y_gyro/131;
float b = accel_t_gyro.value.x_accel/182;

[/code]

Here is an image of the filter at work on my PC:

Filtering gives us a smooth, accurate indication of angular deviation

1. Then green lines shows gyro turn rate in deg/sec. These values are integrated over time to produce the yellow line.
2. The noisy blue line is data from the accelerometer
3. The smooth maroon line is data that has been filtered using the complementary filter given above.
As can be seen, the filtered line is free from short term noise and does not drift like the yellow line in the graph.

If our robot is tipping over, we can now use this data to take some sort of corrective action by turning motors etc. In this example I have used data from just one axis - PITCH. We can also use PITCH to create forward/ backward motion. This is what devices like the SEGWAY use, leaning forward on the pedestal causes a permanent PITCH angle that the machine tries to eliminate by moving its motors resulting in constant forward motion.

To determine both ROLL and PITCH, we need to manipulate data from two axes, which is fairly straightforward and can be dome using the filter given above. Three axes attitude estimation is quite a different story, involving complications like the Gimbal Lock and Quaternions, and while it can be done internally by the MPU6050's Digital Motion Processor, I haven't yet figured out how to extract and interpret this data.