Building a Directional Gyro
By: Alex Brown
After experiencing dead-reckoning errors due to wheels sliding on floor objects and rugs at the Trinity contest this year, I decided to look for a more dependable way to determine the robot's direction. Something like a directional gyro (DG) would be nice since it should be immune to wheel tracking errors.
Classically, a DG is a horizontally mounted gyroscope, gimbaled to rotate about a vertical axis, which tends to remain pointed in the same direction regardless of how the base is turned. Gyroscopes are quite good at this and have only a very slow drift away from the initial direction.
Unfortunately, gyroscopes are not commonly/cheaply available anymore, which is probably just as well since they have a relatively high failure rate and eat a lot of power compared to our electronics. What is available, and is commonly used in low-end inertial sensors, are rate gyros. Rather than measuring how far the base has turned, these measure the speed at which it is turning (e.g. degrees/second).
Mathematically, it is very easy to derive directional (heading) changes from a rate gyro. All you have to do is integrate the rate gyro signal.
In the real world, it is a little more challenging.
I decided to use a MEMS rate gyro (ADXRS180) manufactured by Analog Devices ( www.analog.com ). It is a chip just 7 mm. square, using a single 5 VDC supply at 8 mA., and is available with maximum ranges of +/- 150- or 300-degrees per second. (it is possible to extend the range considerably with a resistor change.) The chip, unfortunately, has a BGA (ball grid array) mounting; but fortunately Analog sells an evaluation board version for $50 that comes in a 20-pin DIP format.
While this appears to be a pretty good rate gyro, good specifications and such, it has three problem areas: the null, the gradient and noise. (what else is there? ;-)
The output signal of the rate gyro is an analog voltage varying within the range from 0 to 5 VDC. The null point is nominally at the center, 2.5 VDC, but seems to have some variation. My chip puts out a null of 2.342 VDC. So, it is necessary to measure the actual null of the rate gyro being used.
How accurate does the null have to be measured? If the null used is off by just 0.1 deg/sec, the integrated heading will drift by 10 degrees over 100 seconds. So, we need something a lot more accurate than that. I wanted to try to do this as simply as possible, so this attempt will use the analog to digital converter (ATD) built into my processor. My processor (Motorola MC9S12DP256) has a 10-bit ATD that gives it a resolution of about 5 mv/LSB. The rate gyro sensitivity is 12.5 mv/deg/sec,. So, right away, I can only measure down to 0.4 degrees per second. An error of just 1 LSB would result in a drift of 24 degrees per minute.
Perhaps fortunately, the gyro signal is pretty noisy. About 20 mv Peak to Peak in my setup. This means the ATD is bouncing back and forth over 3 or 4 bits. And it appears that if you average a LOT of samples, you can get the true null to much better resolution. I am doing the averaging down to 1/32 of a bit, so at 0.4 deg/sec per bit, that is a resolution of 0.0125 deg/sec. By averaging input samples (with the robot standing still) at 61 samples per second for 1 minute (3660 samples), I seem to be getting a null value about that accurate. 0.0125 deg/sec would be a drift of about 0.75 deg/minute and I am commonly getting 1- to 2-degrees per minute or less. Statistics can be wonderful!
Note: The one-minute worth of samples is somewhat arbitrary. The probability of computing an accurate null increases with the number of samples. You may get a fairly accurate value after just 10 seconds or so; but, once in awhile, you'll get a significantly erroneous number just due to the characteristics of the noise. The longer you sample, the better the noise gets averaged out.
Now that an accurate null is available, the block diagram can be modified as shown. The null calculator will determine the average null value. From then on, the null will be subtracted from the rate gyro output to give a bi-directional rate signal to the integrator.
But, it took a whole minute of samples to get that null, what about when you start using the data and every sample is integrated individually? The statistics still work. Over the course of a minute, the signal may wander up and down a little, but integrating a minute of data is the same as averaging it. The noise averages out.
It would be really nice if the null value always came out the same; but it doesn't. Powering the processor/ATD up can result in a new null significantly different from other readings. This means that I can't just measure the null once and store it in memory. I have to determine it each time the processor powers up. Not only that, the processor ATD takes a number of minutes to stabilize.
It's not that the processor ATD is really bad, it has never been a problem up until this project, but when you want the output to be stable down to about 1/32 of a bit, it takes quite a while. It is stable to the LSB immediately. It takes 15 minutes to stabilize to 1/32 of an LSB. Measuring the rate signal itself with a digital voltmeter seems to indicate that the rate gyro itself stabilizes much more quickly.
So, an accurate null can be computed by waiting for the processor ATD to warm up for about 15 minutes, then averaging data for 1 minute. Not as convenient as could be hoped for, but it works. This does allow the robot to measure its direction quite accurately....when it isn't moving!
As soon as the robot actually starts to turn, the signal gradient comes into play. The spec sheet says the gradient can vary up to +/- 10%. This means a 360-degree turn could come out +/- 36 degrees. After a lot of experimenting, I found some calibration (fudge) factors that would allow the integrator to generate pretty accurate directions. Generally better than 1-degree error for each 360-degree turn. And, so far, this number seems to be consistent enough to make into a constant in the code.
So, now the system can maintain a heading pretty well, but it still drifts slowly over time at null, and generates errors more rapidly if the robot is turning a lot. This isn't actually too bad. Driving around for a minute or two might cause an error of just a few degrees. This would be more than adequate to solve my problems at the Trinity contest, which SHOULD take only a minute or so to complete.
Since the contest at Trinity is run in a maze with many walls all at 90-degree angles, it is possible to update a heading estimate (e.g. the encoder dead reckoning estimate I referred to above, or this new inertial heading estimate) by correcting the heading to agree with the observed wall angles. This method has worked well to correct my heading errors based on wheel encoder data. It only failed when the wheels slipped so much that the robot rotated over 45 degrees. At this time, the robot aligned itself to the nearest wall, which was 90-degrees different from where it started. The inertial heading should provide unambiguous direction data to figure out this kind of problem.
However, it would be nice for other situations to have a way to control gyro drift without the need for tracking walls. In a gyroscope (not rate gyro) based system, this may be done with a compass. This is done by taking the difference between the gyro's heading and the compass heading and applying a torque to the gyroscope to keep it in alignment. This alignment is done with a very slow time constant to keep short-term compass errors, caused by anomalies in the earth's magnetic field, from having much effect on the gyro heading. Since the gyroscope's drift rate is very low, a very slow correction rate is adequate to keep the gyroscope aligned properly.
Compasses have the unfortunate tendency to not always point to magnetic north. If the compass is brought close to a large iron object, the local magnetic field may be far different than expected. Some tests in a room where we run a contest with the San Diego Robotics Society showed that the compass could be off by up to 90 degrees as it passed over specific areas of the floor (where we suspected there were probably steel beams). But, most of the time, the compass pointed pretty much toward north.
So, the trick is to combine the inertial signal, which is quite accurate short-term (but drifts long-term), with the compass signal, which is unreliable in the short-term but generally knows where north is in the long run.
The method will be to take the difference between the compass indication and the inertial heading and SLOWLY change the inertial signal to agree with the compass. Since the rate of change is slow, then driving the robot past a magnetic anomaly will not have time to make much difference. And since MOST of the time, the compass will know where north is, it will be able to keep the inertial reading from drifting very far.
I chose the Devantech CMPS03 compass to use. It has the advantage that it is a complete compass system on one card. It has a build in calibration procedure that allowed it to read direction within +/- 1 degree all the way around a compass rose at 10-degree intervals (for me anyway, Devantech specs it at +/- 3 or 4 degrees) . It provides data via I2C or by a pulse width. I used the pulse width . It updates the data about 10 times a second and provides a pulse from 1.00 msec to 36.99 msec representing 0 to 359.9 degrees. It's available from www.acroname.com for $46.
I compared the compass heading to the inertial heading and subtracted the difference from the input of the integrator. This caused the integrator output to slew into agreement with the compass. The time constant of this correction was 12 seconds (determined by the gain "K"). I also limited the amount of the correction so that corrections smaller than the limit would happen at the time constant rate, but very large errors, probably caused by magnetic anomalies, would attempt to make corrections at a relatively slow 0.1 deg/sec maximum rate.
This method should only allow a large error to build up if the robot stays in the area of a large magnetic anomaly. The best way to avoid this remaining problem is to keep moving. Alternatively, if the robot is actually stopped, it may be best to stop the heading calculation totally. If the heading was (approximately) right when the robot stops, it should still be right when it resume motion.
My test setup
Incidentally, when implementing an integrator on your processor, it is necessary that the time increment between calculations be consistent and that it is accounted for in the calculation. For example, I perform my calculations in a 61Hz real time interrupt routine. Hence, rate is added to heading 61 times per second making the heading change 61 times too fast. The equation should be:
Heading + Turn Rate/61
If you do your calculations in floating point, that is all there is to it. If you use fixed point, as I do, it may be different. The "/61" effectively reduces the resolution of the Turn Rate signal. Hence, I sum the entire Turn Rate signal into a Long Int and divide it by 61 when I want to get the actual heading.
Of course, you probably want to set up the integrator to rollover from 359.9 degrees to 0 degrees (and vise versa).
So, sample code might be: (pseudo-code!)
// calculate null (do once) For i = 1 to 3660 TR = (turn rate signal from ATD scaled as necessary to get degrees/second) Sum = Sum + TR next null = Sum/3660 //when starting, initialize Heading to compass reading (do once) Heading = (compass reading) //calculate compass correction (do the next two items continuously) corr = ((compass reading) - Heading) * K //K sets the time constant if (corr > limit) corr = limit if (corr < -limit) corr = -limit //calculate heading TR = (turn rate signal from ATD scaled as necessary to get degrees/second) Heading = Heading + (TR - null + corr)/61
Some results and comments:
When the test set was rotated on a level surface, the calculated heading generally stayed within a couple degrees of actual heading (measured geometrically on a compass rose) for some normal turns as might be expected of a robot to do.
If the test set is placed on a tilted surface, certain errors can be expected. Particularly, the compass measurement changes significantly due to the fact that the lines of the Earth's magnetic field are not parallel to the surface, but (in northern latitudes like California) are dipping downward as they go north. This means that as the compass sensor is tilted, it may see different directions for north. I found that a 10-degree tilt of the sensor could cause as much as 15 degrees of compass error. This error would, of course, only affect the gyro corrected value by the limit rate of 0.1 degrees per second. So, as long as the tilt doesn't last a long time, the gyro value will remain pretty accurate.
Such a tilt also has a direct effect on the rate sensor itself. The rate sensor reads turn rate most accurately, only when the turn is directly around its vertical axis. For example, if you tilted the rate gyro 90 degrees on its side, then rotated it about the original axis, it would see no rate at all. This error is, I believe, a 1/cosine effect and is negligible for very small angles. I tilted the platform on which I was testing to a 10-degree angle and rotated the test set 360 degrees. It showed only an error of about 4 degrees. This was actually less than the 6 degrees that would be expected. Since 10 degrees is actually a pretty steep slope, and I don't expect my robots to be doing much turning on such slopes, I think I can live with this.
Of course, if you have some indication of tilt angle on your robot, e.g. a vertical gyro or a pendulum, etc., you can correct for this cosine effect.
Another thing that can go wrong is if the robot's turn rate exceeds the range of the rate gyro. The one I used is good up to +/- 150 degrees per second. If the robot exceeds this rate, the calculated heading will be off. The obvious way to correct this is to get a rate gyro with a larger range. Analog also has a model that has a +/- 300 deg/sec range (and the rate gyros can be extended up to 4 times their range with a resistor change), however, range increase will decrease resolution and make the system less accurate. So this is a tradeoff.
Of course, the rate gyro not only provides a way to generate heading information, it also supplies a turn rate signal directly which may be used for damping in your steering equations.
As described above, the system appears accurate enough for my immediate purposes. I intend to do some future tests using an external 16-bit ATD converter. Hopefully, it will also stabilize rapidly to the LSB, and hence provide the same accuracy immediately as my processor's ATD gets in 15 minutes. I can also hope the more accurate ATD will also reduce the one minute time needed to gather samples to determine the null; but, I suspect that the signal noise will still require a lot of sampling.
In any case, this sensor setup is showing a lot of promise. I intend to incorporate it into my future robots and see how well it does. If anyone tries it and finds any problems or makes any improvements, I'd appreciate it if you would let me know.