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.

For two wheel differential drive, robot's position is tracked in 2D Cartesian space with the values X,Y and 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.a. calculate distance and heading since last sampledistance = (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,Ydistance = distance / COUNTS_PER_INCH;

X += distance * sin(heading);

Y += distance * cos(heading);

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.

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.a. calculate distance for each wheelleft_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 degreestheta += (left_inches - right_inches) / WHEEL_BASE;

theta -= (float)((int)(theta/TWOPI))*TWOPI;

c. accumulate location in X and YX += distance * sin(theta);

Y += distance * cos(theta);

The two values

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 targettarget_angle = (90 - (atan2(yd,xd)*(180/PI))) - (heading*(180/PI));

The navigation code uses the bearing to the target contained intarget_angleto 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.

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;

Wheredeg_minis the NMEA latitude or longitude value after its conversion from ASCII to binary, andsis 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));

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)));

Compassoffsetis 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.