61. Exploring the Pi B+'s Ports VII - Controlling Servos Using an IMU and Complementary Filter

Let's look more closely at what this amazing MPU-6050 device (Inertial Measurement Unit - IMU) can do: it consists of an accelerometer and a gyroscope.  These two components are known as MEMS - Micro-Electro-Mechanical Systems.  In other words, they have moving parts. The chip also incorporates an on-board Digital Motion Processor (DMP) which executes a lot of the motion algorithms for rotation matrix, quaternion, Euler Angle, or raw data formats, inside the chip.  Other features include an embedded temperature sensor
and an on-chip oscillator with ±1% accuracy over the operating range -40°C to +85°C.  Let's look at some of these features:

Temperature

Lines 19 to 22 of the code read one of the registers which contains the chip's core temperature.  This number needs to be calibrated to degrees, as recommended in the InvenSense Product Specification.  The performance of the unit could vary if there was a build-up of heat, so a measurement of its temperature could assist in making temperature corrections if necessary.

Accelerometer

Accelerometers measure linear acceleration. When at rest, the only acceleration experienced by a body is that due to gravity, acting in the -z direction.  When the device is tilted, as in applying roll (about the x-axis) or pitch (about the y-axis) the x- or y- components of gravity increase and these changes are detected by the unit, allowing the measurement of angles of pitch and roll.  Accelerometers cannot measure yaw, which is rotation in the plane perpendicular to the gravity vector, because yaw does not affect the force experienced due to gravity.  Furthermore, the accelerometer is very sensitive to vibrations, causing high-frequency noise.

Gyroscope

A gyro provides information on how fast it is turning (rotation rate about one of the axes) only.  Because it is possible to time-integrate the rotation rate about any axis, the angle through which the unit turns can be calculated.  This information alone would be very valuable if it were not for the tendency of the gyroscope signals to drift, as integration introduces low-frequency noise.

Combining the Accelerometer and Gyroscope Data

By combining the best of both worlds, it is possible to come up with a good estimate of the extent of rotation of the unit about the x- and y-axes.  The idea is to minimise the high-frequency noise (low-pass filter) of the accelerometer and the low-frequency noise (high-pass filter) of the gyroscope.

Acknowledgement: http://www.starlino.com/imu_guide.html

The diagram shows, on the x-y-z axis system, the direction vector R, representing the direction that the MPU-6050 's zenith has moved from the z-axis.  The projection of R on to the x-z plane makes the angle A_xz with the z-axis, and its projection on to the y-z plane makes the angle A_yz with the z-axis.  R_x, R_y and R_z are the components of R along the 3 axes, respectively.  The accelerometer reads out the 3 components of the vector R.  

This makes it relatively easy to calculate the angles of rotation about the y-axis, A_xz  and about the x-axis, A_yz from the expressions:

A_xz = atan2[R_x, R_z],   and A_yz = atan2[R_y, R_z].

Use is made of the atan2 function, instead of the atan function, because it has a range of -π to + π, while the atan function has the less useful range of  -π/2 to +π/2. 

Please note that the angles which I am calculating for the accelerometer are not the same as the angles between the vector R and the x and y-axes, A_xr and A_yr which Andrew Birkett seems to be calculating.

The quantities measured by the gyro are the angular velocities:

 Δ(G_xz)/Δt,  and  Δ(G_yz)/Δt  

where Δ(G) is the change in the angle during the small increment in time Δt.

So if the gyro output is sampled repeatedly at the time interval Δt, the readings can be multiplied by Δt to give the angle from the angular rotation rate.  We can then get the rotation about the y-axis,  G_xz, and the rotation about the x-axis, G_yz,of the vector R.

We now have estimates of the angles from both the accelerometer and the gyroscope, the accelerometer data being imperfect because of high frequency noise, and the gyro data being subject to low frequency noise (drift).  

We need a way of combining the two estimates of angles, A and G, so that we get the best accuracy and least noise.  This is where there is a mixture of art and science.  The accuracy should look after itself in that both methods should be right, no matter what proportions they are taken in.  The method of choosing the proportions may need tuning to get the best mix of high-pass filter and low-pass filter which give the most acceptable levels of noise. The best approach is an iterative algorithm, which starts with the accelerometer's first reading as an approximation.  Let's look at the rotation about the y-axis first:

pitch_xz[0] = A_xz[0]   .....the initial accelerometer data

The initial gyro data is represented by G_xz[0]

The accelerometer noise time constant, τ, typically about 0.5 to 1 second, and the time interval Δt, typically about 10 milliseconds, combine to give a value of α of something like 0.95 to 0.98. α is used to apportion the contributions of gyroscope (α) and accelerometer (1 - α) to the final estimate of the x-axis and y-axis rotations.

α = τ/(τ + Δt)

and these are plugged into the loop:

G_xz[1] = G_xz[1] - G_xz[0]  

pitch_xz[1] = α{pitch_xz[0] + G_xz[1]Δt} + (1 - α)A_xz[1]

Here the gyro increment data updates the expression at each iteration, every Δt seconds. The second term then combines this with a small proportion of the latest accelerometer data.

The generalised form of the loop for the rotation about the y-axis is then:

G_xz[n] = G_xz[n] - G_xz[n-1]          ie    Δ(G_xz)

pitch_xz[n] = α{pitch_xz[n-1] + G_xz[n]Δt} + (1 - α)A_xz[n]

and included in the loop is, for the rotation about the x-axis:

G_yz[n] = G_yz[n] - G_yz[n-1]          ie    Δ(G_yz)

roll_yz[n] = α{roll_yz[n-1] + G_yz[n]Δt} + (1 - α)A_yz[n]

This is carried out for both the x-axis rotation and the y-axis rotation, and is known as a version of the Complementary Filter.  These angle values are fed to the x- and y-axis servos.

Plotting the Outputs and Filter Results

I followed Andrew Birkett's excellent blog to use Gnuplot to plot graphs on the Pi:  Firstly the program needs to be downloaded and installed on the Pi:

sudo apt-get install gnuplot-x11

I named my Python script KC6050ServoD.py and then entered the following instruction:

sudo ./KC6050ServoD.py > plot.dat

This directs the output to a file named plot.dat, instead of printing it on the terminal window.

I put the following Gnuplot commands into a file named gnuplot-command.plg:

set terminal wxt persist size 800,600 background '#000000' # enhanced font 'Consolas,10' 

set style line 99 linecolor rgb "#ffffff" linetype 0 linewidth 2
set key top right textcolor linestyle 99 
set grid linestyle 99
set border linestyle 99

set xlabel "time (s)" textcolor linestyle 99
set ylabel "degrees" textcolor linestyle 99

set yrange [-180:180]

plot filename using 1:2 title "Accelerometer X" with line linewidth 2 , 

\filename using 1:3 title "Gyroscope X" with line linewidth 2 , 

\filename using 1:4 title "Filter X" with line linewidth 2 lc rgb 'gold'

 
#plot filename using 1:5 title "Accelerometer Y" with line linewidth 2 , 

\filename using 1:6 title "Gyroscope Y" with line linewidth 2 , 

\filename using 1:7 title "Filter Y" with line linewidth 2 lc rgb 'gold'

Note that I have commented the last line out.  This enables the X components to be displayed.  Similarly, by commenting out the second last line instead, the Y components are shown.  This file containing Gnuplot commands was then run by entering:

gnuplot -e "filename='plot.dat'" gnuplot-command.plg

Below is a plot of the output of the Accelerometer, Gyroscope and Complementary Filter, as displayed by the Pi:

 

These waveforms represent the angles of roll (upper graph) and pitch (lower graph) respectively, recorded simultaneously as I rotated the MPU-6050 board about firstly its x-axis (1 to 4 seconds), and then about its y-axis (4 to 7 seconds)  Around 7 seconds there was a combination of roll and pitch, and this is apparent in both the x- and y plots.  Within each graph can be seen the Accelerometer's estimates of rotation angle (in red), the Gyroscope's estimates of rotation (in green) and the combination of Accelerometer and Gyroscope readings using the complementary filter (in yellow).

Note that up to about 1 second, the device was at rest on a table, and all three were in agreement (zero degrees).  The red Accelerometer plot then exhibits large spikes, which come from the high frequency vibrations that the device is subject to.  The yellow filter plot 'ignores' the spikes, and is generally between the Accelerometer and Gyro readings.  The Gyro, however, tends to overshoot or undershoot when the tilt is changed.  In the upper plot, after about 6 seconds, the Gyro slowly drifts to a significantly higher level, representing the low frequency noise it exhibits.  The filter, however, seems to 'detect' this, and sticks with the Accelerometer, omitting the spike which occurred - probably as I finally (quite gently) set the board down on the table.

I did a compound plot of a different series of rotations.  It starts to get over-crowded, but here it is anyway:

Once again, the Gyroscope plots end up having drifted well away (more than ±50⁰) from the true reading of 0⁰, corresponding to the device sitting horizontally on the table, while the filter results have 'ironed out' the high frequency noise from the Accelerometer data and have fully corrected Gyroscope drift.

The Python Program

For the script below, I used the code for the above plots, modified to drive the servos using the Complementary Filter data.  Many thanks to Andrew Birkett, as it is largely based on his work:

and here is the video:

You can see that the motion of the servos is now nice and smooth.