Sei sulla pagina 1di 22

Corso di Robotica 1

Cinematica inversa
Prof. Alessandro De Luca

Robotica 1

Problema cinematico inverso

data una posa (posizione + orientamento) dellend-effector determinare linsieme di valori delle variabili di giunto che la realizza problema di sintesi, con dati nella forma:

T=

R p 000 1

r =

tipico problema nonlineare


esistenza soluzione (definizione workspace) unicit/molteplicit soluzione metodi di risoluzione


2

Robotica 1

Risolubilit e spazio di lavoro

workspace primario WS1: insieme dei punti p raggiungibili con almeno un orientamento ( o R)

fuori da WS1 il problema non ha soluzione per p WS1 e (o R) opportuno esiste almeno una soluzione

workspace secondario WS2 (o di destrezza): insieme dei punti p raggiungibili con qualsiasi orientamento (tra quelli realizzabili dal robot)

per p WS2 esiste almeno una soluzione per ogni (o R)

WS2 WS1

Robotica 1

Spazio di lavoro
Fanuc R-2000i/165F

WS2 (polso sferico)

Robotica 1

Spazio di lavoro
robot 2R planare
2 orientamenti y l2 l1 q1 x WS1 1 orientamento p q2 |l1-l2| l1+l2

if l1 l2

WS1 = {p R2: |l1-l2| p l1+l2} WS2 = WS1 = {p R2: p 2} WS2 = {p = 0} (infiniti orientamenti possibili allorigine)
5

if l1 = l2 =

Robotica 1

Posizionamento robot articolato


SN DW
Unimation PUMA 560

DX DW

4 soluzioni fuori dalle singolarit

SN UP

8 soluzioni se si considera anche lorientamento dellE-E (polso sferico: 2 soluzioni alternative per gli ultimi 3 giunti)

DX UP

Robotica 1

Molteplicit soluzioni

posizionamento robot 2R planare


posizionamento robot 3R articolato

2 soluzioni in WS1 1 soluzione su WS1 per l1 = l2: soluzioni in WS2 4 soluzioni in WS1

singolari

robot 6R

16 soluzioni, fuori dalle singolarit: questo upper bound effettivamente raggiunto da un particolare robot ortogonale, cio con i = 0, /2 (i) analisi basata su trasformazioni algebriche della cinematica in una equazione polinomiale di grado pi basso possibile

Robotica 1

Spazio di lavoro
robot 3R planare
q3 l2 l1 q1 q2 l3 p y

l1 = l2 = l3 = WS1 = {p R2: p 3}
x

WS2 = {p R2: p }

1. in WS1 : 1 soluzioni tutte non singolari (tranne casi 2. e 3.) in cui lE-E pu assumere un continuum di orientamenti (ma non tutti!) 2. if p= 3 : 1 soluzione singolare 3. if p= 4. if p<
Robotica 1

: 1 soluzioni di cui 3 singolari : 1 soluzioni mai singolari, qualsiasi orientamento (WS2)


8

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) soluzioni n-m soluzioni soluzioni singolari in numero finito o infinito

se m < n (robot ridondante per il compito)


Robotica 1

Robot Dexter 8R

m = 6 (posizione e orientamento dellorgano terminale) n = 8 (tutti giunti rotatori) 2 soluzioni cinematiche inverse (ridondanza)

video

esplorazione delle soluzioni cinematiche inverse con un auto-movimento


Robotics 1 10

Metodi di soluzione
soluzione ANALITICA (in forma chiusa) preferibile, se possibile trovarla* ispezione geometrica ad hoc metodi algebrici (soluzione equazioni polinomiali*) metodi sistematici di generazione di set minimali di equazioni da risolvere
*

soluzione NUMERICA (in forma iterativa) necessaria se n>m (ridondanti) o in singolarit pi lenta, ma di facile derivazione richiede il calcolo dello Jacobiano analitico della cinematica diretta

3 assi di giunto rotoidali consecutivi incidenti (es. polso sferico), oppure 3 assi di giunto rotoidali consecutivi paralleli
Robotica 1

condizioni sufficienti per un 6R

fr (q) Jr(q) = q

metodi di Newton, del gradiente, ecc.

11

Cinematica inversa 2R
y py l1 l2 q1 px x p q2 cinematica diretta px = l1 c1 + l2 c12 py = l1 s1 + l2 s12 dati q1, q2 incognite

quadrando e sommando le equazioni della cinematica diretta px2 + py2 - (l12 + l22) = 2 l1 l2 (c1 c12 + s1 s12) = 2 l1 l2 c2 da cui in forma analitica

c2 = (px2 + py2 - l12 - l22)/ 2 l1 l2, s2 = 1 - c22, q2 = ATAN2 {s2, c2} deve essere in [-1,1] (altrimenti, fuori dal workspace!)
Robotica 1

2 soluzioni
12

Cinematica inversa 2R
y py q1 px x p q2

(continua)

per ispezione geometrica

q1 = -

2 soluzioni (una per ogni valore di s2)

q1 = ATAN2 {py, px} - ATAN2 {l2 s2 , l1 + l2 c2}


N.B. da riportare eventualmente in [- , ]!

{q1,q2}UP/LEFT q1 q1
Robotica 1

q2

p q2

{q1,q2}DW/RIGHT
q2 e q2 uguali in modulo e opposti in segno
13

Soluzione (algebrica) alternativa


px = l1 c1 + l2 c12 = l1 c1 + l2 (c1 c2 - s1 s2) py = l1 s1 + l2 s12 = l1 s1 + l2 (s1c2 + c1s2) l1 + l2c2 - l2 s2 l2s2 l1 + l2c2 c1 s1 = px py tranne per l1 = l2 e c2 = -1 per cui q1 non definita lineari in s1 e c1

det = (l12 + l22 + 2 l1l2c2) > 0 q1 = ATAN2 {s1, c1}

Note: a) automaticamente in [- , ]! b) in ATAN2, dalle espressioni di s1 e c1 si pu togliere il det >0


Robotica 1 14

Cinematica inversa robot polare


pz q3 q2 d1 q1 py px = q3 c2 c1 py = q3 c2 s1 pz = d1 + q3 s2 px2 + py2 + (pz - d1)2 = q32 q3 = + px2 + py2 + (pz - d1)2
qui solo, q3 0

px

q2 = ATAN2{(pz - d1)/q3, (px2 + py2) /q32 } se c2 0, q1 = ATAN2{py/c2, px/c2} (2 soluzioni {q1,q2,q3})

altrimenti, q1 indeterminata (singolarit, soluzioni)


Robotica 1 15

Cinematica inversa per robot con polso sferico


primi 3 giunti (RRR, RRP, PPP, )

j6

W z3

z4 z5 x6

y6 O6 = p z6 = a

z0 y0 x0 j1

j5

j4

d6 trovare q1, , q6, assegnati: p (origine O6) R = [n s a] (orientamento SR6)

1. W = p - d6 a q1, q2, q3

(cinematica inversa struttura portante)

2. R = 0R3(q1, q2, q3) 3R6(q4, q5, q6) q4, q5, q6 (cinematica inversa polso) data
Robotica 1

nota, dopo 1.

matrice di Eulero ZYZ o ZXZ


16

Esempio 6R: Unimation PUMA 600


polso sferico
0x 6

0y

0z

esistono 8 soluzioni inverse in forma chiusa (vedi Paul, Shimano, Mayer; 1981)
Robotica 1 17

Soluzione numerica della cinematica inversa

la soluzione analitica di r = f(q) non esiste o di difficile derivazione fr (Jacobiano analitico) Jr(q) = q metodo di Newton (qui nel caso m=n)

r = fr(q) = fr(qk) + Jr(qk) (q - qk) + o(q - qk2) si trascura

qk+1 = qk + Jr-1(qk) [r - fr(qk)]


converge se q0 (stima iniziale) sufficientemente vicina a q*: fr(q*) = r ha problemi vicino alle singolarit dello Jacobiano Jr(q) non si applica per robot ridondanti (m<n) [uso pseudoinversa Jr#(q)] ha convergenza quadratica vicino alla soluzione (rapido!)
18

Robotica 1

Soluzione numerica della cinematica inversa (continua)

metodo del gradiente (massima discesa)

si minimizza la funzione derrore

H(q) = r - fr(q)2 = [r - fr(q)]T [r - fr(q)] qk+1 = qk - qH(qk)


poich

H(q) = - JrT(q) [r - fr(q)], si ottiene qk+1 = qk + JrT(qk) [r - fr(qk)]

il passo deve garantire che la funzione derrore decresca ad ogni passo (per troppo grande si potrebbe scavalcare il minimo) per troppo piccolo, la convergenza eccessivamente lenta

Robotica 1

19

Interpretazione in feedback
rd
+ -

Jr

T(q)

. q

q(0)

rd = cost ( = 1)

fr(q)

e = rd - fr(q) 0

sistema ad anello chiuso asintoticamente stabile

V = eTe 0 candidata di Lyapunov . . . T e = eT d ((r - f (q)) = - eT J q = - eT J J Te 0 V=e d r r r r dt . V = 0 e Ker(JrT) in particolare e = 0


asintotica stabilit
Robotica 1 20

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 levoluzione della stima a tempo discreto data da

qk+1 = qk + T JrT(qk) [r - f(qk)]

che equivale a un passo del metodo del gradiente (con =T) si pu accelerare con una matrice di guadagno K>0, ponendo

. q = JrT(q) K e

nota: K pu usarsi per uscire da una situazione di stallo in un minimo locale: permette la rotazione dellerrore e fuori dal nucleo di JrT (se si incontrata una singolarit)
Robotica 1 21

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 0 inizializzazione q (genera una sola delle soluzioni) passo ottimale nel metodo del gradiente criteri di arresto qk+1-qk q
incremento dellalgoritmo

r - f(qk)
(evtl. separato per posizione e orientamento)
Robotica 1

errore cartesiano

min{J(qk)} 0
condizionamento dello Jacobiano (SVD)
22

Potrebbero piacerti anche