Math Routines
for the
DPRG Outdoor Robot Challenge

David P. Anderson


As a follow on to the previous post concerning techniques for running the outdoor navigation challenges, here are some basic math routines that might be handy for dealing with coordinate systems, transforms, odometry, and GPS.


1. Basic odometry.

For two wheel differential drive, robot's position is tracked in 2D Cartesian space with the values X,Y and Heading.
a. calculate distance and heading since last sample
distance = (left_encoder + right_encoder)/2;
heading += (right_encoder - left_encoder)/WHEEL_BASE;

Or heading can be read directly from compass or IMU and converted from degrees to radians: heading = compass / (180/PI);
b. accumulate position in X,Y
distance = distance / COUNTS_PER_INCH;
X += distance * sin(heading);
Y += distance * cos(heading);
WHEEL_BASE is a constant that is the distance between the drive wheels for a differentially steered platform. In this example it is specified in encoder counts, which is not handy, but see the update below for specifying in conventional units.

COUNTS_PER_INCH is a constant that relates encoder counts to distance traveled. It usually convenient to multiple the distance value by some constant number of encoder counts per inch (or meters or whatever) so that X and Y are expressed in conventional units rather than as encoder counts.

--------------------------------------------------------------------------------

Update: Odometry calculations with wheel size error and windup corrections. as referenced in Section 5, Navigation Calibration and Tuning.

a. calculate distance for each wheel
left_inches = (float)left_encoder / LEFT_COUNTS_PER_INCH;
right_inches = (float)right_encoder / RIGHT_COUNTS_PER_INCH;
distance = (left_inches + right_inches) / 2.0;

b. accumulate heading and clip to +- 360 degrees
theta += (left_inches - right_inches) / WHEEL_BASE;
theta -= (float)((int)(theta/TWOPI))*TWOPI;

c. accumulate location in X and Y
X += distance * sin(theta);
Y += distance * cos(theta);
This version of the odometry function uses a separate counts per inch constant for each wheel to allow for tuning out minor differences in wheel size, and to allow WHEEL_BASE to be specified in conventional units. It also clips the theta value to a single 360 degree range to prevent theta from "winding up" in the course of complex maneuvers.


2. Locate_target() distance and bearing.

The two values target_distance and target_angle are calculated by locate_target() and used by the navigation software to seek toward a target coordinate location at X_target, Y_target, from a robot located at X,Y.
a. calculate distance from robot location to target location
xd = X_target - X;
yd = Y_target - Y;
target_distance = sqrt((xd*xd)+(yd*yd));
b. calculate bearing from current heading to target
target_angle = (90 - (atan2(yd,xd)*(180/PI))) - (heading*(180/PI));

The navigation code uses the bearing to the target contained in target_angle to know if the target is left of its current heading ( <0) or right ( >0), and uses target_distance to know when it has arrived at the target.


3. Distance and heading between two latitude/longitude coordinates.
First coordinate pair is lat1,lon1, and second pair is lat2,lon2.

a. Define degrees per radian and nautical miles per degree:
#define RADS (180/M_PI)
#define NAUTICAL_MILES 3437.7387
#define METERS_PER_NM 1852
b. Then calculate angular distance between two lat,lon pairs:
a_distance = acos(sin(lat1/RADS)*sin(lat2/RADS) +cos(lat1/RADS)*cos(lat2/RADS)*cos((lon1-lon2)/RADS));

n_distance = a_distance * NAUTICAL_MILES;
c. Calculate azimuth angle from lat1,lon1 to lat2,lon2:
azimuth = acos((sin(lat2/RADS) - sin(lat1/RADS)
* cos(a_distance)) / (cos(lat1/RADS)
* sin(a_distance)));
d. Bearing and distance from lat1,lon1 to lat2,lon2 in degrees and meters are:
degrees_azimuth = azimuth * RADS;
meters_distance = n_distance * METERS_PER_NM;


4. Convert GPS NMEA degrees_min.minutes format to decimal degrees.
Where deg_min is the NMEA latitude or longitude value after its conversion from ASCII to binary, and s is the ASCII character 'N', 'S', 'E', or 'W' for the sign.
double a,b,decimal_degrees;
if ((s == 'S') || (s == 'W')) deg_min = -deg_min;

a = (double)((int)(deg_min/100));
b = (double)deg_min - (a*100);
decimal_degrees = (a+(b/60));


5. Convert GPS lat/lon to robot X,Y.
Convert a location from lat/lon coordinates to local robot X,Y coordinates in inches. This uses a base_lat/base_lon location which is located at the robot's Cartesian coordinate 0,0.
#define FEET_PER_NM 6076.11549

input: double lat,lon,base_lat,base_lon;
output: float x, y;

y = (float)((lat - base_lat)*(NAUTICALMILES*FEET_PER_NM*12));
x = (float)((lon - base_lon)*((NAUTICALMILES*FEET_PER_NM*12)
* cos((lat+base_lat)/2)));


6. Rotate a target X,Y coordinate into new coordinate system defined by compass offset.
Compass offset is difference between compass heading and the robot's desired heading. This allows a robot using a compass, GPS, or IMU for steering or odometery to drive the same waypoint list in arbitrary orientations. So {0,1} is not necessarily north of {0,0}.
inputs: x_target, y_target, offset
outputs: new_x, new_y

offset = -offset; /* for clockwise rotations */

new_x = (x_target * cos(offset)) - (y_target * sin(offset));
new_y = (y_target * cos(offset)) + (x_target * sin(offset));



PREV NEXT



Rev 1.1
Copyright (c) 2008 David P. Anderson. Verbatim copying and distribution of this entire article are permitted worldwide, without royalty, in any medium, provided this notice is preserved.