Archivi tag: giroscopio

Come combinare i dati del giroscopio e dell’accelerometro

Ok,quindi dopo alcuni esperimenti sui differenti approcci sul filtraggio dei dati, ora sono interessato su come mischiare i dati del GYRO e del ACC in modo da ottenere dati ottimizzati.

Come sempre,anche questa volta ho trovato un articolo molto interessante che descrive esattamente questo approccio: kalman filter vs complementary filter.

Qui e’ possibile trovare come impelemtare 2 tipi di filtri complementari e il filtro di  kalman.Se siete interessati al filtro di kalman in particolare ,allora e’ fortemente consigliata la lettura dato che spiega come impelmentarlo in maniera molto semplice. Devo avvisarvi che e’ scritto per arduino , non in python, ma non credo che ci possa allarmare.

Personalmente ho solo implementato il filtro complementare di primo ordine e gia cosi mi ritengo soddisfatto.

IMU_Fusion1

In figura potete vedere il confronto fra l’angolo calcolato dal giroscopio, dal acc,  col filtro complementare e anche col filtro complementare a cui ho passato i dati dell acc in precedenza filtrati con kalman.

Si puo notare a destra come la deriva del gyro sia molto evidente (linea blu) sia compeltamente assente nel dato del angolo con filtro complementare.

IMU_Fusion2

Questa invece e’ l’ingrandimento dell’area di sinistra dove era moloto evidente il rumore del acc. Di nuovo il filtro complementare arrotonda sufficientemente bene il valore..

Quindi in conclusione ho deciso di affidarmi nel mio quadricottero ai dati derivanti dal sensore filtrati con il filtro complementare.

Questo e’ lesempio in cui ancora ci sono inclusi tutti i filtri da me testati: IMU_Test3.

Nota che il filtro compelmentare e’ incluso direttamente nel file imu_test.py code in getAngleCompl() .

Posso quindi considerare questa fase di sviluppo quasi conclusa.Voglio solo creare una classe sensor che possa girare in un thread  in parallelo in modo da potere mostrare sempre i dati sul monitor.

Dopo di che si parte con i test sul PID.

Tutorial: come leggere i dati dal IMU

Nel precedente post ho descritto come settare raspberry pi per la connessione col sensore IMU.

Ora e’ tempo di vedere come leggere dati dal sensore.

Prima di tutto e’ necessario il sw che gestisca l’interfaccia i2c.Si possono trovare diversi esempi. Di seguito ne trovi uno : adafruit_i2c.py da github.

Quindi e’ necessario definire un codice specifico per il proprio sensore , nel mio caso un MPU6050.

Ho cercato per diversi giorni di scrivere il mio codice incontrando un problema di inconsistenza sui dati:anche tenendo il sensore fermo ricevevo dei risultati sempre differenti.Sospetto fosse un problema di formattazione dei dati.

Alla fine , grazie al grande lavoro di Hove nel suo blog, ho utilizzato il suo codice code e sono ora in grado di ricevere dati correttamente dal sensore.

Ho fatto alcune minime modifiche e preparato questo IMU_test file.

Cosi ho iniziato alcuni test preliminare per verificare il comportamento del sensore.

Ho montato il sensore su una barra orizzontalmente,quindi ho fatto ruotare la barra di una quantita’ ( 13 gradi, misurata con la bolla dello smartphone); infine l ho riposto orizzontale.

IMU_test1

Ho registrato su di un file i dati: accelerazione lungo gli assi (da ACCelerometro) e la velocita’ angolare(dal GYROscopio). Su di un foglio excel ho calcolato l’angolo sia rispetto ACC che il GYRO:

  • rx ACC=DEGREES(ATAN2(accZ+9.8;accY))
  • rx(i) GYRO=wx(i) *dt+wx(i-1)

Sotto trovi il grafico risultante.

IMU

Ho sottolineato in figura i due tipici problemi sul IMU :

  • lo slittamento del Gyro (puoi vedere un angolo che da 1 grado deriva mentre dovrebbe essere uguale a 0)
  • la sensibilita’ al rumore dell’ Accelerometro.

Quindi il prossimo passo dello sviluppo e’ cercare di ridurre questi due problemi cercando di filtrare e combinare i risultati dai due tipi di sensori.

SW Dev : intro loop di controllo

Finora lo sviluppo sw ha incluso funzionalita di jogging base andando a modificare direttamente le velocita’ dei motori e lasciando all utilizzatore la loro modulazione.

A questo punto ho introdotto il concetto di controllo in cui l’ operatore indica l’ angolo desiderato ed il controllore , il nostro rapberry pi, deve decidere le velocita’ dei motori.

control_loop

La relazione teorica fra angolo del quadricottero (sia esso roll, pitch, yaw o una combinazione di essi) puo’ essere ritrovata nella pagina del quadricottero.
L equilibrio che si ottiene e’ di tipo instabile: e’ sufficiente un minimo disturbo (ad esempio un alito di vento) perche tale equilibrio si perda.

Nella realta’ bisogna anche considerare dissimetrie costruttive (struttura, eliche etc.), o comportamenti non costanti dei motori.

Cio si traduce nella necessita’ di sapere:

  • qual’e’ il comportamento istantaneo del velivolo
  • qual’e’ la reale relazione fra angoli e velocita motori in quanto differente da quella teorica

Per il primo punto si deve introdurre qualcosa che possa leggere lo stato attuale del quadricottero, diciamo un sensore.
Nella maggior parte dei casi si utilizza un sensore montato a bordo come un giroscopio o un giro+ accelerometro.

Il giroscopio ritorna la velocita angolare dal cui integrale si ottiene l’angolo.
L’ accelerometro ritorna l’ accelerazione nelle sue componenti x,y,z

Quindi, noto l’ angolo attuale e l’ angolo desiderato (target) posso calcolare l’ errore e decidere una strategia di modulazione della velocita’ dei motori.

Tale strategia si traduce in un metodo di controllo denominato PID.

Si ipotizza cioe’ che la relazione fra due variabili (nel nostro caso velocita’ angolare del motore e angolo) sia approssimabile a tre componenti:
Proporzionale (P) = kp*Ei
Integrale(I) = Ki* sum (Ei)*t
Derivativa(D)= kd * (Ei-Ei-1)/t

Dove Ei e’ errore fra w target e w current all’istante i-esimo, t e’ il tempo ciclo del loop
Dove i vari k vanno determinati empiricamente.

Il pid qundi mi restituisce un valore di variazione motori che deve permettere di minimizzare E.

Un loop di lettura da sensore, calcolo correzione con pid e applicazione correzione w motori costituisce la base del controllo d’ angolo.

Nei post seguenti dedichero’ una descrizione specifica agli specifici blocchi: sensore, PID e control loopcon lo scopo di definire la struttura finale del programma.