Mindstormsforum
Dateiupload | Links | Lexikon | NXT Shop | RobotC.de | Wettbewerbe |

NXT NXC: Navigation mit Odometrie, Gyro, Kompass

Modelle zum Nachbauen oder wo gibt es etwas interessantes oder Projekte?

Moderator: Moderatoren

Benutzeravatar
HaWe
Administrator
Administrator
Beiträge: 4565
Registriert: 11. Jan 2006 21:01
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze

NXT NXC: Navigation mit Odometrie, Gyro, Kompass

Beitragvon HaWe » 20. Aug 2011 12:29

hi,

Hier der Navigations-Roboter mit Odometrie, Gyro und Kompass, alle Sensoren werden dabei nach Plausibilität ihrer Werte verknüpft.
Ich habe das Programm mal aus dem Gyro-Thread ausgekoppelt, weil diese Version umfangreicher ist.
Danke an ketaljen und Alphasucht für eure Ideen und Diskussionsbeiträge!
Robot640.jpg
Johnny 1


Zwischenstand: Tetrix-Tribot-Chassis, Räder mit ca. 8cm Durchmesser, Antrieb 1:2 untersetzt (Motorachse gegenüber Radachsen), Achsenlänge (Radabstand) ca. 30cm;
max. Geschwindigkeit: 360°/1s = ca. 25cm/s.
bei Dreiecksfahrt (3 virtuelle Raumpunkte) über ca. 5m: Fehler am Start/Zielpunkt mit Doppel-Antriebsachse (grob vergleichbar mit Kettenantrieb) 20cm, mit einfacher Antriebsachse (wie bei Tribot) <10cm, meist noch weniger.
Stärkste Fehlerquellen: Gyro-Drift und Kompass-Störfelder in Räumen.
Bei wiederholtem Runden-Fahren (5x, also 25 m ) schwankt die Genauigkeit (Distanzbetrag) um den Start-Zielpunkt im Bereich um +- 2%.
Hier ist der letzte Entwicklungsstand OHNE stochastischen Filter (nur Sensor-Mittelwerte etc.), evtl. kommt bald noch ein Accelerometer (Beschleunigungs-Sensor) dazu, vllt sogar einmal ein Kalmanfilter.

Plan für weitere künftige Verbesserung:
eigener low-priority Task (delay: 200 ms) nur für low-pass-Filter sowohl für Kompass als auch für Gyro.
Die Differenz beider Ergebnisse ermitteln => =Gyro-Offset !
Rekalibrierung nur, wenn aktuelle Offset-Änderung größer als 2 Standardabweichungen vom bisherigen Mittelwert.

Code: Alles auswählen

// Navigator Robot, TriBot architecture
// for NXC, tested with BricxCC 3.3.8.10 (2011-July-03)
// version 0.43
// (c) HaWe 2011
// based on:
// Odometry
// Compass
// Gyro sensor

//--------------------------------------------------
// mechanics, motors + sensors
//--------------------------------------------------

#define WheelDiameter   7.7    // Raddurchmesser
#define WheelGauge     28.8    // Achsbreite (Mittellinie Antriebsräder)
#define WheelGearRatio  0.5    // Untersetzungsgetriebe 1:2


#define Mleft   OUT_B
#define Mright  OUT_C
#define Mboth   OUT_BC
#define Menc(a) MotorRotationCount(a)

#define SMenc            1
#define SEncoderLeft     2
#define SEncoderRight    3
#define SCompass        10
#define SGyro           11
#define SAccel          12
#define SUSFrLeft       20
#define SUSFrRight      21
#define SUSFrMiddle     22
#define SUSLeftSide     23
#define SUSRightSide    24
#define SUSBackw        25


                               // Radumfang
float WheelCircumference   = PI*WheelDiameter;   // 2*PI*r = PI*d

                               // Abrollstrecke pro 1° MotorEncoderTick
float WheelLegPerEncDegree = (PI*WheelDiameter*WheelGearRatio)/360.0;

float MencHdg;                // Heading to Odometry, geographical sense

float fxpos=0, fypos=0;


//--------------------------------------------------

#define GYRO    S3
#define COMPASS S4

float CompHdg;
float GyroHdg, GyroValue, GyroOffset=0.0;


//--------------------------------------------------
// IO functions
//--------------------------------------------------

#define Beep(f,d) PlayTone(f,d)

#define printf1( _x, _y, _format1, _value1) { \
  string sval1 = FormatVal(_format1, _value1); \
  TextOut(_x, _y, sval1); \
}

const char LCDline[]={56,48,40,32,24,16,8,0};

const string clrln="                 ";

inline bool btnhit(){
   return ( ButtonPressed(BTN1, false) || ButtonPressed(BTN2, false)
         || ButtonPressed(BTN3, false) || ButtonPressed(BTN4, false));
}



//--------------------------------------------------
// math
//--------------------------------------------------

#define min(a,b) (a<b?a:b)

#define max(a,b) (a>b?a:b)

inline long round(float f) { return ((f>=0) ? (f + 0.5) : (f - 0.5)); }


float ArrayDegreesMean (float src[], int len) {
   int i;
   for (i=0; i<len; ++i) {
     if (src[i]>180) src[i]=(src[i]-360);
   }
   return (ArrayMean(src, NA, NA));
}


inline float ArrayMedianF(float src[], int len)
{
  float ftemp[];
  ArraySort(ftemp, src, NA, NA)
  return ftemp[(len-1)/2];
}


//--------------------------------------------------
// sensor calibration
//--------------------------------------------------


safecall void SetGyroOffset()
{
  float old, delta;
  old=GyroOffset;
  do {
    float tot = 0;
    for (int i=0; i<200; ++i)
    {
       tot += SensorHTGyro(GYRO, 0); // tot var is 200 gyro readings
       Wait(4);                     // 5ms between analog readings
    }
    Beep(880,100);
    GyroOffset=tot/200.0;
    delta=old-GyroOffset;
    old=GyroOffset;
  } while (abs(delta)>0.005)
  Wait(20);
  CompHdg= SensorHTCompass(COMPASS);
  GyroHdg=MencHdg=CompHdg;
}

//--------------------------------------------------

safecall void TestGyroOffset() {
  float old, delta;
  old=GyroOffset;

    float tot = 0;
    for (int i=0; i<200; ++i)
    {
       tot += SensorHTGyro(GYRO, 0); // tot var is 200 gyro readings
       Wait(4);                     // 5ms between analog readings
    }
    Beep(880,100);
    GyroOffset=tot/200.0;
    delta=old-GyroOffset;
    old=GyroOffset;

  if (abs(delta)>0.01)  SetGyroOffset();
}

//--------------------------------------------------
// move commands
//--------------------------------------------------

void SpinUntilHdg(float TargHdg, char tSensor) {

  float currHdg, dHdg;
  int dirle, dirri, speedle, speedri, dle, dri, nspeed=80;

  if (tSensor==SCompass)  {currHdg=SensorHTCompass(COMPASS);}
  else
  if (tSensor==SGyro)     {currHdg=GyroHdg;}
  else
  if (tSensor==SMenc)     {currHdg=MencHdg;}
 
  dHdg=TargHdg-currHdg;
  speedle=speedri=20;

  while(TargHdg < 0)  TargHdg+=360;
  while(TargHdg>=360) TargHdg-=360;

  while(abs(dHdg)>0.5) {

    if ( (abs(dHdg)>10) || ( (speedle<30) && (speedri<30)))  {  // ramp up
      if (speedle<nspeed) speedle+=5;
      if (speedri<nspeed) speedri+=5;
    }

    if (tSensor==SCompass)  {currHdg=SensorHTCompass(COMPASS);}
    else
    if (tSensor==SGyro)     {currHdg=GyroHdg;}
    else
    if (tSensor==SMenc)     {currHdg=MencHdg;}

    dHdg=TargHdg-currHdg;

    if (dHdg<-180) dHdg= 360-dHdg;
    if (dHdg>=180) dHdg-=360;

    dirle=(dHdg>=0?1:-1);
    dirri=-dirle;
 
    OnFwd(Mright, dirri*(speedri+dri));
    OnFwd(Mleft,  dirle*(speedle+dle));

    if (abs(dHdg)<50) {
      if (dHdg<0) {
        speedle = abs(dHdg)*5;             // left turn:    // ramp down
        speedri = abs(dHdg)*5;
      }
      else if (dHdg>0) {
        speedle = abs(dHdg)*10;            // right turn:   // later ramp down
        speedri = abs(dHdg)*10;
      }

      if (speedle>nspeed) speedle=nspeed;
      if (speedle<30) speedle=30;

      if (speedri>nspeed) speedri=nspeed;
      if (speedri<30) speedri=30;
    }
   
    Wait(10);
  }
  Off (Mboth);
  Beep(100,100);  Wait(100);
  Coast(Mboth);
 
}

//--------------------------------------------------


void MoveToPoint(float xTarg, float yTarg) {

  float currHdg, dHdg, tx, ty, TargHdg, total, rest;
  int dirle, dirri,  speed, dle, dri, nspeed=80;

  tx=xTarg-fxpos;
  ty=yTarg-fypos;
  TargHdg=atan2(tx,ty)*(180/PI) ;
  total=sqrt(tx*tx+ty*ty);
 
  SpinUntilHdg(TargHdg, SGyro);
  if (abs(MencHdg-GyroHdg)>4) {
    MencHdg=GyroHdg;
  }

  speed=20;

  while(TargHdg < 0)  TargHdg+=360;
  while(TargHdg>=360) TargHdg-=360;

  while( (abs(fxpos-xTarg)>1) || (abs(fypos-yTarg)>1)) {
    rest= sqrt(tx*tx+ty*ty);

    if ( (rest>3) || (speed<30) )  {  // ramp up
      if (speed<nspeed) speed+=5;
     }
    if (abs(MencHdg-GyroHdg)>4) MencHdg=GyroHdg;
    currHdg=MencHdg;

    tx=xTarg-fxpos;
    ty=yTarg-fypos;
    TargHdg=atan2(tx,ty)*(180/PI) ;
    while (TargHdg >=360) TargHdg-=360;
    while (TargHdg < 0)   TargHdg+=360;
   
    dHdg=TargHdg-currHdg;
    if (dHdg<-180) dHdg= 360-dHdg;
    if (dHdg>=180) dHdg-=360;
   

    if  (abs(dHdg>10)) {
      Off(Mboth); Wait(100);
      tx=xTarg-fxpos;
      ty=yTarg-fypos;
      TargHdg=atan2(tx,ty)*(180/PI) ;
      total=sqrt(tx*tx+ty*ty);
      MencHdg=GyroHdg;
      SpinUntilHdg(TargHdg, SGyro);
      MencHdg=GyroHdg;
    }
    else
    if (abs(dHdg>1) || (rest<30) || (rest>total-10))  {
      if ((dHdg)>0) {
        Coast(Mright);
        OnFwd(Mleft, speed);
      }
      if ((dHdg)<0) {
        Coast(Mleft);
        OnFwd(Mright, speed);
      }
    }
    else OnFwd(Mboth, speed);
   
    if (rest<5) {                                         // ramp down
      speed = rest*200/nspeed;

      if (speed>nspeed) speed=nspeed;
      if (speed<30) speed=30;
    }
    Wait(10);

  }

  Off (Mboth);
  Beep(100,100);  Wait(100);
  Coast(Mboth);
 
}


//--------------------------------------------------
// motor control
//--------------------------------------------------

float VwMleft, VwMright;  // deg per sec

task MotorSpeed() {
 float Mle, OMle, dVMle;
 float Mri, OMri, dVMri;

 while (1) {
     Mle=Menc(Mleft);
     Mri=Menc(Mright);
     dVMle=Mle-OMle;
     dVMri=Mri-OMri;
     OMle=Mle;
     OMri=Mri;
     VwMleft=dVMle*(100);
     VwMright=dVMri*(100);
     Wait(10);
  }
}

//------------------------------------------------------------------------------


task Navigator(){
  long itemp;
  float ftemp, dfG, dfC, dfE;
  float fbuf[3];
  long  encLeft, encRight, encLeftOld, encRightOld, dEncLeft, dEncRight;
  float fxold=0, fyold=0, Urw, Ulw, Um, dMencHdgRad, MencHdgRad;
 
  float GyroHdgRad, GyroHdgOld, dGyroHdg, dGyroHdgRad;
  float CompHdgRad, CompHdgOld, dCompHdg, dCompHdgRad;

  long startTComp, startTGyro, startTMenc, dTComp, dTGyro, dTMenc;

  encLeft =Menc(Mleft);
  encRight=Menc(Mright);

  //-------------------------------------------//

  CompHdg=SensorHTCompass(COMPASS);

  //-------------------------------------------//

  GyroHdg=MencHdg=CompHdg;

  startTComp=startTGyro=startTMenc=CurrentTick();

     //-------------------------------------------//

  while(1) {

     if (ButtonPressed(BTNCENTER, false)) {  // press BTNCENTER:
        SetGyroOffset();                    // recalibrate gyro offset
     }

     if (ButtonPressed(BTNLEFT, false)) {          // press BTNLEFT:
        MencHdg=GyroHdg=SensorHTCompass(COMPASS);  // reset Hdgs to compass
        while (btnhit()); Beep(1000,10);
     }

     if (ButtonPressed(BTNRIGHT, false)) {   // press  BTNRIGHT:
        fxpos=fypos=0;                       // reset x,y to 0
        while (btnhit()); Beep(1000,10);
     }

     CompHdgOld=CompHdg;
     startTComp = CurrentTick();

     CompHdg=SensorHTCompass(COMPASS);       //----------------------------//
     dTComp = CurrentTick() - startTComp;    // Compass heading            //
     CompHdgRad=CompHdg*PI/180;              //----------------------------//

     dCompHdg=CompHdg-CompHdgOld;
     if (dCompHdg<-180) dCompHdg= 360-dCompHdg;
     if (dCompHdg >180) dCompHdg-=360;
     dCompHdgRad=dCompHdg*PI/180;
     
     startTComp = CurrentTick();

     //-------------------------------------------//

     GyroHdgOld=GyroHdg;
     GyroValue=(SensorHTGyro(GYRO,0) - GyroOffset) ;    //
     dTGyro = CurrentTick() - startTGyro;
     GyroHdg+=GyroValue*dTGyro /1000.0;

     startTGyro = CurrentTick();

     while (GyroHdg>=360) GyroHdg-=360;      //----------------------------//
     while (GyroHdg<  0)  GyroHdg+=360;      // Gyro heading               //
     GyroHdgRad=GyroHdg*PI/180;              //----------------------------//
     
     dGyroHdg=GyroHdg-GyroHdgOld;
     if (dGyroHdg<-180) dGyroHdg= 360-dGyroHdg;
     if (dGyroHdg >180) dGyroHdg-=360;
     dGyroHdgRad=dGyroHdg*PI/180;

     //-------------------------------------------//
     
     encLeftOld =encLeft;                       // save old encoder values
     encRightOld=encRight;
     encLeft =Menc(Mleft);                      // get new encoder values
     encRight=Menc(Mright);

     dEncLeft=encLeft-encLeftOld;
     Ulw= WheelLegPerEncDegree*dEncLeft;       // left wheel leg way
     dEncRight=encRight-encRightOld;
     Urw= WheelLegPerEncDegree*dEncRight;      // right wheel leg way

     Um=(Urw+Ulw)/2;                                // intermedium leg way

     dMencHdgRad=atan2((Ulw - Urw),WheelGauge);     // delta heading !! GEO !!

     //-----------------------------------------------------------------------//

     if (abs(dCompHdgRad-dGyroHdgRad)<(PI/90))
        dMencHdgRad=(dGyroHdgRad+dCompHdgRad)/2;
     else {
       fbuf[0]=dCompHdgRad;
       fbuf[1]=dGyroHdgRad;
       fbuf[2]=dMencHdgRad;
       ftemp=ArrayMean(fbuf,NA, NA);
       dfC=abs(dCompHdgRad-ftemp);
       dfG=abs(dGyroHdgRad-ftemp);
       dfE=abs(dMencHdgRad-ftemp);
       if ((dfC<dfE) && (dfG<dfE)) {MencHdgRad=ArrayMedianF(fbuf, 3); }
     }
     //-----------------------------------------------------------------------//

     fxold=fxpos; fyold=fypos;                         // save old pos (x,y)

     MencHdgRad=MencHdg*PI/180;                        // Hdg in deg => rad

     fxpos=fxold + Um*(sin((MencHdgRad+(dMencHdgRad /2))));  // new xpos !! GEO !!
     fypos=fyold + Um* cos(-(MencHdgRad+(dMencHdgRad /2)));   // new ypos !! GEO !!

     MencHdgRad = MencHdgRad + dMencHdgRad;        // new motor enc Heading
     MencHdg=MencHdgRad*180/PI;
                                              //----------------------------//
     while (MencHdg >=360) MencHdg-=360;      // odometry heading           //
     while (MencHdg < 0)  MencHdg+=360;       //----------------------------//
     
     //-----------------------------------------------------------------------//
     fbuf[0]=CompHdg;
     fbuf[1]=GyroHdg;
     fbuf[2]=MencHdg;
     MencHdg=ArrayMedianF(fbuf,3);
     MencHdgRad=MencHdg*PI/180;
     //-----------------------------------------------------------------------//

     Wait(19);
  }
}

//------------------------------------------------------------------------------

task DisplayValues() {
  float fCompass;
  while (1) {
     fCompass=SensorHTCompass(COMPASS);
     
     printf1(0, 56, "mot_B%6d", Menc(Mleft));
     printf1(0, 48, "mot_C%6d", Menc(Mright));

     printf1(0, 40, "GValu %9.5f", GyroValue);
     printf1(0, 32, "GOffs %9.5f", GyroOffset);
     printf1(0, 24, "GyroHdg %8.3f", GyroHdg);
     printf1(0, 16, "MencHdg %8.3f", MencHdg);
     printf1(0,  8, "CompHdg %8.3f", fCompass);
     
     printf1(0,  0, "x %5.1f", fxpos); printf1(50, 0, "y %5.1f", fypos);

     Wait(20);
  }
}


//------------------------------------------------------------------------------
void InitGyro() {
    int OldHdg;
   
    OldHdg=SensorHTCompass(COMPASS);
    Wait(20);

    SpinUntilHdg(OldHdg-90, SCompass);
    CompHdg=SensorHTCompass(COMPASS);
    Wait(2000);

    SpinUntilHdg(OldHdg, SCompass);
    CompHdg=SensorHTCompass(COMPASS);
    SetGyroOffset();
}
//------------------------------------------------------------------------------

task main(){
  float fold;
  SetSensorHTGyro(GYRO);
  SetSensorLowspeed(COMPASS);

  start DisplayValues;
  start MotorSpeed;

  while(!btnhit());

  if (ButtonPressed(BTNCENTER, false))  {
    while (!ButtonPressed(BTNCENTER, false));
    InitGyro();
  }
 
  start Navigator;

rep:
  Wait(1000);
  fold=CompHdg;

  MoveToPoint(150,100);
  Wait(2000);  MencHdg=GyroHdg;

  MoveToPoint(150,-100);
  Wait(2000);  MencHdg=GyroHdg;

  MoveToPoint(0,0);
  Wait(2000);  MencHdg=GyroHdg;

  SpinUntilHdg(fold, SCompass);
  Wait(2000);  MencHdg=GyroHdg=CompHdg;


  while(1) {
    if (ButtonPressed(BTNCENTER, false))  {
    while (!ButtonPressed(BTNCENTER, false));
    InitGyro();
    goto rep;
  }
  }
}



keywords: navigation, navigator, dead reckoning, odometry, gyro, Kompass, compass, accelerometer, sensor fusion, statistisch statistischer statistical statistic filtering, stochastisch stochastischer stochastic stochastical filter
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E

Zurück zu „Projekte, Showcase, Bauanleitungen“

Wer ist online?

Mitglieder in diesem Forum: Peter28 und 1 Gast

Lego Mindstorms EV3, NXT und RCX Forum : Haftungsauschluss