Navigazione di Veicoli Autonomi

Transcript

1 Luca Baglivo Navigazione i Veicoli Autonomi Pianificazione e Controllo i Traiettoria Appunti per il corso i Robotica Spaziale per Ingegneria Aerospaziale

2

3 PREFAZIONE Lo scopo ella presente ispensa è quello i fornire una panoramica introuttiva elle teorie e applicazioni i base sulla pianificazione e il controllo i traiettoria ei robot mobili in particolare ei veicoli autonomi. Nell ambito trattato vi è un inevitabile coinvolgimento ello stuio ei sistemi non olonomi, e anzi proprio a causa i questa caratteristica ei robot mobili si è creato e si va ancora sviluppano un filone i ricerca che mira a fornire ei metoi generali per la navigazione ei robot non olonomi al pari i quelli forniti al più maturo stuio ei problemi inerenti i robot olonomi. E naturale, quini, fare riferimento ai robot manipolatori in quanto sistemi olonomi per comprenere le ifferenze i applicazione e i complessità elle teorie nell uno e nell altro campo. Desiero 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 i questa ispensa. SOMMARIO Dopo una necessaria introuzione sui sistemi non olonomi vengono escritti alcuni ei principali moelli cinematici i veicoli autonomi su ruote la cui costruzione è strettamente connessa all imposizione el vincolo i non slittamento elle ruote. Il moello cinematico el robot coniziona in moo stringente i task i pianificazione e controllo el moto. Si riportano alcuni metoi i pianificazione el moto che sfruttano la proprietà i nilpotenza i molti sistemi non olonomi. E escritto un metoo per la pianificazione i percorsi a curvatura continua che tengano conto ei vincoli non olonomi e ei vincoli meccanici e inamici el robot. Per il moello i tipo automobile è escritto un metoo per la pianificazione ei controlli a ciclo aperto. Ai controlli pianificati è necessario aggiungere i controlli i feeback che consentano i portare a termine il task correttamente anche in presenza egli inevitabili isturbi i misura, eviazioni alle conizioni iniziali previste e eviazioni al moello ieale. Si presenta un introuzione al controllo in feeback sia con approccio teorico (feeback statico linearizzato), sia con approccio euristico (algoritmi euristici i inseguimento).

4 INDICE. INTRODUZIONE Ambienti strutturati, semi-strutturati, non strutturati PRINCIPALI MODELLI CINEMATICI DI AGV Sistemi non olonomi Moello monociclo Moello a guia ifferenziale Moello a tre ruote e car-like Moello automobile con N rimorchi Sistemi olonomi e non olonomi Altri esempi i sistemi non olonomi PIANIFICAZIONE E CONTROLLO DELLA TRAIETTORIA Percorsi e traiettorie ammissibili Path following e Trajectory tracking Pianificazione el percorso e pianificazione el moto Metoi i pianificazione el moto per sistemi non olonomi Sistemi in chaine form Pianificazione i traiettorie open-loop Percorsi a curvatura continua. Clotoii Percorsi a curvatura polinomiale Controllo i traiettoria Controllabilità Feeback statico linearizzato Applicazione el feeback linearizzato al controllo i traiettoria rettilinea Metoi euristici per l inseguimento i percorso BIBLIOGRAFIA... 43

5 . INTRODUZIONE La robotica mobile si occupa ello stuio ei sistemi robotici in grao i spostarsi autonomamente in un certo tipo i ambiente per assolvere un eterminato compito e trova al giorno oggi molte applicazioni in svariati campi, al settore civile a quello inustriale, al militare all aerospaziale. Nell ambito ella robotica mobile lo stuio elle tecniche i navigazione autonoma è essenziale poiché si occupa el problema principale, quello i renere il robot in grao i localizzarsi, pianificare il proprio moto e controllarne l esecuzione. Il problema ella localizzazione vee la necessità i utilizzare egli strumenti i misura ella posizione che siano aatti all ambiente in cui il robot opera e i integrare più sensori in un sistema i localizzazione. Per ottenere un unica misura i posizione che integri in moo ottimo le singole misure ei vari strumenti, si utilizzano opportune tecniche che sono oggetto i stuio el più generale problema el sensor fusion []. Altro problema fonamentale ella navigazione autonoma è la pianificazione el moto, intesa a ue livelli: un livello alto i strategia globale che riguara il raggiungimento ell obiettivo finale, e, a esso funzionale, un livello più basso i pianificazione locale. A causa i inevitabili fattori i isturbo, è necessario, come per i robot manipolatori a maggior ragione anche per i robot mobili, un controllo in feeback el moto pianificato. A meno i poche eccezioni, al punto i vista cinematico i robot mobili su ruote sono sistemi non olonomi. I sistemi non olonomi sono caratterizzati a equazioni i vincolo sulle velocità elle variabili che escrivono il sistema. Queste equazioni non sono integrabili e tipicamente compaiono quano il sistema ha un numero i controlli inferiore al numero ei grai i libertà. A esempio un robot i tipo automobile ha ue controlli, la velocità lineare e angolare, mentre le variabili che escrivono l evoluzione el sistema sono tre, a esempio la posizione cartesiana i un suo punto e l assetto. La conseguenza ei vincoli non olonomi è che non tutte le traiettorie nello spazio elle variabili generalizzate el robot sono ammissibili. Per questo le tecniche ella geometria classica sviluppate per il moto ei sistemi olonomi quali i manipolatori, non sono applicabili a quelli non olonomi... Ambienti strutturati, semi-strutturati, non strutturati La strategia complessiva i pianificazione e controllo el moto ipene fortemente alla tipologia ell ambiente in cui il robot si muove. Si possono classificare tre macro-categorie i ambienti in base alle informazioni ell ambiente i cui si ispone: Ambienti strutturati: si ispone i una completa conoscenza topologica el layout egli spazi liberi e ella islocazione egli ostacoli oltre alla posizione el target. Un esempio tipico è quello i un ambiente logistico inustriale (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 al robot per portare a compimento il proprio task Ambienti semi-strutturati: la conoscenza ell ambiente non è completa poichè, a esempio non si ispone a priori i sufficienti informazioni sul target o perché è prevista una gestione flessibile el percorso a livello locale e in tempo reale per l aggiramento i ostacoli non conosciuti a priori. Il robot può pianificare il moto in uno spazio a esso circostante moificano il percorso già pianificato globalmente off-line. Ambienti non strutturati: non si ispone i informazioni sufficienti per pianificare globalmente il moto a priori. Il robot eve essere in grao i localizzarsi in tempo reale rispetto all ambiente e pianificare il moto in itinere per portare a termine il task (problema SLAM, Simultaneous Localization an Map Builing). 3

6 . PRINCIPALI MODELLI CINEMATICI DI AGV In questo capitolo vengono riportati i principali moelli cinematici i veicoli autonomi utilizzati nella robotica mobile. Questi moelli servono a escrivere l evoluzione nel tempo elle variabili i interesse el sistema, ossia i ciò che viene efinito lo stato el sistema nello spazio elle sue possibili configurazioni efinite al vettore i coorinate generalizzate o variabili i configurazione. Queste coorinate sono in numero pari a quello ei grai i libertà el sistema e possono avere o meno significato fisico. La costruzione ei moelli cinematici parte a una fonamentale ipotesi che li accomuna, quella i sussistenza el vincolo i non slittamento elle ruote. Questo vincolo appartiene alla classe ei vincoli non olonomi o anolonomi, ciò che matematicamente, nel caso ei robot mobili su ruote, si trauce in una restrizione ei possibili valori i moulo e irezione ella velocità e, i conseguenza, elle possibili traiettorie realizzabili a a partire a una certa configurazione. I moelli che rispettano questo vincolo sono ieali; in realtà sussistono sempre elle eviazioni al moello a causa ella stessa effettiva non iealità ei vincoli. In questa trattazione si focalizza l attenzione al moello i controllo el sistema consierato ieale. Inoltre, anche nell ipotesi in cui vengano rispettati, i vincoli introucono elle complicazioni nella pianificazione e nel controllo i traiettoria ei veicoli non olonomi... Sistemi non olonomi Si consieri il sistema meccanico la cui configurazione può essere escritta completamente al vettore i coorinate generalizzate q Q, appartenente allo spazio vettoriale reale Q i imensione n. Tipicamente il moto el sistema è soggetto a vincoli ovuti alla struttura stessa el sistema oppure al moo in cui è attuato o controllato. Tali vincoli possono essere bilaterali o unilaterali (rispettivamente espressi a equazioni o isequazioni) e possono ipenere o meno al tempo. Limitatamente ai vincoli bilaterali non ipenenti al tempo, le relazioni matematiche che esprimono il vincolo possono essere equazioni nelle coorinate generalizzate e/o nelle loro erivate (in tal caso il vincolo è etto ifferenziale). I vincoli etti olonomi possono avere la forma: h( q) =, i =,.., k < n ( -) i Le funzioni hi : Q si assumono continue e inipenenti e il sistema soggetto a tali vincoli è etto anch esso olonomo. La maggior parte ei manipolatori forniscono un tipico esempio i vincoli olonomi Un sistema soggetto a tali vincoli è costretto a restringere l insieme elle proprie configurazioni possibili a un sottospazio i Q i imensione n k.. La configurazione el sistema può essere escritta non più a n ma a n k nuove coorinate generalizzate che rappresentano i grai i libertà el sistema. A esempio, nel caso i una catena cinematica a base fissa costituita a ue elementi vincolati con ue coppie rotoiali (o prismatiche), una a telaio e l altra tra i ue elementi, il sistema privo i vincoli possiee n = m = grai i libertà ( m = 6 per ogni corpo libero nello spazio), mentre i vincoli olonomi elle coppie rotoiali (ciascuna impone 5 vincoli singoli) tolgono k = 5 = g..l. per cui la configurazione el sistema può essere completamente escritta a n k = g..l. I vincol etti cinematici incluono le coorinate generalizzate e le velocità. Nella forma lineare rispetto alle velocità generalizzate (etta i Pfaffian): a ( q) q =, i =,..., k < n, oppure T i 4

7 T A ( q) q = ( -) T La matrice A è costituita a k vettori inipenenti. Questi vincoli limitano i possibili movimenti el sistema restringeno l insieme elle velocità generalizzate che possono essere realizzate in una ata configurazione, il tipico esempio è quello i una ruota con il vincolo i non strisciamento riportato nel paragrafo seguente. E naturale che l esistenza i vincoli olonomi implica l esistenza i vincoli cinematici ottenuti per erivazione rispetto al tempo: hi q =, i =,..., q k Invece il contrario non è necessariamente vero. Accae infatti che i vincoli cinematici non sono integrabili, ossia non possono essere messi nella forma (-). A esempio, nel caso i un solo vincolo cinematico, se il vincolo è integrabile allora esiste una funzione hq ( ) tale per cui h/ q= a T ( q) e hq ( ) = c, ove c è una costante i integrazione legata alle conizioni iniziali q. Se i vincoli non sono integrabili, i vincoli e il sistema meccanico sono etti non olonomi o anolonomi. Un esempio tipico i sistema non olonomo è l automobile. Le velocità i un automobile sono vincolate perché le ruote non possono muoversi in irezione laterale. Perciò, la stessa automobile non può muoversi i lato né ruotare sul posto. Nonostante ciò, sappiamo che è possibile parcheggiare un autovettura in qualsiasi punto e con qualsiasi orientazione compatibilmente con la presenza i ostacoli... Moello monociclo δ y Figura : Moello uniciclo con quattro coorinate generalizzate. Si consieri un isco che può rotolare senza strisciare su un piano (Figura ), manteneno verticale il proprio piano meio. La configurazione è completamente escritta a quattro variabili: ue coorinate cartesiane (, y ) el punto i contatto con il terreno rispetto a un sistema fisso; l angolo δ efinisce l orientazione el isco rispetto all asse ; l angolo φ tra un asse raiale fissato sul isco e l asse verticale. A causa el vincolo i non-slittamento, le velocità generalizzate el sistema non possono assumere valori arbitrari. Infatti, inicano con R il raggio el isco, evono soisfare i vincoli Rφ cos( δ) = 5

8 y Rφ sin( δ) =, oppure in forma matriciale: cos T R δ A ( q) q = y = Rsinδ φ che chiaramente esprimono la conizione che la velocità el centro el isco si mantenga nel piano meio el isco, ossia risulti sempre tangente alla traiettoria. Da questi vincoli ifferenziali non è possibile ottenere una relazione sulle coorinate generalizzate, il numero i grai i libertà elle velocità el sistema è riotto a ue (4 meno ) ma non si possono riurre le coorinate 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 al isco. Infatti è possibile muovere il isco a una configurazione [, y, δ, φ ] a una configurazione [, y, δ, φ ] attraverso la seguente sequenza i moto:. Si fa rotolare il isco portano il punto i contatto a (, y ) a (, y ) lungo una qualsiasi curva i lunghezza R( φ φ + kπ ), con k intero positivo. Si fa ruotare il isco attorno al proprio asse verticale a δ a δ. Chiaramente le curve possibili sono infinite. Dalle equazioni ei vincoli si evince subito il moello cinematico el monociclo che costituisce la base per qualsiasi tipo i moello ottenuto a vincoli i non slittamento: cosδ y = sinδ v+ w δ ( -3) Nel moello sono introotti i ue controlli v e w, rispettivamente la velocità el centro el isco e la velocità angolare el isco attorno all asse verticale, che sono in numero pari a quello ei grai i libertà sulle velocità. φ Figura : Disco vincolato su guia rettilinea. 6

9 Diverso è il caso, anche se quasi ientico, i un isco che rotola senza strisciare su i una guia rettilinea verticale che, chiaramente, genera un vincolo integrabile, quini olonomo, e ciò riuce il numero i coorinate generalizzate a ue a una poiché in questo caso la coorinata rettilinea e l angolo i rotazione φ sono legati a una relazione finita i proporzionalità tramite il raggio el isco. Matematicamente: = φr Perciò, integrano: = φr+ c ove c è una costante che ipene alle conizioni inziali. In questo esempio il vincolo singolo T sulle velocità è scritto nella forma i Pfaffian con a ( q) = [ R] e è possibile ottenere la T soluzione i h/ q= a ( q), che è hq ( ) = φr = c. Appare anche eviente come, in presenza i vincoli sulle velocità, è importante stabilire se essi siano integrabili o meno al fine i analizzare qual è lo spazio elle configurazioni raggiungibili al sistema, cioè se questo spazio è ristretto ai vincoli oppure no e in tal caso i vincoli sono non olonomi..3. Moello a guia ifferenziale ICR RC P L Y X δ b P v = [ y δ] b P R y O Figura 3: Schema el moello ifferential rive. Un classico sistema i locomozione per robot mobili è costituito a ue ruote motrici parallele, controllate in velocità o in accelerazione a ue motori inipenenti. Poiché ue ruote non sono sufficienti per un appoggio stabile, si aggiungono elle ruote folli per mantenere il veicolo con inclinazione costante rispetto al terreno. Preneno come riferimento il punto meio tra le ruote motrici, P v, la configurazione el sistema è efinita a tre coorinate: il vettore [, y ] che iniviua la posizione i P v e l angolo i assetto δ che efinisce la irezione istantanea i moto el veicolo e coincie con l orientazione elle ruote motrici. Detta b la istanza tra le ruote, vr, vl, v rispettamene le velocità ella ruota estra e sinistra e quella el punto i riferimento, il moello cinematico può essere scritto come 7

10 vr + vl cosδ vcosδ vr + vl y = sinδ = vsinδ ( -4) δ vr v v L R v L b b Il moello si ottiene semplicemente consierano la rotazione el veicolo attorno al proprio centro i istantanea rotazione (ICR) e esprimeno la velocità v el punto i riferimento in funzione elle velocità elle ruote estra e sinistra. Si riportano i seguito i calcoli per maggior chiarezza. Nel sistema i riferimento mobile soliale al robot, le componenti non nulle (lungo l asse ) ei vettori velocità sono: Pv v = = δ Rc t PR vr = = δ ( Rc + b/ ) t PL vl = = δ ( Rc b/ ) t Sommano e sottraeno membro a membro le ultime ue equazioni si ottiene: vr + vl v = v v = δb R L Il moello cinematico cambia a secona elle coorinate generalizzate e ei controlli che si scelgono, riguaro a questi ultimi si riportano ue esempi i moelli ello stesso sistema. Il primo è un moello inamico controllato irettamente utilizzano accelerazioni u e u elle ruote: vr + vl cosδ vr + vl y sinδ δ = ur u vr v + L L ( -5) v R b v L Il secono ha come controlli la velocità el punto meio tra le ruote motrici e la velocità angolare el veicolo, perciò: cosδ y = sinδ v+ w ( -6) δ R L 8

11 Il moello (-6) è formalmente ientico a quello ricavato per il monociclo. Come si verà nel paragrafo eicato, le caratteristiche i controllabilità e gli stessi vincoli el sistema cambiano al variare el moello che si consiera..4. Moello a tre ruote e car-like y ICR b/sin(α) α y ICR α Y X Y P v P v X b b O δ Figura 4: Moello a tre ruote O δ Figura 5: Moello automobile (car-like) y ICR α P v O δ Figura 6: Moello biciclo Si vuole costruire il moello i un veicolo a tre ruote, con ruote posteriori a asse fisso e ruota anteriore sterzante (Figura 4). Si utilizzano le coorinate generalizzate [ yδ,,, α ], ove (, y ) sono le coorinate cartesiane el punto meio ell asse posteriore (P v ), δ è l angolo che misura l assetto el veicolo rispetto all asse, α è l angolo i sterzata ella ruota anteriore misurato rispetto all asse i simmetria longituinale el veicolo. Innanzitutto si iniviuano i vincoli el sistema, risulta eviente che essi sono ue vincoli i 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 P v, infatti qualsiasi ruota che si trovi con il proprio asse i rotolamento allineato con l asse posteriore è vincolata a seguire lo stesso atto i moto rigio in moo soliale a quello ell asse e secono una velocità a esso ortogonale. Dette (, y ) le coorinate ella ruota anteriore si può scrivere: a vincolo non olonomo per la ruota anteriore sin( δ + α) y cos( δ + α) = a a vincolo non olonomo per la ruota posteriore a 9

12 sinδ y cosδ = Poiché = + bcosδ a y = y+ bsinδ a Il primo vincolo iventa: ( δbsin δ)sin( δ + α) ( y + δbcos δ)cos( δ + α) = = sin( δ + α) y cos( δ + α) δbcosα = Per cui la matrice ei vincoli è T sin( δ + α) cos( δ + α) bcosα A ( q) = sinδ cosδ Il rango ella matrice è costante per qualsiasi valore i δ e α e vale. Ciò implica che lo spazio elle soluzioni el sistema costituito ai ue vincoli, cioè elle velocità generalizzate ammissibili, ha imensione : possiamo controllare il sistema con ue grai i libertà sulle velocità. Esistono vari moi per ottenere il moello cinematico i un sistema soggetto a vincoli T cinematici, uno i essi è quello i partire all equazione ei vincoli Aq=, valutare il rango ella T matrice A, e calcolare il nucleo associato alla matrice. Tuttavia il risultato i questa operazione non porta alla scrittura i un moello con immeiato significato fisico perciò è preferibile ricavare il moello ai metoi cinematici ella meccanica. Si consieri la rotazione el veicolo attorno al proprio centro i istantanea rotazione (ICR); etto P a il vettore posizione ella ruota anteriore rispetto al sistema i riferimento fisso, R e R rispettivamente il raggio i curvatura in corrisponenza ella ruota anteriore e el punto i riferimento : P v ca cv v v a b = δrca = δ sinα b = δrc v = δ tanα Le granezze sopra ricavate esprimono rispettivamente la velocità ella ruota anteriore e el punto i riferimento e i loro segni ipenono alle convenzioni stabilite per i segni i δ e i b. Dalla prima equazione: δ = v a sinα b ( -7) Dalla secona equazione risulta anche l espressione ella curvatura κ Il eterminante el minore formato alle prime ue righe e alle prime ue colonne vale sinα, per α = i ue eterminanti ei minori formati alla terza colonna e rispettivamente alla prima e secona colonna non si annullano mai contemporaneamente per ogni valore i δ.

13 tanα δ = v = κv κ = R b ( -8) cv Perciò, voleno esprimere le velocità e ell assetto e ell angolo i sterzo: y in funzione ella velocità el punto i riferimento, b = vcosδ = δ cosδ = va cosαcosδ tanα b y = vsinδ = δ sinδ = va cosαsinδ tanα ( -9) In tal moo il moello cinematica el sistema, nella forma el sistema i controllo, il seguente: cos cos α δ cosαsin δ y = sinα u δ + u ( -) b α Dove i controlli in questo caso scelti, i più comunemente usati, sono la velocità i trazione ella ruota anteriore e la velocità i sterzata. Se invece il sistema è controllato tramite la velocità i trazione ell asse posteriore è sufficiente sostituire a u il controllo u' = ucosα e il moello iventa: cos δ sinδ y = tanα u' + u ( -) δ b α Come atteso, le prime ue equazioni el moello (-) sono quelle el monociclo ma in questo caso la velocità i variazione i assetto è ipenente alla velocità i sterzata. E anche importante notare che quest ultimo moello possiee una singolarità in α =± π / che corrispone alla situazione la ruota sterzante è isposta ortogonalmente all asse longituinale. Questa singolarità invece non compare nel moello (-) a trazione anteriore. Un altra importante consierazione è che i moelli ricavati, quano si prene come riferimento il punto meio elle ruote posteriori, sono valii non solo per il veicolo a tre ruote ma anche per il sistema quariciclo (tipo automobile). Infatti entrambi si possono sintetizzare in un sistema in cui le ruote, sia quelle sterzanti anteriori, che quelle posteriori, vaano a coinciere con ue sole ruote virtuali, una anteriore sterzante e l altra posteriore, poste sull asse i simmetria longituinale, ossia al sistema biciclo (Figura 6). Nel caso el sistema quariciclo le ruote sterzanti evono essere orientate in funzione ell angolo i sterzo α ella ruota sterzante virtuale, in moo a iniviuare un unico centro i istantanea rotazione el veicolo (Figura 5).

14 .5. Moello automobile con N rimorchi y δ N δ N- α δ δ = P v b b Figura 7: Moello automobile con N rimorchi Un moello più complesso i quello appena visto si ottiene aggiungeno N rimorchi a un robot triciclo o moello automobile con trazione posteriore. Il moello cinematicamente più semplice prevee che ciascun rimorchio sia agganciato nel punto meio ell asse posteriore el preceente. Si può scegliere come vettore i coorinate generalizzate quello el moello automobile a cui si aggiunge l angolo i assetto i ciascun rimorchio, misurato tra il proprio asse i simmetria e l asse (Figura 7): q= [, y, αδ,, δ,..., δ, δ ] T N N La imensione ello spazio elle coorinate generalizzate, il numero i grai i libertà, è perciò N+4. I vincoli non olonomi sono ue per la motrice e altri N vincoli per l asse posteriore ell i-esimo rimorchio, le cui coorinate si ottengono a : i i = bjcosδ j j= i yi = y bjsinδ j j= i =,.., N Gli N+ vincoli sono: vincolo per l asse anteriore ella motrice sin( δ + α) y cos( δ + α) = a a vincolo per l asse posteriore ella motrice sinδ y cos δ = vincoli per l asse posteriore ell i-esimo rimorchio sinδ y cosδ = i =,.., N i i i i Esprimeno i vincoli solo in funzione elle coorinate generalizzate si ottiene:

15 sinδ y cos δ = sin( δ + α) y cos( δ + α) δ b cosα = i sinδ y cos δ + δ b cos( δ δ ) = i =,.. N i i j j i j j= ( -) Anziché trovare il nucleo associato alla matrice ei vincoli per ottenere il moello cinematico el sistema i controllo, si possono scrivere irettamente le equazioni cinematiche: = vcosδ y = vsinδ v tanα δo = b δ = v sin( δ δ ) i i i i bi v = v cos( δ δ ) i =,.., N i i i i Dove v i è la velocità lineare ell asse posteriore (punto meio) ell i-esimo rimorchio. Infine il moello cinematico: y α δ cosδ sinδ tan α / b sin( δ δ) / b δ = cos( δ δ)sin( δ δ) / b δ cos( δ j δ j ) sin( δi δi ) / bi δ i δ cos( δ δ ) sin( δ δ ) / b i ( j= ) N ( j= ) N N N N N N + u u ( -3) I controlli restano ue come nel caso el moello automobile, la velocità i trazione posteriore la velocità i sterzata. u u e 3

16 .6. Sistemi olonomi e non olonomi SISTEMI OLONOMI SISTEMI NON OLONOMI Esempio: manipolatori con coppie rotoiali e/o Esempio: veicoli su ruote con puro rotolamento prismatiche Tipo i vincoli e numero i controlli applicabili N corpi + p vincoli geometrici semplici (es. N corpi+p vincoli geometrici semplici moto nel piano) N =6*N-p grai i libertà(g..l.) N = 6*N-p g..l. N velocità inipenenti N velocità inipenenti N g..l. + k vincoli cinematici integrabili N corpi + k vincoli cinematici non integrabili n = N -k g..l. n = N variabili i configurazione (v..c.) q i, i =,..n variabili i giunto (o coorinate q i, i =,..n variabili i configurazione (o generalizzate) coorinate generalizzate) n velocità inipenenti m = n-k velocità inipenenti m = n controlli non ifferenziali m < n controlli ifferenziali (cinematici, a partire alle velocità) Pianificazione el moto Traiettoria cartesiana esierata () t Traiettoria cartesiana esierata () t Calcolo la traiettoria nello spazio ei giunti: qt () = f( ()) t Calcolo le m velocità i controllo esierate v (t) = v = g( ( t)).7. Altri esempi i sistemi non olonomi I vincoli non olonomi possono genearasi a causa i iversi fattori, i natura fisica o legati al tipo i controllo, alcuni i essi sono:. Superfici che rotolano senza strisciare Esempi: oltre ai robot mobili su ruote, la manipolazione i oggetti con ita robotiche (Figura 9). Conservazione el momento angolare in sistemi multiboy Esempi: manipolatori flottanti nello spazio senza attuatori esterni robot saltanti bilanciati inamicamente (Figura 8) tuffatori e astronauti in sospensione aerea satelliti con stabilizzazione i assetto 4

17 Figura 8: Robot saltellante Figura 9: Mano robotica con 5 ita 3. Speciali compiti i controllo robot rionanti (a esempio con 3 g..l nel piano ma target senza specifica sull assetto) robot subacquei (6 coorinate generalizzate, 4 inputs i velocità) 3. PIANIFICAZIONE E CONTROLLO DELLA TRAIETTORIA Gli obiettivi ella pianificazione el moto si ifferenziano a secona che essa sia a alto livello o a basso livello. Gli obiettivi i basso livello sono le strategie i pianificazione el moto che consentono i conseguire gli obiettivi primari, i alto livello. Strategie i moto a basso livello possono essere: Percorsi geometrici Traiettorie, ovvero percorsi parametrizzati nel tempo Sequenze i comani i movimento basate sulle informazioni ei sensori i cui il robot ispone Mentre gli obiettivi primari possono essere: Muoversi verso il target senza colliere con ostacoli (ambiente qualsiasi) Costruire una mappa ell ambiente (non strutturato) Trovare un oggetto (in ambiente non strutturato o semi-strutturato) Il compito più ostico è quello ella pianificazione a basso livello i un percorso compatibile (ammissibile) con i vincoli cinematici non olonomi. Mentre per un robot omniirezionale (Figura ) qualsiasi percorso è ammissible, per i robot non olonomi è richiesto un certo grao i continuità nelle funzioni che efiniscono geometricamente il percorso. A esempio, i percorsi ottimi per il veicolo i Rees e Shepp [] preveono una iscontinuità nella curvatura (sulla quale si impone un limite superiore a causa el limite sull angolo i sterzata) e possono essere effettuati all asse posteriore i un veicolo sterzante solo a conizione che il veicolo si fermi nei punti i iscontinuità. Per questo tipo i veicoli, naturalmente, i percorsi con iscontinuità nella tangente non sono ammessi oppure, se pianificati, non possono essere effettuati nemmeno iealmente con errore nullo. Risulta anche importante, per l analisi sulla fattibilità elle traiettorie pianificate e sulla controllabilità el veicolo, la scelta el punto i riferimento per il quale il percorso e la traiettoria sono pianificati. 5

18 a) b) Figura : a) robot con quattro ruote sterzanti e motrici. 8 grai i libertà i controllo. b) robot con tre ruote, ciascuna può traslare trasversalmente senza strisciare. 3.. Percorsi e traiettorie ammissibili Lo spazio elle configurazioni i un robot mobile è efinito al minimo numero i parametri (coorinate generalizzate) che permettono i localizzare l intero sistema nel suo ambiente (a esempio il vettore [ yδ,,, α ] nel caso el moello trattato nel.4). Come per i manipolatori si è soliti istinguere tra traiettorie cartesiane e traiettorie nello spazio ei giunti, per i robot mobili si efinisce traiettoria una ata funzione vettoriale continua nel ominio el tempo costituita a m funzioni scalari, tante quanti sono i controlli el sistema (a esempio velocità i trazione e angolo i sterzata). Una traiettoria è ammissibile se e solo se, ate le conizioni iniziali e finali ella configurazione, è una soluzione el sistema i equazioni ifferenziali che costituiscono il moello cinematico el robot. Un percorso è ciò che nello spazio elle configurazioni corrispone a una ata traiettoria. In altri termini, se si applica una ata legge i moto (traiettoria) al robot, ciò che ne risulta nello spazio cartesiano è un percorso. Un percorso ammissibile corrispone a una traiettoria ammissibile. Le relazioni matematiche che intercorrono tra percorso e traiettoria non sono banali per un sistema non olonomo proprio a causa ella non integrabilità el sistema i equazioni ifferenziali el moello cinematico. Infatti, a esempio, per il moello automobile in cui il vettore elle variabili i configurazione è [ yδ,,, κ ] e è costituito alle coorinate cartesiane (, y ) el punto meio ell asse posteriore, all assetto δ e alla curvatura κ, si ha: t () = vt ()cos δ () t t () cos δ ( t) yt () = vt ()sin δ () t yt () sin δ ( t) vt () = + ut () () δ() t κ() t v() t δ () t κ t = κ () t κ () t = u() t ( 3-) I controlli sono la velocità ell asse posteriore, v, e la variazione i curvatura rispetto al tempo, u. Voleno passare alla variabile temporale a t alla coorinata curvilinea s è sufficiente tenere presente che: s vt () = t δ () s s κ() s = δ() t = κ() t = κ() t v() t s t 6

19 Per cui il moello iventa: () s = cos δ () s ys () = sin δ () s δ() s = κ() s κ () s = u() s Se infine si integrano le equazioni el moello si ottiene s () s = cos δ() s s ; δ() s = κ() s s s s y() s = sin δ() s s ; κ() s = u() s s s ( 3-) Queste equazioni, pur esseno integrabili in forma numerica e utilizzate come moello per la ricostruzione oometrica (noti che siano i controlli applicati) non sono una soluzione el sistema i equazioni ifferenziali in senso classico in quanto l assetto, che è una elle variabili i configurazione (stato) el sistema, compare all interno egli integrali; in altre parole per ottenere la soluzione el sistema, ate le conizioni iniziali e finali, si ovrebbe integrare una elle incognite. 3.. Path following e Trajectory tracking Un ulteriore efinizione i traiettoria è quella i percorso a cui viene associata la legge temporale con cui il percorso viene seguito al robot. Questa accezione viene utilizzata per istinguere tre classi i problemi in base al compito assegnato al robot: Moto punto-punto Inseguimento i percorso (path following) Inseguimento i traiettoria (trajectory tracking) Moto punto-punto? q i =( i,y i,δ i,α i ) Figura : Moto punto-punto. Non ci sono specifiche sulla traiettoria ma solo sulle configurazioni iniziale e finale Il robot eve arrivare a una posa (posizione e assetto) finale parteno a una ata posa iniziale. In generale eve raggiungere una configurazione finale ata una configurazione iniziale nello spazio elle coorinate generalizzate. Dal punto i vista el controllo il problema è quello i 7

20 stabilizzare il robot in un punto i equilibrio nello spazio elle variabili i configurazione. Per un veicolo tipo automobile sono isponibili solo ue controlli, a esempio velocità i trazione anteriore e velocità i sterzata, per moificare le quattro variabili i configurazione ( yδ,,, α ). La situazione resta la stessa anche per un veicolo con N rimorchi in cui si hanno ue input e N+4 variabili i configurazione. Un eventuale retroazione sul controllo utilizza come errore la ifferenza tra la configurazione corrente e quella esierata. Inseguimento i percorso s q i =( i,y i,δ i,α i ) Figura : Inseguimento i percorso. L'obiettivo è annulare la istanza alla traiettoria inipenentemente al tempo Il robot eve raggiungere e seguire un percorso nello spazio cartesiano parteno a una ata configurazione iniziale entro o fuori al percorso. L algoritmo i controllo è basato sulla escrizione geometrica ella traiettoria cartesiana, usualmente parametrizzata secono la coorinata curvilinea s. La legge temporale i controllo non è specificata poiché l obiettivo principale è quello i avvicinare il più possibile il veicolo alla traiettoria riucenone la istanza a essa. Perciò l evoluzione el parametro s nel tempo può essere scelta arbitrariamente e i ue input possono essere scalati rispetto al tempo senza cambiare il percorso. Una scelta può essere quella i mantenere costante l input i velocità i trazione o i variarla secono una qualsiasi legge temporale, e i effettuare il controllo sull inseguimento ageno solo sull angolo i sterzata. Inseguimento i traiettoria e(t) () t ( t ) ( t ) et () = [ e (), t e ()] t y t q(t)= ((t), y(t),δ(t),α(t)) Figura 3: Inseguimento i traiettoria. Il compito è i inseguire il percorso secono una legge temporale imposta 8

21 In questo caso il robot eve raggiungere e seguire una traiettoria nello spazio cartesiano parteno a una ata configurazione iniziale entro o fuori alla traiettoria, ovvero un percorso con una legge temporale imposta. Si può pensare che il robot ebba inseguire e raggiungere un robot virtuale che si muove lungo il percorso esierato con velocità variabile secono una legge esierata. Dal punto i vista el controllo, l obiettivo è quello i minimizzare l errore cartesiano et () tra la posizione effettiva el robot e quella prevista per ogni istante i tempo t Pianificazione el percorso e pianificazione el moto Per quanto etto, la pianificazione non olonoma el moto corrispone alla pianificazione i traiettorie ammissibili e può essere assimilata al problema ella pianificazione ei controlli a ciclo aperto per ottenere un ato percorso, ciò che nel caso ei manipolatori corrispone a un problema i cinematica inversa. Il problema, a monte, è quello i pianificare un percorso compatibile sia con i vincoli non olonomi che con la presenza i ostacoli potenzialmente interposti tra il punto i partenza e quello i arrivo. Questo problema applicato ai veicoli autonomi è etto i steering. In ambiente strutturato la pianificazione el percorso e el moto può essere calcolata a priori a livello globale, ossia inclueno il task completo (Figura 4). Tuttavia sussiste l esigenza i pianificare in tempo reale il percorso a livello locale (in un certo intorno el robot) per permettere al robot i aggirare ostacoli imprevisti (Figura 5) o inforcare un pallet in posizione non nota a priori. Per un moto punto-punto è sufficiente risolvere in moo iretto il problema calcolano una legge i moto che consenta i guiare il robot alla configurazione i partenza a quella i arrivo. In tal moo il percorso risulta essere un output ell algoritmo i pianificazione e non può essere imposto come input, non ci sono vincoli costituiti a ostacoli fisici ma bisogna tenere comunque conto egli ostacoli virtuali, come a esempio la limitazione sull angolo i sterzata ( α < α π /). La trasformazione elle coorinate generalizzate in coorinate chaine form ma (vei 3.4.), quano il sistema sia trasformabile, consente i risolvere agevolmente il problema. Figura 4: Esempio i pianificazione el percorso per un veicolo con rimorchio. Figura 5: Pianificazione el percorso e aggiramento ostacoli imprevisti. Se invece è richiesto che il robot segua un ato percorso (ammissibile, libero a ostacoli, ottimizzato per il minimo tempo i percorrenza, minima accelerazione angolare etc.) è necessario seguire una elle seguenti strategie: A. Inseguire il percorso o la traiettoria cartesiana con algoritmi euristici (vei 3.5.4) B. Pianificare il moto con metoi teorico-matematici (vei 3.4) calcolano la legge i moto esierata 9

22 C. Integrare il metoo B. con un algoritmo i controllo retroazionato sulla legge i moto pianificata per tenere conto ei possibili errori i inseguimento e riurli (vei 3.5.) Metoi i pianificazione el moto per sistemi non olonomi Sistemi in chaine form. Esistono forme canoniche per i moelli cinematici che sono funzionali allo sviluppo i strategie i controllo sia a ciclo aperto che retroazionato. La forma canonica più utilizzata è la forma a catena o chaine form. Un sistema a catena è nella seguente forma: = u = f( ) u 3 = f3(, ) u p = fp(,,.., p ) u m Dove i i sono le variabili i configurazione el sistema e Σ imi = n, la somma elle loro imensioni è pari al numero i grai i libertà. Per un sistema i controllo con ue input la forma canonica è etta a forma a catena singola (, n ): = u = u 3 = u n = u n ( 3-3) Dove si è supposto che tutte le i siano granezze scalari. L input u è chiamato input generatore e le variabili e sono chiamate variabili i base. Si nota che se u è costante (o costante a tratti), il sistema è lineare (o lineare a tratti). Si imostra che i sistemi nella forma (, n ) a catena sono completamente controllabili. Essi sono caratterizzati a una particolare proprietà etta nilpotenza [4]. Esistono conizioni necessarie e sufficienti perché un sistema a ue input sia trasformabile in chaine form tramite:. un cambio i coorinate = φ( q). una trasformazione invertibile egli input v = β ( q)u. Si imostra che un sistema non olonomo con m = input e n = 3,4 coorinate generalizzate può sempre essere messo nella forma a catena. Inoltre i moelli ei veicoli con N rimorchi, se con ciascun rimorchio incernierato nel punto meio ell asse posteriore el preceente, possono essere trasformati in chaine form. Nel caso in cui vi sia un offset nella cerniera, solo moelli i veicoli con un numero i rimorchi minore i ue possono essere trasformati in chaine form. Per esempio, si consieri il moello cinematico (-3) el monociclo. Utilizzano la trasformazione i coorinate

23 = cosδ + ysinδ = sinδ + ycosδ 3 = δ e la trasformazione i input v = u + u = ( sinδ + ycos δ ) u + u v = u 3 è immeiato verificare, ricorano che = v cosδ e y = v sinδ che il sistema trasformato risulta nella forma canonica a catena. E interessante notare inoltre che le nuove variabili e 3 sono semplicemente le coorinate cartesiane el monociclo espresse nel sistema i coorinate soliale al robot e orientato con l asse allineato con l asse rispetto a cui è misurato l assetto el veicolo. Per quanto riguara il moello triciclo o, equivalentemente, automobile, si può utilizzare il seguente cambio i coorinate: = tanα = bcos δ = tanδ = y ( 3-4) e la trasformazione i input u v = cosδ u v = 3sinδ sin α + bcos δ cos αu bcos δ 3 ( 3-5) per cui il sistema assume la forma a catena = u = u 3 = u 4 = u 3 ( 3-6) In questo caso la trasformazione i coorinate, e quini la forma a catena, è efinita per δ π / ± kπ, k. Si verifica che, come preveibile, anche il sistema a catena singola è non olonomo ma in questo caso si possono utilizzare proceure opportune per ottenere classi i soluzioni al problema ella pianificazione el moto punto-punto, ovvero traiettorie (leggi i moto) che guiano il robot a una configurazione i partenza a una configurazione i arrivo assegnate. E possibile utilizzare ifferenti tipi i ingressi ( ut ( ) ): ingressi sinusoiali ingressi costanti a tratti

24 ingressi polinomiali Ingressi sinusoiali Sceglieno per i ue input u = a + a sin( ωt) u = b + b cos( ωt) + b cos( ωt) b cos[( n ) ωt] n ( 3-7) si generano n + incognite per variare n coorinate generalizzate a una ata conizione iniziale a una ata conizione finale in un ato tempo T. Integrano (a partire alla prima equazione a cascata fino all ultima) le equazioni ifferenziali el moello (3-3) in cui si sostituiscono gli ingressi nella forma sinusoiale (3-7), si ottiene un sistema lineare che ha come incognite gli n + coefficienti. Si imostra [7] che, se a, il sistema è invertibile. Nel caso specifico el moello (3-6) si hanno quattro coorinate e cinque parametri ( a, a, b, b, b). Una possibile proceura risolutiva [8] è la seguente - si calcolano le coorinate trasformate iniziali e finali - si integrano le equazioni in cascata a partire alla prima nell intervallo i tempo T = π / ω - si sceglie un valore i a - si risolve il sistema calcolano gli altri quattro coefficienti ( a, b, b, b ). Al variare el tempo imposto T e el parametro, varierà la geometria el percorso pianificato. In Figura 6 è riportato un esempio i applicazione (utilizzano il coice i calcolo simbolico Mathematica ) con ue iversi valori i. Inoltre la geometria el percorso non è invariante alla a rotazione, ossia ipene all assetto iniziale e finale δ e δ f e non solo alla ifferenza δ δ f. a goal goal start start Figura 6: Pianificazione el moto con le stesse conizioni inziali e finali ma con iversi valori i a Ingressi costanti a tratti Si suivie il tempo totale i moto T in sottointervalli i lunghezza ue input sono costanti Δ in ciascuno ei quali i u ( τ ) = u k u ( τ ) = u k con τ [( k ) Δ, kδ ]

25 Se si mantiene u sempre costante e si prenono n sottointervalli ( k =,.., n-) in moo che ( n ) Δ = T e u = f T ove n è il numero i coorinate generalizzate, f e sono rispettivamente il valore finale e iniziale ella variabile, gli n valore costanti ( u,, u,,.., u, n ) ell input u si ottengono risolveno un sistema lineare triangolare che consegue all integrazione elle equazioni el moello a catena. Nel caso in cui le ascisse el punto i partenza e i arrivo coinciono è necessario aggiungere un punto intermeio, inoltre non è possibile effettuare manovre automatiche con inversione ella velocità. 4 Traiettoria goal 3 start 3 Figura 7: Traiettoria ottenuta con input costanti a tratti mês Velocità motoruota vs Tempo seconi ra Angolo i sterzo vs Tempo seconi Figura 8: Input i velocità ella motoruota Figura 9: Comano angolo i sterzata. Presenta iscontinuità i velocità (tangente al grafico) In Figura 8 e in Figura 9 risulta eviente come gli input i velocità i trazione e i angolo i sterzata siano elle funzioni continue ma le conizioni iniziali e finali su v e α sono libere in quanto non vengono imposte nelle equazioni risolutive. Inoltre la velocità i sterzata (erivata el grafico i Figura 9) può presentare elle iscontinuità. 3

26 Ingressi polinomiali Un algoritmo simile a quello egli input costanti a tratti, ma con migliori prestazioni nel livello i continuità ( smoothness ) elle funzioni i input, è quello i utilizzare ei polinomi negli input. Sceglieno per i ue input: u = sign( ) f n = n u c ct c t Come tempo totale si può scegliere T = f Si otterrà un sistema lineare nelle incognite ( c, c,.., cn ) ottenuto integrano le equazioni el moello a catena, el tipo c AT ( ) (, ) u f c u f 3 + f ui T = c n u f n Dove la matrice AT ( ) è invertibile per T Nel caso in cui le ascisse el punto i partenza e i arrivo coinciono è necessario aggiungere un punto intermeio, inoltre non è possibile effettuare manovre automatiche con inversione i velocità Pianificazione i traiettorie open-loop I metoi finora visti consentono i calcolare un percorso ammissibile e i relativi controlli per portare un veicolo a una configurazione i partenza a una i arrivo (moto punto-punto). Per contro, non è possibile preveere esattamente quale sarà il percorso pianificato e ciò rene ancora più complesso il compito i far muovere il robot in un ambiente enso i ostacoli. E pertanto preferibile, se l ambiente è strutturato, pianificare un percorso libero a ostacoli e che sia anche ammissibile, non solo per quanto riguara i vincoli cinematici ma anche per ciò che concerne i limiti sui controlli (angolo massimo i sterzata, velocità massima i trazione e i sterzata, accelerazioni etc.). Per il moello automobile a trazione posteriore (-) è possibile calcolare gli input i velocità i trazione e i sterzata (traiettoria i input) in funzione el percorso cartesiano esierato, assumeno che questa sia una traiettoria ammissibile e con un certo grao i continuità [5]. E sufficiente imporre le coorinate cartesiane esierate ( (), t y() t ) in funzione el tempo a partire a un certo istante iniziale t. Dalle coorinate cartesiane è possibile ricavare le restanti variabili i configurazione esierate (assetto e angolo i sterzata) e i comani i input associati. Ciò è possibile perché a causa ei vincoli non olonomi il veicolo è obbligato a avere velocità e assetto tangenti alla traiettoria cartesiana e inoltre l angolo i sterzo è legato geometricamente alla curvatura el percorso e la velocità i sterzata è legata all accelerazione angolare esierata. Tutte queste variabili possono essere ottenute matematicamente a consierazioni geometriche e alle erivate rispetto al tempo ella traiettoria cartesiana. La traiettoria cartesiana esierata (percorso geometrico in funzione el tempo) è ammissibile quano può essere ottenuta come risultato ell evoluzione nel tempo ella configurazione el sistema secono le equazioni el moello cinematico ell automobile: 4

27 = cosδ v ( 3-8) ( 3-9) δ y = sin v tanα v δ = b ( 3-) α = v ( 3-) con input v t e v () t almeno continui a tratti. () v Si può ricavare alle (3-8) e (3-9) v () t =± () t + y () t il cui segno ipene alla scelta ella irezione el moto, in avanti o all inietro. Divieno la (3-9) per la (3-8) si ottiene l assetto esierato () t y () t δ () t, ( 3-) = ATAN v() t v() t Derivano la (3-8) e la (3-9) rispetto al tempo e moltiplicano la prima per y, la secona per e sottraeno membro a membro si ottiene () t y() t () t y () t δ () t = ( 3-3) v () t Il risultato appena ottenuto si può sostituire nella (3-) e ottenere così l angolo i sterzo il cui segno ipene al segno ella velocità i trazione α ( t) arctan () t y() t () t y () t ( 3-4) = b 3 v() t Infine per ottenere l input ottenuto: v è sufficiente erivare rispetto al tempo l angolo i sterzata appena v bv [( y y ) v 3( y y )( + y y )] () t = 6 v + b ( y y ) Le equazioni appena ricavate legano biunivocamente la traiettoria cartesiana alla traiettoria i input, ossia permettono i ottenere i controlli in funzione el tempo a ciclo aperto in seguito ai quali il robot segue il percorso esierato con la legge temporale imposta. Il controllo a ciclo aperto richiee che il robot si trovi esattamente nella configurazione iniziale prevista ( ( t), y( t), δ( t), α ( t) ) perché la traiettoria in uscita al sistema i controllo sia effettivamente quella esierata. Dalle equazioni risulta che gli input ipenono solo alla traiettoria cartesiana esierata in uscita e alle sue erivate fino al terzo orine. Perciò per garantire l esatta riproucibilità ella traiettoria esierata è necessario, oltre alla conizione suetta sulla configurazione iniziale, che la traiettoria sia erivabile tre volte rispetto al tempo. 5

28 E importante tenere conto che gli input contengono una singolarità, ossia non sono efiniti nel caso in cui v ( t ) = per qualche t t. Per questo inconveniente si utilizzano traiettorie cartesiane parametrizzate in funzione i un parametro geometrico el percorso, in tal moo la escrizione geometrica el percorso è separata a quella temporale. Se si inica con σ il parametro i percorso (quello più utilizzato è la coorinata curvilinea s ), la pianificazione ella traiettoria cartesiana implicherà una legge temporale σ ( t) che si può introurre nell equazione ella traiettoria esprimeno le coorinate cartesiane in funzione i σ : () t = ( σ ()) t = ( σ()) t σ() t = ( σ) σ() t t σ y () t = y ( σσ ) () t ( 3-5) (si utilizza l apice per istinguere la erivata rispetto al parametro i percorso a quella rispetto al tempo). La velocità lineare i input ella traiettoria nelle coorinate parametrizzate è etta pseuo-velocità e in ato punto corrisponente a un ato σ vale w =± + y σ ( 3-6) ( ) ( ) ( ) σ σ mentre la velocità i comano effettiva (che si ricora essere la velocità lineare el punto i riferimento el moello automobile, ossia il punto intermeio ell asse posteriore) vale v () t = w ( σ () t σ () t ( 3-7) y v w () s = v () t = s () t s Figura : Velocità i trazione in funzione ella coorinata curvilinea. La pseuo velocità è il moulo el versore tangente alla traiettoria, perciò sempre unitaria. Se si sceglie σ () t = s() t, le erivate () s = ()/ s s e y = y()/ s s sono le componenti cartesiane el versore tangente alla traiettoria nel punto corrisponente a s e perciò la pseuovelocità, che è il moulo i questo versore, avrà valore unitario, quini sempre iverso a zero. La conizione per cui v ( ) t = si ottiene quano σ ( t ) = mentre w ( ) σ. L espressione ell assetto esierato in funzione el parametro i percorso è sempre efinita senza punti singolari: ( σ) y ( σ) δ ( σ) = ATAN, w( σ ) w( σ ) 6

29 Grazie alla separazione tra ipenenza al tempo e ipenenza allo spazio anche le formule i α ( σ ) e w ( ) σ (con v() t = w( σ ) σ () t ) risultano senza singolarità poiché compaiono solo informazioni geometiche relative alla curvatura el percorso e alle erivate i orine superiore rispetto al parametro σ. Infatti ( σ ) y ( σ) ( σ) y ( σ) α ( σ) = arctan b 3 w ( σ ) w bw y y w y y + yy ( σ ) = 6 w + b ( y y ) [( ) 3( ' )( )] L espressione egli input in funzione ella traiettoria cartesiana el moello i robot automobile a trazione posteriore (per quella anteriore la trasformazione i input è immeiata e è funzione solo ell angolo i sterzata) si può ottenere anche per il moello a catena (,4). L evoluzione temporale ella traiettoria esierata è escritta al moello cinematico in chaine form = u = u = u = u ( 3-8) Dalla traiettoria esierata in coorinate cartesiane ( (), ()) t y t e al cambio i coorinate (3-4) è possibile ottenere la traiettoria esierate nelle variabili trasformate in funzione i quelle cartesiane e elle loro erivate () t = () t y () t () t y () tt () () t = 3 () t y () t 3() t = () t () t = y () t 4 e anche l espressione egli input trasformati u () t = () t y y y y () t = 4 u 3( ) Percorsi a curvatura continua. Clotoii. E noto, a partire agli stui i Dubin e Rees & Shepp, che i percorsi ottimi in termini i lunghezza, per un moello automobile semplificato (tre variabili i configurazione controllato in velocità angolare) sono costituiti a segmenti e archi i circonferenza. Uno svantaggio notevole i questa classe i percorsi è la presenza i iscontinuità ella curvatura. Queste iscontinuità si presentano in ogni transizione tra segmenti e archi in quanto la curvatura salta a zero a un valore non nullo. Poiché la curvatura è legata all angolo i sterzata alla relazione 7

30 tanα κ = b se un veicolo effettuasse un inseguimento i percorso, per eseguire esattamente senza errori il percorso pianificato ovrebbe arrestarsi a ogni iscontinuità i curvatura per poter orientare le ruote anteriori (la ruota anteriore nel caso i moello triciclo). E preferibile, perciò, che il percorso sia caratterizzato alla continuità nella curvatura. E poi necessario, a causa ei limiti meccanici sull angolo i sterzata, che la curvatura sia limitata. Inoltre, poiché l accelerazione angolare, ossia la velocità i variazione ella curvatura, è legata alla velocità i sterzata el veicolo, è preferibile che la erivata ella curvatura sia limitata superiormente per assicurare che il veicolo esegua la traiettoria con una certa velocità i trazione (proporzionale al limite sulla erivata ella curvatura. Il problema i pianificare percorsi a curvatura continua fa parte ella classe ei problemi etti i path smoothing, in cui si cerca i smussare le iscontinuità a partire a un percorso nominale, a esempio una linea poligonale, o a una sequenza i configurazioni i passaggio per ottenere una curva smooth. Le curve utilizzate si possono iviere in ue categorie:.. con coorinate aventi un espressione in forma chiusa, a esempio: a. B-splines b. Polinomi i quinto grao c. Spline polari. curve parametriche in cui la curvatura è funzione ella lunghezza arco (coorinata curvilinea s ):. Clotoii e. Spirali cubiche f. Spline G g. Spline intrinseche Curve parametriche alle caratteristiche interessanti sono i clotoi. Essi erivano alla scelta i una classe i percorsi in cui la curvatura varia linearmente con la lunghezza arco. Se si impone che la curvatura segua la legge lineare, δ κ = = λs + κ() ( 3-9) s con λ costante e κ () = c la curvatura iniziale, per il percorso cartesiano si ottiene = s s + = + + = + + = = s s + δξ ξ= + λξ + ξ s s + δξ ξ= + λξ + ξ ξ ξ δ() s δ κ() ξ ξ δ ( λξ c) ξ δ λs s () cos () cos c ys () y sin ( ) y sin c cs ( 3-) Supponeno nulle le conizioni iniziali, per conoscere la traiettoria cartesiana è necessario calcolare gli integrali 8

31 s = λξ () s cos s = λξ y() s sin ξ ξ Questo tipo i integrali non ha soluzione in forma chiusa ( y = g( ) ), essi sono noti come integrali i Fresnel. La forma canonica rispettivamente el coseno e el seno i Fresnel è ( ) cos π CF t t = π SF t t ( ) = sin ( 3-).8 Clotoie in forma canonica.6.4. S > y Figura : Grafico el clotoie in forma canonica, con parametro negativo (terzo quarante) e positivo (primo quarante). Il grafico si ottiene a una variazione lineare ella curvatura in s. Figura : Il grafico parametrico nello spazio (,y,s) è noto come spirale i Cornu e la sua proiezione nel piano (,y) genera un arco i clotoie. Si noti che il parametro costante λ è l accelerazione angolare espressa in termini geometrici, ossia è la erivata ella curvatura rispetto alla lunghezza arco. Gli integrali i Fresnel possono essere calcolati tramite approssimazione con sviluppo in serie i potenze oppure con metoi numerici. Come applicazione ella pianificazione el percorso tramite clotoii si consieri il moello (3- ). Il sistema ha ue controlli: la velocità i trazione elle ruote posteriori, v ; l accelerazione angolare γ che è irettamente legata alla velocità i sterzata α. Se si assume velocità v costante vale la seguente relazione δ tanα αv γ() t = = κv = v = ( 3-) t t b bcos α Mentre in funzione ella coorinata curvilinea: 9

32 δ κ γ() s = = = κ () s s s Si introuce un limite superiore alla curvatura e alla sua erivata κ κ ; γ γ ma ma Il limite sulla curvatura è legato al limite meccanico sull angolo i sterzata, relazione α ma, tramite la κ = ma tanα b ma mentre il limite sull accelerazione angolare è legato al limite sulla velocità i sterzata e eve essere scalato secono il moulo ella velocità v, infatti γ ma α = v bcos ma αma È possibile calcolare un valore i γ ma valio per v = e, se si sceglie un iverso valore v, è opportuno (necessario se v > ) imporre un nuovo valore γ ' ma = γ ma / v. In tal moo, imponeno un valore massimo per l accelerazione angolare, resta rispettato anche il vincolo sulla velocità massima i sterzata, poiché α α/cosα, per qualsiasi valore i α. Se si impone alla curvatura l anamento lineare escritto alla (3-9) valgono le relazioni: k αv α = = = = = s bcos α v bcos α γ kv v λv λ Da cui è possibile ricavare il valore massimo el coefficiente i curvatura, esso inica quanto rapiamente la curvatura el clotoie aumenta all aumentare ella lunghezza arco s γ λ = v ma ma I clotoii permettono i gestire irettamente la curvatura ella traiettoria cartesiana e si prestano a essere uniti con continuità geometrica a archi i circonferenza e segmenti rettilinei. 3

33 P3 μ Pf III Δ y C Pk P μ Pi I Pj II Δ β θ f P Figura 3: Elementi costruttivi ella traiettoria a curvatura continua. In rosso (linea continua) la curva, in vere (tratto-punto) la circonferenza i riferimento ella curva. I e III sono i tratti i clotoie, II è l eventuale arco i circonferenza. Si escrive un metoo [8] per costruire un percorso a curvatura continua costituito a segmenti, archi i clotoie e archi i circonferenza (Figura 3). Si supponga che il robot ebba effettuare un cambio i orientazione passano al segmento PP al segmento PP3 che forma con il primo un certo angolo θ f. Quest angolo è anche la variazione complessiva i assetto el robot. La curvatura iniziale e quella finale sono entrambe nulle poiché la curva è preceuta e seguita a ue segmenti. Se si raccorassero i segmenti solo con un arco i circonferenza (che abbia k k ma ) la curvatura avrebbe ue salti iscontinui passano a zero (sul segmento iniziale PP) a un valore non nullo costante (su tutto l arco i circonferenza) e poi nuovamente a zero (sul segmento finale PP3), ciò implicherebbe una variazione istantanea ell angolo i sterzo non fattibile a meno che il veicolo non si fermi nei punti i iscontinuità. Perciò si introucono ue tratti i clotoie necessari a portare la curvatura, in moo lineare con la lunghezza arco s, al valore nullo al valore massimo (primo clotoie) e poi, opo un eventuale arco circolare, al valore massimo al valore nullo (secono clotoie). Un anamento caratteristico ella curvatura è riportato in Figura 5. 3

34 Planne Continuous Curvature path [m] Curvature [m - ] II y[m] I planne simulate III [m] Figura 4: Curva i raccoro a curvatura continua. Percorso simulato s[m] Figura 5:Anamento ella curvatura el percorso in in funzione ella lunghezza arco. Confronto tra pianificazione e simulazione Gli archi circolari hanno raggio k ma mentre i clotoii hanno coefficiente i curvatura λ λ. La curva comincia al punto Pi, a cui corrisponono le variabili i configurazione ( s, ys, δ s, ), e termina nel punto Pf, a cui corrisponono le variabili i configurazione ( f, y f, δi + θ f,). Senza perere in generalità è conveniente utilizzare un sistema i riferimento relativo in moo tale che Pi (,,,) e assumere che la curva sia percorsa in senso antiorario. Il primo tratto ella curva consta i un arco i clotoie con coefficiente i curvatura λ ma e i lunghezza kma / λ ma, esso consente i passare a curvatura nulla, in Pi, a curvatura k ma, fino a raggiungere Pj. Le variabili i configurazione i Pj sono ma j = π / λ CF( k /( πλ yj = π / λ SF( k /( πλ Pj = δ j = kma /( λma ) κ j = kma ma ma ma ma ma ma )) )) Le prime ue equazioni si ottengono a un semplice cambio i variabile negli integrali canonici i Fresnel (3-). Il secono tratto ella curva consta i un arco circolare i raggio k ma che termina nella configurazione Pk = ( k, yk, δ k, kma ). Il centro C ella circonferenza i cui l arco fa parte si trova a istanza k ma a Pk in irezione ortogonale a δ k, e ha coorinate = k = + sinδ C j ma j δ yc yj kma cos j Il terzo e ultimo tratto ella curva consta i un arco i clotoie con coefficiente i curvatura λ ma e i lunghezza pari a quella el primo tratto, esso consente i passare a curvatura k ma, in Pk, a curvatura nulla, fino a raggiungere la configurazione finale Pf. 3

35 Mentre gli archi i clotoie hanno lunghezza costante poiché sempre caratterizzati a una variazione i curvatura kma con coefficiente i curvatura λ ma, l arco i circonferenza può variare la sua lunghezza e è esso che etermina la posizione finale i tutta la curva. La sua lunghezza si riuce a zero quano la eflessione totale θ f è pari a ue volte la variazione i assetto che si ottiene con ciascuno ei ue tratti i clotoie e cioè quano θ f k = θmin = λ ma ma Si osserva che il luogo elle configurazioni raggiungibili è una circonferenza con raggio r = + y C C e che l angolo tra la irezione i assetto ella configurazione finale Pf e la tangente alla circonferenza in Pf è costante e vale C μ = arctan( ) y C Se è richiesto i raggiungere una posizione finale con assetto δ f < θ, la curva sarà priva i un arco i circonferenza e consisterà in ue clotoii con coefficiente i curvatura calcolato in moo a ottenere la variazione i assetto esierata. Questo metoo può essere utilizzato, congiuntamente a altri opportuni algoritmi, per la pianificazione locale el percorso e consente i generare percorsi ammissibili a curvatura continua teneno conto ei vincoli sul massimo angolo i sterzata e sulla massima accelerazione angolare esierata. La caratteristica i curvatura continua imostra i seguenti vantaggi: il rispetto ei vincoli non olonomi e sulla velocità i sterzata massima consente i minimizzare eventuali slittamenti elle ruote otteneno così una maggiore accuratezza per la misura con encoer la continuità sulla curvatura e il limite superiore sull accelerazione angolare migliorano e renono più stabili le prestazioni i un eventuale sensore laser a triangolazione, la cui qualità ella misura ecae in presenza i brusche variazioni i assetto Percorsi a curvatura polinomiale min Figura 6: Spirale polinomiale ( o spirale i Cornu generalizzata) nello spazio (,y,s). La sua proiezione nel piano (,y) rappresenta il percorso cartesiano. In questo esempio la curvatura è un polinomio i terzo grao in s. La curvatura cambia i segno a circa metà percorso, il quale involve verso l origine. 33

36 Nell arco i clotoie la curvatura varia linearmente con la lunghezza arco s ks () = a+ bs Si faccia riferimento al moello (3-) senza la quarta equazione, in cui si utilizza come input la curvatura in funzione ella lunghezza arco. Questo moello ha il vantaggio i non over consierare la ipenenza al tempo. Per connettere ue configurazioni, iniziale e finale: q = (, y, δ, κ) q = (, y, δ, κ ) f f f f f se si consierano solo le variazioni relative i posizione (, y) e assetto (in un sistema i riferimento con l origine coinciente con il punto meio ell asse posteriore e l asse elle ascisse orientato come l assetto el veicolo la configurazione iniziale è (,,,κ )) si evono imporre cinque vincoli, ossia [ κ, f, yf, δ f, κ f]. Con un arco i clotoie si possono imporre tre soli vincoli, ossia [ abs,, f ] i ue coefficienti ell espressione lineare ella curvatura e la lunghezza totale el percorso s f. Quini con un arco i clotoie è possibile raggiungere la posizione finale esierata parteno alla curvatura iniziale imposta ma senza vincoli sull assetto e sulla curvatura finali (o sulla posizione finale). Piuttosto che utilizzare metoi articolati come quello visto in preceenza nel 3.4.3, si può aumentare il grao el polinomio ella curvatura [], aumentano così il numero i coefficienti incogniti con la possibilità i rispettare un maggior numero i vincoli sulle conizioni al contorno. Per ottenere una classe i percorsi ammissibili a curvatura continua che connettono è sufficiente utilizzare un polinomio i terzo grao ks () = a+ bs+ cs + s 3 qi con q f Figura 7: La erivata secona ella curvatura rispetto a s mostra che il percorso in alto a sinistra è ottenuto a una spirale polinomiale i terzo grao. I percorsi ottenibili appartengono alla classe elle cosiette spirali polinomiali. Il vettore ei parametri a calcolare sarà perciò p = [ abcs f ] Il coeffiente a si calcola semplicemente alla curvatura iniziale: a = κ (). Per il resto ei coefficienti è necessario, a partire alle conizioni al contorno, risolvere un sistema non lineare. Si 34

37 può isaccoppiare il sistema sostitueno l espressione in forma chiusa ell assetto (ottenuta analiticamente alle prime ue equazioni) negli integrali elle ue equazioni i posizione e y. Si ottiene il sistema 3 ks ( ) = a+ bs+ cs + s bs cs s δ ( s) = as s 3 4 bs cs s ( s) = cos( as ) s 3 4 s 3 4 bs cs s y( s) = sin( as ) s 3 4 in cui gli integrali sono etti integrali i Fresnel generalizzati. Questi integrali sono trascenenti e il loro calcolo eve essere eseguito con metoi numerici. Un vantaggio elle spirali polinomiali è che consentono i ottenere il grao i continuità esierato (a esempio sulla continuità ella velocità i sterzata ma anche sulla coppia i sterzo) a secona el grao el polinomo. Inoltre semplificano la soluzione el sistema grazie al isaccoppiamento elle equazioni sulla curvatura e sull assetto. Nello scegliere il percorso ottimo tra quelli calcolati è possibile minimizzare una funzione i costo esierata, un esempio può essere il funzionale s f Jk ( p) = [ κ ( p)] s 3.5. Controllo i traiettoria La pianificazione ei controlli in open-loop non è robusta rispetto alle eviazioni el sistema alle conizioni ieali previste (isturbi sulla misura, errori sulle conizioni iniziali, in accuratezza el moello) Il controllo retroazionato (feeback) sulla traiettoria pianificata migliora le prestazioni el sistema consenteno i portare a termine il task assegnato al robot anche in presenza i isturbi e i eviazioni alle conizioni iniziali previste. Per stimare lo stato el sistema, ovvero la posa el robot, si utilizzano misure provenienti a vari sensori in real-time. Per poter calcolare l input correttivo ella legge i feeback è necessario isporre a ogni istante i una stima ella configurazione attuale el robot. A tale scopo, tipicamente si ispone i strumenti propriocettivi (encoers, giroscopio) e eterocettivi (laser, ultrasuoni) le cui misure sono combinate assieme a un algoritmo i sensor fusion che rene a ogni istante isponibile la miglior stima (secono un ato criterio) ella posa el robot. In una tipica architettura i controllo un pianificatore a alto livello gestisce la pianificazione ei percorsi liberi a ostacoli e fornisce una serie i obiettivi al controllo i livello più basso. In tal moo il task i controllo ella traiettoria si occupa i trasformare il percorso ieale in una legge i moto eseguibile agli attuatori. Inoltre, se il percorso pianificato non è ammissibile rispetto ai vincoli non olonomi (iscontinuità nella tangente, nella curvatura etc.), l algoritmo i feeback consente i recuperare gli errori transitori ovuti a iscontinuità geometriche. Dal punto i vista el controllo, la pianificazione è equivalente al controllo a ciclo aperto o feeforwar, e costituisce una parte el controllo in feeback. Infatti un algoritmo i controllo a ciclo chiuso è costituito all unione ell azione i feeback a un termine i feeforwar. Quest ultimo è ottenuto alla conoscenza a priori ell ambiente e ell obiettivo el task, mentre l azione i feeback ipene alla posizione attuale misurata el robot e è calcolata in tempo reale 35

38 in base alle misure ei sensori i cui il robot è otato. Comunque la separazione tra strategie i controllo a ciclo aperto e a ciclo chiuso non è così netta: è possibile infatti scegliere i ottenere un controllo in feeback tramite una ripianificazione in tempo reale el percorso sulla base ella posizione attuale fornita ai sensori, ossia che tiene conto in tempo reale elle eviazioni alle conizioni nominali, teneno a ripristinarle Per i robot mobili tipicamente la fase i pianificazione termina con il calcolo i un percorso cinematicamente ammissibile e libero a ostacoli, a cui sono associati gli input i controllo a ciclo aperto. La ammissibilità è garantita se si tiene conto ei vincoli non olonomi el sistema. Inoltre è possibile, in fase i pianificazione, scegliere un percorso che soisfi un criterio i ottimo insieme ai vincoli sui controlli. Un possibile criterio i ottimo è la minimizzazione i una funzione i costo, a esempio la eterminazione el percorso libero a ostacoli che rene minima la lunghezza el percorso o la accelerazione angolare massima o una combinazione elle ue. Tra i vincoli sui controlli si eve tenere conto el massimo angolo i sterzata, ella massima velocità lineare e angolare. In ogni caso in questa fase i controlli sono calcolati off-line. Perciò se si verificano eventi imprevisti, come lo slittamento elle ruote o l errata localizzazione iniziale, questi inficiano la realizzazione corretta el moto punto-punto o ell inseguimento i percorso o i traiettoria La soluzione è quella i ricorrere a un controllo retroazionato sull errore stimato e ottenere così un certo grao i robustezza el controllo complessivo. Il controllo i feeback agisce solo nel caso in cui l errore sia non nullo, coaiuvano l azione ei controlli a ciclo aperto. Nel caso in cui l errore sia nullo e il robot si trovi esattamente nella configurazione esierata, agiranno solo i controlli a ciclo aperto. Un altra possibile soluzione è quella i non utilizzare i controlli a ciclo aperto e emanare a una legge i feeback l intero compito i controllo ell inseguimento. In tal caso l algoritmo eve essere tale a fornire i controlli necessari a effettuare il task i movimento anche nel caso in cui l errore sia nullo; si ha perciò un unico algoritmo in feeback rispetto alla posizione attuale e a quella esierata, che funge sia a pianificatore che a controllore Controllabilità Si è visto che all imposizione ei vincoli non olonomi i non slittamento si arriva irettamente alla costruzione i un moello el sistema che è non lineare, el tipo q = G q v = g v + g v + + g v ( 3-3) ( )... m m che rappresenta il moello cinematico el sistema e può essere utilizzato per lo stuio el controllo. q è il vettore elle n variabili i configurazione, v il vettore elle m velocità i input, con m< n. Le colonne gi ( i =,.. m) ella matrice G sono funzioni vettoriali continue elle variabili i configurazione. Il sistema (3-3) è etto senza eriva poiché v = q =. Se si sceglie una funzione vettoriale i controllo vt ( ) continua o continua a tratti e una conizione iniziale q() = q, esiste un unica soluzione el sistema per t, qt (,, q, v). Il sistema è controllabile se q, q fissati, T <, v( t) efinita nell intervallo [,T ] tale che qt (,, q, v) = q. Le caratteristiche ei sistemi non olonomi fanno si che il livello i complessità el compito sia inverso rispetto ai robot manipolatori. Infatti, per i manipolatori la stabilizzazione su una configurazione fissa è più semplice rispetto all inseguimento i traiettoria; ciò vale anche per qualsiasi sistema meccanico in cui il numero ei grai i libertà è uguale a quello ei controlli. Per un sistema non olonomo quale un robot su ruote, in cui è sempre m< n (sottoattuato), la complessità el compito è invertita. Dal punto i vista qualitativo ciò appare eviente se si consiera che il compito i controllo nel moto punto-punto è un problema i stabilizzazione attorno a una configurazione fissa e corrispone a controllare n g..l. con m = ingressi. Per il compito i 36

39 controllo nell inseguimento i percorso il problema è quello i controllare una sola uscita (la istanza al percorso) con m = ingressi (a esempio l angolo i sterzo, manteneno costante l altro input i velocità) e gli altri n g..l. vengono controllati inirettamente. Analogamente all inseguimento i percorso, anche nell inseguimento i traiettoria il numero i ingressi, m =, è uguale al numero i uscite controllate (gli errori cartesiani e e ey ), e gli altri n g..l. vengono controllati inirettamente. Perciò il problema più complesso è quello ella stabilizzazione su una configurazione fissa, in cui il numero i ingressi è inferiore al numero i uscite controllate, mentre sia per l inseguimento i percorso che per l inseguimento i traiettoria il numero i ingressi è uguale a quello elle uscite, si tratta cioè i problemi quarati. Se si effettua un analisi i controllabilità [7] è possibile verificare se i problemi su citati ammettono una soluzione approssimata con tecniche i controllo lineare. Nei sistemi lineari la controllabilità implica la stabilizzabilità attorno a una configurazione i equilibrio con un feeback continuo lineare sullo stato, el tipo q e vq ( ) = kq ( q e ) Se, inoltre, il sistema approssimato linearmente attorno a q δ q = Jδq+ Bδv δ q= q qe δv = kδq è controllabile, allora anche il sistema originario può essere stabilizzato localmente attorno a (o anche attorno a una traiettoria q ( t) ) con un feeback continuo. e q e Esistono iversi classi i feeback applicabili: Feeback continuo i stato, per sistemi lineari o linearizzati v = k q q ), ove è la configurazione esierata, anche variabile nel tempo ( q Feeback iscontinuo v = f( q), ove f è una funzione continua a tratti Feeback tempo-variante v = f( q, t), ove f è una funzione continua i q, ma ipenente al tempo. Nel caso ei sistemi non olonomi, se il sistema è linearizzato attorno a una configurazione fissa, il risultante sistema lineare non è controllabile. Ciò eriva irettamente al Teorema i Brockett: un sistema senza eriva con m vettori g i linearmente inipenenti è asintoticamente stabilizzabile con un feeback continuo i stato se e solo se il numero i controlli è maggiore o uguale al numero egli stati (variabili i configurazione), m n. Per contro, se si linearizza il sistema attorno a una traiettoria continua ne risulta un sistema lineare tempo-variante controllabile (poiché il numero i controlli è pari a quello elle variabili controllate) a conizione che la traiettoria soisfi eterminate conizioni sulla velocità i percorrenza (non sia mai nulla). Defineno vt ( ) = vt ( ) v ( t ) la variazione egli input e come qt ( ) = qt ( ) q ( t) l errore i inseguimento ( o i tracking), l approssimazione el sistema (3-3) a un sistema lineare attorno alla traiettoria i riferimento si ottiene nel moo seguente q = J() t q + B() t v ( 3-4) 37

40 ove la matrice n n J() t e la matrice n m B(t) si ottengono come Jt () = m i vi() t i= q q= q () t Bt () = Gq ( ()) t g ( 3-5) Feeback statico linearizzato Si escrive un metoo i controllo a ciclo chiuso per l inseguimento i traiettoria ottenuto all approssimazione lineare el moello automobile in forma a catena[5]. Si consieri il moello (3-6) a catena singola (,4) e si supponga i aver ricavato la legge i moto [ u(), t u() t ] a ciclo aperto per escrivere la traiettoria [ (), t (), t 3(), t 4() t ]. Si inichino rispettivamente gli errori i inseguimento e le variazioni ei controlli come: =, i =,..,4 i i i v = v v, j =, j j j Il sistema non lineare che escrive l evoluzione ell errore i inseguimento è: = u = u = u u = u u Linearizzano il sistema attorno alla traiettoria i riferimento secono le (3-5) si ottiene il sistema lineare con ipenenza al tempo: = ( ) u t + ( t) u = J() t + B() t u u( t) 3( t) Da un analisi i controllabilità (qui omessa) si vee che il sistema linearizzato è controllabile e perciò il sistema originario può essere localmente stabilizzato attorno a una traiettoria i riferimento tramite un feeback lineare. La matrice A ( ac t ) etta ell anello chiuso, tale che = Aac( t), inica come efinire la legge i feeback u ( t ) in moo che gli autovalori i Aac( t) siano costanti o comunque con parte reale negativa. Se si efinisce la legge i feeback u = k k k u = k u u La matrice ell anello chiuso iventa: 38

41 A ac k k k3/ u( t) k4 u t = / ( ) k () t u() t k 3() t u() t 3 e i suoi quattro autovalori sono k e le tre raici el polinomio caratteristico k4 + k3λ + kλ + λ. Pertanto, se k > e k, k3, k4 sono scelti in moo che il polinomio caratteristico sia un polinomio i Hurwitz, gli autovalori i A ( cc t) sono costanti e con parte reale negativa, il che permette i ottenere convergenza a zero ell errore i inseguimento i traiettoria. L espressione el controllo i feeback u mostra che esso non è efinito per u =. Il problema può essere risolto se si assegna agli autovalori, anziché un valore costante, una funzione i. A esempio si possono calcolare i coefficienti el polinomio caratteristico in moo che u abbia, oltre a, tre raici reali coincienti e pari a k γ u, con γ >. In tal moo si ottiene: 3 u = 3γ u 3γ u 3 γ u 4 ( 3-6) Con questo metoo (input scaling), il secono controllo i feeback, anziché a infinito, tene a zero quano la variabile ella traiettoria esierata tene a arrestarsi ( u = ). Il controllo complessivo el sistema in forma a catena sarà ato a u = u + u cioè consiste nella somma i un termine calcolato per l anello aperto, i fee-forwar, e i un termine i anello chiuso, i feeback. Per poter calcolare i controlli effettivi vt ( ) el moello automobile si eve utilizzare la funzione i trasformazione (3-5) e perciò la velocità i trazione e la velocità i sterzata saranno elle leggi i feeback non lineari e tempo-varianti Applicazione el feeback linearizzato al controllo i traiettoria rettilinea Si riporta l applicazione ell algoritmo i controllo con feeback statico linearizzato all inseguimento i traiettoria rettilinea con velocità v () t. Detti - v l input i velocità elle ruote posteriori - v l input i velocità angolare i sterzata - e y le coorinate cartesiane esierate, in coorinate relative rispetto alla traiettoria ( y ) - α l angolo i sterzo - δ l assetto el veicolo in coorinate relative rispetto alla retta a inseguire La traiettoria i riferimento in coorinate cartesiane relative è () t = v () t t y () t = Con input esierati 39

42 v () t = v () t v () t = La traiettoria in coorinate trasformate per un sistema a catena secono le (3-4) e (3-5), teneno presente che α ( t) =, δ ( t) =, è 3 4 () t = v () t t () t = () t = () t = Mentre gli input trasformati esierati sono u () t = = v () t u () t = Secono quanto visto nel paragrafo preceente la legge i controllo, utilizzano la (3-4) per esprimere le, 3, 4 in funzione i, y, è: u() t = u() t + u = v() t k( ) 3 γ v ( t) tanα 3 u() t = + 3 γ v ()tan () 3 t δ + γ v t y bcos δ Gli input effettivi complessivi, in base alla (3-5) sono perciò: v () t u () t cosδ = = v() t k( ) cosδ 3sinδsin ( α) u( t) 3 = + δ α v () t bcos cos u () t bcos δ Moificano il parametro γ la convergenza a zero ell errore i inseguimento è più o meno rapia e la stabilità ell algoritmo ipene alla istanza elle conizioni iniziali effettive alle conizioni nominali e al valore i γ. 4

43 Feeback i traiettoria su percorso rettilineo 4 3 y[m] [m] Figura 8: Simulazione el controllo i traiettoria per l allineamento su una retta. Esempio con conizioni iniziali: y = m; δ = 5 ; velocità =.4 m/s Assetto Veicolo [eg] time [s] Figura 9: Anamento ell assetto el veicolo urante la simulazione i Figura Metoi euristici per l inseguimento i percorso Per l inseguimento i percorso o i traiettoria è possibile ricorrere a algoritmi i controllo i tipo euristico. In tal caso le prestazioni el controllo non sono ottime in senso teorico-matematico e vengono valutate con prove i simulazione o sperimentali. L algoritmo viene valiato tramite una serie i test che permettano i verificare il comportamento el sistema in termini i convergenza e stabilità in un ato set i conizioni nominali. A titolo esemplificativo, un semplice tipo i controllo per l inseguimento i percorso consiste nel pianificare (e memorizzare in una matrice i ati) il percorso a far eseguire al veicolo attraverso una serie i punti i passaggio. Risulta eviente che la spezzata che unisce i punti i passaggio non è un percorso ammissibile per robot non olonomo la cui velocità sia sempre non nulla. Il percorso 4

44 effettivo che risulta all applicazione sarà perciò un percorso ammissibile che tene verso la spezzata i riferimento. Con riferimento alla Figura 3 a ciascun punto P i ( i,y i,r i ) è associata una terna i valori le cui componenti sono: posizione e y el punto rispetto a un sistema i riferimento fisso nell ambiente e R la istanza al punto al i sotto ella quale il riferimento passa al punto successivo. I riferimenti per il controllo sono: il punto già raggiunto, il prossimo punto a raggiungere e la loro retta congiungente, in moo che la traiettoria escritta sia simile alla spezzata che congiunge in moo sequenziale tutti i punti stabiliti. Se il veicolo viene controllato per inseguire il segmento P k- -P k e la istanza al punto scene al i sotto el corrisponente valore i R k il controllo può passare al segmento successivo P k -P k+. In particolare, pensano i utilizzare un veicolo tipo triciclo, il controllo agisce in moo a allineare lo sterzo el veicolo secono la irezione ella retta orientata che congiunge l asse ello sterzo con un opportuno punto situato sulla congiungente tra il punto P k e il punto P k+. P k P k+ R k+ R k Passaggio al controllo su P k+ STOP R P START P Figura 3: Simulazione i percorso con inseguimento La Figura 3 schematizza i parametri geometrici che regolano il controllo i traiettoria. P k è il punto a raggiungere imposto al pianificatore, P k- è il punto preceente che, connesso con il punto attuale, efinisce la retta i riferimento. 4

45 P R e R P S α e P P Sk e S P k- y P P Pk O Figura 3:escrizione ell algoritmo euristico. Il setpoint i sterzo è scelto in moo a puntare lo sterzo verso un punto appartenente al segmento i riferimento corrente e ipenente alla proiezione ella posizione ello sterzo su tale segmento. L angolo i sterzo viene controllato allineano lo sterzo secono la irezione ella retta che congiunge i seguenti ue punti: il punto in cui si trova l asse ello sterzo, P S, e un punto P P iniviuato proiettano sulla congiungente P k- P k il punto P Sk. Quest ultimo viene scelto sulla congiungente P S P k e la sua posizione può variare a P k a P S secono il fattore K che scala la istanza tra P S e P k : PP = KPP sk k s k e R, δ R e P, δ P e S versore ella irezione i assetto el veicolo, angolo formato al versore rispetto al sistema i riferimento assoluto versore in irezione el segmento P S P P, angolo formato al versore rispetto al sistema i riferimento assoluto versore in irezione el segmento P k- P k Tabella : parametri geometrici el controllo La legge i controllo sul punto P k e il segmento P k- P k etermina il riferimento per lo sterzo, calcolato come segue: α = δ P - δ R In Figura 3 è rappresentata una traiettoria simulata ottenibile con questo tipo i controllo. La velocità i trazione è il secono controllo e la sua legge i variazione può essere pianificata in moo inipenente. 4. BIBLIOGRAFIA [] L. Baglivo, M. De Cecco, Navigazione i veicoli autonomi. Sensor Fusion, ispensa iattica. [] J Boisonnat, A Cérézo, J Leblon, Shortest Paths of Boune Curvature in the Plane, Proc ICRA, pp. 35-3, Nice, France 99 [3] R. W. Brockett, Asymptotic stability an feeback stabilization", in Differential Geometric Control Theory, R. W. Brockett, R. S. Millman, H. J. Sussmann (Es.), Birkhäuser, Boston, MA, pp. 8-9,

46 [4] H Delingette, M. Herbert, an K Ikeuchi, "Trajectory Generation with Curvature Constraint base on Energy Minimization", Proc, IROS, pp 6-, Osaka, Japan, 99. [5] A. De Luca, G. Oriolo, C. Samson, Feeback control of a nonholonomic car.like robot, in Robot Motion Planning an Control (J. P. Laumon E.), Springer-Verlag, 998. [6] L. E. Dubins, On Curves of Minimal Length with a Constraint on Average Curvature an with Prescribe Initial an Terminal Positions an Tangents. American Journal of Mathematics, 79:497-56,957. [7] M. Fliess, J. Lévine, P. Martin, an P. Rouchon, Design of trajectory stabilizing feeback for riftless flat systems", 3r European Control Conf., Roma, I,pp , 995. [8] T. Fraichar an A. Scheuer, From Rees an Shepp s to Continuous-Curvature Paths, IEEE Trans. on Robotics, Vol., No. 6, pp.5-35, 4 [9] Y. Kanayama, N.Miyake, "Trajectory Generation for Mobile Robots", Robotics Research, MIT Press, Cambrige,985. [] Y. Kanayama an B.I. Hartman, "Smooth Local Path Planning for Autonomous Vehicles" Technical Report, Dept. of Computer Science, University of California, Santa Barbara, 988. [] H. Kano, M. Egerstet, H. Nakata, an C.F. Martin. B-Splines an Control Theory. To appear in Applie Mathematics an Computation, 3. [] Kelly an B. Nagy, Reactive Nonholonomic Trajectory Generation via Parametric Optimal Control, trovare pagine e rivista!!! [3] G. Laferriere an H. J. Sussmann, Motion planning for controllable systems without rift", 99 IEEE Int. Conf. on Robotics an Automation, Sacramento, CA, pp , 99. [4] G. Lafferriere an H.J. Sussmann, A ifferential geometric approach to motion planning", Nonholonomic Motion Planning, Zeiang Li an J.F. Canny Es, The Kluwer International Series in Engineering an Computer Science 9, 99. [5] J. C. Latombe, Robot Motion Planning, Kluwer Acaemic Publishers, 99. [6] J. P. Laumon (eitor) Robot Motion Planning an Control, LAAS report [7] J.-P. Laumon, Controllability of a multiboy mobile robot", IEEE Trans. on Robotics an Automation, vol. 9, no. 6, pp , 993. [8] R. M. Murray an S. S. Sastry, Nonholonomic motion planning: Steering using sinusois", IEEE Trans. on Automatic Control, vol. 38, no. 5, pp. 7-76, 993. [9] R. M. Murray, Control of nonholonomic systems using chaine forms", Fiels Institute Communications, vol., pp. 9-45, r European Control Conf., Roma, I, pp. 6-65, 995. [] B. Nagy an A. Kelly, Trajectory Generation for Car-Like Robots Using Cubic Curvature Polynomials, in Fiel an Service Robots, June,, Helsinki, Finlan. [] J. A. Rees an R. A. Shepp, Optimal paths for a car that goes both forwars an backwars, Pacific Journal of Mathematics, 45 (), 99. [] Johannes Reuter, Mobile Robot Trajectories With Continuously Differentiable Curvature: An Optimal Control Approach, Proceeings of the 998 IEEE/RSJ Conference on Intelligent Robots an Systems, Victoria, B.C, Canaa, October 998. [3] C. Samson an K. Ait-Aberrahim, Feeback stabilization of a nonholonomic wheele mobile robot, 99 IEEE/RSJ Int. Work. on Intelligent Robots an Systems, Osaka, J, pp. 4-47, 99. [4] D. H. Shin an S. Singh, "Path Generation for Robot Vehicles Using Composite Clothoi Segments", Technical Report, CMU-RITR- 9-3, The Robotics Institute, Carnegie Mellon University, 99. [5] O. J. Soralen, Conversion of the kinematics of a car with n trailers into a chaine form", 993 IEEE Int. Conf. on Robotics an Automation, Atlanta, GA, vol., pp , 993. [6] O. J. Soralen, Feeback control of nonholonomic mobile robots", Ph. D. Thesis, The Norwegian Institute of Technology, Tronheim, NO, Mar [7] D. Tilbury, J.P. Laumon, R. Murray, S. Sastry an G. Walsh, Steering car-like systems with trailers using sinusois" in IEEE Conf. on Robotics an Automation, pp , Nice,