Università degli Studi di Roma “La Sapienza”
Facoltà di Ingegneria
Tesi di Laurea in
Ingegneria Elettronica
Pianificazione del moto per robot mobili
anolonomi
Emanuele Mancino
Anno Accademico 2005 - 2006
Università degli Studi di Roma “La Sapienza”
Facoltà di Ingegneria
Tesi di Laurea in Ingegneria Elettronica
Pianificazione del moto per robot mobili
anolonomi
Emanuele Mancino
Relatore
Prof. Giuseppe Oriolo
...........................
Co-Relatore
Ing. Marilena Vendittelli
...........................
Anno Accademico 2005 - 2006
Indirizzo e-mail dell’Autore:
[email protected]
Questa tesi è disponibile al sito:
Indice
Introduzione
1
1 Robotica mobile e esplorazione di ambienti sconosciuti
3
1.1
Obiettivo della tesi e sue applicazioni . . . . . . . . . . . . . . . .
4
1.2
Problema dell’esplorazione di ambienti ignoti . . . . . . . . . . . .
5
1.2.1
La localizzazione . . . . . . . . . . . . . . . . . . . . . . .
6
1.2.2
La pianificazione del moto e obstacle avoidance . . . . . .
7
1.3
Algoritmo di esplorazione SRT . . . . . . . . . . . . . . . . . . . .
7
1.4
Robot mobili su ruote . . . . . . . . . . . . . . . . . . . . . . . .
8
1.4.1
Strutture cinematiche dei WMR . . . . . . . . . . . . . . .
10
1.4.2
I sensori . . . . . . . . . . . . . . . . . . . . . . . . . . . .
14
2 Descrizione del robot mobile Magellan Pro
2.1
2.2
16
Il robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
16
2.1.1
I Sonar . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
18
2.1.2
I sensori a raggi infrarossi . . . . . . . . . . . . . . . . . .
19
2.1.3
I sensori di contatto
. . . . . . . . . . . . . . . . . . . . .
19
2.1.4
Le comunicazioni . . . . . . . . . . . . . . . . . . . . . . .
19
La telecamera . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
20
i
2.3
Architettura software . . . . . . . . . . . . . . . . . . . . . . . . .
22
2.3.1
Corba e NamingService . . . . . . . . . . . . . . . . . . . .
22
2.3.2
Il Base Server . . . . . . . . . . . . . . . . . . . . . . . . .
23
2.3.3
Il Frame-grabber Server ed il Pan-tilt Server . . . . . . . .
24
2.3.4
MOM . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
25
Il modello cinematico . . . . . . . . . . . . . . . . . . . . . . . . .
26
2.4.1
Vincoli per le ruote ad orientamento fisso . . . . . . . . . .
29
2.4.2
Vincoli per le ruota anteriore . . . . . . . . . . . . . . . .
30
2.4.3
Equazioni del modello . . . . . . . . . . . . . . . . . . . .
31
2.4.4
Il modello nelle velocità lineari delle ruote . . . . . . . . .
32
3 Nonholonomic Motion Planning e Reactive Path Deformation
34
2.4
3.1
3.2
3.3
Introduzione ai sistemi anolonomi [6] . . . . . . . . . . . . . . . .
35
3.1.1
Vincoli cinematici . . . . . . . . . . . . . . . . . . . . . . .
36
3.1.2
Punto di vista del controllo . . . . . . . . . . . . . . . . .
38
Nonholonomic Motion Planning . . . . . . . . . . . . . . . . . . .
39
3.2.1
Chained Forms . . . . . . . . . . . . . . . . . . . . . . . .
40
3.2.2
Implementazione in Matlab . . . . . . . . . . . . . . . . .
42
Reactive Path Deformation for Nonholonomic Mobile Robots [4] .
46
3.3.1
3.4
Nonholonomic Path Deformation come sistema dinamico di
controllo . . . . . . . . . . . . . . . . . . . . . . . . . . . .
47
3.3.2
Algoritmo di Nonholonomic Path Deformation . . . . . . .
50
3.3.3
Implementazione in MatLab . . . . . . . . . . . . . . . . .
58
Unione dei due algoritmi ( 3.2 e 3.3) . . . . . . . . . . . . . . . .
62
4 Controllo in feedback sul cammino pianificato
ii
66
4.1
Dynamic Feedback Linearization [5] . . . . . . . . . . . . . . . . .
67
4.2
Simulazioni MatLab con DFL . . . . . . . . . . . . . . . . . . . .
70
Bibliografia
70
iii
Introduzione
In questa tesi ci si è occupati della pianificazione del moto di robot mobili
anolonomi immersi in uno spazio sconosciuto utilizzando un pianificatore per la
generazione di traiettorie e un algoritmo di deformazione in grado di modificare
la traiettoria pianificata nel caso questa fosse in collisione con ostacoli presenti
nello spazio di esplorazione. I problemi da affrontare per raggiungere l’obiettivo
finale sono stati principalmente i seguenti:
• Pianificazione di una traiettoria a partire da un punto iniziale prestabilito
che interpolasse quest’ultimo con un punto finale stabilito ( detto goal ).
• Esecuzione della traiettoria pianificata e iseguimento della stessa.
• Deformazione reattiva del percorso nel caso di presenza di ostacoli sulla
traiettoria pianificata.
• Ricerca di una legge di controllo adeguata per l’inseguimento della
traiettoria da parte del robot.
• Simulazione dei risultati teorici tramite l’utilizzo del software MathLab.
• Infine sperimentazione dei risultati teorici tramite l’utilizzo di un modello
del robot Magellan Pro in ambiente Webots, fornito dalla Cyberbotics.
1
Introduzione
2
La stesura della tesi rispecchia pienamente il percorso secondo il quale si sono
svolti i lavori.
Il primo capitolo comprende un’introduzione alla robotica mobile, al problema
di esplorazione di ambienti ignoti, al metodo SRT ( Sensor-based Random Tree)
e chiarisce l’utilizzo dello studio eseguito nell’applicazione dell’SRT. Il secondo
capitolo descrive brevemente le caratteristiche tecniche del robot Magellan Pro.
Nel terzo capitolo viene effettuata una breve trattazione sulla pianificazione
di traiettorie e sulla loro deformazione da parte dell’algoritmo “Reactive Path
Deformation”.
Nel quarto capitolo viene descritto il contollo in “Dynamic
Feedback Linearization” sul cammino pianificato. Nel quinto capitolo vengono
presentati i risultati delle simulazioni e delle sperimentazioni in Webots e una
breve descrizione dell’ambiente di simulazione.
Infine chiude la tesi il sesto
capitolo con un’esposizione di alcune idee per dei possibili sviluppi futuri.
Capitolo 1
Robotica mobile e esplorazione di
ambienti sconosciuti
La robotica ha come obiettivo lo studio di macchine che possano sostituire l’uomo
nell’esecuzione di un compito, sia in termini di attività fisica che decisionale. Nel
corso dei secoli l’uomo ha costantemente cercato di individuare dei suoi sostituti
che fossero in grado di emulare il suo comportamento nelle molteplici occasioni
di interazione con l’ambiente circostante.
Gli enormi progressi in campo tecnologico, di cui siamo testimoni in
questi ultimi anni, hanno consentito un sempre più rapido sviluppo della
Robotica.
La rapida nascita di componenti hardware sempre più potenti e
veloci ( come i moderni processori) e la nascita di sempre più capienti unità di
memorizzazione dati hanno permesso la creazione di progetti robotici in grado di
svolgere compiti che fino a poche decine di anni fa erano cosiderati fantascentifici.
3
1.1. Obiettivo della tesi e sue applicazioni
4
Un robot è una macchina dotata di un certo grado di capacità decisionale,
che gli consente di svolgere tutti quei lavori caratterizzati da un’elevata
ripetitività e/o pericolosità per l’essere umano. La Robotica, che inizialmente
era usata prettamente in ambito industriale quale componente essenziale per la
realizzazione di sistemi automatizzati di produzione, col passare degli anni entra
sempre di più a far parte della vita quotidiana dell’uomo per svolgere compiti di
servizio e assistenza alla persona in ambienti domestici o in strutture mediche e
paramediche.
Robot mobili vengono realizzati per missioni in ambienti ostili o difficilmente
accessibili per l’uomo, come l’ambiente sottomarino, quello extraterrestre o
quello confinato all’interno di strutture industriali o civili non praticabili (si
pensi, ad esempio, all’impiego in operazioni di bonifica di terreni minati e nella
manipolazione di sostanze pericolose per l’uomo).
1.1
Obiettivo della tesi e sue applicazioni
L’obiettivo di questa tesi consiste nel pianificare traiettorie per un robot
mobile in un ambiente sconosciuto ed eseguirle applicando, nel caso in cui
queste traiettorie entrino in collisione con ostacoli presenti nell’ambiente, un
algoritmo di deformazaione reattiva del percorso che deformi la prepianificata
traiettoria trasformandola in un percorso ammissibile libero da ostacoli. Dopo
uno studio teorico e varie simulazioni in MatLab si è utilizzato come simulatore
il software Webots. Come robot della simulazione si è utilizzato il Magellan Pro,
definito nell’ambiente Webots come Differential wheels, che grazie ai suoi sensori
esterocettivi, quali ultrasuoni e/o infrarossi, è in grado di rilevare la presenza di
1.2. Problema dell’esplorazione di ambienti ignoti
5
ostacoli durante il moto del robot.
Una sua applicazione è l’estensione al caso anolonomo dell’algoritmo di
esplorazione di ambienti sconosciuti SRT. In questo modo il robot mobile
potrà raggiungere la prossima configurazione, cioè quella desiderata, attraverso
l’esecuzione di una traiettoria pianificata che interpola le due configurazioni
iniziale e finale rimanendo in una porzione dell’ambiente esplorato libera da
ostacoli ( o LSR, Local Safe Region).
1.2
Problema
dell’esplorazione
di
ambienti
ignoti
Una delle questioni più interessanti e delicate, nella quale la robotica mobile sta
investendo notevoli risorse negli ultimi anni, è quella dell’esplorazione di ambienti
ignoti. Questo problema porta a focalizzare l’attenzione su due quesiti:
• Come un robot può conoscere la sua posizione all’interno dell’ambiente che
lo circonda?
• In che modo può raggiungere la posizione desiderata senza urtare contro
ostacoli presenti nell’ambiente?
Il primo quesito affronta il problema della localizzazione del robot all’interno
dell’ambiente a lui sconosciuto. Il secondo si riferisce a tecniche di pianificazione
del moto e obstacle avoidance.
1.2. Problema dell’esplorazione di ambienti ignoti
1.2.1
6
La localizzazione
Esistono due metodi di base per la stima della posizione: i metodi di localizzazione
relativa e quelli di localizzazione assoluta.
I metodi di localizzazione relativa si basano di solito sull’odometria,
che consiste nell’integrazione delle rotazioni incrementali delle ruote nel tempo,
consentendo una stima dello spostamento del robot da una posizione di partenza
nota. Il suo principale svantaggio sta nel fatto che l’errore di posizione cresce
durante il moto del veicolo a causa dello slittamento delle ruote e degli attriti
col pavimento. L’accumularsi degli errori di orientamento causa grossi errori
di posizionamento che crescono proporzionalmente con la distanza percorsa dal
robot. Ne consegue la necessità di un sistema di localizzazione: partendo da una
buona stima della posizione iniziale del robot, gli encoders aquisiscono dati sulla
rotazione e sterzata delle ruote e successivamente viene fatta una stima della
posizione finale.
Un altro tipo di approccio consiste nella misura della posizione basata sui
riferimenti inerziali e magnetici: in questo caso la localizzazione si basa
sulla navigazione inerziale con giroscopi e accellerometri.
I metodi di localizzazione
assoluta determinano la posizione e
l’orientamento del robot rispetto ad un sistema di riferimento fisso. Diversi sono
gli approcci di localizzazione assoluta e la loro implementazione dipende dal tipo
di sensori utilizzati.
1.3. Algoritmo di esplorazione SRT
1.2.2
7
La pianificazione del moto e obstacle avoidance
È proprio alla risposta del secondo di questi quesiti che si intende dare un
contributo, introducendo brevemente il metodo di esplorazione SRT ed esponendo
il lavoro fatto per l’estensione al caso anolonomo.
1.3
Algoritmo di esplorazione SRT
L’algoritmo SRT si propone come metodo utile all’esplorazione di ambienti
sconosciuti. L’algoritmo SRT, il cui acronimo sta per Sensor-based Random
Tree, sfrutta l’espansione di una struttura randomizzata ad albero, costruito per
mezzo di una generazione casuale di configurazioni del robot, per realizzare un
algoritmo fortemente indirizzato all’esplorazione, da parte di un robot mobile, di
ambienti sconosciuti, con il solo utilizzo dei sensori in dotazione ( ultrasuoni o
infrarossi). Esistono due versioni di SRT:
- SRT versione Ball
- SRT versione Star
Queste due versioni differiscono tra loro nel modo di gestire le informazioni
dei sensori; tali informazioni si riflettono sulla costruzione dell’area salva intorno
alla posizione corrente, nella quale il robot è libero di muoversi senza pericolo di
urtare ostacoli.
1.4. Robot mobili su ruote
8
Figura 1.1: Alcuni esempi di robot su gambe
1.4
Robot mobili su ruote
I robot mobili (o AMR, Autonomous Mobile Robots) rappresentano uno dei più
recenti sviluppi all’interno della Robotica. A differenza dei robot che operano in
ambienti industriali, siano essi fissi o anche capaci di spostarsi come gli AGV
(Automated Guided Vehicles), i robot mobili più avanzati sono principalmente
destinati ad operare in ambienti non strutturati, le cui caratteristiche sono
variabili e non note a priori. In conseguenza di ciò, il paradigma di funzionamento
di un robot mobile si basa in larga misura sulla capacità del sistema di compiere,
in maniera autonoma, scelte e decisioni che consentano di ottimizzare il proprio
comportamento in relazione alle circostanze e al compito da svolgere.
Esistono diverse categorie di robot mobili, da quelli dotati di gambe ( alcuni
esempi in figura 1.1) a quelli che sfruttano lo scivolamento su una superficie a
basso attrito tangenziale (in genere tramite la creazione di un cuscino d’aria), da
quelli dotati di cingoli a quelli che si muovono tramite delle eliche in un ambiente
sottomarino.
Sicuramente la categoria più diffusa è quella dei robot mobili su ruote (o
1.4. Robot mobili su ruote
9
Figura 1.2: Alcuni esempi di robot mobili
WMR, Wheeled Mobile Robot). Un WMR è un veicolo dotato di ruote, di
sensori di vario genere e di un computer (remoto o di bordo,) che coordina le sue
attività e lo rende capace di moto autonomo controllando l’azione dei motori. In
figura 1.2 è possibile osservare alcuni esempi di robot mobili.
Le ruote risultano sicuramente il mezzo di locomozione più semplice in un
ambiente terrestre. Presentano ridotti problemi di stabilità, usano, a parità
di distanza percorsa, meno energia rispetto ad altri sistemi di locomozione e
permettono ad un robot di spostarsi molto velocemente. I veicoli che si muovono
su ruote sono agevolmente manovrabili, alcuni sono capaci di girare su se stessi o
di muoversi parallelamente al proprio asse. Per contro, le ruote si possono usare
solamente su terreni solidi e relativamente lisci; su terreni morbidi potrebbero
infatti impantanarsi, mentre sui terreni troppo lisci sono soggette a fenomeni di
slittamento. Per superare terreni scabri e irregolari i WMR devono essere dotati
di un efficace sistema di sospensioni che consenta alle ruote di mantenersi sempre
1.4. Robot mobili su ruote
10
Figura 1.3: Configurazione “Tricycle Drive”
in contatto con il terreno.
1.4.1
Strutture cinematiche dei WMR
Esistono diversi tipi di strutture cinematiche su ruote, a seconda del numero di
ruote impiegate e della posizione e struttura di quelle motrici e direttrici. Le
strutture più usate sono le seguenti:
• Tricycle Drive: questo sistema impiega una ruota anteriore attuata sia
in rotazione sia in sterzata, e due ruote passive posteriori (figura 1.3).
In alternativa la coppia motrice può essere fornita dal motore alle ruote
posteriori tramite un differenziale meccanico e la ruota anteriore viene
utilizzata solo per la sterzata.
• Car-like: è la configurazione cinematica usata nelle comuni automobili
(figura 1.4).
Due ruote posteriori sono motrici e ad orientamento
fisso rispetto al telaio.
Due anteriori sono rese sterzanti tramite un
quadrilatero articolato (sterzo di Ackerman) che mantiene l’intersezione dei
prolungamenti degli assi delle ruote sul centro di istantanea rotazione del
telaio (ciò riduce i fenomeni di strisciamento).
1.4. Robot mobili su ruote
11
Figura 1.4: Configurazione “Car-Like”
• Skid steering: quattro ruote motrici fisse sono disposte sui vertici di un
rettangolo (figura 1.5). Su ognuno dei due lati le ruote sono accoppiate ed
attuate con lo stesso comando. Una realizzazione costruttiva differente, ma
cinematicamente equivalente, prevede l’utilizzo di una coppia di cingoli al
poste delle due coppie di ruote.
• Syncro Drive: la struttura è composta da tre (o quattro) ruote mantenute
sempre parallele. Un accoppiamento meccanico impone alle ruote uno stesso
orientamento ed una stessa velocità di rotazione(figura 1.6).
• Uniciclo: l’uniciclo ideale è la struttura più semplice per un veicolo
terrestre e rappresenta una ruota libera di ruotare su se stessa e di
muoversi su un piano. La struttura reale che presenta lo stesso modello
cinematico dell’uniciclo è costituita da un robot con due ruote attuate
indipendentemente ed un ruotino passivo (castor ) che mantiene la struttura
1.4. Robot mobili su ruote
Figura 1.5: Configurazione “Skid Steering”
Figura 1.6: Configurazione “Syncro Drive”
12
1.4. Robot mobili su ruote
13
Figura 1.7: Configurazione “Differential Drive”
Figura 1.8: Configurazione “Stanford Wheel”
in equilibrio. Questo sistema è detto differential drive (vedi figura 1.7 ).
Esistono tuttavia strutture più sofisticate capaci di conferire al veicolo
caratteristiche cinematiche notevoli. Senza entrare nei particolari, citiamo, a
titolo di esempio, un tipo molto particolare di ruota, la Stanford wheel ( si veda la
figura 1.8), la quale, permettendo spostamenti nella direzione del suo asse, risulta
essere priva dei vincoli cinematici propri delle comuni ruote. Essa è costituita
da un sistema di mozzi circolari rotanti aventi l’asse di rotazione parallelo alla
direzione di normale avanzamento.
1.4. Robot mobili su ruote
1.4.2
14
I sensori
Per interagire con il mondo esterno, un robot è dotato di una serie di sensori.
I sensori più comunemente impiegati nella strumentazione di bordo di un robot
mobile si possono suddividere in due categorie:
• Sensori interni o propriocettivi : misurano lo stato interno del robot, ovvero
i movimenti delle sue parti meccaniche in un sistema di riferimento solidale
al robot. I dati provenienti da tali sensori sono interpretati temporalmente
e permettono al robot di stimare la sua posizione nell’ambiente in cui si
muove.
Tipo di misura
Trasduttori relativi
Posizione
Potenziometri, Resolvers, Syncros, Encoders
Velocità
Tachimetri, Encoders
Accelerazione
Accelerometri
Forza e coppie
Sensori di forza e coppia
Orientamento
Giroscopi, Bussole
Posizione
GPS
• Sensori esterni o esterocettivi :
forniscono informazioni sull’ambiente
circostante. Le misure effettuate sono tipicamente rivolte a localizzare il
robot all’interno di un sistema di riferimento solidale con l’ambiente, ed
a localizzare oggetti appartenenti all’ambiente. La tabella seguente elenca
alcuni di questi sensori.
1.4. Robot mobili su ruote
15
Tipo di misura
Trasduttori relativi
Contatto
Bumpers, Antenne, Epidermics
Forza o coppia
Sensori di forza e coppia
Prossimità
Induttivi, Capacitivi, Ottici, Magnetici
Distanza
Sonar, Visione, Laser
Per ogni tipo di sensore montato è fondamentale sviluppare un accurato
modello in modo da poter correttamente interpretare i dati ricevuti. Un’attenta
calibrazione ed una buona identificazione dei modelli, una volta nota l’accuratezza
delle misure, garantiscono un buon risultato negli algoritmi di integrazione
sensoriale.
Capitolo 2
Descrizione del robot mobile
Magellan Pro
2.1
Il robot
Magellan Pro è un robot mobile prodotto dalla iRobot1 ; in figura 2.1 è
inquadrato da tre prospettive visuali differenti. La sua scocca esterna è costituita
da alluminio e presenta una struttura cilindrica con un diametro di base di 40.6
cm ed un’altezza di 25.4 cm 2 . Al suo interno trovano alloggio i motori, gli
accumulatori e tutta l’elettronica di bordo. Il peso complessivo della struttura
è di circa 18.2 kg. Magellan Pro è dotato di due ruote motrici indipendenti e
di un castor mobile passivo (struttura cinematica differential drive, accennata
al paragrafo 1.4.1), la sua cinematica può essere descritta dal modello uniciclo
che sarà dedotto nel paragrafo 2.4. Le ruote sono di gomma morbida ed hanno
un diametro di 16.5 cm. Ognuna di esse è attuata da un motore in corrente
1
2
http://www.irobot.com
L’altezza da terra della sua base (ground clearance) è di circa 3.8 cm.
16
2.1. Il robot
17
Figura 2.1: Il robot Magellan Pro
continua da 24 V comandato in PWM. Il robot può raggiungere una velocità
lineare massima di 2.5 m/s ed una velocità massima di rotazione intorno al
suo asse di circa 270◦ /s (4.71 rad/s). Su ogni motore è calettato un encoder
incrementale impiegato sia per il controllo di velocità sia per la localizzazione
tramite l’odometria. Il sistema di alimentazione è costituito da due batterie da
12 V/12 Ah che gli permettono un’autonomia di circa due ore.
L’elettronica e la strumentazione a bordo di Magellan Pro comprendono:
• un PC Intel Pentium III 850 Mhz;
• un sistema di controllo rFLEX con schermo LCD;
2.1. Il robot
18
• una telecamera mobile (pan-tilt) a colori con relativo frame-grabber;
• 16 sensori di distanza ad ultrasuoni (Sonar3 );
• 16 sensori di prossimità ad infrarossi;
• 16 sensori di contatto (Bump Switches);
• un radiomodem (l’altro è collegato al PC remoto)
Il sistema di controllo rFLEX, prodotto dalla iRobot, costituisce un semplice
interfaccia utente per la gestione, la configurazione e la diagnostica del robot. Lo
schermo consente l’accesso ad un menu di funzioni e di comandi su tutti i sistemi
di base del robot (le varie console FARnet, Sonar, InfraRed, Joystick Drive, Motor
Test, Digital I/O, System I/O, Host Console).
2.1.1
I Sonar
Magellan Pro è dotato di 16 sensori sonar distribuiti in modo uniforme e circolare
attorno alla sua superficie laterale. Ognuno di essi può misurare la distanza da
un ostacolo frontale in un range che va dai 20 cm ai 4 m 4 .
I sonar quasi mai sottostimano la distanza da un ostacolo, quindi un metodo
semplice ed efficiente per ridurre il rischio di collisioni è quello di tenere sempre
sotto controllo la distanza minima rilevata dal gruppo di sonar in dotazione.
3
4
SOund Navigation And Ranging.
Le letture dei sonar vengono “tagliate” in questo range. Errori di lettura (e.g. quando
l’hardware non risponde) e letture non valide (e.g. nessuna eco è stata ricevuta) appariranno
come valori massimi
2.1. Il robot
2.1.2
19
I sensori a raggi infrarossi
Un sistema di 16 sensori di prossimità ad infrarossi Sharp è disposto circolarmente,
sotto la cinta dei sonar, attorno al Magellan Pro. Ciascun sensore è composto da
un emettitore LED5 e da un fotorilevatore. Un raggio infrarosso6 viene emesso e,
in base alla misura dell’intensità della luce riflessa, viene stimata una distanza tra
il sensore e l’oggetto riflettente. Il raggio d’azione dei sensori a raggi infrarossi
è ridotto rispetto a quello dei sonar: per il nostro robot la distanza minima
percepibile è di circa 7 cm, mentre quella massima è di circa 1 m. A differenza dei
sonar, il cono di emissione degli infrarossi è più ristretto ed ha una semi-apertura
di circa 8◦ .
2.1.3
I sensori di contatto
Sotto la cinta dei sonar e degli infrarossi sono disposti circolarmente 16 sensori
di contatto (bump switches). La loro funzione fondamentale è quella di rilevare
collisioni non previste durante l’esecuzione di movimenti. In caso positivo, un
processo supervisore provvede a fermare il moto del robot ed eventualmente ad
attivare delle routine ad hoc.
2.1.4
Le comunicazioni
Due radiomodem della BreezeNET, un Access Point radio, collegato direttamente
alla LAN7 , ed un Station Adapter radio, quest’ultimo alloggiato all’interno di
5
6
Light Emission Diode.
Radiazione elettromagnetica con lunghezza d’onda superiore ai 0.75 mm; essa è invisibile
ad occhio nudo.
7
Local Area Network.
2.2. La telecamera
20
Magellan Pro, consentono di comunicare con il PC a bordo del robot tramite una
wireless LAN ad una velocità di 3 Mbps.
All’interno del robot un bus di controllo FARnet connette i vari dispositivi in
un rete interna, la cui topologia può essere esaminata nella FARnet console del
sistema di controllo rFLEX.
2.2
La telecamera
La telecamera presente a bordo del robot Magellan Pro è il modello EVI-D31
costruito dalla Sony ed è rappresentata in figura 2.2. La telecamera è costituita
di due attuatori che le consentono le rotazioni intorno a due assi, che vengono
chiamati rispettivamente “asse di pan ed “asse di tilt. L’asse di rotazione di pan è
diretto ortogonalmente rispetto alla base di appoggio della telecamera. L’asse di
tilt è invece parallelo rispetto a tale piano d’appoggio e risulta sghembo rispetto
all’asse di pan.
Insieme alla telecamera è fornito in dotazione un telecomando che consente
lo svolgimento dei compiti basilari, come ad esempio la rotazione intorno agli
assi di pan e di tilt, ma anche più complessi.
Infatti tramite telecomando
è possibile impostare la telecamera per l’inseguimento di oggetti selezionati
dall’utente, utilizzando l’informazione del colore. Inoltre la telecamera è in grado
di determinare le differenze tra un’immagine iniziale di riferimento e l’immagine
corrente.
Le dimensioni di ingombro della telecamera, che sono riportate in figura 2.3,
consentono un corretto posizionamento a bordo del robot.
2.2. La telecamera
21
Figura 2.2: Il robot Telecamera EVI-D31
Figura 2.3: Dimensioni Telecamera EVI-D31
2.3. Architettura software
2.3
22
Architettura software
Mobility Robot Integration Software [3] è l’ambiente di sviluppo software
fornito dalla iRobot.
Il sistema operativo che lo supporta è Linux. L’ambiente Mobility definisce
un modello ad oggetti del robot usando lo standard CORBA 2.X
8
per la
comunicazione tra le varie componenti. Il modello definisce un sistema robotico
come un insieme di oggetti distribuito e organizzato gerarchicamente.
Ogni
oggetto è un’unità software separata, con una sua identità, una sua interfaccia
e un suo stato interno. Gli oggetti rappresentano astrazioni delle parti del robot:
sensori, attuatori, raccolta dati, ecc.
Mobility fornisce un set base di classi che rispecchia il modello ad oggetti del
robot e costituisce uno “scheletro su cui specializzare ed estendere le funzionalità
del sistema in base alle proprie esigenze. Ogni utente può sviluppare il suo proprio
software che agirà da client rispetto ai componenti software forniti. Sia Java che
C++ sono supportati, ma la release 1.1 di Mobility fornisce le classi solo per il
linguaggio C++.
2.3.1
Corba e NamingService
Nell’ambiente ad oggetti distribuiti Corba, gli oggetti possono comunicare fra
loro solo se hanno i loro reciproci riferimenti.
8
Gli oggetti importanti di
Common Object Request Broker Architecture. Il consorzio OMG (Object Management
Group), dedito alla ricerca di metodi aperti e standardizzati per lo sviluppo di software ad
oggetti distribuiti, ha sviluppato CORBA come suo primo standard. CORBA costituisce un
libreria dedicata alla comunicazione e permette un accesso trasparente ad oggetti in differenti
spazi di indirizzi, sullo stesso o su diversi computer.
2.3. Architettura software
23
ogni programma si registrano nel NamingService. Ogni programma può allora
interrogare il NamingService per ottenere il riferimento (sotto forma di stringa)
degli oggetti registrati.
2.3.2
Il Base Server
I programmi applicativi, che controllano il moto ed i flussi input-output del robot,
si possono considerare distribuiti su due livelli.
Il livello più basso comprende il Base Server. Questo server interagisce
direttamente con l’hardware del robot tramite una collezione di device driver che
si occupano del controllo di basso livello del moto e dell’acquisizione dei dati dai
sensori.
Il Base Server contiene al suo interno un insieme di buffer, uno per ogni
sensore.
Ogni buffer raccoglie due strutture dati 9 : queste contengono le
due ultime letture ottenute da ogni dispositivo insieme al relativo istante di
campionamento. Il Base Server si occupa autonomamente di aggiornare in modo
continuo i vari buffer dei sensori.
Al livello più alto, ogni programma, che necessiti l’uso dei motori o
l’acquisizione di dati dall’odometria, dai sonar o dagli infrarossi, si comporta come
un client, inoltrando, tramite il NamingService, le sue richieste al Base Server10 .
Quest’ultimo, ricevute tali richieste, si occupa poi di trasmettere gli eventuali
comandi agli attuatori e di comunicare al client il contenuto delle struttura più
aggiornata raccolta in ognuno dei buffer di interesse.
9
La verifica che il buffer ha dimensione due è stata effettuata tramite la funzione
history length della classe base StateComponent. Questa viene ereditata, come una sorta di
interfaccia, da tutte le classi Mobility che rappresentano lo stato di un particolare dispositivo.
10
Il tutto avviene in maniera trasparente tramite l’utilizzo delle classi fornite da Mobility.
2.3. Architettura software
2.3.3
24
Il Frame-grabber Server ed il Pan-tilt Server
Sullo stesso livello del base server si collocano il server relativo al frame-grabber
ed il server di movimento relativo alla telecamera. Il primo si occupa di acquisire
ed elaborare i dati di basso livello ottenuti dall’acquisizione digitale del segnale
video proveniente dalla telecamera. Il secondo, in modo identico al base server, si
interfaccia con gli attuatori e gli encoder della telecamera comunicando eventuali
comandi di movimento pan-tilt ed aggiornando un vettore di stato corrispondente.
Come il base server, sia il frame-grabber sever sia il pan-tilt server devono essere
attivati sul PC a bordo di Magellan Pro.
Per quanto riguarda il Pan-tilt Server, si sono riscontrati dei problemi dovuti
al lento refresh dell’odometria della telecamera, che viene aggiornata ogni circa
0.4 secondi. Questo rappresenta un forte limite per un corretto svolgimento
della tesi, in quanto con tempi di refresh di tale portata non è possibile
ottenere l’inseguimento di oggetti in movimento, a meno che non si tratti di
movimenti di modesta entità. Fortunatamente il dottorando Ing. Luigi Freda,
che precedentemente si era occupato della movimentazione del robot Magellan
Pro nella sua tesi [2], ha messo a punto un nuovo server che consente di ottenere
un aggiornamento dell’odometria nettamente più veloce (nell’ ordine degli 0.015
secondi).
Con tale risultato, il limite alle prestazioni della telecamera è dato dal
Frame-grabber, che ha un refresh di circa 0.14 secondi, che corrisponde ad una
acquisizione di immagini pari a circa 7 frames al secondo.
2.3. Architettura software
2.3.4
25
MOM
Mobility fornisce un’interfaccia grafica che permette di monitorare e configurare
gli oggetti che compongono il modello software di Magellan Pro: MOM (Mobility
Object Manager) è scritto in Java e comunica con i programmi Mobility tramite
Corba. Al momento dell’attivazione MOM interroga il NamingService per avere
i riferimenti di tutti gli oggetti attivi registrati.
Una finestra visualizza la
gerarchia degli oggetti: i nodi principali che possono essere visualizzati sono
tre e rappresentano l’attivazione del base server, del frame-grabber server e del
pan-tilt server11 . Come in una gerarchia di directory, ogni nodo contiene i suoi
oggetti componenti, rispecchiando il modello del robot. Cliccando il tasto destro
del mouse su ognuno di essi appare un menù di visualizzatori che restituiscono
informazioni riguardo lo stato dell’oggetto.
Ad esempio l’attivazione del base server viene visualizzata con un
nodo “Magellan Pro”.
Questo contiene al suo interno i sottonodi
“Drive” (permette di comandare il robot con un “joystick grafico”),
“Sonar ”,“Infrared ”,“Odometry”,“Hardware” e “Debug”. Il nodo “Sonar” a sua
volta contiene altri tre sottonodi che rappresentano i tre formati disponibili per i
dati dei sensori ad ultrasuoni.
Nella figura 2.4 è mostrato uno screen-shot del MOM.
11
Questi tre server sono indipendenti l’uno dall’altro e vanno attivati sul PC a bordo dopo
l’avvio del NamingService.
2.4. Il modello cinematico
26
Figura 2.4: Intefaccia grafica MOM
2.4
Il modello cinematico
Magellan Pro è dotato di due ruote laterali attuate indipendentemente, di un
ruotino posteriore passivo (castor), che mantiene la struttura in equilibrio, e di
un roller frontale, che minimizza i battimenti dovuti alle frenate brusche.
Il carico del veicolo è sostenuto prevalentemente dalle due ruote motrici
laterali12 . Questo permette di trascurare la dinamica del castor e di considerare
Magellan Pro come un uniciclo.
Formuliamo le ipotesi preliminari che ci consentiranno di dedurre in maniera
semplice il modello cinematico del robot:
12
L’asse delle ruote motrici cade in posizione diametrale. Data la simmetria assiale del
veicolo e la distribuzione uniforme dei carichi, la proiezione perpendicolare del baricentro cade
approssimativamente su tale asse.
2.4. Il modello cinematico
27
Figura 2.5: Grandezze cinematiche del Magellan Pro
• Il robot è schematizzato come un insieme di cinque corpi rigidi (vedi figura
2.5): una struttura portante piana orizzontale, due ruote motrici di raggio
R, un ruotino di raggio r ed un piantone verticale che lo supporta13 .
Le ruote sono considerate ideali, ossia di spessore praticamente nullo ed
indeformabili.
• La superficie di appoggio del veicolo è perfettamente piana ed orizzontale.
• Si esclude la presenza di slittamenti per le ruote: ovvero si ipotizza che,
durante il moto, la velocità del punto di contatto delle ruote sul piano di
appoggio sia nulla.
Sia le ruote motrici che il ruotino hanno piani di rotazione ortogonali rispetto
a quello della struttura portante. Quelli delle ruote motrici sono fissi rispetto ad
essa, mentre quello del castor può ruotare intorno all’asse del piantone verticale.
13
Il roller non è in contatto con il piano di appoggio del veicolo e quindi non sarà considerato
come “ruota.
2.4. Il modello cinematico
28
Per la deduzione del modello cinematico seguiamo l’approccio indicato in [1],
effettuando prima di tutto una classificazione delle tre ruote per determinare il
tipo di vincolo cinematico associato ad ognuna di esse:
• le due ruote posteriori sono di tipo “svedese”. Ovvero, per ognuna di esse,
è possibile definire sul piano di appoggio una direzione lungo la quale la
componente di velocità del punto di contatto sia nulla.
• il ruotino anteriore è di tipo “off centered” perchè il suo asse di rotazione
orizzontale ha un disassamento non nullo rispetto a quello del piantone
verticale.
Con riferimento alla figura 2.5 consideriamo una terna mobile {x1 , x2 }, solidale
con il robot e con origine coincidente con il centro dell’asse delle ruote motrici,
ed una terna fissa {I1 , I2 }, solidale con il piano di moto del veicolo. Sia


x
 
 
ξ= y 
 
θ
(2.1)
il vettore delle coordinate di postura del veicolo, dove x, y e θ sono
rispettivamente le coordinate dell’origine e l’angolo di rotazione di {x1 , x2 }
rispetto alla terna fissa. La matrice di rotazione di {x1 , x2 } rispetto ad {I1 , I2 }
risulta:


cos ϑ
sin ϑ 0




R (ϑ) =  − sin ϑ cos ϑ 0 


0
0
1
(2.2)
2.4. Il modello cinematico
29
Figura 2.6: Parametri cinematici della ruota svedese
2.4.1
Vincoli per le ruote ad orientamento fisso
Consideriamo nella figura 2.6 i parametri relativi al modello di ruota svedese che
rappresenta la cinematica delle ruote laterali di Magellan Pro. In particolare γ
individua la direzione lungo la quale deve essere nulla la velocità di slittamento
e ρ il raggio della ruota in esame.
Come riportato in [1], l’equazione che descrive il vincolo di anolonomia sulla
velocità del punto di contatto con il suolo è la seguente:
[ − sin(α + δ + γ) cos(α + δ + γ) l cos(δ + γ) ]R(ϑ)ξ˙ + ρ cos(γ)ϕ̇ = 0 (2.3)
Detta L la lunghezza dell’asse delle ruote, per la ruota sinistra (sx) abbiamo:
l=
L
;
2
π
α= ;
2
δ= 0; ρ= R; γ=
π
2
(2.4)
Sostituendo tali valori nell’equazione (2.3) si ottiene:
[ 0 −1 0 ]R(ϑ)ξ˙ = 0
Per la ruota destra (dx):
(2.5)
2.4. Il modello cinematico
30
Figura 2.7: Parametri cinematici del castor
l=
L
;
2
π
α= − ;
2
δ= 0; ρ= R; γ=
π
2
(2.6)
da cui:
[ 0 1 0 ]R(ϑ)ξ˙ = 0
2.4.2
(2.7)
Vincoli per le ruota anteriore
Per le ruote di tipo “off centered” (vedi figura 2.7) le equazioni di vincolo sono le
seguenti:

 [− sin(α + β)
 [ cos(α + β)
cos(α + β)
l cos β]R(θ)ξ˙ + ρϕ̇ = 0
sin(α + β) d + l sin β]R(θ)ξ˙ + dβ̇ = 0
(2.8)
I parametri, nel nostro caso, hanno i seguenti valori:
ϕ = variabile;
e le equazioni diventano:
α = 0;
l = D;
ρ = r;
β = variabile
(2.9)
2.4. Il modello cinematico

 [− sin β
 [ cos β
2.4.3
31
cos β
D cos β]R(θ)ξ˙ + rϕ̇ = 0
sin β
d + D sin β]R(θ)ξ˙ + dβ̇ = 0
(2.10)
Equazioni del modello
Possiamo riscrivere in maniera compatta le equazioni (2.5) e (2.7):



0
1
0
 R(ϑ)ξ˙ = 0
(2.11)
0 −1 0
ed indicando con:


1 0




Σ= 0 0 


0 1
(2.12)

una matrice le cui colonne sono una base per N 

0
1
0
 si può
0 −1 0
scrivere:
˙ RT (θ)Ση
ξ=
η∈R
2
(2.13)
che costituisce il cosiddetto modello cinematico di postura14 . Se si indica con
v la velocità lineare dell’origine della terna mobile {x1 , x2 } lungo l’asse x1 , con ω
la velocità angolare della struttura portante15 e si pone η T = [v ω], l’equazione
cinematica di postura assume un più chiaro significato fisico:
14
Le equazioni (2.5) e (2.7) sono equivalenti al vincolo di anolonomia dell’uniciclo ideale
(paragrafo 1.1.1): −ẋ sin θ + ẏ cos θ = 0.
15
Stiamo considerando il caso di moto piano per la struttura portante. Le rotazioni sono
assunte positive in senso antiorario, secondo le normali convenzioni.
2.4. Il modello cinematico
32


ẋ = v cos θ



ẏ = v sin θ




θ̇ = ω
(2.14)
˙ ma permettono di
Le equazioni (2.10) non pongono vincoli espliciti su ξ,
determinare in dipendenza delle coordinate di postura e del vettore η dei comandi
di velocità, i valori di β̇ e ϕ̇ relativi al castor.
2.4.4
Il modello nelle velocità lineari delle ruote
Può essere interessante conoscere la relazione che esiste tra le coordinate di
postura del robot e le velocità vd e vs degli estremi dell’asse delle ruote motrici. Se
per tali ruote vale l’ipotesi di puro rotolamento, le velocità suddette sono legate
alle velocità angolari ωd e ωs delle ruote tramite le seguenti equazioni:
vd = ωd R
(2.15)
vs = ωs R
dove con R si è il raggio delle ruote.
Da semplici considerazioni geometriche, indicando con L la lunghezza dell’asse
delle ruote motrici (si veda la figura 2.5), si ottiene la relazione:


1
1
v
2  d 
 = 2
(2.16)
1 −1
ω
v
s
L
L
In fine, sostituendo tali relazioni nella (2.14), si ottiene il nuovo modello


v
cinematico:

2.4. Il modello cinematico
33


ẋ

  
  
 ẏ  = 
  
θ̇
cos θ
2
sin θ
2
1
L
cos θ
2
sin θ
2
1
−L



 vd



 v
s
(2.17)
Capitolo 3
Nonholonomic Motion Planning
e Reactive Path Deformation
L’intero lavoro di tesi può essere suddiviso in due fasi:
• La prima fase consiste nello studio ed implementazione di un pianificatore
di traiettorie anolonome che interpola due configurazioni del robot: quella
corrente ( configurazione di partenza) definita come qcurr e quella successiva
( configurazione di arrivo o goal ) definita come qgoal .
• La seconda fase consiste nello studio e implementazione del “Reactive Path
Deformation for Nonholonomic Mobile Robots” (vedi [4]).
Entrambi queste fasi sono state realizzate tramite l’ausilio del software
MatLab e poi implementate in un listato C++ utilizzato come controllore del
Magellan Pro nell’ambiente di simulazione Webots.
34
3.1. Introduzione ai sistemi anolonomi [6]
35
Figura 3.1: Esempio di vincolo anolonomo su robot mobile dotato di due ruote
3.1
Introduzione ai sistemi anolonomi [6]
Tra i vari sistemi onolonomi vi sono anche i WMR ( wheeled mobile robots o robot
mobili su ruote). Un WMR, data la presenza delle ruote, non può muoversi in
direzioni perpendicolari a quella di avanzamento, questa condizione rappresenta
un caso particolare di comportamento anolonomo ed è un vincolo di movimento
senza scivolamento. In generale:
un sistema meccanico anolonomo non può muoversi in qualsiasi direzione nel
proprio spazio delle configurazioni.
Nascono a questo punto dei problemi:
• La nostra esperienza quotidiana ci indica che i WMR sono controllabili, ma
per provarlo è necessaria una caratterizzazione matematica di anolonomia.
• In ogni caso, se il robot deve muoversi tra due configurazioni, è richiesto
un percorso realizzabile ( ad esempio un moto che soddisfa ai vincoli
anolonomi).
3.1. Introduzione ai sistemi anolonomi [6]
36
• Il problema del feedback control è molto complicato, perchè:
- un WMR è sottoattuato: meno ingressi di controllo delle coordinate
generalizzate;
- un WMR è non facilmente stabilizzabile in un punto.
C’è quindi bisogno di un’appropriata tecnica di controllo reazionato.
3.1.1
Vincoli cinematici
La configurazione di un sistema meccanico può essere unicamente descritta da un
vettore n-dimensionale di coordinate generalizzate:






q=



q1 

q2 

.. 
. 


qn
(3.1)
Le velocità generalizzate in un generico punto della traiettoria q(t) ∈ Q,
dove Q è la configurazione spaziale, è il vettore tangente:






q̇ = 



q˙1 

q˙2 

.. 
. 


q˙n
(3.2)
Vincoli geometrici possono esistere o essere imposti sul sistema meccanico
restringendo le possibilità di moto a dimensioni inferiori a quelle dello spazio delle
configurazioni.
3.1. Introduzione ai sistemi anolonomi [6]
37
Figura 3.2: Rappresentazione dell’unicicloe
Un sistema meccanico può ance essere soggetto a un insieme di vincoli
cinematici coinvolgendo le sue coordinate generalizzate e le loro derivate. Nella
maggior parte dei casi sono vincoli Pfaffiani . Un insieme di vincoli Pfaffiani è
chiamato olonomo se è integrabile ( e rappresenta quindi una limitazione vincolo)
altrimenti è definito anolonomo ( e rappresenta una limitazione cinematica).
Vincoli olonomi e anolonomi influenzano la mobilità in modi completamente
differenti: se consideriamo un singolo vincolo Pfaffiano
• nel primo caso il moto è confinato a giacere su un particolare livello dei
piani che formano l’insieme delle configurazioni spaziali;
• Nel secondo caso in ogni configurazione la velocità istantanea del sistema
è ristretta ad uno spazio di una dimensione inferiore a quello delle
configurazioni spaziali originale.
Un esempio canonico di anolonomia è quello dell’uniciclo ( vedi fig. 3.2):
Le sue coordinate generalizzate sono q = (x, y, θ) e il vincolo anolonomo di
puro rotolamento è dato da:
3.1. Introduzione ai sistemi anolonomi [6]
38
µ
ẋ sin θ − ẏ cos θ = 0 ⇒
ẏ
= tan θ
ẋ
¶
(3.3)
Le velocità possibili del sistema sono contenute nello spazio nullo della matrice
dei vincoli:

  

0 
cos
θ




  

  

T
T
a (q) = (sin θ − cos θ 0) ⇒ N (a (q)) = span  sin θ  ,  0 


  






0
1
(3.4)
Ogni configurazione qf = (xf , yf , θf ) può essere raggiunta ruotando il disco
finchè non punta la nuova configurazione, rotolare il disco finchè non ha raggiunto
la nuova configurazione e ruotarlo nuovamente finchè il suo orientamento non è
θf .
3.1.2
Punto di vista del controllo
L’olonomia e l’anolonomia dei vincoli può essere convenientemente studiata
attraverso un approccio duale considerando le direzioni in cui il moto è
consentito piuttosto che quelle direzioni in cui il moto è proibito. Nasce quindi
un’interessante questione:
date due configurazioni arbitrarie qi e qf , quando esiste una traiettoria q(t) che
le connette soddisfacendo ai vincoli cinematici?
Associando all’insieme di vincoli cinematici una base del loro spazio nullo (
ad esempio un insieme di vettori gj ) tale che:
aTi (q)gj (q) = 0 i = 1, . . . , k
j = 1, . . . , n − k
(3.5)
3.2. Nonholonomic Motion Planning
39
le traiettorie realizzabili del sistema meccanico sono le soluzioni q(t) di
q̇ =
m
X
gj (q)uj = G(q)u
(3.6)
j=1
per ogni ingresso u(t) ∈ Rm ,
dove m=n-k ( le u sono anche dette
pseudovelocità).
Il precedente è un sistema nonlineare senza deriva ( cioè u(t) ⇒ q̇ = 0) è
conosciuto come modello cinematico di un sistema meccanico vincolato.
Tornando al caso dell’uniciclo 3.2 la matrice le cui colonne sono una base dello
spazio nullo della matrice dei vincoli anolonomi è:


cos θ


G(q) =  sin θ

0
0


0  = (g1 g2 )

1
(3.7)
da qui il modello cinematico dato dalla:
q̇ = G(q)u = g1 (q)u1 + g2 (q)u2
(3.8)
con le u velocità di ingresso.
3.2
Nonholonomic Motion Planning
Come già detto all’inizio del capitolo l’obiettivo della prima fase di progetto è
quella di pianificare una traiettoria per un sistema anolonomo e questo è possibile
tramite la costruzione di una sequenza di ingressi di controllo in grado di dirigere
il sistema da una configurazione iniziale qi a una finale qf soddisfacendo ai vincoli
anolonomi.
Esistono dei modelli canonici
di strutture i quali possono risolvere
3.2. Nonholonomic Motion Planning
40
efficientemente il problema della pianificazione.
Questi sono:
- Chained form
- Power form
- Caplygin form
Per poter raggiungere l’obiettivo desiderato bisogna eseguire una trasformazione
delle originali equazioni del modello in esame in una delle precedenti forme.
Questi modelli permettono oltretutto anche una semplice progettazione di una
reazione stabilizzatrice.
Il nostro interesse si focalizza sulla prima di queste forme, Chained form.
3.2.1
Chained Forms
Nel nostro caso, dovendo lavorare con il Magellan Pro, il quale può essere
assimilato ad un uniciclo, siamo dinanzi ad un sistema anolonomo a 3 dimensioni e
a due ingressi. Il modello chained form da utilizzare avrà quindi dimensioni (2,3),
dove 3 è la dimensione del sistema uniciclo, cioè sarà un sistema di controllo senza
deriva a due ingressi descritto dalla:
ż = g1 (z)v1 + g2 (z)v2
(3.9)
e trasformato nella seguente forma:
ż1 = v1
ż2 = v2
ż3 = z2 v1
(3.10)
3.2. Nonholonomic Motion Planning
41
In un Chained Form possono essere usati diversi ingressi di comando e noi
useremo gli ingressi polinimiali ( o polynomial inputs). Lo steering with
polynomial inputs, nel caso di un sistema di dimensione 3 con due ingressi,
prevede che il controllo sia scelto come:
v1 = sign(zf 1 − zi1 )
(3.11)
v2 = c 0 + c 1 t
con T = |zf1 − zi1 | e c0 , c1 ottenuti risolvendo il sistema lineare proveniente
dall’integrazione in forma chiusa dell’equazioni del modello, cioè:




z
c0
 + m(zi , T ) =  f 2 
M (T ) 
zf 3
c1
(3.12)
con M(T) non singolare per T 6= 0.
Se T = 0, e di conseguenza zf1 = zi1 , allora deve essere aggiunto un punto
intermedio tra i due che permette la buona riuscita del processo di pianificazione.
La trasformazione globale delle coordinate utilizzata per il modello
dell’uniciclo è:
z1 = θ
z2 = x cos θ + y sin θ
(3.13)
z3 = x sin θ − y cos θ
per gli ingressi invece la trasformazione è la seguente:
v = u2 + z 3 u1
(3.14)
w = u1
Applicando quindi la precedente trasformazione insieme alle 3.10, 3.11 e 3.12 per
il calcolo di c0 , c1 si ha:

 ż2 = v2
1
⇒ z2 = c0 t + c1 t + k
 v =c +c t
2
2
0
1
(3.15)
3.2. Nonholonomic Motion Planning
42


ż = z2 v1


 3
s 2 s 3
v1 = sign(zf 1 − zi1 ) = s ⇒ z3 = c0 t + c1 t + skt + d

2
6



1
z2 = c0 t + 2 c1 t + k



zf 2

=
zf 3



c0
c1

2
T
T /2
sT 2 /2
sT 3 /6

=


c0

+
c1

k
T 2 /2
2
sT /2


3
sT /6

skT + d
−1 
T
(3.16)

zf 2 − k
(3.17)

zf 3 − skT − d
dove k e d sono costanti di integrazione.
3.2.2
Implementazione in Matlab
A questo punto, una volta chiarito il modello chained form con polynomial inputs
e la conseguente trasformazione di coordinate e ingressi, è stato creato un modello
Simulink (vedi 3.3) formato da:
• un blocco per l’ingresso della configurazione finale desiderata;
• un sottosistema per la scalatura dell’angolo di orientamento nel caso quello
inserito sia maggiore o uguale 2π;
• un blocco ( S-Function) che contiene l’intero corpo del programma di
pianificazione ( vedi appendice);
• due blocchi di uscita per la visualizzazione grafica della posizione (x,y) e
l’orientamento finale ( XY Graph e Scope) e uno per la verifica numerica
della configurazione raggiunta ( Display)
3.2. Nonholonomic Motion Planning
43
Figura 3.3: Modello Simulink per la pianificazione
Diamo ora una breve spiegazione del programma di pianificazione. Il corpo
del programma si trova nella function mdlDerivatives del blocco S-Function.
Inizialmente viene inserita la configurazione finale desiderata, per l’uniciclo, da
console. Successivamente questa passa in sottosistema all’interno del quale le
prime due componenti del vettore della configurazione passano indisturbate (
cioè la posizione x e y dell’uniciclo) mentre viene effettuata una scalatura della
terza componente che indica l’orientamento, da parte della funzione:
atan2(sinθ, cosθ)
(3.18)
A questo punto giunge in ingresso della S-Function il vettore della configurazione
iniziale che viene trasformato secondo la trasformazione globale vista in 3.13,
dopo di che si passa ad un controllo delle prime due posizioni della configurazione
finale ed iniziale. Questo controllo è utile affinchè non si verifichi il caso in cui
3.2. Nonholonomic Motion Planning
44
T = 0. Se ciò si verificasse il programma generebbe un punto intermedio che in
questo caso è stato preso come: q = (0, 0, π/2) in modo che T 6= 0 e può essere
generata la traiettoria desiderata. In uscita dalla funzione mdlDerivatives però
gli stati della traiettoria sono trasformati secondo la 3.13 quindi affinchè siano
graficati correttamente c’è bisogno di un ulteriore trasformazione che riporti
le configurazioni nel sistema di riferimanto piano (x, y, θ). Questo compito è
affidato alla funzione della S-Function mdlOutputs nella quale si opera la seguente
trasformazione:
x = cos(x(1)) ∗ x(2) + sin(x(1)) ∗ x(3)
y = sin(x(1)) ∗ x(2) − cos(x(1)) ∗ x(3)
(3.19)
θ= x(1)
dove il vettore x = (x(1), x(2), x(3)) rappresenta il vettore delle coordinate
trasformate degli stati del sistema.
Riportiamo alcune simulazioni dell’algoritmo di pianificazione:
• nella prima ( vedi figura 3.4) il sistema si trova nella posizione iniziale
qi = (0, 0, 0) e viene pianificata una traiettoria che lo conduce nella posizione
finale qf = (0.5, 0.5, π);
• nella seconda ( vedi figura 3.5) il sistema si trova nella posizione iniziale
qi = (0, 0, 0) e viene pianificata una traiettoria che lo conduce nella posizione
finale qf = (0.5, 0.5, 0).
In questo caso l’uniciclo ruota prima su se
stesso portandosi nella configurazione qi = (0, 0, π/2) e poi si porta nella
configurazione finale eseguendo la traiettoria pianificata.
3.2. Nonholonomic Motion Planning
45
1
0.8
0.6
0.4
0.2
0
−0.2
−0.4
−0.6
−0.8
−1
−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
Figura 3.4: Traiettoria pianificata dall’algoritmo di pianificazione
1
0.8
0.6
0.4
0.2
0
−0.2
−0.4
−0.6
−0.8
−1
−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
Figura 3.5: Traiettoria pianificata dall’algoritmo di pianificazione nel caso di zf1 = zi1
3.3. Reactive Path Deformation for Nonholonomic Mobile Robots [4]
3.3
Reactive
Path
Deformation
46
for
Nonholonomic Mobile Robots [4]
La meggior parte dei veicoli sono soggetti a vincoli di rotolamento senza
slittamento e cosı̀ prendono parte alla grande classe dei sistemi anolonomi.
Gli sforzi della ricerca si sono focalizzati in passato sulla comprensione e il
controllo del moto di tali sistemi. Oggi i maggiori produttori di auto sono molto
interessati nel controllo del moto dei veicoli su ruote. Molti dei loro progetti
sono rivolti ad equipaggiare i loro veicoli, in un prossimo futuro, con funzioni
di movimento assistite da computer, come parcheggio parallelo automatico o
stop-and-go automatico in ingorghi stradali. Quindi una migliore comprensione
e controllo di tali sistemi può aprire la strada a un vasto campo di applicazioni
industriali nel dominio dei trasporti.
Produrre un moto automatico per un robot mobile anolonomo si è rivelata
una richiesta difficile. Per questa ragione il problema è stato diviso in due fasi:
• la prima fase consiste nel calcolare un moto libero da ostacoli usando una
mappa dell’ambiente;
• la seconda fase consiste nell’eseguire il moto.
Come conseguenza le passate ricerche su tali sistemi hanno focalizzato l’attenzione
su due aspetti:
• il path planning ( pianificazione del percorso)
• il motion control ( controllo del moto)
Queste due fasi sollevano a loro volta tre questioni:
incertezza della
localizzazione, imprecisione della mappa dell’ambiente e presenza di ostacoli
3.3. Reactive Path Deformation for Nonholonomic Mobile Robots [4]
inaspettati non presenti sulla mappa.
47
Tutte e tre le precedenti hanno una
conseguenza in comune: un percorso pianificato inizialmente libero da collisioni
può andare in collisione nel momento dell’esecuzione.
Per superare questa
difficoltà si propone un metodo che abilita una deformazione on-line del percorso
che deve essere eseguito dal robot per portarlo lontano dagli ostacoli percepiti
durante il moto.
Il metodo proposto in [4] abilita il robot a deformare il percorso iniziale per
sfuggire degli ostacoli percepiti e rendere il percorso corrente libero da ostacoli,
quindi il percorso corrente varia nel tempo. Un percorso e una mappatura da un
intervallo di numeri reali nella configurazione spaziale del robot, naturalmente il
processo di deformazione del percorso è modellato come una mappatura di due
variabili reali s e τ nello spazio delle configurazioni.
Il processo del path deformation è modellato come un sistema di controllo
dinamico, e [4] fornisce un algoritmo che controlla il processo di deformazione.
3.3.1
Nonholonomic Path Deformation come sistema
dinamico di controllo
Un percorso per un sistema robotico è usualmente rappresentato da una
mappatura da un intervallo di R nello spazio delle configurazioni del sistema.
In questa sezione si introduce la nozione di path deformation come mappatura da
un intervallo di R nell’insieme dei percorsi.
Un sistema anolonomo di dimensione n è caratterizzato da un insieme di
k < n vettori di campo X1 (q), . . . , Xk (q) dove q ∈ C = Rn è la configurazione del
sistema. Per ogni configurazione q le velocità ammissibili del sistema è l’insieme
delle combinazioni lineari degli Xi (q) vettori di campo.
3.3. Reactive Path Deformation for Nonholonomic Mobile Robots [4]
48
Un percorso q(s) è una curva “smooth” nello spazio delle configurazioni
definito su un intervallo [0, S]. Un percorso è detto ammissibile se e solo se
esiste un vettore k -dimensionale u = (u1 , . . . , uk ) definito su [0, S] e tale che:
∀s ∈ [0, S]
q̇(s) =
k
X
ui (s)Xi (q(s))
(3.20)
i=1
Chiamiamo path deformation una mappatura da un sottosistema [0, S] × [0, +∞]
di R2 dello spazio delle configurazioni del sistema:
(s, τ ) → q(s, τ )
per ogni valore di τ e s, s → (s, τ ) è un percorso.
s → (s, 0) è chiamato
percorso iniziale. Siamo interessati in deformazioni q(s, τ ) composte da solo
percorsi ammissibili. Cosı̀ le deformazioni soddisfano ai seguenti vincoli: esiste
un vettore k -dimensionale u = (u1 , . . . , uk ) definito su [0, S] × [0, +∞] tale che
∀(s, τ ) ∈ [0, S] × [0, +∞]
k
X
∂q
(s, τ ) =
ui (s, τ )Xi (q(s, τ ))
∂s
i=1
(3.21)
per ogni valore di τ e s, s → u(s, τ ) è la funzione di ingresso del percorso
s → q(s, τ ).
Proprio come ogni percorso è unicamente definito dalla
configurazione iniziale e le funzioni di ingresso, cosı̀ una deformazione del percorso
è unicamente definita dalla configurazione iniziale q(0, τ ) di ogni suo percorso e
dalle funzioni di ingresso ui (s, τ ).
Differenziando la 3.21 otteniamo una relazione tra le variazioni degli ingressi
∂u
∂τ
e l’infinitesima deformazione di percorso
deformazione τ :
k
X
∂ 2q
(s, τ ) =
∂s∂τ
i=1
µ
∂q
∂τ
al crescere del parametro di
¶
∂ui
∂Xi
∂q
(s, τ ) Xi (q (s, τ )) + ui (s, τ )
(q (s, τ ))
(s, τ )
∂τ
∂q
∂τ
(3.22)
3.3. Reactive Path Deformation for Nonholonomic Mobile Robots [4]
49
Chiamiamo rispettivamente input perturbations e direction of deformation i
seguenti vettori di funzioni:
∆ ∂u
∂τ
(s, τ )
∆ ∂q
∂τ
(s, τ )
v (s, τ ) =
η (s, τ ) =
con queste notazioni la precedente equazione diviene:
η̇ (s, τ ) = A (s, τ ) η (s, τ ) + B (s, τ ) v (s, τ )
doveη̇ =
∂η
∂s
(3.23)
e A e B sono rispettivamente matrici n × n e n × k:
A (s, τ ) =
k
X
ui (s, τ )
i=1
∂Xi
(q (s, τ ))
∂q
B (s, τ ) = (X1 (q (s, τ )) . . . Xk (q (s, τ )))
In accordo con la 3.23 la derivata del percorso rispetto a τ è in relazione con gli
ingressi di perturbazione attraverso un sistema dinamico lineare. Questo sistema
è infatti il sistema linearizzato di 3.20. Per un dato percorso q(s, τ ) di ingresso
u(s, τ ), per ogni ingresso di perturbazione v(s, τ ) e per ogni condizione iniziale
η0 = η0 (0, τ ) possiamo integrare la 3.23 rispetto a s per avere la corrispondente
direzione di deformazione η(s, τ ).
Un processo di deformazione del percorso per un sistema anolonomo può cosı̀
essere considerato come un sistema dinamico di controllo dove:
- τ è il tempo
- s → q(s, τ ) è lo stato
- l’ingresso è una coppia (η0 , s → q(s, τ ))
3.3. Reactive Path Deformation for Nonholonomic Mobile Robots [4]
50
Il metodo di deformazione del percorso deve calcolare in ogni momento τ
un vettore η0 e una funzione s → v(s, τ ) su [0, S] in modo che il processo di
deformazione raggiunga il goal specificato. Il goal è espresso in termini di minimo
valore potenziale sull’insieme dei percorsi possibili. Il valore del potenziale di un
percorso è definito dall’integrazione lungo lo stesso di un campo potenziale U
sullo spazio delle configurazioni. Definiamo V(τ ) il valore potenziale del percorso
s → q(s, τ ):
ZS
∆
U (q (s, τ ))ds
V (τ ) =
0
Se il l’obiettivo da rangiungere è evitare l’ostacolo il campo potenziale dello
spazio delle configurazioni è definito in modo che il suo valore è alto per
configurazioni vicine all’ostacolo e basso per configurazioni lontane. Cosı̀ percorsi
vicini all’ostacolo hanno alti valori potenziali e viceversa per quelli lontani.
La variazione del potenziale del percorso rispetto a τ è relazionato a η(s, τ )
tramite la seguente equazione:
dV
∆
(τ ) =
dτ
ZS
dU
(q (s, τ ))T η (s, τ ) ds
dq
0
Il principio del metodo di deformazione del percorso consiste nello scegliere
(η0 , v(s, τ )) in modo che
3.3.2
dV
dτ
(τ ) sia negativo.
Algoritmo di Nonholonomic Path Deformation
Partendo da un percorso ammissibile q(s, 0), l’algoritmo iterativamente calcola
una sequenza di percorsi ammissibili s → q(s, τj ) per valori discreti τj di τ dove
j è un intero. Ad ogni iterazione dell’algoritmo, una direzione di deformazione
3.3. Reactive Path Deformation for Nonholonomic Mobile Robots [4]
51
η(s, τj ) è generata basandosi sullo spazio delle configurazioni del campo potenziale
U e un nuovo percorso q(s, τj+1 ) è calcolato come segue:
q (s, τj+1 ) = q (s, τj ) + ∆τj η (s, τj )
(3.24)
τj+1 = τj + ∆τj
(3.25)
dove ∆τj è il passo di discretizzazione.
Le variabili di controllo del processo di deformazione del percorso sono gli
ingressi di verturbazione v e le condizioni iniziali η0 . s → v(s, τj ) fa parte di
un infinito-dimensionale spazio di funzioni vettoriali appartenenti a ∈ C ∞ ([0, S])
definiti su [0, S].
Per semplificare il controllo di deformazione del percorso,
scegliamo di restringerev su un sottospazio di funzioni di dimensioni finite. Sia p
un intero positico. Definiamo e1 , . . . , ep come un insieme di funzioni vettoriali
∈ C ∞ ([0, S]) di dimensione k definite su [0, S]:
ei : [0, S] → Rk
Varie scelte sono possibili per gli ei . Per ognuna di queste funzioni definiamo
E1 (s, τj ) come soluzione di 3.23 con condizione iniziale η0 = 0 e con ei (s) come
ingresso:
Ėi (s, τj ) = A (s, τj ) Ei (s, τj ) + B (s, τj ) ei (s)
(3.26)
Ei (0, τj ) = 0
(3.27)
3.3. Reactive Path Deformation for Nonholonomic Mobile Robots [4]
52
Se restringiamo v(s, τj ) all’insieme di funzioni aventi base:
v (s, τj ) =
p
X
λi ei (s)
(3.28)
i=1
dove λ = (λ1 , . . . , λp ) ∈ Rp è un vettore, come 3.23 è lineare, la direzione di
deformazione η corrispondente a v è la stessa combinazione lineare di solezione
Ei
η (s, τj ) =
p
X
λi Ei (s, τj )
(3.29)
i=1
Usando questa restrizione gli ingressi di perturbazione v sono definiti
unicamente dal vettore λ.
Si vuole che il pocesso di deformazione non modifichi le configurazioni iniziale
e del goal del percorso. Si impongono cosı̀ le seguenti condizioni al contorno:
∀j > 0,
q (0, τj ) = (0, 0)
q (S, τj ) = (S, 0)
Questi vincoli sono equivalenti a:
∀j > 0,
η (0, τj ) = 0
(3.30)
η (S, τj ) = 0
(3.31)
l’equazione 3.27 e la 3.29 ci assicurano che la 3.30 è soddisfatta. La 3.31
insieme alla 3.29 diventano un vincolo lineare sul vettore λ:
Lλ = 0
(3.32)
3.3. Reactive Path Deformation for Nonholonomic Mobile Robots [4]
53
dove L è una matrice n × p le cui colonne sono le Ei (S, τj ):
L = (E1 (S, τj ) . . . Ep (S, τj ))
Come detto prima un campo potenzialeU è definito sullo spazio delle
configurazioni. Questo campo potenziale definisce una funzione potenziale V
sullo spazio dei percorsi tramite un’integrazione. Alla luce della 3.29 si ha che:
dV
dτ
=
(τj ) =
p
P
λi
i=1
RS
0
RS
0
dU
dq
dU
dq
(q (s, τj ))T η (s, τj ) ds
(3.33)
T
(q (s, τj )) Ei (s, τj ) ds
Si definiscono quindi i seguenti coefficienti:
ZS
∆
µi =
dU
(q (s, τj ))T Ei (s, τj ) ds
dq
0
Questi coefficienti rappresentano la variazione del potenziale del percorso indotta
da ogni direzione di deformazione Ei . Con questi coefficienti la 3.33 può essere
riscritta come segue:
p
X
dV
(τj ) =
λi µi
dτ
i=1
(3.34)
λi = −µi
(3.35)
Cosı̀ se imponiamo
otteniamo una deformazione η(s, τj ) che porta a soddisfare ai vincoli cinematici e
rende il potenziale decrescente. Infatti:
p
X
dV
(τj ) = −
µ2i ≤ 0
dτ
i=1
3.3. Reactive Path Deformation for Nonholonomic Mobile Robots [4]
54
Definiamo con λ0 questo valore di λ. Oltretutto λ0 soddisfa anche la 3.32.
La 3.32 afferma che l’insieme dei vettori λ che soddisfano le condizioni al
contorno è un sottospazio lineare di Rp . Definiamo λ e proiettiamo λ0 su questo
sottospazio:
¡
¢
λ̄ = Ip − L+ L λ0
dove L+ è la pseudo-inversa di L. Come LL+ L = L, λ soddisfa Lλ = 0. Ci si
chiede naturalmente se la direzione di deformazione dopo la proiezione continua
a rendere il potenziale decrescente. La seguente proposizione ci da una risposta:
Proposition1 :per ogni η ∈ Rp e ogni matrice n × p L, se λ0 = −µ e
λ = (Ip − L+ L)λ0 , then:
µT λ̄ < 0
Ricordiamo che la 3.24 è un’approssimazione al primo ordine rispetto a τ . Per
questa ragione ∆τj kηk∞ con kηk∞ , maxs∈[0,S] kη(s, τj )k deve essere piccolo. ∆τj
è cosı̀ scelto in modo che ∆τj kηk∞ è limitato superiormente da un valore positivo
dato ηmax . Il modo in cui sono scelti i λi in 3.35 non è ottimo a tal proposito.
Infatti, l’obiettivo che vogliamo raggiungere ad ogni iterazione è quello di creare
un percorso potenziale V decrescente il più possibile per |ηk∞ costante.
L’idea è quella di esprimere η in una base ortonormale. Per fare ciò si applica
l’ortonormalizzazione di Gram-Schmidt ai vettori (E1 , . . . , Ep ) in modo
da avere una base ortonormale (F1 , . . . , Fp ). A questo punto:
η (s, τj ) =
p
X
λ⊥
i Fi (s, τj )
i=1
e la 3.34 diventa
p
X
dV
⊥
(τj ) =
λ⊥
i µi
dτ
i=1
(3.36)
3.3. Reactive Path Deformation for Nonholonomic Mobile Robots [4]
con
ZS
∆
µ⊥
i =
55
dU
(q (s, τj ))T Fi (s, τj ) ds
dq
0
Definiamo P la matrice cambiamento di coordinate che ha come colonne i vettori
delle coordinate di Fj espresse in (E1 , . . . , Ep ). L’espressione di η nella base
(E1 , . . . , Ep ) è data da
λ = P λ ⊥ = P P T λ0
(3.37)
Per soddisfare la condizione al contorno 3.31 nella base ortonormale creata si
ha che:
¡
¢
λ̄ = Ip − P (LP )+ L P P T λ0
L’approssimazione 3.24 induce un effetto indesiderato: dopo molte iterazioni i
vincoli anolonomi non sono più soddisfatti e il percorso diviene non ammissibile.
Questo effetto viene chiamato nonholonomic constraint deviation ( deviazione
dei vincoli anolonomi). Bisogna quindi correggere tale deviazione; per portare
a termine tale problema si procede quindi ad una estenzione del sistema
dinamico.
Si aggiungono per ogni configurazione q, n − k vettori di campo
(Xk+1 (q), . . . , Xn )(q) ai k vettori di campo di controllo del sistema in modo che
(X1 (q), . . . , Xn )(q) sia base di Rn . Definiamo il sistema esteso come il sistema
controllato dai vettori di campo:
q̇ =
n
X
ui Xi (q)
(3.38)
i=1
Il sistema 3.38 non è soggetto ad alcun vincolo cinematico.
Un percorso
q(s) di sistema 3.38 è ammissibile per il sistema 3.20 se e solo se per ogni
j ∈ {k + 1, . . . , n} e ogni s ∈ [0, S], uj (s) = 0.
3.3. Reactive Path Deformation for Nonholonomic Mobile Robots [4]
56
Da ora indichiamo con u(s, τ ) = (u1 (s, τ ), . . . , un (s, τ )) la funzione di ingresso
del sistema 3.38 e con v(s, τ ) = (v1 (s, τ ), . . . , vn (s, τ )) le perturbazioni delle
precedenti funzioni di ingresso:
∀i ∈ {1, . . . , n} ,
vi (s, τ ) =
∂ui
(s, τ )
∂τ
La relazione tra gli ingressi di perturbazione v(s, τ ) e la direzione di deformazione
η(s, τ ) è:
η̇ (s, τ ) = Ā (s, τ ) η (s, τ ) + B̄ (s, τ ) v̄ (s, τ )
(3.39)
ma ora le due matrici A(s, τ ) e B(s, τ ) sono entrambe matrici n × n:
Ā =
n
X
i=1
ui
∂Xi
(q)
∂q
and
¡
¢
B̄ = BB ⊥
(3.40)
dove B⊥ = {Xk+1 (q), . . . , Xn (q)} è la matrice le colonne della quale sono i
vettori addizionali di campo. Con ciò la 3.39 può essere riscritta come segue:
η̇ (s, τ ) = Ā (s, τ ) η (s, τ ) + B̄ (s, τ ) v (s, τ ) + B ⊥ (s, τ ) v ⊥ (s, τ )
(3.41)
dove v⊥ (s, τ ) = (vk+1 (s, τ ), . . . , vn (s, τ ) le quali sono le derivate rispetto a τ
delle funzioni di ingresso (uk+1 (s, τ ), . . . , un (s, τ ). Si vuole che le funzioni di
ingresso tendano a 0 al crescere di τ , poniamo cosı̀:
∀i ∈ {k + 1, . . . , n} ,
vi (s, τ ) = −αui (s, τ )
dove α è un numero costante positivo. Questo implica che al crescere di τ ,
∀s ∈ [0, S] ,
ui (s, τ ) = e−ατ ui (s, 0)
3.3. Reactive Path Deformation for Nonholonomic Mobile Robots [4]
e quindi l’ingresso converge a zero esponenzialmente.
57
Si indichi con η1 la
corrispondente direzione di deformazione per τ = τj :
η̇1 (s, τj ) = Ā (s, τj ) η1 (s, τj ) + B ⊥ (s, τj ) v ⊥ (s, τj )
(3.42)
η1 (0, τj ) = 0
(3.43)
Si restringono quindi le funzioni di ingresso (u1 , . . . , uk ) al sottospazio di
dimensione finita delle funzioni base di (e1 , . . . , ep ) e calcoliamo λ = (λ1 , . . . , λp )
in accordo con la 3.37. Si indica con η2 la direzione di deformazione corrispondente
a questi coefficienti:
η2 (s, τj ) =
p
X
λi Ei (s, τj )
i=1
dove ora gli Ei sono soluzioni del sistema 3.39:
Ėi (s, τj ) = Ā (s, τj ) Ei (s, τj ) + B (s, τj ) ei (s)
(3.44)
Ei (0, τj ) = 0
(3.45)
Dato che il sistema 3.41 è lineare, la deformazione ottenuta sommando gli
ingressi di perturbazione v e v⊥ è la somma di η1 e η2 . Vogliamo sempre che la
deformazione non cambi la configurazione iniziale e finale. Le condizioni 3.30 e
3.31 divengono:
η1 (0, τj ) + η2 (0, τj ) = 0
η1 (S, τj ) + η2 (S, τj ) = 0
La prima è sempre soddisfatta. La seconda può essere riscritta come un sistema
affine di equazioni sui parametri (λ1 , . . . , λp ):
η2 (S, τj ) = Lλ = −η1 (S, τj )
Il vettore utile a soddisfare la 3.46 tramite l’ortonormalizzazione è:
¡
¢
λ̄ = −P (LP )+ η1 (S, τj ) + Ip − P (LP )+ L λ
(3.46)
3.3. Reactive Path Deformation for Nonholonomic Mobile Robots [4]
58
Figura 3.6: Modello Simulink per l’algoritmo di deformazione
mentre la direzione di deformazione è:
η (s, τ ) =
p
X
λ̄i Ei (s, τj ) + η1 (s, τj )
i=1
che soddisfa le condizioni al contorno 3.30 e 3.31 e rende decrescenti le componenti
(vk+1 , . . . , vn ) delle velocità lungo i vettori di campo addizionali.
3.3.3
Implementazione in MatLab
Per l’implementazione dell’algoritmo in MatLab del 3.3.2 si è seguito lo stesso
metodo dell’implementazione del pianificatore anolonomo. Lo schema Simulink
del deformatore di traiettorie è rappresentato in figura 3.6. In questo caso lo
schema è formato da:
• un blocco ( S-Function) che contiene l’intero corpo del programma di
Reactive Path Deformation ( vedi appendice);
• due blocchi di uscita per la visualizzazione grafica della posizione (x,y) e
3.3. Reactive Path Deformation for Nonholonomic Mobile Robots [4]
59
l’orientamento ( XY Graph e Scope) e uno per la verifica numerica della
configurazione raggiunta ( Display)
Si dà ora una breve descrizione del codice MatLab dell’implementazione
dell’algoritmo di deformazione: la funzione di inizializzazione mdlInitializeSizes
contiene, oltre l’inizializzazione dei vari vettori delle configurazioni, la posizione
degli ostacoli e la definizione delle costanti presenti nell’algoritmo, che in questo
caso sono state poste pari a:
• a = α = 10 e U0 = ui (s, 0) = 0.001: queste costanti sono utili al calcolo
dell’ingresso addizionale per la correzione delle deviazioni anolonome;
• d0 = 0.0001 e d1 = 0.6:
utili per il calcolo dei potenziali relativi agli
ostacoli.
Questi campi potenziali sono dati da:
vi (M ) =
1
d+d0
vi (M ) =
1
d1 +d0
+
d
(d1 +d0 )2
+
se 0 ≤ d ≤ d1
d1
(d1 +d0 )2
se d > d1
dove M è un punto del piano, d è la distanza tra M e Pi che rappresenta un
obstacle point e d0 < d1 sono delle costanti. Sia fi (M) = −∇vi (m) la forza
nel piano derivante da tali potenziali. La norma di questa campo di forza
rispetto a Pi è:
kfi (M )k =
1
(d+d0 )2
−
1
(d1 +d0 )2
se 0 ≤ d ≤ d1
(3.47)
kfi (M )k = 0 se d > d1
Il campo potenziale totale è definito come la somma dei campi potenziali
relativo ad ogni obstacle point:
U (q) =
X
i
Ui (q)
3.3. Reactive Path Deformation for Nonholonomic Mobile Robots [4]
60
Il gradiente del campo potenziale, relativo al corpo dell’uniciclo, è dato da:
∂Ui
∂R
(q) = −fi (R)
∂q
∂q
• etamax = ηmax = 0.3 che indica la massima deformazione della traiettoria
per ogni iterazione del ciclo di deformazione del percorso.
Dopo le varie inizializzazioni si passa alla funzione mdlDerivatives nella quale
l’algoritmo di Reactive Path Deformation è applicata ad una traiettoria rettilinea
di configuazione iniziale qi = (−1, 0, 0) e finale qf = (1, 0, 0).
Lo spazio degli ingressi è di dimensione 2. Sono state scelte per il sottospazio
finito delle funzioni di perturbazione degli ingressi una base di un sottospazio di
dimensione 6 della serie troncata di Fourier :
e1 (s) = (1, 0)T
e2 (s) = (0, 1)T
¡ ¡ ¢ ¢T
,0
e3 (s) = cos 2πs
S
¡
¡ ¢¢T
e4 (s) = 0, cos 2πs
S
¡ ¡ 2πs ¢ ¢T
e5 (s) = sin S , 0
¡
¡ ¢¢T
e6 (s) = 0, sin 2πs
S
Vediamo alcune simulazioni Simulink dell’algoritmo nelle figure 3.7 e 3.8.
Nella figura 3.7 si notano due traiettorie che indicano come c’è stato il bisogno
di eseguire due iterazioni dell’algoritmo affinchè il percorso risultasse libero da
ostacoli. Le circonferenze blue indicano un area intorno agli obstacle point di
0.2 m che rappresentano un area in cui non deve esserci la presenza di alcuna
configurazione del robot dato che queste sono riferite al centro del Magellan Pro
il quale ha un raggio di 0.182 m. Nella figura 3.8 i due percorsi indicano le
3.3. Reactive Path Deformation for Nonholonomic Mobile Robots [4]
1
0.8
0.6
0.4
0.2
0
−0.2
−0.4
−0.6
−0.8
−1
−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
Figura 3.7: Deformazione di traiettoria rettilinea con un ostacolo
1
0.8
0.6
0.4
0.2
0
−0.2
−0.4
−0.6
−0.8
−1
−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
Figura 3.8: Deformazione di traiettoria rettilinea con due ostacoli
61
3.4. Unione dei due algoritmi ( 3.2 e 3.3)
62
deformazioni relative al primo ostacolo ( percorso rosso) e rispetto ad entrambi
dopo che il robot ha raggiunto una configurazione ad una distanza uguale a 0.6 m
dal secondo obstacle point e quindi ha nuovamente deformato la traiettoria anche
in relazione alla sua presenza( percorso rosso scuro).
3.4
Unione dei due algoritmi ( 3.2 e 3.3)
Dopo aver implementato i due algoritmi separatamente, questi sono stati uniti
in un unico algoritmo di pianificazione e deformazione. Si è usato lo stesso
schema Simulink del Reactive Path Deformation cambiando il programma della
S-Function. Infatti in questo caso nella funzione mdlInitializeSizes oltre alle
definizioni delle varie costanti e all’inizializzazione delle varie matrici utilizzate è
stato inserito l’algoritmo di pianificazione del moto per robot anolonomi illustrato
in 3.2. Quindi quando si entra nella mdlDerivatives la traiettoria è già pianificata
e comincia immediatamente la sua esecuzione fin quando non viene riscontrata la
presenza di un ostacolo sul percorso tracciato. A questo punto come già spiegato
in 3.3 interviene l’algoritmo di deformazione reattiva deformando il percorso
pianificato fin tanto che non è lontano da ostacoli o entra nuovamente in collisione
con nuovi ostacoli prima non percepiti. Andiamo ora a vedere delle simulazioni
eseguite con le seguenti costanti:
• a = α = 10
• U0 = ui (s, 0) = 0.001.
• d0 = 0.0001
• d1 = 0.6.
3.4. Unione dei due algoritmi ( 3.2 e 3.3)
63
1
0.8
0.6
0.4
0.2
0
−0.2
−0.4
−0.6
−0.8
−1
−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
Figura 3.9: Deformazione di traiettoria pianificata in presenza di un solo obstacle
point: configurazione finale qf = (1, 0, π)
• etamax = ηmax = 0.05.
Le simulazioni del metoodo sono illustrate nelle figure 3.9 3.10 3.11 3.12 per
varie posizioni degli obstacle point.
3.4. Unione dei due algoritmi ( 3.2 e 3.3)
64
2
1.5
1
0.5
0
−0.5
−1
−1.5
−2
−2
−1.5
−1
−0.5
0
0.5
1
1.5
2
Figura 3.10: Deformazione di traiettoria pianificata in presenza di un solo obstacle
point: configurazione finale qf = (2, 0, π)
2.5
2
1.5
1
0.5
0
−0.5
0
0.5
1
1.5
2
Figura 3.11: Deformazione di traiettoria pianificata in presenza di tre obstacle point:
configurazione finale qf = (2, 2, 0)
3.4. Unione dei due algoritmi ( 3.2 e 3.3)
65
2
1.5
1
0.5
0
−0.5
−1
−1.5
−2
−2
−1.5
−1
−0.5
0
0.5
1
1.5
2
Figura 3.12: Deformazione di traiettoria pianificata in presenza di due obstacle point:
configurazione finale qf = (1, 2, 1)
Capitolo 4
Controllo in feedback sul
cammino pianificato
In questo capitolo viene trattato il problema dell’inseguimento di una traiettoria (
trajectory tracking ). L’obiettivo è quello di trovare una legge di controllo tale
che un punto di riferimento sul robot mobile insegua una traiettoria nello spazio
Cartesiano partendo da una configurazione iniziale1 . Si procede alla formulazione
del problema. Considerando un veicolo di riferimento del tipo uniciclo le cui
equazioni sono:


ẋ = vr cos θr


 r




1
ẏr = vr sin θr
θ̇r = wr
La configurazione iniziale non necessariamente deve appartenere alla traiettoria di
riferimento
66
4.1. Dynamic Feedback Linearization [5]
67
Si indichi lo stato del veicolo da controllare con:
 
x
 
 
z= y 
 
θ
e lo stato del veicolo di riferimento con:


xr




zr =  yr 


θr
Il problema dell’inseguimento di traiettoria consiste nel trovare una legge di
controllo a controreazione:



v
w
 = k (z, zr , vr , wr )
tale che si abbia:
lim (zr (t) − z (t)) = 0
t→∞
per qualsiasi configurazione iniziale z(0).
4.1
Dynamic Feedback Linearization [5]
Il modello cinematico dell’uniciclo non può essere trasformato in un sistema
lineare e controllabile usando un feedback statico dallo stato. Una soluzione
alternativa consiste nel ricorso al feedback dinamico. La linearizzazione del
modello risulta infatti possibile aumentando lo spazio delle variabili di stato. In
4.1. Dynamic Feedback Linearization [5]
68
particolare dato il modello dell’uniciclo:


 ẋ = v cos θ


ẏ = v sin θ




θ̇ = w
consideriamo un integratore aggiuntivo la cui variabile di stato è denotata da ξ:
ξ˙ = u1
Si definisca poi la seguente legge di controllo:

 v=ξ
 w=u
2
che consiste nel porre l’integratore sull’ingresso dell’uniciclo. Il sistema aggiornato
risulta:



ẋ = ξ cos θ





 ẏ = ξ sin θ

 θ̇ = u2





 ξ˙ = u1
(4.1)
effettuando ora le derivate delle velocità cartesiane si ha:
ẍ = ξ˙ cos θ − ξ sin θθ̇
ÿ = ξ˙ sin θ + ξ cos θθ̇
Tenendo conto della 4.1 si ha usando la forma matriciale:
  


ẍ
cos θ −ξ sin θ
u
 =
 1 
ÿ
sin θ ξ cos θ
u2
si noti che:

det 

cos θ −ξ sin θ
sin θ
ξ cos θ
=ξ
(4.2)
4.1. Dynamic Feedback Linearization [5]
69
La 4.2 mostra la relazione lineare che si viene ad instaurare nel sistema 4.1
tra le derivate seconde delle componenti cartesiane e gli ingressi.
Questa
relazione cade in singolarità solo per ξ = 02 , indipendentemente dalle variabili
di stato dell’uniciclo. Introdotte due variabili ausiliarie γ1 eγ2 , possiamo operare
un’inversione della cinematica del secondo ordine descritta dalla 4.2:
 
−1 
 



cos θ sin θ
γ
cos θ −ξ sin θ
u
γ
  1 =
 1 =
 1 
γ2
− sinξ θ cosξ θ
sin θ ξ cos θ
u2
γ2
(4.3)
Il sistema esteso è cosı̀ completamente linearizzato in una forma controllabile
e descritto da due catene di doppi integratori:
ẍ = γ1
ÿ = γ2
A questo punto data la traiettoria del veicolo di riferimento (xd , yd ), il problema
del trajectory tracking risulta di facile soluzione.
Al fine di ottenere una
convergenza asintotica a zero degli errori cartesiani di velocità e posizione
possiamo imporre:
γ1 = ẍr + kd1 (ẋr − ẋ) + kp1 (xr − x)
(4.4)
γ2 = ÿr + kd2 (ẏr − ẏ) + kp2 (yr − y)
con kdi > 0 e kpi > 0. Il risultante compensatore dinamico è:
ξ˙ = γ1 cos θ + γ2 sin θ
v=ξ
w=
(4.5)
γ2 cos θ−γ1 sin θ
ξ
Lo stato del compensatore dinamico deve essere inizializzato al valore
ξ(0) = vd (0). Questo garantisce l’esatto inseguimento della traiettoria per un
dato stato iniziale del robot.
2
cioè quando l’uniciclo è fermo
4.2. Simulazioni MatLab con DFL
70
1
0.8
0.6
0.4
0.2
0
−0.2
−0.4
−0.6
−0.8
−1
−1
Figura 4.1:
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
Controllo via DFL sul cammino pianificato:
1
configurazione finale
qf = (1, 0, π)
4.2
Simulazioni MatLab con DFL
Questo tipo di controllo è stato implementato nell’algoritmo di pianificazione
e deformazione per permettere al robot di eseguire correttamente la traiettoria
deformata. Si è quindi posto:
• ξ(0) =
p
dx2 + dy2 ;
• kp = 100
• kd =
√
201
I risultati ottenuti dalle simulazioni sono rappresentati in figura 4.1 4.2 4.3
4.4, dove il cammino verde rappresenta le configurazioni del robot che eseguono
il trajectory tracking sul cammino deformato.
4.2. Simulazioni MatLab con DFL
71
2
1.5
1
0.5
0
−0.5
−1
−1.5
−2
−2
Figura 4.2:
−1.5
−1
−0.5
0
0.5
1
1.5
Controllo via DFL sul cammino pianificato:
2
configurazione finale
qf = (2, 0, π)
2
1.5
1
0.5
0
−0.5
−1
−1
Figura 4.3:
qf = (2, 2, 0)
−0.5
0
0.5
1
1.5
2
Controllo via DFL sul cammino pianificato:
2.5
configurazione finale
4.2. Simulazioni MatLab con DFL
72
2
1.5
1
0.5
0
−0.5
−1
−1.5
−2
−2
Figura 4.4:
qf = (1, 2, 1)
−1.5
−1
−0.5
0
0.5
1
1.5
Controllo via DFL sul cammino pianificato:
2
configurazione finale
Bibliografia
[1] G. Campion, G. Bastin, and B. Dandrea Novel, Structural properties and
classification of kinematic and dynamic models of wheeled mobile robots, IEEE
Transactions on Robotics and Automation 12 (1996), no. 1, 47–62.
[2] L. Freda, Controllo del moto del robot magellan pro con applicazione
all’esplorazione sensoriale di ambienti ignoti, Tesi di Laurea in Ingegneria
Informatica, Università di Roma la Sapienza, AA 2002-2003.
[3] iRobot, Mobility robot integration software user’s guide, iRobot, 2001.
[4] F. Lamiraux, D. Bonnafous, and O. Lefebvre, Reactive path deformation for
nonholonomic mobile robots, IEEE Transactions on Robotics 20 (2004), no. 6,
967–977.
[5] G. Oriolo, A.D̃e Luca, and M. Vendittelli, Wmr control via dynamic
feedbacklinearization: Design, implementation and experimental validation,
IEEE Transactions on Control System Technology 10 (2002), no. 6, 835–852.
[6] Giuseppe Oriolo, Control of nonholonomic systems, 2004, Dispense del corso
di Dottorato in Ingegneria dei sistemi,Università di Roma ”La Sapienza”.
73