CRI08 - Robotica Mobile.ppt [modalità compatibilità]...Cristian Secchi Pag. 6 Principi di...

Cristian Secchi Pag. 1

CONTROLLO DI ROBOT INDUSTRIALILaurea Magistrale in Ingegneria Meccatronica

ROBOTICA MOBILEROBOTICA MOBILE

Ing. Cristian SecchiTel. 0522 522235

e-mail: [email protected]://www.dismi.unimo.it/Members/csecchi

Dalla Robotica Fissa a quella mobileDalla Robotica Fissa a quella mobile• A partire dagli anni ’60, compaiono i primi robot industriali• Utilizzati per sostituire l’uomo in compiti pericolosi o semplici e

ripetitivi

Controllo di Robot -- 2

• Si trattava di compiti da svolgere in loco• Gli ambienti di lavoro iniziarono ad essere modellati in base alle

esigenze di controllo dei robotCristian Secchi


Dalla Robotica Fissa a quella mobileDalla Robotica Fissa a quella mobile

Robotica Mobile

Imita la locomozione degli animali e le relative funzionalità

Robotica Mobile

Imita la funzionalità degli arti umani superiori

Controllo di Robot -- 3Cristian Secchi

Definizione:Un robot mobile è una macchina automatica in grado di muoversi nell’ambiente che la circonda.

Possibili ApplicazioniPossibili Applicazioni

Applicazioni Indoor• Pulizia di ambienti estesi• Robotica di servizio per musei, negozi,

ecc...ecc...• Sorveglianza interna di edifici• Stoccaggio merci in magazzini

automatizzati• ...

Applicazioni Outdoor• Applicazioni militari


pp• Sminamento• Esplorazione spaziale e subacquea• Protezione Civile e sorveglianza boschiva• Agricoltura automatizzata• …


Cenni storiciCenni storici

• Primi robot mobili a partire dal 1940• Razzi V1 - V2 (rozzo autopilota

inerziale)

Teletank

Goliath


• Filoguidato• Motori Elettrici

• Carro telecontrollato• 500 m di distanza

Uso industriale: Uso industriale: AutomaticAutomatic GuidedGuided VehiclesVehicles (AGV)(AGV)

Usato per la gestione di fine linea di produzione. Porta i pallet di prodotto stoccato dal magazzino alle baie di carico camion.In un impianto possono circolare più di 30 AGV contemporaneamente.


Elettric80 AGV


EsempiEsempi

Uso Ospedaliero: HELPMATE

Usato negli ospedali per trasporto dimedicinali e di logistica

Si muove autonomamente

Naviga tramite una telecamera e usa come riferimento le lampade sul soffitto

Domestico: Roomba

A i l li i i


Aspirapolvere per applicazionidomestiche

Grazie ad algoritmi di path planningpulisce la stanza senza bisogno diimpostare mappe

EsempiEsempi

Robot Sottomarini• Utilizzati per archeologia sottomarina• Recupero relitti (Titanic)• Ispezione struttureIspezione strutture• Monitoraggio acque

Applicazione Spaziali• R li ti ti d l 1965


• Realizzati a partire dal 1965• Altamente autonomi• In grado di lavorare senza

supervisione• Dotati di alta ridondanza dei sensori


EsempiEsempi

Robot Agricoli (attesi circa per il 2015)

Robot mobili concepiti per i i i l


operazioni agricole:

• Ambienti strutturati (frutteti ad hoc)

• vasti campi di cereali

Cristian Secchi

EsempiEsempi

Robot Pioneer

• Sviluppato dalla Stanford University


• Sviluppato dalla Stanford University• Scelto per esplorare il sarcofago di Chernobyl• Robot Modulare. Consente di montare:

• braccio robotico• telecamera• scanner laser


Principi di LocomozionePrincipi di Locomozione

• I concetti che si trovano in natura sono difficili da riprodurre ingegneristicamente

• La maggior parte dei sistemi ingegneristici usano le ruote o i cingoli

• Il rotolamento è il principio di locomozione più efficiente, ma non si trova in natura (la natura non ha mai inventato la ruota!)

• Tuttavia, la camminata di un bipede è vicina al rotolamento



Camminata Bipede• Approssima il rotolamento• Rotolamento di un poligono con lato

pari al passo della personap p p• Più il passo è corto, meglio il

rotolamento viene approssimato

I robot che camminano• si possono adattare meglio a un

ambiente variabile• iù l ti fi i i tt


• sono più lenti su superficie piatte • sono fortemente sovra-attuatiI robot con ruote• sono più veloci su superfici piatte• sono poco adattabili




LocomotionLocomotion!!



Robot con Ruote (Robot con Ruote (WheeledWheeled Mobile Mobile RobotsRobots -- WMR)WMR)

• Le ruote sono la soluzione più appropriata per molte applicazioni

• Tre ruote sono sufficienti per garantire la stabilità del robot

• Quante/Quali ruote usare e come disporle dipende dall’applicazione


Tipi di ruoteTipi di ruote

Ruota Fissa Ruota Centrata Orientabile

Ruota Scentrata e Orientabile(Ruota Castor) Ruota Svedese:proprietà

omnidirezionali



SwedishSwedish WheelsWheels

• Si basano sull’idea di poter sfruttare l’attrito anche trasversalmente alla direzione di moto della ruota


Possibili ConfigurazioniPossibili Configurazioni

Due Ruote

Tre Ruote



(Lego(Lego--))SegwaySegway


Possibili ConfigurazioniPossibili Configurazioni

• Quattro Ruote

• Sei Ruote



Esempio 6 ruote: NASA Esempio 6 ruote: NASA AthleteAthlete


SpazioSpazio delledelle ConfigurazioniConfigurazioni

• E’ caratterizzato dal numero di parametri che servono a localizzare la configurazione del robot

• E’ l’analogo dello spazio di giunto per un robot mobile

• Dipende dalla struttura del robot mobile

Esempio: Uniciclo Esempio: Biciclo



VincoliVincoli

• Si dice vincolo una qualunque condizione imposta ad un sistema materiale che impedisce di assumere una generica posizione e/o atto di moto

• Un sistema materiale si dice soggetto a vincoli olonomi se tra le coordinate del sistema esistono dei legami espressi da relazioni finite (vincoli di posizione) oppure se tra le coordinate del sistema esistono dei legami espressi da relazioni differenziabili integrabili finite.

• Un vincolo si dice anolonomo se la relazione differenziale tra le


• Un vincolo si dice anolonomo se la relazione differenziale tra le coordinate non è riducibile a forma finita.

Cristian Secchi

VincoliVincoli AnolonomiAnolonomi

• Ipotesi semplificativa: Ogni ruota rotola senza strisciamento (cioè senza slittare nè longitudinalmente nè lateralmente)

• Ogni ruota introduce un vincolo anolonomo nel sistema in quanto non consente una traslazione normale alla direzione di rotolamento

• Le ruote limitano la mobilità istantanea del robot senza però, in genere, precludere la possibilità di raggiungere configurazioni arbitrari (es.: Parcheggio parallelo di un automobile).



Vincoli Vincoli AnolonomiAnolonomi

⎪⎨

⎧ −+= )2

cos(cos θπθ nt vvx&θ

⎪⎩

⎨−+= )

2sin(sin θπθ nt vvy&

θ

Il disco rotola senza strisciare sul piano, la velocità normale al verso di rotolamento è nulla: vn=0

⎧


⎩⎨⎧

==

θθ

sincos

t

t

vyvx

&

&

dxdy

=θtan θθ cossin dydx =

0cossin =− θθ yx && Vincolo sulla mobilità


• Per ogni ruota è possibile scrivere il vincolo anolonomo in termini di coordinate generalizzate q come

0)( =qqa &

• Per N ruote i vincoli possono essere riscritti come

• I vincoli anolonomi non comportano una riduzione dello spazio delle

)( qq

0)( =qqA & Vincolo Pfaffiano


I vincoli anolonomi non comportano una riduzione dello spazio delle configurazioni ma comportano un riduzione della mobilitàistantanea del robot

Cristian Secchi



0)( =qqA & Limitazione della velocità

)(ker qAq∈&

Le velocità ammissibili possono essere generate da una matrice G(q) tale che:

Cq Ker(A(q))Im(G(q)) ∈∀=


vqGq )(=&

Modello cinematico tipico di un robot mobile con vincoli anolonomi

Ingresso cinematico

ModelloModello cinematicocinematico didi un WMRun WMR

vqGq )(=&

• Rappresenta le direzioni di moto ammissibili nello spazio delle configurazioni

• Lega gli ingressi in velocità nello spazio operativo con le derivate delle configurazioni

• E’ necessario per affrontare le problematiche principali della robotica mobile• Pianificazioni di traiettoria


• Pianificazioni di traiettoria• Controllo• Localizzazione• ….

Cristian Secchi


Modello cinematico dell’Modello cinematico dell’UnicicloUniciclo

Un uniciclo è un veicolo avente una sola ruota orientabile

⎟⎟⎟⎞

⎜⎜⎜⎛

= yx

q⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

0sincos

θθ

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

100 • Configurazione:

x

y θ

⎟⎠

⎜⎝θ

0]0,cos,[sincossin =−=− qyx &&& θθθθ

]0,cos,[sin)( θθ −=qA

⎦⎣⎥⎦⎢⎣1

• Vincolo:

• In forma Pfaffiana: 0)( =qqA &


],,[)(q

))(Im()100

,0

sincos

())(( qGspanqAKer =⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡= θ

θ

In forma Pfaffiana:

• modello cinematico:

0)( =qqA

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

100sin0cos

)( θθ

qG

Modello Cinematico dell’Modello Cinematico dell’UnicicloUniciclo

⎥⎦

⎤⎢⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡+

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

ωθθ

ωθθ

vvq

100

0sincos

100

0sincos

&

Modello cinematico dell’uniciclo

⎪

⎪⎨

⎧

==

θθ

&

&

sincos

vyvx

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

Gli ingressi v e ω hanno un chiaro significato fisico:

1. v è la velocità lineare del punto di contatto tra la ruota e il suolo ed è pari al prodotto tra velocità di rotazione della ruota attorno al suo asse orizzontale e il raggio della stessa

⎪⎩ =ωθ&


orizzontale e il raggio della stessa2. ω è la velocità angolare del robot, pari alla velocità di rotazione della

ruota intorno all’asse verticale

Agendo su v e ω è possibile variare la configurazione del robot


Modello cinematico dell’Modello cinematico dell’UnicicloUniciclo

• L’uniciclo propriamente detto ha gravi problemi di bilanciamento in condizioni statiche

• In pratica si ricorre a strutture cinematicamente equivalenti ma più stabili da un punto di vista meccanico. Le strutture più utilizzate p psono l’uniciclo a trazione differenziale (differential drive) e quello a trazione sincronizzata


UnicicloUniciclo differentialdifferential drivedrive

• Ha due ruote fisse coassiali e attuate indipendentemente e una ruota castor (solitamente di dimensioni ridotte) il cui scopo è di mantenere il robot in equilibrio statico

• q=[x,y,θ]. (x,y) sono le coordinate del punto medio dell’asse delle ruote e θ è l’orientamento del robot

• La ruota castor è passiva e le due ruote sono attuate indipendentemente. Il robot trasla se ωR=ωL e ruota sul posto se ωR=-ωL


su pos o se ωR ωL

• E’ la realizzazione più diffusa dell’unicicloKephera III

Pioneer

Sentinel



La velocità del punto medio dell’asse delle ruote v e la velocità angolare del robot ω sono legate in maniera biunivoca alle velocità angolare delle ruote dalla seguente relazione:

⎪⎪⎧ +

=rv LR )( ωω ωR = velocità della ruota destra

⎪⎪⎩

⎪⎪⎨ −

=d

r

v

LR )(2ωωω

ωR e oc tà de a uota dest aωL = velocità della ruota destrar= raggio delle ruoted= distanza tra i centri delle ruote


Dato un ingresso cinematico [v, ω]T da imporre all’uniciclo è sempre possibile trovare una coppia [ωR, ωL]T che lo riproducono


⎥⎤

⎢⎡⎥⎤

⎢⎡

=⎥⎤

⎢⎡ Rrrv ω2/2/

E’ possibile trovare il modello cinematico dell’uniciclo differential drive con le velocità angolari delle ruote come ingresso:

⎥⎦

⎤⎢⎣

⎡

⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎡

=⎥⎦

⎤⎢⎣

⎡⎥⎥⎤

⎢⎢⎡

⎥⎥⎤

⎢⎢⎡

= RR r

r

r

rrr

qωθ

θ

θ

θ

ωθθ

sin2

cos

sin2

cos

2200

sincos

&

⎥⎦

⎢⎣⎥⎦

⎢⎣ −

=⎥⎦

⎢⎣ Ldrdr ωω //


⎥⎦

⎢⎣

⎥⎥⎥

⎦⎢⎢⎢

⎣−

⎥⎦

⎢⎣⎥⎥

⎦⎢⎢

⎣−⎥

⎥⎦⎢

⎢⎣

LL

dr

drd

rdrq

ωω 2210


UnicicloUniciclo a trazione differenzialea trazione differenziale

Ha tre ruote orientabili e allineate che sono comandate in modo solidale da due soli motori attraverso un accoppiamento meccanico (es.:attraverso un accoppiamento meccanico (es.: catena, cinghia di trasmissione).

Un motore impone la rotazione delle ruote attorno al loro asse orizzontale e, quindi, determina la trazione al robot. L’altro motore impone la rotazione delle ruote attorno al loro asse verticale e, quindi, determina l’orientamento del robot


Questo tipo di robot è cinematicamente equivalente all’uniciclo. Il punto di coordinate (x,y) può rappresentare un punto qualsiasi del robot mentre θrappresenta l’orientamento del veicolo. Gli ingressi cinematici dell’uniciclo ideale sono gli stessi dell’uniciclo a trazione differenziale.

Modello cinematico del BicicloModello cinematico del Biciclo

Un biciclo è un veicolo avente una ruota orientabile e una ruota fissa disposte come in figura.

⎥⎥⎤

⎢⎢⎡yx

C fi i

⎥⎥⎥

⎦⎢⎢⎢

⎣

=

ϕθy

qConfigurazione:

),(),( rr yxyx =


Consideriamo il caso in cui il biciclo è a trazione anteriore.

C= Centro di istantanea rotazione



Il sistema è soggetto a due vincoli di puro rotolamento, uno per ogni ruota.

0)cos()sin( =+−+ ϕθϕθ yx ff && (xf,yf) è la posizione cartesiana del punto di contatto della ruota

0)cos()sin( =− θθ yx &&del punto di contatto della ruota anteriore

θ

θ

sin

cos

lyy

lxx

f

f

+=

+=

xf e yf non fanno parte della configurazione del biciclo perché dipendono da x,y e θ


0)cos()sin( =+−+ ϕθϕθ ff yx &&

0cos)cos()sin( =−+−+ ϕθϕθϕθ &&& lyx

Il vincolo introdotto dalla ruota anteriore è:


I vincoli cinematici del biciclo sono

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

=−=−+−+

θθϕθϕθϕθ

yxlyx

&&

&&&

0)cos()sin( θθ yx

⎥⎦

⎤⎢⎣

⎡−+−+

−=

0cos)cos()sin(00cossin

)(ϕϕθϕθ

θθl

qA

⎤⎡⎤⎡ θ


))(Im()

1000

,

0

sin1cossincoscos

())(( qGl

spanqAKer =

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢

⎣

⎡

=ϕ

ϕθϕθ



⎤⎡⎤⎡⎤⎡ θθ

v = velocità lineare di trazioneω = la velocità angolare


⎥⎦

⎤⎢⎣

⎡

⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢

⎣

⎡

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

+

⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢

⎣

⎡

=ωϕ

ϕθϕθ

ωϕ

ϕθϕθ

v

lv

lq

1000

0

sin1cossincoscos

1000

0

sin1cossincoscos

&


Il biciclo propriamente detto ha gravi problemi di bilanciamento in condizioni statiche


In pratica si ricorre a strutture cinematicamente equivalenti ma più stabili da un punto di vista meccanico. Le strutture più utilizzate sono la struttura a triciclo e l’automobile.


Il triciclo e l’automobileIl triciclo e l’automobile

Triciclo• Due ruote fisse sull’asse posteriore e una

ruota orientabile in posizione anteriore

• Le ruote fisse sono controllate da un unico motore che determina la trazione. Un altro motore controlla l’orientamento della ruota anteriore che determina lo sterzo

Automobile (Car-like)• Due ruote fisse sull’asse posteriore e due


Due ruote fisse sull asse posteriore e due ruote orientabili sull’asse anteriore.

• Il sistema è controllato da due motori. Uno determina la trazione (anteriore o posteriore) e l’altro lo sterzo.

Il triciclo e l’automobileIl triciclo e l’automobile

• Sia il triciclo che l’automobile hanno lo stesso modello cinematico del biciclo.

• Il t di di t ( ) è il t di d ll’ t i• Il punto di coordinate (x,y) è il punto medio dell’asse posteriore,

• θ è l’orientamento del veicolo

• ϕ è l’angolo di sterzo

• v e ω sono rispettivamente la velocità di trazione e la velocità di


• v e ω sono rispettivamente la velocità di trazione e la velocità di sterzo


UnicicloUniciclo e Bicicloe Biciclo

• La struttura cinematica a uniciclo e a biciclo sono le più utilizzate e diffuse nelle applicazioni (in particolare quelle industriali)

• Altre strutture cinematiche sono utilizzate per applicazioni particolari

• I modelli cinematici di strutture più complesse si ottengono tenendo in considerazione i vincoli introdotti da ciascuna ruota.


CompitiCompiti didi Moto Moto ElementareElementare

• Parcheggio

• Inseguimento di Cammini (Path Following)


• Inseguimento di Traiettorie


Schema Schema didi principio principio didi un robot mobileun robot mobile

• Attuatori (Att) : Motori DC con riduttori• End-Effector : attrezzo, pinza, mano• Sensori:

• Propriocettivi (encoder, giroscopi, …)


p ( , g p , )• Eterocettivi (bumbers, rangefinders (infrarossi, ultrasuoni),

laser, visione (mono, stereo, …)• Controllo:

• Alto livello• Basso livello

Schema Schema didi ControlloControllo a basso a basso livellolivello

• Controllori PI ad alto guadagno controllano i motori del robot affinchè il robot si muova secondo il profilo di velocità desiderato

• Il controllo a basso livello si occupa solamente di controllare gli


attuatori del robot secondo le istruzioni del controllo ad alto livello

• Se i guadagni sono abbastanza alti, il controllo di basso livello rende il robot un sistema puramente cinematico

Cristian Secchi


Schema Schema didi ControlloControllo ad alto ad alto livellolivello

• A seconda il della configurazione di setpoint, il controllo ad alto livello deve decidere che velocità imporre al robot

• Dal punto di vista del controllo ad alto livello il robot è un sistema p amente cinematico con ing essi in elocità


puramente cinematico, con ingressi in velocità

• Il controllo di alto livello si occupa di decidere il movimento mentre quello a basso livello si occupa di controllare i motori affinché tale movimento sia attuato

Controllo di WMRControllo di WMR

• Il progetto del controllo di basso livello • è semplice in quanto si riduce al progetto di un PI per un motore

elettrico (sistema lineare)• non risente dei vincoli introdotti dalle ruotenon risente dei vincoli introdotti dalle ruote

• La pianificazione della traiettoria da seguire• Fornisce il setpoint al controllo ad alto livello• Deve tenere in considerazione i vincoli sulla mobilità del robot

• Il progetto del controllo ad alto livello


p g• richiede di considerare considerare il modello cinematico di un

robot• deve tenere in considerazione i vincoli introdotti dalle ruote• E’ complesso perché deve controllare un sistema non lineare

Cristian Secchi


PianificazionePianificazione

• E’ il bl di d t i t i tt i ll i d ll• E’ il problema di determinare una traiettoria nello spazio delle configurazioni per portare il robot da una certa configurazione iniziale ad una certa configurazione finale ammissibili (cioè compatibili con i vincoli cinematici del robot)

• Ogni punto della traiettoria DEVE essere compatibile con i vincoli cinematici del robot

Esempio: Uniciclo


TRAIETTORIA INAMMISSIBILEIl robot non potrà mai inseguire il riferimento perché esso non è compatibile con i vincoli cinematici

Esempio: Uniciclo


Pianificazione: Trovare una traiettoria ammissibile q(t) per t in [ti,tf] che porti il robot da una configurazione iniziale q(ti)=qi a una configurazione finale q(tf)=qf in assenza di ostacoli

Una traiettoria può essere

s=0

s=L

Una traiettoria può essere decomposta in un cammino q(s) e una legge oraria s=s(t)

Legge oraria: come vado dal punto di distanza zero dall’inizio della curva al punto di distanza L dall’inizio della curva.

C i Q l è l fi i


s 0 Cammino: Qual è la configurazione corrispondente al parametro s nella curva?

Il cammino rappresenta la forma geometrica della curva parametrizzata da s mentre la legge oraria mi rappresenta il modo in cui la curva è percorsa.



sqsdsdq

dtdqq &&& '===Separazione spazio temporale della traiettoria:

Affinché la traiettoria sia ammissibile per un WMR, essa deve soddisfare i vincoli anolonomi del robot per cui:vincoli anolonomi del robot per cui:

0')()( == sqqAqqA &&

0>s&0')( =qqA

Condizione di ammissibilità geometrica del cammino

Un cammino ammissibile sarà dato da uqGq ~)('=


Il cammino geometrico si trova determinando gli ingressi geometrici ũ. Una volta trovato il cammino geometrico ammissibile, occorrerà scegliere una legge oraria s(t), ovvero la velocità a cui il cammino è percorso, per determinare completamente la traiettoria.


Una volta trovato l’ingresso geometrico ũ e determinata la legge oraria s(t), qual è l’ingresso cinematico da applicare al robot per farlo muovere lungo la traiettoria desiderata?

uqGq )(=&

)(~)()(' suqGsq =

ssuqGsdsdq

&& )(~)(=


?=ussuqGq && )(~)(=

ssutu &)(~)( =


EsempioEsempio

Per un uniciclo i vincoli anolonomi implicano la seguente condizione di ammissibilità per il cammino geometrico

0cos'sin'']0,cos,[sin =−=− θθθθ yxq

La condizione esprime il fatto che la velocità cartesiana deve essere orientata lungo la direzione del moto. I cammini ammissibili per l’uniciclo sono dati da:

ωθθθ

~'

~sin'

~cos'

===

vyvx


ωθ

stsvtv&

&

ωω ~)(

~)(==

Una volta determinati gli ingressi geometrici, gli ingressi cinematici sono dati da

Pianificazione tramite Pianificazione tramite differentialdifferential flatnessflatness

uxgxfx )()( +=&Un sistema dinamico non lineare

possiede la proprietà di differential flatness (o piattezza differenziale) se esiste un insieme di variabili misurabili y, dette uscite flat ( o piatte), tali che lo stato x e l’ingresso u possono essere espressi algebricamente in funzione di y e di un certo numero delle sue derivate

),...,,,(),...,,,(

)(

)(

r

r

yyyyuuyyyyxx

&&&

&&&

=

=


Una volta determinata una traiettoria y nel tempo, risultano determinate anche la traiettoria dello stato x e dell’ingresso u



Per l’uniciclo e il biciclo le coordinate cartesiane x e y sono uscite flat

θ ~'

Modello geometrico dell’uniciclo l’orientamento dipende dalla derivata prima delle uscite flat

ωθθθ

~'

~sin'

~cos'

===

vyvx

1,0)'/'arctan()','( =+== kkxyyx πθθ

p

Le due scelte possibili per θ dipendono dal fatto che lo stesso cammino può essere percorso a marcia avanti (k=0) o a marcia indietro (k=π). Se l’orientamento iniziale del robot è assegnato, solo una di queste scelte è co etta


corretta.

k=0 k=π


Se un robot mobile ammette uscite piatte y, queste possono essere utilizzate per risolvere i problemi di pianificazione. Infatti, è possibile usare un qualsiasi schema di interpolazione per pianificare il cammino di tali uscite in modo da soddisfare le condizioni al contorno. L’evoluzione delle altre variabili associate e gli ingressi cinematici si potranno calcolarevariabili associate e gli ingressi cinematici si potranno calcolare algebricamente a partire da y(s). Il cammino nello spazio delle configurazioni generato in tal modo soddisferà automaticamente i vincoli anolonomi del robot.




Si consideri il problema di pianificare un cammino per l’uniciclo che porti il robot da una configurazione iniziale qi=(xi,yi,θi) a una configurazione finale qf=(xf,yf,θf). Suppongo di parametrizzare la curva con un parametro s=[0,1].

x e y sono uscite flat! Pianifico la traiettoria sulle uscite flat!

2233

2233

)1()1()1()(

)1()1()1()( −+−+−−= ssssxsxssx xxif

β

βα

Una possibile pianificazione su x e y si può ottenere tramite un polinomio cubico


2233 )1()1()1()( −+−+−−= ssssysyssy yyif βα

fi

fi

yyyyxxxx

====

)1()0()1()0( Le condizioni al contorno

su x e y sono soddisfatte


Occorre che siano soddisfatte le condizioni al contorno anche sull’orientamento che è legato a x’ e y’. Devono allora essere soddisfatte le seguenti condizioni:

ki e kf sono parametri liberi (non nulli) ma dello stessokk θθ )1(')0(' (non nulli) ma dello stesso segno. Rappresentano le velocità geometriche all’istante iniziale e a quello finale e influenzano il tipo di cammino che si ottiene.

ffii

ffii

kykykxkx

θθθθ

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

====

Le condizioni al contorno su θ consentono di determinare i parametri αx, αy, βx e βy Per esempio, se ki=kf=k si ottiene:


y, βx βy. p , i f

⎥⎦

⎤⎢⎣

⎡++

=⎥⎦

⎤⎢⎣

⎡⎥⎦

⎤⎢⎣

⎡−−

=⎥⎦

⎤⎢⎣

⎡

ii

ii

y

x

ff

ff

y

x

ykxk

ykxk

3sin3cos

3sin3cos

θθ

ββ

θθ

αα



Una volta trovata la traiettoria nello spazio delle configurazioni, quali sono gli ingressi geometrici e gli ingressi cinematici corrispondenti?

Dal modello cinematico del robot si ottengonoDal modello cinematico del robot si ottengono

22

22

))('())('()(')('')(')('')(~

))('())('()(~

sysxsysxsxsys

sysxsv

+−

=

+±=

ω

il segno della velocità lineare dipende dal senso di marcia


Pianificazione dell’Pianificazione dell’UnicicloUniciclo -- AlgoritmoAlgoritmo

• Require: qi=(xi,yi,θi), qf=(xf,yf,θf)

1. Pianificare il cammino sulle uscita flat (x,y)

2233

2233

)1()1()1()(

)1()1()1()(

−+−+−−=

−+−+−−=

ssssysyssy

ssssxsxssx

f

xxif

βα

βα

2. Imporre le condizioni al contorno per determinare i coefficienti

3. Determinare gli ingressi geometric

)1()1()1()( ++ ssssysyssy yyif βα

⎥⎦

⎤⎢⎣

⎡++

=⎥⎦

⎤⎢⎣

⎡⎥⎦

⎤⎢⎣

⎡−−

=⎥⎦

⎤⎢⎣

⎡

ii

ii

y

x

ff

ff

y

x

ykxk

ykxk

3sin3cos

3sin3cos

θθ

ββ

θθ

αα

22 ))('())('()(~


4. Determinare la legge oraria e gli ingressi da applicare al robot

Cristian Secchi

22

22

))('())('()(')('')(')('')(~

))('())('()(~

sysxsysxsxsys

sysxsv

+−

=

+±=

ω

sstssvtv&

&

)(~)()(~)(

ωω ==



[ ]0100=iq [ ]000=fqParcheggio parallelo

k=10 k=100



[ ]3/710 π−=iq [ ]000=fqk=10

k=100k=50



Controllo del motoControllo del moto

Data una traiettoria o una configurazione desiderata, costruire una legge di controllo che porti il robot a seguire la traiettoria o nella configurazione desiderata

• Il bl d l t ll d l t di b t bil i• Il problema del controllo del moto di un robot mobile viene affrontato utilizzando il modello cinematico

• Si suppone che gli ingressi cinematici agiscano direttamente sulle variabili di configurazione

• Nel caso dell’uniciclo e del biciclo, gli ingressi di controllo sono v e ω• Questo è lecito perché nella maggior parte dei robot mobili non è

possibile imporre direttamente la coppia motrice sulle ruote a causa di lli di ll b li ll i i ll’ hi


di anelli di controllo a basso livello integrati nell’architettura hardware o software.

• Considereremo principalmente il problema del controllo del robot a uniciclo.

Controllo del motoControllo del moto

Regolazione di configurazione: Il robot deve raggiungere una configurazione desiderata qd=(xd, yd, θd)T a partire da una configurazione iniziale q0=(x0, y0, θ0)T .

E’ il problema più difficile

Inseguimento di traiettoria: Il robot deve riprodurre asintoticamente una traiettoria cartesiana desiderata (x (t) y (t)) a partire da

Problema più li di


cartesiana desiderata (xd(t),yd(t)) a partire da una configurazione iniziale q0=(x0, y0, θ0)T che può essere o meno agganciata alla traiettoria

semplice e di maggior interesse pratico


Inseguimento di traiettorie Inseguimento di traiettorie –– II--O SFLO SFL

• Dipende dal punto preso come riferimento del robot (uscita del sistema), ossia al quale vogliamo assegnare il moto desiderato• Il punto (x,y) del centro asse non può mai avere una velocità

I-O SFL: Input-Output state feedback linearization

laterale rispetto all’orientamento del veicolo

θθ

==

&

&

&

sincos

vyvx

Il sistema deve fermarsi e ruotare. Non è possibile percorrere una traiettoria discontinua

l i àP1

P2 P3

P4


ωθ =

Movimento istantaneo proibito

Prendendo il centro asse (x,y) come uscita da controllare abbiamo dei vincoli nella mobilità del robot.

a velocità costanteP1 P4


Si definisce come uscita da controllare un punto fuori dall’asse delle ruote di coordinate

0≠bθcosbxxB +=

),( BB yx

b

θsinbyyB +=

Il punto (xB,yB) non ha più vincoli cinematici e può muoversi, istantaneamente, anche lateralmente rispetto alla direzione di moto


),( yx

Posso definire due ingressi cinematici che determinano la velocità lungo l’asse x e lungo l’asse y del punto B



ωθ

θωθθωθωθθω

=

+=+=−=−=

&

&&

&&

cossincossincossin

bvbyybvbxx

B

B

dyB

dxB

vyvx

==

&

&

⎥⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡⎥⎦

⎤⎢⎣

⎡ −=⎥

⎦

⎤⎢⎣

⎡ω

θωθθ

θθ vT

vbb

yx

B

B )(cossinsincos

&

&0)(det ≠= bT θ

La matrice T(θ) è sempre invertibile

⎤⎡⎤⎡⎤⎡⎤⎡ θθ i


⎥⎦

⎤⎢⎣

⎡

⎥⎥⎦

⎤

⎢⎢⎣

⎡−=⎥

⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡ −

dy

dx

dy

dx

vv

bbvv

Tv

θθθθ

θω cos1sin1

sincos)(1


Dalle relazioni precedenti si ricava il seguente sistema lineare e disaccoppiato sui canali ingressi-uscite

dxB vx =& La direzione x e la direzione y del punto B possono essere controllate

)sincos(1 θθθ dxdy

dyB

vvb

vy

−=

=

&

&possono essere controllate indipendentemente l’una dall’altra tramite gli ingressi vdx e vdy

Data una traiettoria (xdes,ydes) da inseguire, è possibile costruire trovare vdxe vdy che garantiscono l’inseguimento utilizzando queste semplici leggi


lineari:

)()(

2

1

Bdesdesdy

Bdesdesdx

yykyvxxkxv−+=−+=

&

& yyexxe

desy

desx

−=−=

00

2

1

=+=+

yy

xx

ekeeke

&

&

00

→→

y

x

ee



Una volta trovati vdx e vdy, si trovano gli ingressi cinematici da dare in ingresso all’uniciclo per ottenere il movimento desiderato del punto B tramite

⎥⎦

⎤⎢⎣

⎡

⎥⎥⎦

⎤

⎢⎢⎣

⎡−=⎥

⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡ −

dy

dx

dy

dx

vv

bbvv

Tv

θθθθ

θω cos1sin1

sincos)(1



Il punto (x,y) arrotonda l’angolo vivo. L’arrotondamento può essere reso piccolo a

P2 P3021 ==→ dydx vvvPP

piacere scegliendo b abbastanza piccolo


P1 P4014

043

032

=−=→

−==→

==→

dydx

dydx

dydx

vvvPP

vvvPP

vvvPP


IOIO--SFL per l’SFL per l’UnicicloUniciclo -- AlgoritmoAlgoritmo

Require: qdes(t)=(xdes(t),ydes(t),θdes(t))

1. scegliere il punto fuori dall’asse (xB,yB)=(x+bcosθ,y+bsinθ) definendo b

2. while True3. Determinare vdx(t)e vdy(t) tramite

4. Determinare v(t) e ω(t) tramite

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

2

1

tytyktytvtxtxktxtv

Bdesdesdy

Bdesdesdx

−+=−+=

&

&


5. Applicare gli ingressi 6. end while

Cristian Secchi

⎥⎦

⎤⎢⎣

⎡

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

−=⎥

⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡ −

)()(

)(cos1)(sin1)(sin)(cos

)()(

))(()()( 1

tvtv

tb

tb

tt

tvtv

tTttv

dy

dx

dy

dx

θθ

θθθ

ω

EsempioEsempio



NavigazioneNavigazione

Spesso un robot si trova a doversi muovere in un ambiente in cui sono presenti ostacoli (fissi o mobili). Il robot, tramite la conoscenza dell’ambiente e/o tramite l’utilizzo dei sensori a bordo deve riconoscere dove sono gli ostacoli ed evitarli al fine di navigare in maniera sicura (cioè

lli i i) ll’i t d ll i di lsenza collisioni) all’interno dello spazio di lavoro.

Navigazione: Data una configurazione di partenza e una di arrivo note, è il problema di trovare una percorso libero da collisioni che porti il robot dalla configurazione di partenza a quella di arrivo

goalq


startq

Bug Bug AlgorithmsAlgorithms

Gli algoritmi bug si basano sulla possibilità del robot di percepire, tramite opportuni sensori eterocettivi (bumpers, range finders), la presenza di un ostacolo quando vi sono molto vicini o addirittura quando lo stanno toccando.tocca do

Idea di base: Muovere il robot in linea retta verso l’obiettivo. Quando si incontra un ostacolo, lo si circumnaviga fino a quando non è possibile muoversi in linea retta verso l’obiettivo.


Si assume che il robot sia un punto (es.: no controllo sull’orientamento per uniciclo) dotato di un sensore di contatto. Si assume inoltre che il robot sia in grado di conoscere la sua posizione, oltre a quella della configurazione di partenza e di quella obiettivo.


Bug1Bug1

Il robot ha due tipi di comportamento tra i quali commuta:

Si definisca come m-line il segmento che collega un punto a qgoal

motion to goal: A partire da un leave point, il robot si muove lungo la m-line che connette il leave point al goal finchè non raggiunge il goal o incontra un ostacolo. Se il robot incontra un ostacolo, il punto di contatto viene detto hit point e il comportamento del robot commuta al boundary following.

boundary following: a partire da un hit point, il robot circumnaviga l’ostacolo finchè non ritorna all’hit point. Poi determina il punto più vicino al


p p pgoal lungo il perimetro dell’ostacolo e si sposta lungo il perimetro fino a raggiungere quel punto, detto leave point e il comportamento del robot commuta al motion to goal.

Bug1Bug1

LiqHiq

leave point i-esimo

hit point i-esimo

startL qq =0 0=i Inizializzazione

All’inizio il sistema si trova in un leave point



Bug1Bug1

Se la linea che collega il leave point i-esimo con il goal intereseca l’ostacolo i-esimo, allora non esiste nessun percorso che consenta di raggiungere il goal. In tal caso l’algoritmo lo segnala


Bug1 Bug1 AlgorithmAlgorithm

Input: Un robot con un sensore di contattoOutput: Un percorso fino a qgoal o una conclusione che tale percorso non esiste

0: i=1; qL0=qstart;

1: while Forever do2: repeat2: repeat3: Da q Li-1, muoviti verso qgoal. 4: until qgoal è raggiunto or si tocca un ostacolo a qH

i5: if Il goal è raggiunto then6: Exit. 7: end if8: repeat9: Segui il bordo dell’ostacolo. 10: until qgoal è raggiunto or ci si ritrova a qH

i11: Determina il punto qL

i sul perimetro che è alla minima distanza dal goal.


12: vai a qLi.

13: if la m-line che passa da qLi interseca l’ostacolo su cui si trova qL

i then14: Concludi che qgoal non è raggiungibile e exit. 15: end if16: i=i+116: end while


Bug2 Bug2

• Come per il Bug1 anche il Bug2 commuta tra due comportamenti: il motion to goal e il boundary following

• Per il Bug2, la m-line è fissa ed è il segmento di linea che connette qstart a qgoal

motion to goal: A partire da un leave point, il robot si muove lungo la m-line che connette qstart a qgoal finchè non raggiunge il goal o incontra un ostacolo. Se il robot incontra un ostacolo, il punto di contatto viene detto hit point e il comportamento del robot commuta al boundary following.


boundary following: a partire da un hit point, il robot circumnaviga l’ostacolo finchè non raggiunge un punto sulla m-line, detto leave point e il comportamento del robot commuta al motion to goal.

Bug2Bug2

LiqHiq

leave point i-esimo

hit point i-esimo

goalL qq =0 0=i Inizializzazione

All’inizio il sistema si trova in un leave point



Bug2Bug2

Se durante il boundary following il robot incontra di nuovo il punto da cui è partito sulla m-line allora non esiste nessun percorso che consenta di raggiungere il goal. In tal caso l’algoritmo lo segnala


Bug2 Bug2 AlgorithmAlgorithm

Input: Un robot con sensore di contatto Output: Un percorso fino a qgoal o una conclusione che tale percorso non esiste

0: i=1; qL0=qstart;

1: while True do2 t2: repeat3: Da qL

i-1, muoviti verso qgoal lungo la m-line. 4: until qgoal è stato raggunto or si tocca un ostacolo all’hit point qH

i. 5: Gira a sinistra (o a destra). 6: repeat7: Segui il bordo dell’ostacolo 8: until9: qgoal è raggiunto or10: qH

i è raggiunto di nuovo or11: la m-line è raggiunta di nuovo a un punto m tale che


11: la m line è raggiunta di nuovo a un punto m tale che 12: m ≠ qH

i (il robot non ha raggiunto l’ hit point), 13: d(m, qgoal) < d(m, qH

i (il robot è più vicino), e14: Se il robot si muove verso il goal, non entra in contatto con l’ostacolo. 15: Si pongta qL

i+1 = m16: i=i+117: end while


Bug1 o Bug2?Bug1 o Bug2?

A prima vista sembra che Bug2 sia meglio di Bug1 in quanto esso non comporta una circumnavigazione completa degli ostacoli. Tuttavia, questo non è sempre vero e dipende dal tipo di ostacoli

B 1 è li di B 2Bug2 è meglio di Bug1 Bug1 è meglio di Bug2


Bug1 o Bug2?Bug1 o Bug2?

• Bug1 fa una ricerca esaustiva e trova il leave point migliore. Questo richiede che il robot percorra tutto il perimetro dell’ostacolo

• Bug2 usa un approccio opportunistico. Quando trova un leave point che è migliore di quelli che ha visto prima , lo sceglie (approccio greedy)

• Quando gli ostacoli sono “semplici”, tipicamente Bug2 è migliore di Bug1


• Nel caso in cui gli ostacoli siano “complessi”, Bug1 è migliore di Bug2

Cristian Secchi


TangentTangent BugBug

• E’ un miglioramento del Bug2.• Proposto in:

Kamon I., Rivlin E., Rimon, E., A new range-sensor based globally convergent navigation algorithm for mobile robots IEEE Internationalconvergent navigation algorithm for mobile robots, IEEE International Conference on Robotics and Automation, 1996

disponibile all’indirizzohttp://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=503814&isnum

ber=10815accessibile solo dagli indirizzi IP UNIMORE.


Navigazione attraverso funzioni PotenzialiNavigazione attraverso funzioni Potenziali

• Gli algoritmi bug sono indicati solamente per robot mobili che si muovono sul piano e non si considera l’intera variabile delle configurazioni

• Attraverso la pianificazione basata su campi potenziali è possibile pianificare il moto anche per sistemi con variabili di configurazione di dimensione elevata.

• Come gli algoritmi bug, consentono di generare il percorso


Come gli algoritmi bug, consentono di generare il percorso incrementalmente, cambiandolo man mano che un ostacolo è percepito

Cristian Secchi



RRU m →: Funzione potenziale

TUU ⎤⎡ ∂∂

n

qqUq

qUqU ⎥

⎦

⎤⎢⎣

⎡∂∂

∂∂

=∇ )()()(1

L Gradiente di U

La funzione potenziale può essere interpretata come una funzione energia sullo spazio delle configurazioni e il gradiente di U rappresenta la


energia sullo spazio delle configurazioni e il gradiente di U rappresenta la forza indotta dalla presenza del potenziale. Il gradiente in una punto q punta verso la direzione in cui U cresce.


22 111 T 22

21 2

121

21)( qqqqqU T +==

[ ]21)( qqqU =∇




L’approccio delle funzioni potenziali guida il robot come se fosse una particella immersa in un campo potenziale dato dai gradienti di svariate funzioni potenziali. Alla configurazione obiettivo è associata una funzione potenziale che genera un gradiente che attira la particella mentre ad ogni ostacolo è associato una funzione potenziale che genera unogni ostacolo è associato una funzione potenziale che genera un gradiente che respinge le particelle. La combinazione dei due gradienti produce un percorso libero da collisioni verso la configurazione obiettivo.



Le funzioni potenziali possono essere viste come dei paesaggi in cui il robot è immerso e lungo i quali si muove spostandosi da un punto “in alto” a un punto più “in basso”. Il robot segue un percorso in discesa, seguendo il gradiente negato della funzione potenziale. Questa tecnica di movimento si chiama discesa del gradiente (gradient descent).movimento si chiama discesa del gradiente (gradient descent).

U(q) )()( qUtq −∇=&


q

Il robot termina quando raggiunge una configurazione q* nella quale il gradiente si annulla. Questo punto può essere un minimo, un massimo o un punto di sella. Per come verranno disegnati i potenziali, q* sarà un minimo (locale)



Come si costruisce la funzione potenziale che genera il cammino da qstart a qgoal evitando gli ostacoli?

)(UPotenziale attrattivo. Ha il minimo in

i d di Il l è di)(qUatt

∑=

=obs

i

n

ireprep qUqU

1

)()(

corrispondenza di qgoal.Il suo ruolo è di attirare il robot verso l’obiettivo.

Potenziale repulsivo. E’ dato dalla somma dei potenziali repulsivi per ogni ostacolo. Ha il ruolo di allontanare il


robot dagli ostacoli presenti nell’ambiente

)()()( qUqUqU repatt += )()( qUtq −∇=&

Il potenziale attrattivoIl potenziale attrattivo

Il potenziale Uatt deve essere monotonicamente crescente al crescere della distanza da qgoal. In tal modo il robot, tramite gradien descent, viene attirato alla configurazione obiettivo qgoal. Ciò equivale a dire che qgoal è una configurazione di minimo isolata e che il sistema non ha punti critici. I di d d( ) l di t t ibil lt èIndicando con d(q,qgoal) la distanza tra q e qgoal, una possibile scelta è:

),(21 2

goalaatt qqdkU = )(),(21 2

goalagoalaatt qqkqqdkU −=∇=∇


Quando il robot è lontano dall’obiettivo, vi si avvicina velocemente (la forza che lo attrae è grande) mentre quando il robot è vicino all’ obiettivo, vi si avvicina dolcemente (la forza che lo attrae è piccola)


Il potenziale attrattivoIl potenziale attrattivo


Il potenziale repulsivoIl potenziale repulsivo

E’ composto da una somma di potenziali repulsivi, ciascuno associato a un ostacolo. Il termine i-esimo deve essere tale da generare una forza repulsiva tanto più grande quanto più il robot è vicino all’ostacolo i-esimo. Inoltre, esso non deve generare nessuna forza se il robot è sufficientemente lontano dall’ostacolo ostacolo i-esimo. Una possibile scelta è:

⎪⎩

⎪⎨

⎧

>

≤⎟⎟⎠

⎞⎜⎜⎝

⎛−

=

*)(0

*)(*

1)(

121

)(

2

Qqd

QqdQqd

kqU

i

ii

rrep

ii ∑

=

=obs

i

n

ireprep qUqU

1)()(


Q*=distanza oltre la quale il robot ignora l’ostacolodi(q)=distanza tra il robot e l’ostacolo i esimo.nobs=numero di ostacoli


Il potenziale repulsivoIl potenziale repulsivo

⎪

⎪⎨

⎧≤∇⎟⎟

⎠

⎞⎜⎜⎝

⎛−

=∇*)()(

)(1

*1

)(

2

QqdqdqdQ

kqU ii

ir

repi

i

⎪⎩ > *)(0 Qqdi

Se di(q) va a zero allora il gradiente va a infinito!

∑∇=∇obsn

qUqU )()(


∑=

∇=∇i

ireprep qUqU

1)()(

L’algoritmo di discesa del gradienteL’algoritmo di discesa del gradiente

Input: Un modo di calcolare il gradiente U (q) in un punto qOutput: Una sequenza di punti {q(0), q(1), ..., q(i)}

repatt UUUtq ∇−−∇=−∇=)(&

p q p {q( ), q( ), , q( )}

1: q(0) = qstart

2: i = 0 3: while U (q(i)) ≠ 0 do4: q(i + 1) = q(i) - α(i) U (q(i)) 5: i = i + 1 6: end while

Il l (i) d t i l l h d l i il b t d


Il valore a(i) determina la lunghezza del passo con cui il robot scende lungo il gradiente di U. Se a(i) è piccolo, l’algoritmo è accurato ma computazionalmente pesante mentre se a(i) è grande l’algoritmo è più veloce ma meno accurato. La scelta di a(i) dipende dal tipo di applicazione.


Minimi LocaliMinimi Locali

E’ possibile che l’algoritmo di discesa del gradiente si fermi in un minimo locale


Il potenziale attrattivo e quello repulsivo si controbilanciano!!

Minimi LocaliMinimi Locali

Esistono approcci per risolvere il problema dei minimi locali:

1) Wave front planners:J. Barraquand, B. Langlois, J.C. Latombe, Numerical potential field techniques for robot path planning, IEEE u e ca pote t a e d tec ques o obot pat p a g,Transactions on Man and Cybernetics 22(2),1992disponibile a

http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=148426&isnumber=3929

da un indirizzo IP UNIMORE


2) Navigation Functions. Si veda il libro Principles of Robot Motiondispobilile in biblioteca


Altri algoritmi di navigazioneAltri algoritmi di navigazione

• Esistono altre tecniche di navigazione per certi versi più complete ed efficienti di quelle illustrate sinora:• Roadmaps• Cell Decomposition• Sampling based algorithms• …

• Si consulti il seguente testo disponibile in biblioteca per una trattazione esaustiva:• Choset Lynch Hutchinson Kantor Burgard Kavraki Thrun


• Choset, Lynch, Hutchinson, Kantor, Burgard, Kavraki, Thrun, Principle of Robot Motion- Theory, algorithms and Implementations, MIT Press 2005

Cristian Secchi

ControlloControllo del Motodel Moto

• Altre problematiche interessanti• Pianificazione in presenza di vincoli cinematici (velocità limitata)• Cammini a tempo minimo• Sistemi multi robot• …



SLAM SLAM –– SimultaneousSimultaneous LocalizationLocalization and and MappingMapping

• Per poter seguire una traiettoria desiderata un robot mobile deve conoscere la propria configurazione

• Per poter eseguire una strategia di navigazione non banale, un robot deve conoscere una mappa dell’ambiente circostante

• Ogni controllore in retroazione richiede la conoscenza dello stato del robot (posizione assoluta)


• In generale, per rendere veramente autonomo un robot mobile, è necessario che esso sia in grado di localizzarsi e di costruire una mappa dell’ambiente circostante

Cristian Secchi

SLAM SLAM –– SimultaneousSimultaneous LocalizationLocalization and and MappingMapping

Lo SLAM cerca una risposta alla domanda:

E’ possibili usare un veicolo che parte da

1. una posizione iniziale ignota, in un 2. ambiente ignoto, per 3. costruire incrementalmente una

mappa dell’ambiente, e 4. allo stesso tempo

usare la mappa per determinare la


usare la mappa per determinare la posizione del robot?


Origini: la localizzazione Origini: la localizzazione odometricaodometrica

OdometriaL’odometria è la tecnica per stimare la posizione di un veicolo su ruote basandosi su informazioni provenienti da sensori che misurano lo spazio percorso da alcune delle ruote e l’angolo di sterzo (se presente).

• Gli encoder incrementali sulle ruote ne misurano la rotazione, ma non l’assetto del robot in un sistema di riferimento fisso assoluto

• Se una ruota slitta l’encoder rileva uno spostamento che non è


• Se una ruota slitta, l encoder rileva uno spostamento che non è coerente con il cambio di assetto del robot

Cristian Secchi

Localizzazione Localizzazione odometricaodometrica: l’: l’uniciclouniciclo

⎪⎩

⎪⎨

⎧

==

θ

θθ

&

&

&

sincos

vyvx Supponiamo comandi costanti durante ogni

intervallo di campionamento (generalmenteverificato se controllo digitale)

• Se all’istant k(=kT) siano noti • la configurazione q(k) del robot• i comandi v(k) e ω(k) che saranno applicati nell’intervallo [k,k+1)

⎪⎩ =ωθ verificato se controllo digitale)


• il periodo di campionamento T

• è possibile utilizzare il modello cinematico del robot per determinare q(k+1)

Cristian Secchi


Localizzazione Localizzazione odometricaodometrica: l’: l’uniciclouniciclo• Discretizzando il modello con il metodo di Eulero si ottiene

⎪⎩

⎪⎨

⎧

+=+=+

+=+

)()()1()(sin)()1(

)(cos)()()1(

kTkkkTkvky

kTkvkxkx

ωθθθ

θ

• E’ molto semplice ma vi sono alcuni errori nella localizzazione• si considera θ(k) come costante in [k,k+1)• non è detto che i valori comandati v(k) e ω(k) siano quelli realmente

applicati (non idealità del sistema di attuazione)• errori dovuti allo slittamento delle ruote• errori di calibrazione (d e r)

⎩ )()()(


( )• errori introdotti dal sistema di trazione• …

• La localizzazione odometrica produce un dato inaffidabile e caratterizzato da un errore non deterministica significativo e che sia accumula durante il moto del robot

Cristian Secchi

Uso della sola odometriaUso della sola odometria

GPS (for reference)


Odometry


Sensori Sensori EterocettiviEterocettivi

• Per compensare gli errori introdotti dall’odometria e per costruire una mappa dell’ambiente circostante si utilizzano sensori eterocettivi, solidali al robot e capaci di misurare la posizione del robot rispetto all’ambiente esternop

• I sensori eterocettivi più utilizzati sono gli scanner laser, le telecamere (monoscopiche o stereoscopiche) e il GPS (outdoor o indoor) e, per semplici applicazioni, il sonar


Lo Scanner LaserLo Scanner Laser

• Un fascio di raggi è emesso con un certo passo angolare e ciascun f i i i l di d ll’ l h l i


fascio restituisce la distanza dell’ostacolo che colpisce. • Per ogni fascio viene restituita l’informazione di range (distanza) e

bearing (angolo di emissione del fascio laser)• Sono misurate le coordinate polari di ciascun elemento dell’ambiente

in un sistema di riferimento solidale al robot

Cristian Secchi


Il problema della localizzazioneIl problema della localizzazione

• Data una mappa m di punti di riferimento la cui posizione è nota apriori

• Misurare la posizione relativa dei punti di riferimento y(k) (es.:range e bearing)

• Determinare la posizione del veicolo x(k) basandosi sulla misura y(k)• Determinare la posizione del veicolo x(k) basandosi sulla misura y(k)• un filtro è necessario perché le misure sono tipicamente

rumorose• La posizione dei landmark viene utilizzata per ridurre l’incertezza

nella posizione del robot

• xk: posizione del veicolo all’stante k

• u : l’ingresso di controllo applicato all’stante k-1 y

yk+1


• uk: l ingresso di controllo applicato all stante k-1e tenuto costanto nell’intervallo [k-1,k)

• yk: misura del landmark pressa allistante k

• Xk: storia delle posizioni {x1,..,xk}

• Uk: storia degli ingressi di controllo {u1,…,uk}

• m: insieme dei landmark

yk-1

yk

Il problema del Il problema del mappingmapping

• Date le posizioni del veicolo Xk

• Misurare la posizione relativa dei punti di riferimento y(k) (es.:range e bearing)

• Determinare la mappa basandosi sulla misura y(k)• un filtro è necessario perché le misure sono tipicamente

rumorose• La posizione del veicolo viene

usata per ridurre l’incertezza nelrilevamento dei landmark

yk,j



• mi: posizine del landmark i


yk-1,i

yk,i


Il problema dello SLAMIl problema dello SLAM

• Dalla conoscenza delle misure Yk

• Determinare la posizione del veicolo Xk

• Costruire la mappa m delle posizioni dei landmark

• xk: posizione del veicolo all’stante k

• uk: l’ingresso di controllo applicato all’stante k-1e tenuto costanto nell’intervallo [k-1,k)

• yk: misura del landmark pressa allistante k



p { 1, , k}

• Uk: storia degli ingressi di controllo {u1,…,uk}

• Yk: storia delle misure {y1,…,yk}


SLAMSLAM

• La localizzazione e il mapping sono problemi correlati• due quantità incerte devono essere inferite tramite una sola

misura• Una soluzione può essere ottenuta solo se i problemi di

localizzazione e mapping sono considerati insiemelocalizzazione e mapping sono considerati insieme


L’incertezza nella posizione si correla con l’incertezza della mappa!


SLAM SLAM -- SettingSetting

• Un veicolo con un modello cinematico noto si muove in un ambiente popolato di landmark fissi (modello del processo)

• Il veicolo è dotato di un sensore che può misurare la posizione relativa tra ciascun landmark e il veicolo stesso (modello dell’osservazione)


Modello del processoModello del processo

• Supponiamo, per ora, che il modello cinematico del robot sia un sistema lineare descritto da:

)()()1()( kwkukAxkx vvvv ++−=dove• A è la matrice di transizione dello stato• uv è il vettore degli ingressi• wv è un vettore aleatorio gaussiano con media nulla e matrice di

covarianza Qv(k) che modella l’incertezza del modello

• L’ i di t i i d ll t t il l d k i i è d t


• L’equazione di transizione dello stato per il landmark i-esimo è data da:

Cristian Secchi

)1()( −= kpkp ii



• E’ possibile raggruppare la dinamica del robot e la dinamica dei landmark in un unico modello del processo

• Lo stato del processo è dato da

• Il modello del processo p descritto da:

( )TTN

TT ppkxkxv

L1)()( =

)()()1(0

)(0

)()1()1(

0000

)()(

)( 111 11 kwkukAx

kwkukpkx

IA

kpkx

kx p

v

p

vv

p

v

++−=⎟⎟⎟⎞

⎜⎜⎜⎛

+⎟⎟⎟⎞

⎜⎜⎜⎛

+⎟⎟⎟⎞

⎜⎜⎜⎛

−−

⎟⎟⎟⎞

⎜⎜⎜⎛

=⎟⎟⎟⎞

⎜⎜⎜⎛

=L

L


dove Ipi e 0pi sono rispettivamente la matrice identità e la matrice nulla di ordine pi

Cristian Secchi

)()()1(

00)1(00

)( kwkukAx

kpIp

kx

NNN ppNpN

++=

⎟⎟⎟

⎠⎜⎜⎜

⎝

+

⎟⎟⎟

⎠⎜⎜⎜

⎝

+

⎟⎟⎟

⎠⎜⎜⎜

⎝ −⎟⎟⎟

⎠⎜⎜⎜

⎝

=

⎟⎟⎟

⎠⎜⎜⎜

⎝

=MMM

L

MOMMM

Modello dell’OsservazioneModello dell’Osservazione

• Supponiamo, per ora, che i sensori che osservano i landmark siano lineari. Il modello dell’osservazione del i-esimo landmark è dato da:

)()()( kvkxCky iii +=• dove:

• vi(k) è un vettore aleatorio gaussiano con media nulla e matrice di covarianza Rv(k) che modella l’incertezza della misura

• Ci è la matrice di osservazione (cioè la matrice di uscita) che mette in relazione lo stato del processo con la misura relativa del landmark yi e può essere scritta come:

)()()( kvkxCky iii +


• Il modello dell’osservazione può essere pertanto riscritto come

Cristian Secchi

( )0000 LLipi CAC −=

)()( kAxpCky vipi i−=

misura del landmarkrelativa alla posizione del veicolo


SLAMSLAM

• Determinare la posizione del robot e quella dei landmark equivale a stimare lo stato del processo in base alle misure

• Si l di i d l h ll d ll i è tt i t• Sia la dinamica del processo che quella delle misure è caratterizzata da un’incertezza gaussiana

• Il Filtro di Kalman risolve il problema! E consente di risolverlo online!


Il Filtro di Il Filtro di KalmanKalman

• Predizione• Stato Predetto

• Covarianza Predetta

)1()1|1(ˆ)1|(ˆ −+−−=− kukkxAkkx

)()1|1()1|( kQAkkAkk T +−−Σ=−Σ

• Aggiornamento• Innovazione

• Covarianza Innovazione

• G d di K l

)1|(ˆ)()( −−= kkxCkyke

)()1|()( kRCkkCkS T +−Σ=

1)()1|()( −Σ kSCkkkK T


• Guadagno di Kalman

• Aggiornamento dello stato

• Aggiornamento Covarianza

)()1|()( −Σ= kSCkkkK

)()()1|(ˆ)|(ˆ kekKkkxkkx +−=

)1|())(()|( −Σ−=Σ kkCkKIkk


Il filtro di Il filtro di KalmanKalman

(1) Predizione dello stato

(2) Predizione della covarianza)1()1(ˆ)(ˆ −+−=− kukxAkx

Predizione

( ))()1()( kQAkAk T +−Σ=Σ −

(1) Calcolo il guadagno di Kalman1))(()()( −−− +ΣΣ RCkCCkkK TT

Aggiornamento


(2) Aggiorno la stima con la misura

(3) Aggiorno la covariaza

1))(()()( +ΣΣ= RCkCCkkK TT

)()()(ˆ)(ˆ kekKkxkx += −

−Σ−=Σ )())(()( kCkKIk)1()1(ˆ −Σ− kkx

Filtro di Filtro di KalmanKalman Esteso (Esteso (ExtendedExtended KalmanKalman FilterFilter –– EKF)EKF)

• Il filtro di Kalman può essere utilizzato solo se il modello del processo e il modello dell’osservazione sono sistemi lineari

• Il modello cinematico di un robot mobile (es : uniciclo automobile)• Il modello cinematico di un robot mobile (es.: uniciclo, automobile) sono nonlineari

• Il modello dell’osservazione spesso è nonlineare (es.: utilizzando scanner laser)

• Il Filtro di Kalman Esteso (EKF) permettere di superare l’ipotesi à


di linearità sul processo e sulla misura

Cristian Secchi


EKFEKF

• Il processo e le misure sono in realtà descrivibili da funzioni nonlineari:

⎨⎧ +−−= )())1(),1(()( kwkukxfkx

• dove w(k) e v(k) sono vettori aleatori di media nulla che descrivono l’incertezza di processo e di misura e le cui matrici di covarianza sono rispettivamente Q(k) e R(k)

⎩⎨ += )())(()(

)())()(()(kvkxhky

f


EKFEKF

• L’idea che sta alla base dell’EKF è quella di approssimare il sistema il cui stato è da stimare linearizzandolo intorno allo stato stimato. Data la stima dello stato all’istante k-1, si linearizza il sistema intorno a quello stato e poi si stima lo stato all’istante k.

• Ad ogni istante si lavora su un sistema lineare e, quindi, è possibile utilizzare il filtro di Kalman per la stima dello stato

• Ad ogni istante, il sistema lineare che si considera è diverso perché si linearizza intorno a diversi punti di equilibrio

∂f


))1(ˆ)1()(())1(,)1(ˆ())1(ˆ)1((|))1(,)1(ˆ())1(),1(( ))1(),1(ˆ( −−−+−−=−−−∂∂

+−−≈−− −− kxkxkFkukxfkxkxxfkukxfkukxf kukx

))(ˆ)()(())(ˆ())(ˆ)((|))(ˆ())(( ))(ˆ(−−−− −+=−

∂∂

+≈ − kxkxkHkxhkxkxxhkxhkxh kx


L’algoritmo EKFL’algoritmo EKF

• Predizione• Stato Predetto

• Covarianza Predetta

))1(),1(ˆ()(ˆ −−=− kukxfkx

)()()1()()( kQkFkkFk T +−Σ=Σ −

• Aggiornamento• Innovazione

• Covarianza Innovazione

• G d di K l

))(ˆ()()( −−= kxhkyke

)()()()()( kRkHkkHkS T +Σ= −

1)()()()( −−Σ kSkHkkK T


• Guadagno di Kalman

• Aggiornamento dello stato

• Aggiornamento Covarianza

1)()()()( Σ= kSkHkkK T


−Σ−=Σ )())()(()( kkHkKIk

EKF EKF –– LocalizzazioneLocalizzazione

⎪

⎪⎨

⎧

==

θθ

&

&

sincos

vyvx

Modello non lineare!

Un robot di tipo differential drive deve seguire una traiettoria circolare

⎪⎩ =ωθ&


Un robot di tipo differential drive deve seguire una traiettoria circolare predefinita. L’inseguimento di traiettoria è implementato tramite IO-SFL ed è quindi necessario una misura della posizione del robot per costruire l’azione di controllo. Il modello cinematico del robot è affetto da una incertezza di processo. La posizione del robot è misurata da una telecamera e la misura è affetta da un’incertezza.



⎪⎩

⎪⎨

⎧

==

ωθ

θθ

&

&

&

coscos

vyvx

⎪⎩

⎪⎨

⎧

++

+−−+−=+−−+−=

)()1()()(

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

kwkTkk

kwkkTvkykykwkkTvkxkx

y

x

ωθθ

θθ

⎩ =ωθ ⎩ +−+= )()1()()( kwkTkk θωθθ

01.0001.0

==

bT

θθ

sincos

byybxx

B

B

+=+=

)()(

2

1

Bdesdesdy

Bdesdesdx

yykyvxxkxv−+=−+=

&

&


⎥⎦

⎤⎢⎣

⎡

⎥⎥⎦

⎤

⎢⎢⎣

⎡−=⎥

⎦

⎤⎢⎣

⎡

dy

dx

vv

bb

vθθθθ

ω cos1sin1sincos

30)cov( ==⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡Q

www

y

x

θ

Modello dell’OsservazioneModello dell’Osservazione

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛+⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

)()()(

)()()(

)(kvkvkv

kkykx

ky y

x

θθ ⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛==

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

8.00008.00008.0

)())()()(

cov( kRkvkvkv

y

x

θ

• Tramite l’uscita ho un accesso diretto allo stato. • L’uscita è però caratterizzata da un’incertezza



RisultatiRisultati

Il robot si perde e la traiettoria l (bl ) è di d llreale (blu) è diversa da quella

desiderata (nero). I cerchi rossi indicano le letture del sensore.


EKFEKF

• Utilizzando un EKF è possibile costruire una stima dello stato robusta rispetto alle incertezze

• Il modello del robot è non lineare e quindi devo costruire i Jacobianidel modello del processo e del modello dell’osservazione ad ognidel modello del processo e del modello dell osservazione ad ogni step



EKF PredizioneEKF Predizione

• Considero il modello del processo senza rumore

⎟⎟⎞

⎜⎜⎛

−−+−−−+−

=−−= )1(sin)1()1()1(cos)1()1(

))1(),1(()( kkTvkykkTvkx

kukxfkx θθ

• costruisco il Jacobiano di f calcolato in

⎟⎟

⎠⎜⎜

⎝ −+ )1()()()()())(),(()(

kTkyf

ωθ

)1(),1(ˆ −− kukx

111

sin01⎟⎞

⎜⎛ −

⎟⎟⎟⎞

⎜⎜⎜⎛

∂∂∂∂∂

∂∂

∂∂

Tvfff

fyf

xf

θθ


))1(),1(ˆ|(333

222

100cos10)(

−−⎟⎟⎟

⎠⎜⎜⎜

⎝

=

⎟⎟⎟⎟⎟

⎠⎜⎜⎜⎜⎜

⎝ ∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

=

kukx

Tv

fyf

xf

fyf

xfkF θ

θ

θ

EKF PredizioneEKF Predizione

• Predico lo stato utilizzando il modello e aggiorno la covarianza

))1(),1(ˆ()(ˆ −−=− kukxfkx

)()()1()()( kQkFkkFk T +−Σ=Σ −



EKF AggiornamentoEKF Aggiornamento

• Costruisco il Jacobiano della funzione di uscita

⎟⎟⎟⎞

⎜⎜⎜⎛

+⎟⎟⎟⎞

⎜⎜⎜⎛

== )()(

)()(

))(()( kvkv

kykx

kxhky y

x

⎟⎠

⎜⎝

⎟⎠

⎜⎝ )()( kvk θθ

))(ˆ|(333

222

111

100010001

)(−

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

⎟⎟⎟⎟⎟⎟⎟⎞

⎜⎜⎜⎜⎜⎜⎜⎛

∂∂∂∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

=

kxhhh

hyh

xh

hyh

xh

kHθ

θ


• Costruisco l’innovazione e la sua covarianza

Cristian Secchi

⎟⎠

⎜⎝ ∂∂∂ yx θ

))(ˆ()()( −−= kxhkyke)()()()()( kRkHkkHkS T +Σ= −

EKF AggiornamentoEKF Aggiornamento

• Costruisco il guadagno di Kalman e aggiorno la stima e la sua covarianza

1)()()()( −−Σ= kSkHkkK T


−Σ−=Σ )())()(()( kkHkKIk



RisultatiRisultati

Il robot (blu) segue la traiettoria desiderata (nero)

La stima iniziale è molto incerta ma il filtro di Kalman


e la stima della posizione del robot (verde) è accurata

la rende sempre più accurata.

CONTROLLO DI ROBOT INDUSTRIALILaurea Magistrale in Ingegneria Meccatronica

ROBOTICA MOBILEROBOTICA MOBILE

Ing. Cristian SecchiTel. 0522 522235

e-mail: [email protected]://www.dismi.unimo.it/Members/csecchi

CRI08 - Robotica Mobile.ppt [modalità compatibilità]...Cristian Secchi Pag. 6 Principi di...

Documents

Transcript of CRI08 - Robotica Mobile.ppt [modalità compatibilità]...Cristian Secchi Pag. 6 Principi di...