Themabewertung:
  • 0 Bewertungen - 0 im Durchschnitt
  • 1
  • 2
  • 3
  • 4
  • 5
"Eigenbau" Gyro: GY521(MPU6050) + Arduino
25-11-2014, 23:13 (Dieser Beitrag wurde zuletzt bearbeitet: 25-11-2014 23:16 von Schattenleufer.)
Beitrag #25
RE: "Eigenbau" Gyro: GY521(MPU6050) + Arduino
So es gibt auch mal wieder ein Update:
Da das Lenkservo z.B. beim Lastwechsel doch etwas zappelig war, habe ich mal verschiedene Filter probiert...
Am besten funktionierte das sogenannte Kalman-Filter. Die Funktion zu erklären ist nicht ganz einfach und ich verzichte an der Stelle darauf.

Hier das "wissenschaftliche" Ergebnis:
Die beiden Kurven zeigen das Servo-PWM-Signal (zwischen 1000 und 2000 us, also Vollausschlag links bzw. rechts) über der Zeit.
[Bild: Kalman.jpg]
Blau ist das bisherige Signal. Man kann ganz gut erkennen, dass beim Lastwechsel (bei über/unterschreiten der 1500us) das Signal meistens kurz die Richtung wechselt. Beim Fahren hat sich das dann in einem kurzen "Zucken" der Lenkung in die falsche Richtung bemerkbar gemacht.

Rot ist die neue Kurve mit dem gefilterten Signal. Der Lastwechsel verläuft jetzt ohne kurze Richtungsänderung und damit auch ohne, dass das Servo zuckt (merkt bzw. sieht man direkt beim Fahren/Driften). Extremstellen werden auch etwas gedämpft.

Insgesamt läuft das ganze jetzt um einiges smoother.
Hier noch etwas Video-Material von der Modellmesse Süd in Stuttgart, an der ich das ganze stundenlang getestet habe. Der Arduino-Gyro ist im roten Evo X.
https://www.youtube.com/watch?v=WErhtZjDONY#t=4m3s


Hier noch der Kalman-Algorithmus:
(An der Filter-Stärke kann natürlich auch noch sehr viel verstellt werden...)
Code:
double q = 4; //process noise covariance
double r = 16; //measurement noise covariance
double p = 2000; //estimation error covariance
double k = 0; //kalman gain
int16_t x = 1500; //value

  p = p + q;
  k = p / (p + r);
  x = x + k * (measurement - x);
  p = (1 - k) * p;

Und hier noch der gesammte Code:
Code:
// Drift Gyro
// Invensense MPU6050 Sensor und Arduino Pro Mini
//
// Version 1.0
// - SteeringIn Signal mit Kalman-Filter
//
// Version 0.9
// - RAW Daten Filter Mittelwert (bringt nichts)
//
// Version 0.8
// - RAW Daten Filter Kalman (bringt verzögerung)
//
// Version 0.6
// - ServoGeschwindigkeit einstellbar
//
// Version: 0.5
// -mit PinChangeInt.h zum Auslesen aller 3 RC-Channels
// -Bremslicht-Funktion hinzugefügt

#define NO_PORTB_PINCHANGES // to indicate that port b will not be used for pin change interrupts
#define NO_PORTC_PINCHANGES // to indicate that port c will not be used for pin change interrupts
#include <PinChangeInt.h>
#include <Servo.h>
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

// Assign your channel in pins
#define THROTTLE_IN_PIN 4
#define STEERING_IN_PIN 2
#define AUX_IN_PIN 3

// Assign your channel out pins
#define STEERING_OUT_PIN 9

// Assign your Brake-Light pin
#define BRAKE_PIN 8

// this is the duration in µs of min, neutral, max Position of Servo
#define MIN_STEERING 1000     //1000
#define MAX_STEERING 2000     //2000
#define NEUTRAL      1500     //1500

// These bit flags are set in UpdateFlags to indicate which
// channels have new signals
#define THROTTLE_FLAG 1
#define STEERING_FLAG 2
#define AUX_FLAG 4

// holds the update flags defined above
volatile uint8_t UpdateFlags = 0;

// Servo Speed
#define SERVO_SPEED 80 // Geschwindigkeit in 0..100%
int SteeringInOld = NEUTRAL;
int SteeringInDifference = 0;

// Variabeles for the RC-Channel reading
volatile int ThrottleIn = NEUTRAL;
volatile int SteeringIn = NEUTRAL;
volatile int AuxIn = NEUTRAL;
volatile unsigned long ThrottleInStart;
volatile unsigned long SteeringInStart;
volatile unsigned long AuxInStart;

Servo SteeringServo; // Lenkservo
int GyroVal;         // Gyro-Eingriff in µs
int gain = 50;       // Verstärkung in % (0..100%)
int gainNew = 50;    // Neuer gain-Wert
MPU6050 mpu;         // Sensor Invensense MPU6050
int16_t gx, gy, gz;  // Gyroskop RAW-Daten

// Eindimensionales lineares Kalman-Filter
double q = 4; //process noise covariance 4, höher -> direkter
double r = 16; //measurement noise covariance 32, kleiner -> direkter
double p = 2000; //estimation error covariance, wird stets neu berechnet
double k = 0; //kalman gain, wird berechnet
int16_t x = 1500; //value 1500 Mittelstellung, Ergebnis wird berechnet
int16_t kalman_update(int16_t measurement); // Funktion gibt aktuellen Wert x aus


void setup()
{
  Serial.begin(115200);

  Serial.println("Drift Gyro");
  
  pinMode(BRAKE_PIN,OUTPUT);
  
  SteeringServo.attach(STEERING_OUT_PIN, MIN_STEERING, MAX_STEERING);
  
  // using the PinChangeInt library, attach the interrupts
  // used to read the channels
  PCintPort::attachInterrupt(THROTTLE_IN_PIN, calcThrottle,CHANGE);
  PCintPort::attachInterrupt(STEERING_IN_PIN, calcSteering,CHANGE);
  PCintPort::attachInterrupt(AUX_IN_PIN, calcAux,CHANGE);
  
  Wire.begin();
  Serial.print("Initialize MPU: ");
  mpu.initialize(); // Power on and prepare for general usage
  Serial.println(mpu.testConnection() ? "Connected" : "Connection failed");
  
  // Setup Empfindlichkeit ///////////////////////////////////////////////////////
  mpu.setFullScaleGyroRange(0); // 0 = +/- 250 degrees/sec
                                // 1 = +/- 500 degrees/sec
                                // 2 = +/- 1000 degrees/sec
                                // 3 = +/- 2000 degrees/sec
  Serial.print("Empfindlichkeit-Stufe ");
  Serial.println(mpu.getFullScaleGyroRange());
  
  // Setup Filter ////////////////////////////////////////////////////////////////
  mpu.setDLPFMode(6);
  /*                      |   ACCELEROMETER    |       GYROSCOPE
                 DLPF_CFG | Bandwidth | Delay  | Bandwidth | Delay  | Sample Rate
                 ---------+-----------+--------+-----------+--------+-------------
                 0        | 260Hz     | 0ms    | 256Hz     | 0.98ms | 8kHz
                 1        | 184Hz     | 2.0ms  | 188Hz     | 1.9ms  | 1kHz
                 2        | 94Hz      | 3.0ms  | 98Hz      | 2.8ms  | 1kHz
                 3        | 44Hz      | 4.9ms  | 42Hz      | 4.8ms  | 1kHz
                 4        | 21Hz      | 8.5ms  | 20Hz      | 8.3ms  | 1kHz
                 5        | 10Hz      | 13.8ms | 10Hz      | 13.4ms | 1kHz
                 6        | 5Hz       | 19.0ms | 5Hz       | 18.6ms | 1kHz
                 7        |   -- Reserved --   |   -- Reserved --   | Reserved
  */
  Serial.print("Filter-Stufe ");
  Serial.println(mpu.getDLPFMode());  
}


// HAUPTPROGRAMM //////////////////////////////////////////////////////////////////
void loop()
{
  if(UpdateFlags & STEERING_FLAG)  // Wenn neues Lenk-Signal vorhanden,...
  {
    //Serial.println(" ");
    //Serial.print("SteeringIn/us    ");
    //Serial.print(SteeringIn);
    //Serial.print("\t");
    //Serial.print("\t");
  
    //gx = mpu.getRotationX(); // X-Gyroskop-Daten einlesen
    gy = mpu.getRotationY(); // Y-Gyroskop-Daten einlesen
    //gz = mpu.getRotationZ(); // Z-Gyroskop-Daten einlesen
    //Serial.print("GyroRAWdata      ");
    //Serial.println(gy);  
  
    GyroVal = map(-gy, -32768, 32767, -10*gain, 10*gain); // Umrechnen in µs außerhalb Neutralposition
  
    SteeringIn += GyroVal;    // Servo Signal korrigieren
    //Serial.print("SteeringIn kor   ");
    Serial.print(SteeringIn);
    Serial.print("\t");
    Serial.print("\t");
    
    SteeringIn = kalman_update(SteeringIn); // Kalman-Filter
    Serial.println(SteeringIn);
    
    // Servo Geschwindigkeit
    SteeringInDifference = SteeringIn - SteeringInOld; // max 200µs je Iteration für Servo mit 0,05s Stellzeit
    SteeringIn = SteeringInOld + ((SteeringInDifference*SERVO_SPEED)/100);
    
    //Serial.print("SteeringIn speed ");
    //Serial.println(SteeringIn);
    
    // Begrenzungen
    if(SteeringIn<MIN_STEERING){
       SteeringIn=MIN_STEERING;}
    if(SteeringIn>MAX_STEERING){
       SteeringIn=MAX_STEERING;}
      
    SteeringServo.writeMicroseconds(SteeringIn); // Servo ansteuern mit korrigiertem Signal
    
    SteeringInOld = SteeringIn; // aktuellen Wert zwischenspeichern
   }

  if(UpdateFlags & AUX_FLAG)  // Wenn neues Aux-Signal vorhanden,...
  {
    //Serial.println(" ");
    //Serial.print("AuxIn/us     ");
    //Serial.println(AuxIn);
  
    gainNew = map(AuxIn, 1000, 2000, 0, 100); //Umrechnen in %
  
    // Begrenzungen
    if(gainNew>100){
      gainNew=100;}
    if(gainNew<0){
      gainNew=0;}
  
    gain = gainNew;   //gain mit neuem gain-Wert überschreiben
  
    //Serial.print("gain         ");
    //Serial.println(gain);
  }

  if(UpdateFlags & THROTTLE_FLAG)  // Wenn neues Gas-Signal vorhanden,...
  {
    //Serial.println(" ");
    //Serial.print("ThrottleIn/us");
    //Serial.println(ThrottleIn);
    
    if(ThrottleIn < (NEUTRAL-50)) // Bremse
    {
      digitalWrite(BRAKE_PIN, HIGH);
    }
    else
    {
      digitalWrite(BRAKE_PIN,LOW);
    }
  }

  UpdateFlags = 0;
}



// interrupt service routines ///////////////////////////////////////////////////
void calcThrottle()
{
  if(PCintPort::pinState)
  {
    ThrottleInStart = micros();
  }
  else
  {
    ThrottleIn = (int)(micros() - ThrottleInStart);
    UpdateFlags |= THROTTLE_FLAG;
  }
}

void calcSteering()
{
  if(PCintPort::pinState)
  {
    SteeringInStart = micros();
  }
  else
  {
    SteeringIn = (int)(micros() - SteeringInStart);
    UpdateFlags |= STEERING_FLAG;
  }
}

void calcAux()
{
  if(PCintPort::pinState)
  {
    AuxInStart = micros();
  }
  else
  {
    AuxIn = (int)(micros() - AuxInStart);
    UpdateFlags |= AUX_FLAG;  
  }
}


// Kalman-Filter Berechnung
int16_t kalman_update(int16_t measurement)
{
  p = p + q;
  k = p / (p + r);
  x = x + k * (measurement - x);
  p = (1 - k) * p;
  return x;
}

Greetz und viel Spaß beim Ausprobieren!
Marco bzw. DDC Schattenleufer

Driften heißt Beherrschen des instabilen Fahrzustands im dynamischen Grenzbereich.
Quote
09-01-2018, 23:38
Beitrag #26
RE: "Eigenbau" Gyro: GY521(MPU6050) + Arduino
Huhu, ich wollte mir gerne den Gyro nach bauen.

Allerdings hab ich Probleme beim Upload: C:\Users\Pepe\Documents\Arduino\sketch_jan09a\sketch_jan09a.ino:22:26: fatal error: PinChangeInt.h: No such file or directory

#include <PinChangeInt.h>

^

compilation terminated.

exit status 1
Fehler beim Kompilieren für das Board Arduino Nano.

Brauche ich dazu spezielle Libraries?

Gruß

Pepe
Quote
11-01-2018, 11:54
Beitrag #27
RE: "Eigenbau" Gyro: GY521(MPU6050) + Arduino
Moin,
Hast du die PinChangeInt.h denn?

Schau mal hier:

http://playground.arduino.cc/Main/PinChangeInt

https://github.com/GreyGnome/PinChangeInt

Damit sollte Deine Library vollständig sein.


Gruß
Oji

Die Fahrt zu einer Strecke ist die beste Investition in Dein Projekt!
DoriBrothers
Quote
11-01-2018, 16:35 (Dieser Beitrag wurde zuletzt bearbeitet: 11-01-2018 18:15 von Pepe0405.)
Beitrag #28
RE: "Eigenbau" Gyro: GY521(MPU6050) + Arduino
Hab ich mir runter geladen Smile Aber schon das nächte Problem, die i2cdev Library habe ich auch nicht. Die lässt sich aber nicht einbinden Sad

Runter geladen da: https://github.com/jrowberg/i2cdevlib als zip

Fehler: Angegebener Ordner/ZIP-Datei enthält keine gültige Bibliothek

Edit:

Das hat jetzt funtioniert, indem ich die zip einfach in den Lib Ordner entpackt habe.

Jetzt bin ich an der stm32f10x.h dran :/

Die ich aber nicht finden kann Sad

Hat vielleicht jemand die Links zu den passenden Librarys?
Quote


Möglicherweise verwandte Themen...
Thema: Verfasser Antworten: Ansichten: Letzter Beitrag
  Gyro Probleme, Ursachen für zittern, wobble, etc. HannesTDX 0 510 25-02-2017 17:25
Letzter Beitrag: HannesTDX
  MST Gyro vs Savöx Servo Sasuke85 13 2,516 27-09-2016 23:26
Letzter Beitrag: HuFu-ETZ
  Lage des Gyro`s x-Racer 4 1,633 21-02-2015 11:19
Letzter Beitrag: x-Racer
  Gyro Davis 58 22,554 13-02-2015 21:17
Letzter Beitrag: Qualle
  Gyro einstellen ( futaba gy401 oder ähnlich) - gelöst! denni 6 2,564 29-01-2015 23:47
Letzter Beitrag: Crankhank

Gehe zu:


Benutzer, die gerade dieses Thema anschauen: 1 Gast/Gäste