Winkelgeschwindigkeit Integrieren

[Tomatenbaum]

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:


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:



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:



#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

[Che Guevara]

Hallo,

ich selbst kann zwar kein C programmieren (bzw. nur schlecht), aber etwas fällt mir schon auf:
In deiner Offset-Routine addierst du 500 mal den gleichen Wert und teilst dann durch 500. Das macht wenig Sinn, du müsstet in deiner For-Schleife den Sensor immer wieder neu auslesen. Ich benutze in meinen Koptern auch den MPU60X0 und bestimme den Offset wie folgt:
1. Ich sample 50 Werte von jeder Achse (gyrox, gyroy und gyroz)
2. Ich vergleiche alle Werte miteinander (also Gyrox(1) mit Gyrox(2), Gyrox(2) mit Gyrox(3), usw...) für jede Achse (also jeweils Gyrox, Gyroy & Gyroz)
3. Wenn der Unterschied zwischen zwei benachbarten Werten eine bestimmte Schwelle überschreitet (bei mir: 3 (bei 2000°/s und 42Hz DLPF)), sample ich wieder neue 50 Werte
4. Wenn der Unterschied geringer ist, addiere ich die 50 Werte und teile durch 50, so kann ich eine minimale Abweichung, welche die Grenze nicht überschreitet, trotzdem einigermassen ausgleichen

Die Rohdaten musst du, bis auf das Abziehen des Offsets, nicht weiter aufbereiten. Welche Einstellungen hast du den im MPU gemacht bzgl. Empfindlichkeit und DLPF?

Gruß
Chris

EDIT:
Dass bei schnellen Drehungen die Winkeländerung zu langsam erfolgt, könnte daran liegen, dass du innerhalb der Main-loop Schleife die Daten über die serielle Schnittstelle ausgibst. Evtl. wäre es sinnvoller, dies nur alle xx-Durchläufe zu machen, etwa so (Pseudocode):


Byte: Cnt

Mainloop:
getSensorData()
gz = gz - gz_null
winkel = winkel + gz
cnt = cnt + 1
wenn cnt = 100 dann
cnt = 0
serialport.write(winkel)
ende wenn
Springe zu Mainloop

Außerdem fällt mir noch das dt auf: IMHO braucht man das nicht zwingend, zumindest funktionierts in meinen Koptern auch hervorragend ohne diesen Parameter.

[Tomatenbaum]

Danke

zu 1: Habe das mit dem mitteln falsch gepostet, habe den Code noch mal richtig geschrieben.
zu 2, 3, 4: Das mit dem Vergleich zweier Werte und eventuelles neu einlesen ist eine gute Idee, das werde ich auch so umsetzten.

Wie meist du das, Einstellungen und Empfindlichkeit?
Ich benutze diese Bibliothek:
https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
Dort habe ich nichts weiter eingestellt.

Auf was soll ich achten?

Edit: der Link zur Bibliothek stimmt nun.

[Che Guevara]

Man kann den MPU unterschiedlich konfigurieren bzgl. der maximalen Winkelgeschwindigkeit (2000°/s, 1000°/s, 500°/s, 250°/s) und bzgl. des DLPF (zwischen 256 & 5Hz in 6 Schritten beim Gyro).
Was jetzt bei dir genau eingestellt wird, da bin ich ehrlich gesagt etwas zu faul, den ganzen Code zu durchsuchen...
Hast du den schon probiert, den Winkel nur alle paar Durchläufe ausgeben zu lassen?

Gruß
Chris

[Tomatenbaum]

EDIT:
Dass bei schnellen Drehungen die Winkeländerung zu langsam erfolgt, könnte daran liegen, dass du innerhalb der Main-loop Schleife die Daten über die serielle Schnittstelle ausgibst. Evtl. wäre es sinnvoller, dies nur alle xx-Durchläufe zu machen, etwa so (Pseudocode):


Byte: Cnt

Mainloop:
getSensorData()
gz = gz - gz_null
winkel = winkel + gz
cnt = cnt + 1
wenn cnt = 100 dann
cnt = 0
serialport.write(winkel)
ende wenn
Springe zu Mainloop

Außerdem fällt mir noch das dt auf: IMHO braucht man das nicht zwingend, zumindest funktionierts in meinen Koptern auch hervorragend ohne diesen Parameter.

Hmm, was ist das für eine Sprache?

Habe das was du meinst mal geproggt:



#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;
unsigned long prev;


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;

prev = millis();

}

void loop(){

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //MPU6050 auslesen


gz = gz - gz_null; //offset abziehen
winkel=winkel+gz; //Winkel Integrieren, winkel + winkel * 2.8ms (dauer zwischen 2 loop durchgängen.


if((millis() - prev)>100){
prev =millis();
Serial.println(winkel); //Winkel ausgeben
}

}


Ist schon viel besser, auch bei schnellere Drehungen.
Was mache ich aber nun mit diesem Wert, kann ich diesen nun gleich für einen PID Regler verwenden?

[Che Guevara]

Das ist Pseudocode, also gar keine Sprache ;)
Ich dachte mir doch, dass es an der Seriellen liegt...
Nunja, da der MPU ja einen 3Achs ACC eingebaut hat, könntest du z.b. die Daten fusionieren (mithilfe eines Komplementärfilters oder Kalman oder...).
Oder aber du nutzt diesen Winkel ohne den ACC, allerdings wird der nicht sehr lange stabil sein.
Was genau hast du den vor?

Gruß
Chris

[Tomatenbaum]

Man kann den MPU unterschiedlich konfigurieren bzgl. der maximalen Winkelgeschwindigkeit (2000°/s, 1000°/s, 500°/s, 250°/s) und bzgl. des DLPF (zwischen 256 & 5Hz in 6 Schritten beim Gyro).
Was jetzt bei dir genau eingestellt wird, da bin ich ehrlich gesagt etwas zu faul, den ganzen Code zu durchsuchen...
Hast du den schon probiert, den Winkel nur alle paar Durchläufe ausgeben zu lassen?

Gruß
Chris

Hmm, da muss ich mal in die Bibliothek schauen.
Der ACC ist einfach, der Spuckt bei ACC Z 16000 aus wenn er ruhig auf dem Tisch liegt, also ist +-2G eingestellt.
Bei den Gyros muss ich das erst herausfinden.
Was hast du für deinen Kopter eingestellt?




Das ist Pseudocode, also gar keine Sprache ;)
Ich dachte mir doch, dass es an der Seriellen liegt...
Nunja, da der MPU ja einen 3Achs ACC eingebaut hat, könntest du z.b. die Daten fusionieren (mithilfe eines Komplementärfilters oder Kalman oder...).
Oder aber du nutzt diesen Winkel ohne den ACC, allerdings wird der nicht sehr lange stabil sein.
Was genau hast du den vor?

Gruß
Chris

Eigentlich will ich am Schluss einen kleinen Kopter mit einem Komplementärfilter in die Lüfte bewegen.
Auch der Komplementärfilter hat einen *dt in der Fomel:


angle = (0.98)*(angle + gyro * dt) + (0.02)*(x_acc);

ich Denke den dt ist also wichtig!

Was meinst du?

[Che Guevara]

Also ich habe meinen MPU auf 2000°/s und 42Hz Tiefpass eingestellt, da hierbei das Rauschen minimal und die Performance noch gut ist. Vorher war ich bei 250°/s und 256Hz Tiefpass, da hat er öfters mal ein Rad geschlagen, obwohl ich das nicht wollte, sehr ärgerlich.....
Der Tiefpass ist IMHO Pflicht und die max. Winkelgeschwindigkeits-einstellung würde ich auch eher in Richtung 2000 machen, da du dann den Kopter schneller drehen lassen kannst und er agiler ist... Irgendjemand hier im Forum meinte mal, dass der Sensor bei 250°/s eigentlich auch auf 2000°/s eingestellt ist und die Werte nur verstärkt werden. Das würde auch das deutlich höhere Rauschen erklären.

Auch ich verwende (noch) einen Komplementärfilter in meinen Kopter, dieser benötigt bei mir auch kein dt. Meine Formel ist einfach (iwo ausm Netz):


Gyro_xangle = Gyro_xangle + Gyrox

Gyro_xangle = Gyro_xangle * Gyro_influence
ACC_xangle = ACC_x * Acc_influence
Gyro_xangle = Gyro_xangle + ACC_xangle

Möchtest du nur einen Hover-Mode integrieren oder auch HH?

Gruß
Chris

[Tomatenbaum]

Danke für deine Erfahrungen.

Sieht so aus als ob ich die MPU6050 Einstellungen in der Bibliothek anschauen muss.

Also dein Code hat wirklich kein dt.

Wie verwendest du dein Wert weiter?
Direkt zum PD Regler?

Ich versuche den einen Berühmten Komplementärfilter zu verstehen:

http://web.mit.edu/scolton/www/filter.pdf

Du kennst dieses Paper sicher.
Nur das dt verwirrt mich nun, auf Seite 3 werden die Werte aufbereitet, eventuell hat das dt auch etwas mit dem Scale zu tun?

[Che Guevara]

Ich habe in meinen Koptern zwei Modi: Acro (HH) und Hover
Beide besitzen einen PID-Regler. Im Acro-Mode wird der Fehler in der Winkelgeschwindigkeit mit p_gain multipliziert und auf die Motoren gebeben, außerdem wird der Fehler integriert und mit i_gain multipliziert. Und dann gibts noch den D-Anteil, der den Fehler mit dem vorherigen vergleicht und auch diesen wieder auf die Motoren gibt.
Wenn du konkrettere Fragen hast, stell sie einfach, aber ich kann dir jetzt nicht alles erklären.

Dieses Paper kenne ich nicht, sieht aber auf den ersten Blick ganz informativ aus.

Gruß
Chris

[Tomatenbaum]

Ja gut, werde ich machen mit den Fragen.

Ich bleib erst mal beim Integrieren und danach mit ACC verrechnen.
Bleibt nun die Frage nach dem dt und dem scale, also aufbereiten, so wie auf dem Paper.

Das verstehe ich noch nicht.

[Che Guevara]

Also bei deinem Kopter ist das Integral nicht sooo wichtig, ein Kopter fliegt auch mit einem reinen P-Anteil (zumindest im ACRO (HH) Mode).
Ich persönlich würde mich nicht nur stur an das Paper halten, sondern einfach selbst einwenig experimentieren. So erhältst du auch einen tieferen Einblick und weißt im Endeffekt dann mehr über das Thema, als wenn du 10 Papers liest ;)

Gruß
Chris

[Tomatenbaum]

Ja Danke Chris

Ist der liebe Chris der einzige hier der sich mit Integrieren und Komplementärfilter auskennt?
Wo sind denn die ganzen Profis hier? :--)

[RP6conrad]

Bei meinen Balance Roboter wird jeden 4 ms der Gyro und die ACC gesampled. Danach wird den offset von Gyro abgesogen und dan integriert. Wichtig ist diesen festen Sampletime, oder du muss gans genau diesen Periode messen konnen zwischen zwei Messungen. Den offset verlauft mit temperatur, so das wird auch nog mal gemessen und korrigiert.
Ergebnis kannst du here sehen :


https://www.youtube.com/watch?v=a1EfgqLcNzc&amp;feature=player_embedded


/*
De gyro wordt elke 4 mS uitgelezen via adc0. Dit wordt 10 keer geintegreerd (oversampling) . Bij stilstand geeft dit ca 6232.
Elke 40 mS wordt de hoek berekend.
Ook wordt elke 400 mS de temperatuur van de gyro gemeten via ADC1. Hiermee wordt de driftcorrectie aangepast.
In principe kan de drift kleiner zijn dan 0.1 °graad/sek
*/
void task_initial(void)
{
static uint8_t i=0;
static uint8_t j=0;


int16_t offset_temp=6140;

if(getStopwatch1() > 3) { //elke 4 mS worden de gyro + ADXL345 uitgelezen + opgeteld
setStopwatch1(0); //sample rate is zeer belangrijk voor integraal gyro !!!
adc0=adc0+readADC(ADC_0); //oversamplen gyro signaal
adc5= adc5+readADC(ADC_5); //oversamplen acc X signaal
adc6= adc6+readADC(ADC_6); //oversamplen acc Y signaal
i++;

if(i>9){ //na 40 ms worden de gemiddelde waarde gebruikt
aXX=(adc5-4200);adc5=0; // 0 g = 4200 voor de X-as
aYY=(adc6-4300);adc6=0; // 0 g = 4300 voor de Y-as
hoek=atan2(aYY,-aXX)/M_PI*1800;//hieruit volgt dan de hoek van de versnellingsvector in 0,1°
i=0;
gyro_d=(adc0-6323);adc0=0; //turnrate gyro over 10 samples-offset, 6323 bij stilstand als i>9
gyro_hoek=gyro/35;//dit geeft dan de hoek weer in 0.1 graden
gyro=gyro-gyro_d+0.25*(hoek-gyro_hoek); //sommatie = actuele hoek van gyro+ compensatie drift van ACC
drift=drift+hoek-gyro_hoek;
oldgyro=oldgyro-gyro_d; //dummy variable om de drift te kunnen zien
oldgyro_hoek=oldgyro/35;
if(program>1) task_balance();else setMotorPower(0,0);
j++;
if (j>9){
adc1=0.8*adc1+0.2*readADC(ADC_1); //uitlezen temp gyro voor driftcorrectie
offset_temp=(adc1-640); //temp compensatie gyrodrift, 640 is ca 20°C
gyro=gyro-offset_temp; //elke 400 ms wordt de temperatuur correctie uitgevoerd
j=0;
}
}
}

}

[Tomatenbaum]

Hallo RP6conrad

Vielen Dank für deine Antwort.
Ich muss mir deinen Code richtig anschauen, einiges verwirrt mich noch, könnte auch an der Sprache liegen. :--)

Das Video von deinem Balance Roboter ist toll, man sieht schön die Regelung.

Ich schaue mal ob ich deinen Code verstehe, brauche vermutlich etwas Zeit.

Gruss

[RP6conrad]

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);
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.