Questo è il secondo articolo che dedico ad un progetto a cui sto lavorando ormai da anni insieme al mio compare e amico Roberto, il Seastick.

In questo breve articolo voglio parlare di uno dei sensori a mio parere più complessi, tuttavia necessario in qualunque robot autonomo. Sto parlando del senso dell' "equilibrio", se così si può dire...

Per poter controllare gli attuatori del Seastick affinché corregga e mantenga la posizione e l'inclinamento voluti, è necessario un sensore che possa stimare l'angolo di rotazione rispetto all'asse terrestre.

yaw_pitch_roll.png

Il problema è che non esiste (almeno per ora) un sensore che da solo possa farlo, anche perché forse non esiste nemmeno un riferimento assoluto a cui fare riferimento, ma mi spiego meglio.

Supponiamo di voler misurare uno dei tre angoli in forma di Eulero, per esempio l'angolo di rotazione lungo l'asse x, che in aeronautica possiamo chiamare "pitch" o "beccheggio". Sul mercato sono disponibili moltissimi tipi di sensori inerziali, realizzati con svariate tecnologie e ognuna ha dei pro e dei contro. Si possono trovare accelerometri realizzati con tecnologia mems, ottici e meccanici anche se i più usati per contenere costi pesi e dimensioni sono i mems (sono anche quelli che abbiamo utilizzato noi).

Ragionando solo sugli accelerometri si potrebbe pensare di utilizzare due accelerometri montati a 90° tra loro, uno lungo l'asse Z e l'altro lungo l'asse Y. In questo modo con il nostro sensore fermo sul banco di misura, l'accelerometro Z misura la gravità e l'Y un valore pari a zero. Ruotanto il sensore attorno ad X abbiamo le due misure di accelerazione che rappresentano le componenti del vettore G, più si ruota la coppia attorno ad X e più la componente Y diventa grande a scapito di Z. A questo punto basterebbe una actg() per avere l'angolo! sarebbe bello ma purtroppo non è così.
Infatti anche se nella misura è statica tutto sembra funzionare, in quella dinamica gli accelerometri si muovono e quindi alle componenti vettoriali di G si vanno a sommare le componenti dell'accelerazione impressa, impossibili da considerare se non si conosce il vettore accelerazione (va da sé che se lo conosciamo non abbiamo bisogno di ricalcolarlo...).
Visto che gli accelerometri il più delle volte non sembrano aiutarci, potremmo pensare di ricorrere a qualcosa di altro, ad esempio i giroscopi. Anche loro possono essere realizzati con tecnologia mems e misurano una velocità angolare attorno ad un asse di riferimento.
Il principio di funzionamento dei giroscopi elettromeccanici è lo stesso dei giroscopi tradizionali, in cui una massa in rotazione, grazie al principio di conservazione del momento angolare, tende a mantenere il suo asse di rotazione in una direzione fissa. Nel caso dei mems non ci sono masse in rotazione bensì un corpo centrale che viene fatto vibrare, mentre in quelli ottici si effettua una misura interferometrica considerando l'effetto Sagnac. Comunque il risultato è sempre la velocità angolare, e per passare da velocità angolari ad angoli dobbiamo necessariamente integrare le velocità. Questo sarebbe perfetto se non fosse che come tutti i sensori, anche i giroscopi hanno caratteristiche poco desiderabili come noise, drift, offset, level change vs temperature etc... basta dare un'occhiata ad un datasheet per rendersene conto. Il problema in questo caso è particolamente grave perché per quanto piccolo sia l'errore, integrando i dati abbiamo un effetto di deriva esponenziale fortemente dipendente dalla frequenza di integrazione. Questo è il motivo per il quale i giroscopi da soli non sono sufficienti.
Con queste due tipologie di sensori possiamo al massimo ottenere solo due dei tre angoli, il pitch e il roll (beccheggio e rollio), per lo yaw l'accelerometro non ci aiuta per niente e il giroscopio può fornirci solo un angolo relativo rispetto alla posizione di partenza (con tutti i limiti già descritti). Volendo riferire il sensore al nord magnetico è necessario utilizzare anche dei sensori megnetici (nel nostro caso sempre mems).
Quindi ricapitolando, per stimare i tre angoli yaw pitch e roll ci servono accelerometri, giroscopi e magnetometri, uno per ogni asse, per un totale di 9 sensori o 9 libertà di movimento.
Per arrivare al risultato finale di avere yaw pitch e roll serve un controllore che implementa un algoritmo di data fusion, in grado acquisire e miscelare questi dati ad una velocità sufficientemente alta. Algoritmi capaci di questo ne esistono diversi e si basano su fondamenti matematici abbastanza complessi, nei quali non intendo entrare. Per chi volesse approfondire l'argomento può cercare documenti sul web che riguardano l'implementazione del filtro di Kalman esteso oppure del direct cosine matrix.
In commercio esistono diversi tipi di sensori che tentano di risolvere questo problema, si parte da poche decine di euro per arrivare a svariate decine di migliaia di euro... si trova molto anche nel mondo dell'open source.
Noi ne abbiamo provati diversi, partendo dai più economici per arrivare a quelli commerciali da qualche migliaio di euro, senza trovare enormi differenze e neanche nulla di veramente soddisfacente. Per realizzare la nostra scheda c'è voluto circa un anno di tentativi, studio e delusioni, ma con grande soddisfazione alla fine ci siamo riusciti.

IMU.jpg

La nostra scheda ha un formato circolare di 50mm di diametro che a noi piace chiamare "round 5", e integra 3 accelerometri, 3 giroscopi, 3 magnetometri, 2 sensori di temperatura e uno di pressione atmosferica. I dati vengono campionati ed elaborati ad una freq. di 500Hz tramite un uC ARM Cortex M4, che implementa l'algoritmo per la stima degli angoli. La scheda si interfaccia con il mondo esterno tramite un'interfaccia CAN isolata galvanicamente, in più è dotata di porta USB e una seriale utilissima per la configurazione ed il debug (in più abbiamo lasciato alcuni GPIO, analog input, una I2C, un comparatore per eventuali espansioni).

Dopo il periodo iniziale di sviluppo e di test, una delle sfide più grandi che abbiamo affrontato per realizzare questa scheda è stato il montaggio. Infatti la stragrande maggioranza dei sensori mems vengono venduti in package del tipo LGA, quindi a montaggio superficiale senza reofori e senza le sfere dei BGA.. molto piccoli e difficili da centrare. Inoltre la scheda è di dimensioni molto ridotte e per installare tutto il necessario abbiamo dovuto realizzare un pcb a 4 layer. Tuttavia con un po' di esperienza e la giusta attrezzatura adesso le possiamo montare, anche perché farsi montare pochi prototipi da una ditta specializzata avrebbe avuto dei costi per noi inaccettabili.
La scheda IMU può essere impilata sopra altre schede dello stesso formato connesse tramite CAN, nel caso del Seastick abbiamo montato la IMU sopra una scheda da noi realizzata che svolge la funzione di GPS ad alta risoluzione (se gli astri sono favorevoli ha un errore <1m).

imu+gps.png

GPS_ublox+IMU.png

A scopo di debug e dimostrazione abbiamo realizzato un software che mostra come i dati dei sensori, dopo essere stati elaborati dal uC, vengano inviati al modello grafico tridimensionale della scheda stessa che replica la rotazione di quella vera. Per trasferire i dati tra la scheda ed il computer abbiamo utilizzato la porta seriale normalmente utilizzata per il debug.

La nostra scheda rileva una rotazione di 360° ed in condizioni statiche ha una risoluzione di 0.2°. Difficile invece stimare la reale risoluzione in condizioni dinamiche, in cui le accelerazioni lineari non sono note. Anche i prodotti più professionali e costosi hanno datasheet pocho chiari a riguardo e a volte glissano del tutto. Dall'esperienza che ci siamo fatti, ci siamo convinti che l'ideale sia tarare il proprio algoritmo per le proprie necessità, considerando parametri come la reattività, la stabilità, la precisione, l'accuratezza, la robustezza alle accelerazioni, etc.. spesso tutto non si può avere, noi ad esempio avendo accelerazioni basse del mezzo, prediligiamo la stabilità, un velivolo come un quadricottero al contrario necessità di una buona reattività.

Il link alla pagina relativa del concorso Toradex a cui stiamo partecipando è il seguente:

Toradex Contest - Seastick - IMU

Spero che l'articolo vi abbia interessato!

Flavio