Universita degli Studi di Siena` - Danny's LAB · rativo BrickOS [9] [13] (ex LegOS), che consente...

Universita degli Studi di Siena

Facolta di Ingegneria

Corso di Laurea in Ingegneria Informatica

Squadre di robot mobili basati su

tecnologia Lego Mindstorms

Relatore

Prof. Andrea Garulli

Correlatori Tesi di laurea di

Ing. Antonio Giannitrapani Daniele Benedettelli

Ing. Marco Casini

ANNO ACCADEMICO 2005/2006

Indice

1 Introduzione 1

1.1 Obiettivi e motivazioni . . . . . . . . . . . . . . . . . . . . . . . 3

1.2 Organizzazione della tesi . . . . . . . . . . . . . . . . . . . . . . 3

2 La tecnologia Lego Mindstorms 4

2.1 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.1.1 I motori . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.1.2 I sensori . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2.1.3 RCX . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.1.4 Le porte di input . . . . . . . . . . . . . . . . . . . . . . 7

2.2 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3 Il veicolo 9

3.1 Descrizione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3.2 Modello del robot . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3.2.1 Curve di raggio assegnato . . . . . . . . . . . . . . . . . 13

3.3 Controllo ad anello aperto . . . . . . . . . . . . . . . . . . . . . 14

3.3.1 Stima della curva dei motori . . . . . . . . . . . . . . . . 14

3.3.2 Raccolta dei dati . . . . . . . . . . . . . . . . . . . . . . 14

3.3.3 Elaborazione dei dati . . . . . . . . . . . . . . . . . . . . 15

3.3.4 Osservazioni . . . . . . . . . . . . . . . . . . . . . . . . . 15

i

INDICE ii

3.4 Controllo in retroazione . . . . . . . . . . . . . . . . . . . . . . 17

3.4.1 Controllore PI incrementale discreto . . . . . . . . . . . 17

3.4.2 Misurazione della velocita delle ruote . . . . . . . . . . . 19

3.4.3 Implementazione del ciclo PI su RCX . . . . . . . . . . . 20

3.4.4 Prestazioni del controllo PI . . . . . . . . . . . . . . . . 21

3.4.5 Conclusioni . . . . . . . . . . . . . . . . . . . . . . . . . 23

4 Il Range finder 24

4.1 Sensore IR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

4.1.1 Circuito di interfacciamento . . . . . . . . . . . . . . . . 25

4.1.2 Stima della risposta del sensore . . . . . . . . . . . . . . 27

4.2 Torretta orientabile . . . . . . . . . . . . . . . . . . . . . . . . . 29

4.3 Utilizzo del range finder . . . . . . . . . . . . . . . . . . . . . . 29

5 Il sistema di supervisione centralizzato 30

5.1 Il sistema di visione . . . . . . . . . . . . . . . . . . . . . . . . . 31

5.2 Elaborazione delle immagini . . . . . . . . . . . . . . . . . . . . 31

5.3 Protocollo di comunicazione . . . . . . . . . . . . . . . . . . . . 33

5.3.1 La struttura del pacchetto . . . . . . . . . . . . . . . . . 33

5.3.2 La libreria di comunicazione . . . . . . . . . . . . . . . . 35

6 Prove sperimentali di moto collettivo 36

6.1 Cenni sulla legge di controllo . . . . . . . . . . . . . . . . . . . . 36

6.1.1 Legge di controllo per un singolo robot . . . . . . . . . . 37

6.1.2 Legge di controllo per il sistema multi-robot . . . . . . . 38

6.1.3 Scelta dei parametri . . . . . . . . . . . . . . . . . . . . 39

6.2 Il setup sperimentale . . . . . . . . . . . . . . . . . . . . . . . . 41

6.3 Esperimenti con un singolo robot . . . . . . . . . . . . . . . . . 42

6.4 Esperimenti di moto collettivo . . . . . . . . . . . . . . . . . . . 43

6.5 Commenti . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

INDICE iii

7 Conclusioni e sviluppi futuri 47

A Dettagli implementativi 49

A.1 Misurazione della velocita delle ruote . . . . . . . . . . . . . . . 49

A.2 Controllore PI incrementale discreto . . . . . . . . . . . . . . . . 52

A.3 Conversione sensore distanza . . . . . . . . . . . . . . . . . . . . 53

B Istruzioni di montaggio 55

Bibliografia 65

Elenco delle figure

2.1 Il microcomputer RCX . . . . . . . . . . . . . . . . . . . . . . . 6

2.2 Porta input e sensore generico . . . . . . . . . . . . . . . . . . . 7

3.1 Uno dei robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3.2 Veicolo schematizzato . . . . . . . . . . . . . . . . . . . . . . . . 11

3.3 Curva di raggio assegnato . . . . . . . . . . . . . . . . . . . . . 13

3.4 Curva dei motori . . . . . . . . . . . . . . . . . . . . . . . . . . 16

3.5 Stima della curva dei motori . . . . . . . . . . . . . . . . . . . . 16

3.6 Anello di controllo . . . . . . . . . . . . . . . . . . . . . . . . . 18

3.7 Finestra mobile . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

3.8 Errore di inseguimento 1 . . . . . . . . . . . . . . . . . . . . . . 21



4.1 Schema elettrico dell’interfaccia . . . . . . . . . . . . . . . . . . 25

4.2 Schema pratico dell’interfaccia . . . . . . . . . . . . . . . . . . . 26

4.3 Risposta del sensore . . . . . . . . . . . . . . . . . . . . . . . . 27

4.4 Risposta linearizzata del sensore . . . . . . . . . . . . . . . . . . 28

5.1 Sistema di supervisione . . . . . . . . . . . . . . . . . . . . . . . 30

5.2 Scenario simulato . . . . . . . . . . . . . . . . . . . . . . . . . . 31

5.3 Elaborazione dello scenario simulato . . . . . . . . . . . . . . . 33

i

ELENCO DELLE FIGURE ii

6.1 Singolo veicolo . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

6.2 Due veicoli . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

6.3 Regione di visibilita . . . . . . . . . . . . . . . . . . . . . . . . . 39

6.4 Esperimenti con un robot . . . . . . . . . . . . . . . . . . . . . 42

6.5 Esperimenti con due robot . . . . . . . . . . . . . . . . . . . . . 43

6.6 Esperimenti con tre robot . . . . . . . . . . . . . . . . . . . . . 44

7.1 Rover costruito con Mindstorms NXT . . . . . . . . . . . . . . . 48

A.1 Misurazione della velocita . . . . . . . . . . . . . . . . . . . . . 51

B.1 Elenco dei pezzi necessari . . . . . . . . . . . . . . . . . . . . . 55

Capitolo 1

Introduzione

Negli ultimi anni si e assistito ad un crescente interesse della ricerca verso la

robotica mobile cooperativa. Gruppi di robot mobili sono creati con l’obiettivo

di studiare problemi come la competizione per l’ottenimento di risorse, l’ori-

gine della cooperazione e organizzazione di gruppo, l’auto apprendimento e il

movimento coordinato. Lo studio di sistemi multi-robot nasce come estensione

dei sistemi a singolo robot, diventando una disciplina indipendente; modellare

e coordinare un tale sistema distribuito e presumibilmente piu complesso ri-

spetto ai tradizionali sistemi distribuiti (come reti di calcolatori o database),

data l’intrinseca interazione fisica tra il sistema e l’ambiente reale. Far coope-

rare piu agenti consente di eseguire compiti che un solo robot non sarebbe in

grado di svolgere da solo.

In generale, un sistema a piu robot mostra un comportamento cooperati-

vo se riesce a portare a termine un compito specifico assegnato, aumentando

l’efficienza totale del sistema, in termini di velocita o qualita.

Alcuni ambiti di ricerca che coinvolgono squadre di robot mobili sono [14]:

• il controllo del traffico e path-planning, in cui piu agenti si muovono in

un ambiente condiviso, evitando di scontrarsi;

1

2

• la manipolazione cooperativa;

• la raccolta di oggetti sparsi, che rievoca operazioni potenzialmente peri-

colose come la bonifica da rifiuti tossici o da esplosivi;

• l’esplorazione dell’ambiente, localizzazione e ricostruzione collettiva di

mappe di luoghi inaccessibili all’uomo;

• i giochi di squadra, che trovano applicazione nel semplice intrattenimento,

come il calcio robotico;

• il problema del moto collettivo coordinato o della geometria di grup-

po, che mira a far raggiungere e mantenere alla squadra una precisa

configurazione geometrica.

Gli aspetti fondamentali dell’architettura di un gruppo di robot mobili sono:

• Controllo: un sistema centralizzato e controllato da un singolo agente (o

supervisore), mentre in un sistema decentralizzato ogni agente e uguale

agli altri dal punto di vista del controllo;

• Differenziazione: il sistema e composto da agenti dotati delle medesime

capacita (sistema omogeneo) oppure no (sistema eterogeneo);

• Comunicazione: determina l’interazione tra gli agenti e puo avvenire

tramite interazione fisica, tramite sensori oppure direttamente tramite

protocolli di comunicazione.

1.1. OBIETTIVI E MOTIVAZIONI 3

1.1 Obiettivi e motivazioni

L’obiettivo del presente lavoro e la realizzazione di una squadra di robot mobili

mediante la tecnologia Lego Mindstorms (LMS). La squadra e stata utilizzata

per testare una strategia di moto collettivo finalizzata al raggiungimento di

una specifica configurazione del sistema multi-robot: a regime, i robot devono

posizionarsi su una circonferenza e ruotare intorno ad un riferimento virtuale

(beacon), cercando di mantenere una distanza costante tra di loro. La legge di

controllo, concepita per una architettura decentralizzata, e implementata su

un calcolatore che supervisiona i robot tramite una telecamera e li controlla a

distanza.

Il supporto Lego Mindstorms e stato scelto per verificare se la legge di

controllo potesse essere validata da un sistema dalle capacita limitate; sfrut-

tare tale supporto ha permesso la costruzione di robot con grande flessibilita

nella progettazione e nella modifica dell’hardware. Inoltre, il basso costo di

ciascun robot rispetto ai modelli commerciali, viene incontro alla prospettiva

di allestire una squadra formata da un elevato numero di agenti.

1.2 Organizzazione della tesi

I capitoli che seguono trattano in dettaglio il processo che ha portato alla

nascita di una squadra di robot mobili. Dopo una breve panoramica delle ca-

ratteristiche hardware e software del sistema Lego Mindstorms (cap. 2), viene

descritto il veicolo dal punto di vista fisico e del controllo in velocita (cap. 3),

mentre il cap. 4 tratta del range finder di bordo. Seguono i capitoli riguar-

danti l’implementazione del sistema di supervisione (cap. 5) e l’allestimento

delle prove sperimentali di laboratorio. Infine, vengono presentati e discussi i

risultati degli esperimenti (cap. 6).

Capitolo 2

La tecnologia Lego Mindstorms

Per realizzare la squadra di robot mobili sono stati utilizzati componenti della

linea Lego Mindstorms [1], il cui set principale e il Robotics Invention System,

che include elementi Lego Technic, motori, sensori e un microcomputer pro-

grammabile, l’RCX; nei paragrafi seguenti vengono descritte le caratteristiche

principali di questa tecnologia.

2.1 Hardware

2.1.1 I motori

Nel set sono presenti dei motori con tensione nominale 9V, dotati di riduzione

interna 1 : 14 del numero di giri e protezione da surriscaldamento [5].

4

2.1. HARDWARE 5

2.1.2 I sensori

Esistono 4 tipi di sensori:

• Sensore di contatto, semplice interruttore a pulsante

• Sensore di luce, misura luce ambientale e riflessa [4]

• Sensore di temperatura, misura da -20◦ a 70◦ C

• Sensore di rotazione incrementale (encoder ottico da 16 tick/giro) [6]

2.1. HARDWARE 6

2.1.3 RCX

L’RCX (Robotic Command eXplorer) e un microcomputer con le seguenti

caratteristiche:

Figura 2.1: Il microcomputer RCX

• Microprocessore Hitachi H8/3292 a 8 bit. Lavora a 16Mhz con 5V di ali-

mentazione; include 8 convertitori A/D a 10 bit (di cui solo 4 utilizzati),

512B di RAM e 16KB di ROM (per il microcodice di boot). Dispone di

due timer a 8 bit, uno a 16 bit e Watchdog timer

• RAM esterna da 32KB, per il firmware e i programmi utente

• 3 porte sensore analogiche a 10 bit (0–5V)

• 3 porte motore pilotate in PWM1, corrente massima erogabile 400mA

• Porta di comunicazione a infrarossi (IR) con portante a 38KHz, da 2400

bit/s

• display LCD a 5 caratteri numerici

• Alimentazione con 6 batterie stilo

1Pulse Width Modulation: modulazione in duty-cycle. La tensione picco-picco e costante

ma, variando il rapporto Ton

Toffdell’onda quadra, si ottiene una VRMS variabile.

2.1. HARDWARE 7

2.1.4 Le porte di input

Figura 2.2: Porta input e sensore generico

Nel sistema Lego Mindstorms, si definisce passivo un sensore che non ne-

cessita di alimentazione (sprovvisto di elettronica di condizionamento) e attivo

un sensore con condizionamento del segnale trasdotto, che necessita di alimen-

tazione. Lo schema interno di una porta di input dell’RCX e di un sensore

generico (fig. 2.2) mostra come sia possibile alimentare i sensori e leggere il

valore misurato con due soli fili. Quando un sensore e impostato come passivo

(sensore di contatto o di temperatura), il transistor rimane sempre interdetto.

Nel caso sia necessario alimentare un sensore attivo, il transistor conduce per

3ms caricando un condensatore interno al sensore, quindi il transistor si inter-

dice per 100µs e viene convertita dall’A/D la caduta di tensione sul resistore

di pull-up, proporzionale alla corrente che scorre sulla resistenza equivalente

offerta dal sensore. Nella fase in cui il transistor e interdetto (che si alterna

ciclicamente con la fase di conduzione) la circuiteria interna del sensore viene

alimentata dal condensatore, per cui e importante limitare l’assorbimento di

corrente in fase di progetto. I diodi presenti nello schema servono a polarizzare

in modo corretto il sensore, comunque venga collegato alla porta di ingresso

dell’RCX.

2.2. SOFTWARE 8

2.2 Software

Per sfruttare al massimo le potenzialita dell’RCX, si e scelto il sistema ope-

rativo BrickOS [9] [13] (ex LegOS), che consente un migliore controllo del-

l’hardware e una maggiore efficienza rispetto al firmware Lego originale (basti

pensare che con il firmware originale il numero di variabili e limitato a 16).

• BrickOS consente di scrivere programmi in C o C++, utilizzando variabili

intere e floating point (in emulazione).

• Rende possibile il multithreading (scheduler Round-Robin a priorita con

prelazione) e la programmazione concorrente.

• Estende a 256 i livelli PWM per il controllo dei motori (contro gli 8 livelli

del firmware originale).

• Permette un controllo di basso livello di ogni segmento del display LCD.

• Consente l’allocazione dinamica della memoria.

• Il protocollo di comunicazione tramite infrarossi, denominato LNP [8]

(LegOS Network Protocol), rende possibile lo scambio di pacchetti che

garantiscono dati privi di errori (integrity layer) o di pacchetti indirizzati

(logical layer).

Capitolo 3

Il veicolo

Per questo progetto sono stati costruiti dei piccoli robot mobili con elementi

Lego Technic; tali robot sono identici tra loro, a parte la differente disposizio-

ne dei LED montati a triangolo isoscele verso l’alto, che servono da marker

al sistema di supervisione (v. cap. 5). In questo capitolo, oltre ai dettagli sul-

l’hardware e sul software, verranno motivate le scelte che hanno portato alla

realizzazione dei robot nella loro forma attuale.

3.1 Descrizione

Il robot ha una cinematica di tipo differential drive con due motori che aziona-

no una ruota ciascuno; una terza ruota passiva libera di sterzare (idler wheel)

fornisce il terzo punto di appoggio (fig. 3.2). Si tratta di un robot anolonomo,

cioe non puo accelerare lungo una direzione arbitraria in modo indipendente

dall’orientazione corrente.

I motori azionano le ruote tramite una riduzione a ingranaggi di 9:1, men-

tre gli encoder ottici, che forniscono una misura della rotazione delle ruote,

sono accoppiati direttamente ai pignoni: poiche gli encoder Lego (v. 2.1.2)

commutano solo 16 volte in un giro, questa configurazione permette di aumen-

9

3.2. MODELLO DEL ROBOT 10

Figura 3.1: Uno dei robot

tare meccanicamente la risoluzione di un fattore 9. Quindi, per ogni conteggio

dell’encoder, la ruota compie 2π9·16 = π

72= 2, 5 gradi ≃ 0, 04363 rad.

La procedura per la costruzione del robot visibile in fig. 3.1 e illustrata

dettagliatamente nell’appendice B.

3.2 Modello del robot

Il robot mobile e modellabile come un uniciclo (fig. 3.2).

x(t) = V (t) cos θ(t)

y(t) = V (t) sin θ(t)

θ(t) = ω(t)

(3.1)

In ogni istante, lo stato del robot e descritto dalle variabili x, y, θ, che rap-

presentano la posizione e l’orientazione del veicolo in un opportuno sistema


V

ω

VRVL

o

drw

RCX

Figura 3.2: Veicolo schematizzato

di riferimento; gli ingressi del sistema sono la velocita tangenziale V (t) e la

velocita angolare ω(t).

Con riferimento alla figura 3.2, siano ωR e ωL le velocita angolari delle

ruote destra e sinistra rispettivamente; sia d la lunghezza del semiasse del

robot; detto rw il raggio delle ruote motrici, si ha:

VR = ωR · rw

VL = ωL · rw

(3.2)


La relazione che lega le velocita del veicolo alle velocita lineari delle ruote

risulta:

V = VR+VL

2

ω = VR−VL

2d

(3.3)

La prima si ricava applicando il principio di sovrapposizione degli effetti,

considerando separatamente i contributi delle ruote.

V |VL=0 =VR

2

V |VR=0 =VL

2

da cui V =VR

2+VL

2=VR + VL

2

La seconda, di conseguenza, si ricava sottraendo la velocita di una ruota la

velocita del centro del robot (si ha la velocita relativa della ruota), e dividendo

per la lunghezza del semiasse.

VR −VR + VL

2=

VR − VL

2VR − VL

2= ω · d

da cui ω =VR − VL

2d

Sostituendo la (3.2) nella (3.3) e risolvendo rispetto a ωR e ωL, si ha:

ωR = V + d·ωrw

ωL = V − d·ωrw

(3.4)

che mostra come calcolare la velocita angolare di ciascuna ruota affinche il

veicolo abbia velocita tangenziale V e angolare ω desiderate.


3.2.1 Curve di raggio assegnato

Come semplice esempio di controllo in velocita, si consideri di voler curvare a

velocita tangenziale V con un raggio ρ arbitrario, come illustrato in fig. 3.3.

Considerando che V = ω · ρ, per compiere una curva di raggio ρ ad una certa

velocita tangenziale V basta impostare ω = Vρ.

Figura 3.3: Curva di raggio assegnato

3.3. CONTROLLO AD ANELLO APERTO 14

3.3 Controllo ad anello aperto

3.3.1 Stima della curva dei motori

L’RCX controlla la velocita dei motori con 255 livelli PWM, di cui quelli utili

sono nell’intervallo [39, 255]: infatti, per livelli bassi, attriti e inerzia vincono la

coppia sviluppata dal motore. Volendo controllare ad anello aperto la occorre

stimare il legame tra il livello PWM e l’effettiva velocita delle ruote.

3.3.2 Raccolta dei dati

Per misurare la velocita angolare delle ruote, e stato montato temporaneamente

un sensore di rotazione accoppiato in rapporto 1:1 con una ruota, collegata al

motore con un fattore di riduzione 3; e stato scritto un programma (eseguibile

su RCX) per automatizzare il piu possibile la raccolta di un numero ingente

di dati. Tale programma:

1. aziona una sola ruota del veicolo, facendolo girare imperniato sull’altra

ruota (questo permette di eseguire l’esperimento in uno spazio limitato),

variando il livello PWM da 39 a 255;

2. per ogni iterazione, misura (in millisecondi) il tempo che occorre alla

ruota per compiere 2 rotazioni complete (grazie al sensore di rotazione);

3. per ogni iterazione, memorizza il tempo misurato in un vettore di interi;

4. alla fine delle 255 − 39 = 216 iterazioni, invia il vettore dei tempi al PC

tramite la porta IR dell’RCX.

Per ricevere i dati tramite la torretta IR Lego e scriverli su un file, sul

PC si e ricorso a un programma scritto appositamente per lo scopo [10], che

comunica con l’RCX mediante il protocollo LNP.


3.3.3 Elaborazione dei dati

I dati sono stati processati in ambiente MATLAB, per effettuare una stima

della relazione esistente tra livello PWM e velocita delle ruote. Disponendo

del tempo t, espresso in ms, occorso alla ruota per compiere 2 giri, la velocita

angolare della ruota dell’ i-esimo livello PWM, espressa in rad/s risulta pari a

ξi =2 · 2π

t1000

=4000π

t

rad

s(3.5)

Le velocita angolari vengono mediate sulle 20 prove per ridurre la varianza

dei valori. In figura 3.4, le curve chiare rappresentano le 20 realizzazioni, mentre

la curva scura mostra l’andamento della velocita media. Per stimare l’equazione

della curva, si parte dall’osservazione del grafico, da cui si intuisce la funzione:

ξ = α−β

xPWM + δ(3.6)

Invertendo la (3.6), si ricava il livello PWM x corrispondente ad una velocita

angolare desiderata ξ:

xPWM =β

α− ξ− δ (3.7)

Minimizzando l’errore quadratico tra i valori misurati e quelli stimati tramite

la (3.6), si ottiene (fig. 3.5):

α = 13, 0464

β = 1050, 6

δ = 43, 9280

3.3.4 Osservazioni

Da prove sperimentali si e riscontrato che l’affidabilita di questo tipo di approc-

cio ad anello aperto e totalmente insoddisfacente. Infatti, anche se alimentati

con la stessa tensione, due motori nominalmente identici hanno velocita diver-

se; inoltre, la velocita varia con l’esaurirsi delle batterie e gli errori numerici

nel calcolo della (3.7) corrompono il risultato. Percio, nel corso del progetto e

stato adottato un piu robusto controllo ad anello chiuso.


0 50 100 150 200 250 3000

2

4

6

8

10

12

PWM power

Sp

ee

d (

rad

/s)

Figura 3.4: Curva dei motori

0 50 100 150 200 250 3000

1

2

3

4

5

6

7

8

9

10

PWM power

Spe

ed (

rad/

s)

Figura 3.5: Stima della curva dei motori

3.4. CONTROLLO IN RETROAZIONE 17

3.4 Controllo in retroazione

Il robot riceve tramite IR (v. 5.3) il comando in velocita V in mm/s e ω in

mrad/s , calcola ωR e ωL in mrad/s e li converte in tick/s del sensore di rota-

zione. Questi due valori sono i riferimenti che il controllore dovra inseguire. Per

avere la garanzia che il robot si muova effettivamente alle velocita impostate,

infatti, e stato implementato sull’RCX un anello di controllo proporzionale in-

tegrale (PI) incrementale [12] per ciascuna ruota. L’azione integrale dovrebbe

garantire un errore nullo a regime , mentre nel paragrafo 3.4.4 si vedra come

questo non sia sempre vero in pratica.

3.4.1 Controllore PI incrementale discreto

Un controllore PI posizionale discreto ha la forma (fig. 3.6):

u(k) = P (k) + I(k) (3.8)

con

P (k) = Kp e(k)

I(k) = I(k − 1) +Kp

Ti

e(k)

e(k) = r(k) − y(k)

(3.9)

dove r(k) e il segnale di riferimento da inseguire, y(k) e l’uscita del sistema,

e(k) l’errore di inseguimento, Kp il guadagno proporzionale e Ti il tempo di

integrazione; questo controllore calcola il nuovo comando u(k) ad ogni passo.

Dovendo implementare questo controllore sull’RCX e dato che il comando viene

saturato tra -255 e 255 (livelli PWM), si e preferita la forma incrementale, che

elimina problemi di saturazione numerica e di wind-up.

Nel controllore PI incrementale, ad ogni passo si calcola la variazione del

comando, che viene poi sommata al valore all’istante precedente, per ottenere


il nuovo valore da attuare:

∆u(k) = ∆P (k) + ∆I(k) (3.10)

dove

∆P (k) = Kp [e(k) − e(k − 1)]

∆I(k) =Kp

Ti

e(k)

e(k) = r(k) − y(k)

(3.11)

I(k)

P(k) u(k)

u(k)

u(k-1)

e(k)

y(k)

255

-255

r(k)

e(k-1)z -1

+ + +

i

KT

p

+

+

+Kp

z -1

Motor + Encoder

Figura 3.6: Anello di controllo

Le due forme sono perfettamente equivalenti, infatti

u(k) = ∆u(k) + u(k − 1)

= ∆P (k) + ∆I(k) + u(k − 1)

= [P (k) − P (k − 1)] + [I(k) − I(k − 1)] + P (k − 1) + I(k − 1)

= P (k) + I(k)

(3.12)


3.4.2 Misurazione della velocita delle ruote

Considerando di voler variare il riferimento r(k) con una certa frequenza, il

controllore PI deve lavorare almeno ad una frequenza 10 volte superiore: di

conseguenza anche la misura dell’uscita y(k) deve essere aggiornata a frequen-

za elevata. Come gia accennato nel par. 3.1, su entrambe le ruote e montato

un encoder ottico incrementale per avere la misura dell’uscita y(k): l’RCX puo

leggere il valore attuale del conteggio e resettarlo da programma. La velocita

in tick/s si puo misurare conoscendo quanti conteggi si sono verificati in un

determinato intervallo di tempo: scegliere un intervallo troppo lungo (> 1s)

comporta una frequenza di aggiornamento dell’errore e(k) troppo bassa, men-

tre un intervallo troppo breve (10ms) non permette misurazioni consistenti di

basse velocita (data la scarsa risoluzione del sensore).

Si puo ovviare al problema tramite una media mobile, come illustrato in

fig. 3.7.

5T 2T

t(s)

6T 2T

t(s)T+1

T T+2 2Tt(s)7

T T+8 2Tt(s)9

Figura 3.7: Finestra mobile

Nell’esempio di fig. 3.7, si suppone che i tick nel primo periodo di durata

T siano 5 e nel successivo 10. Se la misura di velocita fosse effettuata ad ogni

kT (k = 1, 2, . . .), avremmo un salto da 5 a 10 nella lettura all’istante 2T;


invece, utilizzando una finestra mobile, si ha una variazione graduale della

velocita, operando una media mobile. Per i dettagli sull’implementazione in

codice C di questo sistema di misurazione, si consulti l’appendice A.1.

3.4.3 Implementazione del ciclo PI su RCX

Il programma in esecuzione sul robot attende costantemente dei comandi in

velocita rappresentati come interi con segno su 2 Byte (v. 5.3.1). Poiche sia i

controllori PI che il thread di misurazione della velocita angolare delle ruote

(v. A.1) lavorano in tick/s, i comandi ricevuti devono essere convertiti. Vale il

rapportorad/s

tick/s=

2π

9 · 16=

π

72(3.13)

dove compaiono i radianti in un giro al numeratore, il fattore di riduzione a

ingranaggi 9:1 e i 16 tick/giro dell’encoder al denominatore.

Considerando le unita di misura, la (3.4) diventa:

ωRL

=V ± d ω

1000

rw

[

mms

± mmmrads

mm

]

(3.14)

con rw = 40, 8mm, d = 68mm; applicando la conversione (3.13), si ha:

ωRL

[

tick

s

]

=V ± 0, 068ω

40, 8·72

π≃ 0, 559V ± 0, 039ω. (3.15)

La velocita massima di ciascuna ruota risulta ξmax ≃ 3, 2rad/s per cui

Vwmax≃ 130mm/s; i valori massimi delle velocita tangenziale e angolare del

veicolo sono:

V = VR+VL

2= 2Vwmax

2≃ 130 mm/s

ω = VR−VL

2d= Vwmax−(−Vwmax )

2d≃ 1, 9 rad/s

I dettagli sull’implementazione del controllore PI incrementale discreto sono

illustrati nell’appendice A.2.


3.4.4 Prestazioni del controllo PI

Per valutare le prestazioni in termini di tempo di salita e errore a regime del

controllore PI incrementale, sono stati svolti esperimenti pratici con un robot,

facendo partire una ruota da ferma e ponendo come riferimento in ingresso un

gradino (velocita angolare costante): nei grafici che seguono (figg. 3.8−3.10) si

puo osservare l’andamento dell’errore.

0 0.5 1 1.5 2 2.5 3 3.5 4−10

−5

0

5

10

15

20

25Set point: 1 rad/s

Time (s)

Err

or (

tick/

s)

Figura 3.8: Errore di inseguimento 1

Dai grafici si nota che il tempo di salita (entro cui il valore dell’uscita

raggiunge il 90% del riferimento) e circa 0,5 secondi e che l’overshoot tende a

diminuire con l’aumentare dell’ampiezza del riferimento.

Tendenzialmente, l’errore non si annulla mai completamente: le principali

cause di questo problema sono l’errore numerico nel calcolo della (3.15) e il

metodo con cui viene misurata la velocita. Infatti la misura non e continua nel

tempo ma costante a tratti e discreta nei valori assunti, in quanto multiplo


0 0.5 1 1.5 2 2.5 3 3.5 4−20

−10

0

10

20

30

40


Time (s)

Err

or (

tick/

s)


0 0.5 1 1.5 2 2.5 3 3.5 4−10

0

10

20

30

40

50

60


Time (s)

Err

or (

tick/

s)



intero di π72

(cfr. par. 3.1). Si puo valutare l’errore percentuale a regime (dopo

2 secondi), nei tre casi riportati:

1. L’errore e di 1 tick/s

e% =π/72

1· 100 ≃ 4% (3.16)

2. L’errore e di 2 tick/s

e% =2 · π/72

2· 100 ≃ 4% (3.17)

3. L’errore e praticamente nullo

3.4.5 Conclusioni

Perche i robot possano venire impiegati come agenti all’interno di un sistema

sperimentale senza comprometterne la stabilita, devono muoversi con precisio-

ne alle velocita desiderate: e percio stata necessaria l’adozione del controllo in

retroazione, a scapito di una maggiore complessita hardware e maggiore carico

computazionale sull’RCX.

Capitolo 4

Il Range finder

In questo capitolo viene illustrata la progettazione e la realizzazione di un

sensore di tipo range finder per i veicoli descritti nel capitolo 3. Il range finder

di bordo e composto da due parti:

• Il sensore a raggi infrarossi (IR) con l’elettronica di interfacciamento

all’RCX

• La torretta mobile, controllata da un motore e provvista di un encoder

ottico.

4.1 Sensore IR

Il sensore adottato per la costruzione del range finder di bordo e un economico

SHARP GP2Y0A02YK, con un range di distanza che va da 20 a 150cm circa.

Questo sensore misura la distanza di un oggetto posto entro il suo cono visivo,

con una frequenza di 25 Hz circa: in uscita si trova una tensione analogica

continua che varia tra 0, 3V e 2, 4V circa, per la massima e minima distanza

rispettivamente.

24

4.1. SENSORE IR 25

Per la misura della distanza, il GP2Y ha un array di ricevitori che misurano

l’angolo di incidenza formato dal raggio IR emesso e riflesso dall’oggetto. Mag-

giore e la distanza, minore sara tale angolo, e viceversa. Si e reso necessario

un circuito di interfacciamento per il collegamento del sensore GP2Y con una

porta d’ingresso dell’RCX.

4.1.1 Circuito di interfacciamento

L’interfaccia e necessaria in quanto il sensore Sharp deve essere alimentato a

5V stabilizzati, assorbendo in media 30-40 mA: l’alimentazione e fornita da

una pila ricaricabile a 9V. Inoltre, il circuito serve per disaccoppiare l’uscita

del sensore dalla porta di ingresso dell’RCX.

Figura 4.1: Schema elettrico dell’interfaccia

4.1. SENSORE IR 26

La tensione della batteria viene stabilizzata a 5V da IC2, uno stabilizza-

tore di tensione DC a bassa caduta L4931CDT50. Questo integrato, insieme

ai due condensatori, fornisce al sensore una tensione priva di fluttuazioni, che

si ripercuoterebbero sulla misura. L’uscita del sensore non sarebbe in grado di

pilotare un carico come la porta di ingresso dell’RCX (v. par. 2.1.4): percio

viene utilizzato IC1/A (parte di un doppio amplificatore operazionale LM358 )

come inseguitore di tensione, e il transistor BC548 che serve da generatore di

corrente pilotato: senza questo transistor, l’operazionale non potrebbe erogare

una corrente sufficiente, a un livello di tensione di 0, 3V . I 4 diodi 1N4148

sono montati a ponte raddrizzatore: il loro scopo e polarizzare correttamen-

te l’interfaccia in qualunque modo venga collegata alla porta RCX, evitando

malfunzionamenti ed eventuali danneggiamenti.

Il circuito di fig. 4.1 e stato costruito su circuito stampato con componenti

SMD1.

Figura 4.2: Schema pratico dell’interfaccia

1SMD: Surface Mount Devices, componenti miniaturizzati a montaggio superficiale

4.1. SENSORE IR 27

4.1.2 Stima della risposta del sensore

Collegato il sensore all’RCX tramite l’interfaccia, non si dispone ancora di una

lettura tarata in cm; l’RCX provvede alla conversione A/D e fornisce solamente

una indicazione grezza (raw value). Per disporre di una lettura diretta in cm e

necessaria una stima della curva distanza–raw value e una correzione software.

Raccolta dei dati

La curva del sensore e stata ottenuta mediante prove ripetute. Le linee rosse

sono le prove, mentre quella blu e la loro media. La curva e compatibile con

quella riportata nel datasheet del sensore SHARP, tranne che per il fatto che

degenera per distanze inferiori a 40cm (cf. 4.1).

4000 5000 6000 7000 8000 9000 1000020

40

60

80

100

120

140

160

RCX raw value

dist

ance

(cm

)

Figura 4.3: Risposta del sensore

4.1. SENSORE IR 28

Elaborazione dei dati

Non considerando il tratto degenere, la curva (4.3) ha un andamento di propor-

zionalita inversa. Come suggerito nei datasheet dei sensori Sharp della serie

GP2, tracciando un grafico κd

su v (con κ costante arbitraria), si ottiene una

curva ben approssimabile con una retta di equazione

κ

d= αv + β α, β ∈ R (4.1)

da cui si ottiene

d =κ

αv + βκ = 10000 (4.2)

La stima dei parametri α e β fornisce i valori α = 0, 0294 e β = −50, 4845.

Nell’appendice A.3 si trova il codice che calcola la (4.2) tramite aritmetica

intera.

4000 5000 6000 7000 8000 900060

80

100

120

140

160

180

200

220

240

RCX raw value

1000

0/di

stan

ce (

cm)

Figura 4.4: Risposta linearizzata del sensore

4.2. TORRETTA ORIENTABILE 29

4.2 Torretta orientabile

Il sensore e montato su una torretta mobile, il cui asse di rotazione e ortogonale

al piano del veicolo e incidente con il semiasse delle ruote, cosı che tale asse

non abbia movimenti eccentrici quando il veicolo gira su se stesso.

La torretta e azionata dal terzo motore del veicolo, e controllata in posizione

da un sensore di rotazione (encoder ottico da 16 counts/giro). Data la scarsa

risoluzione dell’encoder, e necessario che il rapporto tra il numero di giri della

torretta rispetto all’asse del sensore sia ≪ 1: nel caso di un accoppiamento vite

senza fine–ruota a 8 denti, ntur

nsens= 1

8.

Per un giro di 360 gradi della torretta, il sensore fornisce 128 conteggi(16·8).

Quindi,

x =360

128≃ 2, 813 gradi per conteggio (4.3)

dove x rappresenta la risoluzione. La (4.3) viene utilizzata nella libreria di

gestione della torretta per orientarla di un angolo arbitrario.

4.3 Utilizzo del range finder

In questo lavoro il range finder non e stato effettivamente installato sui veicoli

ne utilizzato negli esperimenti: infatti, la visione dei robot e stata simulata per

mezzo di un sistema di supervisione centralizzato (v. fig. 6.3). Nella prospettiva

di voler adottare una strategia di controllo decentralizzata, questi sensori pos-

sono risultare utili per stimare approssimativamente la distanza da altri robot

o la presenza di eventuali ostacoli; le capacita del sensore SHARP (v. par. 4.1)

sono ovviamente inferiori a quelle di un laser range finder e non permettono

di distinguere forme ne distanze angolari tra punti precisi.

Capitolo 5

Il sistema di supervisione

centralizzato

In questo capitolo viene descritto un sistema di supervisione centralizzato che

consente di testare varie strategie di controllo sulla squadra di robot mobili.

Image Processing software

Control law

USB

Interfacing

DLL

Figura 5.1: Sistema di supervisione

30

5.1. IL SISTEMA DI VISIONE 31

5.1 Il sistema di visione

Per avere un feedback sui movimenti dei veicoli viene utilizzata una webcam

fissata sul soffitto, che inquadra un’area di circa 2, 4 × 3, 4m. I robot sono

individuati in posizione, orientazione e univoca identita per mezzo di led rivolti

verso l’alto, montati a triangolo isoscele. L’acquisizione delle immagini, la stima

di distanze ed angoli, l’implementazione della legge di controllo sono effettuate

in ambiente MATLAB ed i comandi sono inviati ai robot tramite il protocollo

LNP.

5.2 Elaborazione delle immagini

L’elaborazione delle immagini puo essere schematizzata nel modo seguente:

Figura 5.2: Scenario simulato

1. Il fotogramma viene catturato e salvato in una matrice RGB ∈ Rn×m×3,

dove n ×m e la risoluzione della webcam in pixel e 3 sono i piani Red,

Green e Blue. I tre piani di colore vengono divisi in tre matrici, a loro

volta filtrate per mezzo di una soglia di luminosita: si ottengono matrici

5.2. ELABORAZIONE DELLE IMMAGINI 32

sparse costituite da zeri (aree nere) con gruppi di uno (punti luminosi).

I tre piani filtrati vengono riuniti con un and logico bit a bit per estrarre

i soli punti luminosi comuni, che rappresentano i marker da individuare.

2. L’algoritmo bwboundaries dell’Image Processing Toolbox estrae i con-

torni, e determina la posizione (in pixel) dei centroidi delle aree lumino-

se. Verrano considerati validi solo i centroidi delle aree rotondeggianti,

corrispondenti ai veri e propri marker, filtrando cosı eventuali disturbi.

3. I marker vengono raggruppati tre a tre in base alla prossimita: questo

implica che la distanza minima tra due robot dovra essere sempre stret-

tamente maggiore della distanza massima tra due marker dello stesso

robot.

4. Una volta raggruppati i marker, vengono distinti i vertici di base da

quello dell’altezza, considerando che la base deve sempre essere il lato piu

corto del triangolo. La posizione del robot viene stimata come media delle

coordinate dei tre punti luminosi; l’orientazione come angolo (θ ∈ [0, 2π))

positivo in senso orario tra l’asse x e la retta passante per l’altezza del

triangolo.

5. L’identita dei robot e determinata a priori dalla distanza del led di altezza

da quelli che costituiscono la base: il triangolo con altezza minima sara

il numero 1, e cosı via fino all’N-esimo robot.

6. Vengono stimate tutte le distanze e gli angoli tra i robot e un riferimento

virtuale; l’area visiva di ogni robot, che dovrebbe essere determinata dai

sensori di bordo, viene simulata via software.

5.3. PROTOCOLLO DI COMUNICAZIONE 33

Acquired scenario

45°

136° 237°

306°

4

3 2

1

X

Y

Figura 5.3: Elaborazione dello scenario simulato

5.3 Protocollo di comunicazione

I comandi in velocita vengono convertiti opportunamente prima di essere tra-

smessi via IR ai robot, che possono ricevere messaggi strutturati secondo il

protocollo LNP [8]. Il computer invia, tramite la torre IR Lego, un messaggio

che include i comandi per tutti i robot: per far sı che ogni agente riceva il

proprio comando, scartando i dati degli altri, e necessaria una struttura parti-

colare. Per consentire l’invio tramite MATLAB di pacchetti LNP e stata scritta

appositamente una MEX DLL1.

5.3.1 La struttura del pacchetto

L’elaborazione in ambiente MATLAB produce dei risultati in virgola mobile

rappresentati su 32 bit, che devono essere opportunamente convertiti per essere

1DLL: Dynamic Linked Library


inviati ai robot. In particolare, la velocita lineare e espressa in mm/s e, per

le caratteristiche meccaniche del robot, sara −130 < V < 130 e la velocita

angolare, espressa in mrad/s sara −1900 < ω < 1900. Tali unita di misura

sono state scelte perche consentono una discreta granularita dei valori espressi

come numeri interi.

Il protocollo LNP trasmette dati del tipo unsigned char, che occupano

2 Byte ciascuno: quindi possono essere rappresentati numeri interi n tali che

0 6 n 6 65535. Il metodo piu semplice per trasmettere i numeri negativi e

sommare al comando (arrotondato a intero) un numero che garantisca sempre

la positivita del risultato (ad es. 0x4000); al momento della ricezione, l’offset

introdotto viene annullato (sottraendo ad es. 0x4000) e il numero con segno

utilizzato come riferimento del controllore PI a bordo (v. 3.4).

Per quanto riguarda la struttura di basso livello del pacchetto, sono date

le condizioni:

• N robot ricevono il messaggio;

• ogni robot e abilitato a ricevere k dati;

• l’RCX gestisce numeri rappresentati su 2 Byte;

• la dimensione massima di un pacchetto LNP e 125 Byte;

• il messaggio va inviato un Byte alla volta;

per cui il messaggio ha la forma (si ringrazia Andreas Dreier):

D1,1,LSB|D1,1,MSB | . . . |D1,k,LSB|D1,k,MSB| . . . |DN,1,LSB|DN,1,MSB| . . . |DN,k,LSB|DN,k,MSB

dove Di,j,LSB rappresenta il Byte meno significativo del dato i-esimo che rice-

vera il robot j-esimo e Di,j,MSB rappresenta il Byte piu significativo: il robot


j-esimo leggera dal messaggio solo i Byte che lo riguardano, riunendoli a forma-

re dati a 16 bit. In particolare, il messaggio per comandare 4 robot in velocita

ha la forma:

0xF0|V1,LSB|V1,MSB |ω1,LSB|ω1,MSB| . . . |V4,LSB|V4,MSB |ω4,LSB|ω4,MSB|checksum

5.3.2 La libreria di comunicazione

MATLAB consente di chiamare, dall’interno di file M, funzioni incluse in libre-

rie dinamiche scritte e in C/C++, ADA o FORTRAN. La DLL MATLAB_LNP,

scritta appositamente per lo scopo, permette al programma di elaborazione

delle immagini di inviare un pacchetto LNP (strutturato come in 5.3.1) a N

robot contemporaneamente. La DLL crea un handle per la porta USB a cui

e collegata la torre a infrarossi Lego, assembla il pacchetto LNP aggiungendo

un header e un checksum al messaggio (passato da MATLAB come stringa),

invia i dati e distrugge l’handle; eventuali errori di trasmissione o di apertura

della porta USB sono comunicati sotto forma di errori a riga di comando.

Capitolo 6

Prove sperimentali di moto

collettivo

In questo capitolo vengono presentate alcune prove sperimentali effettuate sulla

squadra di veicoli descritta nei capitoli precedenti, al fine di testare una legge

di controllo, presentata in [3]: in tale documento, sono descritti i risultati di

simulazioni in ambiente MATLAB e di esperimenti svolti con un gruppo di

due robot mobili commerciali.

6.1 Cenni sulla legge di controllo

Come gia accennato nel paragrafo 1.1, gli esperimenti sono mirati a testare

una legge di controllo di moto collettivo: l’obiettivo e quello di far sı che, a

regime, i robot si muovano in senso antiorario su una circonferenza intorno ad

un riferimento virtuale fisso (beacon), a distanza costante l’uno dall’altro. Di

seguito si riporta e si discute brevemente la legge di controllo per un singolo

robot, per poi estenderla al caso multi-robot.

36

6.1. CENNI SULLA LEGGE DI CONTROLLO 37

6.1.1 Legge di controllo per un singolo robot

γ

ρy

x

θ

Figura 6.1: Singolo veicolo

Con riferimento al modello cinematico (3.1), l’ingresso di comando u(t) e

dato dalla velocita angolare ω, mentre la velocita lineare V viene mantenuta

costante. Si ha:

u(t) =

k · g(ρ(t)) · αdist(γ(t)) se ρ(t) > 0

0 se ρ(t) = 0(6.1)

con

g(ρ) = ln

(

(c− 1) · ρ+ ρ0

c · ρ0

)

(6.2)

e

αdist(γ) =

γ se 0 ≤ γ ≤ ψ

γ − 2π se ψ < γ < 2π. (6.3)

Nelle (6.1)-(6.3), ρ e la distanza tra la posizione del veicolo e il riferimento;

γ ∈ [0, 2π) rappresenta l’angolo tra la direzione del veicolo e della congiungente

veicolo-riferimento; k > 0, c > 1, ρ0 > 0 e ψ ∈ (32π, 2π) sono parametri del

controllore.

Il termine g(ρ) fa sı che la legge di controllo sterzi il veicolo verso il beacon se

ρ ≥ ρ0 e lo allontani se ρ ≤ ρ0. Il termine αdist serve a privilegiare la rotazione

antioraria rispetto a quella oraria; la soglia ψ evita che, se ρ e grande e γ e

vicino a 2π, il veicolo compia giri inutili invece di andare diritto verso il beacon.


6.1.2 Legge di controllo per il sistema multi-robot

γi

γj

γij

γji

ρi

ρj

ρij

y

x

Figura 6.2: Due veicoli

Nel caso di piu agenti, l’ingresso ui(t) del veicolo i-esimo ha un termine

additivo che tiene conto dell’interazione tra i robot:

ui(t) = fib(ρi, γi) +∑

j 6= i

j ∈ Ni

fij(ρij , γij) (6.4)

Nella (6.4), fib e come in (6.1), ovvero

fib =

kb · g(ρi, cb, ρ0) · αdist(γi) se ρi > 0

0 se ρi = 0(6.5)

mentre

fij = kv · g(ρij, cv, d0) · βdist(γij), (6.6)

dove g(ρ, c, ρ0) e uguale a g(ρ) della (6.2), kv > 0, cv > 1, d0 > 0 e

βdist(γij) =

γij se 0 ≤ γij ≤ π

γij − 2π se π < γij < 2π.(6.7)


L’insieme Ni contiene gli indici dei veicoli che cadono nella regione di vi-

sibilita del veicolo i-esimo (v. fig. 6.3) e che quindi danno contributo nella

legge di controllo. Tale regione di visibilita, che dovrebbe essere determinata

dai sensori di bordo, viene simulata via software: ogni robot vede oggetti entro

un settore circolare di raggio dl e ampio 2αv e intorno a se entro una regione

circolare di raggio ds (sensore di prossimita).

i

j

dl

d0

ds

2αv

Figura 6.3: Regione di visibilita

Ogni agente e guidato dal termine fib verso il movimento circolare antiorario

intorno al beacon, mentre il termine fij privilegia traiettorie prive di collisioni,

cercando di mantenere ρij = d0 per tutti gli agenti j ∈ Ni. Qualitativamente,

il veicolo i e attratto da ogni veicolo j ∈ Ni se ρij > d0 e respinto se ρij < d0.

Per maggiori dettagli, cfr. [3].

6.1.3 Scelta dei parametri

Perche la legge di controllo garantisca effettivamente il raggiungimento del

moto circolare, devono essere scelte opportunamente alcune costanti per il

caso di un robot singolo e altre per il caso multi-veicolo. Per quanto riguarda


il singolo robot i parametri V , kb, ρ0, cb devono essere scelti in modo che

minρρ g(ρ) > −

2V

3πkb

(6.8)

Questo garantisce l’esistenza di un punto di equilibrio per il sistema, per cui

ρe g(ρe) =2

π

V

kb

(6.9)

Impostati i parametri per rispettare l’ipotesi (6.8) e risolvendo per via numerica

la (6.9) rispetto a ρe, si ottiene il raggio della circonferenza intorno alla quale

il robot ruotera all’equilibrio.

Nel caso generale con n veicoli, bisogna considerare un’ulteriore relazione

che lega il raggio ρe al numero n di robot, alla distanza d0 che questi dovranno

mantenere tra di loro e al range di visibilita dl. Infatti, per fare in modo che

gli n veicoli possano disporsi sulla circonferenza desiderata occorre che:

(n− 1) arcsind0

2ρe

+ ϕ < π (6.10)

con

ϕ = min

{

αv, arcsin

(

min{dl, 2ρe}

2ρe

)}

Scegliendo ds < d0 e αv < π/2, si ha all’equilibrio card(Ni) ∈ {0, 1}, ovve-

ro ciascun veicolo deve poter vedere al massimo un solo veicolo, una volta

raggiunta la configurazione di rotazione desiderata. Cio impone un ulteriore

vincolo sulla regione di visibilita:

arcsin

(

min{dl, 2ρe}

2ρe

)

< 2 arcsin

(

d0

2ρe

)

. (6.11)

Una possibile scelta per i parametri e la seguente:

• per le (6.1)-(6.3), relative al comportamento beacon-robot :

kb = 0.12, cb = 4, ρ0 = 0.4m, ψ = 2918π;

• per la (6.6) e le (6.8)-(6.9), relative all’interazione tra veicoli:

kv = 0.2, cv = 4, d0 = 0.5m, V = 0.06m/s;

6.2. IL SETUP SPERIMENTALE 41

• per le (6.10) e (6.11), relative alla regione di visibilita :

αv = 1736π, dl = 0.9m, ds = 0.45m,

da cui risulta ρe ≃ 0.7m.

Rispetto al caso puramente teorico, nello scegliere i parametri bisogna con-

siderare i vincoli imposti dalle dimensioni dei robot e dell’arena. In particolare,

per la scelta di ds si deve tenere conto della necessita di distinguere i robot

mediante i marker visivi (v. 5.2, punto 3); si deve inoltre dimensionare kb e V

in modo tale che una circonferenza di raggio ρe possa essere contenuta entro

l’area ripresa dalla telecamera.

6.2 Il setup sperimentale

I robot vengono posizionati all’interno dell’arena (v. 5.1), inquadrata da una

telecamera posta sul soffitto. L’area e priva di ostacoli e recintata tramite dei

pannelli; questi, oltre a proteggere l’esperimento in corso, riflettono i raggi in-

frarossi emessi dalla torretta USB Lego, permettendo una copertura adeguata

di tutta l’area utile. Su ogni robot viene eseguito il programma per la ricezione

dei messaggi che accende i LED per il sistema di visione; per ogni robot deve

essere impostato il numero identificativo, che deve essere coerente con la di-

sposizione dei LED (v. 5.2, punto 5). I robot sono fermi fino a che non ricevono

un messaggio dal supervisore, per poi muoversi alle velocita desiderate control-

lati in retroazione; inoltre, un timeout di sicurezza azzera automaticamente la

velocita se il comando non viene aggiornato entro un termine fissato. Questo

accorgimento evita di dover arrestare i robot esplicitamente, ed e utile nel caso

di errori o instabilita del software di supervisione. Nei paragrafi che seguono

saranno presentati e commentati i risultati degli esperimenti.

6.3. ESPERIMENTI CON UN SINGOLO ROBOT 42

6.3 Esperimenti con un singolo robot

Le traiettorie e le forme dei robot sono state acquisite mediante la telecamera

di supervisione; il cerchio chiaro e la circonferenza di raggio ρe centrata nel

beacon, sulla quale si desidera che il robot si muova. Il triangolo pieno rap-

presenta la posizione finale del robot, l’altro triangolo la posizione iniziale. In

figura 6.4 si osserva il comportamento di un robot guidato dalla legge (6.1),

variando le condizioni iniziali ρ(0) e γ(0):

(a) (b)

(c) (d)

Figura 6.4: Esperimenti con un robot

6.4. ESPERIMENTI DI MOTO COLLETTIVO 43

• in fig. 6.4(a) γ(0) ≃ 74π: il veicolo tende inizialmente a muoversi in senso

orario ma non appena si avvicina al beacon, γ diventa minore della soglia

ψ e il veicolo finisce per girare in senso antiorario;

• in fig. 6.4(b) γ(0) = 0: il robot procede diritto verso il beacon e dopo

averlo superato, tende al movimento circolare antiorario;

• in fig. 6.4(c) γ(0) ≃ π4: il veicolo inizia subito a ruotare intorno al beacon;

• in fig. 6.4(d) γ(0) = π: e il caso simmetrico rispetto a (b).

6.4 Esperimenti di moto collettivo

In figura 6.5 si osserva il comportamento di due robot guidati dalla legge (6.4):

2

1 2

1

2

1

(a) (b)

(c)

Figura 6.5: Esperimenti con due robot

6.4. ESPERIMENTI DI MOTO COLLETTIVO 44

• in fig. 6.5(a) i robot sono posti inizialmente in modo che il robot 1 veda

il 2 e procedono intorno al beacon in gruppo cercando di mantenersi a

distanza d0: le anse compiute dai robot sono dovute al contributo della

legge interveicolare per evitare collisioni;

• in fig. 6.5(b) i robot hanno una posizione iniziale tale da non entrare

mai l’uno nel campo visivo dell’altro, e quindi procedono formando due

gruppi divisi di un robot ciascuno;

• in fig. 6.5(c) i robot tendono inizialmente l’uno verso l’altro e il 2, entrato

nel campo visivo di 1, viene inseguito.

3

2

1

3

2

1

1

2

3

(a) (b)

(c)

Figura 6.6: Esperimenti con tre robot

6.5. COMMENTI 45

In figura 6.6 si osserva il comportamento di una squadra di tre robot:

• in fig. 6.6(a) i robot sono posti inizialmente in modo che 2 veda 1, e 3

non veda gli altri: dopo pochi secondi, 2 entra nel campo visivo di 3, e si

forma un gruppo di tre robot;

• in fig. 6.6(b) il robot 3, pur essendo piu lontano da 2 che da 1, viene

agganciato da 2 e si formano due gruppi: uno formato dai robot 2 e 3,

l’altro costituito dal solo robot 1;

• in fig. 6.6(c) i robot, dopo aver curvato per tendere al movimento cir-

colare, si uniscono in un solo gruppo per ruotare intorno al beacon. Il

caso di tre robot che procedono senza vedersi e difficilmente ottenibile:

infatti il raggio ρe e piccolo rispetto alle dimensioni dei robot, a causa

dello spazio operativo limitato dall’angolo visivo della webcam.

6.5 Commenti

I risultati sperimentali si discostano notevolmente da quanto predetto dalla

teoria e dalle simulazioni, le quali mostrano che la configurazione di moto col-

lettivo circolare viene sempre raggiunta, dopo un transitorio di durata variabile

(v. [3]). La durata degli esperimenti e stata sufficiente perche il sistema andasse

a regime, per cui la forma non circolare delle traiettorie non e da imputarsi

al fatto di non aver atteso la fine del transitorio. I limiti di questo sistema

sono principalmente dovuti a tre fattori: la bassa frequenza di campionamento

e di aggiornamento del comando da parte del supervisore, gli errori numerici

commessi dall’RCX nella conversione delle unita di misura delle velocita, e la

scarsa risoluzione degli encoder sulle ruote.

Tali problemi possono essere in parte risolti: mentre il software di acquisi-

zione e controllo del supervisore puo essere ottimizzato, diminuendone il tempo

6.5. COMMENTI 46

di esecuzione, ci si deve limitare a prendere atto degli errori numerici commes-

si dal processore dell’RCX. Per quanto riguarda i sensori, ricordando quanto

detto nel paragrafo 3.4.3, e generalizzando la (3.13), si ottiene l’espressione

della minima velocita angolare misurabile:

rad/s

tick/s=

2π

Kg ·Ne

(6.12)

dove Ne e il numero di tick/giro dell’encoder e Kg e il rapporto di trasmissione

tra la ruota e l’encoder stesso. Nel caso in cui Ne = 16 e Kg = 9, un errore di

velocita inferiore a π72

non e apprezzabile e nemmeno correggibile (v. par. 3.4.4).

E chiaro che, supponendo di avere un controllore perfetto, all’aumentare della

risoluzione degli encoder e del rapporto di riduzione degli ingranaggi, l’errore di

inseguimento della velocita a regime diviene trascurabile. Dato che non e possi-

bile aumentare la risoluzione degli encoder Lego, si puo aumentare il rapporto

Kg, portandolo ad esempio, da 9 a 15: la minima velocita apprezzabile sara di

π120

rad/s a scapito delle velocita massime raggiungibili, Vmax ≃ 78 mm/s e

ωmax ≃ 1, 15 rad/s.

Nonostante i limiti evidenziati, la squadra di robot mobili realizzata ha per-

messo di testare in modo soddisfacente la legge di controllo di moto collettivo

con un numero di agenti superiore rispetto alle prove descritte in [3].

Capitolo 7

Conclusioni e sviluppi futuri

In questo lavoro di tesi e stato descritto il processo di progettazione e sviluppo

di una squadra di robot mobili costruiti con i Lego Mindstorms, e il loro impiego

in esperimenti di moto collettivo coordinato; inoltre e stato presentato un

sensore di tipo range finder da installare sui veicoli e un sistema di supervisione

centralizzato per il controllo della squadra durante gli esperimenti.

I possibili sviluppi di questo lavoro prevedono:

• il miglioramento della struttura hardware dei veicoli per avere una mag-

giore precisione nel controllo in velocita;

• l’ottimizzazione del software di supervisione per aumentare la frequenza

di campionamento del sistema;

• la decentralizzazione parziale della legge di controllo presentata mediante

l’utilizzo effettivo dei range finder a bordo, per rilevare la presenza di

oggetti nel campo visivo dei robot;

• l’applicazione della squadra in esperimenti di moto circolare collettivo

con tracking di un beacon in movimento;

47

48

• infine, l’utilizzo della squadra per testare altre leggi di controllo, variando

il software del supervisore.

Al momento della redazione di questo lavoro, stanno per essere lanciati sul

mercato nuovi componenti Lego per la robotica, della linea Mindstorms NXT.

Il nuovo controllore a 32 bit, la comunicazione Bluetooth e i servomotori con

encoder da 360 tick/giro e controllo di velocita PID integrato, potrebbero esse-

re la base per le prossime squadre di robot mobili basate su Lego Mindstorms.

Figura 7.1: Rover costruito con Mindstorms NXT

Appendice A

Dettagli implementativi

In questa appendice sono riportati e commentati alcuni stralci del codice C del

programma dei robot, relativi alla misurazione della velocita delle ruote tramite

media mobile, all’implementazione del controllore PI incrementale discreto, e

al calcolo della relazione uscita–distanza del sensore SHARP.

A.1 Misurazione della velocita delle ruote

Nel codice seguente, si assume per semplicita di misurare la velocita ogni 100ms

con una finestra mobile della durata di 1 secondo, mentre nel programma

effettivamente utilizzato la misurazione della velocita avviene ad una frequenza

di circa 40Hz.

49

A.1. MISURAZIONE DELLA VELOCITA DELLE RUOTE 50

//global variables

int speed_mis = 0; [...]

//speed in ticks per second (shifting window)

void angSpeed_t(){

int Oldticks;

int mem[10]={0};

short int j = 0;

while(true){

Oldticks = mem[j]; //backup old value

mem[j] = sens; //refresh with new value

speed_mis = (mem[j]-Oldticks); //Delta ticks

j = (j+1)%10; //shift pointer along array

msleep(100); // work at 10 Hz

}

}

Il thread angSpeed_t e in esecuzione ad alta priorita insieme al ciclo di control-

lo PI, e aggiorna la variabile globale speed_mis alla frequenza di circa 10Hz;

nonostante questa frequenza di aggiornamento, la velocita e misurata contando

il numero di tick in un intervallo di un secondo. La variabile Oldticks contiene

ad ogni passo il valore del contatore di 10 istanti di campionamento prima, cioe

di un secondo prima (10 × 100ms = 1s). Il vettore mem[10] rappresenta una

memoria dei conteggi passati ampia 1 secondo; l’indice j percorre il vettore

ciclicamente. Il ciclo di funzionamento puo essere cosı riassunto (cfr. fig. A.1):

1. Durante i primi 10 passi dall’avvio del programma, la memoria inizial-

mente piena di zeri si riempie; i valori di velocita misurati non sono cor-

retti, in quanto la differenza speed_mis=(mem[j]-Oldticks) e calcolata

tra valori istantanei di posizione e 0 (Oldticks assume valori nulli dalla

memoria). Dopo 10 passi la memoria e piena dei conteggi del sensore

campionati negli istanti precedenti.

A.1. MISURAZIONE DELLA VELOCITA DELLE RUOTE 51

s(0.0)

0

s(0.1) s(0.2) s(0.3) s(0.4) s(0.5) s(0.6) s(0.7) s(0.8) s(0.9)mem

t = 0.9s

J(1)Oldticks

s(1.0) s(1.1) s(1.2) s(0.3) s(0.4) s(0.5) s(0.6) s(0.7) s(0.8) s(0.9)mem

t = 1.3s

J(2)

s(0.3)Oldticks

s(1.0) s(1.1) s(1.2) s(1.3) s(0.4) s(0.5) s(0.6) s(0.7) s(0.8) s(0.9)mem

t = 1.3s

J(3)

senss(0.3)Oldticks

s(1.0) s(1.1) s(1.2) s(1.3) s(0.4) s(0.5) s(0.6) s(0.7) s(0.8) s(0.9)mem

t = 1.3s

J

(4)

s(0.3)Oldticks

speed = mem[J] - Oldticks

(5)

Figura A.1: Misurazione della velocita

2. Il valore nella memoria circolare puntato da j (aggiornato 10 passi prima)

viene salvato temporaneamente in Oldticks;

3. il valore in mem[j] viene sovrascritto da un nuovo campionamento del

sensore;

4. la velocita viene calcolata come differenza di conteggi tra l’istante attuale

e quello di 1 secondo prima speed_mis=(mem[j]-Oldticks);

5. j viene aggiornato per puntare alla casella di memoria successiva (se

raggiunge la fine viene riportato all’inizio) e dopo 0,1 s il ciclo ricomincia.

A.2. CONTROLLORE PI INCREMENTALE DISCRETO 52

A.2 Controllore PI incrementale discreto

Il calcolo della (3.15) viene effettuato dall’RCX mediante l’emulazione dei nu-

meri in virgola mobile non comporta un percettibile calo di prestazioni (in

alternativa, il calcolo puo essere svolto con l’aritmetica intera, come in A.3). Il

seguente codice costituisce il ciclo di controllo vero e proprio, dove speed_ref e

il risultato di (3.15), speed_mis contiene il valore dell’effettiva velocita angola-

re della ruota misurato dall’encoder (v. 3.4.2) e Fc e una costante di programma

che rappresenta la frequenza di campionamento del controllore.

//PI control loop

while(1){

err = speed_ref - speed_mis; //error

DeltaP = Kp*(err - eOld); //proportional incremental control

eOld = err; //refresh error

DeltaI = (Kp*err)/Ti; //integrative incremental control

u = DeltaP + DeltaI + uOld; //PI incremental control

if(u > Hsat) u = Hsat; //hi saturation

if(u < Lsat) u = Lsat; //lo saturation

uOld = u; //refresh command

command(u); //motor actuation

msleep(1000/Fc); //loop works at Fc

}

A.3. CONVERSIONE SENSORE DISTANZA 53

A.3 Conversione sensore distanza

La (4.2) puo essere implementata in BrickOS utilizzando numeri floating point

in emulazione: questo aumenta il carico computazionale, per cui si ricorre

all’aritmetica intera1. Si riporta il codice della libreria che gestisce il sensore.

#define A_lin 2

#define B_lin 9

#define C_lin -50

#define D_lin 0

#define averaging 1

#define reading sensor>>2

#define acq_time 40

unsigned int GP2Y (unsigned int sensor){

int i;

unsigned int temp,average;

unsigned int sample;

ds_passive(&sensor);

average=reading;

for (i=0;i<averaging;i++)

{

average += reading;

average /=2;

msleep(acq_time);

}

temp= A_lin*(average/2)+B_lin*(average/20);

sample=10000/(temp/50+C_lin)+D_lin;

return sample;

}

Le letture del sensore possono essere mediate, rallentando, pero, il processo

di acquisizione (cf. 4.1); le costanti Alin, Blin, Clin e Dlin definite in testa

al codice sono α e β della (4.2), trasformate per i calcoli interi; temp e una

1le divisioni troncano il quoziente per difetto

A.3. CONVERSIONE SENSORE DISTANZA 54

variabile temporanea.

temp = Alin

average

2+Blin

average

20(A.1)

sample =10000

temp

50+ Clin

+Dlin (A.2)

Sostituendo la (A.1) nella (A.2), con Alin = 2, Blin = 9, Clin = −50, Dlin = 0,

si ottiene:

sample =10000

Alinaverage

2+Blin

average

20

50+ Clin

=10000

Alinaverage

100+Blin

average

1000+ Clin

=10000

2average

100+ 9average

1000− 50

=10000

0, 02average+ 0, 009average− 50

=10000

0, 029average− 50(A.3)

Notare che la (A.3) e proprio la (4.2), con le costanti approssimate e con

sample = d e average = v. I valori delle variabili devono sempre essere inter-

ni al campo di rappresentazione2; visti gli elevati valori raw in gioco — per

altro gia scalati per 4 con il bit shifting(sensor >> 2)— e diverso calcolare

Blin(average

20) e (Blin· average)

20. Nel secondo caso, infatti, calcolando Blin · average,

si rischia l’overflow (per interi senza segno, il valore massimo rappresentabile

e 65535).

2Su n bit: variabili con segno∈ [−2n−1, 2n−1 − 1], variabili senza segno∈ [0, 2n − 1].

Su 16 bit, con segno∈ [−215, 215 − 1], senza segno∈ [0, 215 − 1].

Appendice B

Istruzioni di montaggio

Figura B.1: Elenco dei pezzi necessari

55

Bibliografia

[1] Lego Mindstorms homepage. http://www.mindstorms.com.

[2] Lugnet (Lego Users Group Network), robotics forum.

http://www.lugnet.com/robotics/.

[3] Nicola Ceccarelli. Collective Motion of Multi-Vehicle Systems. PhD thesis,

Universita degli Studi di Siena, February 2005.

[4] Michael Gasperi. Lego light sensor internals. Technical report.

http://personal.pitnet.net/usr/gasperi/light.htm.

[5] Philippe Hurbain. Lego motors compared characteristics. Technical

report. http://www.philohome.com/motors/motorcomp.htm.

[6] Philippe Hurbain. Lego rotation sensor internals. Technical report.

http://www.philohome.com/sensors/legorot.htm.

[7] Liqiang Feng, Johann Borenstein, H.R. Everett. Navigating Mobile

Robots: systems and techniques.

[8] Markus L. Noga. LNP, LegOs Network Protocol.

http://legos.sourceforge.net/HOWTO/x405.html.

[9] Markus L. Noga. Brickos. http://brickos.sourceforge.net/.

65

BIBLIOGRAFIA 66

[10] Pavel Petrovic. LNP communication. http://www.robotika.sk/,

http://www.idi.ntnu.no/grupper/ai/eval.

[11] Kekoa Proudfoot. RCX internals. Technical report.

http://graphics.stanford.edu/ kekoa/rcx/.

[12] K.J.Astrom, T.Hagglund. PID Controllers: theory, design, and tuning.

1995.

[13] Luis Villa. Legos howto. http://legos.sourceforge.net/HOWTO/.

[14] A.B. Kahng, Y.U. Cao, A.S. Fukunaga. Cooperative Mobile Robotics:

Antecedents and Directions. Autonomous Robots, 1997.

Universita degli Studi di Siena` - Danny's LAB · rativo BrickOS [9] [13] (ex LegOS), che consente...

Documents

Transcript of Universita degli Studi di Siena` - Danny's LAB · rativo BrickOS [9] [13] (ex LegOS), che consente...