Winkelgeschwindigkeit Integrieren
Hallo
Ich versuche eine simple Integration von Winkelgeschwindigkeit eines Gyros um auf die Winkel zu kommen.
Mir ist klar das das Integrieren auch das aufsummieren von Fehlern bedeutet und der Winkel innert einigen Sekunden nicht mehr stimmt.
Mir geht es hier aber um das verstehen, später kommt dann eventuell das verrechnen von einem ACC.
Ich habe nun viele Tage Gesucht und die Beispiele sehen immer gleich aus:
Code:
angle = angle + gyro * dt;
Ich denke angle ist Winkel, gyro ist die Winkelgeschwindigkeit, dt die Zeit die ein Loop Durchgang benötigt.
nur bin ich vermutlich zu Blind um die Rohdaten aufzubereiten.
Die Hardware ist ein simpler Arduino uno mit dem 328.
Breakout mit MPU6050 = http://www.sparkfun.com/products/11028
Mit I2C ausgelesen lese ich nun folgende max -min werte = -32767 bis 32768.
Wie muss ich nun die Werte aufarbeiten?
Angefangen habe ich damit den Offset abzuziehen:
Im Setup mache ich dies:
Code:
for (int i=0;i<500;i++) //mittelung von nullpunkt
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
gz_null = gz_null+gz;
}
gz_null = gz_null/500;
Danach versuche ich das Integrieren:
Code:
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
float gz_null=0;
float winkel=0;
void setup() {
Wire.begin();
Serial.begin(38400);
accelgyro.initialize();
for (int i=0;i<500;i++) //mittelung von nullpunkt
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
gz_null = gz_null+gz;
}
gz_null = gz_null/500;
}
void loop() {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //MPU6050 auslesen
gz = gz - gz_null; //offset abziehen
winkel=winkel+gz*2.8; //Winkel Integrieren, winkel + winkel * 2.8ms (dauer zwischen 2 loop durchgängen.
Serial.println(winkel); //Winkel ausgeben
}
Jetzt habe ich einen Wert, das sich schön ändert wenn ich den Gyro langsam um die Achse drehe, je nach Richtung nimmt dieser Wert zu oder ab.
Aber wenn ich den Gyro schneller drehe, verschluckt sich das und die Werte ändern viel zu wenig.
Habe ich nun kompletten Murks Programmiert?
Muss ich die Gyro Rohdaten ( -32767 bis 32768 ) zuerst aufbereiten?
Wie bekomme ich hier nun den Winkel in °?
Gruss Tomatenbaum
Liste der Anhänge anzeigen (Anzahl: 1)
Lass mich das kurz erklaren :
Erst werden den Gyro, ACC_X_achse und ACC_Y_achse jeden 4 ms gemessen mit einen 10 bit AD (Mega32). Diesen Werten werden 10 mal addiert, so nach 40 ms habe ich eine Mittelwert von jeden Sensor. Dan wird aus diesen Werten die Drehrate von gyro berechnet(gyro_d=(adc0-6323);adc0=0; //turnrate gyro over 10 samples-offset, 6323 bei stillstand als i>9). Das ist nog alles in integer, so noch keine Umrechnung in °/sek.
Zweitens wird dan auch die Winkel berechnet von ACC : hoek=atan2(aYY,-aXX)/M_PI*1800;
Aus diesen Gyro-Drehrate wird dan die absolute Winkel berechnet rein durch integrieren, und gleich auch noch eine drift-ausgleich gemacht mit die Winkel von ACC gegeben : gyro=gyro-gyro_d+0.25*(hoek-gyro_hoek);
Anhang 22578
In diesen graph erkennen sie das integrierte Gyro-signal (blaue curve) das leicht wegdriftet, und die Winkel von ACC gemessen (rote curve). In meinen algoritmus wird diesen drift dan corrigiert.
