Robot con ridondanza cinematica

Transcript

1 Corso di Robotica 2 Robot con ridondanza cinematica Prof. Alessandro De Luca A. De Luca

2 Robot ridondanti cinematica diretta del compito r = f(q) f: Q R spazio dei giunti (dim Q = N) spazio del compito (dim R = M) robot ridondante se N > M (più gradi di libertà di quelli necessari ad eseguire il compito) r può essere la posizione e/o orientamento dell organo terminale o qualsiasi altra parametrizzazione del compito (anche non nello spazio di lavoro) si parla quindi propriamente di ridondanza solo rispetto a un compito (task) A. De Luca 2

3 Alcuni compiti e la loro dimensione TASK (per l organo terminale) dimensione M posizionamento nel piano 2 posizionamento nello spazio 3 orientamento nel piano 1 puntamento nello spazio 2 posizione e orientamento nello spazio 6 un robot planare con N = 3 giunti è ridondante rispetto al posizionamento nel piano (M = 2), ma non rispetto al posizionamento + orientamento nel piano (M = 3) A. De Luca 3

4 Casi tipici di robot ridondanti robot 6R montato su binario robot a 6-dof usato per saldature ad arco il compito non richiede di specificare l angolo finale di roll manipolatore su base mobile mano robotica ad elevata destrezza squadre di robot mobili cooperanti la ridondanza cinematica non è l unico tipo ridondanza di componenti (attuatori, sensori) ridondanza nell architettura di controllo e supervisione A. De Luca 4

5 Robot del DLR: LWR-III e Justin manipolatore 7R 2 bracci 7R + tronco 3R + 2 mani A. De Luca 5

6 Esempi video - 1 Dexter 8R: automovimento Planare 6R: obstacle avoidance su cammino assegnato dell E-E A. De Luca 6

7 Esempi video - 2 Uniciclo + planare 2R: controllo di traiettoria con compito su link 1 Magellan + 3R: controllo di traiettoria con puntamento dell E-E A. De Luca 7

8 Esempi video - 3 Magellan + 3R: visual servoing nello spazio immagine di 2 features (M=4) A. De Luca 8

9 La ridondanza può essere usata per evitare ostacoli (nello spazio cartesiano) o singolarità cinematiche (nello spazio dei giunti) aumentare la manipolabilità in determinate direzioni distribuire uniformemente/limitare le velocità di giunto rimanere entro i limiti di fondo corsa dei giunti minimizzare il consumo di energia ottimizzare il tempo di percorrenza, le coppie richieste aumentare l affidabilità rispetto a guasti obiettivi misurabili vantaggi ottenibili al costo di A. De Luca 9

10 Svantaggi della ridondanza maggiore complessità strutturale meccanica (bracci, trasmissioni) di attuazione algoritmi di cinematica inversa e di controllo più complessi A. De Luca 10

11 Problema cinematico inverso trovare q(t) che realizzi f(q(t)) = r(t) (il compito nel tempo) esistono infinite soluzioni se il robot è ridondante (anche per r(t) = r = costante) N = 3 > 2 = M r = posizione E-E costante esistono movimenti interni che sono inosservabili dal punto di vista dell esecuzione del compito (es., moto dell E-E) possono essere scelti in modo da ottimizzare in qualche modo il comportamento del sistema automovimento (self-motion): riconfigurazione del braccio che lascia inalterato il valore delle variabili r del compito A. De Luca 11

12 Risoluzione della ridondanza (come ottimizzazione di una funzione obiettivo) Locale Globale. note r(t) e q(t), t = kt s note r(t), t [t 0, t 0 + T], q(t 0 ) (ottimizzazione di H(q, q& )) t 0 + T (ottimizzazione di H(q, q )d") # t 0 q&(t) ON-LINE q(t), t [t 0, t 0 + T] q ((k +1)T s ) = q(kt s ) + T s " q (kt s ) OFF-LINE A. De Luca 12

13 Metodi di risoluzione locale Tre classi di metodi per risolvere r = J(q) q metodi basati sullo Jacobiano (qui, analitico!) tra le infinite soluzioni, si sceglie ad esempio quella che minimizza una certa norma (eventualmente) pesata metodi con lo spazio nullo si aggiunge alla soluzione a norma minima un contributo che non modifica la traiettoria del compito, ovvero che ℵ(J(q)) metodi basati sul compito aumentato si riduce/elimina la ridondanza aggiungendo S N-M compiti ausiliari (se S = N-M, si quadra il problema) A. De Luca 13

14 Metodi Jacobian-based si cerca una soluzione di J = 1 N M r = J(q) q q = K(q) r K = nella forma requisito minimo per K: J(q)K(q)J(q) = J(q) ( K inversa generalizzata di J) M N " r #$( J(q) ) J(q) [ K(q) r ] = J(q)K(q)J(q) q = J(q) q = r # ad es., se J = [J a J b ], det(j a ) 0, un inversa (destra) di J è K r = J a % $ 0 "1 & ( ' A. De Luca 14

15 Pseudoinversa q = J # (q) r scelta per K = J # J # esiste sempre ed è l unica matrice che soddisfa J J # J = J J # J J # = J # (J J # ) T = J J # (J # J) T = J # J se J è a rango pieno, J # = J T (J J T ) -1 ; altrimenti va calcolata numericamente mediante SVD (Singular Value Decomposition) della J la soluzione q minimizza la norma 1 2 q 2 = 1 2 q T q A. De Luca 15

16 Pseudoinversa pesata q& = J # w (q)p& scelta per K = J # w se J è a rango pieno, = W -1 J T (J W -1 J T ) -1 la soluzione q minimizza la norma pesata 1 2 T 2 q& = w 1 2 q& # J w Wq& W > 0, simmetrica (spesso diagonale) peso W i grande piccola q i (ad es., pesi scelti inversamente proporzionali alle escursioni dei giunti) non è una pseudoinversa (non soddisfa la IV a relazione) A. De Luca 16.

17 Svantaggi dei metodi Jacobian-based non è detto che si evitino globalmente le singolarità durante l esecuzione del compito potrebbero fornire soluzioni non ripetibili cammini ciclici nello spazio del compito non corrispondono a cammini ciclici nello spazio dei giunti dopo 1 giro q in q fin q in q(t) = q in + t # K(q) r (")d" 0 A. De Luca 17

18 Robustezza a singolarità: Inversa Smorzata ottimizzazione non vincolata min q " 2 q J q # r 2 = H( q ) minimi quadrati smorzati (DLS) SOLUZIONE q = J T ( JJ T + "I) #1 r è robusta rispetto alle singolarità (si può usare anche per N = M), ma ne risulta un errore sulla traiettoria del compito scelta del fattore di smorzamento λ 0 di compromesso fra minima norma delle velocità di giunto q& e minimo errore di traiettoria r " J q (scelta ragionevole: λ(q) 0 funzione del minimo valore singolare di J distanza da una singolarità) A. De Luca 18

19 2 una soluzione particolare (qui la pseudoinversa) Metodi con lo spazio nullo.. soluzione generale di J q = r q = J # r + ( I " J # J) q 0 più in generale proiettore nel ℵ(J) simmetrica q = K 1 r + I "K 2 J idempotente: [I J # J] 2 = [I J # J] [I J # J] # = [I J # J] ( ) q 0. come scegliere q 0? tutte le soluzioni dell equazione omogenea associata Jq = 0 (automovimenti) proprietà di [I J # J] K 1, K 2 inverse generalizzate di J (J K i J = J) A. De Luca 19.

20 Ottimizzazione lineare-quadratica Generalità 1 min x ( 2 x " x 0) T W( x " x 0 ) = H(x) c. nec. minimo + c. suff. s.t. J x = y L(x, ") T M x N = H(x) + " " x L = #L #x " % L = Jx $ y = 0 2! x L = W > ( Jx! y) = W( x $ x 0 ) + J T % = 0 0 " = ( JW #1 J T ) #1 ( Jx 0 # y) T M x M invertibile x!ir (simmetrica) A. De Luca 20 N, W > 0 M y!i R, ρ(j) = M Lagrangiana! 1 x = x Jx0! JW J "! y = T x = x 0 " W "1 J T " 1 T 0 " W J 0! ( JW "1 J T ) "1 ( Jx 0 " y)

21 Ottimizzazione lineare-quadratica Applicazione ai robot ridondanti PROBLEMA min q 1 q 2 " ( ) T W( q " ) = H( q 0 J(q) q = r q 0 q ). q 0 è una velocità di giunto privilegiata SOLUZIONE q = q 0 " W "1 J T ( ) ( JW "1 J T ) "1 J q 0 " r # J W q = J W# r + ( I " J # W J) q 0 soluzione a minima. norma pesata (per q 0 = 0) proiettore nel nucleo ℵ(J) A. De Luca 21

22 Gradiente proiettato (PG) q = J # r + ( I " J # J) q 0. la scelta q 0 = q H(q) funzione obiettivo differenziabile realizza un passo di un algoritmo di ottimizzazione vincolata mentre si muove lungo il compito r(t) il robot tende ad aumentare il valore di H(q) gradiente proiettato q 3 q H - ad es., per r fissato. q - S q = {q IR N : f(q) = r - }. q = (I - J # J) q H S q q 2 Robotica 2 q 1 A.A A. De Luca 22

23 Tipiche funzioni obiettivo H(q) manipolabilità (massimizza la distanza dalle singolarità) H man (q) = det J(q)J T (q) joint range (minimizza la distanza dal centro delle escursioni di giunto) H range (q) = 1 2. N # q i " q i & ) 2N % $ q M,i " q ( i=1 m,i ' q 0 = - q H(q) obstacle avoidance (massimizza la distanza dagli ostacoli) H obs (q) = min a"robot b"ostacoli a(q) #b 2 possibili problemi di differenziabilità in questo problema di max-min A. De Luca 23

24 in un compito di posizionamento Singolarità 3R planare H(q) = sin 2 q 2 + sin 2 q 3 -π non è la H man ma ha gli stessi minimi! curve di livello H(q) π q 3 -π q 2 q 3 π -π π q 3 -π q 2 π q 2 indipendente da q 1! A. De Luca 24

25 Commenti sui metodi con lo spazio nullo il proiettore nel nullo (I J # J) ha dimensione N x N ma rango solo N M (se J ha rango pieno), con spreco di informazione metodo effettivo di calcolo della soluzione q = J # r + ( I " J # J) q 0 = q 0 + J # r - J viene comunque richiesto il calcolo della pseudoinversa, oneroso computazionalmente (sia se a rango pieno che non) la complessità di un metodo di risoluzione della ridondanza dovrebbe in effetti dipendere solo dal grado di ridondanza N M esiste un metodo di ottimizzazione vincolata più efficiente del gradiente proiettato (PG) nel caso di Jacobiano a rango pieno q 0 ( ) A. De Luca 25

26 Gradiente ridotto (RG) se ρ(j(q)) = M, allora esiste una partizione dei giunti (a valle di un eventuale riordinamento dei giunti) M x M " q = q a % $ # q ' M b & N-M tale che J a (q) = " f " q a per il teorema delle funzioni implicite, una funzione g f(q a, q b ) = r q a = g(r, q b ) è nonsingolare " g $ con = # " f " q & b %" q a ' ) ( #1 " f = # J #1 a (q) J b (q) " q b si usano le N-M variabili indipendenti q b per ottimizzare un criterio H(q) ( ridotto tramite la g ad una funzione delle sole q b ) con il metodo del gradiente si scelgono poi le q a = g(r, q b ) per assicurare l esecuzione del compito A. De Luca 26

27 Algoritmo del gradiente ridotto H(q) = H(q a,q b ) = H(g(r,q b ),q b ) = H (q b ) (con r al valore corrente) gradiente ridotto (rispetto alle sole q b ) [ " q b H' = #( J #1 a J ) T ] b I " qh ( " # q b H!! ) ALGORITMO q b = " q b H' J a q a + J b q b = r "1 q a = J a ( r " J b ) q b passo nella direzione del gradiente nello spazio ridotto (N-M)-dim soddisfacimento degli M vincoli del compito A. De Luca 27

28 Confronto tra i metodi PG e RG Projected Gradient (PG) Reduced Gradient (RG) q = " q a % $ # q ' = " J a $ b & # 0 q = J # r + ( I " J # J)# q H (1 % ' " r + (J a $ & # I (1 J b % ' ( J a & ( ( (1 J ) T ) b I ) H q RG più conveniente di PG sia dal punto di vista analitico che numerico, ma richiede l individuazione di un minore non singolare (J a ) nello Jacobiano. se r = cost & N-M=1 stessa direzione di q, ma passo RG più lungo altrimenti, RG e PG forniscono evoluzioni diverse A. De Luca 28

29 Esempio analitico: robot PPR l q 1 q 2 q 3 # J = 1 0 "ls 3 & % $ 0 1 lc ( 3 ' = J a J b RG: q = " 1 0% $ 0 1 ' $ # 0 0 ' & r + " $ $ # ( ) ls 3 (lc 3 1 " q a = q 1 % $ # q ' q b = q 3 2 & q = J "1 # a & % ( $ 0 # r + "J a % ' $ I "1 J b % '( ls 3 (lc 3 1)) q H ' & ( ( "1 J b ) T I) ) H q & ( " J a ' PG: q = J # r + ( I " J # J)# q H J # = l l 2 2 # c 3 l 2 s 3 c 3 & % l 2 s 3 c l 2 2 ( s 3 % $ "ls 3 lc ( 3 ' ( I " J # J) = l 2 l 2 2 # s 3 sym& % "l 2 s 3 c 3 l 2 2 ( c 3 % $ ls 3 "lc 3 1 ( ' < 1!! A. De Luca 29

30 " % $ ' q = $ ' ( = T( $ ' $ # ' & coordinate assolute relative # & % ( " = % ( q = T -1 q % ( % $ ( ' Limiti di giunto "90 # $ i # +90 "90 # q i " q i"1 # +90 G q 2 q 4 S compito: moto dell E-E da S a G q 1 confronto fra metodo della pseudoinversa (PS), gradiente proiettato (PG) e gradiente ridotto (RG) A. De Luca 30

31 Simulazione numerica 1 giunto 2 giunto 3 giunto 4 giunto A. De Luca 31

32 Obstacle avoidance gradiente ridotto pseudoinversa q = J # r r r ostacolo x con massimizzazione di H(q) = x sinq 4 " 3 # i=1 l i sin( q 4 " q i ) A. De Luca 32

33 Automovimento con manipolabilità (ottima) max H(q) = 3 # i=1 sin 2 ( q i+1 " q i ) in realtà, non è proprio la funzione di manipolabilità r 0 passi di integrazione RG più veloce di PG (a parità di accuratezza su r) A. De Luca 33

34 3 Metodi con compito aumentato si aggiunge un compito ausiliario (task augmentation) S f y (q) = y S N-M corrispondente a qualche aspetto desiderabile della soluzione " r A = r % $ # y ' & = " f(q) % $ # f y (q) ' & r A = " J(q) % $ # J y (q) ' q = J A (q) q & si cerca ancora una soluzione nella forma q = K A (q) r A J A N M+S o con l aggiunta di un termine nel nullo di J A A. De Luca 34

35 Con un compito aumentato vantaggio: maggiore controllo sulla soluzione inversa svantaggio: si introducono singolarità algoritmiche quando ρ(j) = M ρ(j y ) = S ma ρ(j A ) < M+S dovrebbe essere sempre "( J T ) # "( T J ) y = $ difficile da ottenere globalmente! righe di J e righe di J y linearmente indipendenti A. De Luca 35

36 Compito aumentato esempio r(t) N = 4, M = 2 coordinate di giunto assolute f y (q) = q 4 = π/2 (S = 1) ultimo braccio mantenuto verticale A. De Luca 36

37 Jacobiano esteso (S = N M) J A quadrato: in assenza di singolarità algoritmiche, si può prendere q = J A "1 (q) r A lo schema è evidentemente ripetibile se f y (q) = 0 corrisponde a condizioni necessarie (& sufficienti) di ottimo vincolato per una certa funzione H(q), lo schema assicura permanenza dell ottimalità durante l esecuzione del compito in presenza di singolarità algoritmiche, l esecuzione sia del compito originario che dei compiti aggiuntivi risulta affetta da errore A. De Luca 37

38 Jacobiano esteso esempio manipolatore MACRO-MICRO r(t) N = 4, M = 2 y(t) ( ) r = J q 1,Kq 4 q y = J y ( q 1,q 2 ) q " J A = * * % $ #* 0& ' 4 x 4 A. De Luca 38

39 Priorità di compito se il compito originale r = J(q) q ha una priorità maggiore del compito ausiliario y = J y (q) q si fa prima in modo che venga eseguito il compito prioritario q = J # r + (I " J # J)v e poi si sceglie v in modo da soddisfare (se possibile) anche il compito secondario y = J y (q) q = J y J # r + J y (I " J # J)v = J y J # r + J y v la soluzione generale per v è nella forma usuale v = J # y ( y " J y J # r ) + (I " J # y J y )w a disposizione per eventuali ulteriori compiti a priorità più bassa A. De Luca 39

40 Priorità di compito (cont.) sostituendo l espressione di v in q q = J # r + (I " J # J) J # y ( y " J y J # r ) + (I " J # J)(I " J # y J y )w J y # poiché C [BC] # = [BC] # eventualmente = 0 se C idempotente vantaggio della strategia: compito principale non più affetto dalle singolarità algoritmiche Task 1: seguire Task 2: terzo link verticale SENZA priorità di compito CON priorità di compito A. De Luca 40

41 Estensioni la trattazione dei metodi di risoluzione della ridondanza svolta finora: è definita al primo ordine differenziale (velocità) possibile lavorare in accelerazione non tiene conto di errori nell esecuzione del compito possibile usare un controllo cinematico si applica ai robot manipolatori a base fissa possibile considerare manipolatori mobili (su ruote) A. De Luca 41

42 Risoluzione al secondo ordine r = f(q) r = J(q) q posto nella forma J(q) q = r " J (q) q Δ = x da scegliere assegnata (al tempo t) r = J(q) q + J (q) q. note q e q (al tempo t) il problema è formalmente equivalente a prima, con l accelerazione al posto della velocità ad esempio, nel metodo con lo spazio nullo q = J # (q) x + ( I - J # (q)j(q) ) q 0 soluzione a minima norma di accelerazione 1 = " H #K 2 q 2 q D q stabilizzazione automovimenti A. De Luca 42

43 Controllo cinematico dato un compito desiderato r d (t) (M-dimensionale), per recuperare un errore e = r d - r, iniziale o successivo dovuto a disturbi linearizzazione intrinseca nello Jacobiano (moto al primo ordine) implementazione a tempo discreto è necessario chiudere un anello di feedback sul compito, sostituendo (con guadagni K o K P, K V > 0 e diagonali) r r r d + K r d "r ( ) r d + K v con r = f(q), r = J(q) q r d " r ( ) + K ( P r d "r) nei metodi in velocità in accelerazione A. De Luca 43

44 Manipolatori mobili si combinano coordinate della base q b e del manipolatore q m si utilizza il legame differenziale con i comandi effettivi u b disponibili sulla base e u m sul manipolatore q m r = f(q) (ad es., posa dell E-E) " q = q % b $ ' ( IR N # & q m q b = G(q b )u b q m = u m modello cinematico della base (con vincoli anolonomi) q b " u = u % b $ ' ( R N u I # & A. De Luca 44 u m N u N

45 Jacobiano manipolatore mobile r = f(q) = f(q b,q m ) r = " f(q) q b + " f(q) q m = J b (q) q b + J m (q) q m " q b " q m = J b (q)g(q b )u b + J m (q)u m = J b (q)g(q b ) J m (q) = J NMM (q)u e tutto segue sostituendo. J J NMM q u ( ) u b Nonholonomic Mobile Manipulator Jacobian (M x N u ) A. De Luca 45 # % $ u m (ridondanza se N u - M > 0) ossia gli effettivi comandi disponibili & ( '