update: This document has been subsumed by two newer web articles:
http://www.geology.smu.edu/dpa-www/robo/Encoder/imu_odo/
and
http://geology.heroy.smu.edu/dpa-www/robo/subsumption/
dpa
----------------------------------------------------------------------------
howdy
Great robo-contest Saturday! Many 'bots and a good time had by all. I have
some suggestions for the next one (like any 14 year-old that can complete the
course deserves an award whether he places or not!) but we can speak of these
things anon... My kids were quite energized by the whole event, what more can
we ask? Congratulations are due all around, to everyone who built a robot or
contributed, and especially to those who organized and brought off this event!
Roger Arrick and a number of other folks had quite a few questions after the
contest about the dead-reckoning algorithm that I'm currently running on my
robot, which I was not able to answer very satisfactorily, so I thought I'd
try it again here. You guys let me know if this is altogether too much
verbiage for the news group.
An excellent reference is the home page of Johann Borenstein at the University
of Michigan, http://www-personal.engin.umich.edu/~johannb, look for:
"Where am I?" comprehensive report on mobile robot positioning systems
(the new and expanded 1996 edition was just posted! 282 pages,
~200 illustrations, ~400 references). This report is available for free
and can be downloaded via Anonymous FTP as an Adobe Acrobat file named
"pos96rpt.pdf."
Borenstein has also devised a very useful benchmark for testing mobile robot
dead-reckoning accuracy called the "UMBmark" (which would make a good contest)
that is referenced on the same page. I think Clay has a CD from these people
that includes some movie clips of their algorithms in action.
========================================================================
Onward.
The basic proposition is quite simple. For a two wheel differential drive
robot (i.e. almost all of the robots at the contest last weekend), the total
distance the robot has traveled is
distance = (left_encoder + right_encoder) / 2.0;
and the heading of the robot, it's rotation about it's center in radians, is
theta = (left_encoder - right_encoder) / WHEEL_BASE;
where WHEEL_BASE is the distance between the two wheels.
With these two quantities, a bit of trig can give you the robot's position in
X and Y, as follows:
X_position = distance * sin(theta);
Y_position = distance * cos(theta);
You can also multiply theta * (180.0/PI) to get the heading in degrees.
========================================================================
The SR04 dead-reckoning code consists of three parts: the shaft-encoder counts
on each wheel, position calculations, and target acquisition calculations.
What follows is a discussion of each of these some detail.
I. Shaft-Encoder Counts.
SR04 has shaft encoders on the main drive motors which are used for
motor speed control as well as position odometry. I'll reference here the
"sr04.pdf" document that is, or soon will be, available from the DPRG Web
page (is this correct, Larry? Jim? ) for a more exact discussion of the
mechanical design.
These two optical shaft encoders generate 12 interrupts per rotation
of the motors, which works out to about 80 ticks per inch with the present
wheels and gear reduction. This number was determined empirically by setting
the robot down in a long hallway tiled with standard 1 foot square tiles,
printing the accumulated encoder ticks (32 bit "longs") on the robot's LCD
display, and halting the robot manually at the 100th tile. Probably 10 tiles
would do, or just a tape measure for those without access to long institutional
hallways.
I'm presently using a Motorola HC6811 microprocessor and coding in ImageCraft's
"icc11" C compiler and assembler. The interrupt code for the left optical
encoder looks like this:
; variable storage declaration.
; 32 bit distance accumulator, defined as two 16 bit integers.
; icc11 does not support 32 bit longs, so we fake it as a C typedef, "INT32"
; here implemented in assembly.
.area bss
_left_distance: ; INT32
left_distance_hi: .word 0 ; hi 16 bits (HC6811 is Big Endian)
left_distance_lo: .word 0 ; lo 16 bits
.area text
left_interrupt: ; TIC3INT vector
ldd #_left_distance ; inc or dec based on sign
ldx _signL ; Sign of motor direction, 1 or -1
bmi left_dec
jsr _uint32_inc ; left_distance++, icc11 library call
bra left_exit
left_dec:
jsr _uint32_dec ; left_distance--, icc11 library call
left_exit:
ldx #BASE ; HC6811 register base address
bclr TFLG1,x,#0b11111110 ; reset TIC3 interrupt flag
rti ; return from interrupt
With an identical routine running on the right encoder, we have two 32 bit
values in memory, left_distance and right_distance, which collect encoder ticks
accumulated from the last time the robot was reset. At 80 ticks per inch,
signed 16 bit values will overflow at 32767/80 = 409.5 inches, not enough for
our needs, hence the 32 bit counters.
========================================================================
II. Position Calculations.
The SR04 position calculations run 20 times per second in the main
"sensor" loop. These calculations use the left_distance and right_distance
values maintained by the shaft encoders to determine how much the robot has
moved since the last calculation, and accumulate the X_position, Y_position,
and theta variables.
As an aside, these calculations are done in floating point, and the icc11
library is missing a long-to-float conversion utility (itsa always something!)
So I wrote one, which is appended to the end of this correspondence. Further,
the icc11 sin() and cos() functions expect arguments in degrees rather than
radians, for reasons that are not clear to me (this is not the ANSI standard),
so we'll have to fix that on the fly. Here goes:
/* ----------------------------------------------------------------------- */
#include /* icc11 struct definitions for longs */
#include /* trig function header */
#include "../include/srat.h" /* my own hardware definitions */
/* ----------------------------------------------------------------------- */
/* SR04 defines */
#define LEFT_CLICKS_PER_INCH 79.52 /* turns out that the wheels are */
#define RIGHT_CLICKS_PER_INCH 79.48 /* not exactly the same size! */
#define WHEEL_BASE 9.73 /* distance between wheels, inches */
#define TWOPI 6.2831853070 /* nice to have float precision */
#define RADS 57.2958 /* radians to degrees conversion */
/* ----------------------------------------------------------------------- */
/* odometers() maintains these global accumulator variables: */
float theta; /* bot heading */
float X_pos; /* bot X position in inches */
float Y_pos; /* bot Y position in inches */
float total_inches; /* total inches traveled */
/* ----------------------------------------------------------------------- */
/* using these local variables */
float left_inches;
float right_inches;
float inches;
INT32 L_ticks, R_ticks; /* icc11 typedefs defined in */
INT32 last_left, last_right;
INT32 lsamp, rsamp;
/* ----------------------------------------------------------------------- */
/* odometers() is a task run by the round-robin multi-tasking scheduler 20
times per second. It uses the left_distance and right_distance variables as
inputs to maintain a set of global position and heading variables: X_pos,Y_pos,
and theta. */
void odometers()
{
while (1) {
/* sample the left and right encoder counts as close together */
/* in time as possible */
lsamp = left_distance;
rsamp = right_distance;
/* determine how many ticks since our last sampling? */
L_ticks = int32_sub(lsamp,last_left);
R_ticks = int32_sub(rsamp,last_right);
/* and update last sampling for next time */
last_left = lsamp;
last_right = rsamp;
/* convert the longs to floats */
long_to_float(&L_ticks,&left_inches);
long_to_float(&R_ticks,&right_inches);
/* and convert ticks to inches */
left_inches /= LEFT_CLICKS_PER_INCH;
right_inches /= RIGHT_CLICKS_PER_INCH;
/* calculate distance we have traveled since last sampling */
inches = (left_inches + right_inches) / 2.0;
/* and accumulate total inches we have traveled */
total_inches += inches;
/* accumulate total rotation around our center */
theta += (left_inches - right_inches) / WHEEL_BASE;
/* and clip the rotation to plus or minus 360 degrees */
theta -= (float)((int)(theta/TWOPI))*TWOPI;
/* now calculate and accumulate our position in inches */
Y_pos += (inches * (cos(theta*RADS)));
X_pos += (inches * (sin(theta*RADS)));
/* and suspend ourself in the multi-tasking queue for 50 ms */
/* giving other tasks a chance to run */
msleep(50);
}
}
========================================================================
III. Target Acquisition
The X_pos, Y_pos, and theta variables maintained by the odometer()
task give the robot a running account of it's location in Cartesian space.
In order to navigate toward a pre-defined point in that same space, we need
to know, 1) how far away is the target point, and 2) what is the bearing or
angle from our current heading. By knowing the angle, we can generate a
simple steer-left/steer-right command, and by knowing the distance we can
adjust our speed as we approach the target. The determination of these
values is the responsibility of the "locate_target()" task, which also runs
at the 20 Hz sensor loop rate. It takes as its input an X_target and Y_target
value, and returns the target_distance and heading_error. The latter is
the difference between the current robot heading and the desired robot
heading needed to close on the target.
/* ----------------------------------------------------------------------- */
/* locate_target() uses these global variables */
float X_target; /* X lateral target position */
float Y_target; /* Y vertical target position */
float target_bearing; /* bearing in radians from current position */
float target_distance; /* distance in inches from position */
float heading_error; /* heading error in degrees */
/* ----------------------------------------------------------------------- */
/* calculate distance and bearing to target.
inputs are: X_target, X_pos, and Y_target, Y_pos
outputs are: target_distance, heading_error
*/
void locate_target()
{
float x,y;
while(1) {
x = X_target - X_pos;
y = Y_target - Y_pos;
target_distance = sqrt((x*x)+(y*y));
/* no divide-by-zero allowed! */
if (x > 0.00001) target_bearing = 90.0 - atan(y/x);
else if (x < -0.00001) target_bearing = -90.0 - atan(y/x);
heading_error = target_bearing - (theta*RADS);
if (heading_error > 180.0) heading_error -= 360.0;
else if (heading_error < -180.0) heading_error += 360.0;
msleep(50);
}
}
/* NOTE: The ImageCraft icc11 atan() function used above returns degrees rather
than radians. This is not the ANSI standard. For ANSI standard atan(), use:
if (x > 0.00001) target_bearing = 90.0 - (atan(y/x)*RADS);
else if (x < -0.00001) target_bearing = -90.0-(atan(y/x)*RADS);
*/
/* ----------------------------------------------------------------------- */
The navigation routine which uses the "target_distance" and "heading_error"
values as inputs is just another layer in the SR04's subsumption-style
architecture. In it's simplest implementation, it turns right when the
heading_error is positive and turns left when the heading_error is negative,
stopping and declaring victory when the target_distance falls below one inch,
or whatever level of accuracy you want.
The present algorithm is only a bit more complicated, turning more sharply for
larger heading_errors, slowing as it approaches the target, and increasing the
allowed target ellipse accuracy as time and bumper presses accumulate. The
priority of this layer is below the sonar path-finding, IR collision avoidance,
and bumper collision recovery layers, so while it runs continuously, it only
controls the robot when everybody else is happy. This is why I can "dance"
with the robot, the IR and sonar trying their best to avoid my clumsy feet,
and the dead-reckoning taking over from wherever and whenever they leave off.
Hope this is helpful and not too verbose!
cheers
dpa
Attachment: Long to Float conversion utility for ImageCraft's icc11 C
compiler for the Motorola HC6811 microprocessor.
; ------------------------------------------------------------------------
; long -> float conversion
; ------------------------------------------------------------------------
.area bss
_long_hi: .blkb 2
_long_lo: .blkb 2
.area text
; ------------------------------------------------------------------------
; unsigned long to float
; expects: long in long_hi, long_lo
; returns: float conversion in fp_exp, fp_mantissa, fp_sign
pos_long_to_fp: ; entry for positive long
ldx #0x009d ; positive sign and init exponent
bra ulong_to_fp
neg_long_to_fp: ; entry point for negative long
ldx #0xff9d ; negative sign and init exponent
ulong_to_fp:
ldd _long_lo ; return zero if lo and hi = 0
bne ultofp_1
ldd _long_hi
bne ultofp_1
jmp __fp_return_zero
ultofp_1:
dex ; decrement exponent
ldd _long_lo
lsld ; mantissa * 2, lo word
std _long_lo
ldd _long_hi
bcc ultofp_2
addd #1
ultofp_2:
lsld ; mantissa * 2 + carry, hi word
std _long_hi
bpl ultofp_1 ; keep going until normalized
ultofp_3:
std __fpacc1_mantissa ; hi word is hi 16 bits of mantissa
ldd _long_lo
staa __fpacc1_mantissa+2 ; hi byte of lo word is lo 8 bits of mantissa
xgdx ; get sign and exponent bytes
staa __fpacc1_mantissa_sign ; save sign byte
stab __fpacc1_exp ; save exponent byte
clrb ; no errors
rts
; ------------------------------------------------------------------------
; signed long to float
; expects: pointer to long in D
; returns: float in fp_exp, fp_mantissa, fp_sign
_signed_long_to_fp:
xgdx
ldd 2,x
std _long_lo
ldd 0,x
std _long_hi
tsta
bpl pos_long_to_fp
com _long_hi
com _long_hi+1
ldd _long_lo
comb
coma
addd #1
std _long_lo
ldd _long_hi
bcc ltofp_1
addd #1
ltofp_1:
std _long_hi
bra neg_long_to_fp
; ------------------------------------------------------------------------
; long_to_fp convert positive or negative long to fp
;
; expects: pointer to long in D
; pointer to float on stack
;
; returns: float
.globl _long_to_float
_long_to_float:
jsr _signed_long_to_fp
tsx
ldx 2,x
jmp __from_fpacc1
; ------------------------------------------------------------------------
; eof math.s