Docente Vincenzo LIPPIELLO Dipartimento di Informatica e Sistemistica Universitá degli Studi di Napoli Federico II

Transcript

1 MATERIALE DIDATTICO Docente Vincenzo LIPPIELLO Dipartimento di Informatica e Sistemistica Universitá degli Studi di Napoli Federico II Tel lippiell@unina.it Libro di testo B. Siciliano, L. Sciavicco, L. Villani e G. Oriolo, Robotica: Modellistica, Pianificazione e Controllo, III edizione, McGraw-Hill Libri Italia, 2008 Lucidi proiettati a lezione disponibili in formato pdf sul sito web del docente Si ringrazia il Prof. Bruno Siciliano per aver fornito il materiale necessario alla produzione dei lucidi

2 ROBOTICA Studio di macchine che possano sostituire l uomo nell esecuzione di un compito, sia in termini di attività fisica che decisionale Radici culturali mitologia automa robot (robota = lavoro esecutivo) letteratura fantascientifica: le 3 leggi fondamentali (Asimov)

3 Robotica definita come scienza che studia la connessione intelligente tra percezione e azione sistema meccanico (locomozione + manipolazione) sistema sensoriale (sensori propriocettivi ed eterocettivi) sistema di governo Robotica avanzata spiccate caratteristiche di autonomia: applicazioni in ambiente ostile + applicazioni di servizio ancora in età infantile Robotica industriale progettazione, governo e applicazioni dei robot in ambito industriale tecnologia matura

4 L automazione è una tecnologia il cui obiettivo è quello di sostituire la macchina all uomo in un processo di produzione, non solo per quanto riguarda l esecuzione delle operazioni materiali, ma anche per ciò che concerne l elaborazione intelligente delle informazioni sullo stato del processo. automazione rigida (produzione in serie di grossi volumi di manufatti di caratteristiche costanti) automazione programmabile (produzione di piccoli e medi lotti di manufatti di caratteristiche variabili) automazione flessibile (produzione di lotti variabili di manufatti diversi) Robot industriale macchina con elevate caratteristiche di versatilità e flessibilità un robot è una struttura meccanica multifunzionale e riprogrammabile progettato per spostare materiali, parti, utensili o dispositivi specializzati secondo movimenti variabili programmati per l esecuzione di una varietà di compiti diversi (Robot Institute of America, 1980) componente tipico di sistemi di automazione programmabile

5 ROBOT INDUSTRIALE Componenti: una struttura meccanica o manipolatore che consiste in un insieme di corpi rigidi (bracci) interconnessi tra di loro per mezzo di articolazioni (giunti); nel manipolatore si individuano una struttura portante, che ne assicura mobilità, un polso, che conferisce destrezza, e un organo terminale che esegue il compito per cui il robot è utilizzato attuatori che imprimono il movimento al manipolatore attraverso l azionamento dei giunti; si impiegano usualmente motori elettrici, idraulici e talvolta pneumatici sensori che misurano lo stato del manipolatore (sensori propriocettivi) ed eventualmente lo stato dell ambiente (sensori eterocettivi) una unità di governo (calcolatore) con funzioni di controllo e supervisione dei movimenti del manipolatore

6 CAPACITÀ DI IMPIEGO Trasporto palettizzazione carico e scarico di magazzini e macchine operatrici/utensili selezione e smistamento di parti Manipolazione (lavorazione/assemblaggio) saldatura, verniciatura, incollaggio fresatura, trapanatura, avvitatura, taglio laser/getto d acqua assemblaggio di gruppi meccanici/elettrici, montaggio di schede elettroniche, cablaggio Misura collaudo dimensionale, rilevamento di profili, individuazione di difetti di fabbricazione

7 STRUTTURA DEI MANIPOLATORI Struttura meccanica a catena cinematica aperta o a catena cinematica chiusa Gradi di mobilità (giunti prismatici o rotoidali) Gradi di libertà (descrizione di un compito) Spazio di lavoro (porzione dell ambiente circostante a cui può accedere l organo terminale)

8 Manipolatore cartesiano Tre giunti prismatici: ad ogni grado di mobilità corrisponde un grado di libertà ottime caratteristiche di rigidezza meccanica, precisione di posizionamento del polso costante nello lo spazio di lavoro, operazioni di trasporto e assemblaggio, azionamenti elettrici (talvolta pneumatici)

9 Manipolatore a portale manipolazione di oggetti di dimensione e peso rilevanti

10 Manipolatore cilindrico Un giunto rotoidale e due prismatici: ad ogni grado di mobilità corrisponde un grado di libertà (in coordinate cilindriche) buone caratteristiche di rigidezza meccanica, la precisione di posizionamento del polso si riduce al crescere dello sbraccio orizzontale, operazioni di trasporto di oggetti anche di peso rilevante, azionamenti idraulici (o elettrici)

11 Manipolatore sferico Due giunti rotoidali e uno prismatico: ad ogni grado di mobilità corrisponde un grado di libertà (in coordinate sferiche) discrete caratteristiche di rigidezza meccanica, la precisione di posizionamento del polso si riduce al crescere dello sbraccio radiale, operazioni di lavorazione, azionamenti elettrici

12 Manipolatore SCARA (Selective Compliance Assembly Robot Arm) Due giunti rotoidali e uno prismatico elevata rigidezza a carichi verticali e cedevolezza a carichi orizzontali, la precisione di posizionamento del polso si riduce al crescere della distanza del polso stesso dall asse del primo giunto, manipolazione di piccoli oggetti, azionamenti elettrici

13 Manipolatore antropomorfo Tre giunti rotoidali: spalla e gomito (che connette braccio e avambraccio) struttura più destra precisione di posizionamento variabile, applicazioni molteplici, azionamenti elettrici

14 Polso sferico Tre giunti rotoidali: determinano l orientamento dell organo terminale caratteristiche di compattezza e destrezza, disaccoppiamento tra posizione e orientamento Organo terminale Specificato in relazione al compito che il robot deve eseguire pinza (trasporto), utensile o dispositivo specializzato (lavorazione e assemblaggio)

15 MODELLISTICA E CONTROLLO DI MANIPOLATORI Modellistica struttura meccanica (cinematica + cinematica differenziale + statica + dinamica) attuatori sensori Controllo pianificazione del moto controllo nello spazio libero controllo nello spazio vincolato unità di governo

16 Cinematica Relazioni tra posizioni dei giunti e posizione e orientamento dell organo terminale Cinematica differenziale Relazioni tra velocità dei giunti e velocità (lineare e angolare) dell organo terminale Statica Relazioni tra forze e coppie applicate ai giunti e forze e momenti applicati all organo terminale in situazioni di equilibrio

17 Dinamica Equazioni del moto del manipolatore in funzione delle forze e momenti agenti su di esso Pianificazione di traiettorie Generazione delle leggi di moto per le variabili di interesse (giunti/organo terminale) Attuatori e sensori Attuazione del moto, Misura di variabili di interesse

18 Controllo del moto Determinazione delle forze/coppie agli attuatori per garantire l esecuzione delle traiettorie di riferimento Controllo dell interazione Gestione del contatto tra organo terminale e ambiente Unità di governo Implementazione delle leggi di controllo, Interfaccia con operatore

19

20

21

22 CINEMATICA Relazioni tra posizioni dei giunti e posizione e orientamento dell organo terminale Matrice di rotazione Rappresentazioni minime dell orientamento Trasformazioni omogenee Cinematica diretta Spazio dei giunti e spazio operativo Calibrazione cinematica Problema cinematico inverso

23 POSIZIONE E ORIENTAMENTO DI UN CORPO RIGIDO Terna di riferimento Σ : O xyz e Terna solidale al corpo Σ : O x y z Posizione (vettore posizione dell origine) + Orientamento (versori: coseni direttori) o = o x x + o y y + o z z = o x o y o z x = x x x + x y y + x z z x x = x T x... y = y x x + y y y + y z z y x = y T x... z = z x x + z y y + z z z z x = z T x... o vettore applicato (modulo, direzione e punto di applicazione)

24 MATRICE DI ROTAZIONE R = x y z = x x y x z x x y y y z y = x z y z z z x T x y T x z T x x T y y T y z T y x T z y T z z T z Matrice ortogonale: 6 vincoli ortogonalità (3): x T y = 0, x T z = 0, y T z = 0 norma unitaria (3): x T x = 1, y T y = 1, z T z = 1 det(r) = ±1: terna levogira/destrogira R T R = I R T = R 1 R SO(m) gruppo speciale ortonormale delle matrici reali (m m) con colonne ortonormali e determinante unitario

25 Rotazioni elementari Rotazione di α intorno a z (positiva in senso antiorario) R z (α) = cosα sinα 0 sinα cosα

26 Rotazione di β intorno a y R y (β) = cosβ 0 sinβ sinβ 0 cosβ Rotazione di γ intorno a x R x (γ) = cosγ sinγ 0 sinγ cosγ Vale R k ( ϑ) = Rk T (ϑ), con k = x, y, z Prima interpretazione di R come operatore di rotazione Σ Σ : R esprime l operazione di rotazione che bisogna compiere intorno ad un asse dello spazio per sovrapporre gli assi della della terna di riferimento, Σ, agli assi omologhi della terna solidale al corpo, Σ

27 Rappresentazione di un vettore Terna solidale e terna di riferimento con origine comune (o = o) p = p = p x p y p z p x p y p z p = p xx + p yy + p zz p = x y z p { p = Rp p = R T p

28 Esempio p x = p x cosα p y sinα p y = p x sinα + p y cosα p = R z (α)p p z = p z Seconda interpretazione di R come matrice di trasformazione delle coordinate di un vettore espresse nella terna Σ nelle coordinate dello stesso vettore espresse nella terna Σ

29 Rotazione di un vettore p e p = Rp vettori espressi nella stessa terna di riferimento Il vettore p, espresso nella terna Σ, ha norma pari a quella di p e risulta ruotato rispetto a p secondo la matrice R Infatti se p = Rp si ha p T p = p T R T Rp = p T p

30 Esempio p x = p x cosα p y sinα p y = p x sinα + p y cosα p = R z (α)p p z = p z Terza interpretazione di R come operatore matriciale di rotazione di un vettore di un angolo prefissato intorno ad un generico asse di rotazione nello spazio

31 Significati della matrice di rotazione Fornisce l orientamento di una terna di coordinate rispetto ad un altra: i vettori colonna sono i coseni direttori degli assi della terna ruotata rispetto alla terna di partenza Rappresenta una trasformazione di coordinate che mette in relazione le coordinate di uno stesso punto in due terne differenti (di origine comune) È l operatore che consente di ruotare un vettore in una stessa terna di coordinate

32 COMPOSIZIONE DI MATRICI DI ROTAZIONE Tre terne di riferimento con origine comune:σ 0 : O x 0, y 0, z 0, Σ 1 : O x 1, y 1, z 1, Σ 2 : O x 2, y 2, z 2 Stesso vettore espresso nelle tre terne p 1 = R 1 2p 2, p 0 = R 0 1p 1, p 0 = R 0 2p 2 R 0 2 = R 0 1R 1 2 dove R j i é la matrice di rotazione di Σ i rispetto a Σ j, ovvero che descrive la rotazione Σ j Σ i R j i = (Ri j) 1 = (R i j) T Rotazione in terna corrente (moltiplicazione da sx a dx) Σ 0 Σ 1 Σ 2 R1 0 R2 1 Rotazione in terna fissa (moltiplicazione da dx a sx) R 0 2 = R 0 1R 1 2 Σ 0 Σ 1 Σ 0 Σ 2 Σ 2 R1 0 R0 1 R 2 1 R1 0 R 0 2 = R 1 2R 0 1

33 Esempio (rotazioni ZY e Y Z): rotazioni in terna fissa e mobile, non commutatività delle rotazioni

34 RAPPRESENTAZIONI MINIME DELL ORIENTAMENTO Rappresentazioni dell orientamento Matrice di rotazione: 9 parametri con 6 vincoli solo 3 sono indipendenti Asse/angolo: 4 parametri con 1 vincolo solo 3 sono indipendenti Rappresentazioni in termini di 3 angoli che rappresentano 3 rotazioni elementari intorno ad assi coordinati 12 sequenze possibili (escludendo rotazioni successive intorno ad assi paralleli)

35 Angoli di Eulero (ZY Z) Rotazione ZY Z in terna corrente R ZYZ = R z (ϕ)r y (ϑ)r z (ψ) = c ϕc ϑ c ψ s ϕ s ψ c ϕ c ϑ s ψ s ϕ c ψ c ϕ s ϑ s ϕ c ϑ c ψ + c ϕ s ψ s ϕ c ϑ s ψ + c ϕ c ψ s ϕ s ϑ s ϑ c ψ s ϑ s ψ c ϑ Se ϑ = 0, π: rotazioni intorno ad assi paralleli la rappresentazione degenera

36 Problema inverso Assegnata R = r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 i 3 angoli di Eulero sono (segno ): per ϑ (0, π) ϕ = Atan2(r 23, r 13 ) ( ) ϑ = Atan2 r r2 23, r 33 ψ = Atan2(r 32, r 31 ) per ϑ ( π, 0) ϕ = Atan2( r 23, r 13 ) ( ) ϑ = Atan2 r r2 23, r 33 ψ = Atan2( r 32, r 31 ) Per ϑ = 0, π le soluzioni degenerano, in quanto è possibile solo determinare ϕ ± ψ : Singolarità di rappresentazione

37 Angoli di RPY (ZY X) Rotazione ZY X in terna corrente (o XY Z in terna fissa) R RPY = R z (ϕ)r y (ϑ)r x (ψ) = c ϕc ϑ c ϕ s ϑ s ψ s ϕ c ψ c ϕ s ϑ c ψ + s ϕ s ψ s ϕ c ϑ s ϕ s ϑ s ψ + c ϕ c ψ s ϕ s ϑ c ψ c ϕ s ψ s ϑ c ϑ s ψ c ϑ c ψ Se ϑ = ±π/2: rotazioni intorno ad assi paralleli la rappresentazione degenera

38 Problema inverso Assegnata R = r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 i 3 angoli di RPY sono (segno ): per ϑ ( π/2, π/2) per ϑ (π/2, 3π/2) ϕ = Atan2(r 21, r 11 ) ( ) ϑ = Atan2 r 31, r r2 33 ϕ = Atan2( r 21, r 11 ) ( ) ϑ = Atan2 r 31, r r2 33 ψ = Atan2(r 32, r 33 ) ψ = Atan2( r 32, r 33 ) Per ϑ = ±π/2 le soluzioni degenerano, in quanto è possibile solo determinare ϕ ± ψ : Singolarità di rappresentazione

39 ROTAZIONE INTORNO A UN ASSE ARBITRARIO Rappresentazione a 4 parametri (ϑ e r = [r x r y r z ] T ) R(ϑ, r) = R z (α)r y (β)r z (ϑ)r y ( β)r z ( α) sinα = r y r 2 x + r 2 y cosα = r x r 2 x + r 2 y sinβ = r 2 x + r 2 y cosβ = r z

40 Espressione esplicita rx 2 (1 c ϑ) + c ϑ r x r y (1 c ϑ ) r z s ϑ r x r z (1 c ϑ ) + r y s ϑ R(ϑ, r) = r x r y (1 c ϑ ) + r z s ϑ ry 2(1 c ϑ) + c ϑ r y r z (1 c ϑ ) r x s ϑ r x r z (1 c ϑ ) r y s ϑ r y r z (1 c ϑ ) + r x s ϑ rz(1 2 c ϑ ) + c ϑ La rappresentazione non è univoca poichè R(ϑ, r) = R( ϑ, r)

41 Problema inverso Assegnata R = r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 l angolo e l asse equivalente sono (per sinϑ 0): ( ) somma elementi diagonale: ϑ = cos 1 r11 + r 22 + r differenze elementi fuori diagonale: r = 1 r 32 r 23 r 13 r 31 2 sinϑ r 21 r 12 componenti del versore dell asse vincolate: r 2 x + r 2 y + r 2 z = 1 soluzione per sin ϑ = 0: ϑ = { 0 π ϑ = 0 formule ricavabili dall espressione di R asse non definito (singolarità della rappresentazione)

42 TRASFORMAZIONI OMOGENEE Trasformazione di coordinate (traslazione + rotazione) p 0 = o R0 1 p1 Trasformazione inversa (R 1 0 = (R0 1 )T ) p 1 = R 1 0o R 1 0p 0

43 Rappresentazione omogenea p = [ ] p 1 Matrice di trasformazione omogenea A 0 1 = [ R 0 1 o T 1 ] Trasformazione di coordinate p 0 = A 0 1 p1

44 Trasformazione inversa ove Si noti che p 1 = A 1 0 p0 = ( A 0 1) 1 p 0 A 1 0 = [ R 1 0 R 1 0o T 1 A 1 A T A non è ortogonale ] Successione di trasformazioni p 0 = A 0 1 A1 2...An 1 n p n

45 CINEMATICA DIRETTA Catena cinematica aperta base sequenza bracci organo terminale sequenza di bracci connessi tramite giunti variabile di giunto q i = grado di mobilità giunti rotoidali e prismatici Problema della cinematica diretta: determinare posizione+orientamento dell organo terminale in funzione delle variabili di giunto

46 Manipolatore Terne { Terna base Terna utensile: versori n e (normale), s e (sliding) e a e (approach) Equazione cinematica diretta T b e (q) = nb e(q) s b e(q) a b e(q) p b e(q)

47 Manipolatore planare a due bracci T b e (q) = = nb e s b e a b e p b e s 12 c 12 a 1 c 1 + a 2 c 12 0 c 12 s 12 a 1 s 1 + a 2 s

48 CONVENZIONE DI DENAVIT-HARTENBERG Manipolatore ad n giunti. Successione di n + 1 terne solidali ai bracci (Σ 0...Σ n ) Σ 0 : fissata alla base, Σ b : terna base, Σ e : terna utensile Terne fissate in modo arbitrario secondo procedure sistematiche

49 si sceglie l asse z i giacente lungo l asse del giunto i + 1 si individua O i all intersezione dell asse z i con la normale comune agli assi z i 1 e z i, e con O i si indica l intersezione della normale comune con z i 1 si assume l asse x i diretto lungo la normale comune agli assi z i 1 e z i con verso positivo dal giunto i al giunto i + 1 si sceglie l asse y i in modo da completare una terna levogira

50 Definizione non univoca della terna: con riferimento alla terna 0, per la quale la sola direzione dell asse z 0 risulta specificata: si possono quindi scegliere arbitrariamente O 0 ed x 0 con riferimento alla terna n, per la quale il solo asse x n risulta soggetto a vincolo (deve essere normale all asse z n 1 ): infatti non vi è giunto n+1, per cui non è definito z n e lo si può scegliere arbitrariamente quando due assi consecutivi sono paralleli, in quanto la normale comune tra di essi non è univocamente definita quando due assi consecutivi si intersecano, in quanto il verso di x i è arbitrario quando il giunto i è prismatico, nel qual caso la sola direzione dell asse z i 1 è determinata

51 Parametri di Denavit-Hartenberg a i distanza di O i da O i ; d i coordinata su z i 1 di O i ; α i angolo intorno all asse x i tra l asse z i 1 e l asse z i valutato positivo in senso antiorario; ϑ i angolo intorno all asse z i 1 tra l asse x i 1 e l asse x i valutato positivo in senso antiorario. a i e α i sono sempre costanti se il giunto è rotoidale la variabile è ϑ i se il giunto è prismatico la variabile è d i

52 Trasformazione di coordinate Σ i 1 Σ i rotazione di ϑ i intorno a z i 1 traslazione di d i lungo z i 1 A i 1 i = Σ i Σ i rotazione di α i intorno a x i traslazione di a i lungo x i A i 1 i (q i ) = A i 1 i A i i = A i i = c ϑi s ϑi c αi s ϑi s αi a i c ϑi s ϑi c ϑi c αi c ϑi s αi a i s ϑi 0 s αi c αi d i c ϑi s ϑi 0 0 s ϑi c ϑi d i a i 0 c αi s αi 0 0 s αi c αi q i = { di prismatico rotoidale ϑ i

53 Composizione di trasformazioni T 0 n(q) = A 0 1(q 1 )A 1 2(q 2 )...A n 1 n (q n ) T e b (q) = T b 0T 0 n(q)t n e

54 Procedura operativa 1. Individuare e numerare consecutivamente gli assi dei giunti; assegnare, rispettivamente, le direzioni agli assi z 0,..., z n 1 2. Fissare la terna base posizionandone l origine sull asse z 0 ; gli assi x 0 e y 0 sono scelti in maniera tale da ottenere una terna levogira Eseguire i passi da 3 a 5 per i = 1,...,n 1: 3. Individuare l origine O i all intersezione di z i con la normale comune agli assi z i 1 e z i. Se gli assi z i 1 e z i sono paralleli e il giunto i è rotoidale, posizionare O i in modo da annullare d i ; se il giunto i è prismatico, scegliere O i in corrispondenza di una posizione di riferimento per la corsa del giunto (ad esempio un fine-corsa) 4. Fissare l asse x i diretto lungo la normale comune agli assi z i 1 e z i con verso positivo dal giunto i al giunto i Fissare l asse y i in modo da ottenere una terna levogira

55 Per completare: 6. Fissare la terna n scegliendo l asse x n perpendicolare all asse z n 1 ; se il giunto n è di rotazione, allineare z n lungo la direzione di z n 1 7. Costruire per i = 1,...,n la tabella dei parametri a i, d i, α i, ϑ i 8. Calcolare sulla base dei parametri di cui al punto 7 le matrici di trasformazione omogenea A i 1 i (q i ) per i = 1,...,n 9. Calcolare la funzione cinematica diretta Tn(q) 0 = A A n 1 n orientamento della terna n rispetto alla terna base 10. Calcolare T e b (q) = T b 0 T 0 n (q)t n e che fornisce posizione e

56 Manipolatore planare a tre bracci Braccio a i α i d i ϑ i 1 a ϑ 1 2 a ϑ 2 3 a ϑ 3

57 A i 1 i = T 0 3 = A0 1 A1 2 A2 3 = c i s i 0 a i c i s i c i 0 a i s i i = 1, 2, c 123 s a 1 c 1 + a 2 c 12 + a 3 c 123 s 123 c a 1 s 1 + a 2 s 12 + a 3 s

58 Manipolatore sferico Braccio a i α i d i ϑ i 1 0 π/2 0 ϑ π/2 d 2 ϑ d 3 0

59 A 0 1 = c 1 0 s 1 0 s 1 0 c A 2 3 = A 1 2 = d c 2 0 s 2 0 s 2 0 c d T 0 3 = A0 1 A1 2 A2 3 = c 1 c 2 s 1 c 1 s 2 c 1 s 2 d 3 s 1 d 2 s 1 c 2 c 1 s 1 s 2 s 1 s 2 d 3 + c 1 d 2 s 2 0 c 2 c 2 d

60 Manipolatore antropomorfo Braccio a i α i d i ϑ i 1 0 π/2 0 ϑ 1 2 a ϑ 2 3 a ϑ 3

61 A i 1 i = A 0 1 = c 1 0 s 1 0 s 1 0 c c i s i 0 a i c i s i c i 0 a i s i i = 2, 3 T 0 3 = A0 1 A1 2 A2 3 = c 1 c 23 c 1 s 23 s 1 c 1 (a 2 c 2 + a 3 c 23 ) s 1 c 23 s 1 s 23 c 1 s 1 (a 2 c 2 + a 3 c 23 ) s 23 c 23 0 a 2 s 2 + a 3 s

62 Polso sferico Braccio a i α i d i ϑ i 4 0 π/2 0 ϑ π/2 0 ϑ d 6 ϑ 6

63 A 3 4 = c 4 0 s 4 0 s 4 0 c T 3 6 = A3 4 A4 5 A5 6 = A 5 6 = A 4 5 = c 6 s s 6 c d c 5 0 s 5 0 s 5 0 c c 4 c 5 c 6 s 4 s 6 c 4 c 5 s 6 s 4 c 6 c 4 s 5 c 4 s 5 d 6 s 4 c 5 c 6 + c 4 s 6 s 4 c 5 s 6 + c 4 c 6 s 4 s 5 s 4 s 5 d 6 s 5 c 6 s 5 s 6 c 5 c 5 d

64 Manipolatore di Stanford T 0 6 = T 0 3 T 3 6 = n0 s 0 a 0 p

65 p 0 = n 0 = s 0 = a 0 = c 1 s 2 d 3 s 1 d 2 + ( ) c 1 (c 2 c 4 s 5 + s 2 c 5 ) s 1 s 4 s 5 d6 s 1 s 2 d 3 + c 1 d 2 + ( ) s 1 (c 2 c 4 s 5 + s 2 c 5 ) + c 1 s 4 s 5 d6 c 2 d 3 + ( s 2 c 4 s 5 + c 2 c 5 )d 6 ( ) c 1 ( c2 (c 4 c 5 c 6 s 4 s 6 ) s 2 s 5 c 6 ) s1 (s 4 c 5 c 6 + c 4 s 6 ) s 1 c2 (c 4 c 5 c 6 s 4 s 6 ) s 2 s 5 c 6 + c1 (s 4 c 5 c 6 + c 4 s 6 ) s 2 (c 4 c 5 c 6 s 4 s 6 ) c 2 s 5 c 6 ( ) c 1 ( c2 (c 4 c 5 s 6 + s 4 c 6 ) + s 2 s 5 s 6 ) s1 ( s 4 c 5 s 6 + c 4 c 6 ) s 1 c2 (c 4 c 5 s 6 + s 4 c 6 ) + s 2 s 5 s 6 + c1 ( s 4 c 5 s 6 + c 4 c 6 ) s 2 (c 4 c 5 s 6 + s 4 c 6 ) + c 2 s 5 s 6 c 1(c 2 c 4 s 5 + s 2 c 5 ) s 1 s 4 s 5 s 1 (c 2 c 4 s 5 + s 2 c 5 ) + c 1 s 4 s 5 s 2 c 4 s 5 + c 2 c 5

66 Manipolatore antropomorfo con polso sferico Braccio a i α i d i ϑ i 1 0 π/2 0 ϑ 1 2 a ϑ π/2 0 ϑ π/2 d 4 ϑ π/2 0 ϑ d 6 ϑ 6

67 c 3 0 s 3 0 c 4 0 s 4 0 A 2 s 3 = 3 0 c 3 0 A 3 s 4 = 4 0 c d ( ) a 2 c 1 c 2 + d 4 c 1 s 23 + d 6 c1 (c 23 c 4 s 5 + s 23 c 5 ) + s 1 s 4 s 5 p 0 ( ) = a 2 s 1 c 2 + d 4 s 1 s 23 + d 6 s1 (c 23 c 4 s 5 + s 23 c 5 ) c 1 s 4 s 5 a 2 s 2 d 4 c 23 + d 6 (s 23 c 4 s 5 c 23 c 5 ) ( ) c 1 c23 (c 4 c 5 c 6 s 4 s 6 ) s 23 s 5 c 6 n 0 ( ) + s1 (s 4 c 5 c 6 + c 4 s 6 ) = s 1 c23 (c 4 c 5 c 6 s 4 s 6 ) s 23 s 5 c 6 c1 (s 4 c 5 c 6 + c 4 s 6 ) s 23 (c 4 c 5 c 6 s 4 s 6 ) + c 23 s 5 c 6 ( ) c 1 c23 (c 4 c 5 s 6 + s 4 c 6 ) + s 23 s 5 s 6 s 0 ( ) + s1 ( s 4 c 5 s 6 + c 4 c 6 ) = s 1 c23 (c 4 c 5 s 6 + s 4 c 6 ) + s 23 s 5 s 6 c1 ( s 4 c 5 s 6 + c 4 c 6 ) s 23 (c 4 c 5 s 6 + s 4 c 6 ) c 23 s 5 s 6 a 0 = c 1(c 23 c 4 s 5 + s 23 c 5 ) + s 1 s 4 s 5 s 1 (c 23 c 4 s 5 + s 23 c 5 ) c 1 s 4 s 5 s 23 c 4 s 5 c 23 c 5

68 SPAZIO DEI GIUNTI E SPAZIO OPERATIVO Spazio dei giunti q = q 1.. IR n q n q i = ϑ i (giunto rotoidale) q i = d i (giunto prismatico) Spazio operativo p (posizione) φ (orientamento) Equazione cinematica diretta x = [ ] p φ IR m m 6 x = k(q)

69 Esempio x = p x p y φ = k(q) = a 1c 1 + a 2 c 12 + a 3 c 123 a 1 s 1 + a 2 s 12 + a 3 s 123 ϑ 1 + ϑ 2 + ϑ 3 Nel caso più generale il calcolo di φ(q) non è attuabile in forma diretta, ma deve passare attraverso la risoluzione del problema (inverso) R 0 n(q) φ(q)

70 Spazio di lavoro Spazio di lavoro raggiungibile p = p(q) q im q i q im i = 1,...,n elementi di superficie planare, sferica, toroidale e cilindrica Spazio di lavoro destro orientamenti diversi SPD SPR

71 Esempio configurazioni ammissibili (gomito alto/basso q 2 <> 0) spazio di lavoro (gomito alto/basso DAEFD/BCDAB)

72 Accuratezza scostamento tra posizione conseguita con la postura assegnata e posizione calcolata tramite la cinematica diretta valori tipici: (0.2, 1) mm Ripetibilità capacità del manipolatore di tornare in una posizione precedentemente raggiunta valori tipici: (0.02, 0.2) mm Ridondanza cinematica m < n (intrinseca) r < m = n (funzionale)

73 CALIBRAZIONE CINEMATICA Valori precisi dei parametri DH per migliorare l accuratezza di un manipolatore Equazione cinematica diretta in funzione di tutti i parametri x = k(a, α, d, ϑ) a = [a 1...a n ] T... ϑ = [ϑ 1...ϑ n ] T x m : locazione misurata x n : locazione nominale (parametri fissi + variabili giunto) x = x m x n : errore

74 Per piccoli scostamenti: x = k k a + a α = Φ(ζ n ) ζ k k α + d + d ϑ ϑ ζ = [a α d ϑ] T a = a m a n... ζ = ζ m ζ n Φ: matrice di calibrazione m equazioni in 4n incognite ( ζ)

75 l misure (lm 4n) x = x 1. x l = Φ 1. Φ l ζ = Φ ζ Soluzione ζ = ( Φ T Φ) 1 ΦT x ζ = ζ n + ζ... finché ζ converge stime più accurate dei parametri fissi correzioni alle misure dei trasduttori Inizializzazione postura di riferimento (home)

76 PROBLEMA CINEMATICO INVERSO Cinematica diretta q = T oppure q = x Cinematica inversa T = q oppure x = q Complessità esistenza di soluzioni ammissibili? soluzione analitica (in forma chiusa)? soluzioni multiple, infinite soluzioni Intuizione algebrica e geometrica Tecniche numeriche

77 Soluzione del manipolatore planare a tre bracci Soluzione algebrica φ = ϑ 1 + ϑ 2 + ϑ 3 p Wx = p x a 3 c φ = a 1 c 1 + a 2 c 12 p Wy = p y a 3 s φ = a 1 s 1 + a 2 s 12

78 Soluzione per ϑ 2 (± gomito basso/alto), 1 c 2 1 (altrimenti / SPR) c 2 = p2 Wx + p2 Wy a2 1 a2 2, s 2 = ± 1 c 2 2 2a 1 a, ϑ 2 = Atan2(s 2, c 2 ) 2 Soluzione per ϑ 1 (ϑ 2 noto) s 1 = (a 1 + a 2 c 2 )p Wy a 2 s 2 p Wx p 2 Wx + p2 Wy, c 1 = (a 1 + a 2 c 2 )p Wx + a 2 s 2 p Wy p 2 Wy + p2 Wy, ϑ 1 = Atan2(s 1, c 1 ) Soluzione per ϑ 3 da angolo di orientamento (ϑ 1 e ϑ 2 noti) ϑ 3 = φ ϑ 1 ϑ 2

79 Soluzione geometrica OW = p 2 Wx + p2 Wy Teorema dei coseni p 2 Wx + p2 Wy = a2 1 + a a 1a 2 cos(π ϑ 2 )

80 Soluzione per ϑ 2 (ϑ 2 (0, π) / ϑ 2 ( π, 0) gomito basso/alto), (altrimenti / SPR) p 2 Wx + p2 Wy a 1 + a 2 c 2 = p2 Wx + p2 Wy a2 1 a2 2 2a 1 a 2, ϑ 2 = cos 1 (c 2 ) Soluzione per ϑ 1 (± se ϑ 2 <> 0), β (0, π) α = Atan2(p Wy, p Wx ), β = cos 1 p2 Wx + p2 Wy + a2 1 a 2 2 2a 1 p 2 Wx + p2 Wy ϑ 1 = α ± β Soluzione per ϑ 3 ϑ 3 = φ ϑ 1 ϑ 2

81 Soluzione di manipolatori con polso sferico Assegnati p e R = [n s a] Soluzione disaccoppiata: punto polso p W = p d 6 a calcolo di p W (q 1, q 2, q 3 ) cinematica inversa per (q 1, q 2, q 3 ) calcolo di R 0 3(q 1, q 2, q 3 ) calcolo di R 3 6 (ϑ 4, ϑ 5, ϑ 6 ) = R 0 3 T R cinematica inversa per (ϑ 4, ϑ 5, ϑ 6 )

82 Soluzione del manipolatore antropomorfo DisallineamentoΣ 3 :d 4 = a 3,d 6 = 0,ϑ 3 ϑ 3 + π 2 senza polso = cinematica del manipolatore antropomorfo Se p Wx 0 e p Wy 0 (altrimenti soluzioni per ϑ 1 ) ϑ 1 = Atan2(p Wy, p Wx ) (oppure ϑ 1 = π + Atan2(p Wy, p Wx ) se ϑ 2 π ϑ 2 )

83 Bracci 2+3: planare a due bracci, con p Wx p 2 Wx + p2 Wy e p Wy p Wz c 3 = p2 Wx + p2 Wy + p2 Wz a2 2 a2 3 2a 2 a 3 ϑ 3 = Atan2(s 3, c 3 ) s 3 = ± 1 c 2 3 s 2 = c 2 = (a 2 + a 3 c 3 )p Wz a 3 s 3 p 2 Wx + p2 Wy p 2 Wx + p2 Wy + p2 Wz (a 2 + a 3 c 3 ) p 2 Wx + p2 Wy + a 3s 3 p Wz p 2 Wx + p2 Wy + p2 Wz ϑ 2 = Atan2(s 2, c 2 )

84 Quattro configurazioni ammissibili (a meno di fine corsa...) soluzione univoca solo se p Wx 0 p Wy 0 Se p Wx = 0 e p Wy = 0 soluzioni (è possibile ricavare ϑ 2 e ϑ 3 a prescindere dal valore di ϑ 1 ) singolarità cinematica

85 Soluzione del polso sferico Soluzione inversa per angoli di Eulero ZY Z! R 3 6 = n 3 x s 3 x a 3 x n 3 y s 3 y a 3 y n 3 z s 3 z a 3 z ϑ 5 (0, π) ϑ 5 ( π, 0) ϑ 4 = Atan2(a 3 y, a ( 3 x) ) ϑ 5 = Atan2 (a 3 x) 2 + (a 3 y) 2, a 3 z ϑ 6 = Atan2(s 3 z, n3 z ) ϑ 4 = Atan2( a ( 3 y, a3 x ) ) ϑ 5 = Atan2 (a 3 x )2 + (a 3 y )2, a 3 z ϑ 6 = Atan2( s 3 z, n 3 z)

86 CINEMATICA DIFFERENZIALE Relazioni tra velocità dei giunti e velocità dell organo terminale Jacobiano geometrico Jacobiano analitico Singolarità cinematiche Analisi della ridondanza Inversione della cinematica differenziale Algoritmi per l inversione cinematica STATICA Relazioni tra forze all organo terminale e coppie ai giunti

87 JACOBIANO GEOMETRICO T(q) = R(q) p(q) 0 T 1 Obiettivo v = [ ] ṗ ω ṗ = J P (q) q ω = J O (q) q = J(q) q J = [ JP J O ]

88 Derivata di una matrice di rotazione Essendo R(t)R T (t) = I Ṙ(t)RT (t) + R(t)ṘT (t) = O Posto: S(t) = Ṙ(t)RT (t) Ṙ(t) = S(t)R(t) con S(t) + S T (t) = O S(t) è una matrice antisimmetrica

89 Interpretazione (p costante): p(t) = R(t)p ṗ(t) = Ṙ(t)p ṗ(t) = S(ω(t))R(t)p essendo (dalla meccanica dei corpi rigidi) ṗ(t) = ω(t) R(t)p si può interpretare S(ω) come l operatore matriciale che descrive il prodotto vettoriale ω Pertanto Ṙ(t) = S(ω(t))R(t) con S(ω) = 0 ω z ω y ω z 0 ω x ω y ω x 0 Proprietà: RS(ω)R T = S(Rω)

90 Esempio: rotazione elementare intorno a z R z (α(t)) = cosα(t) sinα(t) 0 sin α(t) cos α(t) 0, ω(t) = α(t) S(ω(t)) = α sinα(t) α(t) cosα(t) 0 cosα(t) sinα(t) 0 α cosα(t) α(t) sinα(t) 0 sin α(t) cos α(t) 0 = α(t) 0 α(t)

91 p 0 = o R0 1 p1 = o r0 1 ṗ 0 = ȯ R 0 1ṗ1 + Ṙ0 1p 1 = ȯ R0 1ṗ1 + S(ω 0 1 )R0 1 p1 = ȯ R0 1ṗ1 + ω 0 1 r0 1

92 Velocità di un braccio Velocità lineare p i = p i 1 + R i 1 r i 1 i 1,i = p i 1 + r i 1,i ṗ i = ṗ i 1 + R i 1 ṙ i 1 i 1,i + ω i 1 R i 1 r i 1 i 1,i = ṗ i 1 + v i 1,i + ω i 1 r i 1,i con v i 1,i = R i 1 ṙ i 1 i 1,i

93 Velocità angolare R i = R i 1 R i 1 i S(ω i )R i = S(ω i 1 )R i + R i 1 S(ω i 1 i 1,i )Ri 1 i = S(ω i 1 )R i + S(R i 1 ω i 1 i 1,i )R i ω i = ω i 1 + R i 1 ω i 1 i 1,i = ω i 1 + ω i 1,i con ω i 1,i = R i 1 ω i 1 i 1,i

94 ṗ i = ṗ i 1 + v i 1,i + ω i 1 r i 1,i ω i = ω i 1 + ω i 1,i Giunto prismatico: { ωi 1,i = 0 v i 1,i = d i z i 1 { ωi = ω i 1 ṗ i = ṗ i 1 + d i z i 1 + ω i r i 1,i { { Giunto rotoidale: ω i 1,i = ϑ i z i 1 ω i = ω i 1 + ϑ i z i 1 v i 1,i = ω i 1,i r i 1,i ṗ i = ṗ i 1 + ω i r i 1,i

95 Calcolo dello Jacobiano J = [ j 1... j n ] = j P1... j Pn j O1 j On Velocità angolare giunto i prismatico giunto i rotoidale q i j Oi = 0 = j Oi = 0 q i j Oi = ϑ i z i 1 = j Oi = z i 1

96 Velocità lineare giunto i prismatico q i j Pi = d i z i 1 = j Pi = z i 1 giunto i rotoidale q i j Pi = ω i 1,i r i 1,n = ϑ i z i 1 (p p i 1 ) = j Pi = z i 1 (p p i 1 )

97 Colonna dello Jacobiano geometrico [ jpi j Oi ] [ zi 1 ] 0 = [ ] zi 1 (p p i 1 ) z i 1 per un giunto prismatico per un giunto rotoidale z i 1 = R 0 1 (q 1)...R i 2 i 1 (q i 1)z 0 : da R 0 i 1 (III colonna) p = A 0 1 (q 1)...A n 1 n (q n ) p 0 : da T 0 n (IV colonna) p i 1 = A 0 1(q 1 )...A i 2 i 1 (q i 1) p 0 : con z 0 = e p 0 = da A 0 i 1 (IV colonna)

98 Rappresentazione in terna differente [ ] ṗu ω u = = J u = [ R u O O [ R u O O R u R u [ R u O O R u ][ ] ṗ ω ] J q ] J

99 Manipolatore planare a tre bracci J(q) = [ ] z0 (p p 0 ) z 1 (p p 1 ) z 2 (p p 2 ) z 0 z 1 z 2 p 0 = p 1 = a 1c 1 a 1 s 1 p 2 = a 1c 1 + a 2 c 12 a 1 s 1 + a 2 s p = a 1c 1 + a 2 c 12 + a 3 c 123 a 1 s 1 + a 2 s 12 + a 3 s z 0 = z 1 = z 2 = 0 0 1

100 J = a 1 s 1 a 2 s 12 a 3 s 123 a 2 s 12 a 3 s 123 a 3 s 123 a 1 c 1 + a 2 c 12 + a 3 c 123 a 2 c 12 + a 3 c 123 a 3 c J P = [ ] a1 s 1 a 2 s 12 a 3 s 123 a 2 s 12 a 3 s 123 a 3 s 123 a 1 c 1 + a 2 c 12 + a 3 c 123 a 2 c 12 + a 3 c 123 a 3 c 123 Solo 3 righe non nulle

101 Manipolatore antropomorfo J = [ ] z0 (p p 0 ) z 1 (p p 1 ) z 2 (p p 2 ) z 0 z 1 z 2 p 0 = p 1 = 0 0 p 2 = 0 z 0 = p = c 1(a 2 c 2 + a 3 c 23 ) s 1 (a 2 c 2 + a 3 c 23 ) a 2 s 2 + a 3 s 23 z 1 = z 2 = a 2c 1 c 2 a 2 s 1 c 2 a 2 s 2 s 1 c 1 0

102 J = s 1 (a 2 c 2 + a 3 c 23 ) c 1 (a 2 s 2 + a 3 s 23 ) a 3 c 1 s 23 c 1 (a 2 c 2 + a 3 c 23 ) s 1 (a 2 s 2 + a 3 s 23 ) a 3 s 1 s 23 0 a 2 c 2 + a 3 c 23 a 3 c 23 0 s 1 s 1 0 c 1 c J P = s 1(a 2 c 2 + a 3 c 23 ) c 1 (a 2 s 2 + a 3 s 23 ) a 3 c 1 s 23 c 1 (a 2 c 2 + a 3 c 23 ) s 1 (a 2 s 2 + a 3 s 23 ) a 3 s 1 s 23 0 a 2 c 2 + a 3 c 23 a 3 c 23 Solo 3 righe linearmente indipendenti e s 1 ω y = c 1 ω z

103 JACOBIANO ANALITICO ṗ = p q q = J P(q) q φ = φ q q = J φ(q) q p = p(q) φ = φ(q) } ẋ = [ ] ṗ φ x = k(q) = k(q) q q = [ ] JP (q) J φ (q) q = J A (q) q Jacobiano analitico: J A (q) = k(q) q J J A poichè ω φ

104 Velocità di rotazione in terna corrente di angoli di Eulero ZYZ per effetto di ϕ: [ ω x ω y ω z ] T = ϕ [0 0 1 ] T per effetto di ϑ: [ ω x ω y ω z ] T = ϑ [ s ϕ c ϕ 0] T per effetto di ψ: [ ω x ω y ω z ] T = ψ [ c ϕ s ϑ s ϕ s ϑ c ϑ ] T

105 Composizione di velocità di rotazione elementari ω = 0 s ϕ c ϕ s ϑ 0 c ϕ s ϕ s ϑ φ = T(φ) φ 1 0 c ϑ Essendo: det(t) = s ϑ T(φ) singolare per ϑ = 0, π (singolarità di rappresentazione) in tal caso si ha ωx 2 + ω2 y = ϑ 2 Quindi: φ può essere espressa mediante una ω equivalente ω che non possono essere espresse mediante una φ equivalente (in corrispondenza di singolarità di rappresentazione)

106 Significato fisico di ω ω = [ π/2 0 0 ] T 0 t 1 ω = [ 0 π/2 0] T 0 t 1 ω = [ 0 π/2 0 ] T 1 < t 2 ω = [ π/2 0 0] T 1 < t 2 Due diverse leggi orarie per ω(t) corrispondenti allo stesso integrale Viceversa, 2 0 φ(t)dt = φ(2) φ(0) 2 0 ω(t)dt = [ π/2 π/2 0 ] T

107 Relazione tra Jacobiano analitico e Jacobiano geometrico v = [ ] I O ẋ = T O T(φ) A (φ)ẋ J = T A (φ)j A Jacobiano geometrico grandezze di significato fisico Jacobiano analitico grandezze differenziali di variabili nello spazio operativo Sono equivalenti solo in particolari casi (ad es., quando la struttura è in grado di imporre rotazioni dell organo terminale solo intorno ad un asse fisso nello spazio) Esempio: per il manipolatore planare a 3 bracci (con φ = ϑ 1 + ϑ 2 + ϑ 3 ed eliminando le righe nulle in J) si ha J J A

108 SINGOLARITÀ CINEMATICHE v = J(q) q se J diminuisce di rango = singolarità cinematiche (a) perdita di mobilità ( non è possibile imporre leggi di moto arbitrarie all organo terminale) (b) si possono avere soluzioni al problema cinematico inverso (c) nell intorno di una singolarità si possono generare velocità elevate nello spazio dei giunti (a fronte di velocità ridotte dell organo terminale) Classificazione Singolarità ai confini dello spazio di lavoro raggiungibile: manipolatore tutto steso o ripiegato Singolarità all interno dello spazio di lavoro raggiungibile: allineamento di assi di moto o configurazioni particolari dell organo terminale

109 Manipolatore planare a due bracci [ ] a1 s J = 1 a 2 s 12 a 2 s 12 a 1 c 1 + a 2 c 12 a 2 c 12 det(j) = a 1 a 2 s 2 Due singolarità ai confini dello SdLR: s 2 = 0 ϑ 2 = 0, ϑ 2 = π [ (a 1 + a 2 )s 1 (a 1 + a 2 )c 1 ] T parallelo a [ a 2 s 1 a 2 c 1 ] T (componenti di velocità dell organo terminale non indipendenti)

110 Disaccoppiamento di singolarità Strutture con struttura portante + polso sferico Singolarità della struttura portante Singolarità del polso

111 J ha una struttura a blocchi (3 3) J = [ ] J11 J 12 J 21 J 22 Con (ultimi 3 giunti rotoidali) J 12 = [ z 3 (p p 3 ) z 4 (p p 4 ) z 5 (p p 5 ) ] e J 22 = [ z 3 z 4 z 5 ] Se p = p W = p W p i paralleli a z i (i = 3, 4, 5) J 12 = [ ] J è triangolare a blocchi det(j) = det(j 11 )det(j 22 ) Condizioni di singolarità (di struttura portante e di polso) det(j 11 ) = 0 det(j 22 ) = 0

112 Singolarità di polso Essendo J 22 = [ z 3 z 4 z 5 ] (con z 4 z 5 ) Due singolarità cinematiche per ϑ 5 = 0, ϑ 5 = π z 3 z 5, verificabile ovunque nello SdLR Perdita di mobilità della struttura rotazioni uguali e opposte di ϑ 4 e ϑ 6 non producono alcuna rotazione dell organo terminale la struttura non è in grado di imporre rotazioni intorno a direzioni z 3, z 4

113 Singolarità di struttura portante Si noti che, nonostante si abbia J P J 11, si può comunque utilizzare J P ai fini del calcolo delle singolarità Manipolatore antropomorfo det(j P ) = a 2 a 3 s 3 (a 2 c 2 + a 3 c 23 ) = 0 s 3 = 0 a 2 c 2 + a 3 c 23 = 0

114 Singolarità di gomito (s 3 = 0) ϑ 3 = 0 ϑ 3 = π ai confini dello SdLR

115 Singolarità di spalla (a 2 c 2 + a 3 c 23 = 0) p x = p y = 0 configurazioni singolari caratterizzabili nello spazio cartesiano (asse di rotazione del giunto 1) soluzioni al problema cinematico inverso ( valore di ϑ 1 genera lo stesso p) il manipolatore non è in grado di generare velocità lineari al piano della struttura

116 ANALISI DELLA RIDONDANZA Cinematica differenziale: v = J(q) q n : numero di gradi di mobilità della struttura (dimensione di q) r : numero di gradi di libertà del compito (componenti di v da specificare) n r : gradi di mobilità ridondanti Quindi v : vettore (r 1) delle componenti di velocità dell organo terminale necessarie per specificare il compito J : matrice (r n) estratta dalla Jacobiano (righe corrispondenti alle componenti di v)

117 Cinematica differenziale: trasformazione (configurazione dipendente) fra due sottospazi (sottospazio) immagine (range) di J : R(J) IR r (sottospazio) nullo di J : N(J) IR n in generale: dim ( R(J) ) + dim ( N(J) ) = n se (J) = r (Jacobiano di rango pieno): dim ( R(J) ) = r e dim ( N(J) ) = n r R(J) ricopre tutto IR r N(J) è non vuoto se n > r

118 Se N(J) q = q + P q 0 con q 0 arbitrario e R(P) N(J) (P è un proiettore in N(J)) Infatti J q = J q + JP q 0 = J q = v poichè JP q 0 = 0, q 0 Il vettore di velocità q 0 genera moti interni della struttura che non alterano v

119 INVERSIONE DELLA CINEMATICA DIFFERENZIALE Equazione cinematica non lineare: soluzioni in forma chiusa della cinematica inversa ricavabili solo per strutture semplici, non ridondanti e in configurazioni non singolari Equazione cinematica differenziale lineare nelle velocità Data v(t) + condizioni iniziali = (q(t), q(t)) se n = r q = J 1 (q)v q(t) = regola di integrazione numerica (Eulero) t 0 q(ς)dς + q(0) q(t k+1 ) = q(t k ) + q(t k ) t

120 Manipolatori ridondanti J è (r n) con r < n soluzioni Si pone un problema di ottimo vincolato Per una data configurazione q, trovare le soluzioni q che soddisfino la cinematica diretta (vincolo) v = J q e che minimizzino g( q) = 1 2 qt W q con W > O e simmetrica Metodo dei moltiplicatori di Lagrange (λ IR r ) g( q, λ) = 1 2 qt W q + λ T (v J q)

121 Condizioni necessarie ( ) T g = 0 W q J T λ = 0 q ( ) T g = 0 v J q = 0 λ Soluzione ottima q = W 1 J T (JW 1 J T ) 1 v che corrisponde ad un minimo poiché 2 g q 2 = W > O Se W = I (min locale di q ), la soluzione è dove J è la pseudo-inversa destra di J q = J v J = J T (JJ T ) 1

122 Utilizzo della ridondanza: per una data configurazione q, trovare le soluzioni q che soddisfino la cinematica diretta (vincolo) v = J q e che minimizzino g ( q) = 1 2 ( qt q T 0 )( q q 0 ) = 1 2 q q 0 2 (soluzioni prossime a q 0 e che soddisfano il vincolo cinematico) Procedendo sempre con il metodo dei moltiplicatori di Lagrange g ( q, λ) = 1 2 ( qt q T 0 )( q q 0 ) + λ T (v J q) si ha la soluzione ottima q = J v + (I J J) q 0 I J J è un proiettore in N(J) (I J J) q 0 rappresenta moti interni della struttura (soluzione omogenea)

123 Caratterizzazione dei moti interni: funzione obiettivo secondaria, w(q), da massimizzare (localmente) compatibilmente con l obiettivo primario (vincolo cinematico) ( ) T w(q) q 0 = k 0, k 0 > 0 q misura di manipolabilità w(q) = det ( J(q)J T (q) ) distanza dai fine-corsa dei giunti w(q) = 1 2n n ( qi q i i=1 q im q im ) 2 distanza da un ostacolo w(q) = min p,o p(q) o

124 Singolarità cinematiche Le soluzioni precedenti valgono solo se J è di rango pieno Se J non è di rango pieno (singolarità) il sistema v = J q contiene equazioni linearmente dipendenti se v R(J) soluzione q estraendo tutte le equazioni linearmente indipendenti (traiettoria fisicamente eseguibile) se v / R(J) il sistema non è risolvibile (traiettoria non eseguibile) Inversione nell intorno di singolarità: det(j) piccolo q elevate inversa a minimi quadrati smorzata (k > 0: fattore di smorzamento che rende meglio condizionata l inversione matriciale) J = J T (JJ T + k 2 I) 1 dove q minimizza (vincolo cinematico incorporato in g ) g ( q) = v J q 2 + k 2 q 2

125 ALGORITMI PER L INVERSIONE CINEMATICA Inversione cinematica q(t k+1 ) = q(t k ) + J 1 (q(t k ))v(t k ) t... o anche J, J fenomeni di deriva della soluzione Soluzione algoritmica errore nello spazio operativo e = x d x = x d k(q) ė = ẋ d ẋ = ẋ d J A (q) q trovare q = q(e): e 0 (fascia di tolleranza)

126 (Pseudo-)inversa dello Jacobiano Linearizzazione della dinamica di errore q = J 1 A (q)(ẋ d + Ke) ė + Ke = 0 Se K > O dinamica d errore A.S. Per un manipolatore ridondante q = J A (ẋ d + Ke) + (I J A J A) q a

127 Trasposta dello Jacobiano Si cerca di ricavare una legge q = q(e) senza linearizzare la dinamica di errore Metodo di Lyapunov V (e) = 1 2 et Ke con K simmetrica e d.p. V (e) > 0 e 0, V (0) = 0 Forma quadratica d.p. V (e) = e T Kẋ d e T Kẋ = e T Kẋ d e T KJ A (q) q La scelta q = J T A(q)Ke comporta che V (e) = e T Kẋ d e T KJ A (q)j T A (q)ke se ẋ d = 0 e J A di rango pieno V < 0 con V > 0 (asintotica stabilità di e = 0) se N(JA T) V = 0 se Ke N(JA T ) q = 0 con e 0 (stallo?)

128 Schema a blocchi Utilizzo di sole funzioni di cinematica diretta (k(q) e J A (q)) Se ẋ d 0 ( V non definita in segno) e(t) limitato (conviene aumentare la norma di K)

129 Esempio J T P = c 1 (a 2 s 2 + a 3 s 23 ) s 1 (a 2 s 2 + a 3 s 23 ) 0 N(JP T ) : a 3 c 1 s 23 a 3 s 1 s 23 a 3 c 23 ν y = 1, ν z = 0 ν x tanϑ 1 Se K = ki 3 e N(J T P ) (p d lungo la retta al piano della struttura passante per W) stallo (p d non fisicamente raggiungibile)

130 Confronto tra gli algoritmi per l inversione cinematica Manipolatore planare a tre bracci p x p y φ x = k(q) = a 1c 1 + a 2 c 12 + a 3 c 123 a 1 s 1 + a 2 s 12 + a 3 s 123 ϑ 1 + ϑ 2 + ϑ 3 a 1 = a 2 = a 3 = 0.5 m J = J A = a 1s 1 a 2 s 12 a 3 s 123 a 2 s 12 a 3 s 123 a 3 s 123 a 1 c 1 + a 2 c 12 + a 3 c 123 a 2 c 12 + a 3 c 123 a 3 c

131 Configurazione iniziale: q i = [ π π/2 π/2] T rad p di = [ 0 0.5] T m, φ di = 0 rad Traiettoria desiderata: p d (t) = [ ] 0.25(1 cosπt), φ 0.25(2 + sin πt) d (t) = sin π 24 t, 0 t 4 y x Simulazione in MATLAB con integrazione numerica di Eulero ( t = 1 ms) q(t k+1 ) = q(t k ) + q(t k ) t

132 Integrazione a ciclo aperto di q = J 1 A (q)ẋ deriva numerica 2 x 10 3 pos error norm 0 x 10 5 orien error [m] 1 [rad] [s] [s]

133 Algoritmo di inversione a ciclo chiuso q = J 1 A (q)(ẋ d +Ke) K = diag{500, 500, 100} deriva numerica assente errori di inversione più piccoli 5 joint pos 10 joint vel [rad] [rad/s] [m] [s] x 10 5 pos error norm [s] [s] [rad] x 10 8 orien error [s]

134 Ridondanza funzionale: φ libero (r = 2, n = 3) (senza sfruttamento della ridondanza) q = J P (ṗ d + K P e P ) K P = diag{500, 500} 5 x 10 6 pos error norm 0.5 orien 4 [m] 3 2 [rad] [s] [s] q = J T P (q)k Pe P K P = diag{500, 500} 0.01 pos error norm 0.5 orien [m] [rad] [s] [s]

135 q = J P (ṗ d+k P e P )+(I J ( ) T w(q) P J P) q 0, q 0 = k 0, K P = diag{500, 500}, k 0 = 50 q w(q) = w(ϑ 2, ϑ 3 ) = 1 2 (s2 2 + s 2 3) (misura di manipolabilità) 5 1 joint pos 5 1 joint pos [rad] [rad/s] [s] [m] 5 x pos error norm [s] [rad] [s] manip [s]

136 3 ( qi q i ) 2 (distanza dai fine-corsa dei giunti), k 0 = 250 w(q) = 1 6 i=1 q im q im 2π q 1 2π, π/2 q 2 π/2, 3π/2 q 3 π/2 [rad] joint 1 pos [s] joint 3 pos 5 [rad] joint 2 pos [s] 2 x pos error norm [rad] 0 [m] [s] [s]

137 STATICA Relazione tra forze e momenti (forze) γ all organo terminale e forze e/o coppie (coppie) τ ai giunti con il manipolatore in configurazione di equilibrio lavoro elementare compiuto dalle coppie ai giunti (τ R n ) dw τ = τ T dq lavoro elementare compiuto dalle forze all organo terminale (γ R r ) dw γ = f T dp + µ T ωdt = f T J P (q)dq + µ T J O (q)dq = γ T J(q)dq vincoli olonomi e indipendenti da t spostamenti elementari spostamenti virtuali { δwτ = τ T δq Lavori virtuali associati ai due sistemi di forze δw γ = γ T J(q)δq

138 Principio dei lavori virtuali il manipolatore è in equilibrio statico se e solo se δw τ = δw γ δq τ = J T (q)γ δw γ = 0, δq N(J) τ = 0 all equilibrio

139 Dualità cineto-statica v = J(q) q τ = J T (q)γ N(J) R (J T ) R(J) N (J T ) Forze γ N(J T ) interamente assorbite dalla struttura (reazioni vincolari) un manipolatore in configurazione singolare resta nella q corrente γ N(J T )

140 Interpretazione fisica dello schema con la trasposta dello Jacobiano dinamica ideale (massa nulla e attrito unitario): τ = q τ = J T Ke forza elastica Ke che tira l organo terminale verso la postura desiderata nello spazio operativo ha effetto solo se Ke / N(J T )

141 ELLISSOIDI DI MANIPOLABILITÀ Indici di valutazione delle prestazioni di un manipolatore attitudine ad eseguire un dato compito Ellissoide di manipolabilità in velocità insieme delle velocità ai giunti a norma costante q T q = 1 Sfera manipolatore ridondante in configurazione non singolare: q = J (q)v v T( J(q)J T (q) ) 1 v = 1 Ellissoide

142 Assi dell ellissoide { autovettori ui di JJ T direzioni valori singolari σ i = λ i (JJ T ) dimensioni Volume dell ellissoide proporzionale alla misura di manipolabilità w(q) = det ( J(q)J T (q) ) per manipolatori non ridondanti { w(q) > 0 in configurazione non singolare w(q) = 0 in configurazione singolare w(q) = det(j)

143 Ellissoide di manipolabilità in forza insieme delle coppie ai giunti a norma costante τ T τ = 1 Sfera γ T( J(q)J T (q) ) γ = 1 Ellissoide Dualità cineto-statica stessi assi, dimensione in proporzione inversa una direzione lungo la quale si ha elevata manipolabilità in velocità è una direzione lungo la quale si ha scarsa manipolabilità in forza, e viceversa

144 Manipolatore planare a due bracci Misura di manipolabilità: w(q 2 ) = det(j) = a 1 a 2 s 2 misura normalizzata: w(q 2 ) = s 2 nei casi più complessi: w(q) = 1 cond(j) = σ r σ 1 w : max ϑ2 per ϑ 2 = ±π/2, max a1,a 2 per a 1 = a 2 (a parità di estensione a 1 + a 2 ) Valori singolari (a 1 = a 2 = 1) [m] max min [m]

145 Ellissi di manipolabilità in velocità (a 1 = a 2 = 1) [m] [m] Ellissi di manipolabilità in forza (a 1 = a 2 = 1) [m] [m]

146 Manipolatore trasformatore meccanico di velocità e forze dallo spazio dei giunti allo spazio operativo rapporto di trasformazione lungo la direzione u per l ellissoide in forza α(q) = ( ) 1/2 u T J(q)J T (q)u rapporto di trasformazione lungo la direzione u per l ellissoide in velocità β(q) = (u T( J(q)J T (q) ) 1 u ) 1/2 conservazione energia: α dove β e viceversa utilizzo dei gradi di mobilità ridondanti

147 Compatibilità della struttura ad eseguire un compito assegnato lungo una direzione compiti di attuazione (elevato rapporto di trasformazione) compiti di controllo (basso rapporto di trasformazione) Scrittura su superficie orizzontale (controllo), lancio di un peso in direzione orizzontale (attuazione) velocity force velocity writing plane force throwing direction

148 DINAMICA Equazioni del moto del manipolatore in funzione delle forze e momenti agenti su di esso Formulazione di Lagrange Proprietà notevoli del modello dinamico Dinamica diretta e dinamica inversa

149 FORMULAZIONE DI LAGRANGE Lagrangiana (energia cinetica) (energia potenziale) L = T U λ i, i = 1,...,n: coordinate generalizzate λ 1. = λ n q 1. = q q n Equazioni di Lagrange d L dt λ L = ξ i i λ i i = 1,...,n ξ i, i = 1,...,n: forze generalizzate associate alle λ i (forze esterne, forze non conservative)

150 Equazioni del moto L(q, q) = T (q, q) U(q) = 1 2 n n n ( b ij (q) q i q j + mli g0 T p l i (q)+m mi g0 T p m i (q) ) i=1 j=1 i=1 ( ) d L = d ( ) T = dt q i dt q i T q i = 1 2 U q i = = n j=1 k=1 n j=1 n j=1 n n b ij (q) q j + j=1 b jk (q) q i ( m lj g T 0 ( m lj g T 0 j (l j) q k q j n j=1 p lj q i + m mj g T 0 n i=1 db ij (q) dt n j=1 p mj q i ) Pi (q) + m m j g0 T j (m j) Pi q j = ) (q) n n b ij (q) q j + j=1 n j=1 k=1 n i=1 = g i (q) n j=1 n n j=1 k=1 b ij (q) q k q k q j

151 Quindi (per i = 1,...,n) n b ij (q) q j + j=1 n j=1 k=1 n h ijk (q) q k q j + g i (q) = ξ i con h ijk = b ij 1 b jk q k 2 q i termini in accelerazione: b ii : rappresenta il momento di inerzia visto all asse del giunto i, nella configurazione corrente del manipolatore, quando gli altri giunti sono bloccati (non dipende da q i ) b ij : tiene conto dell effetto dell accelerazione del giunto j sul giunto i termini quadratici in velocità: h ijj q j 2 : rappresenta l effetto centrifugo indotto al giunto i dalla velocità del giunto j; si noti che h iii = 0, poiché b ii / q i = 0 h ijk q j q k : rappresenta l effetto di Coriolis indotto al giunto i dalle velocità dei giunti j e k termini dipendenti solo dalla configurazione: g i : rappresenta la coppia generata all asse del giunto i nella configurazione corrente del manipolatore per effetto della gravità

152 Forze non conservative (ξ i ) coppie di attuazione: τ coppie di attrito viscoso: F v q coppie di attrito statico: f s (q, q) F s sgn ( q) coppie di bilanciamento di forze di contatto (interazione manipolatore-ambiente): J T (q)h Modello dinamico nello spazio dei giunti B(q) q + C(q, q) q + F v q + f s ( q) + g(q) = τ J T (q)h con C = {c ij } i,j=1,n, dove i cofficienti c ij sono scelti in modo tale che: n c ij q j = j=1 n n h ijk q k q j j=1 k=1... sono possibili diverse scelte per i coefficienti c ij

153 PROPRIETÀ NOTEVOLI DEL MODELLO DINAMICO Anti-simmetria della matrice N(q, q) = se i c ij sono scelti opportunamente, si ha Ḃ(q) 2C(q, q) w T N(q, q)w = 0, w IR n N(q, q) = N T (q, q) Principio di conservazione dell energia (Hamilton) 1 2 d ( q T B(q) q ) = q T( τ F v q f s ( q) g(q) J T (q)h ) dt q T N(q, q) q = 0 valida scelta deic ij

154 Linearità nei parametri dinamici vettore (11 1) di parametri dinamici (diventa (13 3) con gli attriti) π i = [ m i m i l Ci x m i l Ci y m i l Ci zîixx Îixy Î ixz Î iyy Î iyz Î izz I mi ] T Î i : tensore d inerzia valutato rispetto all origine della Σ i m i ri,c i i : momento primo d inerzia equazioni del moto in forma lineare nei parametri τ = Y (q, q, q)π con Y = y11 T y12 T... y1n T 0 T y22 T... y2n T, π = T 0 T... ynn T π 1 π 2. π n

155 Manipolatore planare a due bracci Motori masse m mi momenti di inerzia I mi p mi = p i 1 z mi = z i 1

156 B(q) = [ ] b11 (ϑ 2 ) b 12 (ϑ 2 ) b 21 (ϑ 2 ) b 22 b 11 = I l1 + m l1 l kr1i 2 m1 + I l2 + m l2 (a l a 1 l 2 c 2 ) + I m2 + m m2 a 2 1 b 12 = b 21 = I l2 + m l2 (l a 1 l 2 c 2 ) + k r2 I m2 b 22 = I l2 + m l2 l k2 r2 I m2 C(q, q) = [ h ϑ2 h( ϑ 1 + ϑ ] 2 ) h ϑ 1 0 g(q) = h 111 = 1 2 b 11 h 122 = b 12 1 q 2 2 h 212 = h 221 = 1 2 [ g1 g 2 ] q 1 = 0 h 112 = h 121 = 1 2 b 22 = h h 211 = b 21 1 q 1 q 1 2 b 22 = 0 h 222 = 1 b 22 = 0 q 1 2 q 2 b 11 q 2 = m l2 a 1 l 2 s 2 = h b 11 q 2 = h g 1 = (m l1 l 1 + m m2 a 1 + m l2 a 1 )gc 1 + m l2 l 2 gc 12 g 2 = m l2 l 2 gc 12

157 Anti-simmetria N(q, q) = Ḃ(q) 2C(q, q) = [ 2h ϑ2 h ϑ 2 h ϑ 2 0 ] [ h ϑ2 h( 2 ϑ 1 + ϑ ] 2 ) h ϑ 1 0 = [ 0 2h ϑ 1 h ϑ 2 2h ϑ 1 + h ϑ 2 0 ] Equazioni del moto ( Il1 + m l1 l k2 r1 I m 1 + I l2 + m l2 (a l a 1l 2 c 2 ) + I m2 + m m2 a 2 ) 1 ϑ1 + ( I l2 + m l2 (l a ) 1l 2 c 2 ) + k r2 I m2 ϑ2 2m l2 a 1 l 2 s 2 ϑ1 ϑ2 m l2 a 1 l 2 s 2 ϑ2 2 + (m l1 l 1 + m m2 a 1 + m l2 a 1 )gc 1 + m l2 l 2 gc 12 = τ 1 ( Il2 + m l2 l k2 r2 I ) m 2 ϑ2 + ( I l2 + m l2 (l a ) 1l 2 c 2 ) + k r2 I m2 ϑ1 + m l2 a 1 l 2 s 2 ϑ2 1 + m l2 l 2 gc 12 = τ 2

158 DINAMICA DIRETTA E DINAMICA INVERSA Dinamica diretta (simulazione) note q(t 0 ), q(t 0 ), τ(t) (e h(t)), determinare q(t), q(t), q(t) per t > t 0 Dinamica inversa (pianificazione e controllo) note q(t), q(t), q(t) (e h(t)), determinare τ(t) Per un manipolatore ad n giunti, il numero di operazioni per il calcolo della dinamica è: O(n 2 ) per la dinamica diretta, O(n) per la dinamica inversa Schema computazionale per la dinamica diretta q = B 1 (q)(τ τ ) τ (q, q) = C(q, q) q + F v q + f s ( q) + g(q) + J T (q)h note q(t k ), q(t k ), τ(t k ), si calcola q(t k ) = B 1 (q(t k ))(τ(t k ) τ (q(t k ), q(t k ))) si integra numericamente con passo t per calcolare q(t k+1 ) e q(t k+1 ), ad esempio, mediante l algoritmo di Eulero: q(t k+1 ) = q(t k ) + q(t k ) t, q(t k+1 ) = q(t k ) + q(t k ) t