From TheRobotGroupWiki

Robot Navigation

These useful notes and formula were by dpa of DPRG The Dallas Personal Robotics Group. Used with permission.

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;
    angle += (right_encoder - left_encoder)/wheel_base;

    Or heading can be read directly from compass or IMU
    and converted from degrees to radians

    angle = compass / (180/PI);

 b. accumulate position in X,Y

    distance = distance / COUNTS_PER_INCH;

    X += distance * sin(angle);
    Y += distance * cos(angle);

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, as above.

---

2. Target distance and bearing. These two values are used by 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_bearing = 90 - (atan2(yd,xd)*(180/PI));

The navigation code uses target_bearing 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. First 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));

---

Circuit Cellar

Here is another great resource. October 2000 issue of Circuit Cellar Magazine article titled Navigating with GPSwritten by Jeff Stefan. It covers the "great circle" algorithm for computing distances between GPS coordinates.