AVC: My Steering Algorithm Sucks

Sunday testing went... um... sub-optimally.

The latest Data Bus update is that I'm hosed if I don't get the robot navigation nailed in the next day or two so I can work on barrel avoidance. The Sparkfun AVC is in less than two weeks.

Here I thought I had this clever little steering algorithm that worked oh so well. As it turns out, it sucks rocks. The robot keeps driving into curbs.

It even knows full well that its going off course, but it doesn't correct its course.

Strange? Yeah.

Well, here's what's going on...

AHRS, IMU and Acceleration

World and Car axes

In working on my 2012 AVC entry, Data Bus, I've been evaluating the use of a 3D IMU (Inertial Measurement Unit) and AHRS (attitude heading reference system) to provide an accurate heading estimate.


An IMU is comprised of some combination of gyros, accelerometers, and magnetometers and can provide enough sensor data to estimate attitude and heading in 2D or 3D.

These sensors plus onboard processing to determine attitude and heading, is called an Attitude and Heading Reference System (AHRS).

The AHRS fuses sensor data to determine which way is North and which way is "Down" and then it computes heading and vehicle attitude.

That is, provided it doesn't change velocity or direction....

AVC: Is a 3D Compass Necessary?

As you may know, I'm working on Data Bus, my Sparkfun AVC robot (in case you missed it, I'm now officially on the active participant list!)

Having evaluated a couple of AHRS (attitude heading reference system) solutions, and after wondering if a compass was even necessary at all, another fundamental question arose. Suppose I do use a compass...

Do I really need a 3D, tilt-compensated compass in the first place? Or can I get away with a much simpler 2D compass? How much heading error can I expect from a 2D versus 3D solution?

Scott (Team Tobor), AVC winner for the last two years, employed a 2D compass heading calculation. Of course he also fed this value into an Extended Kalman Filter (EKF) along with odometry heading calculations.

What should I do? Time for more math, Octave (Matlab), and Gnuplot fun. Let's figure this out...

Magnetometers and Motors

Test rig for current measurement

Now that I've got a mechanism for measuring motor current, it's time to try and quantify the effects of motor magnetic field on magnetometers.

Data Bus was cursed by Compass heading deflection earlier this year and I never discovered its cause. The error observed was on the order of 20 degrees or more.

As part of my ongoing work to get ready for the just-announced Sparkfun AVC 2012, I want to see if I can correlate motor current and magnetometer readings using a Honeywell HMC5843 3-axis magnetometer.

XV-11 Imaging Sensor Revealed!

Image from Random Workshop,
blog of XV-11 Hacking Guru  Hash79

The XV-11 LIDAR has finally been completely cracked open.

Hobby roboticists are on the cusp of DIY, sophisticated, low cost, obstacle detection.

The XV-11 uses a laser-based "radar" that can map out the environment allowing the vacuum cleaner to navigate its environment avoiding obstacles like chair legs, sofas, cats, and more. With this kind of sensor your robot can also detect which room it's in.

Well, thanks to the winner of the Open Lidar Project, XV-11 hacking contest, now we can build our own: all the chips including the imager are known. Follow the link to find the details.

GPS, Gyro, and Compass Errors

Continuing to flail, here. I still don't have reliable navigation working and I'm a month away. That sounds like a lot of time but it's a flash in the pan. I'm very nearly screwed.

My recent test runs suggest various possible errors in the navigation sensors. Here's what I've come up with so far based on two test runs last night and some offline analysis of the raw sensor data.

GPS Errors

I equipped Data Bus with the iGPS-500 and collected data for both GPS units simultaneously. I thought because the Locosys was getting a fix that it was to be trusted.

But it's been unreliable. Last night the iGPS-500 was the only one of the two modules getting a fix... inside the house!  A multi-satellite fix, at that.  Long story short the iGPS-500 frequently seems to produce better data.

Below, the iGPS is pictured in blue, the Locosys LS20031 in green. It's all over the map. In my other test run,  the iGPS did ok, but went squirrely in the upper right near the big tree. The Locosys looked better in terms of shape but was offset about 50 feet.

The dark red path above is the dead reckoning plot using compass. The lighter red is the dead reckoning plot produced offline with a perl script, using raw gyro data with a hand-tuned gyro bias. It's easily the closest to reality.

Compass Error

I wanted to see if the compass simply had a bias so I tried correcting and replotting for 20 degrees then 25 degrees.

The pink path below is the original on-board dead reckoning plot. The green is the same plot rotated 20 degrees and the blue, 25 degrees. The red plot is the hand-tuned gyro-based dead reckoning.

Clearly the issue isn't a simple bias.  The compass heading does not seem to change with the correct proportion during turns.

Part of that may be due to the tilt from the convex surface of the road throwing off the heading. Possibly also because the road itself slopes slightly downhill. I don't really know for sure. I need to...

• recalibrate the compass with the on-board calibration routine
• gather data a few more times from a site with flat ground
• experiment plotting my prior 3 test runs
• if the error is still there, determine the nature of the error

If the error isn't due to the sloping street, and if I can describe the error in state space form along with the heading and heading rate (gyro), then I 'should' be able to build a Kalman Filter to deal with it.

Gyro Error

I had been trusting the compass most. It looked so good on a graph. But I was wrong. Using it to plot dead reckoning shows the real truth. I hadn't really trusted the gyro until now.

Experimenting with different bias values, the gyro provides the best dead reckoning information and in fact the best positioning information. It appears that the biggest contributor of error is the bias (or drift of the bias). Which suggests that if could only find a way to calibrate for the bias, I'd have reliable navigation.

The plot shows dead reckoning plots with different bias values. The purple plot shows a previously measured bias of 2027 (that's the raw, 12-bit ADC conversion value, approximately equal to 2.4496V). The blue corresponds to 2028 (2.4508V), green to 2029 (2.4520V). So I tried 2028.5 (2.4514V), the red, and I think that's probably the closest match.

I can't just take an average at standstill because I tried averaging the first several seconds of the data before the robot started moving. The resulting plot was off quite a bit from reality. So... is there a bias that comes into play when in motion, or during turns? Or was it due to temperature effects?

At worst, maybe I can make test runs and hand calibrate bias until the plots look right, and somehow correlate that to temperature. What super sucks is that we're talking about nailing the bias down to the nearest half a millivolt.

Dead Reckoning Integration Error

All the off-line plots were done by integrating heading and distance measurements every 20ms intervals, an order of magnitude more often than the robot's onboard code. Yet, the difference between the plots is pretty negligible. So integration error doesn't seem to play in.

I wonder if there's some distance error due to using straight line interpolation to represent the curved vehicle path. If that error was large, I'd expect to see a bigger discrepancy in plot length visible at the end of the path in the lower left part of the pic above. Instead, the length of the two plots are within 10cm of each other.

Kalman Filtering, Fusion

Maybe this is inexperience talking but at this point I don't see how Kalman filtering can possibly make use of the massive compass and gps errors to stand any chance of correcting for bias on the gyro and producing reliable heading information. I would expect a Kalman filter would basically tune out the measurements from GPS and compass to the point of ignoring them.


Oh yeah, almost forgot, I still have to dodge barrels.

And I haven't done squat with obstacle detection let alone avoidance.

Yup, I'm Screwed

Now I'm struggling to get a reliable 5V power supply for the robot that can deal with massive voltage sags from the BEC under heavy acceleration.

And as of last night, I'm fighting with that stupid, buggy RC switch board again. It switches off control of the MCU but doesn't switch on transmitter control. Great.

And on top of everything else, my espresso machine has been broken for a week and my efforts to revive it last night were in vain.

I'm just not winning right now.

But hey, I'm not complaining. Ok, maybe I am.

Meanwhile, you may wonder why I'm bothering to write all this when I should be working on the robot. Believe me I have been working very hard on the 'bot. These posts help me to collect my thoughts. Also, I feel more accountable to get busy when the time permits.

AVC Bot: Dead Reckoning

Eeek!  Only 30 days left before race day!!

You may be wondering what happened. Why Data Bus crashed repeatedly.

I've a high level of confidence that GPS error and, to a lesser degree, heading error were the causes. As I continue to collect more and more test run data, GPS plots show pretty substantial position errors, even when the GPS has a very good fix (HDOP of 1.2 or less).

I've been planning all along to supplement GPS fix information with dead reckoning computations and I've finally gotten the software bugs worked out. Here are the plots I collected last night showing errors in GPS and dead reckoning position estimation.

The Plots

In each test run I'm starting at the exact same geographic location: the intersection of a concrete seam and the asphalt at the end of my driveway. I've plotted the point in Google Earth, and I've initialized the dead reckoning code with those coordinates as starting points.

In general, all the dead reckoning plots have the right shape, but seem to get the distances a bit wrong and they appear rotated counter-clockwise by several degrees. The GPS plots at best have the right shape but are off by around 50' in various directions. At worst, the fix data is nearly unusuable.

I'm taking distance and heading measurements every 200ms and the GPS sends fix information just as often. But the GPS data is obviously much noisier, hence the jagged lines as in the plot above and below.

So what does it all mean?  

• At slower speeds, the wheel encoders effectively have less resolution, increasing error
• Distance calculation error is propagating throughout the run
• The compass appears to have an offset of several degrees
• The GPS may be updating too quickly to get accurate results for the slow speeds traveled
• Google Earth undoubtedly has some positional error

And what do I do about it?

I could optimize the tradeoff between calculation frequency and encoder resolution. The main distance problem is likely wheel circumference error propagating. More on long paths than short ones.

Last night I drove the robot in a straight line for a known distance and captured the total number of encoder ticks. The original circumference measurement was 3% too large.

I'll calibrate the compass with the onboard calibration and using a compass. I can calibrate it to a known straight feature in Google Earth, too.

To improve GPS readings, I'll try setting the update rate to 1Hz and see if that improves accuracy. I'll reinstall the iGPS-500 and gather data for both units and compare.

Google Earth introduces some error that I'll have to address next.

Beyond Individual Sensors

Ultimately there's a limit to each sensor's accuracy and precision and to calibration methods. Using statistical methods to fuse sensor input should improve accuracy.

I really have no idea if it is possible to get down to sub-meter accuracy navigating a 240m path while driving around barrels and colliding with other robots. I hope so. I better find out pretty darn quickly.

Pokey's Software: Part 2

Most of the entrants used dead reckoning to navigate the firefighting maze, using Lego NXTwheel encoders built into the motor modules. The dimensions and layout of the maze are known (approximately) beforehand. Note that if you build the maze it doesn't quite look like the drawing.
Pokey was designed to run through the maze without using dead reckoning. Instead, the little robot looks for what I call "events" such as the appearance of a wall to the left or right. The disappearance of the wall on the right or left. Detecting a floor mark. Of course these events are based on lower level sensor code that we'll get to in future posts.

Most importantly, navigation relies very heavily on good wall following because staying oriented to a wall means Pokey knows where he is in his predictable little world. And, in fact, poor wall following performance resulted in his humiliating defeat. :) But more on the low level stuff later. Here's a rundown on the high level event based navigation functions in nav.c

First off, here's the code that gets Pokey from home circle to the floating room. This code is in the main module, sparky.c

//////////////////////////////////////////////////////////////////////
// Home to Room A //////////////////////////////////////////////////////////////////////
go_straight(NAV_EVENT_WALL_HERE, LEFT);
wall_follow_left(NAV_EVENT_WALL_GONE);
myevent = corner_turn(NAV_EVENT_FLOOR|NAV_EVENT_WALL_FOUND, LEFT, LEFT, TURN_FACTOR * 0.80);
if ((myevent & NAV_EVENT_FLOOR) == 0) {
wall_follow_left(NAV_EVENT_FLOOR|NAV_EVENT_FRONT);
}
// Only way to align to the wall, for now
wall_follow_left(NAV_EVENT_FRONT);
stop_moving(); // Don't delete this you goon!

Pokey wants to follow the wall for the floating room to his left. But that wall doesn't appear right away, so Pokey drives forward looking for the wall to the left. His ranger array points left (second parameter below) and he takes off...

go_straight(NAV_EVENT_WALL_HERE, LEFT);

This routine drops out as soon as the wall is found. Now all he has to do is follow the wall until it disappears again.

wall_follow_left(NAV_EVENT_WALL_GONE);

Now he knows that he's in the middle of the maze and all he has to do is turn left into the first room using a constant radius turn (yes, we could use wall following but I found my way was easier to predictably achieve the proper distance once the robot has turned through 180°).

myevent = corner_turn(NAV_EVENT_FLOOR|NAV_EVENT_WALL_FOUND, LEFT, LEFT, TURN_FACTOR * 0.80);

Once Pokey either (a) crosses the floor marker at the door threshold or (b) finds the wall to the left after having made his 180° turn, we move onto the next step. If we still haven't found the floor marker, well, let's find it:

if ((myevent & NAV_EVENT_FLOOR) == 0) {
wall_follow_left(NAV_EVENT_FLOOR|NAV_EVENT_FRONT);
}

Ok, now Pokey knows he's in the room and that the wall is to the left. Pokey can then orient himself square to the room by relying on wall following that left wall until he gets to the corner-- when a wall appears dead ahead.

wall_follow_left(NAV_EVENT_FRONT);

Now Pokey can scan the room in a predictable fashion (more on that later), winding up either pointed at the candle if there is one, or pointed 180° opposite of how he came in--almost perfectly to leave the room. And since he's in the corner, he has a good stretch of wall to follow to reorient himself perfectly on the way out of the room. This is the same approach I used for every room.

That's the theory, but if you look at the video you'll notice he never makes it to the corner. Pokey doesn't straighten out fast enough after detecting the wall to the left and ends up continuing his turn until he is pointing at a sharp angle into the left wall. He sees the left wall in front of him and mistakes that for being in the corner. Oops.

Fortunately, the scanning routine works so well at orienting him that he's still able to navigate out of the room to the next one at least some of the time.

stop_moving(); // Don't delete this you goon!

During testing I would add stop_moving() calls to pause him at various places in the code so I could visibly troubleshoot what was happening. I'd forget and take out this call, too, and so he'd enter the room and run smack into the corner wall. So I wrote myself a little reminder.

From event.h here are the nav events

#define NAV_EVENT_NONE 0x00 // disables the event check
#define NAV_EVENT_WALL_GONE 0x01 // wall disappears
#define NAV_EVENT_FRONT 0x02 // wall appears in front
#define NAV_EVENT_FLOOR 0x04 // floor mark detected
#define NAV_EVENT_WALL_FOUND 0x08 // wall appears
#define NAV_EVENT_THIN_WALL 0x10 // unused, unneeded
#define NAV_EVENT_SHORT_FRONT 0x20 // wall close in front
#define NAV_EVENT_WALL_HERE 0x40 // wall close
#define NAV_EVENT_ALL 0xFF // all events, unused for now

The navigation event routines take one of these events to "watch for" and once found they drop out. The event type is a bit field, each bit corresponds to an event. So your routines can report multiple events at the same time. Each one of those nav event routines calls this one to watch for events. I threw in some addition comments:

event event_check(void)
{
event myevent = NAV_EVENT_NONE;

get_distances();

// Ranger pointing right?
if ( Pointing == RIGHT ) {
// Is the wall within wall following range?
// do we need to add a little fudge factor to distance?
if (distance_right <>
myevent |= NAV_EVENT_WALL_FOUND;

// Has the wall gone away?
else if (distance_right > 550.0)
myevent |= NAV_EVENT_WALL_GONE;
}

// if ranger is pointing left
else if ( Pointing == LEFT ) {
// Is the wall within wall following range?
// do we need to add a little fudge factor to distance?
if (distance_left <>
myevent |= NAV_EVENT_WALL_HERE;

if (distance_left <>
myevent |= NAV_EVENT_WALL_FOUND;

// Has the wall gone away?
if (distance_left > 550.0)
myevent |= NAV_EVENT_WALL_GONE;
}

// Object ahead, probably at end of hall or something
if (distance_front <>
myevent |= NAV_EVENT_SHORT_FRONT;
}

// this number is kind of fudged, trial and error
if (distance_front <> myevent |= NAV_EVENT_FRONT;
}

// Crossed over something white
if (floor_detected()) {
myevent |= NAV_EVENT_FLOOR;
}

return myevent;
}

You can see there's some dependency on the lower level sensor routines (ranger, floor sensor). Anyway, here's an example of one of the nav event routines:

event wall_follow_right(event wanted)
{
event myevent = NAV_EVENT_NONE;

look_right(); // point the ranger right

// drops out when the desired event is found
while ((myevent & wanted) == 0) {

// find out the distances
range_error = target_distance - distance_right;
range_error_rate = last_range_error - range_error;

// wall correction is a PID type routine based on
// current distance error and rate of change of error
steer = -(WALL_CORRECT());

// limit steering, or if we get too far away the bot will just spin
if (steer < -30) steer = -30;
if (steer > 30) steer = 30;

// record error statistics
last_range_error = range_error;
move_forward(MAX_SPEED, steer);

// Add discovered events to myevent
myevent |= event_check();

} // while

return myevent;
} // wall_follow_right

There's plenty more I could talk about but I don't want a 50 page post, either. For example, momentum and stopping distance: the threshold for detecting the front wall has to depend on speed.

The code has plenty of opportunity for improvement and generalizing to account for different speeds, battery voltages, etc. Reworking the wall following to incorporate constant radius turns would be swell.

Hopefully you get the gist, and I hope you found this useful.