Go Down

Topic: Calcolare angolo assoluto da velocità angolare (Read 2402 times) previous topic - next topic

Dario Gogliandolo

salve a tutti, ho da poco ricevuto questa IMU che ho trovato ad un prezzo molto conveniente... Il problema è che non riesco a trovare la posizione angolare partendo dalla velocità angolare, o meglio analiticamente so come fare ma non riesco a scrivere il programma... Partendo da una velocità in gradi al secondo dovrei fare l'integrale di questa sommando quindi i valori campionati in base alla frequenza di lettura dell'imu... A spiegare potrebbe essere facile, ma dal punto di vista del programma come devo fare? Ovviamente essendo l'errore ridondante dovrò poi applicare un filtro (di kalman o complementare) per avere la posizione corretta...
Grazie a tutti!!!
Video demonstration of my Dashboard OBDII -> http://goo.gl/m8Pqp

flz47655

Devi discretizzare il tuo algoritmo, l'integrale diventa una sommatoria, a questo punto non dovrebbe essere difficile scrivere il programma..

Ciao

astrobeed


Il problema è che non riesco a trovare la posizione angolare partendo dalla velocità angolare, o meglio analiticamente so come fare ma non riesco a scrivere il programma...


Cosa sai realmente a livello analisi matematica ?
Se ti dico che ti serve un Kalman o una DCM, meglio se li usi tutti e due, che mi rispondi ?

Dario Gogliandolo


Cosa sai realmente a livello analisi matematica ?


Considera che sono in 5 superiore e per adesso sono arrivato agli integrali...

Kalman l'ho sempre letto qui sul forum ma non l'ho mai studiato, DCM non l'ho mai sentito...
Video demonstration of my Dashboard OBDII -> http://goo.gl/m8Pqp


Dario Gogliandolo


Su questo sito trovi ottimo materiale
http://www.gioblu.com/tutorials/sensori/193-filtro-complementare-e-filtro-di-kalman

Ciao


Volevo usare proprio quella guida ma non riesco a scaricare l'allegato in quanto mi dice che ho superato la banda giornaliera, anche se non ho mai scaricato da la... Se qualcuno potesse scaricarlo ed allegarlo qui sarebbe molto buono! :)
Video demonstration of my Dashboard OBDII -> http://goo.gl/m8Pqp

astrobeed


Considera che sono in 5 superiore e per adesso sono arrivato agli integrali...


Come volevasi dimostrare, ti mancano gli strumenti matematici, ma anche la fisica, per una cosa di questo genere, tieni presente che ottenere l'asseto 3D, anche se parliamo solamente dell'inclinazione, è una cosa che richiede tanta matematica di livello superiore.
Ti consiglio di "rovistare" dentro MultiWii, software per quadricotteri, che supporta i sensori della tua IMU ed estrapolare le sue routine per integrarle nel tuo software, non sono un gran che visto che implementano solo la parte generica dei vari algoritmi, ovvero non sono ottimizzate, però funzionano decentemente e per il tuo uso dovrebbero bastare-

Dario Gogliandolo

#7
Dec 19, 2012, 10:05 am Last Edit: Dec 19, 2012, 02:55 pm by Dario Gogliandolo Reason: 1
Stamattina ho preso carta e penna e ho cominciato a scrivere qualche calcolo e ne è risultato fuori questo programma:

Code: [Select]
#include <Wire.h>
#include <ITG3200.h>
#define address 0x40
ITG3200 gyro = ITG3200();

float gyroX, gyroY, gyroZ;
float accX, accY, accZ;
float angoloAssoluto=0;

long previousMicros = 0;
unsigned long currentMicros;

void setup(void) {
 Serial.begin(9600);
 Wire.begin();
 initBMA180();
 delay(1000);
 gyro.init(ITG3200_ADDR_AD0_LOW);
 
 Serial.print("zeroCalibrating...");
 gyro.zeroCalibrate(2500, 2);
 Serial.println("done.");
  previousMicros = micros();
}




void loop(void) {

   while (gyro.isRawDataReady()) {  
       
     currentMicros = micros();
     gyro.readGyro(&gyroX, &gyroY, &gyroZ);
     Serial.print(gyroX);Serial.print("\t");
     accX = readAccel3(0x03, 0x02);
     accY = readAccel3(0x05, 0x04);  
     accZ = readAccel3(0x07, 0x06);
 
 
 
     float intervallo = currentMicros-previousMicros; //trovo l'intervallo trascorso fra una lettura e l'altra
 
     angoloAssoluto+=gyroX*(intervallo/1000000); //moltiplico l'accelerazione angolare (°/sec) per il tempo in secondi (dividendo quindi il valore in microsecondi per 1000000). Incremento quindi l'angolo assoluto.
 
     Serial.print(angoloAssoluto);Serial.print("\t");
     Serial.print(intervallo);Serial.print("\n");
     previousMicros = currentMicros;
   }
}




effettivamente ruotando di 90° il socket su cui c'è montata l'imu sul monitor seriale leggo un valore di circa 90 (che poi molto lentamente si incrementa perchè la velocità angolare non è mai 0). Questa cosa però mi suona un po' strana perchè l'accelerazione il la ottengo in rad/sec, e non effettuo nessuna conversione e come è possibile che il risultato è in gradi?
ho detto una bella cazzata, il valore l'ottengo in gradi al secondo... a questo punto dovrebbe essere corretto, non mi resta che applicare il filtro di kalman...

AGGIORNAMENTO: se quando faccio il calcolo dell'angolo assoluto il valore della velocità angolare lo passo come intero noto che il valore finale non viene incrementato col tempo, mantenendo una certa sensibilità; resta ovviamente il problema del drift....

AGGIORNAMENTO 2: Applicando la soluzione di sopra aumenta l'errore in movimento, quindi conviene lasciare il codice scritto in alto...
Video demonstration of my Dashboard OBDII -> http://goo.gl/m8Pqp

Dario Gogliandolo

#8
Dec 19, 2012, 04:05 pm Last Edit: Dec 19, 2012, 04:12 pm by Dario Gogliandolo Reason: 1
Non ho capito bene alcune cose riguardanti il filtro di kalman...
mettiamo che io voglia usare questo codice:

Code: [Select]
// KasBot V1  -  Kalman filter module

float Q_angle  =  0.01; //0.001
float Q_gyro   =  0.0003;  //0.003
float R_angle  =  0.01;  //0.03

float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float  y, S;
float K_0, K_1;

// newAngle = angle measured with atan2 using the accelerometer
// newRate =  angle measured using the gyro
// looptime = loop time in millis()

float kalmanCalculate(float newAngle, float newRate,int looptime)
{
float dt = float(looptime)/1000;
x_angle += dt * (newRate - x_bias);
P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
P_01 +=  - dt * P_11;
P_10 +=  - dt * P_11;
P_11 +=  + Q_gyro * dt;

y = newAngle - x_angle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;

x_angle +=  K_0 * y;
x_bias  +=  K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;

return x_angle;
}


quando richiamo la funzione kalmanCalculate devo passare il primo valore che come dice il commento è l'arctan2 della x e della y dell'accelerometro, ma usando queste funzioni non so che unità di misura hanno i valori restituiti e soprattutto non so se posso usarli cosi come sono per la funziona kalmanCalculate.
Per quanto riguarda newRate non è specificato se devo passare la velocità angolare (non credo) o l'angolo assoluto e se cosi fosse 'unità di misura dovrebbe essere in gradi o in radianti?
Il looptime cosa sarebbe? Devo passare il valore di "intervallo" del codice scritto da me?
Grazie ancora...
Video demonstration of my Dashboard OBDII -> http://goo.gl/m8Pqp

Dario Gogliandolo

Non riesco a calcolare l'angolo di inclinazione partendo da 2 assi dell'accelerometro.
Facendo
Code: [Select]

atan2(accX, accY);

ottengo valori totalmente sbagliati che non centrano niente con l'angolo... Come potrei fare?
Video demonstration of my Dashboard OBDII -> http://goo.gl/m8Pqp

astrobeed


ottengo valori totalmente sbagliati che non centrano niente con l'angolo... Come potrei fare?


Hai calibrato l'accelerometro ?
La formula che usi è totalmente sbagliata, l'angolo altro non è che il seno, o il coseno a seconda del verso, dell'accelerazione previa sua normalizzazione in un valore che varia tra 0 e 1 (0 = 0G = 0°, 1 = 1G = 90°)



uwefed

E poi i computer non calcolano gli angoli in gradi ma in radianti (360Gradi sono 2Pi).
Ciao Uwe

Dario Gogliandolo



ottengo valori totalmente sbagliati che non centrano niente con l'angolo... Come potrei fare?


Hai calibrato l'accelerometro ?
La formula che usi è totalmente sbagliata, l'angolo altro non è che il seno, o il coseno a seconda del verso, dell'accelerazione previa sua normalizzazione in un valore che varia tra 0 e 1 (0 = 0G = 0°, 1 = 1G = 90°)



Si, l'accelerometro è calibrato.
Io vedo che il valore minimo che mi può restituire è -8191 che equivale a -90° e il valore massimo è 8191 che equivale a 90°. Come faccio a far rientrare questi valori in un range compreso fra -1 e 1?
io ho provato facendo cosi:
Code: [Select]

float beccheggio()
{
  float angVal=0;
  if (axis[Y]>0)
  {
    angVal=mapFloat(axis[Y], 0,8191, 0, 1);
  }
  else
  {
    angVal=mapFloat(axis[Y], -8191,0, -1, 0);
  }
 
  //float angVal = mapFloat(axis[Y], -8191,8191, -1, 1);//((float)axis[Y]/8191.0*bma180.getGSense())/2; //mapFloat(axis[Y], -8191,8191, -90, 90);

   return (angVal);


}

float mapFloat(int x, int in_min, int in_max, float out_min, float out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

ma effettivamente utilizzando questo codice quando il valore avis[Y] è superiore a 6800 circa il valore di angVal super l'1 e questo mi sembra molto strano perché quando richiamo la funzione mapFloat come massimo imposto 1 tanto è vero che facendo gli stessi calcoli con carta e penna tutto quadra perfettamente...
Non capisco come mai questi comportamenti cosi strani...
Video demonstration of my Dashboard OBDII -> http://goo.gl/m8Pqp

qsecofr

a me va... non supera mai i limiti impostati...

Code: [Select]
float mapFloat(float x, float in_min, float in_max, float out_min, float out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void loop() {
  // set the cursor to column 0, line 1
  // (note: line 1 is the second row, since counting begins with 0):
  lcd.setCursor(0, 1);
  // print the number of seconds since reset:
  int i ;
 
  for (i = -8191; i < 8191; i++){
      float angVal = mapFloat(i, -8191,8191, -1, 1);
      lcd.clear();
      lcd.print("i:" );
      lcd.print (i);
      lcd.print ("  ->");
            lcd.print (angVal);
      delay(10);
  }
}



pero non capisco nel tuo programma che fine ha poi fatto l'arcoseno...

astrobeed


Io vedo che il valore minimo che mi può restituire è -8191 che equivale a -90° e il valore massimo è 8191 che equivale a 90°.


Io vedo che continui a non capire, ma come puoi pensare di convertire direttamente l'accelerazione in gradi ?
Ti ho detto chiaramente che l'accelerazione rappresenta il seno, o coseno, dell'angolo non che è l'angolo.


Go Up