NXT-Simulink: Gyrosensor Drift

grafische Programmiersprachen für den EV3/NXT.
NXT-G, Robolab, LabView und was noch so schön bunt ist...

Moderator: Moderatoren

Kapaun
Mindstormsfreund
Mindstormsfreund
Beiträge: 8
Registriert: 14. Sep 2012 15:52

NXT-Simulink: Gyrosensor Drift

Beitragvon Kapaun » 14. Sep 2012 16:59

Hallo Forum,

ich versuche gerade über Simulink einen NXT Ballbot zu realisieren. Ich habe aber dabei Probleme den HiTechnic Gyrosensor zu stabilisieren. Ich habe schon die verschiedenen Forenbeiträge die es dazu gibt durchgelesen, sie haben mir aber nur teilweise weitergeholfen.
Ich hab in einen dieser Beitrag folgende Formel gefunden: „Offset_neu=0.99*Offset_alt+0.01*aktuelle_Geschwindigkeit (Zahlen anpassen!)“ … wenn ich jetzt diese Formel anwende, so wird, nach einer gewissen Zeit, der „neue“ Offset seinen Anfangswert wieder erreichen und der Kippwinkel würde dann wieder 0° betragen. Aber das entspricht ja nicht den wahren Winkel, weil wenn ich den Roboter waagrecht stelle so sollte er ja 90° betragen und nicht, nach einer gewissen Zeit, wieder 0°. Kann sein, dass ich da irgendwas nicht richtig verstanden habe oder falsch implementiert habe? Die Offset Berechnung führe ich am Anfang durch, ich mittle über 2 Sekunden die Gyrowerte (Abtastzeit 5ms).

Bin für jeden Tipp dankbar!!!!

Danke schon mal im Voraus
Viele Grüße

Kapaun
Mindstormsfreund
Mindstormsfreund
Beiträge: 8
Registriert: 14. Sep 2012 15:52

Re: NXT-Simulink: Gyrosensor Drift

Beitragvon Kapaun » 24. Sep 2012 14:11

Da ich keine Reaktionen auf meinen letzten Beitrag bekommen habe, gehe ich davon aus, dass ich mein Problem vielleicht nicht ganz verständlich formuliert habe.
Ich bräuchte ein paar Tipps bzw. Anregungen, wie ich aus den Gyrowerten, die mir der Hitechnic Gyrosensor liefert, einen brauchbaren Winkel (Kippwinkel des Roboters) berechnen kann. Zurzeit hab ich das Problem, dass der Winkel weg driftet und diesen Drift versuch ich gerade zu kompensieren, schaff es aber nicht.

Danke&viele Grüße

Benutzeravatar
NXT pro
Super-Enorm-Viel-Schreiber
Super-Enorm-Viel-Schreiber
Beiträge: 930
Registriert: 27. Sep 2010 21:38

Re: NXT-Simulink: Gyrosensor Drift

Beitragvon NXT pro » 24. Sep 2012 16:32

Das Problem, weshalb du hier keine Antwort bekommst, ist wahrscheinlich, dass hier nicht viele den Gyro besitzen...
Real coders don't comment their code - it was hard to write, it should be hard to understand!

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

Re: NXT-Simulink: Gyrosensor Drift

Beitragvon HaWe » 24. Sep 2012 17:12

...oder falls doch, dass sie nicht mit NXT-G programmieren...
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E

Kapaun
Mindstormsfreund
Mindstormsfreund
Beiträge: 8
Registriert: 14. Sep 2012 15:52

Re: NXT-Simulink: Gyrosensor Drift

Beitragvon Kapaun » 24. Sep 2012 20:13

Kann sein dass der Fehler daran liegt, dass ich es in Simulink implementiere und ich es dann mittels Real Time Workshop direkt auf die NXT rüber spiele? Vielleicht entstehen bei der Übersetzung Fehler (Genauigkeit, Datenformat, …)?
Wenn ich es direkt in C programmiere, reicht es da aus, wenn ich am Anfang den Offset bestimme und dann während dem Betrieb ständig den Offset anpasse mit oben genannter Formel? Oder muss ich auch die Spannung des Akkupacks berücksichtigen?

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

Re: NXT-Simulink: Gyrosensor Drift

Beitragvon HaWe » 24. Sep 2012 21:18

An die Akku-Spannung anpassen reicht nicht, der Gyro driftet auch durch andere Effekte, allein durch wechselnde Motordrehung. Das ist leider ein Konstruktionsmangel.
Wenn er allerdings anfangs ein paar Minuten nachkalibriert ist, hält er seine Werte grad so, dass man ihn gebrauchen kann.
Ich habe es nie hinbekommen, die Gyrodrift zu beseitigen, aber das ist für einen Segway gar nicht so wichtig.
Wichig ist vielmehr, schnell und dynamisch auf Kippbewegungen durch Gegenbewegungen zu reagieren. Dazu gibt es Programmbeispiele auf der website des Gyro-Herstellers!
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E

Kapaun
Mindstormsfreund
Mindstormsfreund
Beiträge: 8
Registriert: 14. Sep 2012 15:52

Re: NXT-Simulink: Gyrosensor Drift

Beitragvon Kapaun » 25. Sep 2012 08:15

Wie meinst du das jetzt genau, „anfangs ein paar Minuten nachkalibriert“? Wird die Drift Kompensation nicht ständig durchgeführt?
Ich verwende zur Regelung einen Zustandsregler und das System wurde um die Ruhelage (senkrechte Position) linearisiert und einer meiner Zustandsgrößen ist eben der Kippwinkel. Wenn mir jetzt der Winkel weg driftet, auch nur um 1 Grad, so erhalte ich ja eine falsche Stellgröße und der Roboter kippet um. Also ich weiß nicht, ob ich den Drift einfach vernachlässigen kann… :?

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

Re: NXT-Simulink: Gyrosensor Drift

Beitragvon HaWe » 25. Sep 2012 08:23

also - die gute Nachricht ist: es funktioniert, wenn man es richtig programmiert.
Probier erst mal die Hitechnic-Programme aus!
(Die Drift geht übrigens tatsächlich nach ein paar Minuten auf einen recht geringen Wert zurück)
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E

Kapaun
Mindstormsfreund
Mindstormsfreund
Beiträge: 8
Registriert: 14. Sep 2012 15:52

Re: NXT-Simulink: Gyrosensor Drift

Beitragvon Kapaun » 25. Sep 2012 17:26

Ich habe es jetzt mal in NXT-C versucht, ich berechne zuerst den Offset und dann führe ich eine Drift Kompensation durch, aber nur wenn sich der Roboter nicht bewegt. Es funktioniert aber nur teilweise, der Winkel driftet immer noch ein wenig weg. Wie sieht eigentlich deine Kompensation aus?

Hier mein Quellcode:

Code: Alles auswählen

#define GYRO IN_1

float GyroOffset=0;
float SampleTime, tCalcStart;
float OffsetParam = 0.0005;


void CalcInterval(long cLoop)
{
  if (cLoop == 0) {
    // First time through, set an initial tInterval time and
    // record start time
    SampleTime = 0.0055;
    tCalcStart = CurrentTick();
  } else {
    // Take average of number of times through the loop and
    // use for interval time.
    SampleTime = (CurrentTick() - tCalcStart)/(cLoop*1000.0);
  }
}

void SetGyroOffset()
{
  float old, delta;
  old=GyroOffset;
  do {
    float tot = 0;
    for (int i=0; i<200; ++i)
    {
       tot += SensorHTGyro(GYRO); // tot var is 200 gyro readings
       Wait(5);                     // 5ms between analog readings
    }

    GyroOffset=tot/200.0;
    delta=old-GyroOffset;
    old=GyroOffset;
  } while (abs(delta)>0.005);
}

task main() {
  float GyroValue, GyroSpeed, GyroOffsetFirst;
  float GyroAngle = 0;
  int cLoop = 0;

  SetSensorHTGyro(GYRO);
  Wait(50);
  SetGyroOffset();
  GyroOffsetFirst = GyroOffset;
  while (true)
  {
          CalcInterval(cLoop++);
          GyroValue = SensorHTGyro(GYRO);
          GyroSpeed = GyroValue - GyroOffset;
          if (abs(GyroValue - GyroOffsetFirst) <= 2)
            GyroOffset = OffsetParam * GyroValue + (1-OffsetParam) * GyroOffset;
          GyroAngle = GyroAngle + GyroSpeed * SampleTime;

          ClearScreen();
          NumOut(0, LCD_LINE1,GyroValue);
          NumOut(0, LCD_LINE2,GyroSpeed);
          NumOut(0, LCD_LINE3,GyroOffset);
          NumOut(0, LCD_LINE4,GyroAngle);

          Wait(5);
  }
}

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

Re: NXT-Simulink: Gyrosensor Drift

Beitragvon HaWe » 25. Sep 2012 22:47

edit: noch ausführlicher, vervollständigt:

Code: Alles auswählen

//--------------------------------------------------
// sensor calibration
//--------------------------------------------------
float GyroHdg, GyroValue, GyroHdgOld, GyroOffset=0.0;
char  GYRO=S1;  // gyro port S1, to be adjusted

unsigned long startTGyro=0, dTGyro;

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);                     // 4 ms between analog readings
    }

    GyroOffset=tot/200.0;
    delta=old-GyroOffset;
    old=GyroOffset;
  } while (abs(delta)>0.005)
  Wait(20);

}

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

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 200gyro readings
       Wait(4);                     // 4 ms between analog readings
    }
    GyroOffset=tot/200.0;
    delta=old-GyroOffset;
    old=GyroOffset;

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


task main() {

  //...
  TestGyroOffset();  // <==== just call on me ! ;)
  //...
  GyroValue=(SensorHTGyro(GYRO,0) - GyroOffset) ;  // read calibrated gyro value
  //...

  startTGyro=CurrentTick();
  Wait(20);

  while(1) {                                       // "integration": gyro rotation speed  * ?t ===> total rotation angle (degrees)

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

     startTGyro = CurrentTick();
     Wait(20);
  }
}
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E

Kapaun
Mindstormsfreund
Mindstormsfreund
Beiträge: 8
Registriert: 14. Sep 2012 15:52

Re: NXT-Simulink: Gyrosensor Drift

Beitragvon Kapaun » 26. Sep 2012 11:13

Beobachte mal bei deinen Code den Winkel, wenn du den Roboter bewegst. Der driftet ja ziemlich weg. Eine Kompensation des Gyro Drift hast du also nicht implementiert, du ziehst nur den am Anfang berechneten Offset ab?
Der NXT-C Code auf der Hytechnic Seite funktioniert nicht so schlecht, nur wird dort der Winkel immer um 0° Grad stabilisiert, also man geht davon aus, dass sich der Roboter immer um die 0° Position bewegt.
Meinst du es macht Sinn sich noch weiter mit dieser Drift Kompensation zu beschäftigen, indem man versucht den Einfluss der Motoren, Spannung, Temperatur zu berücksichtigen oder ist besser man lässt es sein und verwendet einen angepassten Hytechnic Code?

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

Re: NXT-Simulink: Gyrosensor Drift

Beitragvon HaWe » 26. Sep 2012 11:52

jein - das ist nur ein Beispiel-Code, damit du nicht überfordert bist.^^
In meiner eigenen Anwendung teste ich auch immer wieder mal in der Schleife. Ich habe aber einen Navigations-Roboter, den kann ich zum Kalibrieren jederzeit mal kurz stoppen.

Für deine Zwecke werden 200 samples aber zu lange dauern, da ist der Segway längst umgekippt - probier's also mal mit deutlich weniger.

Ich würde den HT code als Grundlage nehmen.
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E

Kapaun
Mindstormsfreund
Mindstormsfreund
Beiträge: 8
Registriert: 14. Sep 2012 15:52

Re: NXT-Simulink: Gyrosensor Drift

Beitragvon Kapaun » 26. Sep 2012 15:44

Das Problem beim HT Code ist, dass wenn ich den Segway hin und her bewege, so bekommt er einen positiven bzw. negativen Winkel bevor er überhaupt die waagrechte Position überschritten hat und anschließend geht er zurück auf null. Ich glaub aber dass man das mit einem „gutmütigen“ Regler in Griff bekommt.
Problematisch wird es jetzt nur sobald ich die Motoren hinzu schalte. Der ursprüngliche Offset ändert sich dabei um 1-2°/Sek. Ich vermute stark, dass die Ursache dieser Schwankung von der Betriebsspannung abhängt. Hast du das bei deinen Roboter Berücksichtigst oder vernachlässigst du es?

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

Re: NXT-Simulink: Gyrosensor Drift

Beitragvon HaWe » 26. Sep 2012 16:12

1-2°/sec ist zu viel. da musst du am Anfang noch länger warten, bis die drift nachlässt, evtl. dabei zwischendurch die Motoren an- und wieder ausschalten.
Bei mir verwende ich den Gyro waagerecht und kompensiere ihn mit einem Kompass. Man könnte auch einen Accelerometer nehmen. Und als Filter am besten einen Kalman - wenn man's kann.
(Das hab ich aber nie hingekriegt (ketaljen weiss Bescheid - gell, ketaljen?^^), also nehme ich sowas wie einen gleitenden Median, der sprunghafte Extrem-Peaks filtert, und einen weiteren Filter, der gegenläufige Änderungen ignoriert.)
Wie macht es denn Hitechnic? Funktioniert der Modell-Segway von der HP etwa nicht?
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E

Kapaun
Mindstormsfreund
Mindstormsfreund
Beiträge: 8
Registriert: 14. Sep 2012 15:52

Re: NXT-Simulink: Gyrosensor Drift

Beitragvon Kapaun » 3. Okt 2012 17:23

Sry wenn ich mich erst jetzt wieder melde, konnte aber die letzten Tage nicht an den Projekt weiter arbeiten.
Kalman Filter scheidet bei mir aus, weil das ja nichts anderes als ein besserer Beobachter ist und laut Prof. muss da der Sensor schon ziemlich schlecht sein, dass man eine Größe beobachtet und nicht misst.
Hitechnic schaut immer dass der Winkel zurück auf null geht sobald der Roboter in Ruhe ist. Es ist also keine exakte Winkelmessung und da ich ja einen Zustandsregler benutzte und einer meiner Zustände ja der Kippwinkel ist, ist diese Methode ziemlich unbrauchbar.
Zurzeit berechne ich am Anfang den Offset und zieh den dann ab. Die Idee dabei die Motoren laufen zu lassen hat zwar eine Verbesserung gebracht, aber das Problem nicht ganz gelöst. Würde es einen Sinn machen mehrere Offsets zu berechnen bei unterschiedlichen Motorgeschwindigkeiten und je nachdem wie stark der Motor später dreht diese dann abzuziehen? Berechnest du den Offset während des Betriebs nochmal neu oder berechnest du ihn nur am Anfang?
Wie sieht eigentlich dein Filter aus der die gegenläufigen Änderungen ignoriert? Wenn du den Gyro über den Kompass kompensierst, stellst du da einfach den Winkel zurück auf 0°?


Zurück zu „grafische Programmiersoftware“

Wer ist online?

Mitglieder in diesem Forum: 0 Mitglieder und 7 Gäste

Lego Mindstorms EV3, NXT und RCX Forum : Haftungsauschluss