[Robotica] GyroNav, sensore assetto e orientamento

GyroNav è un sensore assoluto che permette di ottenere in modo preciso i valori di Pitch e Roll, rispetto ad un piano in bolla, e l’orientamento rispetto al Nord magnetico, se si inserisce il valore locale della declinazione magnetica si ottiene l’orientamento rispetto al Nord geografico.
Il progetto è composto da un Arduino Pro Mini, va bene qualunque altro modello di Arduino 8 bit, una coppia di sensori MPU6050 e HMC5883L oppure un singolo MPU9150( MPU6050 + magnetometro dentro lo stesso chip).
La scelta degli MPUxxxx è obbligatoria in quanto viene sfruttato il loro DMP (Digital Motion Processor) per tutta la parte di calcoli inerenti la IMU, implementa un Kalman, e non solo quello, in grado di fare oltre 100 cicli di calcolo al secondo, senza impegnare tempo CPU di Arduino, che comunque non è assolutamente in grado di gestire la mole di calcoli, tutti in virgola mobile a 64 bit (double), per gestire in modo ottimale una IMU 6 d.o.f. (accelerometro 3 assi e giroscopio 3 assi).
In pratica Arduino provvede al calcolo dell’orientamento, rispetto al Nord magnetico, partendo dalle letture raw del magnetometro a triplo asse ed esegue una compensazione del valore calcolato in base all’inclinazione sul Pitch e Roll, questo perché se il magnetometro non si trova perfettamente in bolla la lettura dell’orientamento varia notevolmente, molti gradi, in funzione dell’inclinazione.
Come dati forniti, tramite porta seriale, volendo anche tramite SPI, non ancora implementato, abbiamo i valori del Pitch e Roll, in gradi con risoluzione al decimo di grado, e dell’orientamento, in gradi con risoluzione al singolo grado.
Tramite il file config.h è possibile modificare il formato dati in uscita tra quattro diverse modalità, attivare/disattivare i messaggi per il debug sul monitor seriale, inserire il valore locale della declinazione magnetica, quest’ultimo prossimamente sarà inserito in EEPROM e modificabile tramite comando sulla seriale, invertire il verso di rotazione dell’orientamento, quest’ultimo dipende da come si montano fisicamente i sensori.
Trovate lo sketch per Arduino, una GUI di test per Processing, alcune foto e una illustrazione dei collegamenti tra Arduino e i sensori su github, link sotto, lo sketch è molto commentato e l’intestazione contiene molte informazioni importanti sul codice.

Repository github

Screenshot GUI per Processing

hor_gui.png

Il tutto montato su mille fori e inserito in un box su misura 3D printed.

img2.jpg

Grazie Astro, non è il mio settore e quindi ... c'è sicuramente da imparare :D

Guglielmo

+1 Karma per Astro. :)

Complimenti, anche se non ho capito niente :o Comunque, MOD, i progetti completi li abbiamo sempre messi in MEGATOPIC, temo che qui, considerata la fortissima specificità del lavoro, sarà in breve tempo sommerso da qualche centinaio di nuovi Topic. Vallo a trovare quando decideremo di costruire il nostro drone per andare a controllare se gli utenti mettono mano all'impianto elettrico di casa e bannarli sulla base dell'art. 15 del Regolamento. :grin:

[quote author=Michele Menniti date=1488222111 link=msg=3152500] Comunque, MOD, i progetti completi li abbiamo sempre messi in MEGATOPIC ....[/quote] Si, mi sembra un bel progetto degno del "megatopic "... sposto :D

Guglielmo

P.S.: In "Generale" lascio comunque un link permanente al thread in Megatopic ... così anche dall'area "Generale" ci si arriva.

+1

Mannaggia, mi manca il magnetometro

EDIT:

MPU9050( MPU6050 + magnetometro dentro lo stesso chip).

intendevi dire MPU9250 ?

Io aspetto la nuova versione con la sensorFusion visto che ho sempre voluto provarci ma non ho mai trovato tempo e voglia di studiarla :D

La sensorFusion non viene gestita dal DMP giusto Astro?

Brunello: EDIT: intendevi dire MPU9250 ?

In realtà MPU9150, il 9250 è la stessa cosa però monta un magnetometro migliore, però per il 9250 non ho tutte informazioni per utilizzare il suo magnetometro, dovrebbero arrivarmi a breve e sicuramente prevederò nel codice sia il 9150 che il 9250. Attualmente è facile trovare schede IMU per droni che montano il 6050 più l'HMC5883L, spesso abbinati ad un barometro, per meno di 20 Euro, vanno benissimo al posto delle breakout singole con sopra il magnetometro e l'MPU6050 che ho usato io per il prototipo.

Ho corretto il post inziale con la giusta sigla del MPU.

doppiozero: Io aspetto la nuova versione con la sensorFusion visto che ho sempre voluto provarci ma non ho mai trovato tempo e voglia di studiarla :D

La sensor fusion è già implementata, per la IMU, accelerometro e gyro, ci pensa il DMP e la fa molto, ma molto, bene, tra IMU e magnetometro la fa il codice, in particolare viene usata questa formulazione per ottenere l'orientamento e la compensazione con pan e tilt, la formula è riportata anche all'interno del codice nei commenti.

Yh = XM * sin(Roll) * sin(Pitch) + YM * cos(Roll) - ZM * sin(Roll) * cos(Pitch)
Xh = XM * cos(Pitch) + ZM * sin(Pitch)
Heading = arctan(Yh/Xh)

XM, YM, ZM sono le letture raw dei tre assi del magnetometro.
Roll e Pitch sono i valori, in radianti, delle inclinazioni sugli assi X e Y misurate dalla IMU

Quello che voglio aggiungere in una prossima versione è una ulteriore fusione tra la misura del Yaw (rotazione) fornita dalla IMU con l'orientamento compensato, questo per aumentare la stabilità e la risoluzione, non la precisione perché dipende dal magnetometro e con l'HMC5883L siamo attorno ai 2°. Domani, se trovo il tempo, faccio un breve video del tutto in funzione.

Questo progetto non è pensato per l’uso su i droni, serve come sensore di orientamento su un robot mobile, in pratica permette di dirigersi in modo abbastanza preciso verso una direzione, abbinato ad un sistema di misura della distanza percorsa, può bastare un encoder a media risoluzione, permette di spostarsi attraverso dei waypoint tramite coordinate polari, vettore (distanza) e angolo.

Esempio pratico di installazione del sensore su un robot, si trova nel case giallo posto sul supporto, è importante tenere il magnetometro lontano dai motori per evitare interferenze magnetiche.

robot.JPG

Si scusa, intendevo la sensorFusion per compensare la deriva dello yaw :)

Anche la compensazione usando il pitch e il roll non la conoscevo

+1

Mi sono scordato di allegarlo al primo post, questo è lo schema di cablaggio, fatto con Fritzing che per queste cose si presta molto bene, tra la Pro mini e i sensori, attenzione che sia l’MPU che l’HMC lavorano a 3.3V, idem il bus I2C, pertanto se sulla breakout del MPU non è presente un regolatore a 3.3V e le resistenze di pull up collegate al 3.3 V è necessario prevederli esterni.
Lo schema di cablaggio è presente anche nel download da github, si trova nella cartella hardware.

cablaggio.jpg

Mi piace ,ho una domanda , ma come si fa a calcolare l’offset dell’MPU senza un piano realmente tarato , nel senso, io anche nel mio progetto ho inserito una funzione di calibrazione che richiamo con un pulsante , lei calcola tutto , la inserisce in memoria e a successivo riavvio ho gli offset calcolati.
Adesso la mia domanda . Ma che piano usate per essere precisi :frowning:
Il prossimo test lo faccio con un secchio di acqua, un pezzo di polistirolo che galleggia , e sopra il mio sensore che ho inserito in un pezzo di nylon 10x10x2 , aspetto che si calmino le acque e provo a vedere se riesco ad avere un piano

Ho visto che hai inserito la media mobile. Molto golosa questa cosa =)

alessanddrob: Mi piace ,ho una domanda , ma come si fa a calcolare l'offset dell'MPU senza un piano realmente tarato

Gli MPU6050 e MPU9x50 arrivano già calibrati, non è necessaria una ulteriore calibrazione, è una delle loro caratteristiche principali, inoltre sono in grado di autoricalibrarsi da soli a run time, però è necessario caricare un apposito codice sul DMP per questa funzionalità dato i che registri che contengono i valori di calibrazione non sono accessibili da bus I2C. Unica accortezza è non muovere il sensore durante l'avvio, circa 1 secondo tenuto conto del tempo per caricare il codice del DMP, in questa fase viene rilevato lo zero per i tre giroscopi, non ha importanza se il sensore non è perfettamente in bolla. Quello che richiede una calibrazione, se si vuole ottenere la massima precisione possibile, è il magnetometro, però ho verificato che anche lavorando con i valori di default la precisione è accettabile per la destinazione d'uso di questo progetto, a breve rilascerò una GUI, sempre scritta con Processing, che permette la calibrazione del magnetometro e ne memorizza i relativi valori in EEPROM. Da notare che il sorgente del codice da caricare sul DMP non è open source, invensense ha reso pubblico solo il binario, sotto forma di listato numeri esadecimali, del codice da caricare, va fatto ogni volta che si alimenta l'MPU in quanto non possiede una flash interna, solo ram.

Devo ammettere che, da non programmatore, ancora sto cercando di capirci qualcosa, ma +1 lo stesso per la condivisione di un lavoro che appare molto ben fatto (persino a me che ci capisco poco :D)

Il che mi fa sorgere una domanda: potrei in futuro utilizzarlo per quel famoso progetto di bussola in overlay video per il ROV che al momento e' accantonato per questioni di tempo ? ... la cosa prevedeva una schedina minima con un 328 standalone, con il montaggio delle altre due "a sandwich", e la comunicazione con la scheda dell'overlay era pensata in RS232 ... ma non credo questi siano grossi problemi, giusto ?

Stasera esce la v 1.2, aggiungo un controllo "pronto al funzionamento" tramite il led della Pro mini e lo replico su un pin in modo da poterlo portare in esterno all'eventuale case che contiene il tutto. Il controllo serve per sapere se l'init del MPU è andato a buon fine, può capitare che per problemi legati alla I2C non si riesce a completare l'upload del codice per il DMP, e quando è possibile spostare il sensore dopo aver dato energia, bastano un paio di secondi, per sicurezza imposterò il tempo a 5 secondi. Attualmente se c'è un problema in fase di init il led rimane spento, se è tutto ok inizia a lampeggiare a 5 Hz, il lampeggio è gestito all'interno del ciclo invio dati che avviene ogni 20 ms, 50 pacchetti al secondo, circa 1.8 ms per l'invio dell'intero pacchetto @115200 bps. Sto valutando di inserire una ulteriore define che permette se scegliere tra l'invio continuo dei dati oppure l'invio dietro richiesta, p.e. si manda una @ sulla UART del sensore e questo risponde con l'ultima serie disponibile di valori.

Etemenanki: Il che mi fa sorgere una domanda: potrei in futuro utilizzarlo per quel famoso progetto di bussola in overlay video per il ROV che al momento e' accantonato per questioni di tempo ? ...

Per il ROV è perfetto, hai sia l'assetto che l'orientamento, se non mi ricordo male, devo verificare, occorre inserire una ulteriore compensazione, una cosa simile alla declinazione magnetica, per usare le bussole magnetiche sott'acqua. Hai disponibile anche un ulteriore dato, per il momento non usato ma inviabile nel pacchetto, che è il Yaw della IMU, questo è un valore di tipo relativo che puoi azzerare in qualunque momento e usarlo per ruotare in modo molto preciso con una risoluzione al decimo di grado, precisione reale migliore di 0.5° Magari quando metti mano al ROV questa cosa la vediamo assieme.

Che dire +1

>alessanddrob: le tue domande sono state separate QUI.

Guglielmo