Progettazione, realizzazione ed implementazione del sistema di

annuncio pubblicitario

UNIVERSITÀ DEGLI STUDI DI ROMA
“TOR VERGATA”
FACOLTÀ DI INGEGNERIA
CORSO DI LAUREA IN INGEGNERIA
DELL’AUTOMAZIONE
A.A. 2009/2010
Tesi di Laurea
PROGETTAZIONE, REALIZZAZIONE ED
IMPLEMENTAZIONE DEL SISTEMA DI CONTROLLO
ED AUTOLOCALIZZAZIONE DI UN MICROMOUSE
RELATORE
CANDIDATO
Ing. Daniele Carnevale
Giuseppe Bernieri
CORRELATORE
Ing. Francesco Martinelli
A mio padre.
Indice
Ringraziamenti
1
Introduzione
2
1 Robot LineFollower
6
1.1
L’idea . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
1.2
Descrizione struttura . . . . . . . . . . . . . . . . . . . . . . . . . . .
7
1.3
Motori in corrente continua CC . . . . . . . . . . . . . . . . . . . . .
7
1.4
Ponte H . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9
1.5
PWM e duty cycle . . . . . . . . . . . . . . . . . . . . . . . . . . . .
10
1.6
Sign-Magnitude PWM e Locked Anti-phase PWM . . . . . . . . . . .
12
1.7
Sensori . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
16
1.8
Encoder Ottici . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
17
1.9
Arduino Mega . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
19
1.10 Alimentazione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
21
1.11 Implementazione della legge di controllo . . . . . . . . . . . . . . . .
21
2 Robot micromouse
25
2.1
Robot Rover . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
25
2.2
Descrizione struttura . . . . . . . . . . . . . . . . . . . . . . . . . . .
27
INDICE
I
INDICE
2.3
Motori passo-passo . . . . . . . . . . . . . . . . . . . . . . . . . . . .
27
2.3.1
Dimensionamento motori . . . . . . . . . . . . . . . . . . . . .
28
2.3.2
Micro-relè e libreria stepper.h . . . . . . . . . . . . . . . . . .
32
2.4
Scheda sensori . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
37
2.5
Scheda di potenza, controllo e comunicazione . . . . . . . . . . . . . .
38
2.6
Arduino Nano . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
39
2.7
Modulo wireless: XBee . . . . . . . . . . . . . . . . . . . . . . . . . .
40
2.8
Alimentazione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
41
3 Filtro di Kalman Esteso
42
3.1
Introduzione al filtro . . . . . . . . . . . . . . . . . . . . . . . . . . .
42
3.2
Modello cinematico . . . . . . . . . . . . . . . . . . . . . . . . . . . .
44
3.2.1
Determinazione sperimentale delle incertezze del moto . . . .
45
3.3
Modello di misura . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
49
3.4
Implementazione del filtro . . . . . . . . . . . . . . . . . . . . . . . .
51
3.5
Simulazioni . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
52
4 Conclusioni e sviluppi futuri
55
Appendice A - Schemi ed immagini
58
Appendice B - Listati dei programmi
63
Elenco delle figure
81
Bibliografia
82
INDICE
II
Ringraziamenti
Desidero innanzitutto ringraziare l’Ing. Daniele Carnevale per la sua disponibilità
ed il prezioso supporto sia didattico che morale. Inoltre ringrazio l’Ing. Francesco
Martinelli che, con i suoi consigli, mi ha aiutato a dirimere i dubbi sul mio lavoro.
Grazie ai colleghi ed ai compagni di laboratorio che in questi anni hanno condiviso
con me l’esperienza universitaria.
Vorrei esprimere la mia sincera gratitudine ai miei genitori per l’affetto ed il supporto:
grazie a papà, correlatore aggiunto, a mamma ed a mia sorella Iaia per il prezioso
contributo grafico.
Grazie a tutti gli amici ma soprattutto a chi in questi anni, anche nei momenti difficili,
è sempre stato al mio fianco, in particolare Mauro ed Eros. Infine un ringraziamento
speciale va ad Elena.
Introduzione
1
Introduzione
Nel corso degli ultimi decenni la tecnologia si è evoluta esponenzialmente, permettendo
all’uomo di raggiungere traguardi sempre più sorprendenti. L’abbattimento dei costi
di produzione ha permesso alla tecnologia di essere alla portata di tutti, ed è grazie
a queste condizioni che la robotica, scienza giovane, è riuscita a svilupparsi anche in
campi diversi da quello naturale di elezione, ovvero il campo industriale di alto livello.
La robotica è definita come la scienza che, studiando il comportamento degli esseri
intelligenti, cerca di emulare, in maniera automatizzata, quei compiti che potrebbero
risultare troppo difficili o inadatti all’operatore umano. Grazie alla sua natura globale,
la robotica trova applicazioni in molti campi di utilizzo: dall’industria alle applicazioni
domestiche, dall’arte alla biologia, dalla medicina all’intrattenimento. Tra tali campi,
seppur molteplici, è bene tener presente che talvolta non esiste una vera linea di
confine.
La realizzazione di un robot spesso nasce da una semplice idea e viene generalmente
affrontata su due piani distinti:
lo studio, la progettazione e la realizzazione elettromeccanica del robot;
l’analisi e l’implementazione della logica di controllo.
Per la realizzazione elettromeccanica del robot è necessario un progetto dettagliato
dei cinematismi e degli azionamenti necessari alla sua movimentazione. Il progetto e
Introduzione
2
Introduzione
la realizzazione del sistema di controllo, invece, richiedono l’analisi del modello matematico che descrive la dinamica del robot in relazione agli obiettivi da raggiungere.
Entrambe le fasi, poi, devono convergere nella realizzazione di un meccanismo in grado di rispondere correttamente alle istruzioni fornite dalla logica di controllo, al fine
di ottenere il risultato pratico desiderato. Anche questo lavoro di tesi affronta la realizzazione dei robot proposti (robot linefollower e robot rover) con tale approccio. Gli
esemplari di robot progettati e realizzati verranno descritti sia nella parte puramente
elettronica e meccanica sia nella parte relativa al controllo, che rappresenta in effetti il
cuore scientifico del lavoro. Entrambe le parti vengono trattate in maniera sinergica e
coordinata, in modo da realizzare il dispositivo richiesto in maniera efficiente e rispondente alle specifiche fissate. Le soluzioni di controllo impiegate prevedono l’utilizzo
di sistemi di prototipazione microprogrammabili della famiglia Arduino. Tali sistemi,
molto economici ma contemporaneamente molto versatili e dotati di prestazioni di
tutto rispetto, sono attualmente di grande interesse per gli sviluppatori di soluzioni di
controllo a basso costo grazie anche alla semplicità di realizzazione delle applicazioni
ed alla flessibilità di utilizzo.
Questo lavoro di tesi può essere collocato in tre contesti applicativi: la robotica
industriale, la domotica e l’intrattenimento. Per quanto riguarda la robotica industriale, visto l’evolversi e la diffusione sempre maggiore di strumenti di automazione
e controllo all’interno dei moderni complessi industriali, lo studio svolto sui robot
linefollower, sui robot rover e sui sensori di cui essi sono dotati ricalca le esigenze e
le linee di ricerca e sviluppo di molte realtà industriali moderne. Per lo stesso motivo, anche se con fini diversi, le esperienze sperimentali condotte con tale tipologia
di robot nelle applicazioni per uso domestico, la cosiddetta domotica, hanno fatto
emergere ed esaltato gli utilizzi di tali dispositivi, e della robotica in generale, per
Introduzione
3
Introduzione
facilities casalinghe, quali, ad esempio, i robot-spazzini. Infine, grazie alla versatilità
ed alle ridotte dimensioni dei robot presi in esame, le loro applicazioni rientrano anche
nel campo dell’intrattenimento; in tale contesto, ad esempio, possono essere considerate le micromouse competitions, competizioni di livello internazionale, riconosciute
dall’IEEE, dove piccoli robot rover, dotati di attuatori, sensori e di microcontrollore,
devono risolvere autonomamente un labirinto nel minor tempo possibile.
Per una più agevole lettura, nel seguito è riportata la struttura del lavoro di tesi.
1. nel Capitolo 1 viene discusso l’approccio di primo livello alla robotica sperimentale affrontato nel lavoro di tesi: la realizzazione e l’implementazione di un
piccolo robot linefollower; questa tipologia di robot, grazie al microcontrollore,
ai sensori ed agli attuatori di cui è dotato, ha come unico scopo quello di seguire
una linea disegnata sul pavimento con la massima precisione possibile.
2. nel Capitolo 2 viene discussa la realizzazione pratica e l’implementazione di
un mini-robot rover, in grado di evitare gli ostacoli. La realizzazione è stata
inoltre orientata verso la realizzazione di un prototipo di micromouse. Vengono analizzati i singoli componenti della struttura, dai sensori agli attuatori,
dal microcontrollore ai circuiti elettronici integrati di pilotaggio e di potenza
utilizzati.
3. nel Capitolo 3 viene discusso il metodo di controllo scelto ed utilizzato per l’implementazione del mini-robot rover: il Filtro di Kalman Esteso. Viene analizzato
il modello matematico discreto associato all’applicazione e viene illustrata, passo dopo passo, la tecnica utilizzata per implementare tale controllo, partendo da
misure odometriche effettuate sperimentalmente per ottenere la stima necessaria
dei parametri del filtro.
Introduzione
4
Introduzione
4. nel Capitolo 4 sono brevemente riassunti i risultati conseguiti nella sperimentazione eseguita con i robot realizzati, al fine di evidenziarne le caratteristiche
salienti e le performance. Vengono discussi inoltre i possibili sviluppi futuri del
lavoro svolto.
Introduzione
5
Capitolo 1
Robot LineFollower
In questo capitolo verrà analizzata la progettazione e la realizzazione
di un Robot LineFollower.
1.1
L’idea
Alcuni dei primi veicoli con guida automatizzata sono stati i LineFollower. Come è
chiaro dal nome, l’obiettivo principale di questo tipo di robot è quello di seguire una
linea dipinta o incorporata nel pavimento. La maggior parte di questi robot adottano
un algoritmo di controllo molto semplice che ha come unico scopo quello di mantenere
la linea da seguire al centro del sistema di sensori utilizzato per il rilievo della linea
stessa, mediante una opportuna azione sui motori che ne determinano il movimento.
Un importante esempio di utilizzo di tale robot è quello riferito all’uso che se ne è
fatto per la costruzione e manutenzione del CERN di Ginevra: per spostare le grandi
apparecchiature all’interno dei lunghi tunnel del Large Hadron Collider sono stati
utilizzati dei montacarichi mobili che utilizzano la tecnologia del Linefollower al fine
di avere la massima precisione negli spostamenti in uno spazio molto ristretto.
6
Cap. 1 Robot LineFollower
§1.2 Descrizione struttura
Figura 1.1: Tunnel del LHC, a destra la linea che segue il LineFollower.
1.2
Descrizione struttura
Il prototipo allestito è costituito da una base in lega metallica alla quale è stata fissata
la struttura del doppio motoriduttore Tamiya Twin Motor Gearbox, contenente i
motori in corrente continua. Al di sopra di tale base è stata inserita una bread board
di dimensioni 82mm x 54mm con 400 punti di connessione utilizzata per la gestione
del circuito elettronico. Con l’ausilio di distanziali è stata posta al di sopra della bread
board la scheda Arduino Mega. Le ruote utilizzate, in asse con i motoriduttori, sono
in gomma, hanno un diametro di 4.5 cm ed uno spessore di 2.8 cm.
1.3
Motori in corrente continua CC
Per la movimentazione del robot sono stati utilizzati due motori in corrente continua.
I motori utilizzati sono i FA-130, capaci di ruotare, a vuoto e senza riduzione, a 13230
giri al minuto. Visti gli alti valori di velocità di rotazione, tali motori necessitano di
motoriduttori, anche al fine di incrementare la coppia prodotta. Per il robot è stato
7
Cap. 1 Robot LineFollower
§1.3 Motori in corrente continua CC
Figura 1.2: Il LineFollower realizzato.
Figura 1.3: Motore CC e Tamiya Motor Gearbox
utilizzato il doppio motoriduttore Tamiya Twin Motor Gearbox, caratterizzato da un
ingombro ridotto (7.5cm di lunghezza) e di predisposizione per accogliere entrambi i
motori utilizzati. Il moto è trasmesso alle ruote mediante due assi esagonali da 3mm.
Il motoriduttore è assemblabile con due diversi rapporti di riduzione: 58:1 o 203:1. In
entrambi i casi, i motori producono una coppia motrice sufficiente a muovere il robot.
8
Cap. 1 Robot LineFollower
1.4
§1.4 Ponte H
Ponte H
Per la gestione dei due motori in corrente continua si è scelto di utilizzare un ponte H
integrato L293DNE che permette il pilotaggio dei motori direttamente dalla scheda
Arduino. La versione utilizzata del componente comprende al suo interno anche i
diodi che permettono di evitare i problemi dovuti ad eventuali correnti di ricircolo dei
motori. Con riferimento allo schema elettronico del componente, è stata utilizzata la
configurazione Two-Phase Motor Driver. In particolare, avendo utilizzato un metodo
di controllo del tipo Locked Anti-phase (illustrato più dettagliatamente nel seguito),
ai pin 1 e 9 sono state collegate due uscite digitali della scheda Arduino per gestire
l’attivazione rispettivamente del motore di destra e di sinistra; ai pin 2,7,10 e 15 sono
state collegate le uscite PWM dell’Arduino per controllare la velocità ed il verso di
rotazione dei motori; ai pin 3 e 6 è collegato il motore di sinistra mentre ai pin 11 e
14 quello di destra; al pin 8 è collegata l’alimentazione dei motori mentre al pin 16
l’alimentazione della logica dell’integrato; infine, ai pin 4, 5, 12, 13 è collegata la massa.
Per le informazioni tecniche di utilizzo di tale integrato si rimanda al datasheet[1].
9
§1.5 PWM e duty cycle
Cap. 1 Robot LineFollower
Figura 1.4: Il ponte H.
1.5
PWM e duty cycle
Per il controllo della velocità e del verso di rotazione dei motori è stato utilizzato
un approccio che prevede l’impiego di un segnale di tipo PWM. Un segnale PWM
(Pulse Width Modulation, ovvero modulazione a variazione della larghezza d’impulso)
è assimilabile, in effetti, ad un segnale ad onda quadra con duty cycle variabile.
Nelle usuali applicazioni, mediante un segnale PWM è possibile controllare la corrente
assorbita da un carico elettrico modulando opportunamente il duty cycle. Più nel
10
§1.5 PWM e duty cycle
Cap. 1 Robot LineFollower
dettaglio, un segnale PWM è caratterizzato da:
Frequenza costante f;
Duty cycle variabile;
due possibili valori di ampiezza (nel nostro caso 0V e +5V).
Il duty cycle (D) è il rapporto percentuale tra il periodo (TON ) nel quale il segnale è
alto (+5V) e il periodo totale:
1
= TON + TOFF
f
(1.5.1)
TON
TON
· 100 =
· 100
T
TON + TOFF
(1.5.2)
T =
D=
Figura 1.5: Duty Cycle.
Con riferimento alla figura, un duty cycle del 50% corrisponde ad un’onda quadra
che assume valore alto (+5V) per il 50% del periodo, mentre un duty cycle dell’80%
corrisponde ad un’onda quadra che assume valore alto (+5V) per l’80% del periodo e
basso (0V) per il restante 20%. Tra tutti i valori intermedi, è rilevante un D del 50%
11
Cap. 1 Robot LineFollower
§1.6 Sign-Magnitude PWM e Locked Anti-phase PWM
poiché il tempo TON e quello TOFF si equivalgono, dando luogo ad una onda quadra
standard.
1.6
Sign-Magnitude PWM e Locked Anti-phase PWM
Esistono due modalità di utilizzo del segnale PWM per il pilotaggio di un ponte H: il
PWM Sign-Magnitude e il PWM Locked Anti-Phase.
Figura 1.6: Sign-Magnitude PWM.
Per il controllo SM (Sign-Magnitude) sono necessari due segnali: (a) un segnale PWM
con duty cycle variabile tra 0 e 100%, che stabilisce la velocità di rotazione del motore;
(b) un segnale logico costante che determina il verso di rotazione del motore (segnale
basso per rotazione in un verso, segnale alto per rotazione nell’altro verso).
Nello specifico, il pilotaggio SM è realizzato inviando il segnale PWM all’ingresso
di enable del ponte H e comandando la direzione di rotazione del motore tramite
i due ingressi di controllo. Tali due ingressi devono essere comandati da segnali
invertiti; utilizzando un elemento inverter aggiuntivo si evita di dover ricorrere a due
porte digitali d’uscita del microcontrollore, riducendo il numero di uscite impiegate
12
Cap. 1 Robot LineFollower
§1.6 Sign-Magnitude PWM e Locked Anti-phase PWM
ed evitando di dover implementare funzioni aggiuntive. In questo modo, il motore
presenta coppia motrice solo nel tempo TON del PWM e si ferma nel tempo TOFF ;
in realtà non è un vero e proprio arresto, in quanto il motore continua a girare per
inerzia senza ulteriore coppia motrice. Se il PWM ha un duty cycle del 100%, il motore
presenta sempre coppia motrice e ciò si traduce nella massima velocità possibile nella
direzione scelta tramite i segnali logici di controllo. Se il PWM ha un duty cycle
dello 0%, il motore non presenta coppia motrice e non si muove, a prescindere dalla
direzione selezionata mediante i segnali di controllo. I valori intermedi di duty cycle
inducono variazioni di velocità dal minimo (0%) al massimo (100%). Questa tecnica
presenta le seguenti caratteristiche:
sono necessari due segnali (PWM e segnale logico di controllo) per comandare
ogni motore;
il motore è libero di ruotare anche con D=0%, per cui partenza e arresto sono
più lenti della configurazione LAP (discussa nel seguito);
il motore non consuma da fermo.
13
Cap. 1 Robot LineFollower
§1.6 Sign-Magnitude PWM e Locked Anti-phase PWM
Figura 1.7: Locked Anti-Phase PWM.
Il controllo LAP (Locked Anti-Phase) si basa sulla stessa configurazione circuitale
del controllo SM, ma con i segnali di interesse scambiati di posto. In particolare, il
segnale PWM è posto agli ingressi di ogni lato del ponte H utilizzando un invertitore
per ottenere segnali in opposizione di fase; il segnale logico di controllo è invece
applicato agli ingressi di enable, unicamente per abilitare o disabilitare il lato del ponte
d’interesse (dal punto di vista operativo, tale segnale può anche essere mantenuto
sempre attivo).
Applicando un PWM con duty cycle del 50%, il motore gira in un verso per tutto il
periodo di ON mentre gira nell’altro per tutto il periodo di OFF; poiché la frequenza
del PWM è molto alta, l’inerzia del motore non consente di seguire istantaneamente
il moto imposto, con il risultato che il motore presenta un moto apprezzabile nullo.
Applicando un segnale PWM con duty cycle maggiore del 50%, il motore ruoterà in
un verso con velocità maggiore all’aumentare del duty cycle, raggiungendo la velocità
massima per D=100%. Applicando un segnale PWM con duty cycle minore del 50%,
il motore ruoterà nel verso opposto con velocità maggiore al diminuire del duty cycle,
raggiungendo la velocità massima per D=0%. Per il controllo LAP può quindi bastare
14
Cap. 1 Robot LineFollower
§1.6 Sign-Magnitude PWM e Locked Anti-phase PWM
anche solo un segnale di comando (l’enable può essere fissato alto se non necessario):
l’onda quadra con D variabile stabilisce sia la velocità che il verso di rotazione, come
precedentemente descritto. Questo controllo presenta le seguenti caratteristiche:
è necessario solo un segnale per poter variare la velocità e la direzione;
l’arresto del motore è immediato;
la partenza del motore è più reattiva;
il motore consuma anche da fermo;
la risoluzione per la regolazione di velocità è la metà della corrispondente ri-
soluzione del controllo SM (variazione 50-100% o 50%-0% per il controllo LAP
contro variazione 0-100% per il controllo SM).
Per la realizzazione del robot LineFollower è stato utilizzato il controllo LAP; non
sono stati però utilizzati gli elementi inverter aggiuntivi, per cui è stato necessario
programmare ed utilizzare 4 uscite PWM (a coppie invertite) del microcontrollore
invece di 2.
15
§1.7 Sensori
Cap. 1 Robot LineFollower
1.7
Sensori
Figura 1.8: Sensore QTI.
Per rilevare la linea sul pavimento sono stati utilizzati due componenti QTI1 . Essi
usano un sensore di riflettività ottica infrarossa QRD1114 che consiste in un insieme
emettitore/ricevitore capace di differenziare le superfici scure, con basso potere riflettente, da quelle chiare, con un alto potere riflettente. In base al tipo di superficie che
il sensore rileva, viene fornito un determinato valore di tensione in uscita. Il sensore
inoltre è dotato di filtro per la luce solare. Per il tipo di applicazione realizzato, i due
sensori utilizzati sono stati montati sulla parte inferiore del robot, in asse con le ruote
e posti parallelamente l’uno a l’altro ad una distanza pari alla larghezza della linea che
il robot deve seguire. Le uscite dei sensori sono state collegate a due porte analogiche
di ingresso dell’Arduino, in modo da prelevare i segnali in tensione necessari per il
funzionamento dell’algoritmo di controllo.
1
Per il datasheet si veda [2].
16
Cap. 1 Robot LineFollower
1.8
§1.8 Encoder Ottici
Encoder Ottici
Per il controllo e la stima dello spostamento (misura odometrica) attraverso il moto
dei motori si è fatto ricorso ad un sistema di encoder ottici. L’encoder ottico è un
dispositivo elettro-optomeccanico che converte la posizione o la velocità angolare del
suo asse rotante in segnali elettrici digitali. Per il robot linefollower, tali encoder,
di tipo incrementale, sono stati realizzati artigianalmente in laboratorio utilizzando
componenti di vecchi mouse.
Figura 1.9: Il prototipo di encoder realizzato.
Dopo aver disassemblato i mouse, sono stati prelevati dalla scheda elettronica i diodi
LED, che emettono luce infrarossa, ed i fotodiodi associati, cercando di mantenere
la struttura fisica e le mutue distanze per preservare la precisione dell’insieme. Tra
le due coppie di emettitori e ricevitori sono state inserite due rotelline, calettate
sull’asse di rotazione dei motori, con 6 fori disposti lungo l’arco della circonferenza
ed equidistanziati; in tal modo, l’encoder presenta una risoluzione di 6 impulsi per
ogni giro. Data la ridotta dimensione degli encoder, è stato possibile montare il tutto
nello spazio tra le ruote e la struttura del motoriduttore. Al fine di ottenere un
17
Cap. 1 Robot LineFollower
§1.8 Encoder Ottici
segnale ben definito in uscita agli encoder, è stato utilizzato l’integrato M74HC142 .
con funzione di Trigger di Schmitt. Il Trigger di Schmitt è un particolare tipo di
circuito squadratore a soglia, ovvero un circuito che consente di trasformare un segnale
analogico in ingresso variabile con continuità in un segnale in uscita che varia soltanto
tra due valori fissati di tensione, a seconda che l’ingresso superi una soglia di tensione
predefinita o sia inferiore ad una seconda soglia di tensione (più bassa) anch’essa
predefinita (funzionamento con isteresi).
Figura 1.10: Funzione del Trigger di Schmitt
La commutazione dell’uscita dovrebbe avvenire in un tempo idealmente nullo; in
pratica, è sufficiente che il tempo di commutazione sia molto minore del periodo caratteristico di variazione del segnale in ingresso. Il funzionamento degli encoder è
stato verificato attraverso analisi dei segnali all’oscilloscopio; in tal modo è stata verificata la generazione di un’onda quadra ben definita con periodo variabile in funzione
della velocità di rotazione dell’asse dei motori. La misura dello spostamento è stata
quindi realizzata inviando i due segnali in uscita dal Trigger di Schmitt a due ingressi
analogici dell’Arduino.
Nonostante la buona definizione dell’onda quadra cosı̀ ottenuta, non è stato possibile
ottenere anche l’informazione del verso di rotazione dei motori; infatti, per ottenere il
necessario sfasamento fra i segnali d’uscita utilizzabile per la determinazione del verso
di rotazione sarebbe stata necessaria una maggiore precisione nel posizionamento e
2
Per il datasheet si veda [3].
18
§1.9 Arduino Mega
Cap. 1 Robot LineFollower
Figura 1.11: Segnali in uscita dal fotodiodo (sotto) e in uscita dal Trigger di Schmitt
(sopra).
nell’allineamento mutuo degli encoder; la realizzazione artigianale del sistema, che ha
avuto soprattutto un valore didattico, non ha consentito di raggiungere anche questo
obiettivo.
1.9
Arduino Mega
Figura 1.12: Arduino Mega.
La scheda di prototipazione Arduino Mega3 utilizza il microcontrollore della Atmel Atmega1280. Essa dispone di 54 porte digitali di ingresso/uscita (14 delle quali possono
essere usate in modalità PWM), 16 ingressi analogici e di tutti i componenti elettronici
3
Per il datasheet si veda [4].
19
Cap. 1 Robot LineFollower
§1.9 Arduino Mega
che servono per la gestione del microcontrollore. In effetti, la scheda Arduino Mega è
risultata, al termine della sperimentazione, sovradimensionata per l’applicazione realizzata, ma in fase di progettazione si è seguito il principio di poter disporre di risorse
sovrabbondanti in modo da poter introdurre al momento anche soluzioni inizialmente
non previste. A conclusione, le risorse utilizzate sono state: 4 uscite PWM per pilotare i motori attraverso il ponte H, 2 ingressi analogici per acquisire i dati forniti
dai sensori QTI, 2 ingressi analogici per gli encoder di spostamento; l’alimentazione
è stata regolata a 5V anche per consentire l’alimentazione dei circuiti logici dei sensori. La programmazione della memoria Flash di 256KB del microprocessore avviene
tramite una connessione USB al Personal Computer. È presente anche una memoria
EEPROM di 4KB che può essere utilizzata per salvare i dati di funzionamento del
sistema, vista la natura della memoria che conserva l’informazione anche in assenza di
alimentazione. Nel nostro caso, la memoria EEPROM è stata utilizzata per memorizzare il numero totale degli impulsi forniti dagli encoder durante il percorso effettuato
dal robot. È importante tenere presente che il produttore assicura una vita media
della memoria EEPROM di 100.000 cicli di scrittura, mentre per la memoria Flash di
10.000 cicli di scrittura.
20
Cap. 1 Robot LineFollower
1.10
§1.10 Alimentazione
Alimentazione
Per l’alimentazione dei motori è stato utilizzato un pacco batterie composto da quattro batterie ricaricabili formato AA al NiMh, ognuna da 1.3V ed in grado di erogare
2.600 mAh, collegate in serie per una tensione totale di 5.2V. Per l’alimentazione
della scheda Arduino Mega e dei circuiti logici aggiuntivi è stata utilizzata una batteria ricaricabile 9V al NiMh da 150 mAh. Infine, l’alimentazione dei sensori è stata
prelevata direttamente dal pin 5V della scheda Arduino.
1.11
Implementazione della legge di controllo
Grazie alla soluzione costruttiva adottata, ovvero quella di utilizzare due sensori di
linea posti esternamente al bordo della linea da seguire, il controllo implementato è
semplicemente un controllo di tipo proporzionale: quando uno dei due sensori “vede”
il bordo della linea, il microcontrollore pilota i motori per una rotazione in asse che
permetta di mantenere la linea all’interno dei binari virtuali individuati dai sensori. Il
funzionamento del robot si è dimostrato robusto anche per percorsi tortuosi realizzati
con linea singola non intersecante.
Per maggiore dettaglio, in seguito è riportato il codice utilizzato. In esso si evidenzia
come il controllo sia realizzato mediante la scelta di un’opportuna soglia rispetto alla
quale confrontare le uscite dei sensori; al superamento di tale soglia il controllo attiva
le routine di pilotaggio dei motori in modo da indurre la variazione di moto necessaria
a mantenere il robot al centro della linea da seguire.
21
Cap. 1 Robot LineFollower
§1.11 Implementazione della legge di controllo
Figura 1.13: Il LineFollower nel percorso di test
22
Cap. 1 Robot LineFollower
§1.11 Implementazione della legge di controllo
Listing 1.1: Programma di controllo Linefollower
1
#define
#define
#define
#define
#define
#define
#define
#define
en1 41 //ENABLE1 ponte H
en2 37 //ENABLE2 ponte H
in1 12 //ingresso 1 ponte H
in2 13 //ingresso 2 ponte H
in3 8 //ingresso 3 ponte H
in4 10 //ingresso 4 ponte H
sensorPin1 6
sensorPin2 3
int sensorValue1 = 0;
int sensorValue2 = 0;
int i = 255;
void setup() {
Serial.begin(9600);
pinMode(en1, OUTPUT);
pinMode(en2, OUTPUT);
digitalWrite(en1, HIGH);
digitalWrite(en2, HIGH);
}
void loop() {
//legge il valore dal sensore 1
sensorValue1 = analogRead(sensorPin1);
//legge il valore dal sensore 2
sensorValue2 = analogRead(sensorPin2);
do{
sensorValue1 = analogRead(sensorPin1);
sensorValue2 = analogRead(sensorPin2);
analogWrite(in1,255);
analogWrite(in2,0);
analogWrite(in3,255);
analogWrite(in4,0);
} while(sensorValue1 < 400 && sensorValue2 < 400);
23
Cap. 1 Robot LineFollower
§1.11 Implementazione della legge di controllo
do {
sensorValue1 = analogRead(sensorPin1);
sensorValue2 = analogRead(sensorPin2);
i−−;
analogWrite(in1,0);
analogWrite(in2,i);
analogWrite(in3,i);
analogWrite(in4,0);
} while (sensorValue1 > 400 && sensorValue2 < 400 || i == 0);
do {
sensorValue1 = analogRead(sensorPin1);
sensorValue2 = analogRead(sensorPin2);
i−−;
analogWrite(in1,i);
analogWrite(in2,0);
analogWrite(in3,0);
analogWrite(in4,i);
} while (sensorValue1 < 400 && sensorValue2 > 400 || i == 0);
}
24
Capitolo 2
Robot micromouse
In questo capitolo verrà descritta la realizzazione di un robot rover
dettagliando i componenti utilizzati e l’architettura complessiva del
prototipo.
2.1
Robot Rover
Figura 2.1: Concept
L’idea alla base dello sviluppo di questo tipo di robot è quella di realizzare un sistema
semovente in grado di evitare gli ostacoli sulla base dei segnali provenienti da opportuni sensori di prossimità. Da tale obiettivo principale possono poi derivare obiettivi
25
§2.1 Robot Rover
Cap. 2 Robot micromouse
più particolari, quali, ad esempio, quello di individuare l’uscita in un labirinto (micromouse competition [5]). Per il robot rover realizzato, è stata utilizzata una versione
ridotta della scheda microprocessore Arduino, la Arduino Nano, le cui caratteristiche
e modalità di utilizzo sono illustrate nello specifico paragrafo. Per la movimentazione
del robot, sono stati utilizzati piccoli motori passo-passo di tipo amatoriale. È stata
anche sperimentata una modalità di comunicazione online fra il robot ed il Personal
Computer che fa da supervisore attraverso moduli TX/RX wireless che utilizzano il
protocollo XBee.
Figura 2.2: Il robot realizzato.
26
Cap. 2 Robot micromouse
2.2
§2.2 Descrizione struttura
Descrizione struttura
Il robot è costituito da una base portante in lega metallica di forma rettangolare, di
lati 6.5 cm x 9 cm, alla quale sono intelaiati i motori. Al di sopra della base sono stati
creati, con l’ausilio di distanziali metallici, tre livelli cosı̀ caratterizzati:
Primo livello: vano batteria, di altezza 2.5 cm;
Secondo livello: scheda sensori, di altezza 3 cm;
Terzo livello: scheda di potenza, controllo e comunicazione.
Il robot ha due ruote motrici montate direttamente sugli alberi dei motori, con diametro e spessore rispettivamente di 5.7 cm e di 1.5 cm. Gli ulteriori punti di appoggio
necessari per l’equilibrio del robot sono forniti da tre Ball Caster con sfere in plastica,
due di diametro 9.4mm posizionate frontalmente e una di diametro 12.7mm posta sul
retro. La forma geometrica del robot completamente assemblato è di un parallelepipedo rettangolo di dimensioni 11cm x 11cm x 13cm. Il peso del robot, senza pacco
batterie, è di 600 grammi.
2.3
Motori passo-passo
Per movimentazione del robot sono stati scelti due motori passo-passo unipolari gemelli. I motori passo-passo (o stepper motor) sono particolari motori nei quali il moto
è ottenuto mediante attrazione magnetica generata da una sequenza di solenoidi (fasi)
posti lungo la corona circolare del motore (statore) rispetto ad un rotore di materiale
magnetico sagomato con denti salienti. Alimentando il primo solenoide della serie, il
dente più vicino del rotore subisce un’attrazione magnetica e si allinea al solenoide,
27
Cap. 2 Robot micromouse
§2.3 Motori passo-passo
Figura 2.3: Motore passo-passo unipolare.
eseguendo un passo; diseccitando il solenoide ed eccitando quello successivo si produce
una nuova attrazione sul dente del rotore, che si sposta ulteriormente allineandosi al
secondo solenoide ed eseguendo un altro passo; ripetendo la sequenza di eccitazione in
maniera impulsiva con più solenoidi e con riferimento a più denti del rotore, il motore
ruota uniformemente con una velocità corrispondente alla frequenza degli impulsi di
eccitazione dei solenoidi. Le sequenze di impulsi di eccitazione sono prodotte generalmente mediante circuiti logici a microprocessore. Per la realizzazione del robot
rover sono stati utilizzati due motori PM42L-048-171: essi hanno 4 fasi, una risoluzione di 7.5° per passo, richiedono una tensione di alimentazione di 12V ed hanno un
assorbimento medio di corrente pari a 0.28A.
2.3.1
Dimensionamento motori
Il dimensionamento dei motori passo-passo è un punto fondamentale nella progettazione dei robot che li utilizzano. Tale dimensionamento è effettuato sulla base della
coppia motrice richiesta, che deve essere sempre soddisfatta dalla coppia erogabile
dal motore in ogni condizione di moto. Come precedentemente accennato, nel funzionamento dei motori passo-passo la regolazione della velocità avviene variando la
1
Per il datasheet si veda [6].
28
Cap. 2 Robot micromouse
§2.3 Motori passo-passo
frequenza degli impulsi di pilotaggio del motore stesso. Nel momento in cui improvvisamente, per motivi connessi al moto richiesto al sistema, aumenta la frequenza degli
impulsi di pilotaggio del motore, lo stesso tenta di adeguarsi istantaneamente alla
nuova velocità: se la coppia disponibile risulta sufficiente, vi è una brusca accelerazione, praticamente istantanea; se la coppia disponibile non basta a vincere la coppia di
carico, l’inerzia e gli attriti, il motore non riesce ad adeguarsi al nuovo regime di moto
e, semplicemente, si ferma; senza un intervento esterno, il motore non è più in grado di
ripartire: in tal caso, si dice che il motore “ha perso il passo”. Una simile reazione si ha
anche nel caso di applicazione improvvisa di una maggiore coppia resistente al motore
già in moto. Inoltre, fissato il regime di funzionamento (ovvero fissata la frequenza
degli impulsi di pilotaggio), un motore passo-passo non può rallentare: se la coppia
resistente è relativamente piccola, la velocità non cambia assolutamente; se la coppia
è troppo grande, il motore perde il passo e si ferma. Tale comportamento deriva dal
fatto che la velocità del motore è univocamente e rigidamente definita dalla logica
di funzionamento (sequenza e frequenza degli impulsi di pilotaggio) e che la corrente
assorbita è fissa ed è praticamente indipendente dalla coppia meccanica resistente.
Entrando nel dettaglio, la caratteristica di funzionamento di un motore passo-passo
è descritta mediante due curve, definite come curva di pull-in e curva di pull-out. La
curva di pull-in rappresenta graficamente i valori di coppia di carico massime che,
applicate al motore, permettono la partenza ad una data velocità, mentre la curva di
pull-out indica i valori di coppia di carico massime che, con il motore in moto ad una
data velocità, possono essere applicate senza far perdere il passo al motore. Da notare
che entrambe le curve indicano che il motore passo-passo possiede una caratteristica
inversa della coppia rispetto alla velocità: più alta è la velocità (e quindi la frequenza
degli impulsi di pilotaggio), più bassa è la coppia prodotta dal motore (similmente ai
29
Cap. 2 Robot micromouse
§2.3 Motori passo-passo
motori in corrente continua).
Figura 2.4: Curve di pull-in e pull-out.
Se il motore si trova a funzionare al di sotto della curva di pull-in (zona verde), esso
può subire cambi di velocità e inversioni di marcia senza precauzioni particolari: è
sufficiente modificare la frequenza e/o la sequenza con cui sono applicate le correnti
di eccitazione alle fasi. La zona fra le due curve (zona rossa) è chiamata zona di
slew-range: il motore può funzionare in tale zona solo a condizione di evitare bruschi
cambiamenti di velocità; il cambiamento di frequenza deve quindi essere continuo
e tanto più lento quanto più ci si allontana dalla curva di pull-in verso la curva di
pull-out. La curva di pull-out indica quale è la massima coppia resistente che può
essere applicata al motore in rotazione ad una data velocità costante senza causarne
il blocco. In pratica rappresenta le massime prestazioni del motore. Se il motore
si trova a dover funzionare al di sopra della curva di pull-out, la coppia prodotta è
insufficiente a garantire la rotazione ed il motore perde il passo. La caratteristica
coppia/velocità dipende non solo dal motore ma anche dal complesso dei dispositivi
meccanici ad esso collegati (un aumento del momento di inerzia del carico causa un
30
Cap. 2 Robot micromouse
§2.3 Motori passo-passo
restringimento dell’area di pull-in) e dal tipo di pilotaggio (una migliore gestione delle
correnti fa spostare in avanti entrambe le curve, soprattutto alle velocità più elevate).
In applicazioni critiche è possibile utilizzare un sensore di velocità per riconoscere
l’eventuale blocco del motore; tuttavia, spesso si preferisce ignorare il problema ed,
eventualmente, utilizzare motori con coppia sovrabbondante.
Nel robot realizzato, i motori passo-passo sono pilotati direttamente dalle uscite digitali dell’Arduino attraverso l’integrato ULN2004A2 . Tale integrato è costituito da
una serie di sette paia di transistor Darlington: grazie alla loro elevata potenza in
uscita ed alla piccola potenza richiesta in ingresso, ogni Darlington può pilotare direttamente una fase dei motori passo-passo a partire dalla specifica uscita digitale
dell’Arduino. Dal momento che sono necessarie otto coppie di Darlington (2 motori
e 4 fasi per ogni motore), sono stati utilizzati due integrati ULN2004A. Tali integrati
sono stati montati su zoccolo per una eventuale facile sostituzione e posti sulla scheda
di potenza, controllo e comunicazione (descritta più dettagliatamente nel seguito).
2
Per il datasheet si veda [8].
31
Cap. 2 Robot micromouse
2.3.2
§2.3 Motori passo-passo
Micro-relè e libreria stepper.h
Ovviamente, un robot rover deve poter gestire contemporaneamente ed indipendentemente i due motori e quindi far muovere le ruote in funzione del percorso individuato
mediante l’elaborazione dei segnali provenienti dai sensori di prossimità. Tuttavia,
la scheda Arduino Nano è funzionalmente costruita per eseguire sequenzialmente una
sola serie di operazioni preprogrammate alla volta (nello specifico, le funzioni di pilotaggio dei motori passo-passo tramite la libreria stepper.h disponibile proprio a tale
scopo) e quindi non può eseguire due funzioni di controllo contemporaneamente ed
indipendentemente per i due motori utilizzati. Al fine di superare tale limitazione,
è stata individuata una soluzione funzionale implementata mediante un opportuno
hardware aggiuntivo costituito da due microrelè pilotati anch’essi dalla scheda Arduino. Più dettagliatamente, si è osservato che, una volta fissato il regime di funzionamento dei motori passo-passo in termini di velocità di rotazione, per far eseguire al
robot tutti i movimenti necessari (avanti, indietro, rotazione sul proprio asse a destra, rotazione sul proprio asse a sinistra) è sufficiente pilotare unicamente il verso di
rotazione dei motori: se i motori ruotano in verso opposto, il robot procede avanti o
indietro; se ruotano nello stesso verso, il robot ruota sul proprio asse nella direzione
del motore che avanza. Inoltre, dato che il verso di rotazione di un motore passo-passo
unipolare è stabilito unicamente dalla sequenza di alimentazione delle fasi, se viene
utilizzata una tecnica di pilotaggio ad una fase per volta (wave mode), basta modificare opportunamente tale sequenza per invertire il senso di rotazione del motore.
Per comprendere meglio quest’ultima affermazione, nella figura seguente è riportata
la sequenza di alimentazione delle fasi secondo la tecnica wave mode; tale tecnica
permette la rotazione del motore per passi interi alimentando una sola fase per volta.
Come è possibile dedurre dalla figura 2.5, per ottenere la rotazione destrorsa del
32
Cap. 2 Robot micromouse
§2.3 Motori passo-passo
Figura 2.5: Pilotaggio motore passo-passo
motore è necessario alimentare in sequenza le fasi A-C-B-D, mentre per ottenere la
rotazione sinistrorsa è necessario alimentare in sequenza le fasi A-D-B-C; per cui, per
invertire il senso di rotazione del motore basta scambiare fra loro, nella sequenza di
pilotaggio, le alimentazioni delle fasi C e D. Sulla base di tali considerazioni, si è
trovata la seguente soluzione:
il controllore Arduino, attraverso quattro uscite digitali, genera le quattro se-
quenze di impulsi di pilotaggio delle fasi contemporaneamente per entrambi i
motori, mediante l’utilizzo della libreria stepper.h appositamente predisposta;
33
Cap. 2 Robot micromouse
§2.3 Motori passo-passo
tali impulsi sono collegati in parallelo agli ingressi dei due gruppi Darlington
per l’alimentazione delle fasi dei due motori;
prima di giungere alle fasi, le sequenze di impulsi di pilotaggio, già amplifica-
ti, sono inviate a due microrelè, uno per motore, i cui contatti permettono di
scambiare le alimentazioni delle fasi C e D;
l’eccitazione di ognuno dei microrelè è pilotata da due uscite logiche della scheda
Arduino, opportunamente programmate in funzione di una semplice tabella di
attivazione che dipende dal movimento in avanti o in indietro che è richiesto
al singolo motore; in tal modo, alle fasi di ogni motore giunge la sequenza di
impulsi di pilotaggio corrispondente al movimento richiesto.
Al fine di dettagliare ulteriormente la procedura messa a punto, nella pagina seguente
è riportato l’estratto di codice implementato per la funzione descritta.
34
Cap. 2 Robot micromouse
§2.3 Motori passo-passo
Listing 2.1: Programma test microrelè e stepper.h
1
#include <Stepper.h>
5
#define STEPS 48
#define RELE1 4
#define RELE2 3
7
Stepper stepper(STEPS, 12, 11, 10, 9);
3
9
11
13
void setup()
{
pinMode(RELE1, OUTPUT);
pinMode(RELE2, OUTPUT);
stepper.setSpeed(40);
}
15
17
19
21
void loop()
{
//FAI 100 PASSI AVANTI
digitalWrite(RELE1, HIGH);
digitalWrite(RELE2, HIGH);
delay(10);
stepper.step(100);
23
//FAI 100 PASSI A SINISTRA
digitalWrite(RELE1, LOW);
digitalWrite(RELE2, HIGH);
delay(10);
stepper.step(100);
25
27
29
//FAI 100 PASSI A DESTRA
digitalWrite(RELE1, HIGH);
digitalWrite(RELE2, LOW);
delay(10);
stepper.step(100);
31
33
35
//FAI 100 PASSI INDIETRO
digitalWrite(RELE1, LOW);
digitalWrite(RELE2, LOW);
delay(10);
stepper.step(100);
37
39
41
}
35
Cap. 2 Robot micromouse
§2.3 Motori passo-passo
Come si può notare, l’attivazione della funzione di pilotaggio dei motori passo-passo è
riferita ad un solo motore, in quanto le sequenze di impulsi sono utilizzate contemporaneamente ed in parallelo per entrambi i motori. Il codice mostra come, dopo aver
predisposto le definizioni iniziali ed il settaggio della velocità dei motori, viene ciclicamente ripetuta sempre la stessa istruzione di libreria relativa al pilotaggio dei motori
(istruzione stepper.step(100), esegui 100 passi), ma, grazie alla diverse possibili attivazioni dei microrelè, possono essere ottenuti in maniera differenziata tutti e quattro
i movimenti possibili del robot, con elevata precisione dal momento che i comandi di
pilotaggio sono sempre identici per entrambi i motori. Inoltre, per sopperire ad eventuali piccoli ritardi nell’attivazione dei microrelè (comunque del tutto trascurabili), è
stato inserito un lieve ritardo (10ms) fra l’attivazione dei microrelè e la generazione
degli impulsi di pilotaggio dei motori; tale ritardo non ha alcuna conseguenza sul
movimento sincrono dei motori. Per aumentare la risoluzione del moto, può essere
previsto un numero minore di passi da eseguire per ogni ciclo di funzionamento. Da
notare che tale soluzione è del tutto generale e può essere utilizzata in qualsiasi applicazione che richieda direzioni di moto differenti ma perfettamente sincrone fra due o
più motori passo-passo pilotati in wave mode. In Appendice A è riportato lo schema
elettrico relativo alla soluzione ideata.
36
§2.4 Scheda sensori
Cap. 2 Robot micromouse
2.4
Scheda sensori
Figura 2.6: Scheda sensori
Per il rilevamento degli ostacoli sono stati utilizzati i sensori ad infrarosso Sharp
2D120X3, in grado di rilevare ostacoli ad una distanza da 4 a 30 cm. Essi montano un
led emettitore all’infrarosso ed un ricevitore che, in base alla quantità di luce riflessa
che riceve, restituisce in uscita una tensione proporzionale alla distanza dall’ostacolo.
Sono stati montati sul robot tre sensori: uno posto parallelamente al lato frontale e
due posti parallelamente ai due lati sinistro e destro. Tale scelta permette alla logica
di controllo del robot di ottenere in maniera efficiente le indicazioni per individuare
il percorso migliore da seguire. L’uscita di ogni sensore è inviata ad un ingresso
analogico della scheda Arduino Nano; il microprocessore esegue la stima del valore
restituendo un valore numerico intero tanto più grande quanto più piccola è la distanza
dall’ostacolo. Il tempo di risposta dei sensori è di 38 ± 10ms. L’alimentazione che
deve essere fornita ai sensori deve essere compresa tra i 4.5V e i 5.5V; per tale motivo
3
Per il datasheet si veda [9].
37
Cap. 2 Robot micromouse
§2.5 Scheda di potenza, controllo e comunicazione
è stato utilizzato un regolatore di tensione integrato, modello L78S05CV4 , che, a
partire dalla tensione in ingresso di 7V generata dalla batteria, fornisce una tensione
regolata di 5V. Il consumo medio di corrente per ogni sensore è di circa 33mA, per
un totale di circa 100mA per tutto il gruppo sensori. Per motivi di ottimizzazione
dell’assemblaggio, i sensori sono stati montati su una scheda millefori posta al centro
del robot. È stato inoltre inserito un LED giallo che segnala l’attivazione dei sensori.
Complessivamente, il consumo totale della scheda dei sensori è di circa 110mA.
2.5
Scheda di potenza, controllo e comunicazione
Figura 2.7: Scheda di potenza, controllo e comunicazione.
La scheda di potenza, controllo e comunicazione accoglie il cuore funzionale del robot.
Su di essa è montata la scheda Arduino Nano, il modulo di comunicazione XBee, il
regolatore di tensione L78S05CV per l’alimentazione dei sensori, i microrelè per il
controllo del moto dei motori, le morsettiere per il cablaggio, l’interruttore di ali4
Per il datasheet si veda [10].
38
§2.6 Arduino Nano
Cap. 2 Robot micromouse
mentazione ed un led rosso di controllo. La scheda Arduino ed il modulo XBee sono
montati su zoccoli dedicati, per una veloce e pratica gestione e manutenzione.
2.6
Arduino Nano
Figura 2.8: Arduino Nano
L’Arduino Nano5 è una piccola e completa scheda di prototipazione elettronica basata
sul microcontrollore Atmega328. Le sue misure ridotte ne permettono un uso molto
pratico per progetti di mini-robotica. Essa è dotata di 14 porte di ingresso/uscita digitali, 6 delle quali PWM, e 8 porte di ingresso analogiche. Gli ingressi analogici hanno
un fondo scala di 5V, mentre le porte digitali hanno ingressi/uscite logiche a 0V/5V
e da esse è possibile assorbire corrente fino a 40mA ciascuna. Tale limite di corrente
erogabile, del tutto ovvio data la natura stessa del sistema, va tenuto adeguatamente
in considerazione, al fine di non sovraccaricare le uscite e quindi di non danneggiare
il microcontrollore. Per la realizzazione del robot rover sono state utilizzate tre porte
5
Per il datasheet si veda [11].
39
§2.7 Modulo wireless: XBee
Cap. 2 Robot micromouse
analogiche in ingresso per la lettura dei segnali provenienti dai sensori e sei porte
digitali in uscita, quattro delle quali per la generazione delle sequenze di pilotaggio
dei motori e due per il controllo dei microrelè. Per la comunicazione wireless dei dati,
le porte RX e TX di trasmissione e ricezione seriale sono state collegate alle rispettive
porte del modulo XBee. La programmazione dell’Arduino avviene tramite collegamento remoto via Mini-B USB ad un Personal Computer. L’alimentazione è fornita
direttamente dalla batteria di alimentazione utilizzata (vedi dettaglio nel seguito), in
quanto l’Arduino può lavorare con tensioni che vanno dai 7V ai 12V.
2.7
Modulo wireless: XBee
Figura 2.9: Modulo XBee
Per la comunicazione wireless con il robot rover sono stati utilizzati due moduli XBee6 .
Il primo è stato montato direttamente sul robot e collegato all’Arduino attraverso la
scheda dedicata XBee Explorer; tale scheda adatta sia il segnale di TX/RX proveniente dall’Arduino sia la tensione di alimentazione, regolando entrambi a 3.3V partendo
dal valore di 5V fornito dall’Arduino. Il secondo modulo è collegato direttamente alla
porta USB del Personal Computer al quale vanno inviati i dati trasmessi, funzionan6
Per il datasheet si veda [12].
40
§2.8 Alimentazione
Cap. 2 Robot micromouse
do in effetti come un modem wireless. I moduli XBee hanno un protocollo wireless
compatibile con lo standard ZigBee/IEEE 802.15.4 che soddisfa i requisiti necessari per realizzare un sistema di trasmissione bidirezionale a basso costo ed a basso
consumo, orientato soprattutto per l’impiego con sensori. I moduli sono semplici da
utilizzare, di limitato ingombro, richiedono pochissima energia e costituiscono una
soluzione efficace ed affidabile per la trasmissione di dati a breve raggio. Nell’applicazione realizzata, i moduli XBee si sono rivelati utili sia in fase di taratura dei sensori,
consentendo una lettura on-line sullo schermo del Personal Computer dei dati rilevati,
sia per l’implementazione della logica di controllo attraverso lo scambio bidirezionale
dei dati necessari al dimensionamento dei parametri di funzionamento.
2.8
Alimentazione
Figura 2.10: Batteria al NiMh
L’alimentazione dell’intero sistema è fornita da una batteria ricaricabile al NiMh composta da 5 celle. La batteria è in grado di erogare 1.100mA a 7V; le sue misure sono
85mm x 26mm x 15mm ed il peso è di 110 grammi.
41
Capitolo 3
Filtro di Kalman Esteso
3.1
Introduzione al filtro
Nell’analisi dei sistemi dinamici trova notevole rilievo il Filtro di Kalman. Introdotto
dallo studioso da cui prende il nome negli anni ’60, risulta essere un efficiente algoritmo ricorsivo per la stima dello stato di un sistema dinamico a partire da un insieme di
misure affette da rumore bianco (rumore a media nulla e distribuzione di tipo gaussiano). In tali condizioni, si dimostra che il Filtro di Kalman risulta essere lo stimatore
ottimo, ovvero quello in grado di fornire la migliore stima dei parametri del sistema
osservato in termini di minimizzazione dell’errore quadratico medio sia dello stato
stimato che della matrice di covarianza associata al rumore. Il Filtro di Kalman è
applicabile unicamente a sistemi lineari, per i quali sono dimostrate e verificate le sue
caratteristiche ottime. Nel caso di sistemi non lineari, il Filtro di Kalman non risulta
applicabile in maniera canonica; comunque, per non perdere le positive caratteristiche
possedute dall’algoritmo, è stato sviluppato il Filtro di Kalman Esteso. Tale versione
del filtro si applica a sistemi non lineari ma soggetti comunque a rumore gaussiano a
42
Cap. 3 Filtro di Kalman Esteso
§3.1 Introduzione al filtro
media nulla, per i quali è applicabile una linearizzazione del sistema intorno alla stima corrente (linearizzazione a tratti). In tali condizioni, la stima ottenuta dal Filtro
di Kalman Esteso è ancora valida, ma può perdere le caratteristiche di ottimalità e
di convergenza. Nel campo della robotica, il Filtro di Kalman Esteso è largamente
impiegato nei problemi di controllo della posizione e del moto dei robot nello spazio
(localizzazione o movimento asservito) sulla base delle misure ottenute dai sensori di
posizione e di movimento. Nell’applicazione al robot rover realizzata, il Filtro di Kalman, nella versione estesa, è stato utilizzato per implementare la legge di controllo per
la stima della correzione dinamica da applicare al moto del robot istante per istante.
Infatti, il robot, non essendo dotato di dispositivi per il controllo a catena chiusa e non
essendo perfettamente simmetrico nei componenti costruttivi (ruote, motori, massa
geometrica, ecc.), può essere soggetto a derive del moto reale rispetto al moto teorico.
Il filtro si è dimostrato utile per stimare la correzione da introdurre, mediante un’opportuna azione sull’algoritmo di controllo dei motori, al fine di ricondurre quanto più
possibile il moto reale a quello teorico. Per questo motivo, è stata introdotta una
opportuna modellizzazione degli errori di moto del robot ottenuta mediante misure
odometriche in condizioni di riferimento, mentre per la determinazione della deriva
effettiva del moto sono state utilizzate le misure acquisite dai sensori di prossimità di
cui il robot è dotato. Tali aspetti sono maggiormente dettagliati nel seguito.
43
§3.2 Modello cinematico
Cap. 3 Filtro di Kalman Esteso
3.2
Modello cinematico
Figura 3.1: Modello geometrico associato
Si consideri il sistema di coordinate (x,y) in figura 3.1. Il robot è localizzato nel
sistema bidimensionale con coordinate (xk ,yk ) ed orientamento ϑk . Si può definire la
posizione del robot rispetto a tale sistema utilizzando il vettore Xr di tre parametri:
Xr (k) = [x(k), y(k), ϑ(k)]′
(3.2.1)
Considerando il modello cinematico non-lineare discretizzato nel tempo, le relazioni
che governano il moto del robot sono le seguenti:
xk+1 = xk + ∆ρk · cos ϑk
(3.2.2)
yk+1 = yk + ∆ρk · sin ϑk
(3.2.3)
ϑk+1 = ϑk + ∆ϑk
(3.2.4)
44
§3.2 Modello cinematico
Cap. 3 Filtro di Kalman Esteso
dove lo spostamento ∆ρk e la rotazione ∆ϑk sono legati al moto delle ruote come
segue:
∆ρk =
∆Rk + ∆Lk
2
(3.2.5)
∆ϑk =
∆RK − ∆Lk
d
(3.2.6)
in cui d è la distanza tra i punti di contatto delle ruote col pavimento mentre ∆Rk
e ∆Lk sono gli spostamenti rispettivamente della ruota destra e della ruota sinistra,
definiti come:
∆Rk = NpassiR · l
(3.2.7)
∆Lk = NpassiL · l
(3.2.8)
essendo NpassiR e NpassiL rispettivamente il numero di passi imposti dall’algoritmo
di controllo ai motori passo-passo di destra e di sinistra, l la distanza teorica coperta
dalla ruota per un singolo passo del motore.
3.2.1
Determinazione sperimentale delle incertezze del moto
Al fine di controllare la posizione del robot, in primo luogo è necessario avere informazioni sul posizionamento reale del robot durante il moto e confrontarlo con quello
teorico. Come precedentemente sottolineato, il robot non è dotato di sistemi di misura odometrici per la determinazione della sua posizione effettiva. Tuttavia, sulla base
del modello cinematico illustrato in precedenza e mediante idonee valutazioni delle
incertezze ad esso connesse nel caso reale, è possibile pervenire ad una stima più fedele
45
§3.2 Modello cinematico
Cap. 3 Filtro di Kalman Esteso
della posizione effettiva del robot durante il suo movimento. A tal fine è stato seguito
un approccio sperimentale basato sull’analisi statistica degli scostamenti tra il moto
teorico ottenuto dal modello ed il moto reale rilevato in situazioni di riferimento. Per
tale analisi è stata considerata una distribuzione normale caratterizzata dai parametri
media µ e varianza σ 2 : N(µ; σ 2 ).
Sono state quindi effettuate tre serie di trenta esperimenti ciascuna per tre diversi
spostamenti del robot:
100 passi −→ corrispondenti a 34.8 cm teorici;
380 passi −→ corrispondenti a 142.3 cm teorici;
480 passi −→ corrispondenti a 179.7 cm teorici.
Per tutti gli spostamenti, il robot è stato programmato per eseguire un percorso
rettilineo. Il rilievo del moto effettivo del robot è stato ottenuto impiegando un piano
di lavoro orizzontale opportunamente ricoperto da carta millimetrata.
Figura 3.2: Misure
46
§3.2 Modello cinematico
Cap. 3 Filtro di Kalman Esteso
Per i=1,...,n serie di esperimenti e j=1,...,m numero di esperimenti per serie è possibile
definire, con riferimento al modello cinematico, gli errori rilevati rispettivamente sullo
spostamento e sull’orientamento come segue:
Er(i,j) =
nR(i,j) + nL(i,j)
2
(3.2.9)
Eϑ(i,j) =
nR(i,j) − nL(i,j)
d
(3.2.10)
dove nR(i,j) e nL(i,j) sono gli errori di moto delle singole ruote in tutti gli esperimenti.
Risolvendo il sistema delle due equazioni (3.2.9) (3.2.10) nelle incognite nR(i,j) e nL(i,j) ,
si ottiene:
Eϑ(i,j) · d
2
Eϑ(i,j) · d
= Er(i,j) −
2
nR(i,j) = Er(i,j) +
(3.2.11)
nL(i,j)
(3.2.12)
È possibile quindi calcolarne i valori medi:
µi,R
m
1 X
= ·
nR(i,j)
n j=1
i = 1, ..., n
(3.2.13)
µi,L
m
1 X
= ·
nL(i,j)
n j=1
i = 1, ..., n
(3.2.14)
e successivamente le varianze:
2
σi,R
m
1 X
= ·
(nR(i,j) − µi,R )2
n j=1
i = 1, ..., n
(3.2.15)
2
σi,L
m
1 X
= ·
(nL(i,j) − µi,L)2
n j=1
i = 1, ..., n
(3.2.16)
47
Cap. 3 Filtro di Kalman Esteso
§3.2 Modello cinematico
Al fine di poter generalizzare il comportamento del robot anche per spostamenti diversi da quelli considerati, è possibile ipotizzare una legge di variazione lineare delle
incertezze del moto. Procedendo quindi alla interpolazione lineare riferita ai valori
2
2
medi µi,R e µi,L ed alle varianze σi,R
e σi,L
dei risultati sperimentali, vengono determi-
nati i coefficienti angolari delle rette caratteristiche delle medie (am,R e am,L ) e delle
varianze (KR e KL ) in funzione del numero dei passi di moto.
Conseguentemente, le distribuzioni normali degli errori riferiti al moto delle ruote
possono essere descritte come segue:
nR ∼ N(am,R · ∆R, KR · ∆R)
(3.2.17)
nL ∼ N(am,L · ∆L, KL · ∆L)
(3.2.18)
Al fine di poter applicare correttamente il Filtro di Kalman Esteso, è necessario ricondurre le distribuzioni (3.2.17) e (3.2.18) a distribuzioni a media nulla, modificando
opportunamente il modello cinematico. La (3.2.5) e la (3.2.6) diventano quindi:
∆ρk =
(1 + am,R ) · ∆Rk + (1 + am,L ) · ∆Lk
2
(3.2.19)
∆ϑk =
(1 + am,R ) · ∆Rk − (1 + am,L ) · ∆Lk
d
(3.2.20)
le quali sostituite nelle (3.2.2),(3.2.3) e (3.2.4) forniscono:
(1 + am,R ) · ∆Rk + (1 + am,L) · ∆Lk
· cos ϑk
2
(1 + am,R ) · ∆Rk + (1 + am,L ) · ∆Lk
= yk +
· sin ϑk
2
xk+1 = xk +
(3.2.21)
yk+1
(3.2.22)
48
§3.3 Modello di misura
Cap. 3 Filtro di Kalman Esteso
ϑk+1 = ϑk +
(1 + am,R ) · ∆RK − (1 + am,L ) · ∆Lk
d
(3.2.23)
e di conseguenza le distribuzione normali (3.2.17) e (3.2.18) diventano:
nR ∼ N(0, KR · ∆R)
(3.2.24)
nL ∼ N(0, KL · ∆L)
(3.2.25)
ovvero distribuzioni normali a media nulla per le quali è possibile applicare il filtro
di Kalman.
3.3
Modello di misura
Come precedentemente accennato, le residue incertezze connesse con la non perfetta
rispondenza del modello cinematico corretto con il moto effettivo, possono essere
compensate mediante l’utilizzo di informazioni provenienti dai sensori e relative alla
posizione reale del robot. Quest’ultimo è dotato di tre sensori di prossimità disposti
frontalmente e lateralmente, attraverso i quali il robot riceve le informazioni sulla
distanza dalle pareti del percorso. Pertanto, il modello di misura può essere definito
come segue:
Y (k) = h(Xr,k) + ω(k)
(3.3.1)
dove ω(k) indica il rumore gaussiano a media nulla, con matrice di covarianza diagonale R(k), utilizzato per modellare gli errori di misura, mentre h(Xr,k ) indica la
relazione tra la posizione del robot e il valore letto dai sensori infrarosso:


h1 (Xr,k )
h(Xr,k) = h2 (Xr,k )
h3 (Xr,k )
(3.3.2)
49
§3.3 Modello di misura
Cap. 3 Filtro di Kalman Esteso
Figura 3.3: Modello geometrico associato alle misure
hi (Xr,k) =
q
(Si,x − xp )2 + (Si,y − yp )2
(3.3.3)
dove Si,x e Si,y sono le coordinate del sensore i-esimo nel piano x-y e xp e yp sono le
coordinate del punto rilevato dal sensore sulla parete.
In base a tali definizioni, il problema dell’individuazione della corretta posizione del
robot può essere posto come l’individuazione di una funzione χ che fornisca ricorsivamente la posizione esatta del robot integrando le equazioni del modello cinematico
con il modello della misura:
Xr (k + 1) = χ(Xr (k), Y (k + 1))
(3.3.4)
50
§3.4 Implementazione del filtro
Cap. 3 Filtro di Kalman Esteso
3.4
Implementazione del filtro
L’implementazione del filtro di Kalman Esteso è realizzata mediante l’iterazione delle
fasi di predizione e stima: nella fase di predizione viene individuato, sulla base dello
stato precedente, lo stato successivo del sistema e la relativa matrice di covarianza;
nella fase di stima, sulla base delle misure effettuate, si determina l’aggiornamento
dello stato, si calcola il guadagno del filtro e si aggiorna la matrice di covarianza.
Analiticamente l’algoritmo esegue questi passi:
si consideri la stima della posizione iniziale assunta dal robot X̂r,0 e la matrice
di covarianza sulla stima iniziale P0 ;
−
inizia la fase di predizione e viene calcolato lo stato X̂r,k
:
x̂−
r,k = x̂r,k−1 + ∆ρk−1 · cos ϑk−1
(3.4.1)
−
ŷr,k
= ŷr,k−1 + ∆ρk−1 · sin ϑk−1
(3.4.2)
ϑ̂−
r,k = ϑ̂k−1 + ∆ϑk−1
(3.4.3)
e la matrice di covarianza associata pari a:
′
′
Pk− = Fk−1 · Pk−1 · Fk−1
+ Wk−1 · Qk−1 · Wk−1
(3.4.4)
dove Fk-1 è la jacobiana della dinamica rispetto allo stato, Wk-1 è la jacobiana
della dinamica rispetto al disturbo e Qk-1 è la matrice di covarianza del rumore
sulla dinamica;
inizia la fase di stima e viene calcolata l’innovazione Innovation k = Y (k) −
−
h(X̂r,k
) con riferimento a misure affidabili effettuate dai sensori infrarosso; per
misure affidabili si intendono misure coerenti con l’ambiente noto;
51
§3.5 Simulazioni
Cap. 3 Filtro di Kalman Esteso
viene calcolato il guadagno di Kalman:
Kk = Pk− · Hk′ · (Hk · Pk− · Hk′ + R)
(3.4.5)
dove Hk è la jacobiana delle misure mentre R è la matrice di covarianza dei
rumori sulle misure;
vengono calcolati lo stato e la matrice di covarianza associata:
X̂r,k = X̂k− + Kk · (Innovation k )
(3.4.6)
Pk = (I − Kk · Hk ) · Pk−
(3.4.7)
viene posto k = k+1 e viene riproposto l’algoritmo con una nuova fase di
predizione.
3.5
Simulazioni
Al fine di verificare le prestazioni del Filtro di Kalman Esteso nella correzione dinamica
del moto del robot rover realizzato, sono stati condotti alcuni test in ambiente di
simulazione Matlab su un percorso predefinito.
È stato dapprima simulato il moto del robot rover in assenza di correzione dinamica,
sulla base del modello cinematico del moto individuato nel paragrafo 3.2. La fig. 3.4
mostra a confronto il moto teorico del robot (linea blu) con il moto simulato mediante
il modello cinematico (linea rossa). Da essa è possibile trarre due considerazioni:
Il modello cinematico individuato sulla base dell’analisi statistica dell’errore di
moto risulta essere robusto anche per percorsi molto diversi da quelli utilizzati
per il calcolo dei parametri di moto;
52
§3.5 Simulazioni
Cap. 3 Filtro di Kalman Esteso
Percorso Test
70
60
50
40
30
20
10
0
−10
0
20
40
60
80
100
Figura 3.4: Simulazione percorso # 1 per la verifica del modello cinematico
L’errore di posizione risultante dal modello cinematico del moto è compatibile
con la struttura del robot e con le verifiche sperimentali realizzate in ambiente
reale sul piano di lavoro millimetrato.
I successivi test hanno riguardato la verifica delle prestazioni del filtro nella correzione
dinamica del moto; a tal fine, sono state simulate le misure di posizione fornite dai
sensori sovrapponendo al valore esatto un rumore casuale a media nulla. La figura 3.5
mostra a confronto il moto teorico del robot (linea blu) con quello ottenuto mediante
la correzione dinamica operata dal Filtro di Kalman Esteso (linea rossa). Da essa
si evince come il moto corretto sia praticamente coincidente con il moto teorico, in
quanto il filtro determina ad ogni passo la correzione da imporre al moto al passo
successivo per riportare il robot sul percorso teorico.
53
§3.5 Simulazioni
Cap. 3 Filtro di Kalman Esteso
Percorso Test
70
60
50
40
30
20
10
0
−10
0
20
40
60
80
100
Figura 3.5: Simulazione percorso # 1 con Filtro di Kalman Esteso
In Appendice B è riportato il listato del codice Matlab utilizzato per le simulazioni.
54
Capitolo 4
Conclusioni e sviluppi futuri
La tesi illustra le applicazioni delle schede di prototipazione della famiglia Arduino per
la realizzazione di robot linefollower e rover. Sono state illustrate dettagliatamente le
soluzioni meccaniche, elettroniche e di controllo che hanno consentito la realizzazione
dei prototipi delle due tipologie di robot. Al riguardo, il lavoro di tesi ha evidenziato
le ottime prestazioni possedute dalle versioni di Arduino utilizzate, unitamente alle
notevoli caratteristiche di flessibilità e di semplicità d’uso e programmazione.
Nello specifico, per la realizzazione del robot LineFollower è stata utilizzata una soluzione basata sull’impiego della scheda Arduino Mega e di motori in corrente continua,
la cui alimentazione è regolata dal microcontrollore tramite un segnale PWM opportunamente modulato. Il robot è stato dotato di sensori infrarosso per il rilievo della
linea guida e di encoder incrementali costruiti artigianalmente per la misura del percorso effettuato. Il preciso posizionamento dei sensori di linea ai lati della stessa ha
consentito di sviluppare ed implementare una legge di controllo di tipo proporzionale
semplice e molto efficiente, come dimostrato dalle prove effettuate anche su percorsi
tortuosi.
Per quanto riguarda il robot rover, è stata utilizzata la scheda Arduino Nano, caratterizzata da un ingombro estremamente ridotto ma con prestazioni di ottimo livello.
55
Cap. 4 Conclusioni e sviluppi futuri
Per la movimentazione del robot sono stati impiegati motori passo-passo, per i quali
il microcontrollore fornisce la necessaria sequenza di pilotaggio. È stata inoltre implementata una interessante soluzione hardware per la gestione del moto del robot
sulla base di una sola sequenza di pilotaggio dei motori: ciò ha consentito di superare
la limitazione funzionale del microcontrollore per la gestione contemporanea dei due
motori. Per il controllo del moto, il robot è stato dotato di sensori di prossimità
all’infrarosso, sia per individuare i limiti del tracciato da percorrere o la presenza di
eventuali ostacoli, sia per fornire le necessarie misure di posizione utilizzate dall’algoritmo di controllo basato sul Filtro di Kalman Esteso. È stata sperimentata con
successo anche una soluzione per la comunicazione wireless continua fra il robot ed il
computer di supervisione; tale soluzione, basata sull’impiego di moduli XBee, si è dimostrata particolarmente efficiente grazie all’affidabilità di questo tipo di trasmissione
a breve raggio, unitamente alle piccole dimensioni ed ai consumi ridotti dei dispositivi
utilizzati.
Nel lavoro è stato poi affrontato il problema del controllo del moto del robot rover
mediante l’impiego del Filtro di Kalman Esteso. In particolare, l’assenza di sistemi
di rilevo continuo della posizione assoluta del robot pone il problema della sua esatta
localizzazione nello spazio, a cui riferire il moto imposto dall’algoritmo di controllo.
Al fine di superare tale problema, è stato sviluppato un modello cinematico del moto
del robot basato unicamente sulla caratterizzazione statistica sperimentale degli errori rilevati per percorsi noti e predefiniti. Sfruttando poi le proprietà di predizione
e stima possedute dal Filtro di Kalman Esteso, è stato sviluppato un algoritmo di
controllo in grado di stimare con sufficiente precisione l’effettiva posizione del robot
durante il moto e, sulla base anche di misure locali effettuate dai sensori di prossimità
di cui il robot è dotato, operare una correzione dinamica al fine di ricondurre il robot
56
Cap. 4 Conclusioni e sviluppi futuri
sul percorso teorico imposto. Le prestazioni dell’algoritmo di controllo messo a punto
sono state poi verificate in ambiente di simulazione, mostrando ottime performance nella correzione dinamica del moto del robot per percorsi diversi e più complessi
rispetto a quelli utilizzati per l’individuazione del modello cinematico.
Le prospettive future legate al prosieguo del lavoro sono legate sia alla verifica sperimentale delle prestazioni del filtro di Kalman per la correzione dinamica del moto
del robot rover, sia ad una più approfondita fase di ingegnerizzazione del prototipo;
a tal riguardo, il ricorso a soluzioni SMT (Surface Mount Technology) può consentire
una notevole riduzione delle dimensioni del robot rover senza modifiche nell’architettura elettronica, consentendo di pervenire ad una realizzazione tipica per micromouse
competition. Parallelamente, la messa a punto di ulteriori soluzioni derivate da quelle
realizzate può consentire lo sviluppo di altre famiglie di robot rover. A tal proposito,
appare molto interessante e suscettibile di svariate applicazioni pratiche lo studio di
schiere di robot rover in grado di muoversi autonomamente in ambienti sconosciuti e
di comunicare in tempo reale, al fine di costruire dinamicamente la mappa dei luoghi
sulla base di rilievi effettuati con idonei sensori e mediante l’elaborazione di immagini
catturate da microtelecamere installate sui robot stessi.
57
Appendice A
Schemi ed immagini
Figura 4.1: Schema elettrico realizzato per il controllo della sequenza di alimentazione
dei motori passo-passo
58
APPENDICE A - SCHEMI ED IMMAGINI
Figura 4.2: Robot Rover, vista frontale.
59
APPENDICE A - SCHEMI ED IMMAGINI
Figura 4.3: Robot Rover, vista lato destro.
60
APPENDICE A - SCHEMI ED IMMAGINI
Figura 4.4: Robot Rover, vista lato sinistro.
61
APPENDICE A - SCHEMI ED IMMAGINI
Figura 4.5: Robot Rover, vista posteriore.
62
Appendice B
Listati dei programmi
1
2
% In questo file vengono definiti e inizializzati tutti i valori
% relativi al robot e all'ambiente necessari per gli esperimenti.
3
4
5
% clear all
% close all
6
7
DISEGNA = 1;
8
9
10
11
global numSegmenti n q RaggioCirconfRobot numSensoriLaser M
global thetaLaserHome uRe uLe KR KL
global tol infinito xMin xMax yMin yMax Dx Dy
12
13
14
tol = 0.001; % tolleranza nei confronti di uguaglianza
Infinito = 9999; % infinito pratico
15
16
17
18
19
20
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%
DATI
AMBIENTE
%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
21
22
23
numOstacoli = 0; % numero degli ostacoli presenti nell'ambiente
% (non previsto)
24
25
% Vertici del perimetro in senso antiorario
26
27
28
29
% Coordinate x ambiente
Px(1) = 0; Px(2) = 80; Px(3) = 80; Px(4) = 20; Px(5) = 20;
Px(6) = 100; Px(7) = 100; Px(8) = 0;
30
31
32
33
% Coordinate y ambiente
Py(1) = 0; Py(2) = 0; Py(3) = 20; Py(4) = 20; Py(5) = 40;
Py(6) = 40; Py(7) = 60; Py(8) = 60;
34
63
APPENDICE B - LISTATI DEI PROGRAMMI
35
36
37
38
39
40
41
% dati mappa
xMin = min(Px);
xMax = max(Px);
yMin = min(Py);
yMax = max(Py);
Dx = xMax − xMin;
Dy = yMax − yMin;
42
43
numVertici = 8;
44
45
46
47
48
49
50
51
52
53
54
P=zeros(numVertici,2);
% matrice coordinate vertici: QUESTE VANNO RUOTATE!!
P(1,:)=[Px(1) Py(1)];
P(2,:)=[Px(2) Py(2)];
P(3,:)=[Px(3) Py(3)];
P(4,:)=[Px(4) Py(4)];
P(5,:)=[Px(5) Py(5)];
P(6,:)=[Px(6) Py(6)];
P(7,:)=[Px(7) Py(7)];
P(8,:)=[Px(8) Py(8)];
55
56
57
58
59
for indVertice = 1:numVertici,
[P(indVertice,1),P(indVertice,2)] =
ruota(P(indVertice,1),P(indVertice,2));
end
60
61
62
63
64
65
M = zeros(numVertici,4);
for indVertice = 1:numVertici−1,
M(indVertice,:) = [P(indVertice,:) P(indVertice+1,:)];
end
M(numVertici,:) = [P(numVertici,:) P(1,:)];
66
67
68
69
70
71
%
%
%
%
%
M = [P1 P2; P2 P3; P3 P4; ... P {numVertici} P1];
matrice dei lati: la riga i contiene
i due vertici del lato i del perimetro, quindi ci sono
tante righe quanti sono i lati
del perimetro esterno dell'ambiente
72
73
74
numSegmenti = numVertici;
% numero dei lati del perimetro dell'ambiente
75
76
77
78
79
80
81
82
83
84
85
% Coefficienti delle rette contenenti i lati del perimetro
n = ones (1,numSegmenti); % coefficienti angolari rette
q = ones (1,numSegmenti); % intercette rette
for lato = 1:numSegmenti,
P1x = M(lato,1);
P1y = M(lato,2);
P2x = M(lato,3);
P2y = M(lato,4);
if P1x==P2x,
n(lato) = Inf;
64
APPENDICE B - LISTATI DEI PROGRAMMI
q(lato) = P1x;
% in questo caso il coefficiente
% q ricorda la x dell'equazione x = q
86
87
88
89
else
n(lato) = (P2y−P1y)/(P2x−P1x);
% coefficiente angolare lato
q(lato) = P1y − n(lato)*P1x;
% costante lato
90
91
92
93
94
95
end
end
96
97
98
99
100
101
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%
DATI
ROBOT
%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
102
103
104
105
% Caratteristiche geometriche del robot
numSensoriLaser = 3;
RaggioCirconfRobot = 5.64;
106
107
108
% angoli laser rispetto asse di movimento del robot
thetaLaserHome = zeros(numSensoriLaser,1);
109
110
111
112
%Posizionamento
thetaLaserHome = thetaLaserHome + [−pi/2 0 pi/2]';
thetaLaserHome;
113
114
115
116
117
118
119
120
121
122
% Caratteristiche dinamiche robot
anr = 1.5013e−004;
anl = 0.0012;
KR = 1.2737e−005;
KL = 4.3501e−006;
% anr = 0;
% anl = 0;
% KR = 0.01;
% KL = 0.01;
123
124
125
126
%asse ruote
a = 9.8;
RaggioVeroCirconfRobot = 5.64;
127
128
129
130
131
132
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%
DATI SIMULAZIONE
%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
133
134
135
136
Npassi = 5000; % numero totale di step k della simulazione
Nstep = 100; % numero di passi tra una stima e la successiva
65
APPENDICE B - LISTATI DEI PROGRAMMI
% (maggiore di uno quando la stima viene
% fatta solo dopo un certo numero di passi)
137
138
139
140
141
142
143
144
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%% GENERAZIONE MOVIMENTO
%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
145
146
147
148
% MOVIMENTO LINEARE A TRATTI
% il robot è già orientato per bene
149
150
151
152
153
154
155
156
157
% Coordinate topiche movimento robot:
% QUESTE VANNO RUOTATE!!!
Rx(1) = 90; Ry(1) = 50;
Rx(2) = 8; Ry(2) = 50;
Rx(3) = 28; Ry(3) = 10;
Rx(4) = 90; Ry(4) = 10;
Rx(5) = 90; Ry(5) = 10;
158
159
numVertR = numel(Rx);
160
161
162
163
164
165
for indVertR = 1:numVertR,
[Rx(indVertR),Ry(indVertR)] =
ruota(Rx(indVertR),Ry(indVertR));
end
166
167
168
169
170
x0 = Rx(1); % x iniziale
y0 = Ry(1); % y iniziale
theta0 = atan2(Ry(2)−Ry(1),Rx(2)−Rx(1));
% orientamento iniziale
171
172
173
174
%thetaf = atan2(Ry(3)−Ry(2),Rx(3)−Rx(2));
% orientamento finale
%theta0f = thetaf−theta0;
175
176
177
deltaRho = zeros(1,Npassi);
deltaTheta = zeros(1,Npassi);
178
179
180
181
182
d1
d2
d3
d4
=
=
=
=
sqrt((Rx(2)−Rx(1))ˆ2+(Ry(2)−Ry(1))ˆ2);
sqrt((Rx(3)−Rx(2))ˆ2+(Ry(3)−Ry(2))ˆ2);
sqrt((Rx(4)−Rx(3))ˆ2+(Ry(4)−Ry(3))ˆ2);
sqrt((Rx(5)−Rx(4))ˆ2+(Ry(5)−Ry(4))ˆ2);
183
184
dTOT = d1+d2+d3+d4;
185
186
187
passiTratto1 = round(Npassi*d1/dTOT);
passiTratto2 = round(Npassi*(d1+d2)/dTOT)−passiTratto1;
66
APPENDICE B - LISTATI DEI PROGRAMMI
188
189
190
191
passiTratto3 = round(Npassi*(d1+d2+d3)/dTOT)−
passiTratto1−passiTratto2;
passiTratto4 = Npassi − passiTratto1−passiTratto2 −
passiTratto3;
192
193
194
rhoC = 0.05*1000/Npassi;
nC = 40*Npassi/1000; % sceglierlo pari
195
196
197
198
199
200
% Primo tratto
deltaRho=d1*ones(1,passiTratto1−nC/2)/(passiTratto1);
% variazioni radiali
deltaTheta=ones(1,passiTratto1−nC/2)*0;
% angolo costante che non varia nel tempo
201
202
203
204
205
% Curva
deltaRho = [deltaRho rhoC*ones(1,nC)];
deltaTheta = [deltaTheta (pi/2)/nC*ones(1,nC)];
% rotazione di 90 gradi
206
207
208
209
210
211
212
% Secondo tratto
deltaRho=[deltaRho d2*ones(1,passiTratto2−nC)/
(passiTratto2)];
% variazioni radiali
deltaTheta=[deltaTheta ones(1,passiTratto2−nC)*0];
% angolo costante che non varia nel tempo
213
214
215
216
217
% Curva
deltaRho = [deltaRho rhoC*ones(1,nC)];
deltaTheta = [deltaTheta (pi/2)/nC*ones(1,nC)];
% rotazione di 90 gradi
218
219
220
221
222
223
% Terzo tratto
deltaRho=[deltaRho d3*ones(1,passiTratto3−nC)/
(passiTratto3)]; % variazioni radiali
deltaTheta=[deltaTheta ones(1,passiTratto3−nC)*0];
% angolo costante che non varia nel tempo
224
225
226
227
228
229
230
231
% Curva
deltaRho = [deltaRho rhoC*ones(1,nC)];
angSucc = atan2(Ry(5)−Ry(4),Rx(5)−Rx(4));
angPrec = atan2(Ry(4)−Ry(3),Rx(4)−Rx(3));
angRot = angSucc − angPrec;
deltaTheta = [deltaTheta angRot/nC*ones(1,nC)];
% rotazione di angRot gradi
232
233
234
235
236
237
% Quarto tratto
deltaRho=[deltaRho d4*ones(1,passiTratto4−nC/2)/
(passiTratto4)]; % variazioni radiali
deltaTheta=[deltaTheta ones(1,passiTratto4−nC/2)*0];
% angolo costante che non varia nel tempo
238
67
APPENDICE B - LISTATI DEI PROGRAMMI
239
240
241
242
243
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%% FINE SEZIONE GENERAZIONE MOVIMENTI%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
244
245
246
247
248
249
250
251
252
253
%%
% Definizione dei vettori x,y e theta delle coordinate
% del robot durante la simulazione:
x = zeros(1,Npassi);
y = zeros(1,Npassi);
theta = zeros(1,Npassi);
%spazio = zeros(1,Npassi);
254
255
256
257
258
259
% Assegnazione condizione iniziale assunta dal robot:
x(1)=x0;
y(1)=y0;
theta(1)=theta0;
%spazio(1)=0;
260
261
262
263
264
265
266
267
268
%% Standard deviation degli errori
%della stima della posizione iniziale
sigmaX0 = 0.01;
sigmaY0 = 0.01;
sigmaTheta0 = 0.02;
% sigmaX0 = 0;
% sigmaY0 = 0;
% sigmaTheta0 = 0;
269
270
271
272
273
%% Errore stima iniziale
DeltaX0 = sigmaX0*randn(1);
DeltaY0 = sigmaY0*randn(1);
DeltaTheta0 = sigmaTheta0*randn(1);
274
275
276
277
% DeltaX0 = 0;
% DeltaY0 = 0;
% DeltaTheta0 = 0;
278
279
280
281
x(1)=x0;
y(1)=y0;
theta(1)=theta0;
282
283
%%
284
285
286
287
%Passo simulazione
deltaR = 1;
deltaL = 1;
288
289
68
APPENDICE B - LISTATI DEI PROGRAMMI
290
291
uR = (deltaRho + (a/2)*deltaTheta);
uL = (deltaRho − (a/2)*deltaTheta);
292
293
294
295
296
297
298
299
% Assegnazione di tutte le posizioni assunte dal robot:
for k=1: (Npassi−1),
x(k+1)= x(k)+ ((deltaR*uR(k)+deltaL*uL(k))/2)*cos(theta(k));
y(k+1)= y(k)+ ((deltaR*uR(k)+deltaL*uL(k))/2)*sin(theta(k));
theta(k+1)= theta(k)+ ((deltaR*uR(k)−deltaL*uL(k))/a);
end
300
301
302
303
304
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% GENERAZIONE DISTURBI ODOMETRICI, STIMA INIZIALE E LASER %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
305
306
307
308
309
310
% Caratteristiche dinamiche robot
NiR = randn(Npassi,1)*sqrt(KR);
% sulle letture odometriche di traslazione
NiL = randn(Npassi,1)*sqrt(KL);
% sulle letture odometriche di traslazione
311
312
313
314
315
316
%% Letture odometriche corrotte
deltaRhoe = zeros(1,Npassi);
% vettore letture odometriche traslazionali
deltaThetae = zeros(1,Npassi);
% vettore letture odometriche di orientamento
317
318
319
320
321
322
323
324
325
326
327
uRe = zeros(Npassi,1);
% vettore letture odometriche ruota destra
uLe = zeros(Npassi,1);
% vettore letture odometriche ruota sinistra
for k = 1:Npassi,
uRe(k) = uR(k) + sqrt(abs(uR(k)))*NiR(k);
%letture odometriche ruota destra
uLe(k) = uL(k) + sqrt(abs(uL(k)))*NiL(k);
%letture odometriche ruota sinistra
end
328
329
330
331
332
333
334
335
for k = 1:Npassi,
deltaRhoe(k) = deltaRhoe(k) +
((uRe(k)*(1+anr)) + (uLe(k)*(1+anl)))/2;
deltaThetae(k) = deltaThetae(k) +
((uRe(k)*(1+anr)) − (uLe(k)*(1+anl)))/a;
end
336
337
338
339
340
xe = zeros(1,Npassi);
ye = zeros(1,Npassi);
69
APPENDICE B - LISTATI DEI PROGRAMMI
341
342
343
xe(1)=x0;
ye(1)=y0;
344
345
346
347
348
for k=1:Npassi,
xe(k+1)=xe(k)+deltaRhoe(k)*cos(deltaThetae(k));
ye(k+1)=xe(k)+deltaRhoe(k)*sin(deltaThetae(k));
end
349
350
351
352
353
354
355
356
357
358
% DISTURBI LASER
ZitaLaser = 0.1; % varianza su lettura laser
%(per ora uguali per tutti i laser)
laserNoise = sqrt(ZitaLaser)*
randn(numSensoriLaser,round(Npassi));
%laser Noise −> matrice in cui
%la riga i−esima contiene i disturbi
%sulle letture del laser i−esimo di tutta la simulazione
359
360
%VERIFICA
361
362
363
364
if DISEGNA,
disegnaFig
end
70
APPENDICE B - LISTATI DEI PROGRAMMI
1
2
% Simulazione modello dinamico robot (solo Predizione)
% ESEGUIRE DATI.M PRIMA DI QUESTO PROGRAMMA
3
4
5
6
7
8
9
10
% Vettori di stima
xHatEKFsgl = zeros(1,Npassi);
xHatMenoEKFsgl = zeros(1,Npassi);
yHatEKFsgl = zeros(1,Npassi);
yHatMenoEKFsgl = zeros(1,Npassi);
thetaHatEKFsgl = zeros(1,Npassi);
thetaHatMenoEKFsgl = zeros(1,Npassi);
11
12
13
14
15
% Stima della posizione iniziale assunta dal robot
xHatEKFsgl(1) = x0 + DeltaX0;
yHatEKFsgl(1) = y0 + DeltaY0;
thetaHatEKFsgl(1) = theta0 + DeltaTheta0;
16
17
18
% matrice di covarianza sulla stima iniziale
Pcov = diag([sigmaX0ˆ2, sigmaY0ˆ2, sigmaTheta0ˆ2]);
19
20
numeroStime = 0;
21
% conta il numero di volte che è stata
% effettuata la stima con la correzione
22
23
Jpepe EKFsgl = 0;
24
25
for k = 1:Npassi−1,
26
27
28
% FASE di PREDIZIONE (fatta per tutti i k)
29
30
31
32
33
34
35
xHatMenoEKFsgl(k+1) = xHatEKFsgl(k) + deltaRhoe(k)*
cos(thetaHatEKFsgl(k)); % xHat(k+1|k)
yHatMenoEKFsgl(k+1) = yHatEKFsgl(k) + deltaRhoe(k)*
sin(thetaHatEKFsgl(k)); % yHat(k+1|k)
thetaHatMenoEKFsgl(k+1) = thetaHatEKFsgl(k) + deltaThetae(k);
% thetaHat(k+1|k)
36
37
38
39
40
41
% jacobiano della dinamica rispetto allo stato
F = [1 0 −deltaRhoe(k)*sin(thetaHatEKFsgl(k));
0 1 deltaRhoe(k)*cos(thetaHatEKFsgl(k));
0 0 1];
42
43
44
45
46
% jacobiano della dinamica rispetto al disturbo
W = [sqrt(deltaRhoe(k))*cos(thetaHatEKFsgl(k)) 0;
sqrt(deltaRhoe(k))*sin(thetaHatEKFsgl(k)) 0;
0 sqrt(deltaRhoe(k))];
47
48
Q = [KR*abs(uRe(k)) 0; 0 KL*abs(uLe(k))];
49
50
% Aggiornamento matrice di covarianza
71
APPENDICE B - LISTATI DEI PROGRAMMI
51
Pmeno = F*Pcov*F' + W*Q*W';
52
53
54
55
56
xHatEKFsgl(k+1) = xHatMenoEKFsgl(k+1);
% xHat(k+1|k+1)
yHatEKFsgl(k+1) = yHatMenoEKFsgl(k+1);
% yHat(k+1|k+1)
thetaHatEKFsgl(k+1) = thetaHatMenoEKFsgl(k+1);% thetaHat(k+1|k+1)
57
58
59
% Aggiornamento matrice di covarianza
Pcov = Pmeno;
60
61
end
62
63
%end
64
65
66
67
68
if DISEGNA,
disegnaFig
plot(xHatEKFsgl,yHatEKFsgl,'r')
end
69
70
71
% prestazione normalizzata
J EKFsgl = sqrt(sum((x−xHatEKFsgl).ˆ2+(y−yHatEKFsgl).ˆ2)/Npassi) ;
72
73
74
75
if numeroStime >0,
Jpepe EKFsgl = sqrt(Jpepe EKFsgl/numeroStime);
end
72
APPENDICE B - LISTATI DEI PROGRAMMI
1
2
3
4
%
%
%
%
Riporta la stima del percorso del robot fatta con EKF dotato di
soglia sull'innovazione (scarta valori dell'innovazione troppo
grandi dovuti a spigoli o ad ostacoli imprevisti nell'ambiente
ESEGUIRE DATI.M PRIMA DI QUESTO PROGRAMMA
5
6
7
8
9
10
11
12
% Vettori di stima
xHatEKFsgl = zeros(1,Npassi);
xHatMenoEKFsgl = zeros(1,Npassi);
yHatEKFsgl = zeros(1,Npassi);
yHatMenoEKFsgl = zeros(1,Npassi);
thetaHatEKFsgl = zeros(1,Npassi);
thetaHatMenoEKFsgl = zeros(1,Npassi);
13
14
15
16
17
% Stima della posizione iniziale assunta dal robot
xHatEKFsgl(1) = x0 + DeltaX0;
yHatEKFsgl(1) = y0 + DeltaY0;
thetaHatEKFsgl(1) = theta0 + DeltaTheta0;
18
19
20
% matrice di covarianza sulla stima iniziale
Pcov = diag([sigmaX0ˆ2, sigmaY0ˆ2, sigmaTheta0ˆ2]);
21
22
23
numeroStime = 0; % conta il numero di volte che è stata effettuata
%la stima con la correzione
24
25
26
% matrice di covarianza del rumore sulla dinamica (NiRho e NiTheta)
%Q = [Krho 0; 0 Ktheta];
27
28
29
% matrice covarianza misure
R = ZitaLaser*eye(numSensoriLaser);
30
31
Jpepe EKFsgl = 0;
32
33
for k = 1:Npassi−1,
34
35
36
% FASE di PREDIZIONE (fatta per tutti i k)
37
38
39
40
41
42
43
xHatMenoEKFsgl(k+1) = xHatEKFsgl(k) + deltaRhoe(k)*
cos(thetaHatEKFsgl(k)); % xHat(k+1|k)
yHatMenoEKFsgl(k+1) = yHatEKFsgl(k) + deltaRhoe(k)*
sin(thetaHatEKFsgl(k)); % yHat(k+1|k)
thetaHatMenoEKFsgl(k+1) = thetaHatEKFsgl(k) + deltaThetae(k);
% thetaHat(k+1|k)
44
45
46
47
48
49
% jacobiano della dinamica rispetto allo stato
F = [1 0 −deltaRhoe(k)*sin(thetaHatEKFsgl(k));
0 1 deltaRhoe(k)*cos(thetaHatEKFsgl(k));
0 0 1];
50
73
APPENDICE B - LISTATI DEI PROGRAMMI
51
52
53
54
% jacobiano della dinamica rispetto al disturbo
W = [sqrt(deltaRhoe(k))*cos(thetaHatEKFsgl(k)) 0;
sqrt(deltaRhoe(k))*sin(thetaHatEKFsgl(k)) 0;
0 sqrt(deltaRhoe(k))];
55
56
Q = [KR*abs(uRe(k)) 0; 0 KL*abs(uLe(k))];
57
58
59
% Aggiornamento matrice di covarianza
Pmeno = F*Pcov*F' + W*Q*W';
60
61
62
% FASE di STIMA (fatta solo ogni Nstep passi)
63
64
if mod(k,Nstep) == 0, % va fatta la stima con Kalman Esteso
65
66
numeroStime = numeroStime + 1;
67
68
69
70
71
% distanze laser corrotte (reali)
[latiLetti,distanzeLette] = distanzeLaser(x(k+1),y(k+1),
theta(k+1));
rhoiReali = distanzeLette + laserNoise(:,k);% era numeroStime
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
% jacobiano H delle misure buone
H = zeros(numSensoriLaser,3);
ammessi = ones(numSensoriLaser,1);
[latiAttesi, rhoiAttese] = distanzeLaser(xHatMenoEKFsgl(k+1),
yHatMenoEKFsgl(k+1),thetaHatMenoEKFsgl(k+1));
% calcolo prima i rhoi e i lati con cui si intersecano i laser
if min(latiAttesi)==0, % stima fuori mappa
[latiAttesi1,rhoiAttese1,latiAttesi,rhoiAttese] =
distanzeLaserRob(xHatMenoEKFsgl(k+1),yHatMenoEKFsgl(k+1),
thetaHatMenoEKFsgl(k+1));
for laser = 1:numSensoriLaser,
if latiAttesi1(laser) == 0,
ammessi(laser)=0;
end
end
end
89
90
91
innovation = rhoiReali−rhoiAttese;
92
93
94
xLaser = xHatMenoEKFsgl(k+1);
yLaser = yHatMenoEKFsgl(k+1);
95
96
97
98
99
100
101
for laser = 1: numSensoriLaser, % per ogni laser
if latiAttesi(laser)>0 &&
abs(innovation(laser))/max([rhoiReali(laser),
rhoiAttese(laser)]) < 0.4,
% allora l'innovazione e' accettabile
nLaser = n(latiAttesi(laser));
74
APPENDICE B - LISTATI DEI PROGRAMMI
% determino il coeff. angolare del lato con
% cui si interseca il laser (ni,j nelle formule)
qLaser = q(latiAttesi(laser));
% e l'intercetta di tale lato (qi,j nelle formule)
angLaser = thetaHatMenoEKFsgl(k+1)+
thetaLaserHome(laser);
% l'angolo del raggio laser
% (theta t + theta i nelle formule)
cosLaser = cos(angLaser);% il coseno di tale angolo
sinLaser = sin(angLaser);% e il seno
if nLaser == Inf, % la retta con
%cui si interseca il laser e' verticale
if abs(cosLaser) < tol,
% il raggio Laser e' paralello al lato
H(laser,:) = [Inf 0 Inf];
else % raggio Laser NON e' parallelo al lato
H(laser,:) =
[−1/cosLaser 0 rhoiAttese(laser)*sinLaser/cosLaser];
end
else % retta obliqua (ossia non verticale)
DEN = nLaser*cosLaser−sinLaser;
if abs(DEN) < 0.0001,
% raggio laser parallelo a lato
H(laser,:) = [Inf Inf Inf];
else % il raggio Laser NON e' paralello al lato
H(laser,1) = −nLaser/DEN;
H(laser,2) = 1/DEN;
H(laser,3) =
−(nLaser*xLaser+qLaser−yLaser)*
(cosLaser + nLaser*sinLaser)/DENˆ2;
end
end
else
ammessi(laser) = 0;
end
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
end
138
139
140
141
142
143
144
ammessiLaser = sum(ammessi);
H = riduciM(H,ammessi);
innovation = riduciM(innovation,ammessi);
R = ZitaLaser*eye(ammessiLaser);
% calcolo guadagno Kalman K (MATRICE 3X16)
K = Pmeno*H'*pinv(H*Pmeno*H'+R);
145
146
147
148
149
150
151
152
% Calcolo della stima a posteriori:
xHatEKFsgl(k+1) = xHatMenoEKFsgl(k+1)
+ K(1,:)*innovation; % xHat(k+1|k+1)
yHatEKFsgl(k+1) = yHatMenoEKFsgl(k+1)
+ K(2,:)*innovation; % yHat(k+1|k+1)
thetaHatEKFsgl(k+1) = thetaHatMenoEKFsgl(k+1)
75
APPENDICE B - LISTATI DEI PROGRAMMI
+ K(3,:)*innovation; % thetaHat(k+1|k+1)
153
154
% Aggiornamento matrice di covarianza
Pcov = (eye(3) − K*H)*Pmeno;
Jpepe EKFsgl = Jpepe EKFsgl
+ (x(k+1)−xHatEKFsgl(k+1)).ˆ2+
(y(k+1)−yHatEKFsgl(k+1)).ˆ2;
155
156
157
158
159
160
else
161
162
% non si fa la stima con EKF ma si
% utilizza solo odometria
163
xHatEKFsgl(k+1) = xHatMenoEKFsgl(k+1);
% xHat(k+1|k+1)
yHatEKFsgl(k+1) = yHatMenoEKFsgl(k+1);
% yHat(k+1|k+1)
thetaHatEKFsgl(k+1) = thetaHatMenoEKFsgl(k+1);
% thetaHat(k+1|k+1)
164
165
166
167
168
169
170
% Aggiornamento matrice di covarianza
Pcov = Pmeno;
171
172
173
end
174
175
176
end
177
178
179
180
181
if DISEGNA,
disegnaFig
plot(xHatEKFsgl,yHatEKFsgl,'r')
end
182
183
184
% prestazione normalizzata
J EKFsgl = sqrt(sum((x−xHatEKFsgl).ˆ2+(y−yHatEKFsgl).ˆ2)/Npassi) ;
185
186
187
188
if numeroStime >0,
Jpepe EKFsgl = sqrt(Jpepe EKFsgl/numeroStime);
end
76
APPENDICE B - LISTATI DEI PROGRAMMI
Listing 4.1: Programma taratura diretta sensori
4
int sensorValue1 = 0; //Sensore Sinistro
int sensorValue2 = 0; //Sensore Destro
int sensorValue3 = 0; //Sensore Centrale
6
void setup() {
8
Serial.begin(9600);
2
10
}
12
void loop() {
sensorValue1 = analogRead(1);
Serial.print(sensorValue1);
Serial.print(” ”);
14
16
sensorValue3 = analogRead(3);
Serial.print(sensorValue3);
Serial.print(” ”);
18
20
sensorValue2 = analogRead(2);
Serial.println(sensorValue2);
22
24
}
77
APPENDICE B - LISTATI DEI PROGRAMMI
Listing 4.2: Programma Rover evita ostacoli
#include <Stepper.h>
#define STEPS 48
#define RELE1 4
#define RELE2 3
Stepper stepper(STEPS, 12, 11, 10, 9);
int sensorS = 0;
int sensorD = 0;
int sensorC = 0;
int n = 0;
void setup()
{
pinMode(RELE1, OUTPUT);
pinMode(RELE2, OUTPUT);
Serial.begin(9600);
stepper.setSpeed(40);
}
void loop()
{
n = 0;
sensorS = analogRead(1);
sensorD = analogRead(2);
sensorC = analogRead(3);
do{ //vai avanti
digitalWrite(RELE1, HIGH);
digitalWrite(RELE2, HIGH);
stepper.step(n);
sensorS = analogRead(1);
sensorD = analogRead(2);
sensorC = analogRead(3);
n++;
}while(sensorC < 250);
78
APPENDICE B - LISTATI DEI PROGRAMMI
n = 0;
do{ //gira a sinistra
digitalWrite(RELE1, LOW);
digitalWrite(RELE2, HIGH);
stepper.step(n);
sensorS = analogRead(1);
sensorD = analogRead(2);
sensorC = analogRead(3);
n++;
}while(sensorC > 250 && sensorS < 250);
n = 0;
do{ //gira a destra
digitalWrite(RELE1, HIGH);
digitalWrite(RELE2, LOW);
stepper.step(n);
sensorS = analogRead(1);
sensorD = analogRead(2);
sensorC = analogRead(3);
n++;
}while(sensorC > 250 && sensorD < 250);
n = 0;
do{ //vicolo cieco
digitalWrite(RELE1, HIGH);
digitalWrite(RELE2, LOW);
stepper.step(n);
sensorS = analogRead(1);
sensorD = analogRead(2);
sensorC = analogRead(3);
n++;
}while(sensorC > 250 && sensorD > 250 && sensorS > 250);
79
APPENDICE B - LISTATI DEI PROGRAMMI
Listing 4.3: Esempio programma per misure odometriche (480 passi) con
inizializzazione wifi-seriale
1
#include <Stepper.h>
3
#define STEPS 48
5
Stepper stepper(STEPS, 12, 11, 10, 9);
7
9
11
13
15
17
int count = 0;
int a = 0;
void setup()
{
pinMode(4, OUTPUT);
pinMode(3, OUTPUT);
stepper.setSpeed(40);
Serial.begin(9600);
stepper.step(1);
}
19
21
23
void loop()
{
if (Serial.available() > 0)
{
a = Serial.read();
25
if(a==49)
{
digitalWrite(3, HIGH);
digitalWrite(4, HIGH);
delay(50);
stepper.step(480);
}
27
29
31
33
}
35
}
80
Elenco delle figure
1.1
Tunnel del LHC, a destra la linea che segue il LineFollower. . . . . . .
7
1.2
Il LineFollower realizzato. . . . . . . . . . . . . . . . . . . . . . . . .
8
1.3
Motore CC e Tamiya Motor Gearbox . . . . . . . . . . . . . . . . . .
8
1.4
Il ponte H. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
10
1.5
Duty Cycle. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
11
1.6
Sign-Magnitude PWM. . . . . . . . . . . . . . . . . . . . . . . . . . .
12
1.7
Locked Anti-Phase PWM. . . . . . . . . . . . . . . . . . . . . . . . .
14
1.8
Sensore QTI. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
16
1.9
Il prototipo di encoder realizzato. . . . . . . . . . . . . . . . . . . . .
17
1.10 Funzione del Trigger di Schmitt . . . . . . . . . . . . . . . . . . . . .
18
1.11 Segnali in uscita dal fotodiodo (sotto) e in uscita dal Trigger di Schmitt
(sopra). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
19
1.12 Arduino Mega. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
19
1.13 Il LineFollower nel percorso di test . . . . . . . . . . . . . . . . . . .
22
2.1
Concept . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
25
2.2
Il robot realizzato. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
26
2.3
Motore passo-passo unipolare. . . . . . . . . . . . . . . . . . . . . . .
28
2.4
Curve di pull-in e pull-out. . . . . . . . . . . . . . . . . . . . . . . . .
30
81
ELENCO DELLE FIGURE
ELENCO DELLE FIGURE
2.5
Pilotaggio motore passo-passo . . . . . . . . . . . . . . . . . . . . . .
33
2.6
Scheda sensori . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
37
2.7
Scheda di potenza, controllo e comunicazione. . . . . . . . . . . . . .
38
2.8
Arduino Nano . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
39
2.9
Modulo XBee . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
40
2.10 Batteria al NiMh . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
41
3.1
Modello geometrico associato . . . . . . . . . . . . . . . . . . . . . .
44
3.2
Misure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
46
3.3
Modello geometrico associato alle misure . . . . . . . . . . . . . . . .
50
3.4
Simulazione percorso # 1 per la verifica del modello cinematico . . .
53
3.5
Simulazione percorso # 1 con Filtro di Kalman Esteso . . . . . . . .
54
4.1
Schema elettrico realizzato per il controllo della sequenza di alimentazione dei motori passo-passo . . . . . . . . . . . . . . . . . . . . . . .
58
4.2
Robot Rover, vista frontale. . . . . . . . . . . . . . . . . . . . . . . .
59
4.3
Robot Rover, vista lato destro.
. . . . . . . . . . . . . . . . . . . . .
60
4.4
Robot Rover, vista lato sinistro. . . . . . . . . . . . . . . . . . . . . .
61
4.5
Robot Rover, vista posteriore. . . . . . . . . . . . . . . . . . . . . . .
62
82
Bibliografia
[1] Texas Instruments - “L293,L293D QUADRUPLE HALF-H DRIVERS”,
Settembre 1986 - Revisionato Novembre 2004.
[2] Parallax - “QTI Line Sensor”, www.parallax.com.
[3] ST Microelectronics - “M74HC14 - Hex Schmitt Inverter”, www.st.com - 2001.
[4] “Arduino Mega 1280”, http://arduino.cc/en/Main/ArduinoBoardMega.
[5] Duyao Wang, Xiaoqing Yu, Wanggen Wan, Hongfu Xu - “A New Method of Infrared Sensor Measurement for Micromouse Control”, School of Communication
and Information Engineering, Shangai University - IEEE 2008.
[6] Wantai Motor CO.,LTD. - “PM42L-048-17 - Wantai Mini Stepper Product
Specification”, www.wantmotor.com.
[7] “I motori passo-passo”, Versione 2.1 - http://www.vincenzov.net - Giugno 2001.
[8] Texas Instruments - “High-Voltage High-Current Darlington Transistor Array”,
Dicembre 1976 - Revisionato Febbraio 2003.
[9] Sharp Corporation - “GP2D120XJ00F - Analog Output Type Distance Measuring
Sensor”.
83
BIBLIOGRAFIA
BIBLIOGRAFIA
[10] ST Microelectronics - “L78S05CV - 2A POSITIVE VOLTAGE REGULATOR”,
www.st.com - 2003.
[11] “Arduino Nano 3.0”, http://www.arduino.cc/en/Main/ArduinoBoardNano.
[12] MaxStream - “XBee/XBee-PRO OEM RF Modules v. 1.06”, ZigBee/IEEE
802.15.4 OEM RF Modules by MaxStream, Inc..
[13] Ernest O. Doebelin, Cigada Alfredo, and Gasparetto Michele - “Strumenti e
metodi di misura”, McGraw-Hill - 2008.
[14] Francesco Martinelli - “Mini-course on Filtering and Mobile Robot Localization”,
Università degli Studi di Roma “Tor Vergata” - 2010.
[15] Stefano Al Zanati - “Realizzazione e Localizzazione mediante Kalman di un robot
mobile”, Tesi di Laurea in Ingegneria dell’Automazione, Università degli Studi di
Roma “Tor Vergata” - 2010.
[16] Fulvio Forni - “Filtro di Kalman Esteso per la localizzazione di robot mobili”,
Tesi di Laurea in Ingegneria Informatica, Università degli Studi di Roma “Tor
Vergata” - 2004.
[17] Marco Gerosi - “Navigazione e localizzazione del robot Nomad 150 mediante filtro
di Kalman”, Tesi di Laurea in Ingegneria Informatica, Università degli Studi di
Roma “Tor Vergata” - 2005.
84

Progettazione, realizzazione ed implementazione del sistema di

Documenti correlati

Prodotti

Supporto

Progettazione, realizzazione ed implementazione del sistema di

Documenti correlati

Aggiungere questo documento alla raccolta (s)

Aggiungere questo documento salvato

Suggeriscici come migliorare StudyLib