Luca Baglivo

Navigazione di Veicoli Autonomi

Pianificazione e Controllo di Traiettoria

Appunti per il corso di “Robotica Spaziale” per Ingegneria Aerospaziale

2

PREFAZIONE Lo scopo della presente dispensa è quello di fornire una panoramica introduttiva delle teorie e

applicazioni di base sulla pianificazione e il controllo di traiettoria dei robot mobili in particolare dei veicoli autonomi. Nell’ambito trattato vi è un’inevitabile coinvolgimento dello studio dei sistemi non olonomi, e anzi proprio a causa di questa caratteristica dei robot mobili si è creato e si va ancora sviluppando un filone di ricerca che mira a fornire dei metodi generali per la navigazione dei robot non olonomi al pari di quelli forniti dal più maturo studio dei problemi inerenti i robot olonomi. E’ naturale, quindi, fare riferimento ai robot manipolatori in quanto sistemi olonomi per comprendere le differenze di applicazione e di complessità delle teorie nell’uno e nell’altro campo.

Desidero ringraziare il prof. Mariolino De Cecco e il prof. Francesco Angrilli per gli importanti

spunti, per i preziosi consigli e per lo stimolo alla preparazione di questa dispensa.

SOMMARIO Dopo una necessaria introduzione sui sistemi non olonomi vengono descritti alcuni dei principali modelli cinematici di veicoli autonomi su ruote la cui costruzione è strettamente connessa all’imposizione del vincolo di non slittamento delle ruote. Il modello cinematico del robot condiziona in modo stringente i task di pianificazione e controllo del moto. Si riportano alcuni metodi di pianificazione del moto che sfruttano la proprietà di nilpotenza di molti sistemi non olonomi. E’ descritto un metodo per la pianificazione di percorsi a curvatura continua che tengano conto dei vincoli non olonomi e dei vincoli meccanici e dinamici del robot. Per il modello di tipo automobile è descritto un metodo per la pianificazione dei controlli a ciclo aperto. Ai controlli pianificati è necessario aggiungere i controlli di feedback che consentano di portare a termine il task correttamente anche in presenza degli inevitabili disturbi di misura, deviazioni dalle condizioni iniziali previste e deviazioni dal modello ideale. Si presenta un’introduzione al controllo in feedback sia con approccio teorico (feedback statico linearizzato), sia con approccio euristico (algoritmi euristici di inseguimento).

1

INDICE 1. INTRODUZIONE ........................................................................................................................ 3

1.1. Ambienti strutturati, semi-strutturati, non strutturati ........................................................... 3 2. PRINCIPALI MODELLI CINEMATICI DI AGV ..................................................................... 4

2.1. Sistemi non olonomi ............................................................................................................ 4 2.2. Modello monociclo .............................................................................................................. 5 2.3. Modello a guida differenziale .............................................................................................. 7 2.4. Modello a tre ruote e car-like ............................................................................................... 9 2.5. Modello automobile con N rimorchi .................................................................................. 12 2.6. Sistemi olonomi e non olonomi ......................................................................................... 14 2.7. Altri esempi di sistemi non olonomi .................................................................................. 14

3. PIANIFICAZIONE E CONTROLLO DELLA TRAIETTORIA .............................................. 15 3.1. Percorsi e traiettorie ammissibili ........................................................................................ 16 3.2. Path following e Trajectory tracking ................................................................................. 17 3.3. Pianificazione del percorso e pianificazione del moto ....................................................... 19 3.4. Metodi di pianificazione del moto per sistemi non olonomi ............................................. 20

3.4.1. Sistemi in “chained form”. ......................................................................................... 20 3.4.2. Pianificazione di traiettorie open-loop ....................................................................... 24 3.4.3. Percorsi a curvatura continua. Clotoidi. ..................................................................... 27 3.4.4. Percorsi a curvatura polinomiale................................................................................ 33

3.5. Controllo di traiettoria ........................................................................................................ 35 3.5.1. Controllabilità ............................................................................................................ 36 3.5.2. Feedback statico linearizzato ..................................................................................... 38 3.5.3. Applicazione del feedback linearizzato al controllo di traiettoria rettilinea .............. 39 3.5.4. Metodi euristici per l’inseguimento di percorso ........................................................ 41

4. BIBLIOGRAFIA ....................................................................................................................... 43

2

1. INTRODUZIONE La robotica mobile si occupa dello studio dei sistemi robotici in grado di spostarsi

autonomamente in un certo tipo di ambiente per assolvere un determinato compito e trova al giorno d’oggi molte applicazioni in svariati campi, dal settore civile a quello industriale, dal militare all’aerospaziale. Nell’ambito della robotica mobile lo studio delle tecniche di navigazione autonoma è essenziale poiché si occupa del problema principale, quello di rendere il robot in grado di localizzarsi, pianificare il proprio moto e controllarne l’esecuzione.

Il problema della localizzazione vede la necessità di utilizzare degli strumenti di misura della posizione che siano adatti all’ambiente in cui il robot opera e di integrare più sensori in un sistema di localizzazione. Per ottenere un’unica misura di posizione che integri in modo ottimo le singole misure dei vari strumenti, si utilizzano opportune tecniche che sono oggetto di studio del più generale problema del sensor fusion [1].

Altro problema fondamentale della navigazione autonoma è la pianificazione del moto, intesa a due livelli: un livello alto di strategia globale che riguarda il raggiungimento dell’obiettivo finale, e, ad esso funzionale, un livello più basso di pianificazione locale.

A causa di inevitabili fattori di disturbo, è necessario, come per i robot manipolatori a maggior ragione anche per i robot mobili, un controllo in feedback del moto pianificato.

A meno di poche eccezioni, dal punto di vista cinematico i robot mobili su ruote sono sistemi non olonomi. I sistemi non olonomi sono caratterizzati da equazioni di vincolo sulle velocità delle variabili che descrivono il sistema. Queste equazioni non sono integrabili e tipicamente compaiono quando il sistema ha un numero di controlli inferiore al numero dei gradi di libertà. Ad esempio un robot di tipo automobile ha due controlli, la velocità lineare ed angolare, mentre le variabili che descrivono l’evoluzione del sistema sono tre, ad esempio la posizione cartesiana di un suo punto e l’assetto. La conseguenza dei vincoli non olonomi è che non tutte le traiettorie nello spazio delle variabili generalizzate del robot sono ammissibili. Per questo le tecniche della geometria classica sviluppate per il moto dei sistemi olonomi quali i manipolatori, non sono applicabili a quelli non olonomi.

1.1. Ambienti strutturati, semi-strutturati, non strutturati La strategia complessiva di pianificazione e controllo del moto dipende fortemente dalla

tipologia dell’ambiente in cui il robot si muove. Si possono classificare tre macro-categorie di ambienti in base alle informazioni dell’ambiente di cui si dispone:

• Ambienti strutturati: si dispone di una completa conoscenza topologica del layout degli spazi liberi e della dislocazione degli ostacoli oltre alla posizione del target. Un esempio tipico è quello di un ambiente logistico industriale (magazzino merci al coperto, macchine per la pallettizzazione etc.). Il percorso può essere completamente pianificato a priori off-line e situazioni impreviste non sono gestibili dal robot per portare a compimento il proprio task

• Ambienti semi-strutturati: la conoscenza dell’ambiente non è completa poichè, ad esempio non si dispone a priori di sufficienti informazioni sul target o perché è prevista una gestione flessibile del percorso a livello locale e in tempo reale per l’aggiramento di ostacoli non conosciuti a priori. Il robot può pianificare il moto in uno spazio ad esso circostante modificando il percorso già pianificato globalmente off-line.

• Ambienti non strutturati: non si dispone di informazioni sufficienti per pianificare globalmente il moto a priori. Il robot deve essere in grado di localizzarsi in tempo reale rispetto all’ambiente e pianificare il moto in itinere per portare a termine il task (problema SLAM, Simultaneous Localization and Map Building).

3

2. PRINCIPALI MODELLI CINEMATICI DI AGV In questo capitolo vengono riportati i principali modelli cinematici di veicoli autonomi utilizzati

nella robotica mobile. Questi modelli servono a descrivere l’evoluzione nel tempo delle variabili di interesse del sistema, ossia di ciò che viene definito lo stato del sistema nello spazio delle sue possibili configurazioni definite dal vettore di coordinate generalizzate o variabili di configurazione. Queste coordinate sono in numero pari a quello dei gradi di libertà del sistema e possono avere o meno significato fisico. La costruzione dei modelli cinematici parte da una fondamentale ipotesi che li accomuna, quella di sussistenza del vincolo di non slittamento delle ruote. Questo vincolo appartiene alla classe dei vincoli non olonomi o anolonomi, ciò che matematicamente, nel caso dei robot mobili su ruote, si traduce in una restrizione dei possibili valori di modulo e direzione della velocità e, di conseguenza, delle possibili traiettorie realizzabili a a partire da una certa configurazione. I modelli che rispettano questo vincolo sono ideali; in realtà sussistono sempre delle deviazioni dal modello a causa della stessa effettiva non idealità dei vincoli.

In questa trattazione si focalizza l’attenzione al modello di controllo del sistema considerato ideale. Inoltre, anche nell’ipotesi in cui vengano rispettati, i vincoli introducono delle complicazioni nella pianificazione e nel controllo di traiettoria dei veicoli non olonomi.

2.1. Sistemi non olonomi Si consideri il sistema meccanico la cui configurazione può essere descritta completamente dal

vettore di coordinate generalizzate q Q∈ , appartenente allo spazio vettoriale reale Q di dimensione . Tipicamente il moto del sistema è soggetto a vincoli dovuti alla struttura stessa del sistema oppure al modo in cui è attuato o controllato. Tali vincoli possono essere bilaterali o unilaterali (rispettivamente espressi da equazioni o disequazioni) e possono dipendere o meno dal tempo. Limitatamente ai vincoli bilaterali non dipendenti dal tempo, le relazioni matematiche che esprimono il vincolo possono essere equazioni nelle coordinate generalizzate e/o nelle loro derivate (in tal caso il vincolo è detto differenziale).

n

I vincoli detti olonomi possono avere la forma:

( ) 0, 1,..,ih q i k n= = < ( 2-1)

Le funzioni si assumono continue e indipendenti e il sistema soggetto a tali vincoli è detto anch’esso olonomo. La maggior parte dei manipolatori forniscono un tipico esempio di vincoli olonomi

:ih Q →

Un sistema soggetto a tali vincoli è costretto a restringere l’insieme delle proprie configurazioni possibili a un sottospazio di Q di dimensione n k− .. La configurazione del sistema può essere descritta non più da ma da n n k− nuove coordinate generalizzate che rappresentano i gradi di libertà del sistema. Ad esempio, nel caso di una catena cinematica a base fissa costituita da due elementi vincolati con due coppie rotoidali (o prismatiche), una a telaio e l’altra tra i due elementi, il sistema privo di vincoli possiede 2 1n m 2= ⋅ = gradi di libertà ( per ogni corpo libero nello spazio), mentre i vincoli olonomi delle coppie rotoidali (ciascuna impone 5 vincoli singoli) tolgono g.d.l. per cui la configurazione del sistema può essere completamente descritta da g.d.l.

6m =

2 5 10k = ⋅ =2− =n k

I vincol detti cinematici includono le coordinate generalizzate e le velocità. Nella forma lineare rispetto alle velocità generalizzate (detta di Pfaffian):

( ) 0, 1,...,Tia q q i k n= = < , oppure

4

( ) 0A q qT = ( 2-2)

La matrice è costituita da k vettori indipendenti. TAQuesti vincoli limitano i possibili movimenti del sistema restringendo l’insieme delle velocità generalizzate che possono essere realizzate in una data configurazione, il tipico esempio è quello di una ruota con il vincolo di non strisciamento riportato nel paragrafo seguente. E’ naturale che l’esistenza di vincoli olonomi implica l’esistenza di vincoli cinematici ottenuti per derivazione rispetto al tempo:

0, 1,...,ih q iq

∂= =

∂k

Invece il contrario non è necessariamente vero. Accade infatti che i vincoli cinematici non sono integrabili, ossia non possono essere messi nella forma (2-1). Ad esempio, nel caso di un solo vincolo cinematico, se il vincolo è integrabile allora esiste una funzione tale per cui

e , dove c è una costante di integrazione legata alle condizioni iniziali . Se i vincoli non sono integrabili, i vincoli e il sistema meccanico sono detti non olonomi o anolonomi. Un esempio tipico di sistema non olonomo è l’automobile. Le velocità di un’automobile sono vincolate perché le ruote non possono muoversi in direzione laterale. Perciò, la stessa automobile non può muoversi di lato né ruotare sul posto. Nonostante ciò, sappiamo che è possibile parcheggiare un’autovettura in qualsiasi punto e con qualsiasi orientazione compatibilmente con la presenza di ostacoli.

( )h q/ (Th q a q∂ ∂ = ) 0( )h q c= 0q

2.2. Modello monociclo

δx

y

δx

y

Figura 1: Modello uniciclo con quattro coordinate generalizzate.

Si consideri un disco che può rotolare senza strisciare su un piano (Figura 1), mantenendo verticale il proprio piano medio. La configurazione è completamente descritta da quattro variabili: due coordinate cartesiane ( x , y ) del punto di contatto con il terreno rispetto a un sistema fisso; l’angolo δ definisce l’orientazione del disco rispetto all’asse x ; l’angolo φ tra un’asse radiale fissato sul disco e l’asse verticale. A causa del vincolo di non-slittamento, le velocità generalizzate del sistema non possono assumere valori arbitrari. Infatti, indicando con R il raggio del disco, devono soddisfare i vincoli

cos( ) 0x Rφ δ− =

5

sin( ) 0y Rφ δ− = ,

oppure in forma matriciale:

1 0 cos( ) 0

0 1 sinT

xR

A q q yR

δδ

φ

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

che chiaramente esprimono la condizione che la velocità del centro del disco si mantenga nel piano medio del disco, ossia risulti sempre tangente alla traiettoria. Da questi vincoli differenziali non è possibile ottenere una relazione sulle coordinate generalizzate, il numero di gradi di libertà delle velocità del sistema è ridotto a due (4 meno 2) ma non si possono ridurre le coordinate generalizzate. In altre parole i vincoli restano solo sulle velocità e non sono integrabili e, come conseguenza, non c’è alcuna limitazione sulle configurazioni che possono essere raggiunte dal disco. Infatti è possibile muovere il disco da una configurazione 1 1 1 1[ , , , ]x y δ φ a una configurazione

2 2 2 2[ , , , ]x y δ φ attraverso la seguente sequenza di moto:

1. Si fa rotolare il disco portando il punto di contatto da 1 1( , )x y a 2 2( , )x y lungo una qualsiasi curva di lunghezza )2 1( 2R kφ φ− + π , con k intero positivo

2. Si fa ruotare il disco attorno al proprio asse verticale da 1δ a 2δ . Chiaramente le curve possibili sono infinite. Dalle equazioni dei vincoli si evince subito il modello cinematico del monociclo che costituisce la base per qualsiasi tipo di modello ottenuto da vincoli di non slittamento:

cos 0sin 0

0 1

xy v w

δδ

δ

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

( 2-3)

Nel modello sono introdotti i due controlli e , rispettivamente la velocità del centro del disco e la velocità angolare del disco attorno all’asse verticale, che sono in numero pari a quello dei gradi di libertà sulle velocità.

v w

x

φ

x

φ

Figura 2: Disco vincolato su guida rettilinea.

6

Diverso è il caso, anche se quasi identico, di un disco che rotola senza strisciare su di una guida rettilinea verticale che, chiaramente, genera un vincolo integrabile, quindi olonomo, e ciò riduce il numero di coordinate generalizzate da due a una poiché in questo caso la coordinata rettilinea x e l’angolo di rotazione φ sono legati da una relazione finita di proporzionalità tramite il raggio del disco. Matematicamente:

x Rφ=

Perciò, integrando:

x R cφ= +

dove c è una costante che dipende dalle condizioni inziali. In questo esempio il vincolo singolo sulle velocità è scritto nella forma di Pfaffian con ( ) [1 ]Ta q R= −

c ed è possibile ottenere la

soluzione di , che è / ( )Th q a q∂ ∂ = ( )h q x Rφ= − = . Appare anche evidente come, in presenza di vincoli sulle velocità, è importante stabilire se essi

siano integrabili o meno al fine di analizzare qual è lo spazio delle configurazioni raggiungibili dal sistema, cioè se questo spazio è ristretto dai vincoli oppure no e in tal caso i vincoli sono non olonomi.

2.3. Modello a guida differenziale

x

y

RC

XY

bPv = [x y δ]

O

δ

b

ICR

PL

PR

Figura 3: Schema del modello differential drive.

Un classico sistema di locomozione per robot mobili è costituito da due ruote motrici parallele, controllate in velocità o in accelerazione da due motori indipendenti. Poiché due ruote non sono sufficienti per un appoggio stabile, si aggiungono delle ruote folli per mantenere il veicolo con inclinazione costante rispetto al terreno. Prendendo come riferimento il punto medio tra le ruote motrici, Pv , la configurazione del sistema è definita da tre coordinate: il vettore [ , ]x y che individua la posizione di Pv e l’angolo di assetto δ che definisce la direzione istantanea di moto del veicolo e coincide con l’orientazione delle ruote motrici. Detta la distanza tra le ruote, , , rispettamene le velocità della ruota destra e sinistra e quella del punto di riferimento, il modello cinematico può essere scritto come

b Rv Lv v

7

cos2 cos

sin sin2

R L

R L

R LR L

v v

x vv vy

v vv vbb

δ

vδ

δ δδ

+⎡ ⎤⎡ ⎤⎢ ⎥⎢ ⎥⎡ ⎤ ⎢ ⎥

+ ⎢ ⎥⎢ ⎥ ⎢ ⎥= = ⎢⎢ ⎥ ⎢ ⎥⎢ ⎥−⎢ ⎥ ⎢ ⎥⎣ ⎦ − ⎢ ⎥⎢ ⎥ ⎣ ⎦

⎣ ⎦

⎥ ( 2-4)

Il modello si ottiene semplicemente considerando la rotazione del veicolo attorno al proprio centro di istantanea rotazione (ICR) ed esprimendo la velocità del punto di riferimento in funzione delle velocità delle ruote destra e sinistra. Si riportano di seguito i calcoli per maggior chiarezza. Nel sistema di riferimento mobile solidale al robot, le componenti non nulle (lungo l’asse x) dei vettori velocità sono:

v

( /

( /

vc

x

RR c

x

LL c

x

dPv RdtdPv Rdt

dPv Rdt

δ

δ

δ

⎛ ⎞= =⎜ ⎟⎝ ⎠

⎛ ⎞= = +⎜ ⎟⎝ ⎠

⎛ ⎞= = −⎜ ⎟⎝ ⎠

2)

2)

b

b

Sommando e sottraendo membro a membro le ultime due equazioni si ottiene:

2R L

R L

v vv

v v bδ

+=

− =

Il modello cinematico cambia a seconda delle coordinate generalizzate e dei controlli che si scelgono, riguardo a questi ultimi si riportano due esempi di modelli dello stesso sistema. Il primo è un modello dinamico controllato direttamente utilizzando accelerazioni e delle ruote: Ru Lu

cos2 0 0

0 0sin2

0 01 00 10

0

R L

R L

RR L

R

L

v v

xv vy

u uv vv bv

δ

δδ

+⎡ ⎤⎢ ⎥

⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎢ ⎥+⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥

⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥= +⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥−⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥

⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎣ ⎦ ⎣ ⎦⎣ ⎦

⎢ ⎥⎣ ⎦

L ( 2-5)

Il secondo ha come controlli la velocità del punto medio tra le ruote motrici e la velocità angolare del veicolo, perciò:

cos 0sin 0

0 1

xy v

δδ

δ

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

w ( 2-6)

8

Il modello (2-6) è formalmente identico a quello ricavato per il monociclo. Come si vedrà nel paragrafo dedicato, le caratteristiche di controllabilità e gli stessi vincoli del sistema cambiano al variare del modello che si considera.

2.4. Modello a tre ruote e car-like

x

y

O

ICR

XY

bPv

b/sin(α)

δ

α

x

y

O

ICR

XY

bPv

b/sin(α)

δ

α

Figura 4: Modello a tre ruote

x

y

O

ICR

α

XY

bPv

δ

Figura 5: Modello automobile (car-like)

x

y

O

ICR α

Pv

δ

x

y

O

ICR α

Pv

δ

x

y

O

ICR α

Pv

δ

Figura 6: Modello biciclo

Si vuole costruire il modello di un veicolo a tre ruote, con ruote posteriori ad asse fisso e ruota anteriore sterzante (Figura 4). Si utilizzano le coordinate generalizzate [ , , , ]x y δ α , dove ( , )x y sono le coordinate cartesiane del punto medio dell’asse posteriore (Pv), δ è l’angolo che misura l’assetto del veicolo rispetto all’asse x , α è l’angolo di sterzata della ruota anteriore misurato rispetto all’asse di simmetria longitudinale del veicolo.

Innanzitutto si individuano i vincoli del sistema, risulta evidente che essi sono due vincoli di non slittamento: uno per la ruota anteriore, l’altro vincolo è unico per tutte le ruote posteriori (tra loro parallele) e si può pensare imposto su una ruota fittizia posta nel punto Pv, infatti qualsiasi ruota che si trovi con il proprio asse di rotolamento allineato con l’asse posteriore è vincolata a seguire lo stesso atto di moto rigido in modo solidale a quello dell’asse e secondo una velocità ad esso ortogonale. Dette ( , )a ax y le coordinate della ruota anteriore si può scrivere:

1° vincolo non olonomo per la ruota anteriore

sin( ) cos( ) 0a ax yδ α δ α+ − + =

2° vincolo non olonomo per la ruota posteriore

9

sin cos 0x yδ δ− =

Poiché

cossin

a

a

x x by y b

δδ

= += +

Il primo vincolo diventa:

( sin )sin( ) ( cos )cos( )

sin( ) cos( ) cos 0

x b y b

x y b

δ δ δ α δ δ δ α

δ α δ α δ α

− + − +

= + − + − =

+ =

Per cui la matrice dei vincoli è

sin( ) cos( ) 0( )

sin cos 0 0T bcos

A qδ α δ α α

δ δ+ − + −⎡ ⎤

= ⎢ ⎥−⎣ ⎦

Il rango della matrice è costante1 per qualsiasi valore di δ e α e vale 2. Ciò implica che lo spazio delle soluzioni del sistema costituito dai due vincoli, cioè delle velocità generalizzate ammissibili, ha dimensione 2: possiamo controllare il sistema con due gradi di libertà sulle velocità.

Esistono vari modi per ottenere il modello cinematico di un sistema soggetto a vincoli cinematici, uno di essi è quello di partire dall’equazione dei vincoli 0TA q = , valutare il rango della matrice , e calcolare il nucleo associato alla matrice. Tuttavia il risultato di questa operazione non porta alla scrittura di un modello con immediato significato fisico perciò è preferibile ricavare il modello dai metodi cinematici della meccanica.

TA

Si consideri la rotazione del veicolo attorno al proprio centro di istantanea rotazione (ICR); detto il vettore posizione della ruota anteriore rispetto al sistema di riferimento fisso, aP caR e vcR rispettivamente il raggio di curvatura in corrispondenza della ruota anteriore e del punto di riferimento : vP

v

sin

tan

a ca

c

bv R

bv R

δ δα

δ δα

= =

= =

Le grandezze sopra ricavate esprimono rispettivamente la velocità della ruota anteriore e del punto di riferimento e i loro segni dipendono dalle convenzioni stabilite per i segni di δ e di b.

Dalla prima equazione:

sinavb

αδ = ( 2-7)

Dalla seconda equazione risulta anche l’espressione della curvatura κ

1Il determinante del minore formato dalle prime due righe e dalle prime due colonne vale sinα , per 0α = i due determinanti dei minori formati dalla terza colonna e rispettivamente dalla prima e seconda colonna non si annullano mai contemporaneamente per ogni valore di δ .

10

1

cv

v v tanR b

αδ κ κ= = ⇒ = ( 2-8)

Perciò, volendo esprimere le velocità x e in funzione della velocità del punto di riferimento, dell’assetto e dell’angolo di sterzo:

y

cos cos cos costan

sin sin cos sintan

a

a

bx v v

by v v

δ δ δ αα

δ

δ δ δ αα

= = =

= = = δ ( 2-9)

In tal modo il modello cinematica del sistema, nella forma del sistema di controllo, il seguente:

1

cos cos

cos sin

sin

0

0001

b

xy

u

α δ

α δ

αδα

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

2u ( 2-10)

Dove i controlli in questo caso scelti, i più comunemente usati, sono la velocità di trazione della ruota anteriore e la velocità di sterzata. Se invece il sistema è controllato tramite la velocità di trazione dell’asse posteriore è sufficiente sostituire ad il controllo 1u 1 1' cosu u α= e il modello diventa:

1

cos 0sin 0

'tan 010

xy

ub

δδαδ

α

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

2u ( 2-11)

Come atteso, le prime due equazioni del modello (2-11) sono quelle del monociclo ma in questo caso la velocità di variazione di assetto è dipendente dalla velocità di sterzata. E’ anche importante notare che quest’ultimo modello possiede una singolarità in / 2α π= ± che corrisponde alla situazione la ruota sterzante è disposta ortogonalmente all’asse longitudinale. Questa singolarità invece non compare nel modello (2-10) a trazione anteriore. Un’altra importante considerazione è che i modelli ricavati, quando si prende come riferimento il punto medio delle ruote posteriori, sono validi non solo per il veicolo a tre ruote ma anche per il sistema quadriciclo (tipo automobile). Infatti entrambi si possono sintetizzare in un sistema in cui le ruote, sia quelle sterzanti anteriori, che quelle posteriori, vadano a coincidere con due sole ruote virtuali, una anteriore sterzante e l’altra posteriore, poste sull’asse di simmetria longitudinale, ossia al sistema biciclo (Figura 6). Nel caso del sistema quadriciclo le ruote sterzanti devono essere orientate in funzione dell’angolo di sterzo α della ruota sterzante virtuale, in modo da individuare un unico centro di istantanea rotazione del veicolo (Figura 5).

11

2.5. Modello automobile con N rimorchi

α

b 0

Pv

δ0

y

x

b1

δ1=0

δN-1

δN

α

b 0

Pv

δ0

y

x

b1

δ1=0

δN-1

δN

Figura 7: Modello automobile con N rimorchi

Un modello più complesso di quello appena visto si ottiene aggiungendo N rimorchi a un robot triciclo o modello automobile con trazione posteriore. Il modello cinematicamente più semplice prevede che ciascun rimorchio sia agganciato nel punto medio dell’asse posteriore del precedente. Si può scegliere come vettore di coordinate generalizzate quello del modello automobile a cui si aggiunge l’angolo di assetto di ciascun rimorchio, misurato tra il proprio asse di simmetria e l’asse x (Figura 7):

0 1 1[ , , , , ,..., , ]TN Nq x y α δ δ δ δ−=

La dimensione dello spazio delle coordinate generalizzate, il numero di gradi di libertà, è perciò N+4. I vincoli non olonomi sono due per la motrice e altri N vincoli per l’asse posteriore dell’i-esimo rimorchio, le cui coordinate si ottengono da :

1

1

cos1,..,

sin

i

i j jj

i

i j jj

x x bi N

y y b

δ

δ

=

=

⎧= −⎪

⎪ =⎨⎪ = −⎪⎩

∑

∑

Gli N+2 vincoli sono:

vincolo per l’asse anteriore della motrice

0 0a asin( ) cos( ) 0x yδ α δ α+ − + =

sin cos 0x y

vincolo per l’asse posteriore della motrice

δ δ− =

sin cos 0 1,..,

vincoli per l’asse posteriore dell’i-esimo rimorchio

i i i ix y i Nδ δ− = =

Esprimendo i vincoli solo in funzione delle coordinate generalizzate si ottiene:

12

0 0

0 0 0 0

1

sin cos 0

sin( ) cos( ) cos 0

sin cos cos( ) 0 1,..i

i i j j i jj

x y

x y b

x y b i N

δ δ

δ α δ α δ α

δ δ δ δ δ=

− =

+ − + − =

− + − = =∑

( 2-12)

Anziché trovare il nucleo associato alla matrice dei vincoli per ottenere il modello cinematico del sistema di controllo, si possono scrivere direttamente le equazioni cinematiche:

0

0

0

1 1

1 1

cossintan

1 sin( )

cos( ) 1,..,

o

i i i ii

i i i i

x vy v

vb

vb

v v i N

δδαδ

δ δ δ

δ δ

− −

− −

==

=

= − −

= − =

Dove è la velocità lineare dell’asse posteriore (punto medio) dell’i-esimo rimorchio. Infine il modello cinematico:

iv

( )( )

0

0

00

1 0 11

1 0 2 1 12

11 11

11 11

cossin

0tan /

sin( ) /cos( )sin( ) /

cos( ) sin( ) /

cos( ) sin( ) /

ij j i i ij

i

NN N N N N Nj

xy

bb

b

b

b

δδ

ααδ δ δ

δδ δ δ δ

δδ δ δ δδ

δ δ δ δ δ

−

− −=

−

− −=

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

∏

∏

1

0010000000

u 2u

⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥+ ⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥

⎥ ⎢ ⎥⎣ ⎦⎥

( 2-13)

I controlli restano due come nel caso del modello automobile, la velocità di trazione posteriore e la velocità di sterzata .

1u

2u

13

2.6. Sistemi olonomi e non olonomi SISTEMI OLONOMI SISTEMI NON OLONOMI

Esempio: manipolatori con coppie rotoidali e/o prismatiche

Esempio: veicoli su ruote con puro rotolamento

Tipo di vincoli e numero di controlli applicabili N corpi + p vincoli geometrici semplici (es.

moto nel piano) N corpi+p vincoli geometrici semplici

⇓ ⇓ N’ =6*N-p gradi di libertà(g.d.l.) N’ = 6*N-p g.d.l.

⇓ ⇓ N’ velocità indipendenti N’ velocità indipendenti

N’ g.d.l. + k vincoli cinematici integrabili N corpi + k vincoli cinematici non integrabili

⇓ ⇓ n = N’-k g.d.l. n = N’ variabili di configurazione (v.d.c.)

⇓ ⇓ qi , i = 1,..n variabili di giunto (o coordinate

generalizzate) qi , i = 1,..n variabili di configurazione (o

coordinate generalizzate) ⇓

n velocità indipendenti m = n-k velocità indipendenti ⇓ ⇓

m = n controlli non differenziali m < n controlli differenziali (cinematici, a partire dalle velocità)

Pianificazione del moto Traiettoria cartesiana desiderata ( )dx t Traiettoria cartesiana desiderata ( )dx t

⇓ ⇓ Calcolo la traiettoria nello spazio dei giunti:

( ) ( ( ))dq t f x t= Calcolo le m velocità di controllo desiderate

vd(t) = ( ( ))d dv g x t=

2.7. Altri esempi di sistemi non olonomi I vincoli non olonomi possono genearasi a causa di diversi fattori, di natura fisica o legati al tipo di controllo, alcuni di essi sono:

1. Superfici che rotolano senza strisciare Esempi:

• oltre ai robot mobili su ruote, la manipolazione di oggetti con dita robotiche (Figura 9)

2. Conservazione del momento angolare in sistemi multibody Esempi:

• manipolatori flottanti nello spazio senza attuatori esterni • robot saltanti bilanciati dinamicamente (Figura 8) • tuffatori e astronauti in sospensione aerea • satelliti con stabilizzazione di assetto

14

Figura 8: Robot saltellante

Figura 9: Mano robotica con 5 dita

3. Speciali compiti di controllo

• robot ridondanti (ad esempio con 3 g.d.l nel piano ma target senza specifica sull’assetto)

• robot subacquei (6 coordinate generalizzate, 4 inputs di velocità)

3. PIANIFICAZIONE E CONTROLLO DELLA TRAIETTORIA Gli obiettivi della pianificazione del moto si differenziano a seconda che essa sia ad alto livello

o a basso livello. Gli obiettivi di basso livello sono le strategie di pianificazione del moto che consentono di

conseguire gli obiettivi primari, di alto livello. Strategie di moto a basso livello possono essere:

• Percorsi geometrici • Traiettorie, ovvero percorsi parametrizzati nel tempo • Sequenze di comandi di movimento basate sulle informazioni dei sensori di cui il robot

dispone Mentre gli obiettivi primari possono essere:

• Muoversi verso il target senza collidere con ostacoli (ambiente qualsiasi) • Costruire una mappa dell’ambiente (non strutturato) • Trovare un oggetto (in ambiente non strutturato o semi-strutturato)

Il compito più ostico è quello della pianificazione a basso livello di un percorso compatibile (ammissibile) con i vincoli cinematici non olonomi. Mentre per un robot omnidirezionale (Figura 10) qualsiasi percorso è ammissible, per i robot non olonomi è richiesto un certo grado di continuità nelle funzioni che definiscono geometricamente il percorso. Ad esempio, i percorsi ottimi per il veicolo di Reeds e Shepp [21] prevedono una discontinuità nella curvatura (sulla quale si impone un limite superiore a causa del limite sull’angolo di sterzata) e possono essere effettuati dall’asse posteriore di un veicolo sterzante solo a condizione che il veicolo si fermi nei punti di discontinuità. Per questo tipo di veicoli, naturalmente, i percorsi con discontinuità nella tangente non sono ammessi oppure, se pianificati, non possono essere effettuati nemmeno idealmente con errore nullo. Risulta anche importante, per l’analisi sulla fattibilità delle traiettorie pianificate e sulla controllabilità del veicolo, la scelta del punto di riferimento per il quale il percorso e la traiettoria sono pianificati.

15

a)

b)

Figura 10: a) robot con quattro ruote sterzanti e motrici. 8 gradi di libertà di controllo. b) robot con tre ruote, ciascuna può traslare trasversalmente senza strisciare.

3.1. Percorsi e traiettorie ammissibili Lo spazio delle configurazioni di un robot mobile è definito dal minimo numero di parametri (coordinate generalizzate) che permettono di localizzare l’intero sistema nel suo ambiente (ad esempio il vettore [ , , , ]x y δ α nel caso del modello trattato nel §2.4). Come per i manipolatori si è soliti distinguere tra traiettorie cartesiane e traiettorie nello spazio dei giunti, per i robot mobili si definisce traiettoria una data funzione vettoriale continua nel dominio del tempo costituita da funzioni scalari, tante quanti sono i controlli del sistema (ad esempio velocità di trazione e angolo di sterzata). Una traiettoria è ammissibile se e solo se, date le condizioni iniziali e finali della configurazione, è una soluzione del sistema di equazioni differenziali che costituiscono il modello cinematico del robot. Un percorso è ciò che nello spazio delle configurazioni corrisponde a una data traiettoria. In altri termini, se si applica una data legge di moto (traiettoria) al robot, ciò che ne risulta nello spazio cartesiano è un percorso. Un percorso ammissibile corrisponde ad una traiettoria ammissibile.

m

Le relazioni matematiche che intercorrono tra percorso e traiettoria non sono banali per un sistema non olonomo proprio a causa della non integrabilità del sistema di equazioni differenziali del modello cinematico. Infatti, ad esempio, per il modello automobile in cui il vettore delle variabili di configurazione è [ , , , ]x y δ κ ed è costituito dalle coordinate cartesiane ( , )x y del punto medio dell’asse posteriore, dall’assetto δ e dalla curvatura κ , si ha:

( ) ( )cos ( )( ) 0cos ( )( ) ( )sin ( ) ( ) 0sin ( ) ( ) ( )( ) 0( )( ) ( ) ( ) 0 1( )

( ) ( )

x t v t tx t ty t v t t y t t v t u tttt t v t

tt u t

δδδ δ

κδδ κκ

κ

=⎧⎡ ⎤ ⎡ ⎤⎡ ⎤⎪ =⎪ ⎢ ⎥ ⎢ ⎥⎢ ⎥⇔ = +⎨ ⎢ ⎥ ⎢ ⎥⎢ ⎥=⎪ ⎢ ⎥ ⎢ ⎥⎢ ⎥⎣ ⎦ ⎣ ⎦⎣ ⎦⎪ =⎩

( 3-1)

I controlli sono la velocità dell’asse posteriore, , e la variazione di curvatura rispetto al tempo, . v uVolendo passare dalla variabile temporale a t alla coordinata curvilinea è sufficiente tenere presente che:

s

( )

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

dsv tdtd s dss t t

ds dtδκ δ κ κ

=

= ⇒ = = t v t

16

Per cui il modello diventa:

( ) cos ( )( ) sin ( )

( ) ( )( ) ( )

x s sy s s

s ss u s

δδ

δ κκ

=⎧⎪ =⎪⎨

=⎪⎪ =⎩

Se infine si integrano le equazioni del modello si ottiene

0

0 0

( ) cos ( ) ; ( ) ( )

( ) sin ( ) ; ( ) ( )

s s

s

0

s

x s s ds s s ds

y s s ds s u s

δ δ κ

δ κ

= =

= =

∫ ∫

∫ ∫ ds ( 3-2)

Queste equazioni, pur essendo integrabili in forma numerica e utilizzate come modello per la ricostruzione odometrica (noti che siano i controlli applicati) non sono una soluzione del sistema di equazioni differenziali in senso classico in quanto l’assetto, che è una delle variabili di configurazione (stato) del sistema, compare all’interno degli integrali; in altre parole per ottenere la soluzione del sistema, date le condizioni iniziali e finali, si dovrebbe integrare una delle incognite .

3.2. Path following e Trajectory tracking Un’ulteriore definizione di traiettoria è quella di percorso a cui viene associata la legge temporale con cui il percorso viene seguito dal robot. Questa accezione viene utilizzata per distinguere tre classi di problemi in base al compito assegnato al robot:

• Moto punto-punto • Inseguimento di percorso (path following) • Inseguimento di traiettoria (trajectory tracking)

Moto punto-punto

qi =(xi , yi ,δi ,αi )

?

qi =(xi , yi ,δi ,αi )

?

Figura 11: Moto punto-punto. Non ci sono specifiche sulla traiettoria ma solo sulle configurazioni iniziale e finale

Il robot deve arrivare a una posa (posizione e assetto) finale partendo da una data posa iniziale. In generale deve raggiungere una configurazione finale data una configurazione iniziale nello spazio delle coordinate generalizzate. Dal punto di vista del controllo il problema è quello di

17

stabilizzare il robot in un punto di equilibrio nello spazio delle variabili di configurazione. Per un veicolo tipo automobile sono disponibili solo due controlli, ad esempio velocità di trazione anteriore e velocità di sterzata, per modificare le quattro variabili di configurazione ( , , ,x y δ α ). La situazione resta la stessa anche per un veicolo con N rimorchi in cui si hanno due input e N+4 variabili di configurazione. Un’eventuale retroazione sul controllo utilizza come errore la differenza tra la configurazione corrente e quella desiderata.

Inseguimento di percorso

qi =(xi , yi ,δi ,αi )s

d

d 0

qi =(xi , yi ,δi ,αi )s

d

d 0d 0

Figura 12: Inseguimento di percorso. L'obiettivo è annulare la distanza dalla traiettoria indipendentemente dal

tempo

Il robot deve raggiungere e seguire un percorso nello spazio cartesiano partendo da una data configurazione iniziale dentro o fuori dal percorso. L’algoritmo di controllo è basato sulla descrizione geometrica della traiettoria cartesiana, usualmente parametrizzata secondo la coordinata curvilinea . La legge temporale di controllo non è specificata poiché l’obiettivo principale è quello di avvicinare il più possibile il veicolo alla traiettoria riducendone la distanza da essa. Perciò l’evoluzione del parametro nel tempo può essere scelta arbitrariamente e i due input possono essere scalati rispetto al tempo senza cambiare il percorso. Una scelta può essere quella di mantenere costante l’input di velocità di trazione o di variarla secondo una qualsiasi legge temporale, e di effettuare il controllo sull’inseguimento agendo solo sull’angolo di sterzata.

s

s

Inseguimento di traiettoria

q(t)= (x(t), y(t),δ(t) ,α(t))

t

e(t) 0

( ) [ ( ), ( )]x ye t e t e t=

1 1( )x t

2 2( )x t

( )x t

q(t)= (x(t), y(t),δ(t) ,α(t))

t

e(t) 0e(t) 0

( ) [ ( ), ( )]x ye t e t e t=

1 1( )x t

2 2( )x t

( )x t

Figura 13: Inseguimento di traiettoria. Il compito è di inseguire il percorso secondo una legge temporale imposta

18

In questo caso il robot deve raggiungere e seguire una traiettoria nello spazio cartesiano partendo da una data configurazione iniziale dentro o fuori dalla traiettoria, ovvero un percorso con una legge temporale imposta. Si può pensare che il robot debba inseguire e raggiungere un robot virtuale che si muove lungo il percorso desiderato con velocità variabile secondo una legge desiderata. Dal punto di vista del controllo, l’obiettivo è quello di minimizzare l’errore cartesiano

tra la posizione effettiva del robot e quella prevista per ogni istante di tempo . ( )e t t

3.3. Pianificazione del percorso e pianificazione del moto Per quanto detto, la pianificazione non olonoma del moto corrisponde alla pianificazione di

traiettorie ammissibili e può essere assimilata al problema della pianificazione dei controlli a ciclo aperto per ottenere un dato percorso, ciò che nel caso dei manipolatori corrisponde a un problema di cinematica inversa. Il problema, a monte, è quello di pianificare un percorso compatibile sia con i vincoli non olonomi che con la presenza di ostacoli potenzialmente interposti tra il punto di partenza e quello di arrivo. Questo problema applicato ai veicoli autonomi è detto di steering.

In ambiente strutturato la pianificazione del percorso e del moto può essere calcolata a priori a livello globale, ossia includendo il task completo (Figura 14). Tuttavia sussiste l’esigenza di pianificare in tempo reale il percorso a livello locale (in un certo intorno del robot) per permettere al robot di aggirare ostacoli imprevisti (Figura 15) o inforcare un pallet in posizione non nota a priori.

Per un moto punto-punto è sufficiente risolvere in modo diretto il problema calcolando una legge di moto che consenta di guidare il robot dalla configurazione di partenza a quella di arrivo. In tal modo il percorso risulta essere un output dell’algoritmo di pianificazione e non può essere imposto come input, non ci sono vincoli costituiti da ostacoli fisici ma bisogna tenere comunque conto degli ostacoli virtuali, come ad esempio la limitazione sull’angolo di sterzata ( max / 2α α π< ≤ ). La trasformazione delle coordinate generalizzate in coordinate “chained form” (vedi §3.4.1), quando il sistema sia trasformabile, consente di risolvere agevolmente il problema.

Figura 14: Esempio di pianificazione del percorso per

un veicolo con rimorchio.

Figura 15: Pianificazione del percorso e aggiramento

ostacoli imprevisti.

Se invece è richiesto che il robot segua un dato percorso (ammissibile, libero da ostacoli,

ottimizzato per il minimo tempo di percorrenza, minima accelerazione angolare etc.) è necessario seguire una delle seguenti strategie:

A. Inseguire il percorso o la traiettoria cartesiana con algoritmi euristici (vedi §3.5.4) B. Pianificare il moto con metodi teorico-matematici (vedi §3.4) calcolando la legge di

moto desiderata

19

C. Integrare il metodo B. con un’algoritmo di controllo retroazionato sulla legge di moto pianificata per tenere conto dei possibili errori di inseguimento e ridurli (vedi §3.5.2).

3.4. Metodi di pianificazione del moto per sistemi non olonomi

3.4.1. Sistemi in “chained form”. Esistono forme canoniche per i modelli cinematici che sono funzionali allo sviluppo di strategie di controllo sia a ciclo aperto che retroazionato. La forma canonica più utilizzata è la forma a catena o “chained form”. Un sistema a catena è nella seguente forma:

1

2 2 1

3 3 1 2

1 2 1

( )( , )

( , ,.., )p p p

x ux f x ux f x x u

x f x x x u−

⎧ =⎪ =⎪⎪ =⎨⎪⎪

=⎪⎩

Dove sono le variabili di configurazione del sistema e imix ∈ i im nΣ = , la somma delle loro

dimensioni è pari al numero di gradi di libertà. Per un sistema di controllo con due input la forma canonica è detta a forma a catena singola (2, ): n

1 1

2 2

3 2 1

1 1n n

x ux ux x u

x x u−

=⎧⎪ =⎪⎪ =⎨⎪⎪

=⎪⎩

( 3-3)

Dove si è supposto che tutte le ix siano grandezze scalari. L’input è chiamato input generatore e le variabili

1u

1x e 2x sono chiamate variabili di base. Si nota che se è costante (o costante a tratti), il sistema è lineare (o lineare a tratti). Si dimostra che i sistemi nella forma (2, n ) a catena sono completamente controllabili. Essi sono caratterizzati da una particolare proprietà detta nilpotenza [14].

1u

Esistono condizioni necessarie e sufficienti perché un sistema a due input sia trasformabile in chained form tramite:

1. un cambio di coordinate ( )x qφ= 2. una trasformazione invertibile degli input ( )uv qβ= .

Si dimostra che un sistema non olonomo con 2m = input e 3,4n = coordinate generalizzate può sempre essere messo nella forma a catena. Inoltre i modelli dei veicoli con N rimorchi, se con ciascun rimorchio incernierato nel punto medio dell’asse posteriore del precedente, possono essere trasformati in chained form. Nel caso in cui vi sia un offset nella cerniera, solo modelli i veicoli con un numero di rimorchi minore di due possono essere trasformati in chained form. Per esempio, si consideri il modello cinematico (2-3) del monociclo. Utilizzando la trasformazione di coordinate

20

1

2

3

cos sinsin cos

xx x yx x y

δδ δδ δ

= −= += − +

e la trasformazione di input

1 3 1 2 1 2

2 1

( sin cos )v x u u x y u uv u

δ δ= + = − + += −

è immediato verificare, ricordando che 1 cosx v δ= e 1 siny v δ= che il sistema trasformato risulta nella forma canonica a catena. E’ interessante notare inoltre che le nuove variabili 2x e 3x sono semplicemente le coordinate cartesiane del monociclo espresse nel sistema di coordinate solidale al robot e orientato con l’asse 2x allineato con l’asse rispetto a cui è misurato l’assetto del veicolo. Per quanto riguarda il modello triciclo o, equivalentemente, automobile, si può utilizzare il seguente cambio di coordinate:

1

2 3

3

4

tancos

tan

x x

xb

xx y

αδ

δ

=

=

==

( 3-4)

e la trasformazione di input

11

2 312 22

cos

3sin sin cos coscos

uv

uv bb

δ2 uδ α δ

δ

=

= − + α

1

1

( 3-5)

per cui il sistema assume la forma a catena

1 1

2 2

3 2

4 3

x ux ux x ux x u

=⎧⎪ =⎪⎨ =⎪⎪ =⎩

( 3-6)

In questo caso la trasformazione di coordinate, e quindi la forma a catena, è definita per / 2 , .k kδ π π≠ ± ∈

Si verifica che, come prevedibile, anche il sistema a catena singola è non olonomo ma in questo caso si possono utilizzare procedure opportune per ottenere classi di soluzioni al problema della pianificazione del moto punto-punto, ovvero traiettorie (leggi di moto) che guidano il robot da una configurazione di partenza a una configurazione di arrivo assegnate. E’ possibile utilizzare differenti tipi di ingressi ( ): ( )u t

• ingressi sinusoidali • ingressi costanti a tratti

21

• ingressi polinomiali

Ingressi sinusoidali

Scegliendo per i due input

1 0 1

2 0 1 2 2

sin( )cos( ) cos(2 ) ... cos[( 2) ]n

u a a tu b b t b t b n t

ωω ω ω−

= += + + + + −

( 3-7)

si generano incognite per variare coordinate generalizzate da una data condizione iniziale a una data condizione finale in un dato tempo T . Integrando (a partire dalla prima equazione a cascata fino all’ultima) le equazioni differenziali del modello (3-3) in cui si sostituiscono gli ingressi nella forma sinusoidale (3-7), si ottiene un sistema lineare che ha come incognite gli

1n + n

1n + coefficienti. Si dimostra [27] che, se 1 0a ≠ , il sistema è invertibile. Nel caso specifico del modello (3-6) si hanno quattro coordinate e cinque parametri ( ). Una possibile procedura risolutiva [18] è la seguente 0 1 0 1 2, , , ,a a b b b

- si calcolano le coordinate trasformate iniziali e finali - si integrano le equazioni in cascata a partire dalla prima nell’intervallo di tempo 2 /T π ω= - si sceglie un valore di 1a - si risolve il sistema calcolando gli altri quattro coefficienti ( ). 0 0 1 2, , ,a b b b

Al variare del tempo imposto T e del parametro , varierà la geometria del percorso pianificato. In

1a

0

Figura 16 è riportato un esempio di applicazione (utilizzando il codice di calcolo simbolico Mathematica®) con due diversi valori di . Inoltre la geometria del percorso non è invariante alla rotazione, ossia dipende dall’assetto iniziale e finale

1aδ e fδ e non solo dalla differenza 0 fδ δ− .

start

goal

start

goal

goal

start

goal

start

Figura 16: Pianificazione del moto con le stesse condizioni inziali e finali ma con diversi valori di 1a

Ingressi costanti a tratti

Si suddivide il tempo totale di moto T in sottointervalli di lunghezza in ciascuno dei quali i due input sono costanti

Δ

1 1

2 2

( )( )

k

k

u uu u

ττ

==

con [( 1) , ]k kτ ∈ − Δ Δ

22

Se si mantiene sempre costante e si prendono 1u 1n − sottointervalli ( -1) in modo che 1,..,k = n

( 1)n T− Δ =

e

1 011

fx xu

T−

=

dove è il numero di coordinate generalizzate, n 1fx e 01x sono rispettivamente il valore finale e iniziale della variabile 1x , gli valore costanti (1n − 2,1 2,u u ,2 2, 1,.., nu − ) dell’input si ottengono risolvendo un sistema lineare triangolare che consegue dall’integrazione delle equazioni del modello a catena. Nel caso in cui le ascisse del punto di partenza e di arrivo coincidono è necessario aggiungere un punto intermedio, inoltre non è possibile effettuare manovre automatiche con inversione della velocità.

2u

1 2 3 4x @mD

1

2

3

4

y @mD Traiettoria

goal

start

1 2 3 4x @mD

1

2

3

4

y @mD Traiettoria

goal

start

Figura 17: Traiettoria ottenuta con input costanti a tratti

2 4 6 8 10secondi

0.4

0.5

0.6

0.7

0.8

0.9

mês Velocità motoruota vs Tempo

Figura 18: Input di velocità della motoruota

2 4 6 8 10secondi

-0.6

-0.4

-0.2

0.2

0.4

0.6

rad Angolo di sterzo vs Tempo

Figura 19: Comando angolo di sterzata. Presenta

discontinuità di velocità (tangente al grafico)

In Figura 18 e in Figura 19 risulta evidente come gli input di velocità di trazione e di angolo di sterzata siano delle funzioni continue ma le condizioni iniziali e finali su v e α sono libere in quanto non vengono imposte nelle equazioni risolutive. Inoltre la velocità di sterzata (derivata del grafico di Figura 19) può presentare delle discontinuità.

23

Ingressi polinomiali

Un’algoritmo simile a quello degli input costanti a tratti, ma con migliori prestazioni nel livello di continuità (“smoothness”) delle funzioni di input, è quello di utilizzare dei polinomi negli input. Scegliendo per i due input:

1 1 01

22 0 1 2

( )

..f

nn

u sign x x

u c c t c t −−

= −

= + + +

Come tempo totale si può scegliere

1 0fT x x= − 1

Si otterrà un sistema lineare nelle incognite ( 0 1 2, ,.., nc c c − ) ottenuto integrando le equazioni del modello a catena, del tipo

20

31

2

( ) ( , )f

fi

n f n

ucucA T f u T

c u−

⎛ ⎞⎛ ⎞⎜ ⎟⎜ ⎟ + = ⎜ ⎟⎜ ⎟

⎜ ⎟ ⎜ ⎟⎝ ⎠ ⎝ ⎠

Dove la matrice è invertibile per ( )A T 0T ≠ Nel caso in cui le ascisse del punto di partenza e di arrivo coincidono è necessario aggiungere un punto intermedio, inoltre non è possibile effettuare manovre automatiche con inversione di velocità.

3.4.2. Pianificazione di traiettorie open-loop I metodi finora visti consentono di calcolare un percorso ammissibile e i relativi controlli per

portare un veicolo da una configurazione di partenza a una di arrivo (moto punto-punto). Per contro, non è possibile prevedere esattamente quale sarà il percorso pianificato e ciò rende ancora più complesso il compito di far muovere il robot in un ambiente denso di ostacoli. E’ pertanto preferibile, se l’ambiente è strutturato, pianificare un percorso libero da ostacoli e che sia anche ammissibile, non solo per quanto riguarda i vincoli cinematici ma anche per ciò che concerne i limiti sui controlli (angolo massimo di sterzata, velocità massima di trazione e di sterzata, accelerazioni etc.).

Per il modello automobile a trazione posteriore (2-11) è possibile calcolare gli input di velocità di trazione e di sterzata (traiettoria di input) in funzione del percorso cartesiano desiderato, assumendo che questa sia una traiettoria ammissibile e con un certo grado di continuità [5]. E’ sufficiente imporre le coordinate cartesiane desiderate ( )( ), ( )d dx t y t in funzione del tempo a partire da un certo istante iniziale . Dalle coordinate cartesiane è possibile ricavare le restanti variabili di configurazione desiderate (assetto e angolo di sterzata) e i comandi di input associati. Ciò è possibile perché a causa dei vincoli non olonomi il veicolo è obbligato ad avere velocità ed assetto tangenti alla traiettoria cartesiana e inoltre l’angolo di sterzo è legato geometricamente alla curvatura del percorso e la velocità di sterzata è legata all’accelerazione angolare desiderata. Tutte queste variabili possono essere ottenute matematicamente da considerazioni geometriche e dalle derivate rispetto al tempo della traiettoria cartesiana.

0t

La traiettoria cartesiana desiderata (percorso geometrico in funzione del tempo) è ammissibile quando può essere ottenuta come risultato dell’evoluzione nel tempo della configurazione del sistema secondo le equazioni del modello cinematico dell’automobile:

24

1cosd d dx vδ= ( 3-8)

y 1d dsin dvδ= ( 3-9)

1d dd b

tan vαδ = ( 3-10)

2d dvα = ( 3-11)

con input e almeno continui a tratti. 1( )dv t 2 ( )dv tSi può ricavare dalle (3-8) e (3-9) 1dv

1( ) ( ) ( )d dv t x t y t= ± + d

il cui segno dipende dalla scelta della direzione del moto, in avanti o all’indietro. Dividendo la (3-9) per la (3-8) si ottiene l’assetto desiderato

1 1

( ) ( )( ) 2 ,( ) ( )

d dd

d d

x t y tt ATANv t v t

δ⎛ ⎞

= ⎜⎝ ⎠

⎟ ( 3-12)

Derivando la (3-8) e la (3-9) rispetto al tempo e moltiplicando la prima per dy , la seconda per dx e sottraendo membro a membro si ottiene

21

( ) ( ) ( ) ( )( )( )d

d

x t y t x t y ttv t

δ −= ( 3-13)

Il risultato appena ottenuto si può sostituire nella (3-10) e ottenere così l’angolo di sterzo il cui segno dipende dal segno della velocità di trazione

31

( ) ( ) ( ) ( )( ) arctan( )d

d

x t y t x t y tt bv t

α⎛ −

= ⎜⎝ ⎠

⎞⎟ ( 3-14)

Infine per ottenere l’input è sufficiente derivare rispetto al tempo l’angolo di sterzata appena ottenuto:

2dv

21 1

2 6 2 21

[( ) 3( )( )]( )( )

d d d d d d d d d d d d d dd

d d d d d

bv y x x y v y x x y x x y yv tv b y x x y

− − − +=

+ −

Le equazioni appena ricavate legano biunivocamente la traiettoria cartesiana alla traiettoria di input, ossia permettono di ottenere i controlli in funzione del tempo a ciclo aperto in seguito ai quali il robot segue il percorso desiderato con la legge temporale imposta. Il controllo a ciclo aperto richiede che il robot si trovi esattamente nella configurazione iniziale prevista ( 0 0 0( ), ( ), ( ), ( )d d d d 0x t y t t tδ α ) perché la traiettoria in uscita dal sistema di controllo sia effettivamente quella desiderata. Dalle equazioni risulta che gli input dipendono solo dalla traiettoria cartesiana desiderata in uscita e dalle sue derivate fino al terzo ordine. Perciò per garantire l’esatta riproducibilità della traiettoria desiderata è necessario, oltre alla condizione suddetta sulla configurazione iniziale, che la traiettoria sia derivabile tre volte rispetto al tempo.

25

E’ importante tenere conto che gli input contengono una singolarità, ossia non sono definiti nel caso in cui 1( ) 0dv t = per qualche 0t t≥ . Per questo inconveniente si utilizzano traiettorie cartesiane parametrizzate in funzione di un parametro geometrico del percorso, in tal modo la descrizione geometrica del percorso è separata da quella temporale. Se si indica con σ il parametro di percorso (quello più utilizzato è la coordinata curvilinea ), la pianificazione della traiettoria cartesiana implicherà una legge temporale

s( )tσ che si può introdurre nell’equazione della traiettoria

esprimendo le coordinate cartesiane in funzione di σ :

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

( ) ( ) ( )

d d d d

d d

d dx t x t x t t xdt d

y t y t

tσ σ σ σ σσ

σ σ

′= = =

′= ( 3-15)

(si utilizza l’apice per distinguere la derivata rispetto al parametro di percorso da quella rispetto al tempo). La velocità lineare di input della traiettoria nelle coordinate parametrizzate è detta pseudo-velocità e in dato punto corrispondente a un dato σ vale

2 21( ) ( ) ( )d d dw x yσ σ′ ′= ± + σ ( 3-16)

mentre la velocità di comando effettiva (che si ricorda essere la velocità lineare del punto di riferimento del modello automobile, ossia il punto intermedio dell’asse posteriore) vale

1 1( ) ( ( ) ( )d dv t w t tσ σ= ( 3-17)

s

1

1

( ) 1

( ) ( )

w s

v t s t

=

=

x

y

1v

s

1

1

( ) 1

( ) ( )

w s

v t s t

=

=

x

y

1v

Figura 20: Velocità di trazione in funzione della coordinata curvilinea. La pseudo velocità è il modulo del versore

tangente alla traiettoria, perciò sempre unitaria.

Se si sceglie ( ) ( )t s tσ = , le derivate ( ) ( ) /x s dx s ds′ = e ( ) /y dy s ds′ = sono le componenti cartesiane del versore tangente alla traiettoria nel punto corrispondente a e perciò la pseudo-velocità, che è il modulo di questo versore, avrà valore unitario, quindi sempre diverso da zero.

s

La condizione per cui 1( ) 0dv t = si ottiene quando ( ) 0tσ = mentre 1( ) 0dw σ ≠ . L’espressione dell’assetto desiderato in funzione del parametro di percorso è sempre definita senza punti singolari:

1 1

( ) ( )( ) 2 ,( ) ( )

d dd

d d

x yATANw w

σ σδ σσ σ

⎛ ⎞= ⎜ ⎟

⎝ ⎠

26

Grazie alla separazione tra dipendenza dal tempo e dipendenza dallo spazio anche le formule di ( )dα σ e 2 ( )dw σ (con 2 2( ) ( ) ( )d dv t w tσ σ= ) risultano senza singolarità poiché compaiono solo

informazioni geometiche relative alla curvatura del percorso e alle derivate di ordine superiore rispetto al parametro σ . Infatti

31

( ) ( ) ( ) ( )( ) arctan( )d

d

x y x ybw

σ σ σ σα σσ

⎛ ⎞′ ′′ ′′ ′−= ⎜ ⎟

⎝ ⎠

2′′′ ′ ′′′ ′ ′′ ′′ ′ ′ ′′ ′ ′′

1

1

d

d

1 12 6 2 2

1

[( ) 3( ' )( )]( )( )

d d d d d d d d d d d d d dd

d d d d d

bw y x x y w y x x y x x y yww b y x x y

σ − − − +=

′′ ′ ′′ ′+ −

L’espressione degli input in funzione della traiettoria cartesiana del modello di robot automobile a trazione posteriore (per quella anteriore la trasformazione di input è immediata ed è funzione solo dell’angolo di sterzata) si può ottenere anche per il modello a catena (2,4). L’evoluzione temporale della traiettoria desiderata è descritta dal modello cinematico in chained form

1 1

2 2

3 2

4 3

d d

d d

d d

d d

x ux ux x ux x u

=⎧⎪ =⎪⎨ =⎪⎪ =⎩

( 3-18)

Dalla traiettoria desiderata in coordinate cartesiane ( )( ), ( )d dx t y t e dal cambio di coordinate (3-4) è possibile ottenere la traiettoria desiderate nelle variabili trasformate in funzione di quelle cartesiane e delle loro derivate

1

2 3

3

4

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

( )( )( )( )

( ) ( )

d d

d d dd

d

dd

d

d d

x t x ty t x t y t x tx t

x ty tx tx t

x t y t

=−

=

=

=

e anche l’espressione degli input trasformati

12 2

2 4

( ) ( )

3( )( )

d d

d d d d d d d d d dd

d

u t x t

y x x y x y x x x yu tx

=

− − −=

3.4.3. Percorsi a curvatura continua. Clotoidi. E’ noto, a partire dagli studi di Dubin e Reeds & Shepp, che i percorsi ottimi in termini di

lunghezza, per un modello automobile semplificato (tre variabili di configurazione controllato in velocità angolare) sono costituiti da segmenti e archi di circonferenza. Uno svantaggio notevole di questa classe di percorsi è la presenza di discontinuità della curvatura. Queste discontinuità si presentano in ogni transizione tra segmenti e archi in quanto la curvatura salta da zero a un valore non nullo. Poiché la curvatura è legata all’angolo di sterzata dalla relazione

27

tanb

ακ =

se un veicolo effettuasse un inseguimento di percorso, per eseguire esattamente senza errori il percorso pianificato dovrebbe arrestarsi ad ogni discontinuità di curvatura per poter orientare le ruote anteriori (la ruota anteriore nel caso di modello triciclo). E’ preferibile, perciò, che il percorso sia caratterizzato dalla continuità nella curvatura. E’ poi necessario, a causa dei limiti meccanici sull’angolo di sterzata, che la curvatura sia limitata. Inoltre, poiché l’accelerazione angolare, ossia la velocità di variazione della curvatura, è legata alla velocità di sterzata del veicolo, è preferibile che la derivata della curvatura sia limitata superiormente per assicurare che il veicolo esegua la traiettoria con una certa velocità di trazione (proporzionale al limite sulla derivata della curvatura. Il problema di pianificare percorsi a curvatura continua fa parte della classe dei problemi detti di “path smoothing”, in cui si cerca di “smussare” le discontinuità a partire da un percorso nominale, ad esempio una linea poligonale, o da una sequenza di configurazioni di passaggio per ottenere una curva smooth. Le curve utilizzate si possono dividere in due categorie:.

1. con coordinate aventi un’espressione in forma chiusa, ad esempio: a. B-splines b. Polinomi di quinto grado c. Spline polari

2. curve parametriche in cui la curvatura è funzione della lunghezza d’arco (coordinata curvilinea s ):

d. Clotoidi e. Spirali cubiche f. Spline G2 g. Spline intrinseche

Curve parametriche dalle caratteristiche interessanti sono i clotodi. Essi derivano dalla scelta di una classe di percorsi in cui la curvatura varia linearmente con la lunghezza d’arco. Se si impone che la curvatura segua la legge lineare,

(0)d sdsδκ λ κ= = + ( 3-19)

con λ costante e la curvatura iniziale, per il percorso cartesiano si ottiene (0) cκ =

20 0 0

0 0

20 0

0 0

20 0

0 0

1( ) ( ) ( )2

1( ) cos ( ) cos2

1( ) sin ( ) sin2

s s

s s

s s

s d c d s

x s x d x c d

y s y d y c d

δ δ κ ξ ξ δ λξ ξ δ λ

δ ξ ξ λξ ξ ξ

δ ξ ξ λξ ξ ξ

= + = + + = + +

⎛ ⎞= + = + +⎜ ⎟⎝ ⎠

⎛ ⎞= + = + +⎜ ⎟⎝ ⎠

∫ ∫

∫ ∫

∫ ∫

cs

( 3-20)

Supponendo nulle le condizioni iniziali, per conoscere la traiettoria cartesiana è necessario calcolare gli integrali

28

2

0

2

0

1( ) cos2

1( ) sin2

s

s

x s d

y s d

λξ ξ

λξ ξ

⎛ ⎞= ⎜ ⎟⎝ ⎠

⎛ ⎞= ⎜ ⎟⎝ ⎠

∫

∫

( )y g xQuesto tipo di integrali non ha soluzione in forma chiusa ( = ), essi sono noti come integrali di Fresnel. La forma canonica rispettivamente del coseno e del seno di Fresnel è

2( ) cosx

CF x t dtπ⎛ ⎞= ∫0

2

0

2

( ) sin2

x

SF x t dtπ

⎜ ⎟⎝ ⎠

⎛ ⎞= ⎜ ⎟⎝ ⎠∫

( 3-21)

-0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.8

0.6

Clotoide in forma canonica

x

y

S > 0

-0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.8

0.6

Clotoide in forma canonica

x

y

S > 0

Figura 21: Grafico del clotoide in forma canonica, con

parametro negativo (terzo quadrante) e positivo (primo quadrante). Il grafico si ottiene da una variazione

lineare della curvatura in s.

Figura 22: Il grafico parametrico nello spazio (x,y,s) è noto come spirale di Cornu e la sua proiezione nel

piano (x,y) genera un’arco di clotoide.

Si noti che il parametro costante λ è l’accelerazione angolare espressa in termini geometrici, ossia è la derivata della curvatura rispetto alla lunghezza d’arco. Gli integrali di Fresnel possono essere calcolati tramite approssimazione con sviluppo in serie di potenze oppure con metodi numerici. Come applicazione della pianificazione del percorso tramite clotoidi si consideri il modello (3-1). Il sistema ha due controlli: la velocità di trazione delle ruote posteriori, v ; l’accelerazione angolare γ che è direttamente legata alla velocità di sterzata α . Se si assume velocità costante vale la seguente relazione

v

2

2

tan( )cos

d dt v vdt dt b b

δ αγ κ 2

vαα

⎛ ⎞= = = =⎜ ⎟⎝ ⎠

( 3-22)

Mentre in funzione della coordinata curvilinea:

29

2

2( ) ( )d ds sds ds

δ κγ κ ′= = =

Si introduce un limite superiore alla curvatura e alla sua derivata

max max;κ κ γ γ≤ ≤

Il limite sulla curvatura è legato al limite meccanico sull’angolo di sterzata, maxα , tramite la relazione

maxmax

tanbακ =

mentre il limite sull’accelerazione angolare è legato al limite sulla velocità di sterzata e deve essere scalato secondo il modulo della velocità v , infatti

max max2

maxcosv bγ α

α=

È possibile calcolare un valore di maxγ valido per 1v = e, se si sceglie un diverso valore 1v′ ≠ , è

opportuno (necessario se 1v′ > ) imporre un nuovo valore max max' vγ γ ′= / . In tal modo, imponendo un valore massimo per l’accelerazione angolare, resta rispettato anche il vincolo sulla velocità massima di sterzata, poiché / cosα α α≤ , per qualsiasi valore di α . Se si impone alla curvatura l’andamento lineare descritto dalla (3-19) valgono le relazioni:

2 22 2

1cos cos

dk vkv v vds b v b

α αγ λ λα α

= = = = ⇒ =

Da cui è possibile ricavare il valore massimo del coefficiente di curvatura, esso indica quanto rapidamente la curvatura del clotoide aumenta all’aumentare della lunghezza d’arco s

maxmax 2v

γλ =

I clotoidi permettono di gestire direttamente la curvatura della traiettoria cartesiana e si prestano ad essere uniti con continuità geometrica ad archi di circonferenza e segmenti rettilinei.

30

C

P1

P3

P2

βμ

μ

C

Δ

Pi

Pf

y

x

θfI

II

III

Pj

Pk

Δ

C

P1

P3

P2

βμ

μ

C

Δ

Pi

Pf

y

x

θfI

II

III

Pj

Pk

Δ

Figura 23: Elementi costruttivi della traiettoria a curvatura continua. In rosso (linea continua) la curva, in verde (tratto-punto) la circonferenza di riferimento della curva. I e III sono i tratti di clotoide, II è l’eventuale arco di

circonferenza.

Si descrive un metodo [8] per costruire un percorso a curvatura continua costituito da segmenti, archi di clotoide e archi di circonferenza (Figura 23).

Si supponga che il robot debba effettuare un cambio di orientazione passando dal segmento P1P2 al segmento P2P3 che forma con il primo un certo angolo fθ . Quest’angolo è anche la variazione complessiva di assetto del robot. La curvatura iniziale e quella finale sono entrambe nulle poiché la curva è preceduta e seguita da due segmenti. Se si raccordassero i segmenti solo con un arco di circonferenza (che abbia ) la curvatura avrebbe due salti discontinui passando da zero (sul segmento iniziale P1P2) a un valore non nullo costante (su tutto l’arco di circonferenza) e poi nuovamente a zero (sul segmento finale P2P3), ciò implicherebbe una variazione istantanea dell’angolo di sterzo non fattibile a meno che il veicolo non si fermi nei punti di discontinuità. Perciò si introducono due tratti di clotoide necessari a portare la curvatura, in modo lineare con la lunghezza d’arco s, dal valore nullo al valore massimo (primo clotoide) e poi, dopo un eventuale arco circolare, dal valore massimo al valore nullo (secondo clotoide). Un andamento caratteristico della curvatura è riportato in

maxk k≤

Figura 25.

31

0 0.5 1 1.5 2 2.5-0.5

0

0.5

1

1.5

2Planned Continuous Curvature path [m]

x[m]

y[m

]

Figura 24: Curva di raccordo a curvatura continua. Percorso simulato

3 4 5 6 7 8 90

0.2

0.4

0.6

0.8

1

Curvature [m-1]

plannedsimulated

s[m]

I

II

III

3 4 5 6 7 8 90

0.2

0.4

0.6

0.8

1

Curvature [m-1]

plannedsimulated

s[m]3 4 5 6 7 8 9

0

0.2

0.4

0.6

0.8

1

Curvature [m-1]

plannedsimulated

s[m]

I

II

III

Figura 25:Andamento della curvatura del percorso in

in funzione della lunghezza d’arco. Confronto tra pianificazione e simulazione

Gli archi circolari hanno raggio mentre i clotoidi hanno coefficiente di curvatura 1maxk −

maxλ λ≤ . La curva comincia dal punto Pi, a cui corrispondono le variabili di configurazione ( , , ,0s s sx y δ ), e termina nel punto Pf, a cui corrispondono le variabili di configurazione ( , ,f fx y ,0i fδ θ+ ). Senza perdere in generalità è conveniente utilizzare un sistema di riferimento relativo in modo tale che

e assumere che la curva sia percorsa in senso antiorario. (0,0,0,0)Pi ≡Il primo tratto della curva consta di un arco di clotoide con coefficiente di curvatura maxλ e di

lunghezza max max/k λ , esso consente di passare da curvatura nulla, in Pi, a curvatura k , fino a raggiungere Pj. Le variabili di configurazione di Pj sono

max

2max max max

2max max max

2max max

max

/ ( /(

/ ( /(

/(2 )

j

j

j

j

x CF k

y SF kPjk

k

π λ πλ

π λ πλ

δ λ

κ

⎧ =⎪⎪ =⎪= ⎨⎪ =⎪

=⎪⎩

))

))

1max−

Le prime due equazioni si ottengono da un semplice cambio di variabile negli integrali canonici di Fresnel (3-21). Il secondo tratto della curva consta di un arco circolare di raggio k che termina nella configurazione Pk = ( max, , ,k k kx y kδ ). Il centro C della circonferenza di cui l’arco fa parte si trova a distanza da Pk in direzione ortogonale a 1

maxk −kδ , ed ha coordinate

1max

1max

sin

cosC j j

C j

x x k

y y k j

δ

δ

−

−

⎧ = −⎪⎨

= +⎪⎩

Il terzo e ultimo tratto della curva consta di un arco di clotoide con coefficiente di curvatura maxλ− e di lunghezza pari a quella del primo tratto, esso consente di passare da curvatura , in

Pk, a curvatura nulla, fino a raggiungere la configurazione finale Pf. maxk

32

Mentre gli archi di clotoide hanno lunghezza costante poiché sempre caratterizzati da una variazione di curvatura con coefficiente di curvatura maxk maxλ , l’arco di circonferenza può variare la sua lunghezza ed è esso che determina la posizione finale di tutta la curva. La sua lunghezza si riduce a zero quando la deflessione totale fθ è pari a due volte la variazione di assetto che si ottiene con ciascuno dei due tratti di clotoide e cioè quando

2max

minmax

fkθ θλ

= =

Si osserva che il luogo delle configurazioni raggiungibili è una circonferenza con raggio

2 2C Cr x y= +

e che l’angolo tra la direzione di assetto della configurazione finale Pf e la tangente alla circonferenza in Pf è costante e vale

arctan( )C

C

xy

μ =

Se è richiesto di raggiungere una posizione finale con assetto minfδ θ< , la curva sarà priva di un arco di circonferenza e consisterà in due clotoidi con coefficiente di curvatura calcolato in modo da ottenere la variazione di assetto desiderata.

Questo metodo può essere utilizzato, congiuntamente ad altri opportuni algoritmi, per la pianificazione locale del percorso e consente di generare percorsi ammissibili a curvatura continua tenendo conto dei vincoli sul massimo angolo di sterzata e sulla massima accelerazione angolare desiderata. La caratteristica di curvatura continua dimostra i seguenti vantaggi:

• il rispetto dei vincoli non olonomi e sulla velocità di sterzata massima consente di minimizzare eventuali slittamenti delle ruote ottenendo così una maggiore accuratezza per la misura con encoder

• la continuità sulla curvatura e il limite superiore sull’accelerazione angolare migliorano e rendono più stabili le prestazioni di un eventuale sensore laser a triangolazione, la cui qualità della misura decade in presenza di brusche variazioni di assetto.

3.4.4. Percorsi a curvatura polinomiale

Figura 26: Spirale polinomiale ( o spirale di Cornu generalizzata) nello spazio (x,y,s). La sua proiezione nel piano

(x,y) rappresenta il percorso cartesiano. In questo esempio la curvatura è un polinomio di terzo grado in s. La curvatura cambia di segno a circa metà percorso, il quale involve verso l’origine.

33

Nell’arco di clotoide la curvatura varia linearmente con la lunghezza d’arco s

( )k s a bs= +

Si faccia riferimento al modello (3-2) senza la quarta equazione, in cui si utilizza come input la curvatura in funzione della lunghezza d’arco. Questo modello ha il vantaggio di non dover considerare la dipendenza dal tempo. Per connettere due configurazioni, iniziale e finale:

0 0 0 0 0( , , , )( , , , )f f f f f

q x yq x y

δ κδ κ

==

se si considerano solo le variazioni relative di posizione (x, y) e assetto (in un sistema di riferimento con l’origine coincidente con il punto medio dell’asse posteriore e l’asse delle ascisse orientato come l’assetto del veicolo la configurazione iniziale è (0,0,0,κ0)) si devono imporre cinque vincoli, ossia 0[ , , , , ]f f f fx yκ δ κ . Con un arco di clotoide si possono imporre tre soli vincoli, ossia [ , , ]fa b s i due coefficienti dell’espressione lineare della curvatura e la lunghezza totale del percorso fs . Quindi con un arco di clotoide è possibile raggiungere la posizione finale desiderata partendo dalla curvatura iniziale imposta ma senza vincoli sull’assetto e sulla curvatura finali (o sulla posizione finale). Piuttosto che utilizzare metodi articolati come quello visto in precedenza nel §3.4.3, si può aumentare il grado del polinomio della curvatura [12], aumentando così il numero di coefficienti incogniti con la possibilità di rispettare un maggior numero di vincoli sulle condizioni al contorno. Per ottenere una classe di percorsi ammissibili a curvatura continua che connettono con iq fq è sufficiente utilizzare un polinomio di terzo grado

2 3( )k s a bs cs ds= + + +

Figura 27: La derivata seconda della curvatura rispetto a s mostra che il percorso in alto a sinistra è ottenuto da

una spirale polinomiale di terzo grado.

I percorsi ottenibili appartengono alla classe delle cosiddette spirali polinomiali. Il vettore dei parametri da calcolare sarà perciò

[ ]fp a bc d s=

Il coeffiente a si calcola semplicemente dalla curvatura iniziale: (0)a κ= . Per il resto dei coefficienti è necessario, a partire dalle condizioni al contorno, risolvere un sistema non lineare. Si

34

può disaccoppiare il sistema sostituendo l’espressione in forma chiusa dell’assetto (ottenuta analiticamente dalle prime due equazioni) negli integrali delle due equazioni di posizione x e y. Si ottiene il sistema

2 3

2 3 4

2 3 4

0

2 3 4

0

( ) ...

( ) ...2 3 4

( ) cos( ...)2 3 4

( ) sin( ...)2 3 4

s

s

k s a bs cs dsbs cs dss as

bs cs dsx s as

bs cs ds

ds

y s as

δ

⎧ = + + + +⎪⎪ = + + + +⎪⎪⎨ = + + + +⎪⎪⎪

= + + + +⎪⎩

∫

∫ ds

in cui gli integrali sono detti integrali di Fresnel generalizzati. Questi integrali sono trascendenti e il loro calcolo deve essere eseguito con metodi numerici. Un vantaggio delle spirali polinomiali è che consentono di ottenere il grado di continuità desiderato (ad esempio sulla continuità della velocità di sterzata ma anche sulla coppia di sterzo) a seconda del grado del polinomo. Inoltre semplificano la soluzione del sistema grazie al disaccoppiamento delle equazioni sulla curvatura e sull’assetto.

Nello scegliere il percorso ottimo tra quelli calcolati è possibile minimizzare una funzione di costo desiderata, un esempio può essere il funzionale

2

0

1( ) [ ( )]2

fs

kJ p p dsκ= ∫

3.5. Controllo di traiettoria La pianificazione dei controlli in open-loop non è robusta rispetto alle deviazioni del sistema

dalle condizioni ideali previste (disturbi sulla misura, errori sulle condizioni iniziali, in accuratezza del modello)

Il controllo retroazionato (feedback) sulla traiettoria pianificata migliora le prestazioni del sistema consentendo di portare a termine il task assegnato al robot anche in presenza di disturbi e di deviazioni dalle condizioni iniziali previste. Per stimare lo stato del sistema, ovvero la posa del robot, si utilizzano misure provenienti da vari sensori in real-time. Per poter calcolare l’input correttivo della legge di feedback è necessario disporre ad ogni istante di una stima della configurazione attuale del robot. A tale scopo, tipicamente si dispone di strumenti propriocettivi (encoders, giroscopio) ed eterocettivi (laser, ultrasuoni) le cui misure sono combinate assieme da un algoritmo di sensor fusion che rende ad ogni istante disponibile la miglior stima (secondo un dato criterio) della posa del robot. In una tipica architettura di controllo un pianificatore ad alto livello gestisce la pianificazione dei percorsi liberi da ostacoli e fornisce una serie di obiettivi al controllo di livello più basso. In tal modo il task di controllo della traiettoria si occupa di trasformare il percorso ideale in una legge di moto eseguibile dagli attuatori. Inoltre, se il percorso pianificato non è ammissibile rispetto ai vincoli non olonomi (discontinuità nella tangente, nella curvatura etc.), l’algoritmo di feedback consente di recuperare gli errori transitori dovuti a discontinuità geometriche. Dal punto di vista del controllo, la pianificazione è equivalente al controllo a ciclo aperto o feedforward, e costituisce una parte del controllo in feedback. Infatti un’algoritmo di controllo a ciclo chiuso è costituito dall’unione dell’azione di feedback a un termine di feedforward. Quest’ultimo è ottenuto dalla conoscenza a priori dell’ambiente e dell’obiettivo del task, mentre l’azione di feedback dipende dalla posizione attuale misurata del robot ed è calcolata in tempo reale

35

in base alle misure dei sensori di cui il robot è dotato. Comunque la separazione tra strategie di controllo a ciclo aperto e a ciclo chiuso non è così netta: è possibile infatti scegliere di ottenere un controllo in feedback tramite una ripianificazione in tempo reale del percorso sulla base della posizione attuale fornita dai sensori, ossia che tiene conto in tempo reale delle deviazioni dalle condizioni nominali, tendendo a ripristinarle Per i robot mobili tipicamente la fase di pianificazione termina con il calcolo di un percorso cinematicamente ammissibile e libero da ostacoli, a cui sono associati gli input di controllo a ciclo aperto. La ammissibilità è garantita se si tiene conto dei vincoli non olonomi del sistema. Inoltre è possibile , in fase di pianificazione, scegliere un percorso che soddisfi un criterio di ottimo insieme ai vincoli sui controlli. Un possibile criterio di ottimo è la minimizzazione di una funzione di costo, ad esempio la determinazione del percorso libero da ostacoli che rende minima la lunghezza del percorso o la accelerazione angolare massima o una combinazione delle due. Tra i vincoli sui controlli si deve tenere conto del massimo angolo di sterzata, della massima velocità lineare ed angolare. In ogni caso in questa fase i controlli sono calcolati off-line. Perciò se si verificano eventi imprevisti, come lo slittamento delle ruote o l’errata localizzazione iniziale, questi inficiano la realizzazione corretta del moto punto-punto o dell’inseguimento di percorso o di traiettoria La soluzione è quella di ricorrere a un controllo retroazionato sull’errore stimato ed ottenere così un certo grado di robustezza del controllo complessivo. Il controllo di feedback agisce solo nel caso in cui l’errore sia non nullo, coadiuvando l’azione dei controlli a ciclo aperto. Nel caso in cui l’errore sia nullo e il robot si trovi esattamente nella configurazione desiderata, agiranno solo i controlli a ciclo aperto. Un’altra possibile soluzione è quella di non utilizzare i controlli a ciclo aperto e demandare a una legge di feedback l’intero compito di controllo dell’inseguimento. In tal caso l’algoritmo deve essere tale da fornire i controlli necessari ad effettuare il task di movimento anche nel caso in cui l’errore sia nullo; si ha perciò un unico algoritmo in feedback rispetto alla posizione attuale e a quella desiderata, che funge sia da pianificatore che da controllore.

3.5.1. Controllabilità Si è visto che dall’imposizione dei vincoli non olonomi di non slittamento si arriva

direttamente alla costruzione di un modello del sistema che è non lineare, del tipo

1 1 2 2( ) ... m mq G q v g v g v g v= = + + + ( 3-23)

che rappresenta il modello cinematico del sistema e può essere utilizzato per lo studio del controllo. è il vettore delle variabili di configurazione, il vettore delle velocità di input, con m nq n v m < .

Le colonne della matrice G sono funzioni vettoriali continue delle variabili di configurazione. Il sistema (3-23) è detto senza deriva poiché

( . )ig i m= 1,.0 0v q= ⇒ = .

Se si sceglie una funzione vettoriale di controllo continua o continua a tratti e una condizione iniziale

( )v t

0(0)q q= , esiste un’unica soluzione del sistema per , . 0t ≥ 0( ,0, , )q t q vIl sistema è controllabile se ∀ issati, T v1 2,q q f t, ( )∃ < ∞ ∃ de alefinita nell’intervallo [0,T che

( ,q T] t

1 20, , )q v q= . Le caratteristiche dei sistemi non olonomi fanno si che il livello di complessità del compito sia inverso rispetto ai robot manipolatori. Infatti, per i manipolatori la stabilizzazione su una configurazione fissa è più semplice rispetto all’inseguimento di traiettoria; ciò vale anche per qualsiasi sistema meccanico in cui il numero dei gradi di libertà è uguale a quello dei controlli. Per un sistema non olonomo quale un robot su ruote, in cui è sempre (sottoattuato), la complessità del compito è invertita. Dal punto di vista qualitativo ciò appare evidente se si considera che il compito di controllo nel moto punto-punto è un problema di stabilizzazione attorno a una configurazione fissa e corrisponde a controllare g.d.l. con

m n<

n 2m = ingressi. Per il compito di

36

controllo nell’inseguimento di percorso il problema è quello di controllare una sola uscita (la distanza dal percorso) con ingressi (ad esempio l’angolo di sterzo, mantenendo costante l’altro input di velocità) e gli altri g.d.l. vengono controllati indirettamente. Analogamente all’inseguimento di percorso, anche nell’inseguimento di traiettoria il numero di ingressi,

1m =1n −

2m = , è uguale al numero di uscite controllate (gli errori cartesiani xe

e

ed ), e gli altri g.d.l. vengono controllati indirettamente. Perciò il problema più complesso è quello della stabilizzazione su una configurazione fissa, in cui il numero di ingressi è inferiore al numero di uscite controllate, mentre sia per l’inseguimento di percorso che per l’inseguimento di traiettoria il numero di ingressi è uguale a quello delle uscite, si tratta cioè di problemi “quadrati”.

ye 2n −

Se si effettua un’analisi di controllabilità [17] è possibile verificare se i problemi su citati ammettono una soluzione approssimata con tecniche di controllo lineare. Nei sistemi lineari la controllabilità implica la stabilizzabilità attorno a una configurazione di equilibrio con un feedback continuo lineare sullo stato, del tipo eq

( ) ( )ev q k q q= −

Se, inoltre, il sistema approssimato linearmente attorno a q

e

q J q B vq q qv k q

δ δ δδδ δ

= +⎧⎪ = −⎨⎪ =⎩

è controllabile, allora anche il sistema originario può essere stabilizzato localmente attorno a (o anche attorno a una traiettoria ) con un feedback continuo.

eq( )dq

dq

t

)

Esistono diversi classi di feedback applicabili: • Feedback continuo di stato, per sistemi lineari o linearizzati

( dv k q q= − , dove è la configurazione desiderata, anche variabile nel tempo • Feedback discontinuo

( )v f q= , dove f è una funzione continua a tratti • Feedback tempo-variante

( , )v f q t= , dove f è una funzione continua di q , ma dipendente dal tempo.

Nel caso dei sistemi non olonomi, se il sistema è linearizzato attorno a una configurazione fissa, il risultante sistema lineare non è controllabile. Ciò deriva direttamente dal

Teorema di Brockett: un sistema senza deriva con vettori linearmente indipendenti è asintoticamente stabilizzabile con un feedback continuo di stato se e solo se il numero di controlli è maggiore o uguale al numero degli stati (variabili di configurazione), .

ig

m n≥

m

Per contro, se si linearizza il sistema attorno a una traiettoria continua ne risulta un sistema lineare tempo-variante controllabile (poiché il numero di controlli è pari a quello delle variabili controllate) a condizione che la traiettoria soddisfi determinate condizioni sulla velocità di percorrenza (non sia mai nulla). Definendo v t la variazione degli input e come ( ) ( )v t= − ( )dv t ( ) ( ( )q t t) dq t q= − l’errore di inseguimento ( o di tracking), l’approssimazione del sistema (3-23) a un sistema lineare attorno alla traiettoria di riferimento si ottiene nel modo seguente

( ) ( )q J t q B t v= + ( 3-24)

37

dove la matrice ( ) e la matrice n n× J t n m× B(t) si ottengono come

( )1 ( )

( )

( ) ( ( ))d

mi

di ti q q t

d

gJ t vq

B t G q t= =

∂=

∂

=

∑ ( 3-25)

3.5.2. Feedback statico linearizzato Si descrive un metodo di controllo a ciclo chiuso per l’inseguimento di traiettoria ottenuto

dall’approssimazione lineare del modello automobile in forma a catena[5]. Si consideri il modello (3-6) a catena singola (2,4) e si supponga di aver ricavato la legge di

moto [ ] a ciclo aperto per descrivere la traiettoria [1 2( ), ( )d du t u t 1 2 3 4( ), ( ), ( ), ( )d d d dx t x t x t x t ]. Si indichino rispettivamente gli errori di inseguimento e le variazioni dei controlli come:

, 1,..,4, 1,2

i i di

j j dj

x x x iv v v j

= − == − =

Il sistema non lineare che descrive l’evoluzione dell’errore di inseguimento è:

1 1

2 2

3 2 1 2

4 3 1 3

d d

d d

x u

x u

1

1

x x u x u

x x u x u

=

=

= −

= −

Linearizzando il sistema attorno alla traiettoria di riferimento secondo le (3-25) si ottiene il sistema lineare con dipendenza dal tempo:

1 2

1 3

0 0 0 0 1 00 0 0 0 0 1 ( ) ( )0 ( ) 0 0 ( ) 00 0 ( ) 0 ( ) 0

d d

d d

x x u J t xu t x tu t x t

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

B t u+

Da un’analisi di controllabilità (qui omessa) si vede che il sistema linearizzato è controllabile e perciò il sistema originario può essere localmente stabilizzato attorno a una traiettoria di riferimento tramite un feedback lineare.

La matrice detta dell’anello chiuso, tale che ( )acA t ( )acx A t x= , indica come definire la legge di feedback u t in modo che gli autovalori di siano costanti o comunque con parte reale negativa. Se si definisce la legge di feedback

( ) ( )acA t

1 1 1

3 42 2 2 3 2

1 1d d

u k xk ku k x xu u

= −

= − − − 4x

La matrice dell’anello chiuso diventa:

38

12

2 3 1 4 1

1 2 1

1 3 1

0 0 00 / ( )

( ) ( ) 0 0( ) 0 ( ) 0

d dac

d d

d d

kk k u t k u tA k x t u t

k x t u t

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

/ ( )

e i suoi quattro autovalori sono e le tre radici del polinomio caratteristico 1k− 2 34 3 2k k kλ λ λ+ + + .

Pertanto, se e sono scelti in modo che il polinomio caratteristico sia un polinomio di Hurwitz, gli autovalori di sono costanti e con parte reale negativa, il che permette di ottenere convergenza a zero dell’errore di inseguimento di traiettoria.

1 0k > 2 3 4, ,k k k(ccA )t

L’espressione del controllo di feedback mostra che esso non è definito per 2u 1 0du = . Il problema può essere risolto se si assegna agli autovalori, anziché un valore costante, una funzione di . Ad esempio si possono calcolare i coefficienti del polinomio caratteristico in modo che abbia, oltre a , tre radici reali coincidenti e pari a

1du

1k− 1duγ− , con 0γ > . In tal modo si ottiene:

2 32 1 2 1 33 3d du u x u x uγ γ γ= − − − 1 4d x ( 3-26)

Con questo metodo (input scaling), il secondo controllo di feedback, anziché ad infinito, tende a zero quando la variabile 1dx della traiettoria desiderata tende ad arrestarsi ( ). 1 0du = Il controllo complessivo del sistema in forma a catena sarà dato da

du u u= +

cioè consiste nella somma di un termine calcolato per l’anello aperto, di feed-forward, e di un termine di anello chiuso, di feedback. Per poter calcolare i controlli effettivi del modello automobile si deve utilizzare la funzione di trasformazione (3-5) e perciò la velocità di trazione e la velocità di sterzata saranno delle leggi di feedback non lineari e tempo-varianti.

( )v t

3.5.3. Applicazione del feedback linearizzato al controllo di traiettoria rettilinea

Si riporta l’applicazione dell’algoritmo di controllo con feedback statico linearizzato all’inseguimento di traiettoria rettilinea con velocità . 1( )dv tDetti

- 1v l’input di velocità delle ruote posteriori - 2v l’input di velocità angolare di sterzata - dx e dy le coordinate cartesiane desiderate, in coordinate relative rispetto alla

traiettoria ( 0dy ≡ ) - α l’angolo di sterzo - δ l’assetto del veicolo in coordinate relative rispetto alla retta da inseguire

La traiettoria di riferimento in coordinate cartesiane relative è

( ) ( )( ) 0

d d

d

x t v ty t

=

=

t

Con input desiderati

39

1 1

2

( ) ( )( ) 0

d d

d

v t v tv t

=

=

La traiettoria in coordinate trasformate per un sistema a catena secondo le (3-4) e (3-5), tenendo presente che ( ) 0, ( ) 0d dt tα δ= = , è

1

2

3

4

( ) ( )( ) 0( ) 0( ) 0

d d

d

d

d

x t v tx tx tx t

=

===

t

Mentre gli input trasformati desiderati sono

1 1 1

2

( ) ( )( ) 0

d d d

d

u t x v tu t

= =

=

Secondo quanto visto nel paragrafo precedente la legge di controllo, utilizzando la (3-4) per esprimere le 2 3 4, ,x x x in funzione di ,x y , è:

1 1 1 1

2 32 3

( ) ( ) ( ) ( )

3 ( ) tan( ) 3 ( ) tan ( )

cos

d d d

dd d

u t u t u v t k x x

v tu t v t v t y

bγ α

γ δ γδ

= + = − −

= + +

Gli input effettivi complessivi, in base alla (3-5) sono perciò:

111

23 21

2 22

( ) ( )( )( )cos cos

3sin sin ( ) ( )( ) cos cos ( )cos

d dv t k x xu tv t

u tv t b u tb

δ δδ α δ α

δ

− −= =

−= +

Modificando il parametro γ la convergenza a zero dell’errore di inseguimento è più o meno rapida e la stabilità dell’algoritmo dipende dalla distanza delle condizioni iniziali effettive dalle condizioni nominali e dal valore di γ .

40

0 1 2 3 4 5 6 7 8-3

-2

-1

0

1

2

3

4

Feedback di traiettoria su percorso rettilineo

x[m]

y[m

]

Figura 28: Simulazione del controllo di traiettoria per l’allineamento su una retta. Esempio con condizioni

iniziali: y0 = 1m; δ0 = 15°; velocità = 0.4 m/s

0 5 10 15 20 25-40

-30

-20

-10

0

10

20Assetto Veicolo [deg]

time [s]

Figura 29: Andamento dell’assetto del veicolo durante la simulazione di Figura 30

3.5.4. Metodi euristici per l’inseguimento di percorso Per l’inseguimento di percorso o di traiettoria è possibile ricorrere ad algoritmi di controllo di

tipo euristico. In tal caso le prestazioni del controllo non sono ottime in senso teorico-matematico e vengono valutate con prove di simulazione o sperimentali. L’algoritmo viene validato tramite una serie di test che permettano di verificare il comportamento del sistema in termini di convergenza e stabilità in un dato set di condizioni nominali. A titolo esemplificativo, un semplice tipo di controllo per l’inseguimento di percorso consiste nel pianificare (e memorizzare in una matrice di dati) il percorso da far eseguire al veicolo attraverso una serie di punti di passaggio. Risulta evidente che la spezzata che unisce i punti di passaggio non è un percorso ammissibile per robot non olonomo la cui velocità sia sempre non nulla. Il percorso

41

effettivo che risulta dall’applicazione sarà perciò un percorso ammissibile che tende verso la spezzata di riferimento. Con riferimento alla Figura 30 a ciascun punto Pi (xi,yi,Ri) è associata una terna di valori le cui componenti sono: posizione x e y del punto rispetto ad un sistema di riferimento fisso nell’ambiente e R la distanza dal punto al di sotto della quale il riferimento passa al punto successivo. I riferimenti per il controllo sono: il punto già raggiunto, il prossimo punto da raggiungere e la loro retta congiungente, in modo che la traiettoria descritta sia simile alla spezzata che congiunge in modo sequenziale tutti i punti stabiliti. Se il veicolo viene controllato per inseguire il segmento Pk-1-Pk e la distanza dal punto scende al di sotto del corrispondente valore di Rk il controllo può passare al segmento successivo Pk-P k+1. In particolare, pensando di utilizzare un veicolo tipo triciclo, il controllo agisce in modo da allineare lo sterzo del veicolo secondo la direzione della retta orientata che congiunge l’asse dello sterzo con un opportuno punto situato sulla congiungente tra il punto Pk e il punto Pk+1.

P1

P2

Pk Pk+1

START

STOP

Rk Rk+1

R2

Passaggio al controllo su Pk+1

Figura 30: Simulazione di percorso con inseguimento

La Figura 31 schematizza i parametri geometrici che regolano il controllo di traiettoria. Pk è il punto da raggiungere imposto dal pianificatore, Pk-1 è il punto precedente che, connesso con il punto attuale, definisce la retta di riferimento.

42

eR

xy

O

PR

Pk-1

Pk

eP

α

eS

PS

PP

PSk

eR

xy

O xy

O

PR

Pk-1

Pk

eP

α

eS

PS

PP

PSk

Figura 31:descrizione dell’algoritmo euristico. Il setpoint di sterzo è scelto in modo da puntare lo sterzo verso un punto appartenente al segmento di riferimento corrente e dipendente dalla proiezione della posizione dello sterzo

su tale segmento.

L’angolo di sterzo viene controllato allineando lo sterzo secondo la direzione della retta che congiunge i seguenti due punti: il punto in cui si trova l’asse dello sterzo, PS, e un punto PP individuato proiettando sulla congiungente Pk-1Pk il punto PSk. Quest’ultimo viene scelto sulla congiungente PSPk e la sua posizione può variare da Pk a PS secondo il fattore K che scala la distanza tra PS e Pk:

sk k s kP P K P P= eR , δR versore della direzione di assetto del veicolo, angolo formato dal

versore rispetto al sistema di riferimento assoluto eP, δP versore in direzione del segmento PSPP, angolo formato dal

versore rispetto al sistema di riferimento assoluto eS versore in direzione del segmento Pk-1 – Pk

Tabella 1: parametri geometrici del controllo

La legge di controllo sul punto Pk ed il segmento Pk-1 – Pk determina il riferimento per lo sterzo, calcolato come segue: α = δP - δR In Figura 30 è rappresentata una traiettoria simulata ottenibile con questo tipo di controllo. La velocità di trazione è il secondo controllo e la sua legge di variazione può essere pianificata in modo indipendente.

4. BIBLIOGRAFIA [1] L. Baglivo, M. De Cecco, “Navigazione di veicoli autonomi. Sensor Fusion”, dispensa didattica. [2] J Boisonnat, A Cérézo, J Leblond, “Shortest Paths of Bounded Curvature in the Plane”, Proc ICRA, pp. 2315-

2320, Nice, France 1992 [3] R. W. Brockett, “Asymptotic stability and feedback stabilization", in Differential Geometric Control Theory, R.

W. Brockett, R. S. Millman, H. J. Sussmann (Eds.), Birkhäuser, Boston, MA, pp. 181-191, 1983.

43

44

[4] H Delingette, M. Herbert, and K Ikeuchi, "Trajectory Generation with Curvature Constraint based on Energy Minimization", Proc, IROS, pp 206-211, Osaka, Japan, 1991.

[5] A. De Luca, G. Oriolo, C. Samson, “Feedback control of a nonholonomic car.like robot”, in Robot Motion Planning and Control (J. P. Laumond Ed.), Springer-Verlag, 1998.

[6] L. E. Dubins, On Curves of Minimal Length with a Constraint on Average Curvature and with Prescribed Initial and Terminal Positions and Tangents. American Journal of Mathematics, 79:497-516,1957.

[7] M. Fliess, J. Lévine, P. Martin, and P. Rouchon, “Design of trajectory stabilizing feedback for driftless flat systems", 3rd European Control Conf., Roma, I,pp. 1882-1887, 1995.

[8] T. Fraichard and A. Scheuer, “From Reeds and Shepp’s to Continuous-Curvature Paths”, IEEE Trans. on Robotics, Vol. 20, No. 6, pp.1025-1035, 2004

[9] Y. Kanayama, N.Miyake, "Trajectory Generation for Mobile Robots", Robotics Research, MIT Press, Cambridge,1985.

[10] Y. Kanayama and B.I. Hartman, "Smooth Local Path Planning for Autonomous Vehicles" Technical Report, Dept. of Computer Science, University of California, Santa Barbara, 1988.

[11] H. Kano, M. Egerstedt, H. Nakata, and C.F. Martin. B-Splines and Control Theory. To appear in Applied Mathematics and Computation, 2003.

[12] Kelly and B. Nagy, “Reactive Nonholonomic Trajectory Generation via Parametric Optimal Control”, trovare pagine e rivista!!!

[13] G. Laferriere and H. J. Sussmann, “Motion planning for controllable systems without drift", 1991 IEEE Int. Conf. on Robotics and Automation, Sacramento, CA, pp. 1148-1153, 1991.

[14] G. Lafferriere and H.J. Sussmann, “A differential geometric approach to motion planning", Nonholonomic Motion Planning, Zexiang Li and J.F. Canny Eds, The Kluwer International Series in Engineering and Computer Science 192, 1992.

[15] J. C. Latombe, Robot Motion Planning, Kluwer Academic Publishers, 1991. [16] J. P. Laumond (editor) Robot Motion Planning and Control, LAAS report 97438 [17] J.-P. Laumond, “Controllability of a multibody mobile robot", IEEE Trans. on Robotics and Automation, vol. 9,

no. 6, pp. 755-763, 1993. [18] R. M. Murray and S. S. Sastry, “Nonholonomic motion planning: Steering using sinusoids", IEEE Trans. on

Automatic Control, vol. 38, no. 5, pp. 700-716, 1993. [19] R. M. Murray, “Control of nonholonomic systems using chained forms", Fields Institute Communications, vol. 1,

pp. 219-245, 1993. 3rd European Control Conf., Roma, I, pp. 2620-2625, 1995. [20] B. Nagy and A. Kelly, “Trajectory Generation for Car-Like Robots Using Cubic Curvature Polynomials”, in Field

and Service Robots, June 11, 2001, Helsinki, Finland. [21] J. A. Reeds and R. A. Shepp, Optimal paths for a car that goes both forwards and backwards”, Pacific Journal of

Mathematics, 145 (2), 1990. [22] Johannes Reuter, “Mobile Robot Trajectories With Continuously Differentiable Curvature: An Optimal Control

Approach”, Proceedings of the 1998 IEEE/RSJ Conference on Intelligent Robots and Systems, Victoria, B.C, Canada, October 1998.

[23] C. Samson and K. Ait-Abderrahim, “Feedback stabilization of a nonholonomic wheeled mobile robot”, 1991 IEEE/RSJ Int. Work. on Intelligent Robots and Systems, Osaka, J, pp. 1242-1247, 1991.

[24] D. H. Shin and S. Singh, "Path Generation for Robot Vehicles Using Composite Clothoid Segments", Technical Report, CMU-RITR- 90-31, The Robotics Institute, Carnegie Mellon University, 1990.

[25] O. J. Sordalen, “Conversion of the kinematics of a car with n trailers into a chained form", 1993 IEEE Int. Conf. on Robotics and Automation, Atlanta, GA, vol. 1, pp. 382-387, 1993.

[26] O. J. Sordalen, “Feedback control of nonholonomic mobile robots", Ph. D. Thesis, The Norwegian Institute of Technology, Trondheim, NO, Mar. 1993.

[27] D. Tilbury, J.P. Laumond, R. Murray, S. Sastry and G. Walsh, “Steering car-like systems with trailers using sinusoids" in IEEE Conf. on Robotics and Automation, pp. 1993-1998, Nice, 1992.