GPS guided obstacle avoider seems indecisive

Hello everyone!

I’ve been working on a GPS guided autonomous tank for a few weeks, and it’s almost - but not quite - complete. I’d appreciate a bit of help resolving what seems to be a bug in the code: the robot keeps veering off course as if it changes its mind, then veers back on course. The GPS guidance code on its own seems to work fine (a PID controller turns the robot to adjust heading to match the bearing, although I haven’t quite gotten it to stop when it reaches the target… separate issue). The obstacle avoidance code also seems to work fine, able to navigate my house without bumping into stuff pretty well. However when I merged the two control schemes the robot seems to be fighting something when it’s driving towards the bearing.

I’m pasting the juicy bits of code in this thread but it’s too large to post in its entirety so I’ve attached the INO file for all to enjoy. I haven’t seen too many examples of code posted for this type of robot during Google searches, so if we can work out the bugs maybe this code can help others build a similar one.

The hardware consists of a GPS unit, a 9 axis IMU (MPU9250) which gives me a good heading after calibration for magnetic bias, a Mega 2560 board, a Maxbotix MaxSonar LV1010 ultrasonic range finder mounted on a servo, and tank treads with a motor for each side (no encoders, sigh…). The code uses a PID controller measuring yaw and turning the robot to match the bearing calculated from GPS coordinates. Obstacle avoidance uses a bubble avoidance algorithm (see reference in code) to find a reasonably clear route.

Does anyone know what errors I’ve made that cause the robot to veer off course on a regular basis?

/* Mecha WALL-E the GPS guided autonomous robot tank
 * By Bryan Tiedemann
 * July 7, 2017
 * 
 * Drive to a GPS waypoint and avoid obstacles along the way
 * Relies on code from others - note the long list of libraries
 * Particularly useful was MPU9250 code by Kris Winer, I used
 * the SparkFun library-ized version.  
 */

#include <Wire.h>
#include <PID_v1.h>
#include <quaternionFilters.h>
#include <MPU9250.h>
#include <TinyGPS++.h>
#include <math.h>
#include <Servo.h>
#include <Maxbotix.h>

// CONSTANTS AND VARIABLE DECLARATIONS OMITTED - SEE ATTACHMENT FOR FULL CODE
// setup() OMITTED, AS ARE SOME MATH FUNCTIONS, THEY ARE INCLUDED IN THE ATTACHMENT

void loop() {
  getGPSinfo();         // gets coordinates, calcs currDist and currBearing
  getIMUdata();         // gets accel, gyro and magnetometer data
  calcYawPitchRoll();   // calcs yaw, pitch and roll
  currHeading = -myIMU.yaw;   // compass heading is negative of yaw

  if (!isObstacle) { checkForObstacles();}
  
  if (isObstacle) { avoidObstacles(); }
  
  if ((currDist > range)  && (!looking))
  {
    myPID.Compute();
    pidDrive(Kgas);
  }
  else { stopCar(); }
 
  
}   // end loop()

void checkForObstacles()
{
  float frontDist = 0;
  frontDist = sonar.getRange();       // Head pointing forward, measures distance in cm
  if (frontDist < coastClear) { isObstacle = true; } 
  else { isObstacle = false; }
}

void avoidObstacles()
{
  stopCar();
  if (!looking && !imReady)
  {
    imReady = true;               // I'M READY!  I'M READY!  I'M READY!
  }
  if (imReady)
  {
    counter = 1;                  // Keeps track of place in switch/case statement
    prevMillis = currMillis;      // Going to use this to time my servos
    imReady = false; 
    looking = true;
    finishedLooking = false;      // Yes, this may be extraneous
  }
  if (!finishedLooking) { lookAround(); }
  if (finishedLooking && newResults) 
    {
      turnTowardsFreedom();
    }
  
}

// Turn head six different angles and take distance at each
void lookAround()
{
  currMillis = millis();
  switch (counter) 
  {
    case 1:
      head.write(headA1);
      if ((currMillis-prevMillis) > servoDelay)
      {
        counter = 2;
        prevMillis = millis();
      }
      break;
    
    case 2:
      dist[0] = round(sonar.getRange());
      if ((currMillis-prevMillis) > sonarDelay)
      {
        counter = 3;
        prevMillis = millis();
      }
      break;

    case 3:
      head.write(headA2);
      if ((currMillis-prevMillis) > servoDelay)
      {
        counter = 4;
        prevMillis = millis();
      }
      break;

    case 4:
      dist[1] = round(sonar.getRange());
      if ((currMillis-prevMillis) > sonarDelay)
      {
        counter = 5;
        prevMillis = millis();
      }
      break;

    case 5:
      head.write(headA3);
      if ((currMillis-prevMillis) > servoDelay)
      {
        counter = 6;
        prevMillis = millis();
      }
      break;

    case 6:
      dist[2] = round(sonar.getRange());
      if ((currMillis-prevMillis) > sonarDelay)
      {
        counter = 7;
        prevMillis = millis();
      }
      break;

    case 7:
      head.write(headA4);
      if ((currMillis-prevMillis) > servoDelay)
      {
        counter = 8;
        prevMillis = millis();
      }
      break;

    case 8:
      dist[3] = round(sonar.getRange());
      if ((currMillis-prevMillis) > sonarDelay)
      {
        counter = 9;
        prevMillis = millis();
      }
      break;

    case 9:
      head.write(headA5);
      if ((currMillis-prevMillis) > servoDelay)
      {
        counter = 10;
        prevMillis = millis();
      }
      break;

    case 10:
      dist[4] = round(sonar.getRange());
      if ((currMillis-prevMillis) > sonarDelay)
      {
        counter = 11;
        prevMillis = millis();
      }
      break;

    case 11:
      head.write(headA6);
      if ((currMillis-prevMillis) > servoDelay)
      {
        counter = 12;
        prevMillis = millis();
      }
      break;

    case 12:
      dist[5] = round(sonar.getRange());
      if ((currMillis-prevMillis) > sonarDelay)
      {
        counter = 13;
        prevMillis = millis();
      }
      break;

    case 13:
      head.write(headCenter);
      if ((currMillis-prevMillis) > (3*servoDelay))
      {
        counter = 14;
        prevMillis = millis();
      }
      break;

    case 14:
      counter = 0;
      finishedLooking = true;
      looking = false;
      newResults = true;
      break;
  }  
}

void turnTowardsFreedom()
{
  float turnAngle = bubbleAngle();
  
  getIMUdata();
  calcYawPitchRoll();
  
  currHeading = -myIMU.yaw;
  currBearing = currHeading + turnAngle;
  if (currBearing > 180)  { currBearing = currBearing - 360; }
  if (currBearing < -180) { currBearing = currBearing + 360; }

  if (turnAngle > tol)
  {
    while (turnAngle > tol)
    {
      turnRight();
      getIMUdata();
      calcYawPitchRoll();
      currHeading = -myIMU.yaw;
      turnAngle = currBearing - currHeading;
      if (turnAngle > 180)  { turnAngle = turnAngle - 360; }
      if (turnAngle < -180) { turnAngle = turnAngle + 360; }
    }
    stopCar();
  }

  if (turnAngle < -tol)
  {
    while (turnAngle < -tol)
    {
      turnLeft();
      getIMUdata();
      calcYawPitchRoll();
      currHeading = -myIMU.yaw;
      turnAngle = currBearing - currHeading;
      if (turnAngle > 180)  { turnAngle = turnAngle - 360; }
      if (turnAngle < -180) { turnAngle = turnAngle + 360; }
    }
    stopCar();
  }

  newResults = false;
  isObstacle = false;
}

// Bubble Rebound Algorithm turn angle calculator
// I. Susnea, V. Minzu, G. Vasiliu, "Simple, real-time obstacle
//  avoidance algorithm for mobile robots," Recent Advances in 
//  Computational Intelligence, Man-machine Systems and Cybernetics,
//  WSEAS Press, 2009, pp 24-29, ISBN: 978-960-474-144-1

float bubbleAngle()                   
{
  float sumDist = 0;
  float alphaR = 0;
  float alphaAdj = 0;

  for (int i=0; i<6; i++)
  {
    sumDist = sumDist + dist[i];
  }

  alphaR = (alpha1*(dist[0] - dist[5]) 
          + alpha2*(dist[1] - dist[4]) 
          + alpha3*(dist[2]-dist[3])) / sumDist;

  if (abs(alphaR) < alphaTol)
  {
    if (alphaR <= 0)
      alphaAdj = -45;
    else if (alphaR > 0)
      alphaAdj = 45;
  }
  else
    alphaAdj = alphaR;

  return alphaAdj;
}

void pidDrive(double gasDiff)
{
  int rightGas = round(gasAvg - gasDiff);
  int leftGas = round(gasAvg + gasDiff);
  digitalWrite(pinLD, LOW);
  digitalWrite(pinRD, LOW);
  analogWrite(pinLA, leftGas);
  analogWrite(pinRA, rightGas);
}   // end pidDrive(...)

Thanks a lot!

Bryan

gpsObstaclePID1.00.ino (13 KB)

BryanTmann: Does anyone know what errors I've made that cause the robot to veer off course on a regular basis?

Best guess - as it's an autonomous land vehicle: you probably use it to move the tank for rather short distances (well under 100 meters) and do not take into account the inaccuracy of GPS location, which is at best about a meter in ideal circumstances, 3-5 meters off is quite normal. Disturbances like bounce off of buildings and signal blocking by tree canopies can give errors that are sometimes as much as 10-100 meters. With the GPS location of the tank being somewhat unstable, the direction it thinks it has to go is unstable as well.

That’s what I thought at first, but when I upload the code without the obstacle avoidance routine there is no issue - it drives right to the waypoint about 150 meters away (and keeps going, maybe due to location errors like you mentioned).

One other thing I noticed: when the robot has the obstacle avoidance code loaded without GPS, it pulls to one direction just as it seems to do with the GPS + obstacle code. I’ll attach the obstacle avoidance-only code to this reply for your reference, it’s similar to the code I posted earlier.

ObstacleAvoidance8.03.ino (10.5 KB)

My (very successful) approach was to calculate a bearing from the current GPS coordinates to the endpoint coordinates and use PID with the magnetometer yaw reading (the current heading) to proceed along that bearing line.

Stop when the distance between the current GPS location and the desired location is less than 3 meters.

This requires [u]accurate[/u] distance calculations using GPS coordinates, which can't be done using floating point on AVR-based Arduinos such as the Mega2560. Instead, for all GPS calculations, I use long integer coordinates in degrees*10^6 and the equirectangular approximation for distances. See http://www.movable-type.co.uk/scripts/latlong.html

For debugging, it is very helpful to keep a journey log file, with all the important intermediate variables. I use the serial port and the SparkFun Openlog for painless and completely reliable recording to a uSD card.

You could reinvent that wheel, or you could use a more accurate library: NeoGPS. Like jremington suggested, it uses integer representations internally, to avoid the loss of precision caused by using the Arduino float type. It maintains the full accuracy provided by your device for distance and bearing calculations, whatever that is… :wink:

NeoGPS is also smaller, faster and more reliable than all other libraries. It has many configuration options that can make it even smaller and faster.

I’ve attached a NeoGPS version of your sketch, but I can’t compile it without having the same set of your libraries. The only changes are that the waypoint should be a Location_t type, and you don’t need the coord array:

double currHeading, Kgas;  // PID Setpoint, input and output vars 
//const double waypoint[] = {29.540158, -95.110300};
NeoGPS::Location_t waypoint( 295401580L, -951103000 ); // one more significant digit, if you want
//double coord[2];
double currDist;

Notice how the waypoint can be more accurate. And the getGPSinfo subroutine is simpler:

void getGPSinfo()
{
  while (gps.available( Serial1 )) {
    gps_fix fix = gps.read();
    if (fix.valid.location) {
      currDist    = fix.location.DistanceKm( waypoint ) * 1000.0; // meters
      currBearing = fix.location.BearingToDegrees( waypoint );
    }
  }
}

The distance and bearing routines are not needed.

There are many tips on the Troubleshooting page, especially regarding the overall loop structure. The example programs from all other libraries are not structured in a way that lets you modify them without breaking them. NeoGPS is available from the Arduino IDE Library Manager, under the menu Sketch → Include Library → Manage Libraries.

Cheers,
/dev

BryanTmann.ino (11.9 KB)

EUREKA!

Thank you -dev and jremington for your helpful input! I have just solved my primary problem, which was erratic and aimless wandering when both GPS and obstacle avoidance were working together. It seems there was a conflict between the GPS coordinate acquisition and/or calculations and the MaxBotix ultrasonic sensor in PW distance measurement mode. That behavior didn’t seem to be inaccurate GPS calculations. However, there still may be some issues with distance accuracy - I am currently implementing and testing the recommendations each of you have provided.

The MaxBotix ultrasonic sensor has three outputs: analog, PW and serial. Originally, I used the PW mode in the checkForObstacles() function since it’s supposed to be more accurate. When I changed the checkForObstacles() distance measurement from PW to a single reading from the analog output, the problem went away.

void loop() {
  if (!isObstacle) { checkForObstacles(); }

  if (gpsMode)
  {
    getGPSinfo();
    getIMUdata();
    calcYawPitchRoll();
    currHdg = -myIMU.yaw;
    if (currDist > range)
    {
      myPID.Compute();
      pidDrive(Kgas);
    }
    else { stopCar(); }
  }
  else if (isObstacle)
  {
    avoidObstacles();
  }
}

void checkForObstacles()
{
  long frontDist = 0;   // in centimeters
  long inches;
  
  inches = analogRead(sonarPin)/2;
  frontDist = 254*inches/100;
  
  if (frontDist < coastClear) 
  {
    isObstacle = true; 
    gpsMode = false; 
  }
  else 
  { 
    isObstacle = false; 
    gpsMode = true;
  }
}

My (untested) belief is that in the PW mode the pulseIn() function calls interfere with the GPS coordinate acquisition and/or calculations by interrupting the stream of GPS data mid-sentence during the loop() function.

Attached is the current version of this sketch, which is still a work-in-progress. Note this version is my interpretation of jremington’s suggestion. I plan to test the NeoGPS suggestion from -dev in the very near future. I especially appreciate the time -dev spent modifying my sketch to help with implementation of the NeoGPS library.

Best,
Bryan

gpsObstacle3.01.ino (14.2 KB)