Fondamenti di Geometria nello spazio 3D -...
Transcript of Fondamenti di Geometria nello spazio 3D -...
Corso di Robotica
Prof. Davide Brugali
Università degli Studi di Bergamo
Fondamenti di Geometria
nello spazio 3D
Geometria delle traslazioni
11
0
0QTQ
Semplice traslazione
00
1
1QTQ
x0
y0
z0
x1
y1
z1
Q
T
1
1
1
0
0
0
qz
qy
qx
c
b
a
qz
qy
qx
1
0
0
1TT
2 Robotica - UNIBG - Prof. Brugali
Geometria delle rotazioni
Rotazione rispetto all’asse z
x
y
(Px,Py)
θ ρ
Px = ρ cos(θ)
Py = ρ sin(θ)
22pypx
px
pyarctan
Qx = ρ cos(θ+φ)
Qy = ρ sin(θ+φ)
Qx = ρ cos(θ)cos(φ) - ρ sen(θ)sen(φ)
Qy = ρ cos(θ)sin(φ) + ρ sen(θ)cos(φ)
Qx = Px cos(φ) – Py sen(φ)
Qy = Px sin(φ) + Py cos(φ)
φ (Qx,Qy)
(Px,Py)
x
y
θ ρ
3 Robotica - UNIBG - Prof. Brugali
Geometria delle rotazioni
y
x
y
x
P
P
Q
Q*
cossin
sincos
01
0
0PRQ
φ (Qx,Qy)
(Px,Py)
x
y
θ ρ
Qx = Px cos(φ) – Py sen(φ)
Qy = Px sin(φ) + Py cos(φ)
Q = R * P
x0
y0
P
φ
x1
Q
y1
01PQ
11
0
0QRQ
00
1
1QRQ
4
Rotazione rispetto all’asse z
Robotica - UNIBG - Prof. Brugali
Geometria delle rotazioni
Fornisce l’orientamento di una terna di coordinate rispetto ad
un’altra. I vettori colonna sono i coseni direttori degli assi della terna
ruotata rispetto alla terna di riferimento.
Rappresenta una trasformazione di coordinate che mette in relazione
le coordinate di un punto in due terne differenti con origine comune
p = Rp. Inoltre in virtù della proprietà di ortogonalità della matrice
R, la trasformazione inversa si scrive
p = RTp
La matrice di rotazione R rappresenta l’operatore che permette di
ruotare un vettore (nella stessa terna), di un angolo prefissato, attorno
ad un generico asse di rotazione nello spazio.
5 Robotica - UNIBG - Prof. Brugali
Geometria delle rotazioni
Rotazione dirette e inverse rispetto agli assi x,y,z
100
0cossin
0sincos
1
0
Rz
100
0cossin
0sincos
0
1
Rz
cos0sin
010
sin0cos
0
1Ry
cos0sin
010
sin0cos
1
0Ry
cossin0
sincos0
001
1
0Rx
cossin0
sincos0
001
0
1Rx
T
RR1
0
0
1 Se R-1 = RT allora R è ortogonale
det(R) = 1
6 Robotica - UNIBG - Prof. Brugali
Rotazioni successive rispetto alla terna corrente
La rotazione complessiva è espressa come successione di rotazioni
parziali, ciascuna delle quali è definita rispetto alla rotazione
precedente. La terna rispetto alla quale avviene la rotazione in atto è
definita terna corrente.
La composizione di rotazioni successive rispetto alla terna corrente si
ottiene per moltiplicazione da sinistra verso destra le matrici delle
singole rotazioni, nell’ordine della rotazione.
9
1
2
0
1
0
2RRR
Robotica - UNIBG - Prof. Brugali
Rotazioni successive rispetto ad una terna fissa
0
1
1
2
0
2RRR
La composizione di rotazioni successive rispetto ad una terna
fissa si ottiene moltiplicando da destra verso sinistra le
singole matrici di rotazione nell’ordine delle rotazioni.
11 Robotica - UNIBG - Prof. Brugali
Angoli RPY
Tale rappresentazione trae origine da una descrizione delle rotazioni usate
frequentemente in aeronautica.
In particolare RPY indicano rispettivamente il rollio (Roll) il beccheggio (Pitch)
e l’imbardata (Yaw) di uno scafo.
In questo caso la terna di parametri rappresenta rotazioni definite rispetto ad una
terna fissa solidale al baricentro dello scafo.
13 Robotica - UNIBG - Prof. Brugali
Angoli RPY
Si ruota la terna origine dell’angolo
intorno all’asse x (imbardata); tale
rotazione è descritta dalla matrice di
rotazione
Si ruota la terna originale
dell’angolo intorno all’asse y
(beccheggio); tale rotazione è
rappresentata dalla matrice
Si ruota la terna originale
dell’angolo intorno all’asse z
(rollio); tale rotazione è
rappresentata dalla matrice:
cos0sen0
sencos0
001
)(x
R
cos0sen
010
sen0cos
)(y
R
100
0cossen
0sencos
)(
z
R
14 Robotica - UNIBG - Prof. Brugali
Angoli RPY
La rotazione globale della terna, essendo ottenuta per composizione rispetto ad una
terna fissa è:
ccscs
sccssccssscs
sscsccsssccc
RRRRxyzRPY
)()()(
Problema inverso
333231
232221
131211
rrr
rrr
rrr
R
3332
2
33
2
3231
1121
,2tan
,2tan
,2tan
rrA
rrrA
rrA
appartenete all’intervallo (/2, 3/2)
3332
2
33
2
3231
1121
,2tan
,2tan
,2tan
rrA
rrrA
rrA
15 Robotica - UNIBG - Prof. Brugali
Rappresentazione con angoli di Eulero
T ,,
Relazione di ortogonalità R RT = I
solo 3 parametri indipendenti
R fornisce una rappresentazione ridondante
Rappresentazione minima
16
Euler Angle I Euler Angle II Roll-Pitch-Yaw
Sequence about OZ axis about OZ axis about OX axis
of about OX’ axis about OY’ axis about OY axis
Rotations about OZ’’ axis about OZ’’ axis about OZ axis
Robotica - UNIBG - Prof. Brugali
Rappresentazione con angoli di Eulero
'''
1
0 zxzRRRR
Metodo di Eulero I:
si ruota la terna origine dell'angolo φ intorno all'asse z: tale rotazione è descritta
dalla matrice di rotazione Rz(φ)
si ruota la terna dell'angolo θ intorno all'asse x’ corrente: tale rotazione è
descritta dalla matrice di rotazione Rx1(θ).
si ruota la terna dell'angolo ψ attorno all'asse z’’ corrente: tale rotazione è
descritta dalla matrice di rotazione Rz’’ (ψ)
17 Robotica - UNIBG - Prof. Brugali
Angoli di Eulero I
cossin0
sincos0
001
,
100
0cossin
0sincos
'xzRR
100
0cossin
0sincos
''
zR
cossincossinsin
sincos
coscoscos
sinsin
cossincos
cossin
sinsincoscossin
sincos
cossinsin
coscos
''' zxzRRRR
19 Robotica - UNIBG - Prof. Brugali
Rappresentazione con angoli di Eulero
'''
1
0 zyzRRRR
Metodo di Eulero II:
si ruota la terna origine dell'angolo φ intorno all'asse z: tale rotazione è descritta
dalla matrice di rotazione Rz(φ)
si ruota la terna dell'angolo θ intorno all'asse y’ corrente: tale rotazione è
descritta dalla matrice di rotazione Ry1(θ).
si ruota la terna dell'angolo ψ attorno all'asse z’’ corrente: tale rotazione è
descritta dalla matrice di rotazione Rz’’ (ψ)
20 Robotica - UNIBG - Prof. Brugali
Angoli di Eulero II
cossinsincossin
sinsin
coscos
sincossin
sincos
coscossin
sincoscossin
sincoscos
sinsin
coscoscos
22 Robotica - UNIBG - Prof. Brugali
Angoli di Eulero: Problema inverso
333231
232221
131211
rrr
rrr
rrr
R
csscs
ssccscsscccs
sccssccssccc
REUL
3132
33
2
23
2
13
1323
,2tan
,2tan
,2tan
rrA
rrrA
rrA
Si possono ricavare due soluzioni,
equivalenti (per gli effetti prodotti),
scegliendo appartenente all’intervallo
(0, ) oppure (-, 0)
Le due soluzioni ricavate degenerano
quando s = 0; in questo caso è
possibile determinare soltanto la
somma o la differenza di e . Infatti,
se = 0, , le rotazioni successive di
e sono effettuate intorno ad assi di
terna corrente paralleli fra di loro,
fornendo così effetti di rotazione
equivalenti.
3132
33
2
23
2
13
1323
,2tan
,2tan
,2tan
rrA
rrrA
rrA
23 Robotica - UNIBG - Prof. Brugali
Coordinate omogenee
1
0
11
0
0TQRQ
Rototraslazione
1
*10
1
1
1
1
1
0
1
0
0
0
0
qz
qy
qx
TR
qz
qy
qxx0
y0
z0
x1
y1
z1 Q
T
11
0
0QHQ
24 Robotica - UNIBG - Prof. Brugali
Coordinate omogenee
11000
100
010
001
1
1
1
1
0
0
0
qz
qy
qx
c
b
a
qz
qy
qx Pura traslazione
11000
0
0
0
1
1
1
1
222120
121110
020100
0
0
0
qz
qy
qx
rrr
rrr
rrr
qz
qy
qx
Pura rotazione
Attenzione: La matrice di trasformazione omogenea non gode della
proprietà di ortogonalità!
25 Robotica - UNIBG - Prof. Brugali
Traslazione lungo l’asse z
x
y
z
P
u
v
w
O, O’
x
y
z P
u
v
w
O, O’ h
1000
100
0010
0001
),(h
hzTrans
11
*
1000
100
0010
0001
1
hp
p
p
p
p
p
hz
y
x
w
v
u
w
v
u
26 Robotica - UNIBG - Prof. Brugali
Rotazione attorno all’asse x
1000
00
00
0001
),(
CS
SCxRot
x
z
y
v
w
P
u
11000
00
00
0001
1
w
v
u
p
p
p
CS
SC
z
y
x
27 Robotica - UNIBG - Prof. Brugali
Composizione di rototraslazioni
x
z
y
v
w
P
u
Una sequenza di trasformazioni
regole:
Se il sistema di riferimento O-U-V-W ruota
attorno ad un asse principale del sistema di
riferimento O-X-Y-Z, allora usare la Pre-
moltiplicazione
Se il sistema di riferimento O-X-Y-Z ruota
attorno ad un asse principale del sistema di
riferimento O-U-V-W, allora usare la Post-
moltiplicazione
28 Robotica - UNIBG - Prof. Brugali
Esempio
Rotazione di attorno all’asse OX
Traslazione di a lungo all’asse OX
Traslazione di d lungo all’asse OZ
Rotazione di attorno all’asse OZ
,,,, xaxdzzRTTRH
1000
00
00
0001
*
1000
0100
0010
001
*
1000
100
0010
0001
*
1000
0100
00
00
CS
SC
a
d
CS
SC
29 Robotica - UNIBG - Prof. Brugali