Fountain RC boat: control implementation [2/2]

1

Nel post precedente è stato presentato il progetto S.W.A.T. (Sampling Water Asv Tracking), in particolare del lavoro svolto dal team A.S.V. (Autonomous Surface Vehicle) responsabile del controllo dell’imbarcazione. Le attività analizzate sono state le seguenti:

– analisi delle specifiche di progetto

– sviluppo del modello cinematico
– progettazione e simulazione di una strategia di controllo.

A questo punto del progetto l’imbarcazione si presenta come segue

2La barca è stata equipaggiata con i sensori/attuatori necessari per l’esecuzione della strategia di controllo, ha a bordo una scheda elettronica resistente all’acqua e contenente il microcontrollore, ed è in grado di comunicare con la base attravero un modulo WI-FI.

1) CONTROLLO AUTOMATICO, IMPLEMENTAZIONE

Il controllo automatico da implementare su microcontrollore è stato sviluppato nella prima parte di questo articolo:

Schema di controllo: implementazione in Simulink

Schema di controllo: implementazione in Simulink

La fase successiva alla simulazione e alla validazione consiste nell’implementazione. A livello hardware si è scelto il PIC32, un microcontrollore a 32 bit della Microchip, d’altra parte a livello software ci si è serviti di Labview per la scrittura del codice e del  3Dmicro Toolkit, e  MPLABX per la compilazione e l’upload del codice nel PIC32.

Il primo passo è stato quello di riprodurre lo schema sviluppato in ambiente Simulink in Labview, usando i blocchi messi a disposizione del Control Design & Simulation Toolkit. Di seguito vengono riportati Front Panel e Block Diagram.

LabVIEW block diagram

LabVIEW block diagram

3

1.1) DATA FILTERING

Simulato lo schema in Labview, si è passati ad un’opera di traduzione dello schema precedente in una formattazione adatta al target embedded.

Il passaggio tra simulazione e implementazione è delicato, in quanto nella realtà si ha a che fare con oggetti fisici soggetti a rumore, con determinate caratteristiche.

Ciò si è tradotto:

[*] nell’implementazione di un filtro a media mobile, con finestra 10, per la bussola. Il filtro essenzialmente effettua una media sugli utlimi 10 valori acquisiti dalla bussola, di seguito viene riportato uno schema che ne mostra il funzionamento.

Filtro bussola

Filtro bussola

[*] nell’adattamento del sistema di riferimento “angolare” di GPS e bussola, al fine di rendere possibile la comparazione tra orientamento desiderato e attuale. Le coordinate (latitudine,longitudine) restituite dal GPS, oltre a fornire la posizione dell’imbarcazione vengono utlizzate anche per calcolare la direzione desiderata, attraverso la funzione di atan2:

atan2

La funziona atan2 presenta una discontinuità per cui l’angolo theta varia tra -180° a +180°. Attraverso la funzione di unwrap è possibile mappare tale intervallo in 0° 360°, come mostrato nella figura.

GPS reference system

GPS reference system

Anche l’angolo restituito dalla bussola varia tra -180° e +180°, tuttavia in questo caso l’opera di unwrapping è effettuata considerando in quale quadrante si trova la misura:

unwrapCMP

Attraverso le precedenti equazioni, anche l’angolo della bussola ha una variazione tra 0° e 360° come mostra la figura:

CMP reference system

CMP reference system

Sistemata la questione dei sistemi di riferimento angolare, è possibile confrontare i due angoli. Bisogna tuttavia fare una piccola considerazione a riguardo l’errore di orientazione. Una delle situazioni critiche nella fase di orientamento corrisponde al caso in cui la barca è orientata con la posizione di goal ma con verso opposto, che corrisponde ad un errore di 180°. Tuttavia dato che entrambi i sistemi di riferimento effettuano il calcolo dell’angolo rispetto all’asse positivo delle ascisse, ciò porta non solo ad avere un errore di orientamento maggiore di quello reale, ma costringe anche l’imbarcazione a scegliere sempre la stessa direzione (antioraria) visto che l’errore è sempre positivo. Per tale motivo, occorre sempre togliere all’errore di orientamento un offset pari all’orientamento desiderato -180°. Lo schema che segue spiega a livello grafico quello che è stato appena detto:

Errore di orientamento reference system

Errore di orientamento reference system

1.2) IMPLEMENTAZIONE IN LABVIEW

Lo schema di controllo sviluppato in Labview tuttavia non è adatto al target-embedded, per cui è stao adattato in quanto:

[*] il PIC32 ha una archittettura che non supporta il multi-tasking, per cui il codice deve essere reso procedurale

[*] i blocchi usati nello schema di controllo, quali ad esempio saturazione, unwrap e switch appartengono al Control Design & Simulation Toolkit, possono essere eseguiti all’interno di un particolare frame non supportato dal target embedded.

A tale proposito è stato sviluppato un diagrmma un flow-chart che riassume i passi principali da eseguire ogni ciclo:

flow chart

flow chart

L’intero codice, sulla base del precedente flow-chart, è stato convertito in una grande flat sequence, costituita da differenti frames, case-structures, e subVI costituiti da blocchi supportati da librerie standard c. Il block diagram sviluppato è ampio, per tale motivo viene riportata un pseudo-routine, che spiega a livello intuitivo come avviene la sequenza delle istruzioni.

41.3) RISULTATI

La routine sviluppata è stata caricata nel microcontrollore ed è stata testata con l’imbarcazione reale. Durante la prova sono stati raccolti diversi dati, tra cui il periodo delle PWM[ms] inviate ai motori dall’architettura di controllo, la posizione orientamento attuale e errore di orientamento. Di seguito viene riportato il grafico di uno dei risultati ottenuti:

Risultati

Risultati

L’esperimento è stato condotto considerando il “worst case scenario”, ovvero l’imbarcazione era nella stessa direzione del goal ma con verso opposto. Il grafico precedente può essere suddiviso in due parti, le quali corrispondono alle due parti della strategia di controllo.

-Nella prima parte, una volta effettuato lo switch dalla modalità manuale a quella automatica (al tempo 130s nel plot), dato che l’imbarcazione si trova in direzione opposta rispetto al target, il motore BL e il servo passano dalla posizione/velocità di riposo, ad una velocità costante per il BL, ed un angolo di rudder crescente. Il sistema si trova nell’orientation control law, ovvero l’imbarcazione si sta direzionando verso l’orientamento desiderato, e ciò è testimoniato dalla diminuzione dell’errore di orientamento, mostrato nel secondo plot.

Se si osserva attentamente l’andamento della PWM relativa al rudder, ci si rende conto che ad un tratto il TON del servo viene mantenuto a 1.1ms, ciò è legato al fatto che le uscite di controllo che vengono date sia al servo che al BL sono fatte passare per funzioni di saturazione al fine di non danneggiare gli attuatori.

-Nella seconda parte, (al tempo 150s dei plot), l’errore di orientamento è all’interno dell’intervallo +-15°, per cui il supervisore effettua lo switch dall’orientation control law, allo speed control law. Ciò è testimoniato dal fatto che la PWM del brushless incrementa il TOn (saturato a 1.6ms), mentre la PWM del rudder si mantiene all’interno di un piccolo intervallo centrato sulla posizione di riposo, corrispondente a 1.5ms.

Finalmente, quando si sta raggiungendo la deadzone, il rudder continua a correggere i piccoli drifts rispetto all’orientamento desiderato mentre il motore brusheless comincia a decrementare la propria velocità, fino a quando, raggiunta la deadzone, si spegne.

1.4) GOOGLE MAPS TRACKING SYSTEM

Per monitorare la posizione della barca, è stata sviluppata una pagina HTML usando le API di Google Maps. I compiti della pagina sono:

-per ogni posizione restituita dal GPS disegnare un marker

-inviare al microcontrollore la posizione desiderata, ovvero il target, cliccando un punto sulla mappa.

La versione finale della tracking interface è la seguente:

5dove

[F26: M1] rappresenta la posizione di goal, settata dall’interfaccia

[F27: M2] rappresenta la pozisione iniziale

[F28: M3] rappresenta la poszione attuale restituita dal GPS

Entrando un pò più nel dettaglio, la data-pipeline che parte dalla pagine HTML fino ad arrivare a Labview è la seguente:

[F29: DATA-PIPELINE]

-HTML page, corrisponde alla mappa di GoogleMaps, dove sono visualizzati i markers

-PHP script, ha il compito di scrivere la posizione di goal in due file (uno che memorizza la latitudine e uno per la longitudine)

-TXT files, memorizzano il goal suddiviso in latitudine e longitudine

-Labview, il quale legge la posizione di goal dai precedenti file, la memorizza in due variabili locali, e le invia tramite Ethernet al microcontrollore.

la data-pipeline inversa, ovvero che va da Labview fino alla pagina HTML è la seguente:

[F30: DATA-PIPELINE]

-Labview, il quale scrive la posizione attuale restituita dal microcontrollore attraverso la porta Ethernet, in due file, uno per la latitudine attuale l’altro per la longitudine attuale.

-TXT files, memorizzano la posizione attuale, scritta da Labview

-HTML page, visualizza la posizione attuale della barca, letta sui file .txt, sulla mappa di GoogleMaps.

La prossima immagine mostra una prova effettuata solo con il modulo GPS (non montato sull’imbarcazione):

6GRUPPO A.S.V.

Celiberti Francesco

Correani Giacomo

Rocchi Matteo

Francesco Celiberti

Ciao a tutti,


mi chiamo Francesco, sono laureato in Ing. Informatica e dell’Automazione. Sono attualmente coinvolto in un progetto di ricerca Europeo, MOTORIST. www.motorist-ptw.eu


Tags:
By Francesco Celiberti | marzo 11th, 2014 | LEAVE A COMMENT