Archivi tag: IMU

Beta1test. Alcune considerazioni

Durante gli ultimi giorni ho affettuato dei tet con il seguente setup:

  • Throttle speed = 30 (Questo valore non permette al drone di volare ma può farlo bilanciare sulla semisfera montata sulla base ).
  • Roll target=pitch target =0.
  • Command =1 ( solo il controllore PID per il roll è attivo). In questa modalità motor1 e motor 3 ( pitch) ruotano alla velocità di throttle.
  • PID k : li ho variati nei diversi test per poter avere un primo confronto.
  • Cycletime = 10ms.

Analizzando i logs salvati ho raccolto le seguetni considerazioni :

  • Il  cycle time medio è = 11ms, con alcuni picchi fino a 21ms.Questo è un punto fondamentale che devo chiarire.beta1_cycletime
  • Ho bisogno di comparare dati dai diversi  threads (main loop esensor per il momento).Cosi’ ho modificato il codice includendo una colonna del tempo assoluto.
  • When I switch from command 0 (no PID controller) to 1 (roll PID controller) I notice a pick on the motor speed. I find a bug: one data initialization missing.Now it is corrected.
  • Nell’istante in cui passo da command 0 a 1 ci impiega circa 1,5 sec a tentare di bilanciarsi anche se la differenza fra le velocità motori è elevata. Possibile spiegazione (da investigare) è l’effetto dellinerzia all’avvio del movimento. Dovrei provare con permettere una maggiore differenza di velocità
  • Per ora il miglior risultato si è ottenuto con Kp= 0.6 Ki=0 Kd=0.5. Voglio provare aumentando un poco  Kp e riducendo Kd.beta1_balancing

 

Alfa3test. Introduzione del PID ed altro…

Partendo da questa serie di test Alfa3, ho introdotto alcune novita’  che vanno verso la soluzione finale. alfa3_pic1In particolare ho fatto le necessarie modifiche per lavorare completamente wireless:

  1. Ho aggiunto un secondo ESC ed un secondo motore.
  2. ho modificato la mia connector board aggiungendo  un nuovo  connettore 3 pin in cui ho anche saldato il cavo di potenza (filo rosso).Ho poi collegato lo zero (filo nero) ed ho ponticellato il pin del segnale (filo bianco) con lo stesso  dell’ultimo segnale (file verde nella foto).connector_board In questo modo posso scegliere se alimentare rpi da esc oppure no, a seconda che collego esc su B o su C.
  3. Pertanto lo schema prevede il motore M[0] collegato sul connettore A, ed il motore M[1] sul connettore C (oppure B se decido di alimentare rpi con usb esterna).
  4. Ho connesso la chiave usb Wifi al rpi. Ho pero’ notato che ne il pc ne la chiave hanno la possibilta’ di diventare accesspoint. E’ possibile verificare la cosa usando il comando:

iwconfig ap

Quindi ho utilizzato il mio smartphone attivando la funzione  router wifi e creando  cosi’ una rete che include anche il pc ed il rpi.

Di seguito lo  schema delle convenzioni per questo test:

alfa3_convention

  • M[0] ruota in senso antiorario , con cavo rosso montato su cavo T dell ‘esc (quello vicino alla T del logo turnigy) ed il cavo giallo sul cavo Y dell’esc.
  • M[0] monta un elica sinistra standard (non marcata R).
  • M[1] ruota in senso orario , con cavo giallo montato su cavo T dell ‘esc ed il cavo rosso sul cavo Y dell’esc.
  • M[1] monta un elica destra (marcata R).
  • Per come e’ stato montato l’IMU, ottengo una rotazione negativa del roll (intorno alla x) quando M[0] va verso il basso.

Con questa configurazione ho gia fatto alcuni test preliminari.Nel prossimo post introdurro’ il nuovo modulo del remote control (rc.py)  ed i primi risultati del controllo PID , in cui ho usato solo il P.

Alfa1test .Primi risultati

Dopo diversi rinvii sono finalmente riuscito a tesate il codice  alfa1    test.

Come da programma ho inizialmente verificato di riuscire a pilotare il motore.

Dopo la fase startup (premere enter quando richiesto), una semplice interfaccia grafica mi permette di visualizzare i valori corenti di roll , pitch e yaw  e di poter aumentare (tasto a) e dimimuire (tasto z)  la velocita’ del motore.

Posso inoltre salvare una velocita’ di hover (tasto n) e di poterla richiare (tasto h).

Per il particolare settaggio riportato in figura, la velocita’ di hover per il singolo motore che alza la struttura e’ di 15/17 %.

alfa1

Di seguito alcune note pratiche:

  • Per un motore che ruota in senso orario (gaurdando da sopra)  occorre montare una elica destra.
  • Rispetto alle prime prove effettuate ho verificato che e’ possibile utilizzare una sequenza semplificata per l’avviamento dell’ESC (e del motore) , per cui e’ possibile lasciare esc in potenza e poi avviare l’invio degli impulsi. Ricordo che le prove descritte sono valide solo per il mio specifico set di hardware. Prudenza sempre!
  • Mettere sempre dado e controdado.Le vibrazioni del motore sono tali da far svitare un dado solo, qualunche sia la forza che riesci a appore.

Il sensore attualmente registra ad ogni ciclo su file i seguenti dati: tempo totale, tempo ciclo dt, r,p,y calcolati con il filtro complementare, r_rate,p_rate,y_rate, del giroscopio e x_acc,y_acc,z_acc dell’accelerometro.

Se ora analizziamoi  dati registrati, la prima cosa che salta all’occhio e’ che il periodo di campionamento aumenta linearmente con il tempo:

alfa1_dt

In questo esempio, passa dai circa  8ms dei primi istanti ai  12ms  dopo 2 minuti.

Le possibili cause sono:

  • un baco nel sw sulla misurazion del dt. Ad una prima analisi mi sento di escluderlo.
  • un ritardo che diviene proporzionale al log che sto creando.Un semplice test mi potra’ dare conferma di questo: sara’ sufficiente scrivere, per eesempio, 3 volte i dati di log ad ogni ciclo e dovrei vedere che la variazione di dt sara’ 3 volte piu veloce.
  • un problema sul sensore. Potrebbe essere che richiedo i dati con una frequenza eccessiva? Questa possibilita’ la analizzaro’ solo se la seconda ipotesi da esito negativo.

Analizzando invece l’andamento del roll , di seguito due casi in cui il motore girava alla stessa velocita’, ma  con il filtro passa basso interno del sensore a diversi valori.

In questo caso si vede il roll calcolato con un filtro DLPF = 40Hz : ( in cui il valore oscilla fra 8 e 13 gradi)

alfa1_roll_DLPF_40Hz

In questo secondo caso con il filtro DLPF: 10 Hz: (in cui il valore oscilla di qualche decimo di grado)

alfa1_roll_DLPF_10Hz

L’ultimo confronto e’ l’analisi dei dati di roll ottenuti dal giroscopio, dal accelerometro e dal filtro complementare:

alfa1_roll_tau0_005

Come si puo’ notare il roll ottenuto con il filtro complementare (in grigio) oscilla ancora notevolmente. Questo grafico e’ stato ottenuto utilizzando una costante di tempo tau  pari a 0,005s.

Lo stesso log invece con una costante di tempo di 0,2 s  si presenta in questo modo:

alfa1_roll_tau0_2

Si puo’ vedere come il roll del filtro complementare e’  in queto caso meno affetto da disturbi.

Per periodi inferiori a 0,2 secondi, il valore del giroscopio e’ predominante (eliminando quindi le vibrazioni classiche dell’accelerometro) , mentre per periodi superiori a 0,2 s il valore dell’accelerometro e ‘ predominante, eliminando cosi’ l’effetto di drift (deriva) tipico del giroscopio.

In conclusione per i prossimi test utilizzaro’  DLPF=10 HZ con un  tau=0,2 (sono i valori gia settati nel codice).

Alfa1test. Preparazione

Durante gli ultimi fine settimana sono stato impegnato , cosi’ non mi e’ stato possibile continuare i test.

Ad ogni modo ora in questo post introduco il codice che ho creato per quello che ho creato alfa test, una sessione di verifiche per preparare il codice finale.

Alfa1 test include le seguenti funzionalita’:

  1. jogging manuale del motore
  2. lettura del IMU
  3. Logging dei dati(w motore,e angoli dell’IMU)

Le azioni da prendere durante questi test sono:

  1. verificare i disturbi sull’IMU per via delle vibrazioni motore
  2. testare differenti valori del filtro IMU
  3. avviare il motore e provare a capire con questa configurazione qual’e’ il valore di velocita’ angolare di equilibrio (hover w)
  4. capire al variare di w come varia angolo del quadricottero
  5. montare differenti pesi sul braccio del quadricottero e vedere i diversi wh per verificare il mio modello matematico
  6. misurare la velocita’ angolare reale del motore con il mio smartphone (solo curioso sulla possibilita’ di misurare la frequenza usando un sound spektrum analyzer)

Spero di iniziare domani alfa1test , poi potr’ condividere il codice.

Preparazione della sessione di test

E’ giunto il tempo di muovermi dalla scrivania al garage per iniziare la seconda fase dello sviluppo.

Gli obiettivi sono:

  • verificare la procedura di avvio dell’esc. Voglio vedere se e come avviare l’esc anche se e’ gia alimentato.Questo sara’ utile per alimentare rpi direttamente dal BEC di un esc.
  • verificare i disturbi sui dati IMU per la vibrazione dei motori.Settare al meglio la severita’ dei filtri.
  • verificare il loop di controllo e ottimizzare i parametri PID per la rotazione del quadricottero.
  • sviluppare il controllo da remoto per l’utente via wifi per controllare i motori e la posizione del quadricottero.

Per fare questi test ho costruito una struttura che spero sicura.Di seguito alcune foto del progetto e della struttura realizzata.

Test_Frame

Ho anche finito i cablaggi del sistema includendo l’IMU e una basetta fatta in casa per la connessione dei motori.L’alimentazione di rpi e’ ancora via usb e la connessione ethernet via cavo.

wiring_IMU_MOTORS

Quindi il setup della connessione dei motori e’ la seguente:

  • GPIO 18 (pin 12) >M0
  • GPIO 23 (pin 16) >M1
  • GPIO 24 (pin 18) >M2
  • GPIO 25 (pin 22) >M3

Classe sensor.py pronta

Dopo alcune attivita’ collaterali (costruirmi il nuovo tavolo da lavoro in garage per ospitare le prossime prove) ho completato i test sull’IMU e creato l’oggetto sensor che puo lavorare in un thread paralllelo in  modo da poter avere a disposizione le informazioni aggiornate del sensore(ogni 5-8 ms).
E’ possibile scaricare l’esempio  sensor_test .Lanciandolo si puo’ vedere sul terminale i valori attuali di roll,pitch eyaw.

I dati messi a disposizione dalla classe sono: posizione  angolare [deg],velocita’ angulare[deg/sec], accelerazione lineare[m/sec]

Le principali modifiche rispetto al precedente IMU_test sono:

  • Ho notato che l’inizializzazione di alcuni parametri del mpu6050 puo fallire. L’ordine con cui si inizializza i dati ne influenza la riuscita. Pertanto ho aggiunto una funzione che verifica la corretta inizializzazione del sensore.
  • Il vettore accelerazione e’ stato normalizzato e scalato in modo che la risultante quando fermo sia uguale alla gravitaì .Questo puo’  aiutare se si vuole integrare l’acccelerazione per stimare velocita’ lineare e movimento lineare.
  • l’oggetto sensor.py puo’ lavorare in un thread in parallelo.
  • minori modifiche fatte sui nomi delle variabili.

Prossimi passi:

  • Comprare alcuni connettori e predisporre un collegamento definitivo per il sensore e i motori
  • montare il sensore sulla struttura con i motori in funzione e verificare se occorre utilizzare sul sensore dei filtri piu severi.

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.

Come filtrare i dati dal IMU

Nel precedente post puoi trovare come leggere i dati dal sensore.

Una nota: i dati usati inquesta fase sono presi da un IMU non ancora montato sul quadricottero,per evitare di sovrapporre troppi fattori. Testero l effetto delle vibrazioni dei motori in una seconda fase.

Come descritto l accelerometro ritorna dati affetti da rumore.
Per ridurre questo rumore e’ necessario introdurre dei filtri che addolciscano i risultati.
Nell IMU che sto usando e’ gia presente un filtro passabasso che lavora nel range 260Hz -5Hz.
Per avere un sw generico ho comunque sviluppato un esempio di filtro.

Ho usato alcune informazioni preliminari da wikipedia.Qui il link
Il parametro RC deve essere variato a seconda del rumore presente.

lp_filter1

Un altro interessante approccio usato spesso e’ il filtro di Kalman, specialmente quando ho diversi parametri fra loro legati da un modello matematico noto.

Per il nostro caso particolare ,ho trovato un semplicissimo e chiaro esempio di come sviluppare il filtro di Kalman : Kalman filter for undergrads

Questo descrive il caso di Variabile singola. Nel caso nostro infatti ACC x,ACC y e ACC z non hanno una relazione fra loro.

Nel mio codice ho semplicemente trasposto al caso di “3 variabili singole”.

Nella foto successiva si puo vedere il filtri di Kalman applicato.Interessante notare che tarando opportunamente i parametri di Q si ottiene un risultato comparabile al filtro passabasso.

lp_filter2

Ho deciso di mantenere comunque questo sviluppo nel progetto dal momento che potrei tentare di applicarlo anche alle posizioni angolari, creando un modello matematico del quadricottero.

Qui e’ possibile scaricare il codice IMU_test2 che include i 2 tipi di filtri.

Nota che potrebbe essere necessario istallare la libreria di python numpy sul raspberry pi.

apt-get install python-numpy

apt-get install python-numpy-doc

In conclusione , col filtro (hw da IMU e/o sw da questo codice ) riesco a ridurre i disturbi dell ACC. Nel prossimo post cerchero di ridurre la deriva del GYRO drift problem tramite la “fusione ” dei dati di ACC e GYRO.

Qui un altro interessante link per comprendere come lavora kalman filter: Kalman filter for dummies

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.