Roboty przemysłowe
KRIM, AGH w Krakowie
1
Katedra Robotyki i Mechatroniki
Akademia Górniczo-Hutnicza w Krakowie
Wojciech Lisowski
2
Opis położenia i orientacji efektora
w przestrzeni trójwymiarowej
Roboty przemysłowe
Roboty przemysłowe
KRIM, AGH w Krakowie
2
Zagadnienia:
Macierz przekształcenia jednorodnego:
interpretacja elementów
Techniki zapisu położenia i orientacji
Konwencja orientowania osi chwytaka
Interpretacja zadanej orientacji w oparciu
o kosinusy kierunkowe lub kąty RPY
Roboty przemysłowe
KRIM, AGH w Krakowie
3
x
y
z
u
v
w
O
P
Położenie:
x, y, z
Orientacja:
ϕ
,
θ
,
ψ
Opis z wykorzystaniem macierzy przekształcenia jednorodnego
Roboty przemysłowe
KRIM, AGH w Krakowie
4
Orientacja:
ϕ
,
θ
,
ψ
)
,
(
)
,
(
)
,
(
)
,
,
(
Ψ
Θ
Φ
=
Ψ
Θ
Φ
w
Rot
v
Rot
z
Rot
EU
)
,
(
)
,
(
)
,
(
)
,
,
(
ψ
θ
φ
ψ
θ
φ
x
Rot
y
Rot
z
Rot
RPY
=
Kąty Eulera
Kąty: RPY
(Fu)
PRECESJI -
Φ
NUTACJI -
Θ
OBROTU WŁASNEGO -
Ψ
OBROTU (Roll) -
φ
POCHYLENIA (Pitch) -
θ
SKRĘTU (Yaw) -
ψ
(Fu)
Roboty przemysłowe
KRIM, AGH w Krakowie
5
Współrzędne jednorodne: reprezentacja wektora n wymiarowego w
przestrzeni n+1 wymiarowej
T
z
y
x
p
p
p
p
]
,
,
[
=
)
,
,
,
(
s
sp
sp
sp
p
z
y
x
=
(
−
−
−
−
≡
≡
10
50
40
30
2
10
8
6
1
5
4
3
[ , , , ]
a b c 0
Wektor Nieokreślony
[ , , , ],
0 0 0
0
n n
≠
[ , , , ]
0 0 0 0
Wektor zerowy
Wektor kierunkowy
Roboty przemysłowe
KRIM, AGH w Krakowie
6
)
,
,
(
)
,
,
(
w
v
u
z
y
x
uvw
xyz
k
j
i
OUVW
k
j
i
OXYZ
p
p
)
)
)
)
)
)
≡
w
w
v
v
u
u
uvw
k
p
j
p
i
p
p
)
)
)
+
+
=
p
p
p r
p
p r
p
p r
r
=
∠
=
=
cos ( , )
)
o )
o )
1
p
i
p
i
i p
i
j p
i
k p
p
j
p
j
i p
j
j p
j
k p
p
k
p
k
i p
k
j p
k
k p
p
p
p
i
i
i
j
i
x
x
uvw
x
u
u
x
v
v
x
w
w
y
y
uvw
y
u
u
y
v
v
y
w
w
z
z
uvw
z
u
u
z
v
v
z
w
w
x
y
z
x
u
x
v
x
=
=
+
+
=
=
+
+
=
=
+
+
=
)
o
)
o
)
)
o
)
)
o
)
)
o
)
o
)
)
o
)
)
o
)
)
o
)
o
)
)
o
)
)
o
)
)
o
)
)
o
)
)
o
)
k
j
i
j
j
j
k
k
i
k
j
k
k
p
p
p
w
y
u
y
v
y
w
z
u
z
v
z
w
u
v
w
)
o
)
)
o
)
)
o
)
)
o
)
)
o
)
)
o
)
xyz
w
w
xyz
v
v
xyz
u
u
p
k
p
p
j
p
p
i
p
o
)
o
)
o
)
=
=
=
Roboty przemysłowe
KRIM, AGH w Krakowie
7
Macierz przekształcenia jednorodnego pozwala określić położenie i
orientację lokalnego układu współrzędnych Puvw w układzie
odniesienia Oxyz
x
y
z
u
v
w
O
P
=
1
0
0
0
P
z
z
z
P
y
y
y
P
x
x
x
z
w
v
u
y
w
v
u
x
w
v
u
A
=
z
z
z
y
y
y
x
x
x
w
v
u
w
v
u
w
v
u
R
w
z
v
z
u
z
w
y
v
y
u
y
w
x
v
x
u
x
k
k
j
k
i
k
k
j
j
j
i
j
k
i
j
i
i
i
)
o
)
)
o
)
)
o
)
)
o
)
)
o
)
)
o
)
)
o
)
)
o
)
)
o
)
=
R
R
T
−
=
1
Roboty przemysłowe
KRIM, AGH w Krakowie
8
R x
C
S
S
C
( , )
α
α
α
α
α
=
−
1
0
0
0
0
Podstawowe macierze rotacji
Roboty przemysłowe
KRIM, AGH w Krakowie
9
)
,
(
)
,
(
)
,
(
)
,
,
(
θ
ϕ
α
z
Rot
y
Rot
x
Rot
c
b
a
Tra
A
=
Rot x
C
S
S
C
( , )
α
α
α
α
α
=
−
1
0
0
0
0
0
0
0
0
0
0
1
Rot y
C
S
S
C
( , )
ϕ
ϕ
ϕ
ϕ
ϕ
=
−
0
0
0
1
0
0
0
0
0
0
0
1
Rot z
C
S
S
C
( , )
θ
θ
θ
θ
θ
=
−
0 0
0 0
0
0
1 0
0
0
0 1
=
⋅
⋅
=
1
0
0
0
1
0
0
0
1
0
0
0
1
)
,
(
)
,
(
)
,
(
)
,
,
(
c
b
a
c
z
Tra
b
y
Tra
a
x
Tra
c
b
a
Tra
4 podstawowe macierze przekształcenia jednorodnego
Uwaga! Składanie przekształceń jednorodnych nie jest przemienne:
)
,
(
)
,
(
)
,
(
)
,
(
α
ϕ
ϕ
α
x
Rot
y
Rot
y
Rot
x
Rot
≠
Roboty przemysłowe
KRIM, AGH w Krakowie
10
T
N
N
N
P N
O
O
O
P O
A
A
A
P A
R
R P
x
y
z
x
y
z
x
y
z
T
T
T
−
=
−
−
−
=
−
1
0
0
0
1
0
1
o
o
o
Przekształcenie odwrotne pozwala wyrazić położenie i orientację
układu współrzędnych odniesienia w układzie lokalnym,
związanym z rozważanym członem
Roboty przemysłowe
KRIM, AGH w Krakowie
11
Przykład 1
z
0
x
0
y
0
O
0
3
5
2
O
1
x
1
z
1
y
1
P
A
1
3
5
2
0 0 0 1
=
? ? ?
? ? ?
? ? ?
T
T
T
z
y
x
]
0
,
1
,
0
[
]
0
,
0
,
1
[
]
1
,
0
,
0
[
1
1
1
=
−
=
−
=
A
1
0
1 0 3
0
0
1 5
1 0
0 2
0
0
0 1
=
−
−
T
P
P
O
]
0
,
1
,
2
[
1
1
=
=
T
P
]
1
,
0
,
1
,
2
[
1
=
(
0
1
1
(
(
P
A P
=
2
5
0
1
0
1 0 3
0
0
1 5
1 0
0 2
0
0
0 1
2
1
0
1
=
−
−
T
P
P
O
]
0
,
5
,
2
[
0
0
=
=
Roboty przemysłowe
KRIM, AGH w Krakowie
12
Przykład 2
[
]
−
=
−
1
5
2
1
1
2
5
1
1
0
0
0
0
0
1
0
0
1
0
0
0
0
0
1
1
2
5
1
)
90
,
(
T
x
Rot
o
z
0
y
0
Układ odniesienia Ox
0
y
0
z
0
Obrót punktu
−
=
−
1
0
0
0
0
0
1
0
0
1
0
0
0
0
0
1
1
1
T
Roboty przemysłowe
KRIM, AGH w Krakowie
13
Konwencja orientowania osi chwytaka
Wektory: n – normalny (x
e
)
o – orientacji (y
e
)
a – zbliżenia (z
e
)
a
o
n
=
×
(Fu)
Roboty przemysłowe
KRIM, AGH w Krakowie
14
=
⋅
⋅
=
1
0
0
0
1
0
0
0
1
0
0
0
1
)
,
(
)
,
(
)
,
(
)
,
,
(
z
y
x
z
z
Tra
y
y
Tra
x
x
Tra
z
y
x
Car
=
=
−
=
1
0
0
0
1
0
0
0
1
0
0
0
1
)
,
(
)
,
(
)
,
(
)
,
(
)
,
,
(
d
rS
rC
z
Rot
r
x
Tran
z
Rot
d
z
Tran
r
d
Cyl
α
α
α
α
α
Opis położenia:
Współrzędne kartezjańskie:
Współrzędne cylindryczne
Roboty przemysłowe
KRIM, AGH w Krakowie
15
Współrzędne sferyczne
Sfe
r
Rot z
Rot y
Tran z r Rot y
Rot z
rC S
rS S
rC
( , , )
( , )
( , )
( , )
( ,
)
( ,
)
α β
α
β
β
α
α β
α β
β
=
−
−
=
1 0 0
0 1 0
0 0 1
0 0 0
1
Roboty przemysłowe
KRIM, AGH w Krakowie
16
Opis orientacji:
=
=
1
0
1
0
0
0
T
z
z
z
z
y
y
y
y
x
x
x
x
P
R
P
A
O
N
P
A
O
N
P
A
O
N
T
EU
Rot z
Rot y
Rot z
C C C
S S
C C S
S C
C S
S C C
C S
S C S
C C
S S
S C
S S
C
( , , )
( , )
( , )
( , )
Φ Θ Ψ
Φ
Θ
Ψ
Φ Θ Ψ
Φ Ψ
Φ Θ Ψ
Φ Ψ
Φ Θ
Φ Θ Ψ
Φ Ψ
Φ Θ Ψ
Φ Ψ
Φ Θ
Θ Ψ
Θ Ψ
Θ
=
′
′′
=
=
−
−
−
+
−
+
−
0
0
0
0
0
0
1
RPY
Rot z
Rot y
Rot x
C C
S C
C S S
S S
C S C
S C
C C
S S S
C S
S S C
S
C S
C C
( , , )
( , )
( , )
( , )
φ θ ψ
φ
θ
ψ
φ θ
φ ψ
φ θ ψ
φ ψ
φ θ ψ
φ θ
φ ψ
φ θ ψ
φ ψ
φ θ ψ
θ
θ ψ
θ ψ
=
=
=
−
+
+
+
−
+
−
0
0
0
0
0
0
1
Macierz przekształcenia
jednorodnego:
Kąty Eulera
Katy RPY
(roll, pitch, yaw)
Roboty przemysłowe
KRIM, AGH w Krakowie
17
Wyznaczanie kątów RPY
−
+
−
+
+
+
−
=
1
0
0
0
0
0
0
)
,
,
(
ψ
θ
ψ
θ
θ
ψ
θ
φ
ψ
φ
ψ
θ
φ
ψ
φ
θ
φ
ψ
θ
φ
ψ
φ
ψ
θ
φ
ψ
φ
θ
φ
ψ
θ
φ
C
C
S
C
S
C
S
S
S
C
S
S
S
C
C
C
S
C
S
C
S
S
S
S
C
C
S
C
C
RPY
=
1
0
0
0
z
z
z
z
y
y
y
y
x
x
x
x
P
A
O
N
P
A
O
N
P
A
O
N
T
φ
θ
ψ
=
=
−
−
=
arctg
N
N
arctg
N
N
arctg
O
A
y
x
z
z
z
z
1
2
Roll (z)
Pitch (y)
Yaw(x)
Wyjątki:
o
90
=
θ
(
)
(
)
y
x
O
O
=
−
=
−
φ
ψ
φ
ψ
cos
sin
y
x
O
O
arctg
=
−
φ
ψ
o
90
−
=
θ
(
)
(
)
y
x
O
O
=
+
=
+
−
φ
ψ
φ
ψ
cos
sin
y
x
O
O
arctg
−
=
+
φ
ψ
Roboty przemysłowe
KRIM, AGH w Krakowie
18
Funkcja ATAN2
x
y
I
y>0
x>0
II
y>0
x<0
III
y<0
x<0
IV
y<0
x>0
α
=
x
y
x
y
arctg
2
atan
I i IV ćwiartka
π
±
=
x
y
x
y
arctg
2
atan
II i III ćwiartka
Roboty przemysłowe
KRIM, AGH w Krakowie
19
Przykład 3
−
−
−
−
=
1
0
0
0
0
0
1
0
0
2
3
0
2
1
0
2
1
0
2
3
1
A
u
v
w
x
±150
°
±90
°
±60
°
y
±120
°
±90
°
±150
°
z
±90
°
±180
°
±90
°
v
w
u
-60
°
-150
°
-150
°
120
°
x
y
z
Roboty przemysłowe
KRIM, AGH w Krakowie
20
( )
0
0
1
0
=
−
=
θ
tg
0
=
θ
( )
−∞
=
−
=
0
1
ψ
tg
φ
θ
ψ
=
=
−
−
=
arctg
N
N
arctg
N
N
arctg
O
A
y
x
z
z
z
z
1
2
o
90
−
=
ψ
x
y
z
-90
°
x
′
z
′
y
′
-150
°
v
w
u
−
−
−
−
=
1
0
0
0
0
0
1
0
0
2
3
0
2
1
0
2
1
0
2
3
1
A
( )
3
1
2
3
2
1
−
−
=
−
−
=
φ
tg
°
−
= 150
φ
Wyszukiwarka
Podobne podstrony:
więcej podobnych podstron