UNIVERSITÀ DEGLI STUDI DI PADOVA

FACOLTÀ DI INGEGNERIA ——

DIPARTIMENTO DI INGEGNERIA DELL’INFORMAZIONE

——

CORSO DI LAUREA TRIENNALE IN INGEGNERIA DELL’AUTOMAZIONE

Controllo del Khepera con localizzazione

tramite rete wireless

Relatore: prof. Luca Schenato

Studente: Giovanni Cosi

ANNO ACCADEMICO 2006-2007

Legge di Murphy: ’Se qualcosa può andar male, lo farà’

Indice

Introduzione........................................................................................................ 2

Motivazioni e sviluppo ....................................................................................... 2

1 Simulazione del Khepera ............................................................................... 4

1.1 Cinematica dell’unicilo ............................................................................ 4

1.2 Algoritmi di controllo .............................................................................. 5 1.2.1 Trajectory tracking ..................................................................... 6 1.2.2 Point to point .............................................................................. 9

2 Modellizzazione della rete wireless............................................................. 12

2.1 Localizzazione tramite rete di sensori wireless...................................... 13 2.1.1 Modello di propagazione del campo elettromagnetico ............ 13 2.1.2 Ottimizzazione della localizzazione con filtro di Kalman ....... 15

2.2 Controllo del Khepera con localizzazione simulata............................... 18 2.2.1 Funzionamento in catena aperta.............................................. 19 2.2.2 Controllo in catena chiusa ....................................................... 20 Point to point ............................................................................... 21 Trajectory tracking ...................................................................... 27

2.3 Conclusioni ............................................................................................ 36

3 Analisi sperimentale..................................................................................... 38

3.1 Utilizzo della rete wireless .................................................................... 38

3.2 Controllo del Khepera ........................................................................... 42 3.2.1 Point to point ............................................................................ 43 3.2.2 Trajectory tracking ................................................................... 45 3.2.3 Difficoltà nell’implementazione degli algoritmi...................... 47

Conclusioni........................................................................................................ 48 Appendice.......................................................................................................... 50 A.1 Il Khepera II. ................................................................................ 51 A.2 Codice Matlab. ............................................................................. 52 A.3 Il metodo diretto di Lyapunov...................................................... 58

Bibliografia ....................................................................................................... 60

Abstract

Questo lavoro propone alcune semplici tecniche di controllo per un veicolo uniciclo WMR (Wheeled Mobile Robot) chiamato Khepera. In particolare verranno analizzati, simulati e verificati due algoritmi di controllo: il primo relativo al problema del Point to point, il cui scopo è quello di far raggiungere all’uniciclo un qualsiasi punto dello spazio a partire da condizioni iniziali note, e un secondo relativo al noto Trajectory tracking, in modo che l’uniciclo segua determinate traiettorie, definite come coordinate spaziali (x(t), y(t)) con riferimento al tempo.

Partendo da lavori precedenti la verifica sperimentale sarà seguita dall’introduzione di nuove problematiche riguardo la localizzazione del WMR nello spazio: questa avverrà attraverso una rete di sensori wireless (in un primo momento simulata e poi anch’essa verificata sperimentalmente), con tutte le complicazioni che ne conseguono. Non ci si soffermerà sulla scelta e sull’implementazione dell’algoritmo di localizzazione del nodo mobile rispetto ai così detti nodi ancora, ma verranno individuate semplici soluzioni per risolvere i problemi introdotti da eventuali ritardi, grossolani errori, e perdita di dati sulla linea di retroazione nell’anello di controllo.

Per migliorare le prestazioni della localizzazione verrà implementato uno stimatore di Kalman, con conseguenti perfezionamenti del controllo stesso.

- 2 -

Introduzione

Il tema della localizzazione dei nodi di una rete wireless è un problema che ormai da anni viene affrontato in letteratura: in modo piuttosto approfondito è preso in considerazione il caso di localizzazione di nodi mobili rispetto ad una serie di nodi detti ancora, fissi e di coordinate note, e più recentemente la ricerca si è orientata verso soluzioni che non prevedano l’ausilio di sensori dedicati come ad esempio quelli ad ultrasuoni. Il problema della localizzazione di nodi mobili è sicuramente il più interessante per quanto riguarda questa tesi, in quanto le informazioni sulla posizione del nodo incognito, non saranno altro che la retroazione di un sistema di controllo di un veicolo uniciclo.

Tuttavia molte altre sono le problematiche che riguardano la ricerca e lo sviluppo di soluzioni valide e utili che riguardino l’impiego di reti wireless: dal semplice monitoraggio ambientale di vaste zone alla rilevazione di intrusi in un’area sorvegliata; è importante sottolineare inoltre che sono comuni le difficoltà incontrate nei vari progetti (perdita di pacchetti, ritardi nelle comunicazioni tra i nodi, grossolani errori dei dati scambiati, alimentazione dei nodi) come comuni sono gli obiettivi desiderati (risparmio energetico, affidabilità delle informazioni, riduzione del traffico di dati). Molti sono i progetti attualmente in fase di sperimentazione (il sistema Cricket, il progetto RADAR [7], il Mote Track dell’università di Harvard i più citati) [2], e molte le relative tecniche di localizzazione implementate in relazione all’hardware disponibile. Motivazioni e sviluppo Si è scelto di lavorare a questo progetto in quanto offre la possibilità di studiare e descrivere matematicamente un sistema fisico, sintetizzare controlli per sistemi non lineari, analizzare i risultati teorici e trovare soluzioni in applicazioni pratiche. Molti degli strumenti appresi durante il corso di studi sono applicabili esclusivamente nell’ambito di sistemi lineari o negli intorni dei punti di equilibrio di sistemi linearizzati; in questo caso non è possibile applicare tale tipo di controllo, e, per la sintesi degli algoritmi, si è spesso fatto ricorso ad uno dei principali strumenti a disposizione nel controllo dei sistemi non lineari: il metodo di Lyapunov. Alla sintesi del controllore segue poi una parte di simulazione: implementando in Matlab gli algoritmi e simulando il comportamento del robot attraverso le leggi imposte dal vincolo anolonomo, si stima la qualità del controllore e si ipotizzano eventuali miglioramenti o limiti che si potrebbero incontrare nell’attuazione pratica del controllo.

- 3 -

- 4 -

CAPITOLO 1 Simulazione del Khepera In questo capitolo viene introdotto il veicolo che si intende controllare secondo i noti algoritmi di Trajectory tracking e di Point to point: vengono brevemente studiati la cinematica e il modello dell’uniciclo utilizzato (il Khepera). In questa prima fase si sono introdotti, analizzati ed implementati i due algoritmi di controllo simulando il veicolo attraverso Matlab, con alcune considerazioni conclusive sugli eventuali problemi che si potranno incontrare nella fase di verifica sperimentale successiva. 1.1 Cinematica dell’uniciclo Il lavoro presentato in questa tesi prevede l’impiego di un uniciclo; in particolare si è scelto di utilizzare il Khepera il cui datasheet è presente in appendice. Questa tipologia di veicolo su ruote, insieme al biciclo, rappresenta una delle parti fondamentali dello studio nel campo della robotica mobile a causa delle sue caratteristiche cinematiche e dinamiche; tali tipologie di WMRs - wheeled mobile robots - infatti, ricoprono gran parte delle problematiche legate allo studio del controllo di sistemi meccanici mobili, pur mantenendo una certa semplicità descrittiva e costruttiva.

L’uniciclo è caratterizzato da due motori che agiscono rispettivamente sulla ruota sinistra e destra del robot, indipendenti, non sterzanti e situate sullo stesso interasse, in modo che il veicolo possa ruotare liberamente senza limite di curvatura, lasciando invariata la posizione spaziale (coordinate x e y): è chiaramente intuibile l’impossibilità di un moto

Figura 1.1: Veicolo uniciclo nello spazio. Una possibile scelta di coordinate è q =[x, y, θ].

- 5 -

parallelo all’interasse delle ruote. Data quindi una superficie su cui si possa far muovere il robot ne deriva che ogni punto del piano può essere raggiunto dal veicolo stesso: il vincolo imposto, che si può esprimere con il seguente vincolo sulle velocità:

sin cos 0x yθ θ− =& & (1.1)

non può essere infatti ottenuto per derivazione di un vincolo olonomo, essendo puramente anolonomo; risulta quindi che il sistema lineare definito dallo stato q = [x, y, theta] è completamente raggiungibile.

Esprimendo il vincolo (1.1) in forma Pfaffiana si ottiene la seguente:

( ) [ ]sin cos 0 0x

A q q yθ θθ

⎡ ⎤⎢ ⎥= − =⎢ ⎥⎢ ⎥⎣ ⎦

&

& &

&

(1.2)

In questo modo, calcolando una base nel nucleo di A(q) si ricava facilmente il modello cinematico del veicolo [1] [6], ottenuto trascurando tutti i termini dinamici e mappando i possibili controlli di velocità, compatibili con i vincoli imposti dal problema reale:

( ) 1

2

cos 0 cos 0sin 0 sin 0

0 1 0 1

vS q q

v

θ θθ θ

⎡ ⎤ ⎡ ⎤⎡ ⎤⎢ ⎥ ⎢ ⎥= → = ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎣ ⎦⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦

& (1.3)

con v1 e v2 velocità lineare (o di avanzamento v) e velocità angolare (ω) rispettivamente. E’ importante notare come entrambe queste velocità si possano esprimere in funzione delle singole velocità della ruota destra e sinistra dell’uniciclo (ωr e ωl) secondo le seguenti relazioni:

v1 = r/2(ωr + ωl) (1.4) v2 = r/L(ωr – ωl) con r raggio delle ruote attuate ed L la lunghezza dell’interasse delle ruote; l’attuazione delle singole ruote è gestita in modo indipendente da un controllore PID integrato nel Khepera, che funge da tramite tra i segnali di controllo di velocità lineare e angolare, e i due motori dell’uniciclo; questo controllore risulta chiuso infatti in una catena interna del loop di controllo, a più alta frequenza rispetto alle temporizzazioni in gioco sul controllo esterno. 1.2 Algoritmi di controllo Ottenuto il modello cinematico (1.3) è possibile adottare varie tecniche di controllo in modo da far eseguire al sistema differenti task, ma per questo lavoro verranno discusse solo alcune soluzioni per quanto riguarda il trajectory tracking e il point to point.

Data la natura del modello cinematico, come visto precedentemente, i controlli calcolati saranno controlli in velocità, supponendo non influenti le masse e le inerzie presenti nel sistema sotto esame.

- 6 -

1.2.1 Trajectory tracking Il problema del trajectory tracking consiste nel far seguire al robot un determinato percorso cartesiano, con una certa legge temporale, e può semplicemente essere visto come il tracking di un veicolo di riferimento con annullamento dell’errore.

La scelta del percorso desiderato può quindi essere vista come la stabilizzazione a zero di una funzione errore tra la configurazione attuale e quella desiderata, poiché risulta più semplice specificare la traiettoria voluta in funzione di qualche coordinata caratteristica del robot, piuttosto che definire dei parametri geometrici del cammino cartesiano dotati di una certa legge temporale [x(t), y(t), θ(t)]. Dato il modello cinematico dell’uniciclo (1.3) si devono trovare gli adeguati controlli che permettano di inseguire perfettamente un veicolo di riferimento con modello:

cos 0sin 0

0 1

r rr

r r rr

r

xv

q yθθ

ωθ

⎡ ⎤ ⎡ ⎤⎡ ⎤⎢ ⎥ ⎢ ⎥= = ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎣ ⎦⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦

&

& &&

(1.5)

per il quale sono note le velocità di riferimento (vr(t) e ωr(t)) per ogni t, con un ulteriore ipotesi di partenza: si ammette la conoscenza degli stati del veicolo da controllare e del veicolo di riferimento, in altre parole si richiede la perfetta localizzazione di entrambi gli unicicli (tale ipotesi non sarà più così valida con l’utilizzo della rete di sensori wireless).

Come detto precedentemente tutto il problema del trajectory tracking può essere

ricondotto al problema di stabilizzazione a zero nello spazio di stato ẽ, vettore degli errori di posizionamento tra veicolo da controllare e il riferimento:

1

2

3

r

r

r

e x xe e y y .

e θ θ

−⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥= = −⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥−⎣ ⎦ ⎣ ⎦

%

% %

%

(1.6)

E’ evidente come tali errori di inseguimento prescindano dalla posizione assoluta dei due veicoli rispetto alla terna fissa di riferimento <W>, ma dipendano solamente dalla loro

Figura 1.2: Riferimento assoluto <W> e riferimento solidale al veicolo inseguitore <I>

- 7 -

posizione relativa; in questo modo è possibile riferire le variabili errore rispetto ad una seconda terna di riferimento <I> solidale con il veicolo da controllare, come riportato in figura 1.2 .

Geometricamente parlando, la relazione tra le due terne è espressa con la rototraslazione ITW:

cos sin cos sinsin cos sin cos0 0 1

IW

x yT x y

θ θ θ θθ θ θ θ

− −⎡ ⎤⎢ ⎥= − −⎢ ⎥⎢ ⎥⎣ ⎦

(1.7)

la quale esprime il legame tra la posizione (xr , yr) rispetto alla terna solidale al veicolo inseguitore [1] [8]; derivando rispetto al tempo per ottenere la nuova cinematica dell’errore di posizione rispetto alla terna <I > si ha:

( )( )

21 1

12 2

3

cossin .

1 1

r rrI

r rW r

r

v v ee x ev ee T y e e

e

θ θ ωθ θ ω

ω ω

⎡ − − + ⎤⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ − −= ⇒ = = ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ −⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦

&

& &

&

(1.8)

A questo punto scegliendo una candidata p.d. di Lyapunov [A.3] ricordando che si richiede la stabilizzazione di tutto lo spazio di stato e, risulta immediato calcolare la legge di controllo da applicare al veicolo inseguitore. La candidata, e la relativa derivata direzionale sono:

( ) ( ) ( )

( ) ( ) ( ) ( )

2 21 2 3

1 3 2 2 3 1 3

1 1 cos2

cos sin sinf r r r

V e e e K e

L V e e v e v e e v e e K eω ω ω ω

⎧ = + + −⎪⎨⎪ = − + + − + −⎩

(1.9)

dove K è un grado di libertà aggiunto per il progetto del controllo stabilizzante. Le leggi di controllo di velocità lineare e angolare saranno quindi:

3 1

2 3

cos1 sin

r

r r

v v e e

e v eK

ω ω

= +⎧⎪⎨

= + +⎪⎩

(1.10)

Una prima fase del progetto prevede la simulazione dell’intero sistema (Khepera di

riferimento e Khepera inseguitore) utilizzando i tools messi a disposizione da Matlab; dapprima si intende quindi simulare il sistema da controllare per testare l’algoritmo appena presentato, e in un secondo momento passare ad una modellizzazione il più verosimile possibile della rete di sensori wireless, e dei suoi effetti negativi sulla retroazione e sul controllo stesso.

Considerato il modello cinematico (1.3) e le leggi di controllo (1.10) si dispone di tutti i

dati per la simulazione dell’algoritmo (si faccia riferimento al codice in appendice): si sono eseguite alcune semplici prove impostando i valori delle velocità dell’uniciclo di

- 8 -

riferimento arbitrariamente, così come le condizioni iniziali dei due robot, e confrontando le prestazioni con diversi valori di K, guadagno del controllo.

In figura 1.3 il robot target percorre orizzontalmente lo spazio partendo dal punto (0, 2) con velocità lineare impostata al valore costante di 0.5 m/s, e velocità angolare nulla (nero tratteggiato). Le traiettorie colorate rappresentano i percorsi seguiti dall’uniciclo inseguitore per diversi valori di K, con comune condizione iniziale [x, y, θ] = [1, 0, π/2]; è evidente come il guadagno presente al denominatore nella legge di controllo comporti migliori prestazioni per valori che tendono a 0.1 (curva rossa), per poi degenerare verso un sistema quasi instabile per valori ancora più piccoli; si noti inoltre la lentezza nel raggiungere il riferimento per K elevati (fuxia).

Come indicato precedentemente, risulta più semplice tracciare il percorso cartesiano desiderato agendo sulle velocità, piuttosto che definirne i singoli parametri geometrici; in questo modo impostando rispetto al caso precedente, la velocità angolare ad un valore non nullo (0.3 rad/s) si ottiene una traiettoria a circonferenza del riferimento come figura 1.4 (nero tratteggiato): in questo caso le varie prove effettuate con diversi valori di K evidenziano differenze simili alla simulazione con velocità angolare nulla. Per guadagni infinitesimi (K=0.001) il sistema risulta pressoché instabile, o comunque presenta controlli caratterizzati da valori non saturati troppo elevati, che conferiscono alla risposta dell’inseguitore prestazioni pessime (curva verde), consentendo di agganciare il riferimento ma mantenendo un’andatura oscillatoria: non si ha annullamento dell’errore di posizione, se non per tempi lunghissimi.

Figura 1.3: trajectory tracking - caso della retta: traiettorie del robot di riferimento (nero tratteggiato) e del robot inseguitore per diversi valori di K di controllo

- 9 -

Per valori del guadagno più ragionevoli si può osservare il differente punto di aggancio dell’inseguitore: la traiettoria risulta più diretta, il sistema più pronto e reattivo per K che assume valori comunque piccoli (curva rossa), mentre perde velocità ed energia nei controlli al suo aumentare (curva fuxia). Come nel caso precedente, l’annullamento dell’errore a regime avviene alla perfezione. 1.2.2 Point to Point Il problema del “point to point” risulta di ancora più immediata comprensione: il robot deve raggiungere una particolare configurazione arbitraria, partendo da una qualsiasi configurazione iniziale; detto in modo pratico partendo da un punto (x, y) con un certo orientamento, deve raggiungere una destinazione finale (xf, yf) con un certo orientamento. In questo caso verrà trascurato il task sull’orientamento finale, in quanto poco utile ai fini del lavoro in questione.

In questo caso risulta particolarmente semplificativo considerare un cambiamento di coordinate, in quanto il problema può essere trattato come un problema di stabilizzazione di un punto di equilibrio nello spazio di stato del veicolo [1]; tale cambio di coordinate è il seguente, confrontabile con quello in figura 1.5:

2 2 cos

arctan ( ) sinarctan ( )

x y xx y y

x y

ρ ρ ϕϕ ρ ϕβ θ π θ ϕ β π

⎡ ⎤+⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥= → =⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥− + − +⎣ ⎦ ⎣ ⎦ ⎣ ⎦⎢ ⎥⎣ ⎦

(1.11)

dove ρ è la distanza del punto di mezzo dell’interasse delle ruote dall’origine, φ è l’orientazione del vettore distanza dall’origine, rispetto all’asse delle ascisse, e β è una terza coordinata indipendente dalle prime due che esprime lo scostamento della direzione di

Figura 1.4 trajectory tracking - caso della circonferenza: traiettorie del robot di riferimento (nero tratteggiato) e del robot inseguitore per diversi valori di K di controllo

- 10 -

avanzamento del veicolo rispetto alla congiungente il punto di mezzo dell’interasse con l’origine del sistema di riferimento.

Annullando le tre nuove variabili di stato si ottiene la stabilizzazione del veicolo nell’origine; mentre, per ottenere la stabilizzazione in un punto generico, scelto arbitrariamente (xf, yf ), basta adottare un ulteriore semplice cambio di coordinate, che trasli il sistema di riferimento dall’origine al punto destinazione ( x = xo - xf ; y = yo - yf ).

Il cambio di variabili introduce anche un nuovo modello cinematico del sistema:

cos

sin

sin

vv

v

ρ β

ϕ βρ

β β ωρ

⎧= −⎪

⎪=⎨

⎪⎪ = −⎩

&

&

&

(1.12)

e anche in questo caso per stabilizzare il veicolo si utilizza un controllore alla Lyapunov, e viene quindi considerata la seguente candidata p.d. (e la relativa derivata direzionale):

( ) ( )

( )

2 2 2

2

12

cos sin sinf

V q K

v v vL V q K K

ρ ϕ β

ρ β ϕ β β β βωρ ρ ρ

⎧ = + +⎪⎪⎨⎪ = − + + −⎪⎩

(1.13)

dove K rappresenta un grado di libertà aggiuntivo per la regolazione del controllore; non resta che calcolare quindi i relativi controlli di velocità:

cossin cos sin cos

vK

K

ρ βϕ β β β βω β

β

=⎧⎪

+⎨ = +⎪⎩

(1.14)

Figura 1.5 nuove coordinate per l’algoritmo di Point to Point. Si imposta il problema come un problema di stabilizzazione a zero del nuovo stato [ρ φ β]’

- 11 -

Anche nel caso del Point to point l’implementazione in Matlab risulta piuttosto semplice, in quanto si necessita esclusivamente del modello (1.3), delle relazioni di cambio di coordinate e delle leggi di controllo appena calcolate. Prima di passare alla fase successiva che prevede la simulazione dell’ambiente wireless, si testa la bontà dell’algoritmo appena descritto, con una semplice prova: l’uniciclo dallo stato iniziale (xo, yo, θo) = (0, 0, π/2) viene fatto muovere fino alla configurazione finale (xf, yf , θf ) = (4, 7, #): si noti come l’orientamento finale non venga preso in considerazione (figura 1.6).

Vengono confrontate le prestazioni per diversi valori di K, e come nel caso del trajectory tracking si osservano differenze simili: per K crescenti (curva rossa in poi) si ha un controllo via via più veloce ed efficiente che comporta traiettorie brevi e più dirette (blu e fuxia), con valori degli ingressi di controllo più energici durante il transitorio iniziale. Per valori decimali del guadagno si ottiene invece un controllo meno potente, caratterizzato da ingressi meno saturati ; in figura si può notare infatti come la prova a K=0.1 (curva verde) risulti la meno efficiente in termini di tempo.

L’algoritmo è stato testato ponendo come tempo di campionamento per il controllo e il modello stesso T=0.1 secondi.

___ Per completare questa prima fase simulativa, si intende modellizzare l’ambiente

wireless che dovrà fornire le informazioni necessarie alla localizzazione del Khepera nello spazio, comportando svariate complicazioni per quanto riguarda il controllo in generale.

Figura 1.6 Point to point - simulazione in Matlab. Vengono confrontate le prestazioni del controllo per diversi valori di K .

- 12 -

CAPITOLO 2 Modellizzazione della rete wireless

Cuore del progetto è

l’implementazione degli algoritmi di controllo presentati precedentemente utilizzando per la retroazione del sistema una rete di nodi wireless (nodi ancora) che avranno il compito di localizzare nello spazio (coordinate x, y) un nodo mobile, posto sul Khepera. Non ci si dilungherà sulla scelta e sull’analisi dell’algoritmo di localizzazione utilizzato, né sul confronto con gli argomenti proposti in letteratura (per questo si faccia riferimento alla bibliografia), ma verrà introdotto l’hardware utilizzato, con una veloce panoramica circa il software adottato.

Le reti di sensori wireless sono reti generalmente utilizzate per il controllo o il monitoraggio di vasti ambienti nei quali perciò è necessario determinare dove siano posizionati i diversi nodi al fine del corretto utilizzo dei dati ambientali rilevati dagli stessi. Vari sono i modelli di nodi wireless presenti in commercio, ma comuni sono le caratteristiche di base; per il lavoro in questione si sono utilizzati i nodi Tmote-Sky sviluppati dalla MoteIv Corporation, società consolidata nello sviluppo di piattaforme

wireless e appartenente alla società Texas Instruments (per il datasheet del nodo presente in figura si faccia riferimento all’appendice). La rete di nodi wireless detti ancora, ovvero i nodi fissi di coordinate note utilizzati per stimare la posizione di un altro nodo mobile, sono stati posizionati sotto un piano 3 x 2 metri circa, che sarà lo spazio disponibile sul quale il Khepera dovrà essere comandato (e localizzato). Tali nodi ancora sono 48, posti su una griglia, distanti l’uno dall’altro approssimativamente 40 cm, come schematizzato in figura 2.1.

Figura 2.1 schema della disposizione dei nodi TmoteSKY nel piano (nodi blu). Il nodo mobile (rosso) posto sul Khepera in movimento sul piano, scambia informazioni con la rete e viene localizzato sfruttando le coordinate note degli altri 48 nodi ancora

- 13 -

2.1 Localizzazione tramite rete di sensori wireless Tra tutti i gli algoritmi di localizzazioni presentati in letteratura se ne è scelto uno che non prevedesse l’utilizzo di sensori dedicati, ma si basasse unicamente sulle informazioni rilevate dal chip radio, le quali sono: potenza del campo elettromagnetico ricevuto RSS (Received Signal Stength) e qualità del segnale LQI (Link Quality Indicator) in modo da alleggerire il traffico di dati all’interno della rete, e velocizzare lo scambio di informazioni; tutto a favore di un controllo più prestante [2].

Il linea teorica, essendo l’RSS (P(d)) funzione della distanza d, stimare la posizione di un nodo mobile triangolando le varie misure su alcuni dei nodi della rete dovrebbe essere una semplice operazione di tipo algebrico:

( ) ( )02

0

P dP d

dd

=⎛ ⎞⎜ ⎟⎝ ⎠

[W ]

o più generalmente, a seconda del mezzo trasmissivo :

[W ]

con d0 distanza minima relativamente alla quale si conosce il valore di P(d0).

Ma come ci si può aspettare a causa delle imperfezioni nella propagazione del campo elettromagnetico e dell’elevata sensibilità delle misurazioni dello stesso chip radio alle perturbazioni dell’ambiente e ai disturbi del mezzo, tale stima risulta spesso grezza e scorretta. 2.1.1 Modello di propagazione del campo elettromagnetico La propagazione delle onde elettromagnetiche, all’aperto come in ambienti chiusi, è soggetta a molteplici fenomeni ed è quindi difficilmente prevedibile o modellizzabile. I fenomeni più evidenti che intervengono risultano essere: riflessione, diffrazione e scattering; questi si verificano in misura maggiore o minore al variare del rapporto fra la dimensione della lunghezza d’onda del campo elettromagnetico e le dimensioni dell’oggetto su cui il campo insiste. Riflessione

La riflessione di un’onda elettromagnetica è dovuta ad ostacoli aventi dimensioni molto maggiori della lunghezza d’onda del segnale elettromagnetico incidente. Nel caso della riflessione, quando un’onda raggiunge l’oggetto riflettente con un angolo θ rispetto alla normale, essa modifica la propria traiettoria tornando indietro secondo un angolo θ’ = - θ (figura 2.2).

( ) ( )0

0

pn

P dP d

dd

=⎛ ⎞⎜ ⎟⎝ ⎠

- 14 -

Diffrazione

Il fenomeno della diffrazione deriva dalla natura ondulatoria del campo elettromagnetico e si verifica in maniera dominante rispetto agli altri fenomeni quando il campo insiste su oggetti impenetrabili separati fra loro da distanze inferiori della lunghezza d’onda. In questo caso, secondo il principio Huygens, dietro tali oggetti si formano delle onde sferiche secondarie che consentono di propagare l’energia dell’onda anche lungo delle direzioni che sarebbero vietate dall’impenetrabilità degli oggetti (figura 2.3) Scattering

Il fenomeno dello scattering è dovuto alla natura corpuscolare del campo elettromagnetico e si manifesta in presenza di oggetti impenetrabili aventi dimensioni paragonabili a quelli della lunghezza d’onda. In questo caso l’energia dell’onda che insiste sull’oggetto viene reirradata in moltissime direzioni che, nei casi reali, sono impredicibili e che dipendono anche dalla lunghezza d’onda del campo incidente.

I fenomeni appena introdotti rappresenteranno uno dei principali problemi di questo

lavoro, in quanto procurano gravi complicazioni per la stima della posizione del nodo mobile causando la ricezione, al ricevitore, di innumerevoli onde elettromagnetiche provenienti da molteplici direzioni e quindi aventi fasi differenti. La somma di tali onde causa un’amplificazione o un’attenuazione della potenza del campo ricevuto, interferendo con il segnale originale che diventa quindi molto meno attendibile; l’attenuazione o l’amplificazione del campo elettromagnetico ricevuto, rispetto al valore nominale, prende il nome di medium scale fading, ed è tanto più evidente quanto è più stretto l’intervallo di frequenze utilizzate nella trasmissione radio.

In assenza di tali fenomeni, avendo comunque considerato un mezzo trasmissivo non ideale caratterizzato da un fattore esponente maggiore di 2 (np), la potenza del campo ricevuto a distanza d, espresso in [dBm] risulta essere:

Figura 2.2 esempio di riflessione Figura 2.3 esempio di diffrazione

- 15 -

( ) ( )( ) ( ) ( )( )

0 010log 10 log 10 log

10 log .p p

TX p

P d P d n d n d

P A n d

= + −

= + − (2.1)

I termini che compaiono nel modello sono:

• P(d) : RSS, potenza del campo elettromagnetico ricevuta, espressa in [dBm]; • P(d0) : RSS, potenza ad una distanza d0 dal trasmettitore, espressa in [dBm]; • np : fattore di decrescenza della potenza in funzione del logaritmo della distanza; • d : distanza dal trasmettitore [m]; Per una più precisa modellizzazione del campo elettromagnetico è necessario introdurre

nel modello il fenomeno del medium scale fading, che come si può dedurre non è assolutamente funzione del tempo e resta costante se l’ambiente non subisce variazioni.

Proprio sulla base di questa ultima precisazione è possibile ipotizzare di modellizzare l’effetto del medium scale fading come una variabile aleatoria χ ; essendo molteplici e indipendenti i cammini seguiti dalle diverse onde interferenti al ricevitore (dove col termine indipendente si indica il fatto che, noto il cammino seguito da un’onda elettromagnetica, nulla si conosce del cammino di qualunque altra onda elettromagnetica), ed essendo innumerevoli le onde che interferiscono al ricevitore, in forza della legge dei grandi numeri la variabile aleatoria χ può essere convenientemente modellata come una variabile aleatoria gaussiana; si ottiene perciò il seguente modello:

( ) ( )10 logTX pP d P A n d σχ= + − + (2.2)

dove la varianza σ di χ è da determinare attraverso le misure (come il fattore np), mentre la media di χ è posta a zero perché non misurabile.

Tale modello risulta sicuramente incompleto, in quanto trascura fenomeni frequenti e comuni come la variabilità del mezzo trasmissivo (basti pensare agli effetti che può avere il movimento delle persone all’interno della stessa stanza in cui si fanno le misurazioni), o l’attenuazione della potenza del campo elettromagnetico a causa dell’attraversamento dell’onda di materiali quasi trasparenti (per esempio i muri), che riducono sensibilmente il valore dell’RSSI misurato al ricevitore a parità di distanza dal trasmettitore: incompleto ma sufficientemente accurato per ciò che si intende fare, localizzare un nodo mobile su un piano di 4 x 3 metri, sfruttando 48 nodi ancora posti sotto di esso. Per l’effettivo funzionamento dell’algoritmo di localizzazione si faccia riferimento a [2]. 2.1.2 Ottimizzazione della localizzazione tramite filtro di Kalman Le stime sulla posizione calcolate soffrono in accuratezza e precisione a causa degli effetti del medium scale fading, come illustrato precedentemente. Risultati migliori possono essere ottenuti sicuramente aumentando il numero di nodi ancora, o ancora affinando il modello di propagazione della potenza del campo elettromagnetico; per un buon controllo è fondamentale avere una retroazione che fornisca informazioni il più attendibili possibile[2]. Avendo introdotto nel modello la variabile aleatoria gaussiana χ a media nulla e varianza σ, è possibile affidare ad un filtro di Kalman il compito di migliorare le stime in questione: è ragionevole infatti pensare che la traiettoria seguita dal Khepera, e quindi dal nodo mobile,

- 16 -

sia descrivibile per mezzo di un sistema dinamico tempo continuo in cui la velocità varia nel tempo con una certa continuità.

Per l’implementazione del filtro si assuma quindi come stato del sistema la posizione e la velocità del nodo mobile M lungo le coordinate x e y:

T

x x y yx M M M M⎡ ⎤= ⎣ ⎦& & (2.3)

e il relativo sistema dinamico, dato da:

x A x B uy C x D u

= +⎧⎨ = +⎩

& (2.4)

dove u è un ingresso incognito che determina la traiettoria seguita dal nodo M. Per semplificare il filtro e i relativi calcoli è possibile supporre un disaccoppiamento fra il moto lungo l’asse x e lungo l’asse y, ottenendo una matrice A del tipo:

1 2

3 4

5 6

7 8

0 00 0

0 00 0

a aa a

Aa aa a

⎡ ⎤⎢ ⎥⎢ ⎥=⎢ ⎥⎢ ⎥⎣ ⎦

(2.5)

che descrive due sistemi indipendenti di ordine due. Essendo la dinamica del sistema lungo l’asse x, coincidente con la dinamica del sistema lungo l’asse y si riescono ad utilizzare due filtri indipendenti e identici per la stima delle singole coordinate, con il conseguente alleggerimento dell’algoritmo di stima.

Si consideri in questo modo il sistema tempo continuo relativo ad un asse soltanto:

[ ]

0 0 11 0 0

0 1 1

x x w

y x v

⎧ ⎡ ⎤ ⎡ ⎤= +⎪ ⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦⎨⎪ = +⎩

& (2.6)

dove w è un processo aleatorio di rumore bianco, gaussiano a media nulla e con varianza σw , indipendente dal processo v, anch’esso bianco gaussiano, a media nulla e varianza σv . Il processo w, detto anche rumore di modello indica la rapidità di variazione della velocità del nodo mobile M, mentre il processo v, rumore di misura, rappresenta l’errore commesso nella stima della coordinata, lungo l’asse delle ascisse o delle ordinate.

Discretizzando il modello (2.6) si ottiene il sistema equivalente tempo discreto:

( ) ( ) ( )

( ) [ ] ( ) ( )

2

1 01

1 2

1 0 1 1

Tx t x t w t

T T

y t x t v t

⎧ ⎡ ⎤ ⎡ ⎤+ = +⎪ ⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦⎨⎪ + = +⎩

(2.7)

La varianza del rumore di modello nel nuovo sistema tempo discreto è data da:

- 17 -

22

2 30

2.

2 3TT A T A

w wT T

Q e BB e dT T

τ τσ τ σ⎡ ⎤

= = ⎢ ⎥⎣ ⎦

∫ (2.8)

Mentre la varianza del rumore di misura R è:

2 .vR σ= (2.9)

Per determinare il valore effettivo della varianza dell’errore di misura si è utilizzata l’informazione sull’errore medio di localizzazione calcolato attraverso una serie di misurazioni fatte a priori [1]; mentre per determinare il valore della varianza del rumore di modello verranno eseguite prove sperimentali sul sistema intero per un tuning manuale ed empirico.

A questo punto si dispone di tutti gli elementi necessari per implementare un filtro di Kalman tempo variante [5], avente le matrici di correlazione dei rumori Q ed R sopra indicate:

( ) ( )

( ) ( )

( ) ( ) ( )( )( ) ( ) ( ) ( )( ) ( )

( ) ( )( )

( )

( )

1

1

1 11 1 1

1

1 1 1 1 1

1

1 1 1

0

ˆ ˆ

ˆ ˆ ˆ

ˆ

t |t t|t

Tt |t t|t

t tt |t+1 t |t t |t

T Tt |t+1 t |t t |t t |t t |t

T Tt t |t t |t

0|-1

0|-1

x Axpredizione

P AP A Q

x x K y Cxaggiornamento

P P P C CP C R CP

K P C CP C R guadagno di Kalman

x x

P P

+

+

+ ++ + +

−

+ + + + +

−

+ + +

= ⎫⎪⎬

= + ⎪⎭

⎫= + − ⎪⎬⎪= − + ⎭

= +

=

= 0

inizializzazioneI

⎫⎪⎬= ⎪⎭

(2.10)

Per la sua natura ricorsiva il filtro di Kalman risulta semplice da implementare

utilizzando Matlab, sia per quanto riguarda le prove simulative che le future prove sperimentali. Si noti inoltre come agendo sulle singole matrici Q ed R, moltiplicandole per arbitrari fattori (M e 1/M rispettivamente) si riesca a pesare in modo adeguato l’influenza del modello o delle misure, dando più attendibilità all’uno rispetto che all’altro, ma su questo discorso si tornerà in fase di verifica, in quanto anche queste scelte fanno parte del tuning dell’intero algoritmo di controllo.

I dati sulla localizzazione del nodo mobile risulteranno in questo modo decisamente più attendibili e vicini alla realtà, conferendo al controllo stesso maggiore stabilità.

- 18 -

2.2 Controllo del Khepera con localizzazione simulata Si è scelto di far precedere l’utilizzo diretto del Khepera e la localizzazione tramite la rete di 48 nodi wireless da una fase simulativa (come di consueto), in modo da poter progettare e implementare il miglior controllore possibile senza agire direttamente sull’hardware in questione; questo permetterà inoltre di avere dei dati con cui confrontare le esperienze pratiche per dedurre limiti, accorgimenti e ottimizzazioni riguardo argomenti già ampliamente trattati in letteratura, e per analizzare le differenze tra la progettazione di un algoritmo di controllo per la simulazione e la sua implementazione pratica.

Verrà quindi controllato il Khepera simulando l’ambiente wireless.

____

La modellizzazione del sistema fisico in esame, e della rete wireless con tutti le relative complicazioni che essa comporta a livello pratico, riveste un ruolo importante in questa prima fase simulativa: più il modello risulta verosimile e meno approssimativo rispetto al sistema fisico, più accurata e precisa sarà la simulazione e lo studio ad essa associato.

Nel nostro caso le difficoltà che ci si aspetta localizzando il nodo mobile tramite la rete di sensori wireless, sono già state brevemente introdotte: principalmente ci si aspettano delle misurazioni dell’RSSI poco attendibili con relative stime grossolane della posizione. Tale problema risulta sicuramente primario in fase di progetto, al quale si cercherà di porre rimedio con un filtro di Kalman . Caratteristiche delle reti wireless inoltre sono tutte quelle complicazioni che derivano dalla comunicazione tra i nodi della stessa rete e tra la rete e il calcolatore centrale, a cui sono affidati i calcoli dell’algoritmo di controllo: ci si aspetta un ritardo nello scambio dei dati, che comporta quindi ritardi nel controllo e nella localizzazione stessa; errori nei dati trasmessi; perdita di pacchetti, che non arrivando a destinazione rischiano di rendere instabile tutto il sistema, (che viene controllato in catena aperta) [4]; anche il fatto che la rete possa essere interrogata con una certa frequenza a seconda dell’algoritmo di localizzazione implementato, frequenza che rimane comunque nell’ordine di 0.5 ÷1 Hz, comporta gravi conseguenze sul controllo di un Khepera in movimento.

In fase di simulazione si è deciso di tenere conto di alcune di queste complicazioni: il rumore nelle misurazioni e nel calcolo per la localizzazione sono state modellate con la semplice aggiunta di rumore bianco nei segnali di retroazione dello stato (anche se in realtà si tratterebbe di un rumore di tipo moltiplicativo) con media e varianza impostati ai valori adeguati (media nulla e varianza da verificare sperimentalmente). Si è inoltre tenuto conto dei ritardi τ (per mezzo dell’implementazione di un semplice buffer) e del fatto di poter interrogare la rete non troppo frequentemente, scegliendo volta per volta i relativi valori desiderati, per spingere il controllo al limite e confrontare i risultati tra il previsto “caso pessimo” e situazioni migliori. Si noti come si sia deciso di non considerare la possibilità di perdere pacchetti, caso frequente in progetti simili, per non complicare troppo il lavoro in questione; si tenga comunque conto che il gap di tempo tra un’interrogazione e l’altra TK (impostabile arbitrariamente in questa fase) permette di avere il tempo utile a ricevere, anche con consistenti ritardi, tutti i pacchetti necessari (per la realizzazione effettiva si veda il codice presente in appendice).

- 19 -

2.2.1 Funzionamento in catena aperta Prima di iniziare l’implementazione dei controllori analizzati, verrà fatto un test in catena aperta per verificare la precisione intrinseca del robot; essendo una prova puramente empirica, non si ha nessuna pretesa di precisione, ma dall’analisi qualitativa dei risultati ci si può fare un’idea sui limiti in termini di prestazioni e precisione del Khepera. A tal proposito è stato scritto un programma Matlab, per provare il controllo in velocità, mentre è stato trascurato quello in posizione; da quanto si deduce da lavori precedenti infatti gli errori di posizione non sono trascurabili: sarebbe difficile esprimerli in termini quantitativi, giacché dipendono da troppe variabili di disturbo (superficie del piano d’appoggio, cavo di comunicazione, punto d’appoggio variabile del robot, gioco delle ruote, ecc...).

Il metodo scelto per le prove in velocità consiste nell’impostare le velocità lineare ed angolare a valori arbitrari, ottenendo a seconda dei casi traiettorie rettilinee o a circonferenza, in modo da valutare le differenze tra sistema ideale simulato e sistema reale. Si sono eseguite più verifiche con diversi tempi di campionamento, in modo da registrare i prevedibili errori che si presentano nella stima della posizione per frequenze basse, nell’ordine di 0.5 ÷1 Hz.

Come spiegato in modo più approfondito in appendice, il Khepera è dotato di encoder indipendenti per la lettura della posizione angolare e delle velocità delle singole ruote: non disponendo di sensori esterni per la lettura della posizione (ruolo che rivestirà la rete wireless, in modo più o meno preciso), ci si affida alla lettura delle velocità per stimare le coordinate del Khepera nel piano attraverso il seguente sistema discreto:

x = x + v cos( ) Ty = y + v sin( ) T

θθ

⎧⎨⎩

con velocità lineare v misurata. Per la lettura della posizione angolare θ si utilizzano invece i dati provenienti dagli encoder per la posizione[8]. Essendo Windows un sistema non-real time è facile immaginare come tale approssimazione possa causare stime non corrette per T elevati.

Figura 2.4: 7 fotogrammi del movimento del Khepera: con velocità lineare e velocità angolare costanti compie una traiettoria circolare (tracciata in rosso) che si avvicina molto a quella simulata (tracciata in verde).

- 20 -

Le velocità impostate per compiere la traiettoria di Fig. 2.4 sono 0.1m/s di velocità lineare e 1 rad/s di velocità angolare; confrontando il comportamento del robot con la simulazione si osserva che il raggio della circonferenza è circa lo stesso (questo significa che a regime le due velocità si eguagliano); il fatto poi che nella realtà le velocità non possano essere raggiunte istantaneamente, ma abbiano un transitorio iniziale (grafici seguenti) provoca lo sfasamento delle due traiettorie.

Osservando i grafici delle velocità misurate dall’encoder del Khepera sono doverose alcune considerazioni: la prima è che la quantizzazione delle velocità delle singole ruote si ripercuote sulle risultanti velocità lineari ed angolari (il passo di quantizzazione risultante è, rispettivamente, di 3mm/s e 0.14rad/s); i trasduttori causano inoltre un’incertezza sui valori delle velocità, che, per l’eventuale controllore, è come se subissero repentini cambiamenti; per brusche variazioni degli ingressi di controllo infine, data la velocità del transitorio, si potrebbe avere un’imprecisa stima della posizione. Ora che si ha un’idea più precisa delle prestazioni che ci si possono aspettare e delle possibili problematiche, si può passare all’implementazione dei controllori analizzati. 2.2.2 Controllo in catena chiusa In questo paragrafo si tratterà l’implementazione in Matlab e la sperimentazione di vari tipi di controllo point to point e trajectory tracking, analizzandone pregi e difetti. Si può dire che questi siano tipi di controllo closed loop, perché, in base ai dati forniti dagli encoder a bordo del Khepera, si può ad ogni passo stimare la posizione del robot e in base ad essa calcolare gli ingressi di controllo.

Solo in un secondo momento tali stime della posizione verranno sporcate dall’aggiunta di un rumore bianco di varianza arbitraria (nell’ordine di 0.5m), per simulare i dati provenienti dalla rete wireless.

- 21 -

Point to Point La realizzazione in Matlab dell’algoritmo visto nel capitolo 1.2.2 risulta simile all’implementazione vista per la simulazione dello stesso; alcune importanti differenze sorgono dal momento che si vuole controllare un sistema reale, il Khepera: la lettura delle velocità delle singole ruote e l’invio dei comandi richiedono ovviamente una particolare attenzione per quanto riguarda la temporizzazione per la stima della posizione, e la scelta del tempo di campionamento risulta quindi fondamentale. La scelta di T=0.1s per esempio, permette di mediare la lettura della velocità su cinque misure, andando quindi a compensare eventuali errori causati dal rumore che come si vede dai grafici precedenti affligge tali rilevazioni.

In figura 2.5 vengono riportate alcune delle prove eseguite con le leggi di controllo (1.14), per diversi valori del guadagno K: si vuole far raggiungere al Khepera posizionato in (-0.4, -0.2) e con orientamento verticale (π/2), il punto (0.3, 0.15). Come si può notare dai diagrammi delle traiettorie sul piano (x,y) e dai grafici delle velocità lineari, le considerazioni che si possono fare rimangono pressoché invariate rispetto al caso della simulazione: per K via via crescenti (0.1<K<5) si hanno traiettorie più

Figura 2.5: Verifica dell’algoritmo Point to point . Traiettorie e velocità lineari per diversi valori di K di controllo. Il tempo di campionamento è T=0.1s.

- 22 -

brevi sia per quanto riguarda lo spazio percorso che il tempo impiegato, con la differenza che superato un certo valore le velocità di controllo risultano più oscillanti e variabili. L’uniciclo per K elevati compie infatti repentini cambi di direzione e verso di percorrenza, raggiungendo comunque in modo preciso lo stato desiderato, senza mai cadere nell’instabilità. Tale comportamento è molto probabilmente dovuto al fatto che le velocità di controllo non raggiungono istantaneamente il valore desiderato: si ricordi infatti che l’attuazione dei controlli è effettuata mediante un PID interno al Khepera stesso.

____

A questo punto basta aggiungere del rumore bianco alle stime della posizione dell’uniciclo per simulare l’effetto della rete wireless. Errori di 0.5m su traiettorie di pochi metri sono sicuramente molto influenti sul controllo, e l’impiego del filtro di Kalman risulta quindi necessario. Verranno eseguite svariate prove per diversi valori del tempo di campionamento T (da T=0.1s si passerà a valori più ragionevoli nell’ordine di mezzo secondo), del guadagno K e di M, coefficiente usato per la differenziazione del peso dato alle misure o al modello nel filtro di Kalman (paragrafo 2.1.3).

Figura 2.6: Point to point: traiettorie e velocità di controllo del Khepera, con ambiente wireless simulato. A) K=10 B) K=1. In entrambi i casi il coefficiente M è stato lasciato invariato a 1, in modo che il filtro di Kalman lavori in modo naturale. (T=0.1s)

- 23 -

In fase di sperimentazione, applicando l’algoritmo precedente senza alcuna limitazione si sono ottenute prestazioni pessime e tendenti all’instabilità del sistema, con velocità lineare e angolare di controllo troppo variabili ed elevate in modulo. Si è quindi deciso di moltiplicarle per dei coefficienti decimali - a, b rispettivamente - , in modo da rendere il controllo più dolce e meno potente: dopo alcune prove si è optato per a=0.5 e b=0.4 .

In figura 2.6 sono riportate le prime prove effettuate. Come nel caso precedente si

pretende che il robot da (-0.4, -0.2) raggiunga il punto (0.3, 0.15); in questa prima verifica si è lasciato invariato il filtro di Kalman calcolato in 2.1.3, impostando il coefficiente M al valore unitario, e si sono confrontate le prestazioni per un K=10 e K=1 (figure A e B rispettivamente).

E’ doveroso fare alcune osservazioni comuni: come è facilmente intuibile dalle figure, le stime sulla posizione effettuate dalla rete wireless (cerchietti verdi), risultano essere decisamente grossolane e praticamente inutilizzabili al fine del controllo; solo le successive stime di Kalman (in rosso in figura) permettono al robot di raggiungere in maniera più o meno precisa il punto finale. Pur avendo una disposizione a “nuvola” attorno alla traiettoria effettuata, gli errori di stima si compensano tra di loro per il fatto che la rete è stata modellata aggiungendo un semplice rumore bianco a media nulla, ed essendo il periodo di campionamento relativamente basso (0.1 secondi) la traiettorie risultano abbastanza dolci e non troppo variabili in direzione. Tali percorsi rimangono ad ogni modo caratterizzati da un errore sul posizionamento finale dell’uniciclo: il punto viene raggiunto infatti a meno di un errore non più grande di 10 cm.

Simili considerazioni possono essere fatte osservando i diagrammi delle velocità di controllo: queste risultano infatti molto differenti rispetto a quelle precedenti (figura 2.5), essendo caratterizzate da oscillazioni e da picchi maggiori in quantità e valore. In questo caso la scelta del guadagno K risulta pressoché ininfluente: solo alcune differenze riguardano la variabilità degli ingressi di controllo che, come nel caso precedente (e in simulazione), risulta maggiore per guadagni più elevati.

La seconda serie di verifiche si basa su di un’importante considerazione: come si è già osservato, le rilevazioni di posizione e le relative stime calcolate basandosi sui dati di potenza di campo ricevuto (cerchietti verdi) sono prevedibilmente erronee, e l’utilizzo del filtro di Kalman risulta necessario. I risultati ottenuti in questo modo (cerchietti rossi) risultano infatti sicuramente più attendibili, pur rimanendo caratterizzati da un’incertezza nell’ordine dei 25cm: tale incertezza è dovuta al fatto che i calcoli eseguiti dal filtro risultano essere troppo legati alle misurazioni e il controllo, pur non essendo compromesso del tutto, risulta comunque impreciso, troppo potente e variabile.

Le successive prove vengono così effettuate impostando il coefficiente M a valori maggiori rispetto al caso precedente, in modo che venga dato più peso al modello piuttosto che alle misure y (paragrafo 2.1.3), ma anche in questo caso si rischia di andar in contro ad un’altra serie di problemi: si ricordi infatti che il modello del ‘sistema-Khepera’ non risulta completo e dettagliato, principalmente per la presenza del PID interno per l’attuazione dei controlli. Infatti oltre agli errori di quantizzazione, le velocità di riferimento non vengono raggiunte in modo istantaneo, presentando caratteristiche simili alle risposte di un sistema di secondo ordine ad un ingresso a gradino, con sovraelongazione e oscillazioni intorno al valori di regime.

- 24 -

Si sono quindi eseguiti più test con diversi M: in figura 2.7 sono riportati i risultati per M=20, e messi a confronto il caso con guadagno di controllo K=10 e K=1.

Le conseguenze di una non perfetta modellizzazione del sistema da controllare sono chiare se si osservano i diagrammi delle traiettorie: in questo caso le stime di Kalman sono sicuramente più verosimili in riferimento al percorso seguito dall’uniciclo e le velocità di controllo (in particolar modo la velocità lineare) risultano quindi meno variabili e più stabili; ma il fatto di non aver tenuto conto del PID interno causa degli squilibri nei calcoli della posizione da parte del filtro di Kalman. Quest’ultimo basandosi sul processo delle misure e sul processo da controllare, e avendo deciso di pesare maggiormente il secondo rispetto al primo, calcola delle stime di posizione che rischiano di compromettere la precisione del sistema complessivo: in figura si può notare infatti come le stime di Kalman (cerchietti rossi) siano spostate di circa 15 cm in avanti secondo la direzione e il verso della traiettoria seguita dal robot (come se tutta la traiettoria così calcolata fosse traslata), poiché il sistema su cui basa le proprie predizioni è un semplice sistema discreto (2.2.1) che risponde in modo istantaneo alle velocità di controllo, mentre il sistema fisico risulta più lento e impreciso. Tali problemi risultano comunque trascurabili per traiettorie di pochi metri.

Figura 2.7: Point to point: traiettorie e velocità di controllo del Khepera, con ambiente wireless simulato. A) K=10 B) K=1. In entrambi i casi il coefficiente M è stato impostato a 20 in modo che venga dato maggior peso al modello rispetto alle misure y (T=0.1s)

- 25 -

Anche in questo caso si possono fare le medesime considerazioni sulla scelta del guadagno K, e sul fatto che il punto finale viene raggiunto a meno di un errore di 10cm.

A questo punto non resta che testare l’algoritmo imponendo un tempo di campionamento maggiore, in modo da poter verificare l’effettiva prestazione dello stesso in un ambiente sicuramente più simile a quello con cui si avrà a che fare successivamente, per la verifica sperimentale. Viene quindi impostato il tempo di campionamento a T=0.5 secondi, ed eseguendo le prove sperimentali lasciando invariati i valori di K, M e soprattutto dei coefficienti a e b precedentemente introdotti per lo smorzamento delle velocità di controllo, si ottengono prestazioni non accettabili, caratterizzate da traiettorie decisamente poco precise o tendenti all’instabilità e da ingressi di controllo troppo variabili ed elevati in modulo. Tale comportamento è dovuto ovviamente al tempo di campionamento troppo alto: il controllore calcola degli ingressi di controllo che via via dovrebbero essere corretti in base alla posizione ricevuta in retroazione, ma un aggiornamento nell’ordine di soli 2 Hz e una retroazione non perfettamente valida come può essere quella calcolata dal filtro di Kalman , portano l’intero sistema a risultare troppo sensibile, rapido e incontrollabile.

Il problema si risolve semplicemente aggiustando i valori di a e b, in modo che gli ingressi attuati non risultino troppo intensi e potenti per l’uniciclo. Sempre in quest’ottica merita di essere fatta un’altra osservazione riguardo il coefficiente b, impiegato per lo smorzamento della velocità angolare. Mentre a viene abbassato al valore 0.2 per evitare che l’uniciclo venga portato (in un tempo di campionamento), troppo lontano dalla traiettoria prevista da un controllo esagerato (causato per esempio da una cattiva stima di posizione), il coefficiente b viene aumentato a 0.8, poiché la velocità angolare attuata è risultata troppo

bassa per un buon raggiungimento del punto desiderato. Come si può notare in figura (che non rappresenta una prova sperimentale, ma appare molto esplicativa), con lo smorzamento della velocità lineare non si hanno problemi di instabilità, ma il ridotto angolo di curvatura (per raggiungere il punto [0, 0.1]) che può essere attuato al Khepera ogni 0.5 secondi risulta insufficiente.

- 26 -

Poche a questo punto sono le considerazioni interessanti se si osservano in figura 2.8 i comportamenti dell’uniciclo per varie configurazioni di K ed M. Da notare il peggioramento delle prestazioni causato dall’aver aumentato il tempo di campionamento e la onnipresente imprecisione sul raggiungimento del punto finale.

Figura 2.8: Point to point: traiettorie e velocità di controllo del Khepera, con ambiente wireless simulato. A) K=10, M=1. B) K=10, M=1. C) K=5, M=100. Il tempo di campionamento impostato a T=0.5s ha reso necessario un diverso filtraggio delle velocità di controllo per farle risultare meno variabili e instabili.

- 27 -

Con un tempo di campionamento così elevato anche il filtro di Kalman lavora in modo pessimo (c’è da osservare ad ogni modo la non perfetta corrispondenza in simulazione tra tempo di campionamento impostato per il filtro e effettiva durata di tale periodo): nelle prove con M=1 si ha infatti una disposizione delle stime di posizione (cerchietti rossi) poco differenti rispetto a quelle ottenute con la rete simulata (verdi), e soltanto con valori di M>50 si ha un effettivo miglioramento.

Trajectory tracking Anche questa seconda serie di verifiche verrà intrapresa solo dopo aver adeguatamente testato l’algoritmo trascurando l’effetto della rete wireless.

In figura 2.9 sono riportati i risultati ottenuti implementando l’algoritmo (1.10), con le dovute differenze: dalle esperienze precedenti, la velocità lineare di controllo è stata smorzata fino all’80% secondo un opportuno coefficiente, impostabile arbitrariamente anche in seguito.

Figura 2.9: trajectory tracking - traiettorie e velocità di controllo del Khepera per diversi riferimenti e valori del guadagno K. A) traiettoria rettilinea. Velocità lineare di riferimento di 0.05m/s e velocità angolare nulla. Il riferimento parte da (-0.35, -0.2) con orientamento di π/6 B) traiettoria circolare. v=0.05m/s e ω=0.28rad/s

- 28 -

In entrambe le prove - riferimento in moto rettilineo uniforme (A), e riferimento in moto circolare uniforme (B) – si osservano comportamenti simili rispetto a quelli ottenuti nella simulazione eseguita precedentemente, arrivando alle medesime conclusioni: per guadagni elevati si hanno controlli meno pronti, potenti e precisi.

Per la verifica dell’algoritmo di trajectory tracking con ambiente wireless simulato si sono dovute applicare alcune leggere modifiche al codice, come nel caso precedente, per evitare che controlli troppo variabili in segno e modulo rischiassero di rendere instabile il sistema, o comunque compromettessero troppo le prestazioni. Come è già stato anticipato, viene impostato il coefficiente a (introdotto per v) a 0.8, ottenendo delle prestazioni assolutamente non accettabili: in figura 2.10 – A si può notare

Figura 2.10: trajectory tracking. Moto rettilineo uniforme, con riferimento a velocità lineare costante (0.05m/s) e velocità angolare nulla. Confronto tra due tipi di smorzamento e saturazione delle velocità di controllo: A) 80% B) 50%. Entrambe le prove sono effettuate per K=10 e T=0.1s.

- 29 -

come il percorso seguito dall’uniciclo risulti caratterizzato da repentini cambi di direzione e verso di percorrenza, causati dalle pessime stime di posizione calcolate dal filtro di Kalman (cerchietti rossi); si noti infatti che in questo caso M=1. Il controllo risulta essere troppo potente ed esageratamente elevato per quanto riguarda la velocità lineare, e soltanto abbassando a al valore di 0.5 si ottiene un miglioramento nell’inseguire il riferimento, figura 2.10 - B. Essendo attuati al Khepera controlli minori in modulo, l’uniciclo, pur ricevendo informazioni comunque grossolane in retroazione, tende ad andare fuori rotta con meno intensità; in questo modo i controlli stessi presentano picchi minori, e risultano decisamente meno variabili in segno.

Analogamente al paragrafo precedente, viene eseguita un’operazione di tuning sul parametro M relativo al filtro di Kalman, in modo che venga dato maggiore peso al modello piuttosto che alle misurazioni (cerchietti verdi): dopo alcune prove si sceglie M=20, e si confrontano le prestazioni per K=10, e K=1 (figura 2.11 – A,B rispettivamente).

Figura 2.11 – B trajectory traking: K=1

Figura 2.11 – A trajectory tracking: K=10

- 30 -

I risultati ottenuti avendo deciso di pesare maggiormente il modello sono sicuramente migliori dei precedenti, in particolare per K elevati, con l’unico inconveniente già affrontato in fase di implementazione del point to point: come si può osservare dalle figure 2.11 pur avendo a che fare con traiettorie più dolci e fedeli al riferimento da inseguire, le stime calcolate dal filtro di Kalman sembrano come traslate rispetto al percorso effettivamente seguito dall’uniciclo; come era già stato fatto notare, questo dipende dal fatto che in fase di modellizzazione non è stato preso in considerazione il PID interno al Khepera che attua i controlli. Anche in questo caso, per traiettorie di alcuni metri tali grossolane stime non compromettono le prestazioni dell’intero sistema.

Un’ultima osservazione merita il caso con K=1: un guadagno eccessivamente basso comporta degli ingressi di controllo troppo potenti (si ricordi che la velocità lineare del riferimento è 0.05m/s), e neanche con un tempo di campionamento relativamente basso (T=0.1s) l’uniciclo riesce ad inseguire in modo adeguato il riferimento. Ovviamente tale inconveniente deriva dall’utilizzo della rete wireless, che ad ogni ciclo rende disponibili informazioni errate sulla posizione del robot.

Vengono quindi eseguite ulteriori prove per diversi valori di K, M e successivamente T.

Figura 2.12: velocità lineare e angolare (v,ω) di controllo delle prove visualizzate in figura 2.11

Figura 2.13 - trajectory tracking: K=10, M=100, T=0.1s

- 31 -

In figura 2.13-2.14 sono state testate alcune configurazioni di parametri (K=10, M=100 e K=9, M=80 rispettivamente); le prove effettuate sono i risultati di un tuning manuale sui coefficienti introdotti, atto al miglioramento delle prestazioni inteso come, controlli meno potenti e variabili, traiettorie più dirette e morbide e in secondo luogo, stime di Kalman che risultino il più attendibili possibili.

Da notare ad ogni modo, come le stime di posizione di Kalman calcolate con un coefficiente M>50, risultino meno dispersive (pur mantenendo l’aspetto di traiettoria traslata) rispetto ai casi iniziali. Proprio per questo motivo i controlli attuati al Khepera sono meno oscillanti e variabili: le informazioni sulla posizione in retroazione sono ad ogni ciclo infatti più uniformi l’una con l’altra, e il controllore deve calcolare dei segnali meno potenti e meno correttivi.

Si conclude questa fase testando l’algoritmo con un tempo di campionamento decisamente superiore impostando T=0.5 secondi.

Figura 2.14 - trajectory tracking: K=9, M=80, T=0.1s

- 32 -

Come nel caso del point to point, un ulteriore smorzamento della velocità lineare di controllo è stato necessario (a=0.4) a causa del tempo di campionamento relativamente lungo; in questo modo si può rimanere decisamente soddisfatti della prestazione ottenuta.

____

Per completare questa sezione si intende applicare l’algoritmo di trajectory tracking per l’inseguimento da parte dell’uniciclo di un riferimento avente entrambe le velocità, lineare e angolare, impostate ad un valore costante, in modo che vengano eseguite traiettorie circolari.

Figura 2.14 - trajectory tracking: K=5, M=50, T=0.5s

- 33 -

Non ci si dilungherà troppo sulla spiegazione dei risultati ottenuti dalle varie prove effettuate, in quanto le osservazioni che si possono fare sul guadagno K, sul coefficiente M, su a e b (usati per lo smorzamento degli ingressi di controllo) sono del tutto simili a quelle già discusse precedentemente.

Figura 2.15 - trajectory tracking: traiettoria circolare. Prove effettuate con M=1 K=10 sopra, e M=60 K=50 sotto, entrambe con T=0.1s

- 34 -

In figura 2.15 si possono notare le differenze delle traiettorie effettuate con M=1 e M=60. In entrambi i casi il riferimento parte da (0, -0.2 ) con orientamento orizzontale (θ=0) velocità v=0.05m/s e ω=0.25rad/s; non avendo smorzato gli ingressi di controllo, le prestazioni per M <10 rimangono inaccettabili, in quanto le stime di Kalman, pur essendo decisamente migliori rispetto alle stime provenienti dalla rete wireless, ingannano il controllore che si vede costretto a correggere la traiettoria ad ogni ciclo con ingressi di controllo troppo potenti e variabili.

Solo una equilibrata scelta di a e M permette un raggiungimento ed un inseguimento del riferimento adeguato: impostando infatti M=60 e filtrando la velocità lineare fino al 50% (a=0.5) si ottiene la traiettoria rappresentata in figura 2.15 in basso. In figura 2.16 sono riportati i rispettivi ingressi di controllo: da notare la variabilità e la saturazione della velocità lineare della prima prova rispetto a quella successiva:

Figura 2.16 - trajectory tracking: ingressi di controllo delle prove riportate in figura 2.15

Figura 2.17 - trajectory tracking: A) M=10 K=1 B) traiettoria del riferimento circolare, con raggio maggiore rispetto ai casi precedenti e M=10 K=4. Entrambe con T=0.1s

- 35 -

In figura 2.17 sono riportate due ulteriori prove per confrontare diverse configurazioni nelle impostazioni dei parametri; inoltre in figura B) si osservi come facendo inseguire all’uniciclo un riferimento che compie traiettorie più ampie (la circonferenza ha un raggio maggiore) gli errori dovuti all’incertezza dei dati in retroazione risultino meno compromettenti per le prestazioni del sistema.

Infine in figure 2.18 - 2.19 sono riportate le ultime prove effettuate con un tempo di campionamento di T=0.5s: sia dalle traiettorie (in blu) che dai diagrammi delle velocità lineari di controllo si evince come un tempo di campionamento relativamente lungo in

Figura 2.18 – trajectory tracking: Prova effettuata con tempo di campionamento T=0.5s, M=10 e K=4. L’inseguitore parte da (0.25, -0.3): ad ogni ciclo vengono calcolati degli ingressi troppo potenti perché in mezzo secondo si abbia un inseguimento adeguato. La velocità lineare (non smorzata) risulta saturata e troppo variabile.

Figura 2.19 – trajectory tracking: Prova effettuata con tempo di campionamento T=0.5s, M=35 e K=1, con velocità lineare di ingresso smorzata del 50% ( a=0.5 ). L’inseguitore parte da (0.5, -0.5): in questo caso la velocità meno variabile e potente permette un inseguimento più preciso del riferimento, comunque non ancora pienamente accettabile.

- 36 -

combinazione con gli errori sulla stima della posizione rischi di compromettere il controllo, e solo con una seconda operazione di tuning sui parametri aggiuntivi (M e coefficienti per lo smorzamento degli ingressi) si ottengono inseguimenti adeguati. 2.3 Conclusioni Come si è potuto constatare, le difficoltà incontrate nel controllo dell’uniciclo simulando l’ambiente wireless risultano consistenti e sempre più presenti per tempi di campionamento lunghi. La possibilità di intervenire sul Khepera solo ogni 0.5 secondi spesso non permette di correggere le imperfezioni della traiettoria, dovute alle grossolane informazioni sulla posizione che si hanno in retroazione; le prestazioni risultano quindi compromesse, e non adeguate per un sistema normale.

Soltanto intervenendo manualmente sugli ingressi di controllo (piuttosto che sul guadagno K), aggiungendo parametri e coefficienti secondari per filtrarli, si riesce a fornire un controllo meno potente, meno rapido e quindi meno soggetto agli errori delle stime di posizione. Ed è proprio su questi, e sul coefficiente M per la scelta del peso da dare al modello o al processo delle misure in Kalman, che si lavorerà in fase di verifica sperimentale dell’intero sistema, argomento del prossimo capitolo.

- 37 -

- 38 -

CAPITOLO 3

Analisi sperimentale

Come si è visto nel capitolo precedente, l’aleatorietà delle stime sulla posizione, derivanti da semplici calcoli di triangolazione basati esclusivamente sull’RSSI (la potenza del campo ricevuto), sono la principale causa di un controllo troppo incerto e meno efficiente rispetto all’implementazione classica degli algoritmi visti nei paragrafi 1.2.1 e 1.2.2 . Pur utilizzando un filtro di Kalman a cavallo tra la rete wireless e il sistema da controllare si sono incontrate infatti svariate difficoltà: in particolare si ha avuto a che fare con ingressi di controllo esageratamente variabili e potenti, con un non perfetto aggancio ed inseguimento di un uniciclo di riferimento nel caso del trajectory traking, e un non adeguato raggiungimento del punto finale nel caso del point to point; problematiche che con l’aumento del tempo di campionamento diventano ancora più centrali ed importanti. Sempre dal capitolo precedente si è visto come solo con una seconda operazione di tuning su parametri e coefficienti aggiuntivi si sono raggiunte prestazioni sufficienti. 3.1 Utilizzo della rete wireless Come è già stato anticipato, la rete wireless utilizzata per la localizzazione è composta da 48 Motes, detti nodi ancora, di coordinate note; queste schede sono state posizionate sotto un piano liscio, a formare dei quadrati di lato l=40cm circa (figura 2.1), e volta per volta è possibile selezionare configurazioni specifiche e arbitrarie di tali nodi utilizzando, per esempio, solo quelli disposti sul perimetro piuttosto che una configurazione a scacchiera.

I nodi ancora hanno il compito di ricevere il segnale di campo trasmesso da un nodo mobile, che sarà posizionato sul Khepera, in modo che questo possa essere localizzato nello

spazio rispetto alle coordinate note. Le informazioni di potenza di campo ricevuto (valori di RSSI) raggiungono il calcolatore, predisposto per l’esecuzione dell’algoritmo di localizzazione, tramite un ulteriore nodo detto TOSBASE collegato via USB al calcolatore stesso: tale nodo agisce da ponte tra le informazioni radio e i dati seriali. Questo processo è gestito da una prima applicazione chiamata qtListen (figura a lato), che riceve i dati dal nodo Tosbase: questi vengono codificati e ritrasmessi in rete attraverso una porta arbitraria (TCP 26996).

- 39 -

I calcoli per la localizzazione sono affidati invece ad una seconda applicazione, indi-pendente dalla precedente: compass4Noks, che è dotato di un’interfaccia grafica con cui è possibile visualizzare differenti disposizioni topologiche dei nodi ancora (semplicemente caricando dei file di configurazione), agisce da client collegandosi alla rete, e, leggendo i pacchetti messi a disposizione da qtListen, recupera le informazioni necessarie per l’algoritmo di localizzazione.

Infine le stime di posizione così calcolate vengono nuovamente trasmesse in rete, in modo che Matlab possa sfruttarli per il controllo dell’uniciclo, collegandosi all’host tramite la porta adeguata, attraverso la semplice serie di comandi: t=tcpip(‘HOST_name’, PORT_number);

fopen(t)

data=fread(t);

fclose(t) E’ necessario fare un’ultima precisazione: l’applicazione compass4Noks, incaricata di calcolare le stime della posizione e di gestire il server per rendere disponibili tali dati in rete, è stata pensata per un tipo di localizzazione non indirizzata a scopi di controllo. Il nodo mobile, localizzato a meno di errori più o meno gravi, viene visualizzato su schermo, e mediando più misurazioni si ottengono buoni risultati; nel nostro caso non è possibile utilizzare i dati mediati (decisamente più stabili), poiché un tempo di campionamento troppo lungo necessiterebbe uno studio di algoritmi di controllo particolari e più complessi.

Per questo tipo di problema si veda per esempio la soluzione [3] in cui vengono studiati controllori per problemi di trajectory following e di posture stabilization con tempi di campionamento molto lunghi. L’idea di base è quella di calcolare segnali di controllo (feedback e feedforward) basati su archi di circonferenza, in accordo con il fatto che gli WMRs seguono proprio archi di circonferenza tra un tempo di campionamento e l’altro.

- 40 -

Prima di procedere con la verifica sperimentale vengono quindi eseguite alcune rilevazioni statiche, posizionando il nodo mobile in diversi punti di coordinate note rispetto alla coppia (x, y) di riferimento considerata in figura 3.1, ed effettuando più misurazioni della stima della posizione: si ricordi che il controllo dell’uniciclo verrà eseguito su una piastra piana e liscia, sotto la quale sano poste le 48 schede wireless (cerchi rossi in figura).

In figura 3.2 sono riportate nove delle misurazioni realizzate: le prestazioni

dell’algoritmo di localizzazione, discrete per quanto riguarda la localizzazione in display del nodo mobile, risultano essere mediocri per scopi di controllo; le stime di posizione (cerchietti blu in figura) oltre ad essere affette da grossolani errori, in media comunque non superiori ai 0.7m, risultano infatti caratterizzate da una asimmetria che molto probabilmente rischia di compromettere il controllo dell’uniciclo.

In modo diverso a seconda della posizione reale del nodo mobile infatti, le stime di posizione così calcolate sono enormemente differenti dalle stime ottenute avendo simulato la rete wireless: più che essere caratterizzate dalla presenza di rumore bianco di pari varianza sull’asse x e sull’asse y, risultano concentrate nella parte inferiore-centrale della piastra; questo, di fatto, restringe in modo drammatico l’area utile per il controllo dell’uniciclo. Non essendo individuato in modo corretto nella parte superiore della piastra (o comunque in modo assolutamente non omogeneo), il Khepera non potrà riceve i segnali di controllo adeguati. Tali imprecisioni sono dovute a molteplici fattori, difficilmente descrivibili in fase di modellizzazione; uno su tutti è l’estrema variabilità dell’ambiente in cui si sono eseguite le verifiche sperimentali: dalla mutevolezza continua dell’umidità dell’aria (il calore prodotto dalle schede wireless, connesse a diversi hubs USB comporta un veloce innalzamento della temperatura sotto la piastra) o la semplice presenza di più o meno persone in movimento nella stessa stanza, a segnali esterni provenienti dai ripetitori wireless installati in tutto l’edificio.

Figura 3.1: pianta del NAVLAB, laboratorio del DEI di Padova. Sono riportati gli assi di riferimento, le coordinate della piastra e la disposizione dei 48 nodi ancora.

- 41 -

E’ inoltre importante fare un’ulteriore precisazione; l’algoritmo di localizzazione prevede la raccolta di dati provenienti dalla rete wireless attraverso un canale radio dedicato, ma i 48 nodi ancora sono programmati in modo da trasmettere e ricevere continuamente (senza che ci sia un’effettiva richiesta da parte del nodo Tosbase): in questo modo il canale risulta spesso saturo di pacchetti. I dati che arrivano al calcolatore per essere elaborati, sono infatti un insieme di pacchetti provenienti solo da alcuni dei 48 nodi ancora nell’intervallo di mezzo secondo, e la selezione dei nodi sorgente è del tutto casuale. Da questo fatto deriva l’asimmetria rilevata in fase di misura, poiché probabilmente vi è una selezione di nodi che più facilmente di altri riesce a comunicare con il calcolatore.

Per risolvere questo tipo di problema si sono sperimentate altre configurazioni per i

nodi ancora disabilitandone alcuni, provando disposizioni perimetrali piuttosto che a scacchiera, senza ottenere comunque miglioramenti utili ai fini del controllo dell’uniciclo: per le prove effettuate successivamente ci si è appoggiati quindi alla rete composta da tutti i suoi 48 nodi.

Figura 3.2 - algoritmo di localizzazione: 50 stime di posizione (cerchietti blu) per 9 differenti posizioni del nodo mobile (croce nera). Le misure risultano essere asimmetriche e concentrate nella parte inferiore della piastra rendendone praticamente inutilizzabile la parte superiore per il controllo dell’uniciclo.

- 42 -

Probabilmente, possibili miglioramenti si sarebbero ottenuti installando i nodi ancora facendo riferimento al diagramma (o solido) di irradiazione dell'antenna presente nel datasheet delle schede wireless utilizzate, piuttosto che disporle in modo del tutto casuale. 3.2 Controllo del Khepera Il controllo in catena chiusa dell’uniciclo localizzato attraverso la rete wireless risulta decisamente meno efficiente rispetto ai risultati ottenuti in fase di simulazione: alcune prime prove di controllo sia mediante point to point che mediante trajectory tracking chiariscono le problematiche descritte nel precedente paragrafo, individuando i pesanti limiti imposti dalla aleatorietà non omogenea dei dati in retroazione.

Figura 3.3 – point to point: prime prove effettuate utilizzando la rete wireless reale. Le velocità di controllo non sono state smorzate. Si noti l’asimmetria delle stime di posizione (verdi), e delle rispettive stime di Kalman (rosse). A sinistra: (-1.7, 2.7) → (-2.5, 2.7). A destra (-2, 2.7) → (-3, 2.7).

Figura 3.4 – trajectory tracking: prime prove effettuate utilizzando la rete wireless reale. Le velocità di controllo non sono state smorzate. Si noti l’asimmetria delle stime di posizione (verdi), e delle rispettive stime di Kalman (rosse). Stato iniziale del riferimento = (-1.9, 2.7, π); stato iniziale dell’inseguitore = (-1.7, 2.5, π/2)

- 43 -

Come è facilmente desumibile dalle figure 3.3 e 3.4 l’asimmetria che caratterizza le stime di posizione calcolate basandosi sui soli valori di RSSI registrati dalla rete wireless è difficilmente eliminabile e dannoso per gli scopi prefissati, in entrambi i casi testati: anche i calcoli del filtro di Kalman risultano compromessi, sia nel caso si decida di dare maggior peso alle misure piuttosto che al modello (M=1) che viceversa (M=500). In quest’ultimo caso si ottengono comunque le prestazioni meno negative, con traiettorie più omogenee, lineari e caratterizzate da meno variazioni della direzione; anche i controlli in questo caso risultano essere decisamente migliori e meno variabili, ma troppo grande rimane la differenza tra traiettoria effettivamente seguita dal Khepera (nera e blu) e traiettoria stimata dal filtro di Kalman. In retroazione arrivano infatti informazioni coerenti per il controllo, ma inverosimili per la posizione: non appena l’uniciclo si trova ad essere fuori dall’area inferiore della piastra, non viene più localizzato in modo adeguato e riceve segnali di controllo che si basano su stime errate. Si tenga conto comunque che il tempo di campionamento del controllo (che dipende dal tempo di risposta della rete wireless e dal tempo necessario ad eseguire i calcoli dell’algoritmo) risulta relativamente lungo, T=0.65s; questo discorso verrà comunque affrontato nelle fasi conclusive (3.2.3).

Visti i risultati si è deciso di testare gli algoritmi con un coefficiente M del filtro di Kalman non inferiore ad un certo valore, e di lavorare, per quanto possibile, nella zona di piastra coperta in modo migliore dalla rete wireless; inoltre gli ingressi di controllo verranno adeguatamente smorzati a seconda dei casi.

3.2.1 Point to point Per evitare che l’uniciclo venga controllato con ingressi troppo grandi in ampiezza e che quindi rischi di trovarsi in poco tempo in una zona della piastra dove non può essere localizzato in modo corretto, le velocità lineare e angolare vengono smorzate impostando i soliti coefficienti a=0.4 e b=0.8 rispettivamente. Vengono quindi eseguite più prove, con diversi valori per i parametri M e K, anche se un vero confronto risulta poco utile in quanto le prestazioni dipendono principalmente dal comportamento (aleatorio) della rete wireless.

Figura 3.5 – point to point: da (-2.3, 2.7) con orientamento orizzontale θ=π il Khepera deve raggiungere il punto (-3.1, 2.7). Le prestazioni in questo caso risultano migliori che in figura 3.3 perché la prova è stata effettuata nella zona inferiore e centrale della piastra, dove cioè la localizzazione è migliore.

- 44 -

Figura 3.6 – point to point: A) e B) da (-2, 2.7) con orientamento orizzontale θ=π il Khepera deve raggiungere il punto (-3, 2.7). C) da (-3.5, -3.5) con orientamento verticale θ=-π/2, a (-2, 2.7) Le differenze tra le tre prove sono dovute alle differenti prestazioni della rete durante le esperienze, piuttosto che ai diversi valori di K e M.

- 45 -

Le prove così effettuate risultano sicuramente migliori rispetto al caso precedente grazie al fatto che l’uniciclo è stato fatto muovere entro precisi limiti, ovvero nella zona di piastra consigliata dalle misurazioni riportate in figura 3.2; anche se sono pochi i confronti e i commenti utili è rilevante osservare le differenze tra traiettoria stimata (dal filtro di Kalman), e la traiettoria reale (in nero). 3.2.2 Trajectory tracking Il caso in esame prevede l’utilizzo di un Khepera come robot inseguitore, e un robot target virtuale generato da Matlab, che si sposta con moto rettilineo uniforme, e di cui sono note ad ogni istante le coordinate.

Anche per questa serie di verifiche si è deciso di smorzare gli ingressi di controllo (a e b come nel caso precedente), per evitare che il Khepera fosse troppo soggetto a stime della posizione eccessivamente errate e ricevesse controlli troppo potenti ed impropri.

Figura 3.7 – trajctory tracking: A) da (-1.75, 2.4) con orientamento verticale θ=π/2 il Khepera deve inseguire il riferimento in moto rettilineo uniforme (v=0.05m/s) con stato iniziale: [-1.8, 2.7, π]. M=4, K=5. B) da (-1.9, 2.4) con orientamento verticale θ=π/2 il Khepera deve inseguire il riferimento in moto rettilineo uniforme (v=0.05m/s) con stato iniziale: [-1.8, 2.7, π]. M=10, K=10.

- 46 -

Figura 3.8 – trajctory tracking: A) da (-1.7, 2.5) con orientamento verticale θ=π/2 il Khepera deve inseguire il

riferimento in moto rettilineo uniforme (v=0.05m/s) con stato iniziale: [-1.8, 2.7, π]. M=300, K=5.

B) da (-1.7, 2.4) con orientamento verticale θ=π/2 il Khepera deve inseguire il riferimento in moto rettilineo uniforme (v=0.05m/s) con stato iniziale: [-1.9, 2.7, π]. M=500, K=10.

C) come caso A) M=1000, K=3

- 47 -

Anche in questo caso sono poche le osservazioni utili (o che non si sono già fatte in fase di simulazione) che si possono fare osservando le figure 3.7, 3.8 : da notare come scegliendo l’area di piano meglio coperta dalla rete wireless le prestazioni del controllo siano accettabili in quasi tutte le prove effettuate, con traiettorie più lineari e dirette laddove si decide di dare maggior peso al modello nel filtro di Kalman.

Si è deciso di tralasciare l’analisi del trajectory tracking con riferimenti in moto

circolare uniforme; testato in simulazione con buoni risultati, l’algoritmo non si è dimostrato altrettanto valido durante l’analisi sperimentale a causa degli errori rilevati nelle misurazioni della rete wireless, e del tempo di campionamento eccessivamente lungo. 3.2.3 Difficoltà nell’implementazione degli algoritmi di controllo Come è stato anticipato in 3.2, il tempo di campionamento del controllo per le prove riportate in figura 3.5, 3.6, 3.7, e 3.8 è stato impostato a T=0.65s circa: tralasciando i commenti sulla difficoltà nel controllare un uniciclo con algoritmi pensati per tempi di campionamento decisamente inferiori (a cui si è posto rimedio smorzando gli ingressi di controllo), è doveroso invece soffermarsi sulle difficoltà incontrate in fase di implementazione in Matlab.

Il tempo di campionamento del controllo è strettamente connesso alla velocità di risposta della rete wireless che si assesta sui 0.5 secondi, e alla velocità di Matlab nell’elaborare i calcoli (si veda il codice presente in appendice); i programmi scritti in Matlab per l’implementazione sia del point to point che del trajectory tracking, sono basati principalmente su un ciclo for con un tempo di esecuzione ad iterazione di circa 0.15 secondi, in cui vengono registrate le velocità delle ruote del Khepera (mediando più misurazioni consecutive con un ulteriore ciclo for annidato) per poter poi ‘plottare’ le traiettorie effettivamente seguite dal robot.

Ogni 4 o 5 iterazioni (questa ovviamente è una scelta arbitraria) si ha la possibilità di interrogare la rete, recuperare le informazioni sulla localizzazione dell’unicilo, eseguire i calcoli necessari al filtro di Kalman e filtrare le misure; queste operazioni spesso risultano molto differenti in tempo di esecuzione. Si ha quindi una variabilità del tempo di campionamento che comporta difficoltà nel controllo, nella stima per la localizzazione e soprattutto nella corretta esecuzione da parte del filtro di Kalman delle predizioni sulla posizione. Questo tipo di problema, oltre all’asimmetria rilevata nelle misure della rete wireless, sono state le principali cause delle non ottime performance dei controllori studiati e poi implementati.

- 48 -

Conclusioni In base al lavoro svolto su questo progetto si può ritenere di aver analizzato validi algoritmi di controllo per il Khepera; non sono state poche comunque le difficoltà incontrate, soprattutto in fase di verifica sperimentale.

Pur non avendo vere specifiche da soddisfare le prestazioni sono risultate ad ogni modo ottime sia in fase di simulazione di entrambi gli algoritmi, che in fase di simulazione della rete wireless, e con le dovute modifiche, quali lo smorzamento degli ingressi di controllo e l’operazione di tuning manuale ed empirico del filtro di Kalman, si è sempre riusciti a compensare i grossolani errori delle stime della posizione, dovute all’aver modellizzato l’effetto della rete aggiungendo un consistente rumore bianco nei dati in retroazione all’uniciclo.

Peggiori invece i comportamenti dei controllori durante l’analisi sperimentale: l’algoritmo di localizzazione utilizzato, caratterizzato da un’asimmetria nelle stime della posizione del Khepera (sbilanciate verso il centro inferiore della piastra su cui si sono eseguite le prove) si è dimostrato spesso inadeguato agli scopi di controllo, permettendo comunque di far eseguire solo alcuni dei task provati in simulazione; è stata tralasciata infatti l’analisi del trajectory tracking per riferimenti in moto circolare uniforme a causa dell’impossibilità di utilizzare in modo corretto l’intera piastra.

Si sono comunque osservate le differenze e le migliorie introdotte dall’utilizzo del filtro di Kalman necessario in questo tipo di progetti, e le varie risposte a seconda della scelta del parametro M, introdotto per differenziare e meglio gestire la priorità da dare alle misure rispetto al modello o viceversa. Più approfonditi studi sarebbero da fare circa il tuning del filtro di Kalman, e sul miglior valore da assegnare alle varianze dei processi aleatori che modellizzano il sistema considerato in (2.6) e in particolar modo il processo che modellizza il rumore di modello, che in questa sede è stato interpretato semplicemente come un ingresso incognito, una forza esterna che determina la traiettoria seguita dal Khepera.

Sono molte, inoltre, le migliorie che si possono applicare sia agli algoritmi di controllo, con particolare attenzione alla temporizzazione degli eventi, che all’algoritmo di localizzazione non analizzato in dettaglio in questo lavoro. Infine un importante ruolo rivestono i metodi alternativi che possono essere utilizzati per il controllo dell’uniciclo con un tempo di campionamento decisamente più lungo, che permetta di mediare più misurazioni della rete wireless, e renda possibile compensare in parte l’asimmetria che caratterizza le sue stime della posizione.

- 49 -

- 50 -

Appendice

- 51 -

A.1 Il Khepera II

Sviluppato dalla K-team, il Khepera II è un robot a due ruote dotato di un microprocessore Motorola 68331 con le seguenti caratteristiche:

• Memoria RAM 512Kbytes • Memoria flash programmabile 512Kbytes • Due motori DC indipendenti che controllano le ruote mediante encoder (velocità da 0.03m/s a 1m/s a passo di 0.03m/s) • 8 sensori di prossimità infrarossi (range massimo circa 10cm) • Comunicazione seriale • Alimentazione esterna o batterie ricaricabili • Diametro: 70mm • Altezza: 30mm • Peso: 80g La comunicazione con il robot è stata gestita interamente con Matlab (comunicazione ASCII attraverso porta seriale), come pure tutti gli algoritmi di controllo e pianificazione di traiettorie. Ci sono due possibili configurazioni in cui si può utilizzare il Khepera: controllo in posizione (in cui gli ingressi sono le distanze che devono percorrere le due ruote) e controllo in velocità (in cui gli ingressi sono le velocità che si desidera raggiungano le due ruote). Il microprocessore montato sul veicolo opera un controllo di tipo PID sulla velocità o sulla posizione (a seconda del tipo di comando dato dall’utente). Nella Tab. 1 sono elencati principali comandi:

Tab. 1: i principali comandi del Khepera

- 52 -

A.2 Codice Matlab POINT TO POINT: function [dati] = ptpLAB2(khepera, x, y, theta, x_fin, y_fin, L, M, K) %% INIZIALIZZAZIONE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Inizializzazzione comunicazione RETE t=tcpip('navlab15',50810); set(t,'InputBufferSize',10); set(t,'Timeout',0.1) pause(0.5); % Inizializzazione Vettori "posizione" e "velocita'" X = zeros(1,L); X(i) = x; x_i = x; Y = zeros(1,L); Y(i) = y; y_i = y; TH =zeros(1,L); TH(i) = theta; th = theta; V = zeros(1,L); V(i) = 0; v = 0; W = zeros(1,L); W(i) = 0; w = 0; velo=[0; 0]; r=0; l=0; data6 = kcmd(khepera, sprintf('G,%d,%d', 0, 0)); speed_left = zeros(1,5); speed_right = zeros(1,5); % Inizializzazione parametri per il filtro di KALMAN % KALMAN % sigmaC=0.5; sigmaW=0.01; % C.I. % Px=[1 0; 0 1]; px11=Px(1); px21=Px(2); px12=Px(3); px22=Px(4); Py=[1 0; 0 1]; py11=Py(1); py21=Py(2); py12=Py(3); py22=Py(4); stimaX=[0 x]'; sx1=stimaX(1); sx2=stimaX(2); stimaY=[0 y]'; sy1=stimaY(1); sy2=stimaY(2); Kx=[1;1]; kx1=Kx(1); kx2=Kx(2); Ky=[1;1]; ky1=Ky(1); ky2=Ky(2); % misure % X_s = zeros(1,L); X_s(i)= x ; x_s = x; Y_s = zeros(1,L); Y_s(i)= y ; y_s = y; X_k = zeros(1,L); X_k(i)= x ; x_k = x; Y_k = zeros(1,L); Y_k(i)= y ; y_k = y; % Inizializzazione altri parametri e tempi i = 1; Tc = 0.05; TK= 0.65; T=0.1; scr='OK!'; scr1='NON LETTA POSIZIONE' %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% pause(0.08) t2=clock;

- 53 -

CICLO DI CONTROLLO %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for n = [1:1:L], t0 = clock; i = i + 1; % aggiornamento indice %% lettura velocita' for m = [1:1:5], data = kcmd(khepera,'E'); speed = sscanf(data,'e,%d,%d'); if length(speed)==2, velo=speed; speed_left(m) = speed(1)*8/1000; speed_right(m) = speed(2)*8/1000; else speed_left(m)=velo(1)*8/1000; speed_right(m)=velo(2)*8/1000; end end speedl_mis_m = sum(speed_left )/5; speedr_mis_m = sum(speed_right)/5; v_mis = (1/2)*(speedl_mis_m + speedr_mis_m); sx1=v_mis*cos(theta); sy1=v_mis*sin(theta); data5 = kcmd(khepera,'H'); pos = sscanf(data5,'h,%d,%d'); if length(pos)==2, l=pos(1); r=pos(2); %scr; else scr1 pos(1)=l+speedl_mis_m*T*(120*2.75); pos(2)=r+speedr_mis_m*T*(120*2.75); end theta = th +(((pos(2)-pos(1))/2)/(120*2.75)); T = etime(clock, t0) + Tc; %% modello cinematico x = x + v_mis*cos(theta)*T ; X(n) = x; y = y + v_mis*sin(theta)*T ; Y(n) = y; %% MISURE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% if rem(n,4)==0 %ogni quattro T prendo la misura TK=etime(clock,t2)+Tc % tempo tra una misurazione e l'altra t2=clock; coordinate_rete=getXY(t, x_s, y_s); x_s = coordinate_rete(1); X_s(n)= x_s; y_s = coordinate_rete(2); Y_s(n)= y_s ; if y > 2.9 y_s=y_s+0.2; end

- 54 -

%%%%%%%%%%%%%%%%%%% KALMAN %%%%%%%%%%%%%%%%%%%%% Tm=TK; % tempo di campionamento di Kalman A=[1 0; Tm 1] ; B=[Tm (Tm^2)/2]'; C=[0 1]; D=1; Q = (1/M)*sigmaW*[Tm (Tm^2)/2;(Tm^2)/2 (Tm^3)/3]; R = M*(sigmaC)^2; risX= KALMA(A, C, Q, R, px11, px21, px12, px22, kx1, kx2, sx1, sx2, x_s); px11=risX(1); px21=risX(2); px12=risX(3); px22=risX(4); kx1=risX(5); kx2=risX(6); sx1=risX(7); sx2=risX(8); x_k = sx2; X_k(n) = x_k; % calcolo stima X risY= KALMA(A, C, Q, R, py11, py21, py12, py22, ky1, ky2, sy1, sy2, y_s); py11=risY(1); py21=risY(2); py12=risY(3); py22=risY(4); ky1=risY(5); ky2=risY(6); sy1=risY(7); sy2=risY(8); y_k = sy2; Y_k(n) = y_k; % calcolo stima Y %%%%%%%%%%%%%%%%%%% CONTROLLO %%%%%%%%%%%%%%%%%%%%%% cor = ChangeWorld(x_k, y_k, theta, x_fin, y_fin); Rho = cor(1); Phi = cor(2); Beta = cor(3); controlli = Point_control(Rho, Phi, Beta, K); v = controlli(1); w = controlli(2); [vel] = converti(v, w); kcmd(khepera, sprintf('D,%d,%d', vel(1), vel(2)); V(n) = v; W(n) = w; Tc=etime(clock,t2); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% else V(n) = v; W(n) = w; pause(Tc) end end % fine ciclo di controllo %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% STOP %%%%%%%%%%%%%%%%%%%%%%%%%% kcmd(khepera, sprintf('D,%d,%d', 0, 0)); %% PLOTTA %%%%%%%%%%%%%%%%%%%%%%%%%%% figure plot(X_s,Y_s,'go') hold on plot(X_k,Y_k,'ro') plot(X,Y,'k') plot(x_i,y_i,'bx') %punto iniziale e finale plot(x_fin,y_fin,'bx') %% plotta velocità figure plot(V,'r') ylabel('v controllo'); figure plot(W,'b') ylabel('w controllo');

- 55 -

TRAJECTORY TRACKING: function [dati]=trkLAB2(khepera,a,b,c,vel,omega, a_r, b_r, c_r, vel_r, omega_r,L,M,K) %% INIZIALIZZAZIONE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Inizializzazzione comunicazione RETE t=tcpip('navlab15',55182); set(t,'InputBufferSize',10); set(t,'Timeout',0.1) pause(0.5); % Inizializzazione Vettori "posizione" e "velocita'" X_R=zeros(1,L); X_R(1)=a_r; Y_R=zeros(1,L); Y_R(1)=b_r; THETA_R=zeros(1,L); THETA_R(1)=c_r; X=zeros(1,L); X(1)=a; Y=zeros(1,L); Y(1)=b; THETA=zeros(1,L); THETA(1)=c; th=c; V=zeros(1,L); v=0; W=zeros(1,L); w=0; velo=[0; 0]; r=0; l=0; data6 = kcmd(khepera, sprintf('G,%d,%d', 0, 0)); speed_left = zeros(1,3); speed_right = zeros(1,3); % Inizializzazione parametri per il filtro di KALMAN % KALMAN % sigmaC=0.5; sigmaW=0.01; % C.I. % Px=[1 0; 0 1]; px11=Px(1); px21=Px(2); px12=Px(3); px22=Px(4); Py=[1 0; 0 1]; py11=Py(1); py21=Py(2); py12=Py(3); py22=Py(4); stimaX=[0 a]'; sx1=stimaX(1); sx2=stimaX(2); stimaY=[0 b]'; sy1=stimaY(1); sy2=stimaY(2); Kx=[1;1]; kx1=Kx(1); kx2=Kx(2); Ky=[1;1]; ky1=Ky(1); ky2=Ky(2); % misure % X_s = zeros(1,L); X_s(I)= a ; x_s = a; Y_s = zeros(1,L); Y_s(I)= b ; y_s = b; X_k = zeros(1,L); X_k(I)= a ; x_k = a; Y_k = zeros(1,L); Y_k(I)= b ; y_k = b; % Inizializzazione altri parametri e tempi I=1; % puntatore Z=1; % puntatore per le velocita' Tc = 0.05; TK= 0.6; %%%%%%%%%%%%%%%%%%%%%%%%%%%%% pause(0.08) t2=clock;

- 56 -

%% CICLO DI CONTROLLO %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for n=[1:1:L], t0 = clock; I=I+1; % aggiornamento puntatore %% lettura velocita' for m = [1:1:3], data = kcmd(khepera,'E'); speed = sscanf(data,'e,%d,%d'); if length(speed)==2, velo=speed; speed_left(m) = speed(1)*8/1000; speed_right(m) = speed(2)*8/1000; else speed_left(m)=velo(1)*8/1000; speed_right(m)=velo(2)*8/1000; end end speedl_mis_m = sum(speed_left )/3; speedr_mis_m = sum(speed_right)/3; v_mis = (1/2)*(speedl_mis_m + speedr_mis_m); sx1=v_mis*cos(c); %aggiornamento stato di Kalman sy1=v_mis*sin(c); data5 = kcmd(khepera,'H'); pos = sscanf(data5,'h,%d,%d'); if length(pos)==2, r=pos(1); l=pos(2); else pos(1)=r; pos(2)=l; end c = th +(((pos(2)-pos(1))/2)/(120*2.75)); %%RIFERIMENTO IN MOVIMENTO%%%%%%%%%%%%%%%%%%%%%%%% V_r = vel_r(Z); W_r = omega_r(Z); T = etime(clock, t0) + Tc pos_r = processo(a_r, b_r, c_r, V_r, W_r, T); a_r=pos_r(1); X_R(I)=a_r; b_r=pos_r(2); Y_R(I)=b_r; c_r=pos_r(3); THETA_R(I)=c_r; %% modello cinematico %%%%%%%%%%%%%%%%%%%% a = a + v_mis*cos(c)*T ; X(n) = a; b = b + v_mis*sin(c)*T ; Y(n) = b; %% MISURE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% if rem(n,4)==0 TK=etime(clock,t2)+Tc % tempo tra una misurazione e l'altra t2=clock; coordinate_rete=getXY(t,a_r,b_r); x_s = coordinate_rete(1); X_s(n)= x_s; y_s = coordinate_rete(2); Y_s(n)= y_s ;

- 57 -

%%%%%%%%%%%%%%%%%%% KALMAN %%%%%%%%%%%%%%%%%%%%% Tm=TK; % tempo di campionamento di Kalman A=[1 0; Tm 1] ; B=[Tm (Tm^2)/2]'; C=[0 1]; D=1; Q = (1/M)*sigmaW*[Tm (Tm^2)/2;(Tm^2)/2 (Tm^3)/3]; R = M*(sigmaC)^2; risX= KALMA(A, C, Q, R, px11, px21, px12, px22, kx1, kx2, sx1, sx2, x_s); px11=risX(1); px21=risX(2); px12=risX(3); px22=risX(4); kx1=risX(5); kx2=risX(6); sx1=risX(7); sx2=risX(8); x_k = sx2; X_k(n) = x_k; risY= KALMA(A, C, Q, R, py11, py21, py12, py22, ky1, ky2, sy1, sy2, y_s); py11=risY(1); py21=risY(2); py12=risY(3); py22=risY(4); ky1=risY(5); ky2=risY(6); sy1=risY(7); sy2=risY(8); y_k = sy2; Y_k(n) = y_k; %%%%%%%%%%%%%%%%%%% CONTROLLO %%%%%%%%%%%%%%%%%%%%%% Tx= [cos(c) sin(c) -a*cos(c)-b*sin(c); -sin(c) cos(c) a*sin(c)-b*cos(c); 0 0 1]; E= Tx*[a_r b_r 1]'; e1= E(1); E_1(n)=e1; e2= E(2); E_2(n)=e2; e3= c_r-c; controlli = track_control(V_r, W_r, e1, e2, e3, K); v = controlli(1); w = controlli(2); Z=Z+1; % aggiornamento contatore velocita' [vel] = converti(v, w); kcmd(khepera, sprintf('D,%d,%d', vel(1), vel(2))); V(n) = v; W(n) = w; Tc=etime(clock,t2) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% else V(n) = v; W(n) = w; pause(Tc) end end % fine ciclo di controllo %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% STOP %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% kcmd(khepera, sprintf('D,%d,%d', 0, 0)); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% PLOTTA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% plot(X_s,Y_s,'go') hold on plot(X_k,Y_k,'ro') plot(X_R,Y_R,'k') plot(X,Y,'b') figure plot(V,'r') ylabel('v controllo') figure plot(W,'b') ylabel('w controllo');

- 58 -

A.3 Il metodo diretto di Lyapunov

Molti tra i più importanti strumenti a disposizione per il controllo di sistemi non lineari derivano dalla teoria di Lyapunov [1]-[2]:

Proposizione 2.1. (criterio di stabilità di Lyapunov). Sia (x, u) = (0, 0) punto di equilibrio per il sistema ( , )x f x u=& , con nx ∈ R e mu ∈ R . Si consideri una funzione

1( )V x C∈ definita positiva, e si consideri la sua derivata direzionale lungo il campo

vettoriale f(x,u), cioè ( )( ) ( , )V xV x f x ux

∂=

∂& .

Se ( )V x& è semidefinita negativa l'equilibrio del sistema è stabile. Se ( )V x& è definita negativa l'equilibrio del sistema è asintoticamente stabile.

In linea di principio, per tutti i tipi di controllo operati sul Khepera, si è proceduto nel

modo seguente: • Ricerca del punto di equilibrio del sistema da stabilizzare (ad esempio, controllare a

zero l’errore di posizione e di orientazione). • Scelta della funzione di Lyapunov V(x, y, θ ) per l’analisi della stabilità. • Sintesi della legge di controllo in base al segno di ( , , )V x y θ& .

- 59 -

- 60 -

Bibliografia [1] Enrico Piaggio. Dinamica e Controllo dei Veicoli Robotica, 2004. [2] Luca Parolini . Metodi di localizzazione per reti di sensori wireless, 2006. [3] Marco Caregaro Negrin, Andrea Nicolettis, Paolo Erasmo Pucci, Luca Schenato. Very-

low sampling rate control of wheeled nmobile robots using circles. 2006 [4] Luca Schenato. Optimal estimation in networked control system subject to random

delay and packet drop. http://www.dei.unipd.it/~schenato/PAPERS/ [5] Luca Schenato, Appunti delle lezioni del corso di progettazione di sistemi di controllo,

Anno Accademico 2005-06 [6] Giuseppe Oriolo, Alessandro de Luca, Marilena Vendittelli. WMR Control Via dynamic

feedback linearization: design, implementation, and esperimental validation. IEEE Transactions on control system technology, vol. 10. 2002.

[7] Paramvir Bahl, Venkata N. Padmanabhan. RADAR: An in-building RF-based usr

location and tracking system. Microsoft Research. [8] Giovanni Barbera. Controllo del Khepera: modellizzazione, simulazione e sintesi di

algoritmi di controllo per l'uniciclo. Elaborato laurea triennale in Ingegneria dell’Automazione presso l’Università di Padova. 2006

Shankar Sastry. Nonlinear Systems. Analysis, stability, and control. Springer-Verlag,NY, 1999. Jean-Jacques E. Slotine, Weiping Li. Applied nonlinear control. Prentice Hall, NJ, 1991. J. Jakubiak, E. Lefeber, K. Tchon, H. Nijmeijer. Two observer-based tracking algorithms for a unicycle mobile robot. Int. J. Appl. Math. Comput. Sci., vol. 12, 2002. Steven M. LaValle. Planning Algorithms. Cambridge University Press, 2006. ByungMoon Kim, Panagiotis Tsiotras. Controllers for unicycle-type wheeled robots: theoretical results and experimental validation. IEEE Transactions on robotics and automation, vol. 18. 2002.