Katedra Robotyki i Mechatroniki

Akademia Grniczo-Hutnicza w Krakowie

Kinematyka i dynamika ukadw mechatronicznych

Wojciech Lisowski
2
Opis pooenia i orientacji efektora
Zastosowanie modelu geometrycznego zadanie proste

KIDUM

KRIM, AGH w Krakowie

Zagadnienia:
Macierz przeksztacenia jednorodnego:
interpretacja elementw
Interpretacja zadanej orientacji w oparciu
o kosinusy kierunkowe lub kty RPY
Notacja Denavita-Hartenberga
Przykady modeli geometrycznych

KIDUM

KRIM, AGH w Krakowie

Pooenie:
x, y, z
v

P
y

Orientacja:
, ,

Opis z wykorzystaniem macierzy przeksztacenia jednorodnego

KIDUM

KRIM, AGH w Krakowie

Macierz przeksztacenia jednorodnego pozwala okreli pooenie i

orientacj lokalnego ukadu wsprzdnych Puvw w ukadzie
odniesienia Oxyz
z

v
P
y

u x
u
A= y
u z

vx
vy
vz

wx
wy
wz

xP
y P
zP

u x
R = u y
u z
KIDUM

vx
vy
vz

wx
w y
wz

) )
ix o iu
) )
)j y o i)u
k z o iu

) )
ix o jv
) )
j oj
)y )v
k z o jv

KRIM, AGH w Krakowie

) )
ix o k w
) )
j ok
)y )w
k z o k w

R 1 = R T
4

Podstawowe macierze rotacji

1 0

R( x , ) = 0 C
0 S

KIDUM

KRIM, AGH w Krakowie

S
C

4 podstawowe macierze przeksztacenia jednorodnego

A = Tra(a, b, c) Rot ( x, ) Rot ( y, ) Rot ( z, )
0
1 0
0 C S
Rot ( x , ) =
0 S C

0
0 0

C S
S C
Rot ( z, ) =
0
0

0
0

0
0
0

0 0
0 0
1 0

0 1

C
0
Rot ( y, ) =
S

0 S 0
1 0 0
0 C 0

0 0 1

1
0
Tra (a, b, c) = Tra ( x, a ) Tra ( y, b) Tra ( z , c) =
0

0 0 a
1 0 b
0 1 c

0 0 1

Uwaga! Skadanie przeksztace jednorodnych nie jest przemienne:

Rot ( x, ) Rot ( y, ) Rot ( y, ) Rot ( x, )
KIDUM

KRIM, AGH w Krakowie

Przykad 1

z0

y1

O1 P =1P = [ 2,1,0]T

z1
O1

O0

y0
P
3

x0

?
?
A1 =
?

? ? 3
? ? 5
? ? 2

0 0 1

x1 = [0,0,1]T

x1

0 1
0 0
A1 =
1 0

0 0

0 3
1 5
0 2

0 1

y1 = [1,0,0]T

(
(
1
P = A1 P

2
5
=
0

1

0 1
0 0

1 0

0 0

0 3 2
1 5 1
0 2 0

0 1 1

O0 P = 0 P = [2,5,0]T

z1 = [0,1,0]T
KIDUM

(
P = [2,1,0,1]T

KRIM, AGH w Krakowie

Orientacja: , ,
Kty: RPY

Kty Eulera
EU (, , ) = Rot ( z , ) Rot (v, ) Rot ( w, )

RPY ( , , ) = Rot ( z , ) Rot ( y, ) Rot ( x, )

PRECESJI -
NUTACJI -
OBROTU WASNEGO -

(Fu)

KIDUM

KRIM, AGH w Krakowie

(Fu)

OBROTU (Roll) -
POCHYLENIA (Pitch) -
SKRTU (Yaw) -
8

Wyznaczanie ktw RPY

N x
N
T = y
N z

Ox

Ax

Oy
Oz

Ay
Az

Px
Py
Pz

CC
SC
RPY ( , , ) =
S

SC + CSS
CC + SSS
CS

SS + CSC
CS + SSC
CC

Wyjtki:
= arctg

Ny
Nz

= arctg
= arctg

Roll (z)

Nx
1 Nz
Oz
Az

KIDUM

= 90o

= 90o

sin ( ) = Ox

Pitch (y)

cos( ) = Oy

Yaw(x)

= arctg

Ox
Oy

KRIM, AGH w Krakowie

sin ( + ) = Ox

cos( + ) = Oy

+ = arctg

Ox
Oy

0
0
0

Przykad 2
3

2
1
A1 =
2
0

1
2

3
2
0

0
0

150

90

60

120

90

150

90

180

90

-150

120

-60
-150

KIDUM

KRIM, AGH w Krakowie

10

1
1
tg ( ) = 2 =
3 3

-90

0
=0
1 0

tg ( ) =

tg ( ) =

= 150
y

=0
x

1
=
0

= 90

= arctg
= arctg
= arctg
KIDUM

-150

Ny
Nx

u
y

Nz
1 Nz
Oz
Az

2
1
v
A1 =
2
0

KRIM, AGH w Krakowie

0

1
2

3
2
0

0
0

1 11

Zaoenia budowy modelu geometrucznego

Rozwaana klasa manipulatorw
- struktura nienadmiarowa
- otwarty acuch kinematyczny
- czony modelowane jako bryy sztywne
- czony poczone ze sob przegubowo lub pryzmatycznie
acuch kinematyczny manipulatora o jednym kocu
zamocowanym do podstawy a drugim swobodnym.
W wolnym kocu acucha kinematycznego manipulatora
zamocowany jest efektor, ktrym moe by:
- chwytak
- narzdzie montaowe
- narzdzie technologiczne
- narzdzie inspekcyjne
KIDUM

KRIM, AGH w Krakowie

12

Wsplne cechy seryjnie produkowanych manipulatorw:

wszystkie zcza stanowi pary kinematyczne klasy 5, obrotowe
lub pryzmatyczne
czony manipulatora s prostoliniowe
o obrotu zczy obrotowych jest albo rwnolega albo
prostopada do bliszego podstawy czonu manipulatora
Rami
Ki

KIDUM

KRIM, AGH w Krakowie

13

Wzajemne pooenie par ssiednich czonw jest opisywane przez

tzw. wsprzdne zczowe (przegubowe):
q1, q2, ..., qn
Pooenie i orientacja efektora (lub dowolnego czonu) w
przestrzeni jest okrelona przez wsprzdne kartezjaskie:
x, y, z, , ,
Model geometryczny manipulatora pozwala wyznaczy
zalenoci midzy wartociami wsprzdnych zczowych a
wartociami wsprzdnych kartezjaskich, okrelajcych
pooenie i orientacj efektora w jego przestrzeni roboczej, co jest
konieczne do zaplanowanie operacji manipulacyjnej lub
technologicznej.
KIDUM

KRIM, AGH w Krakowie

14

Z podstaw manipulatora zwizany

jest ukad wsprzdnych odniesienia
Ox0y0z0
z kadym i-tym czonem zwizany
jest ukad lokalny Oxiyizi
0T
macierz
przeksztacenia
n
jednorodnego
pozwala
opisa
pooenie i orientacj dowolnego
czonu
manipulatora
(efektora)
wzgldem
przyjtego
ukadu
odniesienia Ox0y0z0.
0

T i = T i = A1 A 2 ... A i 1 A i

macierz Ai opisuje pooenie i orientacj ukadu i-tego w i-1

KIDUM

KRIM, AGH w Krakowie

15

Notacj Denavita-Hartenberga okrela:

sposb definiowania lokalnych ukadw wsprzdnych
definicj przeksztacenia jednorodnego Ai
rodki lokalnych ukadw wsprzdnych naley przyjmowa:
w punkcie przecicia osi ruchu, jeli osie te si przecinaj
tak by przesunicia wzdu osi ruchu byy minimalne (najlepiej
0) jeli osie ruchu s rwnolege
w punkcie kocowym odcinka wsplnej normalnej, jeli osie
ruchu s skone
w przypadku wolnego koca acucha kinematycznego
manipulatora: w osi kici, na konierzu kici lub w wybranym
punkcie efektora.

KIDUM

KRIM, AGH w Krakowie

16

Zasady definiowania lokalnych ukadw wsprzdnych:

skierowa osie xi wszystkich lokalnych ukadw wsprzdnych
jak o x0 ukadu odniesienia
osi obrotu czonu i-tego w zczach obrotowych jest o zi-1
osi przemieszczenia czonu i-tego w zczach postpowych jest
o zi-1
osie xi-1 mog by rwnie osiami ruchu przy spenieniu
dodatkowych warunkw
Skadowe przeksztacenia jednorodne opisane macierz Ai
to 4 przeksztacenia podstawowe:
obrotu wok osi zi-1 o kt i
przemieszczenia wzdu osi zi-1 o di
przemieszczenia wzdu osi xi-1 o ai
obrotu wok osi xi o kt i
KIDUM

KRIM, AGH w Krakowie

17

Ai = Rot ( zi 1 , i ) Tra ( zi 1 , d i ) Tra ( xi 1 , ai ) Rot ( xi , i )

i
zi

zi '

yi'
xi

yi
zi-1
ai
i

di

yi-1
xi-1

KIDUM

KRIM, AGH w Krakowie

18

z0

z1

y0

Manipulator SCARA
RRPR

y1

x0

x1

x2

y2
z2

x4

y4
z4
x3

y3
z3

Czon Nr

1 v

a1

-120o120o

2 v

a2

0o150o

d3 v

0.1 m 0.3 m

4 v

-180o180o

KIDUM

Zakres ruchu

KRIM, AGH w Krakowie

19

Wyznaczenie wsprzdnych kartezjaskich SCARA

Nx
N
o
e = y
Nz

Ox

Ax

Oy

Ay

Oz
0

Az
0

Px
Py
Pz

C12 C4 + S12 S 4
S C C S
0
12 4
T 4 = 12 4

C12 S 4 + S12 C4
C12 C4 S12 S 4
0
0

a1C1 + a 2 C12
0 a1 S1 + a 2 S12

1
d 3

0
1

x = a1C1 + a2 C12
y = a1 S1 + a2 S12
z = d 3
= arctg
= arctg
= arctg

S12 C 4 C12 S 4
C12 C 4 + S12 S 4

Ny

= arctg

Nx
Nz
1 Nz
Oz
Az

KIDUM

sin ( 1 + 2 4 )
= arctg
= 1 + 2 4
(
)
+

cos

1
2
4

0
= 0o
1
0
= arctg = 180 o
1

= arctg

KRIM, AGH w Krakowie

20

z5
y5

z2

z1

y1

x1

x5
y2

x2

Link
No.

1 v

d1

-90o

2 v

a2

3 v

a3

4 v

90o

5 v

d5

z4
z0
y4
y0
z3

x4

x0
x3
y3

Manipulator RRR RR
KIDUM

KRIM, AGH w Krakowie

21

z1

y1

y3

z2

x1

z3

x2

y2

x3

y7
z0

z7
y0
x0

x7

z4

y4

x4
z6

z5

manipulator RRRRRR
(RRR+RPY)

KIDUM

d1

-90o

a2

3+90o v

90o

d4

-90o

90o

d7

y6

y5

x5

Link No.

x6

KRIM, AGH w Krakowie

22

y2
x2
y3
z2

Nr czonu

d1 v

a2

d3

x3

z3

z1
x1

y1

z0

90o

x0
y0

Rami kartezjaskie PPP

KIDUM

1
0
T3 =
0

KRIM, AGH w Krakowie

a2
0 1 d3

d1
1 0

0 0
1
0

23

z3
z1
x3

y1

Nr czonu

d1 v

a2

-90o

d3 v

y3
x1

z2

x2

y2
z0
y0

x0

C2
S
2
T3 =
0

S2

0
1

C2
0

a 2 C2 d 3 S 2
a 2 S 2 + d 3 C2

d1

Rami cylindryczne PRP

KIDUM

KRIM, AGH w Krakowie

24

z3
y3

x3

Nr czonu

1 v

d1

-90o

2 v

90o

d3 v

z2
y2

z1

x1

y1

z0
y0

x0

C1C2
S C
1 2
T3 =
S2

S1
C1

C1 S2
S1 S2

0
0

C2
0

d 3C1 S2
d 3 S1S2

d1 + d 3C2

Rami sferyczne RRP

KIDUM

KRIM, AGH w Krakowie

25

Nr czonu

1 v

d1

-90o

2 v

a2

3 v

90o

z3
y3

z1

y1

x3

x1

z2

y2

x2

C1C23
S C
1 23
T3 =
S23

z0
y0

x0

S1

C1S23

C1
0

S1 S23
C23

a 2 C1C2
a 2 S1C2

d1 a 2 S 2

Rami antropomorficzne RRR

KIDUM

KRIM, AGH w Krakowie

26

Konwencja orientowania osi chwytaka

no = a

(Fu)
Wektory: n normalny (xe)
o orientacji (ye)
a zblienia (ze)
KIDUM

KRIM, AGH w Krakowie

27

Ki Eulera (typ RBR)

KIDUM

KRIM, AGH w Krakowie

28

y5
z4

y4

z5

Nr czonu

4 v

d4

-90o

5 v

90o

6 v

d6

x5
x4

y6

z6

y3

x6

z3

x3

C4 C5 C6 S 4 S 6
S C C + C S
4 5 6
4 6
3
T6 =

S 5 C6

C4 C5 S 6 S 4 C6

C4 S 5

S 4 C5 S 6 + C4 C6

S 4 S5

S5 S 6

C5

d 6 C4 S 5
d 6 S 4 S5

d 4 + d 6 C5

Ki Eulera (typ RBR)

KIDUM

KRIM, AGH w Krakowie

29

Ki RPY (typ RBB)

KIDUM

KRIM, AGH w Krakowie

30

z4

y4

y5

z5
x4

y6

Nr czonu

4 v

d4

-90o

5 v

90o

6 v

d7

x5
z6

x6

y3

y7

z3

x3

Ki RPY (typ RBB)

KIDUM

z7

x7

C4 C5

S C
3
T 6 = 4 5
S5

C4 S5 S 6 S 4 C6

C4 S5 C6 + S 4 S 6

S 4 S5 S 6 + C4 C6
C5 S 6

S 4 S5 C6 C4 S 6
C5 C6

KRIM, AGH w Krakowie

d 7 (C4 S5 C6 + S 4 S 6 )

d 7 ( S 4 S5 C6 C4 S 6 )
d 4 + d 7 C5 C6

31

z6

x6

y6

z3
z5
z4

x3

x5
y3

Nr czonu

4 v

5 v

6 v

d6

x4
y5
y4

Ki RPY (typ BBR)

KIDUM

S 4 C5 S 6 + C4 C6
C C S +S C
4 5 6
4 6
3
T6 =

S5 S 6

KRIM, AGH w Krakowie

S 4 C5 C6 C4 S 6

S 4 S5

C4 C5 C6 S 4 S 6
S 5 C6

C4 S 5
C5

d 6 S 4 S5
d 6 C4 S 5

d 6 C5

32

Ocena rnicy pozycji

zd

Zadana pozycja:
yd
xd

zr
P

z0

yr

Pd

N dx
N
0
T d = dy
N dz

xr

N rx
N
0
T r = ry
N rz

y0

Rnica pooenia:
P = P r P d

Rnica orientacji:
d
d
T 0
0
0
dr, dr, dr
Rr = R0 Rr = Rd Rr

KIDUM

Adx

Ody
Odz

Ady
Adz

Pdx
Pdy
Pdz

Osignita pozycja:

Pr

2 =

Odx

Orx

Arx

Ory
Orz

Ary
Arz

Prx
Pry
Prz

1 = dr + dr + dr
2

N d N r + Od Or + Ad Ar
KRIM, AGH w Krakowie

33