GPS Navigointi

Robottien navigoinnissa voi käyttää monia eri tekniikoita, yksi tapa on käyttää robotin paikkatiedon määrityksessä GPS:ä. GPS -laitteita saa nykyään edullisesti ja tarkkuus on hyvä (5-15m), päivitysnopeus on edullisimmilla laitteilla 1Hz, mutta nykyään saa myös 10Hz GPS-vastaanottimia.

NMEA sanomat (NMEA 0183)

GPS -vastaanottimesta yleensä tulee NMEA -sanomia [1]. NMEA -sanomat sisältävät esimerkiksi paikkatiedon, suunnan ja nopeuden. GPS -laitteelta sanomat tulevat yleensä sarjamuodossa ja ASCII merkkeinä, sarjaliitäntä on 4800 bps. Robottien navigoinnissa voi käyttää esimerkiksi RMC -sanomaa (recommended minimum) [2], joka sisältää seuraavat tiedot:

$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A

Where:
RMC Recommended Minimum sentence C
123519 Fix taken at 12:35:19 UTC
A Status A=active or V=Void.
4807.038,N Latitude 48 deg 07.038' N
01131.000,E Longitude 11 deg 31.000' E
022.4 Speed over the ground in knots
084.4 Track angle in degrees True
230394 Date - 23rd of March 1994
003.1,W Magnetic Variation
*6A The checksum data, always begins with *

Robotin navigoinnissa tarvitsee sanomasta kerätä ainakin latitudi (eli leveys) ja longitudi (eli pituus) tiedot ja kulman (track angle/bearing). Esimerkki sanomien käsittelystä löytyy linkistä [3] löytyvästä ohjelmakoodista tai sivun lopusta löytyvästä koodiesimerkistä.

Navigointi

Robottia voidaan ohjata tiettyyn suuntaan kun tiedetään loppupiste ja robotin sijainti (lähtöpiste).
Navigoinnissa tarvitsee tietää kahden pisteen välisen etäisyyden ja pisteiden välisen kulman. Etäisyystietoa tarvitaan robotin ohjaukseen siihen, että robotti saadaan pysäytettyä kun robotti on riittävän lähellä loppupistettä. Käytännössä robotti ei voi kulkea loppupisteeseen, vaan aina jää pieni virhe.
Kahden pisteen välisen etäisyyden laskemisessa maapallon pinnalla käytetään yleensä Haversine kaavaa [4]. Maapallolla olevien kahden pisteiden välistä etäisyyttä kutsutaan nimellä Great-circle distance [5].

Tämä sivun alla olevassa esimerkissä on käytetty euklidista etäisyyttä laskemaan kahden pisteiden välistä etäisyyttä. Tämä johtuu siitä, että kääntäjä tukee vain 32 bit liukulukuja (float) niin tarkkuus ei riitä laskemaan etäisyyttä Haversine -kaavalla. Pienillä etäisyyksillä pyöristysvirheet ovat suuria, 1 m ja 1 km etäisyyksillä Haversine -kaava antoi saman tuloksen.
Eukliidinen etäisyys on tässä tapauksessa kaksiulotteisella pinnalla, joten pidemmillä etäisyyksillä etäisyyden laskenta ei ole tarkka, mutta tässä soveluksessa tällä ei ole väliä, koska etäisyystietoa käytetään vain robotin pysäytysehdossa.

Kulman laskemiseen käytetään normaaleja trigonometrisia laskuja.

Ohjaamiseen eli robotin kääntelyyn tarvitaan tietää robotin nykyinen kulkusuunta ja nykyisen pisteen ja loppupisteen välinen kulma. GPS -vastaanottimen antamasta RMC -sanomasta saatu kulmatieto on nykyinen robotin kulkusuunta. Laskemalla poikkeama robotin kulkusuunnasta ja kahden pisteen välisestä kulmasta, voidaan robottia ohjata kulkemaan oikeaan suuntaan.
Ohjaus tapahtuu siten, että jos poikkeama on positiivinen, käännetään vasemmalle. Muuten oikealle, eli jos poikkeama on negatiivinen, käännetään oikealle. Poikkeaman suuruutta kannattaa käyttää ohjauksessa, eli siihen miten jyrkkiä käännöksiä robotti tekee.

GPS -vastaanottimelta saadut koordinaatit (latitudi ja longitudi) muutetaan desimaaliasteiksi.
Latitudi ja longitudi tulee GPS- vastaanottimelta (yleensä) asteina ja minuutteina.
Asteita käytetään etäisyyden ja kulman laskemiseen toiseen pisteeseen (joka on myös muutettu desimaliasteiksi).

Esimerkki GPS navigoinnista

Alla on kuva testialueesta, punaisella on merkattu robotin kulkema reitti ja sinisellä neliöllä loppupiste. Robotti ei ole kulkenut loppupisteeseen, vaan jäi noin 1.5 m päähän loppupisteestä. Ohjelmassa pysäytysehtona on ollut alle 1m etäisyys loppupisteeseen.

Alla on video testiajosta, testiajo ei ole sama kuin yllä olevassa kuvassa. Tässäkin robotti pysähtyy noin 1.5 m päähän loppupisteestä.

Linkkejä:

[1] http://en.wikipedia.org/wiki/NMEA_0183
[2] http://www.gpsinformation.org/dale/nmea.htm#RMC
[3] http://kotisivu.dnainternet.net/tiihone/gepsi.htm
[4] http://en.wikipedia.org/wiki/Haversine_formula
[5] http://en.wikipedia.org/wiki/Great-circle_distance

Esimerkki ohjelmakoodista

navigointi

Alla oleva koodi on pääohjelman silmukassa

if(GPS_OK)
{
       GPS_OK = false;
       disable_interrupts(INT_RDA); // sarjaportin keskeytykset pois
       gps_parser();
 
       distance = calcDistance(lat_deg, lon_deg, kohde_lat, kohde_lon); // lat1, lon1, lat2, lon2
       bearing = calcBearing(lat_deg, lon_deg, kohde_lat, kohde_lon);
       relBearing = CalcRelBearing(GPSbearing, bearing);
 
       Steer(relBearing); // ohjataan kulman mukaisesti
 
       if(distance < 0.001f) // 1m kohteeseen niin pysäytetään
             stop(); // printf("STOP \r\n");
 
      enable_interrupts(INT_RDA); // sarjaportin keskeytykset päälle
}

// Calculate relative bearing given bearing and heading
// bearing and heading are in degrees
// This function returns +180 - -180 degrees relative bearing with 0 being right on target.
float CalcRelBearing(float bearing, float heading)
{
   float relBearing;
   relBearing = bearing - heading;
 
   if(relBearing > 180.0)
      relBearing = bearing - (heading + 360);
 
   if(relBearing < -180.0)
      relBearing = bearing - (heading - 360);
 
   return(relBearing);
}

Ohjausfunktio.
jos relBearing on positiivinen, käännä vasemmalle
jos relBearing on negatiivinen, käännä oikealle

void Steer(float relBearing)
{
   signed int16 value = 0;
 
   value = (signed int16)relBearing + 127;  // 127 = keskikohta, riippuu ohjaustavasta
   if(value > 255)  
      value = 255; // maksimiarvo
 
   if(value < 0) 
      value = 0; // minimiarvo
 
   //printf("value: %ld",value); putc(10); putc(13);
   kaanna( (255-value), value ); // riippuu ohjaustavasta
}

etäisyyden laskenta

// Euklidinen etäisyys. Ei ole pitkillä matkoilla tarkka! (3 km etäisyydestä antaa n. 150 m virheen)
// Mutta lähietäisyyksillä ( << 100 m ) toimii.. (virhe n. 1-10 m)
float calcDistance(float lat1, float lon1, float lat2, float lon2) // lat ja lon desimaaliasteina
{
   float xError, yError, dist;
 
   xError = lat2 - lat1;
   yError = lon2 - lon1;
   dist = sqrt((xError*xError)+(yError*yError));
   return rad2dec(dist);
}

kulman laskenta

// Calculate bearing from lat1/lon1 to lat2/lon2
// Note lat1/lon1/lat2/lon2 must be in radians
// Returns float bearing in degrees
float calcBearing(float lat1, float lon1, float lat2, float lon2) // lat ja lon desimaaliasteina
{
   float bearing = 0.0;
   lat1 = dec2rad(lat1);
   lon1 = dec2rad(lon1);
   lat2 = dec2rad(lat2);
   lon2 = dec2rad(lon2);
 
   //determine angle
   bearing = atan2(sin(lon2-lon1)*cos(lat2), (cos(lat1)*sin(lat2)) - (sin(lat1)*cos(lat2)*cos(lon2-lon1)));
   //convert to degrees
   bearing = rad2dec(bearing);
   //use mod to turn -90 = 270
   bearing = fmod((bearing + 360.0), 360);
   return bearing;
}

muunnokset

// degrees to radians
float dec2rad(float dec)
{
  return (PI / 180.0f) * dec;
}
 
// radians to degrees
float rad2dec(float rad)
{
   return (180.0f / PI) * rad;
}

NMEA sanoman käsittely

void serial_isr()  // Serial Interrupt
{
   unsigned char merkki = 0;
 
   merkki=getc(GPS_stream);
   GPS[pituus] = merkki;
   pituus++;
 
   if (pituus > 83)
      pituus = 0;    // too long string
 
   if(merkki == 0x0A)        // end character arrived?
   {
      pituus = 0;
 
      if((GPS[3] == 'R') && (GPS[4] == 'M') && (GPS[5] == 'C'))
         GPS_OK = true;
   }
}

void gps_parser(void)
{
   unsigned char i, temp;
   float lat, lon, latMinutes, lonMinutes;
   int16 latDegrees, lonDegrees;
   unsigned int Length=0;
   int Q = 0;
 
   fix = GPS[18];
 
    for(i=0; i<83; i++)    // seek the places of commas in GPS_string
   {
      temp = GPS[i];
        if(temp == ',')
         comma_vector[Q++] = i;
   }
 
   Length = (comma_vector[3] - comma_vector[2]) -1; // Lat
    for(i=0; i<Length; i++)
   {
      Q = comma_vector[2] + i + 1;
      temp_str[i] = GPS[Q];
   }
   lat = atof(temp_str);
 
   Length = (comma_vector[5] - comma_vector[4]) -1; // Lon
    for(i=0; i<Length; i++)
   {
      Q = comma_vector[4] + i + 1;
      temp_str[i] = GPS[Q];
   }
   lon = atof(temp_str);
 
   Length = (comma_vector[8] - comma_vector[7]) -1; // Bearing
    for(i=0; i<Length; i++)
   {
      Q = comma_vector[7] + i + 1;
      temp_str[i] = GPS[Q];
   }
   GPSbearing = atof(temp_str);
 
   if(GPS[30] == 'S') // N/S
      lat = 0-(lat);
   if(GPS[43] == 'W') // E/W
      lon = 0-(lon);
 
   latDegrees = (int16)(lat/100);
   latMinutes = (float)(lat - latDegrees*100);
   lat_deg  = latDegrees + (latMinutes/60);    // Convert to decimal degrees
 
   lonDegrees = (int16)(lon/100);
   lonMinutes = (float)(lon - lonDegrees*100);
   lon_deg  = lonDegrees + (lonMinutes/60);    // Convert to decimal degrees
}