Corso di Robotica 1 Cinematica inversa

Transcript

1 Corso di Robotica 1 Cinematica inversa Prof. Alessandro De Luca Robotica 1 1

2 Problema cinematico inverso data una posa (posizione + orientamento) dell end-effector determinare l insieme di valori delle variabili di giunto che la realizza problema di sintesi, con dati nella forma: T = R p r = tipico problema nonlineare p φ esistenza soluzione (definizione workspace) unicità/molteplicità soluzione metodi di risoluzione Robotica 1

3 Risolubilità e spazio di lavoro workspace primario WS 1 : insieme dei punti p raggiungibili con almeno un orientamento (φ o R) fuori da WS 1 il problema non ha soluzione per p WS 1 e φ (o R) opportuno esiste almeno una soluzione workspace secondario WS (o di destrezza): insieme dei punti p raggiungibili con qualsiasi orientamento (tra quelli realizzabili dal robot) per p WS esiste almeno una soluzione per ogni φ (o R) WS WS 1 Robotica 1 3

4 Spazio di lavoro Fanuc R-000i/165F WS (polso sferico) Robotica 1 4

5 Spazio di lavoro robot R planare y p orientamenti l 1 +l l q l 1 -l l 1 if l 1 l q 1 WS 1 = {p R : l 1 -l p l 1 +l } WS = if l 1 = l = l x WS 1 1 orientamento WS 1 = {p R : p l} WS = {p = 0} (infiniti orientamenti possibili all origine) Robotica 1 5

6 Posizionamento robot articolato SN DW Unimation PUMA 560 DX DW 4 soluzioni fuori dalle singolarità SN UP 8 soluzioni se si considera anche l orientamento dell E-E (polso sferico: soluzioni alternative per gli ultimi 3 giunti) DX UP Robotica 1 6

7 Molteplicità soluzioni posizionamento robot R planare soluzioni in WS 1 1 soluzione su WS 1 per l 1 = l : soluzioni in WS posizionamento robot 3R articolato singolari 4 soluzioni in WS 1 robot 6R 16 soluzioni, fuori dalle singolarità: questo upper bound è effettivamente raggiunto da un particolare robot ortogonale, cioè con α i = 0, ±π/ ( i) analisi basata su trasformazioni algebriche della cinematica in una equazione polinomiale di grado più basso possibile Robotica 1 7

8 y l Spazio di lavoro robot 3R planare q 3 q l 3 p l 1 = l = l 3 = l WS 1 = {p R : p 3l} l 1 q 1 1. in WS 1 : 1 soluzioni tutte non singolari (tranne casi. e 3.) in cui l E-E può assumere un continuum di orientamenti (ma non tutti!). if p = 3l : 1 soluzione singolare 3. if p = l : 1 soluzioni di cui 3 singolari x WS = {p R : p l} 4. if p < l : 1 soluzioni mai singolari, qualsiasi orientamento (WS ) Robotica 1 8

9 Molteplicità soluzioni sommario generale dei casi se m = n soluzioni soluzioni multiple in numero finito (caso generico) infinite soluzioni degeneri o un numero finito diverso da quello generico (in singolarità) se m < n (robot ridondante per il compito) soluzioni n-m soluzioni soluzioni singolari in numero finito o infinito Robotica 1 9

10 Robot Dexter 8R m = 6 (posizione e orientamento dell organo terminale) n = 8 (tutti giunti rotatori) soluzioni cinematiche inverse (ridondanza) video esplorazione delle soluzioni cinematiche inverse con un auto-movimento Robotics 1 10

11 Metodi di soluzione soluzione ANALITICA (in forma chiusa) soluzione NUMERICA (in forma iterativa) preferibile, se è possibile trovarla * ispezione geometrica ad hoc metodi algebrici (soluzione equazioni polinomiali * ) metodi sistematici di generazione di set minimali di equazioni da risolvere * condizioni sufficienti per un 6R 3 assi di giunto rotoidali consecutivi incidenti (es. polso sferico), oppure 3 assi di giunto rotoidali consecutivi paralleli necessaria se n>m (ridondanti) o in singolarità più lenta, ma di facile derivazione richiede il calcolo dello Jacobiano analitico della cinematica diretta J r (q) = f r (q) q metodi di Newton, del gradiente, ecc. Robotica 1 11

12 y p y Cinematica inversa R l q p cinematica diretta p x = l 1 c 1 + l c 1 l 1 p y = l 1 s 1 + l s 1 q 1 p x x dati q 1, q incognite quadrando e sommando le equazioni della cinematica diretta p x + p y - (l 1 + l ) = l 1 l (c 1 c 1 + s 1 s 1 ) = l 1 l c da cui in forma analitica c = (p x + p y - l 1 - l )/ l 1 l, s = ± 1 - c, q = ATAN {s, c } deve essere in [-1,1] (altrimenti, è fuori dal workspace!) soluzioni Robotica 1 1

13 y p y β q 1 Cinematica inversa R (continua) α q p p x soluzioni (una per ogni valore di s ) {q 1,q } UP/LEFT x per ispezione geometrica q 1 = α - β q 1 = ATAN {p y, p x } - ATAN {l s, l 1 + l c } q 1 q 1 q q p N.B. da riportare eventualmente in [-π, π ]! {q 1,q } DW/RIGHT q e q uguali in modulo e opposti in segno Robotica 1 13

14 Soluzione (algebrica) alternativa p x = l 1 c 1 + l c 1 = l 1 c 1 + l (c 1 c - s 1 s ) p y = l 1 s 1 + l s 1 = l 1 s 1 + l (s 1 c + c 1 s ) lineari in s 1 e c 1 l 1 + l c - l s c 1 = p x l s l 1 + l c s 1 p y det = (l 1 + l + l 1 l c ) > 0 q 1 = ATAN {s 1, c 1 } tranne per l 1 = l e c = -1 per cui q 1 non è definita Note: a) automaticamente in [-π, π ]! b) in ATAN, dalle espressioni di s 1 e c 1 si può togliere il det >0 Robotica 1 14

15 Cinematica inversa robot polare p z q 3 p x = q 3 c c 1 p y = q 3 c s 1 p z = d 1 + q 3 s d 1 q p y p x + p y + (p z - d 1 ) = q 3 q 3 = + p x + p y + (p z - d 1 ) p x q 1 qui solo, q 3 0 q = ATAN{(p z - d 1 )/q 3, ± (p x + p y ) /q 3 } se c 0, q 1 = ATAN{p y /c, p x /c } ( soluzioni {q 1,q,q 3 }) altrimenti, q 1 indeterminata (singolarità, soluzioni) Robotica 1 15

16 Cinematica inversa per robot con polso sferico x 0 x z 6 0 j5 j4 j1 primi 3 giunti (RRR, RRP, PPP, ) y 0 j6 W z 4 z 3 z 5 d 6 y 6 O 6 = p z 6 = a trovare q 1,, q 6, assegnati: p (origine O 6 ) R = [n s a] (orientamento SR 6 ) 1. W = p - d 6 a q 1, q, q 3 (cinematica inversa struttura portante). R = 0 R 3 (q 1, q, q 3 ) 3 R 6 (q 4, q 5, q 6 ) q 4, q 5, q 6 (cinematica inversa polso) data nota, dopo 1. matrice di Eulero ZYZ o ZXZ Robotica 1 16

17 Esempio 6R: Unimation PUMA 600 polso sferico 0 x 6 0 y 6 0 z 6 esistono 8 soluzioni inverse in forma chiusa (vedi Paul, Shimano, Mayer; 1981) Robotica 1 17

18 la soluzione analitica di r = f(q) non esiste o è di difficile derivazione J r (q) = f r q (Jacobiano analitico) metodo di Newton (qui nel caso m=n) Soluzione numerica della cinematica inversa r = f r (q) = f r (q k ) + J r (q k ) (q - q k ) + o( q - q k ) si trascura q k+1 = q k + J r -1 (q k ) [r - f r (q k )] converge se q 0 (stima iniziale) sufficientemente vicina a q * : f r (q * ) = r ha problemi vicino alle singolarità dello Jacobiano J r (q) non si applica per robot ridondanti (m<n) [uso pseudoinversa J r# (q)] ha convergenza quadratica vicino alla soluzione (rapido!) Robotica 1 18

19 Soluzione numerica della cinematica inversa (continua) metodo del gradiente (massima discesa) si minimizza la funzione d errore H(q) = ½ r - f r (q) = ½ [r - f r (q)] T [r - f r (q)] q k+1 = q k - α q H(q k ) poiché H(q) = - J rt (q) [r - f r (q)], si ottiene q k+1 = q k + α J rt (q k ) [r - f r (q k )] il passo α deve garantire che la funzione d errore decresca ad ogni passo (per α troppo grande si potrebbe scavalcare il minimo) per α troppo piccolo, la convergenza è eccessivamente lenta Robotica 1 19

20 Interpretazione in feedback r d + - e J rt (q) f r (q). q q(0) q r d = cost (α = 1) e = r d - f r (q) 0 sistema ad anello chiuso asintoticamente stabile V = ½ e T e 0 candidata di Lyapunov... V = e T e = e T d ( q dt (r d - f r (q)) = - et J r = - e T J r J rt e 0. V = 0 e Ker(J rt ) in particolare e = 0 asintotica stabilità Robotica 1 0

21 Proprietà dello schema con gradiente più semplice dal punto di vista computazionale [Jacobiano trasposto, anziché (pseudo)-invertito] si applica direttamente anche a robot ridondanti può non convergere a una soluzione, ma non diverge mai l evoluzione della stima a tempo discreto è data da q k+1 = q k + ΔT J rt (q k ) [r - f(q k )] che equivale a un passo del metodo del gradiente (con α=δt) si può accelerare con una matrice di guadagno K>0, ponendo. q = J rt (q) K e nota: K può usarsi per uscire da una situazione di stallo in un minimo locale: permette la rotazione dell errore e fuori dal nucleo di J T r (se si è incontrata una singolarità) Robotica 1 1

22 Considerazioni aggiuntive uno schema numerico efficiente coinvolge: iterazioni iniziali con il gradiente (convergenza sicura, ma più lenta = lineare) e finali con Newton (convergenza terminale con tasso quadratico) scelte inizializzazione q 0 (genera una sola delle soluzioni) passo ottimale α nel metodo del gradiente criteri di arresto r - f(q k ) ε q k+1 -q k ε q σ min {J(q k )} σ 0 errore cartesiano (evtl. separato per posizione e orientamento) incremento dell algoritmo condizionamento dello Jacobiano (SVD) Robotica 1