Self Navigated robot (using GPS+compass)

hello ,

The target is to make an autonomous self navigated robot.The user will mark a path in Google Earth and save it as *.kml file. The absolute data(Latitude , Longitude) of each point will be sent to the vehicle via Serial Communication using an interface written in visual c++.After that the robot will enter a whole loop moving around, avoiding any obstacle in the path, until it will reach the final point.

the basic idea of the main loop with 1 point in the path is :
(1):get coordinates of the point
loop
{
(2):get GPS data,
(3):find direction to move,
(4):move,
(5):if you are at the target point stop();
}

The first thing that i thought I needed to do was to test every single hardware(GPS module, compass, motors) alone.
I did it and everything worked ok :
(1):I read GPS data in the Serial monitor with a ±5m accuracy.
(2):I read the current Angle in the Serial monitor.
(3):I made any available moves of the robot with the motors (forward, backwards, turn left, turn right) .

well, I as I need to put all together and make them work i started to make some tests trying to reach a point.I made the circuits and put them all to my ArduinoBT board and after a lot of tests I realized that most of times the robot doesn’t take the correct direction to move.

I thought that the precision of the GPS is the problem as the heading (in which direction i need to go?) is being calculated with the current GPS data (lat , lon) and the point to reach data (lat, lon).Same thing happens in the distance calculation function where the returned value maybe will be always useless cause the GPS precision.

Note : sometimes the whole thing works fine (taking the correct direction, moving and finally it stops nearby to the reach point).that’s why i am suspicious about the GPS precision.

The functions I am using to calculate the heading, the distance from the the point to reach and the whole navigation are the following :

int calc_heading(float flat1, float flon1, float flat2, float flon2)
 {
   float a;
   float b;
   float heading;
   float diflon;
 
   flat1 = radians(flat1);
   flat2 = radians(flat2);
   diflon = radians((flon2)-(flon1));
   a = sin(diflon)*cos(flat2);
   b = cos(flat1)*sin(flat2)-sin(flat1)*cos(flat2)*cos(diflon);
   a = atan2(a,b);
   heading = degrees(a);

   if(heading < 0){heading=360+head_calc; }
   return heading;
 }

float calc_distance(float flat1, float flon1, float flat2, float flon2)
{
  float diflon;
  float dist;
  float a;
  float b;
  int R = 6371000;
   
  flat1 = radians(flat1);
  flat2 = radians(flat2);
  diflon = radians((flon2)-(flon1));   
  a = sin(flat1)*sin(flat2);
  b = cos(flat1)*cos(flat2)*cos(diflon);
  dist = acos(a+b)*R;
 
  return dist;
}
void get_direction(float lat1, float lon1, float lat2, float lon2)
{
  int currentAngle = get_compass_angle(); 
  int heading = calc_heading(lat1,lon1,lat2,lon2);
  while(abs(currentAngle-heading) > 8)
    { 
     if((currentAngle - heading < 0 && abs(currentAngle - heading) < 180) || (currentAngle - heading > 0 && abs(currentAngle - heading) > 180))
     { 
       stopmoving();
       turnRight();
     }  
     else 
     { 
       stopmoving();
       turnLeft();
     }
     currentAngle = get_compass_angle();
     heading = calc_heading(lat1,lon1,lat2,lon2);
     Serial.print("Heading : ");Serial.print(heading);Serial.print(" , Current angle : ");Serial.print(currentAngle);
     Serial.print(" Angle difference:"); Serial.println(abs(currentAngle-heading));
    }
  stopmoving();
}
void navigation(float flat1, float flon1, float flat2, float flon2)
{
  if(calc_distance(flat1, flon1, flat2, flon2) < 2) stopmoving();
  else
  {
    get_direction(flat1, flon1, flat2, flon2);
    forward();
  }
}
void loop() 
{ 
  Serial.println("loop Started !!");
  bool newdata = getDataFromGPS();
  if (newdata)
 {
  navigation(CurrentLat, CurrentLon, LatToReach, LonToReach); 
 }
}

I don’t post the whole code cause it’s so big … I just want to see if I have right in the previous thoughts about the GPS precision and if you can give me some ideas so I can find the solution in my problem !

Thank you in advance .

I made an autonomous autopilot not so long ago based off an Arduino serving as a course keeping controller and another Arduino serving as a display control unit with a small graphics touch screen.

There are of course lots of challenges in such a project, but GPS accuracy was never a blocking factor. Floating point precision was however a real challenge on the Arduino (using 4 byte floats rather than doubles). The problem is that when we do triangulation with global coordinates - the earth is pretty big whereas our deltas are tiny in comparison. To overcome this I opted for two measures:

  • I do all calculations with radians (also distance) and do not convert intermediate results back/forth. Only final results will be converted for display/presentation purposes.
  • I changed the distance formulae as the more common version yielded unacceptable precision when using 4 byte floats. The rewrite of the formulae is as follows (north/east is positive):
// calculate distance in rad between two points
float c_dist(float lat1,float lon1,float lat2,float lon2)
{
  // the standard version of formulae yields unacceptable precision
  // return acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon2-lon1));
  // alternative formulae (haversine)
  const float two=2.0;
  return two*asin(sqrt(square(sin((lat1-lat2)/two)) + 
         cos(lat1)*cos(lat2)*square(sin((lon2-lon1)/two))));
}/*c_dist*/

All in all the project turned out very well despite the limited resources of an 8-bit micro controller.

As you can see I am working on radians in all these calculations.I ll test your formula about the distance calculator but my main problem is that the robot doesn't take the correct direction to move.I tried lots of formulas about the heading calculation but with the same result (some times works great and some other with no success ).

Any Ideas about the direction ?

As you can see I am working on radians in all these calculations

Actually I don't. The parameters to your function are in degrees, mine are in radians. The return value from your distance function is in meters, mine is in radians.

Hmmm . . .!

I think that now I can understand you .

You suggest me to work completely in radians ...I ll test it today and let you know the result .