Posizione e orientamento di corpi rigidi

Transcript

1 Corso di Robotica 1 Posizione e orientamento di corpi rigidi Prof. Alessandro De Luca Robotica 1 1

2 Posizione e orientamento terne ortogonali destre SR A A p AB B SR B corpo rigido posizione: A p AB (vettore I R 3 ), espresso in SR A orientamento: matrice 3x3 ortonormale (R T = R -1 A R B B R A = I), con det = +1 A R B = [ A x B A y B A z B ] x A y A z A (x B y B z B ) sono i versori (norma unitaria) della terna SR A (SR B ) le componenti di A R B sono i coseni direttori degli assi di SR B rispetto a SR A Robotica 1 2

3 Matrice di rotazione ortonormale, a det = +1 A R B = x B T x A y B T x A z B T x A x B T y A y B T y A z B T y A x B T z A y B T z A z B T z A coseno direttore di z B rispetto a x A proprietà di concatenazione k R i i R j = k R j struttura algebrica di gruppo SO(3) (elemento neutro = I; elemento inverso = R T ) orientamento di SR i rispetto a SR k orientamento di SR j rispetto a SR k orientamento di SR j rispetto a SR i N.B. il prodotto di matrici di rotazione non commuta in generale! Robotica 1 3

4 Cambiamento di coordinate z p x z 1 P P = p y p z = 1 p x x p y y p z z 1 1 p x y 1 = x 1 y 1 z 1 1 p y SR 1 1 p z SR y = R 1 1 P x x 1 La matrice di rotazione R 1 (orientamento di SR 1 rispetto ad SR ) rappresenta anche il cambiamento di coordinate da SR 1 ad SR Robotica 1 4

5 Es: Orientamento di terne in un piano (rotazione elementare intorno a z) v y C θ P θ u SR C SR x = OB xb = u cos θ - v sin θ y = OC + Cy = u sin θ + v cos θ z = w ossia x C y C z C C OP O x B OP x y z = cos θ sin θ sin θ cos θ 1 u v w = R z (θ) u v w in modo analogo: R x (θ) = 1 cos θ sin θ sin θ cos θ R z (-θ) = R zt (θ) R y (θ) = cos θ sin θ 1 sin θ cos θ Robotica 1 5

6 Es: Rotazione di un vettore intorno a z y y θ v v x = v cos α y = v sin α x = v cos (α + θ) = v (cos α cos θ - sin α sin θ) x = x cos θ - y sin θ O α x x y = v sin (α + θ) = v (sin α cos θ + cos α sin θ) x = x sin θ + y cos θ ossia z = z x y z = cos θ sin θ sin θ cos θ 1 x y z = R z (θ) x y z come prima! Robotica 1 6

7 Interpretazioni equivalenti di una matrice di rotazione una matrice di rotazione, ad es. R z (θ), può rappresentare: SR C P θ SR C θ θ v v SR SR l orientamento di un corpo rigido rispetto a un sistema di riferimento SR es: [ x c y c z c ] = R z (θ) il cambiamento di coordinate da SR C a SR es: P = R z (θ) C P l operatore di rotazione es: v = R z (θ) v la matrice di rotazione R C è l operatore che porta la terna SR sulla terna SR C Robotica 1 7

8 Composizione di rotazioni 1 R 2 porta SR su SR 2 porta SR 1 1 su SR 2 R 3 porta SR 2 su SR 3 R 1 p 23 = 3 p p 1 = p 12 = SR 1 SR 2 SR 3 SR considerazioni sulla complessità computazionale p = ( R 1 1 R 2 2 R 3 ) 3 p = R 3 3 p p = R 1 ( 1 R 2 ( 2 R 33 p)) 63 prodotti 42 somme 27 prodotti 18 somme 2 p Robotica 1 1 p 8

9 Rappresentazione asse/angolo r z z P v r DATI versore r ( r = 1) θ (positivo se antiorario visto da r) x z 1 r x SR 1 θ v SR y 1 r y y SR 1 è il risultato della rotazione di SR di un angolo θ intorno al versore r PROBLEMA DIRETTO trovare R(θ,r) = [ x 1 y 1 z 1 ] ovvero tale che P= R(θ,r) 1 P v = R(θ,r) v x 1 Robotica 1 9

10 Problema diretto asse/angolo z 1 C R(θ,r) = C R z (θ) C T concatenazione di tre rotazioni z 1 3 C -1 = C T 2 R z (θ) r y 1 C = n s r x SR 1 x 1 SR y sequenza di 3 rotazioni che portano la terna SR a coincidere con SR 1 dopo la prima rotazione l asse z coincide con r n ed s versori ortogonali tali che n s = r, ovvero n y s z - s y n z = r x n z s x - s z n x = r y n x s y - s x n y = r z Robotica 1 1

11 Problema diretto asse/angolo R(θ,r) = C R z (θ) C T R(θ,r) = n s r cθ - sθ sθ cθ 1 n T s T r T = r r T + (n n T + s s T ) cθ + (s n T - n s T ) sθ tenendo conto che C C T = n n T + s s T + r r T = I, e che dipende solo da r e θ!! -r z r y s n T - n s T = -r x = S(r) R(θ,r) = r r T + (I - r r T ) cθ + S(r) sθ skew-symmetric(r): r v = S(r)v = - S(v)r = R T (-θ,r) = R(-θ,-r) Robotica 1 11

12 svolgendo i calcoli R(θ,r) = Espressione finale di R(θ,r) r x2 (1- cosθ)+cosθ r x r y (1- cosθ)-r z sinθ r x r z (1- cos θ)+r y sinθ r x r y (1- cosθ)+r z sinθ r y2 (1- cosθ)+cosθ r y r z (1- cos θ)-r x sinθ r x r z (1- cosθ)-r y sinθ r y r z (1- cos θ)+r x sinθ r z2 (1- cosθ)+cosθ Robotica 1 12

13 Problema asse/angolo: esempio R(θ,r) = r r T + (I - r r T ) cθ + S(r) sθ r = = z R(θ,r) = + 1 cθ + 1 sθ 1 cθ -sθ = sθ cθ = R z (θ) 1 Robotica 1 13

14 Formula di Rodriguez v = R(θ,r) v v = v cos θ + (r v) sin θ + (1 - cos θ)(r T v) r dimostrazione: R(θ,r) v = (r r T + (I - r r T ) cos θ + S(r) sin θ)v = r r T v (1 - cos θ) + v cos θ + (r v) sin θ c.v.d. Robotica 1 14

15 Proprietà di R(θ,r) 1. R r = r (r è l asse invariante alla rotazione) 2. se r è il versore di un asse coordinato, R degenera in una delle matrici elementari di rotazione 3. (θ,r) R non è una mappa iniettiva: R(θ,r) = R(-θ,-r) 4. det R = +1 = Π λ i (autovalori) 5. tr(r) = tr(r r T ) + tr(i - r r T )cθ = cθ = Σ λ i 1. λ 1 = e 4. λ 2 + λ 3 = 2 cθ λ 2-2 cθ λ + 1 = λ 2,3 = cθ ± c 2 θ 1 = cθ ± i sθ = e±i θ tutti i λ a modulo 1 ( R ortonormale) Robotica 1 15

16 Problema inverso asse/angolo data una matrice di rotazione R trovare un versore r e un angolo θ: R = r r T + (I - r r T ) cos θ + S(r) sin θ = R(θ,r) tr(r) = R 11 + R 22 + R 33 = cos θ θ = arcos R 11 + R 22 + R 33-1 ma: fornisce solo valori in [,π] (mai angoli θ negativi ) perde definitezza rapidamente per θ Robotica

17 Problema inverso asse/angolo R 12 -R 21 R 13 -R 31 -r z r y R - R T = R 23 -R 32 = 2 sin θ -r x r x r = r y = r z 1 2 sin θ R 32 - R 23 R 13 - R 31 R 21 - R 12 utilizzabile solo se θ ± kπ 1 r = 1 sin θ = ± (R 21 - R 12 ) 2 + (R 13 - R 31 ) 2 + (R 23 - R 32 ) 2 2 θ = ATAN2 {± (R 21 - R 12 ) 2 + (R 13 - R 31 ) 2 + (R 23 - R 32 ) 2, R 11 + R 22 + R 33-1} Robotica 1 17

18 Funzione ATAN2 arcotangente a quattro quadranti con due argomenti assume valori in [-π,+π ] non è definita solo in (,) usa il segno di entrambi gli argomenti per definire il quadrante basata sulla funzione arctan con valori in [-π /2,+π /2] disponibile nei principali linguaggi (C++, Matlab, ) Robotica 1 18

19 Casi particolari Per θ = ± 2kπ non c è soluzione per r (non è definito l asse di rotazione) Per θ = π ± 2kπ, sin θ =, cos θ = -1 r x r y r = = r z R = 2r r T - I ± (R )/2 ± (R )/2 ± (R )/2 con r x r y = R 12 /2 r x r z = R 13 /2 r y r z = R 23 /2 esercizio: determinare tutte le soluzioni (r, θ) per R = risolve ambiguità multiple di segno (sempre due soluzioni finali, di segno opposto) Robotica

20 Quaternione unitario per eliminare problemi di indeterminatezza e singolarità del problema asse/angolo, si può utilizzare il quaternione unitario Q = {η, ε} = {cos(θ/2), sin(θ/2) r} scalare vettore 3-dim η 2 + ε 2 = 1 (da cui il nome) (θ, r) e (-θ, -r) danno lo stesso quaternione Q rotazione nulla associata a Q = {1, } è possibile definire la composizione di quaternioni unitari (in modo simile al prodotto di matrici di rotazione) Q 1 *Q 2 = {η 1 η 2 - ε 1T ε 2, η 1 ε 2 + η 2 ε 1 + ε 1 ε 2 } Robotica 1 2