Pages: [1] 2   Go Down
Author Topic: Calcolare angolo assoluto da velocità angolare  (Read 1892 times)
0 Members and 1 Guest are viewing this topic.
Messina (Italy)
Offline Offline
Sr. Member
****
Karma: 5
Posts: 301
Ciao a tutti!
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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!!!
Logged

Video demonstration of my Dashboard OBDII -> http://goo.gl/m8Pqp

Parma
Offline Offline
Edison Member
*
Karma: 20
Posts: 2361
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Ciao
Logged

Rome (Italy)
Offline Offline
Tesla Member
***
Karma: 120
Posts: 9191
"Il Vero Programmatore ha imparato il C sul K&R, qualunque altro testo è inutile e deviante."
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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 ?
Logged

Messina (Italy)
Offline Offline
Sr. Member
****
Karma: 5
Posts: 301
Ciao a tutti!
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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...
Logged

Video demonstration of my Dashboard OBDII -> http://goo.gl/m8Pqp

Parma
Offline Offline
Edison Member
*
Karma: 20
Posts: 2361
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Ciao
Logged

Messina (Italy)
Offline Offline
Sr. Member
****
Karma: 5
Posts: 301
Ciao a tutti!
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset


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! smiley
Logged

Video demonstration of my Dashboard OBDII -> http://goo.gl/m8Pqp

Rome (Italy)
Offline Offline
Tesla Member
***
Karma: 120
Posts: 9191
"Il Vero Programmatore ha imparato il C sul K&R, qualunque altro testo è inutile e deviante."
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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-
Logged

Messina (Italy)
Offline Offline
Sr. Member
****
Karma: 5
Posts: 301
Ciao a tutti!
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Stamattina ho preso carta e penna e ho cominciato a scrivere qualche calcolo e ne è risultato fuori questo programma:

Code:
#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...
« Last Edit: December 19, 2012, 08:55:52 am by Dario Gogliandolo » Logged

Video demonstration of my Dashboard OBDII -> http://goo.gl/m8Pqp

Messina (Italy)
Offline Offline
Sr. Member
****
Karma: 5
Posts: 301
Ciao a tutti!
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Non ho capito bene alcune cose riguardanti il filtro di kalman...
mettiamo che io voglia usare questo codice:

Code:
// 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...
« Last Edit: December 19, 2012, 10:12:44 am by Dario Gogliandolo » Logged

Video demonstration of my Dashboard OBDII -> http://goo.gl/m8Pqp

Messina (Italy)
Offline Offline
Sr. Member
****
Karma: 5
Posts: 301
Ciao a tutti!
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Non riesco a calcolare l'angolo di inclinazione partendo da 2 assi dell'accelerometro.
Facendo
Code:
atan2(accX, accY);
ottengo valori totalmente sbagliati che non centrano niente con l'angolo... Come potrei fare?
Logged

Video demonstration of my Dashboard OBDII -> http://goo.gl/m8Pqp

Rome (Italy)
Offline Offline
Tesla Member
***
Karma: 120
Posts: 9191
"Il Vero Programmatore ha imparato il C sul K&R, qualunque altro testo è inutile e deviante."
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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°)


Logged

BZ (I)
Offline Offline
Brattain Member
*****
Karma: 252
Posts: 21283
+39 349 2158303
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Messina (Italy)
Offline Offline
Sr. Member
****
Karma: 5
Posts: 301
Ciao a tutti!
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
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...
Logged

Video demonstration of my Dashboard OBDII -> http://goo.gl/m8Pqp

Offline Offline
Sr. Member
****
Karma: 8
Posts: 293
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Code:
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...
Logged

Rome (Italy)
Offline Offline
Tesla Member
***
Karma: 120
Posts: 9191
"Il Vero Programmatore ha imparato il C sul K&R, qualunque altro testo è inutile e deviante."
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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.

Logged

Pages: [1] 2   Go Up
Jump to: