Cinematica differenziale

Транскрипт

1 Corso di Robotica 1 Cinematica differenziale Prof Alessandro De Luca Robotica 1 1

2 Cinematica differenziale studio dei legami tra moto (velocità) nello spazio dei giunti e moto (velocità lineare e angolare) nello spazio operativo (cartesiano) i legami tra velocità possono essere ricavati direttamente o passando attraverso la derivata nel tempo della funzione cinematica diretta stabilire il legame tra velocità angolare e derivata temporale della matrice di rotazione derivata temporale dei parametri di una rappresentazione minima dell orientamento Robotica 1 2

3 Velocità angolare di un corpo rigido v P2 - v P1 vincolo rigido di distanza tra punti: r = costante v P1 P 1 r 13 v P3 v P2 r 12 r 23 P 3 P 2 P 1, P 2, P 3 v Pj = v Pi + ω r ij = v Pi + S(ω) r ij v Pi - v Pj ortogonale ad r ij v P2 - v P1 = ω 1 r 12 v P3 - v P1 = ω 1 r 13 v P3 - v P2 = ω 2 r = 3 ω 1 = ω 2 = ω r ij = ω r ij la velocità angolare ω è associata all intero corpo (non a singoli punti) se P 1 con v P1 = : moto rotatorio puro (traiettorie circolari di tutti i punti) ω = : moto traslatorio puro Robotica 1 3

4 Velocità angolare dell E-E ω 2 ω n ω v ω 1 ω i definizioni alternative della cinematica diretta r = (p,φ) R p T = 1 ω è un vettore, ovvero un elemento di uno spazio vettoriale: si può ottenere come somma dei contributi ω 1,, ω n (in qualsiasi ordine) viceversa, φ (e dφ/dt) non è un elemento di uno spazio vettoriale: la rappresentazione di rotazioni successive, in generale, non si ottiene sommando gli angoli φ corrispondenti ω dφ/dt in generale Robotica 1 4

5 Moti infinitesimi traslazioni (spostamenti lineari) infinitesime commutano sempre, anche se finite dp = J L (q) dq rotazioni infinitesime R(dφ) = R(dφ X, dφ Y,dφ Z ) R X (φ X ) = 1 cos φ X sin φ X sin φ X cos φ X R X (dφ X ) = 1 1 dφ X dφ X 1 R Y (φ Y ) = cos φ Y sin φ Y 1 R Y (dφ Y ) = 1 dφ Y 1 dφ Y 1 R Z (φ Z ) = sin φ Y cos φ Y 1 dφ Z cos φ Z sin φ Z sin φ Z cos φ Z 1 R Z (dφ Z ) = dφ Z 1 1 Robotica 1 5

6 Rotazioni infinitesime R(dφ) = R(dφ X, dφ Y, dφ Z ) = 1 dφ z dφ Y dφ z 1 dφ X dφ Y dφ X 1 in qualsiasi ordine = I + S(dφ) rotazioni infinitesime commutano sempre rotazioni finite commutano solo se effettuate intorno allo stesso asse fisso Robotica 1 6

7 Esempio di rotazioni finite z z orientamento iniziale y φ X = 9 y x z equivale al fatto che ω NON è un differenziale esatto (l integrale di ω dipende dal cammino di integrazione!) x z φ Z = 9 φ Z = 9 x y φ X = 9 x z y x orientamenti finali differenti Robotica 1 7 y

8 Derivata temporale di R sia R = R(t) una matrice di rotazione funzione del tempo poiché I = R(t)R T (t), derivando entrambi i membri = d[r(t)r T (t)]/dt = dr(t)/dt R T (t) + R(t) dr T (t)/dt = dr(t)/dt R T (t) + [dr(t)/dt R T (t)] T da cui dr(t)/dt R T (t) = S(t) è una matrice antisimmetrica sia p(t) = R(t)p un vettore (di modulo costante) ruotato nel tempo dal confronto dp(t)/dt = dr(t)/dt p = S(t)R(t) p = S(t) p(t) dp(t)/dt = ω(t) p(t) = S(ω(t)) p(t) si ha S = S(ω) R = S(ω) R S(ω) = R R T Robotica 1 8

9 Derivata di rotazione elementare R X (φ(t)) = 1 cos φ(t) sin φ(t) sin φ(t) cos φ(t) R X (φ) R T X (φ) = φ sin φ cos φ cos φ sin φ = φ φ = S(ω) ω = φ 1 cos φ sin φ sin φ cos φ Robotica 1 9

10 Relazione tra derivata angoli RPY e ω R RPY (α x, β y, γ z ) = R ZY X (γ z, β y, α x ) T RPY (β,γ) x γ x z γ β x α β y y ω = cβ cγ -sγ cβ sγ cγ -sβ 1 x y z 1a col in 2a col in R ZY (γ z,β y ) R Z (γ z ) α β γ det T RPY (β,γ) = cβ = per β = ±π /2 (singolarità della rappresentazione RPY) NB la trattazione è analoga per gli altri 11 set di rappresentazioni minimali Robotica 1 1

11 Matrici Jacobiane Jacobiano analitico (ottenuto per differenziazione) p f r = f r (q) = r = J φ r (q) q = r (q) q q Jacobiano geometrico (nessuna derivata) v ω = p ω J = J(q) q = L (q) J A (q) q Robotica 1 11

12 Jacobiano analitico robot 2R planare y p y φ cinematica diretta l 2 q 2 p x = l 1 c 1 + l 2 c 12 l 1 r p y = l 1 s 1 + l 2 s 12 q 1 p x x φ = q 1 + q 2 p x = - l 1 s 1 q 1 - l 2 s 12 (q 1 + q 2 ) p y = l 1 c 1 q 1 + l 2 c 12 (q 1 + q 2 ) J r (q) = - l 1 s 1 - l 2 s 12 - l 2 s 12 l 1 c 1 + l 2 c 12 l 2 c 12 J L (q) φ = ω z = q 1 + q J A (q) Jacobiano analitico = geometrico data r, qui è una matrice 3 x 2 Robotica 1 12

13 Jacobiano analitico robot polare p z q 3 cinematica diretta (qui solo r = p) p x = q 3 c 2 c 1 q 2 p y = q 3 c 2 s 1 d 1 p y p z = d 1 + q 3 s 2 p x q 1 differenziando rispetto al tempo -q 3 c 2 s 1 -q 3 s 2 c 1 c 2 c 1 v = p = q 3 c 2 c 1 -q 3 s 2 s 1 c 2 s 1 q = J r (q) q q 3 c 2 s 2 Robotica 1 13

14 Jacobiano geometrico velocità istantanee dell E-E è sempre una matrice 6 x n v E ω E = J L (q) J A (q) q = J L1 (q) J A1 (q) J Ln (q) J An (q) q 1 q n v E = J L1 (q) q J Ln (q) q n sovrapposizione degli effetti ω E = J A1 (q) q J An (q) q n contributo alla velocità lineare dell E-E dovuto a q 1 contributo alla velocità angolare dell E-E dovuto a q 1 velocità lineare e angolare appartengono a spazi vettoriali (lineari) in R 3 Robotica 1 14

15 Contributo giunto prismatico NB i giunti a valle dell i-esimo sono visti come fermi, per cui la parte distale del robot è un unico corpo rigido J Li (q) q i = z i-1 d i z i-1 E SR q i = d i J Li (q) q i giunto i-mo prismatico z i-1 d i J Ai (q) q i Robotica 1 15

16 Contributo giunto rotatorio J Li (q) q i J Ai (q) q i = z i-1 θ i z i-1 O i-1 p i-1,e SR q i = θ i J Li (q) q i J Ai (q) q i giunto i-mo rotatorio (z i-1 p i-1,e ) θ i z i-1 θ i Robotica 1 16

17 Forma finale Jacobiano geometrico p,e = ω E v E ω E = J L (q) J A (q) q = J L1 (q) J A1 (q) J Ln (q) J An (q) q 1 q n giunto i-mo prismatico giunto i-mo rotatorio J Li (q) z i-1 z i-1 p i-1,e p,e q i J Ai (q) z i-1 z i-1 = R 1 (q 1 ) i-2 R i-1 (q i-1 ) p i-1,e = p,e (q 1,,q n ) - p,i-1 (q 1,,q i-1 ) 1 tutti i vettori sono espressi nelle stesse coordinate (quelle della base SR ) Robotica 1 17

18 Esempio: robot 2R planare y 1 y 2 x Tabella di DENAVIT-HARTENBERG 2 E giunto α i d i a i θ i l 2 1 l 1 q 1 y l 1 x 1 2 l 2 q 2 x z = z 1 = z 2 = 1 A 1 = c 1 - s 1 l 1 c 1 s 1 c 1 l 1 s p,1 p 1,E = p,e - p,1 z p,e z 1 p 1,E c 12 - s 12 l 1 c 1 + l 2 c 12 J = z z s 12 c 12 l 1 s 1 + l 2 s 12 1 A 2 = 1 1 Robotica 1 18 p,e

19 Jacobiano geometrico robot 2R planare y 1 y 2 x 2 E z p,e z p 1 1,E l J = 2 z z 1 y l 1 x 1 x NB lo Jacobiano è qui una matrice 6 2, quindi di rango massimo pari a 2 al massimo 2 delle componenti di velocità dell E-E sono comandabili in modo indipendente = - l 1 s 1 - l 2 s 12 l 1 c 1 + l 2 c l 2 s 12 l 2 c 12 1 Robotica 1 19

20 Trasformazioni su Jacobiano geometrico SR i O n b) potrebbe essere E O j (q) O j SR v n ω quella già calcolata = J n (q) q r ne E v E = v n + ω r ne = v n + S(r En ) ω SR B B v E B ω a) potrebbe essere SR B SR i (q) = = B R B R B R (q) B R (q) I S( r En ) I I S( r En (q)) I mai singolari! v n ω J n (q) q = B J E (q) q Robotica 1 2

21 Esempio: Robot Dexter braccio manipolatore 8R con trasmissioni a cavi d acciaio e pulegge (giunti dal 3 all 8) solo circa 15 kg in movimento motori alloggiati nel secondo braccio encoder incrementali (homing) ridondanza cinematica (grado n-m=2) cedevolezza nella interazione Robotica 1 21

22 Jacobiano mid-frame del robot Dexter Jacobiano geometrico J 8 (q) molto complesso Jacobiano mid-frame 4 J 4 (q) relativamente semplice! 8 y 4 x 4 4 z 4 6 righe, 8 colonne x z y Robotica 1 22

23 Relazioni tra espressioni differenziali p φ v ω p = v ω = ω + ω φ1 φ + ω 2 φ = a 3 1 φ 1 + a 2 (φ 1 ) φ 2 + a 3 (φ 1, φ 2 ) φ 3 = T(φ)φ assi intorno ai quali è definita la sequenza di rotazioni φ i r = p φ J(q) = I T(φ) J r (q) J r (q) = I T -1 (φ) J(q) R ω T(φ) ha sempre una singolarità R = S(ω) R singolarità della particolare rappresentazione minima dell orientamento per ogni colonna r i di R (versore di una terna ortonormale) risulta infatti r i = ω r i Robotica 1 23

24 Legami in accelerazione (e oltre ) Cinematica differenziale di ordine superiore i legami differenziali tra moto nello spazio dei giunti e moto nello spazio operativo si estendono al secondo ordine, terzo ordine, e via dicendo lo Jacobiano analitico pesa sempre le derivate di ordine massimo velocità accelerazione jerk snap r = J r (q) q r = J r (q) q + J r (q) q funzione matriciale N 2 (q,q) r = J r (q) q + 2 J r (q) q + J r (q) q funzione matriciale N r = J r (q) q + 3 (q,q,q) in modo analogo, si può procedere con lo Jacobiano geometrico Robotica 1 24

25 Richiami di algebra lineare rango ρ(j) = max # di righe o colonne linearmente indipendenti ρ(j) min(m,n) (se vale l uguale, J ha rango pieno ) se m = n e J ha rango pieno, J è non singolare e l inversa J -1 è definita ρ(j) = dimensione della più grande sottomatrice quadrata non singolare di J immagine R(J) = spazio vettoriale generato da tutte le combinazioni lineari delle colonne di J R(J)={v R m : ξ R n, v = J ξ} dimensione dim(r(j)) = ρ(j) nucleo ℵ(J) = spazio vettoriale di tutti i vettori ξ R n tali che J ξ = dim(ℵ(j)) = n - ρ(j) R(J) + ℵ(J T ) = R m e R(J T ) + ℵ(J) = R n data una matrice J: m n (m righe, n colonne) somma di spazi vettoriali V 1 + V 2 = spazio vettoriale di tutti i vettori v che possono scriversi come v = v 1 + v 2, con v 1 V 1, v 2 V 2 Robotica 1 25

26 Jacobiano del robot: decomposizione in sottospazi lineari di interesse e dualità spazio delle velocità di giunto J spazio delle velocità di compito (cartesiane) spazi duali ℵ(J) R(J) R(J T ) + ℵ(J) = R n R(J) + ℵ(J T ) = R m R(J T ) ℵ(J T ) spazi duali spazio delle coppie di giunto J T spazio delle forze di compito (cartesiane) (in una data configurazione q) Robotica 1 26

27 Jacobiano e analisi della mobilità ρ(j) = ρ(j(q)), R(J) = R(J(q)), ℵ(J T )= ℵ(J T (q)) sono definiti localmente, cioè dipendono dalla configurazione q R(J(q)) = spazio delle velocità generalizzate (cioè con componenti lineari e/o angolari) che possono essere assunte (istantaneamente a partire dalla configurazione q) dall end-effector al variare delle velocità di giunto se J(q) ha rango massimo (in genere = m), nella configurazione q il robot può muovere il suo end-effector in tutte le direzioni dello spazio operativo R m se ρ(j(q)) < m, esistono direzioni di R m in cui l E-E non può muoversi, [sono le direzioni in ℵ(J T ), ovvero il complemento di R(J(q)) rispetto allo spazio operativo, di dimensione m - ρ(j(q))] se ℵ(J(q)) {} (sempre se m<n, ossia nei robot ridondanti), esistono velocità di giunto che non forniscono velocità all E-E ( automovimenti ) Robotica 1 27

28 Singolarità cinematiche sono configurazioni in cui si ha caduta di rango dello Jacobiano perdita di mobilità dell E-E del robot corrispondono in generale a configurazioni nelle quali il numero di soluzioni della cinematica inversa non è quello generico in tali configurazioni non è possibile invertire la cinematica differenziale, ovvero trovare velocità di giunto che realizzino una arbitraria velocità dell organo terminale del robot vicino ad una singolarità potrebbero essere necessarie velocità di giunto elevate per ottenere una velocità anche bassa dell E-E conoscere le singolarità aiuta ad evitarle nella pianificazione e nel controllo del moto se m = n, bisogna trovare le configurazioni q in cui det J(q) = se m < n, le singolarità sono le configurazioni q in cui tutte le sottomatrici m m di J sono singolari (caduta di rango della matrice) il calcolo delle singolarità può essere oneroso computazionalmente Robotica 1 28

29 Singolarità del robot 2R planare p y l 2 q 2 p cinematica diretta p x = l 1 c 1 + l 2 c 12 l 1 q 1 p x Jacobiano analitico - l p = 1 s 1 - l 2 s 12 - l 2 s 12 l q = J(q) q 1 c 1 + l 2 c 12 l 2 c 12 x p y = l 1 s 1 + l 2 s 12 det J(q) = l 1 l 2 s 2 singolarità: braccio steso (q 2 =) e ripiegato (q 2 = π) sono sulla frontiera del workspace e separano le zone con soluzioni cinematiche inverse distinte ( elbow up o down ) Robotica 1 29

30 Singolarità del robot polare (RRP) p z q 3 cinematica diretta p x = q 3 c 2 c 1 q 2 d 1 p x q 1 det J(q) = q 3 2 c 2 p y p y = q 3 c 2 s 1 p z = d 1 + q 3 s 2 Jacobiano analitico - q 3 s 1 c 2 -q 3 c 1 s 2 c 1 c 2 p = -q 3 c 1 c 2 -q 3 s 1 s 2 s 1 c 2 q = J(q) q q 3 c 2 s 2 singolarità: terzo braccio tutto ritratto (q 3 =) o disposto lungo l asse z (q 2 = ± π/2); se avvengono entrambi, il rango di J scende a uno tutte le configurazioni singolari corrispondono qui a punti cartesiani interni allo spazio di lavoro (in assenza di limiti per il giunto prismatico) Robotica 1 3

31 Singolarità robot con polso sferico n = 6, ultimi tre assi rotatori e incidenti in un punto senza perdità di generalità si può porre O 6 = W = centro polso sferico (ovvero con d 6 = ) J(q) = J 11 J 21 J 22 l inversione della J è semplificata risulta det J(q 1,,q 5 ) = det J 11 det J 22 det J 11 (q 1,,q 3 ) = fornisce le singolarità di struttura det J 22 (q 1,,q 5 ) = fornisce le singolarità di polso poiché J 22 = [z 3 z 4 z 5 ], le singolarità di polso corrispondono al caso di assi z 3 z 4 z 5 linearmente dipendenti q 5 = oppure q 5 = ±π/2 Robotica 1 31