Arduino Adventures

Thursday, May 31, 2012

Combining measurements from the L3G4200D Gyroscope and the MicroMag3 Magnetometer using a Kalman Filter

Next up, I wanted to figure out how to combine the gyroscope and compass to provide a filtered estimate of heading. I did a little web searching and came up with a few relevant places with code:

As a reminder, here is a plot showing the measurements we obtained from the two sensors in the last post on this topic.

which shows the accumulated drift error as well as the noise due to erroneous compass measurements. First, I though I would try just a simple combination approach. We know that the integration of the gyro has less noise but drifts. So at each location we predict what the value of the compass setting should be given the previous setting. If the compass disagrees too much, use the gyroscope solution instead. In other words:

predicted = heading(i-1) + gyro(i) * dt;
difference = abs(predicted - compass(i));
if (difference < X)
heading(i) = compass(i);
else
heading(i) = predicted;
endif

There is a sensitivity parameter X that needs to be set based on how much discrepancy between the two sensors is allowed say the compass reading should be rejected. This is not much code so it should run fast on the Arduino. In matlab, here are the results using this approach.

This removed the sharp spikes and got rid of the drift, but there is not much smoothing (the values are still jumping around). To add some smoothing to the result, we can add an IIR filter on top of the selected result. This is accomplished by simply blending the current estimate with the previous.

predicted = heading(i-1) + gyro(i) * dt;

difference = abs(predicted - compass(i));

if (difference < X)

heading(i) = 0.1 * compass(i) + 0.9 * heading(i-1);

else

heading(i) = 0.1 * predicted + 0.9 * heading(i-1);

endif

This smoothing result can be seen in the below figure.

This introduces a slight lag when sharp changes are made and the extremes are also removed (when the servo started spinning in the other direction). Again, this approach is reasonably fast with 8 operations and a branch statement. However, it is somewhat ad hoc.

A better solution might be a Kalman filter. The Kalman filter will incrementally add in new measurement data but automatically learn the gain term (the blending factor picked as 0.1 in the previous example) and allow a more intuitive setting of a noise model. Using the code given at http://nxttime.wordpress.com/2010/10/06/robotc-code-for-the-kalman-filter/, I get the following result.

This approach used a Kalman filter on the compass parameter and only used the gyroscope to detect when the compass gave an erroneous measurement. The Kalman filter also tracks the covariance estimate in how well the prediction matched the new measurements and the Kalman gain. The gain converged to 0.5549 meaning it would average the prediction and the new measurement.

This approach is somewhat limited based on how it is filtering just a single measurement (the compass) and not modeling the dynamics associated with the gyroscope. I did some more searching and found a few good discussions:

I decided to next try out the code on the last link. This combines the gyro and the compass directly within the filter. I had to adjust several of the smoothing parameters to get a good result. I should probably estimate the process and noise covariance directly from sensor measurements, but first I wanted to get all the measurements going into the filter and determine if the Kalman filter is worth the effort and computation (everything is now running in matlab). Here is the result:

Just based on these plots, it is very difficult to determine the best filtering approach. The next steps are to add in the accelerometer into the mix and look at combining heading information from all three sensors. That will involve using the Kalman filter to weight two measures of heading angle (the accelerometer and the magnetometer).

The code to make all of these plots is on the github site in the matlab directory:

https://github.com/mark-r-stevens/Ardadv/tree/master/matlab

Sunday, May 20, 2012

SPI sharing for using the L3G4200D Gyroscope and the MicroMag3 Magnetometer with the Arduino

Next, I decided to compare the output of the magnetometer and the gyroscope. In reading they both use the SPI interface. But according to this link http://arduino.cc/en/Reference/SPI:

Slave Select pin - the pin on each device that the master can use to enable and disable specific devices. When a device's Slave Select pin is low, it communicates with the master. When it's high, it ignores the master. This allows you to have multiple SPI devices sharing the same MISO, MOSI, and CLK lines.

As I understand this, it means I can have both of them share the pins except for the slave pin. All I have to do is tell the one device to ignore the SPI while the other device is being used. A little searching on the web turned up this tutorial http://tronixstuff.wordpress.com/2011/06/15/tutorial-arduino-and-the-spi-bus-part-ii/. First, I need to make sure both measurement devices are all wired up and connected properly. Turns out to do this on my pan/tilt servo required a lot of wires. This has me thinking that at some point when this is all figured out I need a more permanent solution as this will be impossible to keep wired properly. Here is a wiring diagram and a photo of how things are set up.

Next, I repeated what I did before with the gyroscope: turned the servo on, rotated it back and forth and recorded the measurements of the heading from the magnetometer. There is a slight bias subtraction I did to account for the fact that the starting orientation of the magnetometer is 135° off from north. This led to the following plot:

Using a period of 128 for the magnetometer proceed really noisy results. Here is a plot showing using a period of 32.

Still pretty noisy. An averaging filter will be very sensitive to these outliers. As an example here is an IIR filter applied to this using a 0.1 weighting factor:

This dampens the spikes, but is still very noisy. Just for completion, here is a median filter run. This looks better, but requires more computation to compute (and measurement latency).

Now back to the gyroscope. To use the SPI interface for both, I needed to rework the code a little. The SPI setup calls are different for both sensors, so I needed to reset the SPI before calls to update the measurements for each sensor. Then I had to set the slave select pin to low for both devices after the update. A good overview of what needs to be done is at http://www.eeherald.com/section/design-guide/esmod12.html:

1. All the clock lines (SCLK) are connected together.
2. All the MISO data lines are connected together.
3. All the MOSI data lines are connected together.
4. But the Chip Select (CS) pin from each peripheral must be connected to a separate Slave Select (SS) pin on the master-microcontroller.

So the useful links I used to figure this out are:

Using this I am now able to make measurements from both simultaneously. Here is a plot showing the two outputs:

This shows the drift from the gyroscope and the noise from the magnetometer. Next steps will be to combine the two and produce a result that provides a solid heading estimate. I also have to figure out the scaling of the gyroscope during integration. When I account for dt using millis() it does not provide very accurate results.

Monday, May 14, 2012

Using the L3G4200D gyroscope with the Arduino

The last piece of the IMU puzzle is the gyroscope (previous adventures looked at the accelerometer and the magnetometer). Once I have drivers for this, I can start to look into combination of the three sensors to provide an estimate of heading. Then coupling the range sensors to the odometer readings I can build a dead reckoning system that keeps track (as best as possible) of the robot location in the environment.

Gyroscopes measure an angular rate of motion about three axes. Typically, these angles are referred to as roll, pitch, yaw. When mounted in the same orientation as the magnetometer, they provide and additional bit of information at a higher rate. Gyrscopes are a relative measurement sensor and do not provide an absolute angle. Therefore the gyroscope is perfect for fusion with the magnetometer in the IMU. Several different MEMS gyroscope technologies exist, for a an overview, see the links below.

I went with the L3G4200D from SparkFun. This was a little on the pricey size compared to some of the other components, but it is an essential puzzle piece. Here are the parts lists for this experiment:

L3G4200D: http://www.sparkfun.com/products/10612
Arduino UNO R3: http://www.sparkfun.com/products/11021
Jumper wires: http://www.sparkfun.com/products/9387

On the software side, you will need to install a few things (see previous adventures):

ArdAdv code: https://github.com/mark-r-stevens/Ardadv
ROS serial: http://ros.org/wiki/rosserial/
Various macports: http://www.macports.org/

In figuring out how to use the chip, I found the following sites to be very useful:

I wired up the L3G4200D to use the SPI interface. Not sure this is the best as it also supports an I2C interface (meaning less pins, plus the magnetometer also uses SPI). However, it is a place to start. Here is a wiring diagram of what I setup:

The first thing I did was turn on the gyroscope and let it sit stationary on the table. This estimates the zero settings. Here is an example of the output.

The mean of response is <52.5054, -17.3098, -12.8859>. This is used to correct the up front bias in the voltage settings: simply subtract this value as it is read to remove the bias. I figure eventually, I will add a calibration step that can be used to estimate the gain/offset parameters to get better calibration values. The next step was to make some cumulative angle measurements. I printed out a protractor image and then turned the gyroscope back and forth between 0° and 180°.

Then I integrated the z rotation angle to compute the absolute angle. The idea is that the gyroscope produces a delta angle (call it θ):

θ = ∫₀^t θ^t dt

using this formulation, I generated the following plot:

Here is the test code used to call the gyroscope class:

#include "gyroscope.h"
ardadv::sensors::gyroscope::Gyroscope gyroscope;

void setup()
{
Serial.begin(9600);
Serial.flush();
typedef ardadv::sensors::gyroscope::Gyroscope Gyroscope;
gyroscope.setup(Gyroscope::INTA(7), Gyroscope::INTB(6), Gyroscope::CS(10));
}
void loop()
{
gyroscope.update();
const unsigned long t = millis();
::Serial.print(t);
::Serial.print(",");
::Serial.print(gyroscope.x(), DEC);
::Serial.print(",");
::Serial.print(gyroscope.y(), DEC);
::Serial.print(",");
::Serial.println(gyroscope.z(), DEC);
::Serial.flush();
::delay(100);
}

This uses the code locate at the git hub site: https://github.com/mark-r-stevens/Ardadv/tree/master/device/sensors/gyroscope. The last test I did was to see how stable the gyroscope is and how much drift to expect over time. I hooked up a simple servo and had the servo move back and forth between 0 and 180. Here is a movie of it running.

I then repeated the process and performed the integration. This gave the following plot:

Really hard to say at this point if the difference is due to the error in how precise the servo can achieve the requested angle (the servo is fairly cheap) or due to the accumulated drift. However, the plot definitely looks like drift. Here is the matlab code I used to make this plot:


A = csvread('Capture.txt');

a = A(:,2);
z = -A(:,3);
t = 1:length(a);
dt = conv(A(:,1),[1,-1], 'valid');
figure(1), plot(t, a, 'r-', t, cumsum(z/mean(dt)), 'g-');
legend('servo', 'gyro');
ylabel('angle');
xlabel('time');

Next I will try to either make measurements with the gyro on the robot or fuse in the magnetometer or both.....

Thursday, May 10, 2012

Using a roomba 400 series with the Arduino (Part II)

The next step in getting the roomba setup seems to be building out the serial cable to communicate with the device. After some googling, I found several useful sites with information:

I then bought a serial cable that hooks into the serial port on the Roomba. I cut it in talk and mapped out how the wires were connected. Looking at the data sheet for the Roomba serial interface, there are seven key pins that will be used (see below).

To figure out the pin mapping to colored wire, I use the multimeter. This is done by holding the one of the meter leads against each pin and the other lead against a colored wire. When the right pair is found, the circuit closes and the current reads zero (see the two pictures below).

Eventually, I mapped out all the pins into the table below

pin	color	function
1	brown	vpwr
2	black	vpwr
3	yellow	rxd
4	red	txd
5	purple	dd
6	blue	gnd
7	green	gnd

I am not sure how standard this mapping is so I figure I needed to map this out and write it down so I could refer to it later when I start testing things out. The next two figures show the cable connected to the Roomba.

Finally, I soldered header pins onto the wires to make it easy to connect to the Arduino (see below). Next up, I will try out a simple sketch to see if I can make the wheels move.

Tuesday, May 8, 2012

Using a roomba 400 series with the Arduino (Part I)

The more I work with the 4WD and the steer skidding model, the more I realize a 2WD differential steering model might be better suited to the task. I decided to take a small diversion and start work on my second envisioned project: using a roomba with the Arduino. When this all started I had envisioned three different projects: 1) the small df robot, 2) a modified roomba, 3) a larger scale wheel robot with wheelchair motors and wheels.

I went on ebay and bought several roomba 400 series. I picked these for the sole reason that they were about 10$ each (when they were marked as broken). Fully working models were going for 40$. I figured I could buy four broken models for that cost and have enough parts to get at least one working. They finally arrived and the first thing that came to mind was how dirty they were. I guess this is to be expected as they are vacuum cleaners :)

So the first thing I did was strip everything off the models and remove all the brushes and motors associated with its true purpose: vacuuming. I put all the unnecessary pieces to the side. Then I removed all the electronics, sensors, and motors and put them to the side. I then set about washing and scrubbing the dirt off the chassis. Here are some before shots of the robots.

These images do not really convey the level of dirt involved. Next I used some pressurized air to clean dirt off the electronics and wheels. Finally put the wheels and basic electronics back together. I left off most of the sensors for now, figuring I would get the basics to work first. Besides I am mostly interested in just using the wheels for mobility, the battery for power, and the encoders for odometry. I will then mount a camera, the Arduino and some form of processing board (maybe a PC104) on it for image processing. Here is the reassembled shell all nice and clean.

Next, I will look into hooking a cable up to the s-video type connection near the power connector and see if I can make it do some basic functions.

Sunday, May 6, 2012

Using Odometry for Dead Reckoning with the Arduino and a 4WD DFRobot (Part I)

Now that the robot is moving better (the extra L293D chips and capacitors have improved things), it is time to start looking into estimating the robot state over time. I thought I would start simple using just odometry and the ultrasonic sensors. This will also enable me to validate some of the odometry measurements. I have reasonable expectations on the quality of the results. Everything I have read leads me to believe this will not be very accurate. First, here is the parts list (it is getting quite long):

Arduino: Arduino Uno R3
Robot platform: http://www.dfrobot.com/
Motor shield: https://www.adafruit.com/products/81
XBee wireless kit: http://www.amazon.com/CanaKit-Xbee-Wireless-Kit/dp/B004G1EBOU/ref=sr_1_9?ie=UTF8&qid=1331990327&sr=8-9
HY-SRF05: https://www.virtuabotix.com/feed/?p=209
Wheel encoders: http://www.dfrobot.com/index.php?route=product/product&filter_tag=encoder&product_id=98

Next I need a state model. Again, starting simple, the robot pose can be represented as X, Y, θ. This position / orientation model is discussed in several papers. A couple of key links that fully describe this approach are:

Obviously, the DFRobot has four wheels and will not be perfectly modeled using this differential steering model. More likely, it will be better modeled using a tracked robot (or crawler). However, I have to start somewhere and this is the easiest. Starting this very simple model differential steering model, we have two sensors (the encoders) that are producing measurements about how far each wheel has rotated. Using the nomenclature specified by Lucas (see rossum reference), the encoders provide S_l and S_r or the distance each wheel has traveled since the last measurement.

Currently, the github code returned the counts provided by the optical encoder. To convert those counts to meters, we need to make a few adjustments. Based on the wheel encoder data sheet, there are 20 PPR (pulse per revolution) made. This means 20 returns is a full wheel revolution. The wheel is about 0.0635m in diameter (2.5 in). Converting this to the circumference, the S_l / S_r measurements are:

inline float convert(int count)
{
static const float pi = 20.0f;
static const float ppr = 3.1416f
static const float diameter = 0.0635f;
static const float circumference = pi * diameter;
return circumference * count / ppr;
}

I inserted this code into the wheel encoder class so now the values returned are the distance in meters. Next is the representation of the state. I added a new class to the trunk at device/platform/dfrobot/state. This class will represent the state of the robot given encoder updates at regular intervals. Initially, this will be just a simple linear prediction model, but will evolve as more sensors are added. First, the state can be represented explicitly using the equations given in here:

s = (S_r + S_l) / 2

θ_t= (S_r - S_l) / b + θ_t-1

x_t = s cos(θ_t) + x_t-1

y_t= s sin(θ_t) + y_t-1

where b is the baseline between the wheels. This means the current state at time t is based on an incremental update to the previous state based on the current measurements. Obviously this integration is going to be very noisy and subject to drift. But nonetheless, it is a starting point. I started with a simple sketch that drove the robot forward, paused, then backward. I repeated this several times with the robot heading towards a wall and away. Using this with the ultrasonic range sensor, I should see the range distance decrease while the position increases and visa versa. Here is a short movie of the robot moving:

Next, I made a plot of the distance sensor output versus the robot x position:

which clearly shows the relationship I was expecting. Next, here is a plot showing the x/y position of the robot in the world:

Again, the code to make this plot is at the github site:

https://github.com/mark-r-stevens/Ardadv/tree/master/device/platform/dfrobot/state/test01

next up, I will look at more complicated maneuvers with different wheel speeds and see how well the model works. I will also start validating the distances traveled by measuring the actual robot location.

Thursday, May 3, 2012

Using ROS (Robot Operating System) Fuerte with the Arduino

I decided to upgrade to the new ROS installation called Fuerte. The installation instructions are given on the wiki: http://ros.org/wiki/fuerte/Installation/OSX/Homebrew/Source. This was a little bit of a challenge as I have been using macports. So I decided to try out homebrew. I decided to do this as that is what is recommended for OpenCV so I figured I would have to do this sooner or later.

I moved my macports installation temporarily to my home directory (sudo mv /opt/local ~/macports). Then I followed the instructions exactly as shown on the installation page (linked above). I only really ran into one problem with the installation. That was with the step:

brew install ros/fuerte/swig-wx

for some reason, my installation did not like the hyphen in the swig formula. I got this error:

swig-wx/usr/local/Library/Taps/ros-fuerte/swig-wx.rb:4: syntax error

To fix the problem, I first had to rename the directory:

cd /usr/local/Library/Taps/ros-fuerte/
mv swig-wx.rb swigwx.rb

The I edited the file:

vi /usr/local/Library/Taps/ros-fuerte/swigwx.rb

and removed the hyphen from the Swig-wx class. Then I repeated the homebrew install (this time without the hyphen):

brew install ros/fuerte/swigwx

The formula was still not right, so I had to do a manual install:

cd /Library/Caches/Homebrew/swigwx--git
./configure
make
make install

I then resumed the ros installation process. Everything else worked fine. Here is my /usr/local/Library/Taps/ros-fuerte/swig-wx.rb file:


require 'formula'

class SwigWx < Formula

homepage 'http://www.swig.org'
url 'git://github.com/wg-debs/swig-wx.git', {:using => :git, :tag => 'upstream/1.3.29'}

def install
ENV.universal_binary
system "./configure"
system "make"
system "make install"
end

end