Inertial Navigation System - Has it Been Done?

Hi, I’m the lead programmer for team 172. This year I’ve been working on using the gyro and accelerometer in tandem to keep track of our robot’s exact location on the field. I’m using Kevin Watson’s gyro code, and fully adapted all the functions for use with the accelerometer as well, as well as some higher-level functions that tie all the data together. The ultimate goal is that I can give the robot an x/y coordinate, and it will drive itself to that location on the field and be within a few inches.

I’ve been running into all kinds of problems, from malfunctioning hardware to flawed algorithms, but I’m at a point now where I think I’ll actually have it finished before ship date.

So I’m just wondering, has anyone else ever done/attempted this before? What was your level of success, and do you have any pointers or warnings?

We’ve always tried to use the gear tooth sensors to do this in the past, but a combination of my then-inexperience and hardware problems always made it impossible. Gear tooth sensors however are inherently flawed because they only track motor revolutions and not you’re actual motion. If you get knocked to the side the gear tooth sensors won’t be aware of it. We’ve also tried to use the camera in the past, but that’s been riddled with problems as well, such as lack of 360-degree motion, and the requisite line-of-sight.

You could consider using a sensor similar to that found in a roller-ball mouse that is mounted on the bottom of your chassis near the center of rotation, this would allow you to gain velocity measurements in two directions. You could then use gear tooth sensors on the motors to determine heading, and integrate from there to determine position.

It has been done, to varying degrees of success. Do a search for WildstangPS to learn about one of the best systems ever developed for FIRST.

I believe wild stang uses such a system but they use it with their crab drive.

Its the infamous StangPS

I hope that helps.

I have a question as to how this works.
I understand that the accelerometer gives your acceleration at any given point in time. Thus, you take the double integral to give position and you should be able to calculate where you are from that. My main question is how can this controller possibly do calculus functions when it doesn’t even handle floating point numbers well?

I haven’t looked at the accelerometer default code because my team isn’t using any accelerometers, so I apologize if this is an obvious question.

The related thread search (below this thread) has several threads that may be of interest.

@Mr. Freeman: You don’t use any actual integration function. I use Timer 2 on an interrupt, so I know exactly how much time has passed. This is my delta t. From there I essentially do a Riemann Sum approximation, which has so far turned out to be highly accurate. Kevin Watson’s gyro code uses this same technique.

Also, there is no accelerometer default code, hence my trouble. I’ve had all the physics and everything in that StangPS animation worked out for weeks. If you look at Kevin’s gyro code, I have duplicated and modified each individual function to deal with a single axis of the accelerometer at a time (you call each function twice, once for each axis). I also added another function to take the second integral.

This is that function:


long Get_Accel_Dist (char axis) {
    long accel_dist = (axis == ACCEL_X_PORT) ? accel_x_dist : accel_y_dist;
    // Return calculated distance to the caller
    return (((accel_dist * ACCEL*SENSITIVITY * 5L) / (ADC_RANGE * ADC_UPDATE_RATE * ADC_UPDATE_RATE)) * ACCEL_CAL_FACTOR);
}

The second line is just an in-line conditional.
The Get_Accel_Velo() function is mostly the same, except you only multiply by ADC_UPDATE_RATE once. This is comparable to Kevin’s Get_Gyro_Angle() function.

[edit]
@GRS: This mouse sensor sounds pretty cool, do you have any more information on how to implement it? Also, since the gyro code is very reliable (at least over a 15-second period), I would much rather use that than gear tooth sensors for tracking yaw. If our robot gets knocked, the GTS won’t register any change, resulting is drastic error.

After reading those two threads posted above, I’m questioning whether the accelerometer will actually be accurate enough. So it would seem that my other options are gear tooth sensors (which are unable to account for unexpected lateral motion), and an optical mouse sensor (which I have no idea how to use).

[edit] This year’s gyro is > 3 times more sensitive than what we’ve been given in the past. I’m hoping this will allow me to retain a certain amount of precision with a stronger noise filter. My filter right now however is just a dead band similar to the gyro’s. Are there better techniques for eliminating noise?

You’ll need two infinitely-turning shaft encoders, a sizeable rolling ball (robots travel faster than human hands on a mouse), and some supports to keep the ball from coming out when you lift the robot, and 3 axles to keep the ball in the right spot to be read by the shaft encoders. The “sizeable” rolling ball should have a high coefficient of friction so as to negate the “slip” factor when the robot gets “knocked”. This system provides relative velocities in the robot’s coordinate plane for the x and y axis’s.

The gyroscope can be mounted on the robot, and it can be used to determine true heading.

Hi, I was just reviewing a electronics design course textbook and I realize that all the integration that has been mentioned thus far has been implemented in software. How about performing the integral in hardware by cascading two op amp circuits? The first circuit in the chain would be an inverting amplifier(to get rid of the nagative voltage produced by the integrator) with the resistor values the same and the second circuit would be an integrator with the RC close to 1. This would theoretically produce a voltage that is proportional to the current orientation. The op amp could be reset to give small displacements in between measurements. This way we don’t need to perform a Riemann sum and instead evaluate exactly.

Any forseen problems? Maybe some filtering would be required…but maybe this will be a little simpler. Maybe it would be advantageous to integrate the accelorometer data twice as well. For this we could just cascade two integrator circuits.