kidum02

KIDUM KRIM, AGH w Krakowie 1

Katedra Robotyki i Mechatroniki

Akademia Górniczo-Hutnicza w Krakowie

Wojciech Lisowski

2Opis położenia i orientacji efektora

Zastosowanie modelu geometrycznego – zadanie proste

Kinematyka i dynamika układów mechatronicznych


Zagadnienia:Macierz przekształcenia jednorodnego: interpretacja elementówInterpretacja zadanej orientacji w oparciu o kosinusy kierunkowe lub kąty RPYNotacja Denavita-HartenbergaPrzykłady modeli geometrycznych


x

y

z

u

v

w

O

P

Położenie:x, y, z

Orientacja:ϕ, θ, ψ

Opis z wykorzystaniem macierzy przekształcenia jednorodnego


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

=

1000Pzzz

Pyyy

Pxxx

zwvuywvuxwvu

A

=

zzz

yyy

xxx

wvuwvuwvu

R

wzvzuz

wyvyuy

wxvxux

kkjkikkjjjijkijiii

)o

))o

))o

)

)o

))o

))o

)

)o

))o

))o

)

= R RT− =1


R x C SS C

( , )α α αα α

= −

1 0 000

Podstawowe macierze rotacji


),(),(),(),,( θϕα zRotyRotxRotcbaTraA =

Rot xC SS C

( , )αα αα α

=−

1 0 0 00 00 00 0 0 1

Rot y

C S

S C( , )ϕ

ϕ ϕ

ϕ ϕ=

−

0 00 1 0 0

0 00 0 0 1

Rot z

C SS C

( , )θ

θ θθ θ

=

−

0 00 0

0 0 1 00 0 0 1

=⋅⋅=

1000100010001

),(),(),(),,(cba

czTrabyTraaxTracbaTra

4 podstawowe macierze przekształcenia jednorodnego

Uwaga! Składanie przekształceń jednorodnych nie jest przemienne:),(),(),(),( αϕϕα xRotyRotyRotxRot ≠


Przykład 1z0

x0

y0

O0

3

5

2

O1

x1

z1

y1

P

A1

352

0 0 0 1

=

? ? ?? ? ?? ? ?

T

T

T

z

y

x

]0,1,0[

]0,0,1[

]1,0,0[

1

1

1

=

−=

−=

A1

0 1 0 30 0 1 51 0 0 2

0 0 0 1

=

−

−

TPPO ]0,1,2[11 ==

TP ]1,0,1,2[1 =(

01

1( (P A P=

2501

0 1 0 30 0 1 51 0 0 2

0 0 0 1

2101

=

−

−

TPPO ]0,5,2[00 ==


Orientacja: ϕ, θ, ψ

),(),(),(),,( ΨΘΦ=ΨΘΦ wRotvRotzRotEU ),(),(),(),,( ψθφψθφ xRotyRotzRotRPY =

Kąty Eulera KKąąty: RPYty: RPY

(Fu)PRECESJI - ΦNUTACJI - ΘOBROTU WŁASNEGO - Ψ

OBROTU (Roll) - φPOCHYLENIA (Pitch) - θSKRĘTU (Yaw) - ψ

(Fu)


Wyznaczanie kątów RPY

−+−+++−

=

1000000

),,(ψθψθθ

ψθφψφψθφψφθφψθφψφψθφψφθφ

ψθφCCSCS

CSSSCSSSCCCSCSCSSSSCCSCC

RPY

=

1000zzzz

yyyy

xxxx

PAONPAONPAON

T

φ

θ

ψ

=

=−

−

=

arctgNN

arctg N

N

arctg OA

y

x

z

z

z

z

1 2

Roll (z)

Pitch (y)

Yaw(x)

Wyjątki:

o90=θ

( )( ) y

x

OO

=−=−

φψφψ

cossin

y

x

OOarctg=−φψ

o90−=θ

( )( ) y

x

OO

=+=+−

φψφψ

cossin

y

x

OOarctg −

=+φψ


Przykład 2

−

−−

−

=

10000010

0230

21

0210

23

1A

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


( ) 001

0=

−=θtg 0=θ

( ) −∞=−

=01ψtg

φ

θ

ψ

=

=−

−

=

arctgNN

arctg N

N

arctg OA

y

x

z

z

z

z

1 2

o90−=ψ

x

y

z

-90°

x′

z′

y′

-150°

v

w

u

−

−−

−

=

10000010

0230

21

0210

23

1A

( )3

1

23

21

−−

=−

−=φtg °−= 150φ


Założenia budowy modelu geometrucznego

Rozważana klasa manipulatorów - struktura nienadmiarowa- otwarty łańcuch kinematyczny- człony modelowane jako bryły sztywne- człony połączone ze sobą przegubowo lub pryzmatycznie

Łańcuch kinematyczny manipulatora o jednym końcu zamocowanym do podstawy a drugim swobodnym.

W wolnym końcu łańcucha kinematycznego manipulatora zamocowany jest efektor, którym może być: - chwytak- narzędzie montażowe- narzędzie technologiczne - narzędzie inspekcyjne


Wspólne cechy seryjnie produkowanych manipulatorów:wszystkie złącza stanowią pary kinematyczne klasy 5, obrotowe

lub pryzmatyczneczłony manipulatora są prostolinioweoś obrotu złączy obrotowych jest albo równoległa albo

prostopadła do bliższego podstawy członu manipulatora

Ramię

Kiść


Wzajemne położenie par sąsiednich członów jest opisywane przez tzw. współrzędne złączowe (przegubowe):

q1, q2, ..., qn

Położenie i orientacja efektora (lub dowolnego członu) w przestrzeni jest określona przez współrzędne kartezjańskie:

x, y, z, ϕ, θ, ψ

Model geometryczny manipulatora pozwala wyznaczyćzależności między wartościami współrzędnych złączowych a wartościami współrzędnych kartezjańskich, określających położenie i orientację efektora w jego przestrzeni roboczej, co jest konieczne do zaplanowanie operacji manipulacyjnej lub technologicznej.


macierz Ai opisuje położenie i orientację układu i-tego w i-1

iiii AAAATT 1210 ... −==

Z podstawą manipulatora związany jest układ współrzędnych odniesieniaOx0y0z0z każdym i-tym członem związany jest układ lokalny Oxiyizimacierz 0Tn przekształcenia jednorodnego pozwala opisaćpołożenie i orientację dowolnego członu manipulatora (efektora) względem przyjętego układu odniesienia Ox0y0z0.


Notację Denavita-Hartenberga określa:sposób definiowania lokalnych układów współrzędnych definicję przekształcenia jednorodnego Ai

Środki lokalnych układów współrzędnych należy przyjmować:w punkcie przecięcia osi ruchu, jeśli osie te się przecinajątak by przesunięcia wzdłuż osi ruchu były minimalne (najlepiej 0) jeśli osie ruchu są równoległew punkcie końcowym odcinka wspólnej normalnej, jeśli osieruchu są skośne w przypadku wolnego końca łańcucha kinematycznego manipulatora: w osi kiści, na kołnierzu kiści lub w wybranym punkcie efektora.


Zasady definiowania lokalnych układów współrzędnych:skierować osie xi wszystkich lokalnych układów współrzędnych

jak oś x0 układu odniesieniaosią obrotu członu i-tego w złączach obrotowych jest oś zi-1osią przemieszczenia członu i-tego w złączach postępowych jest

oś zi-1osie xi-1 mogą być również osiami ruchu przy spełnieniu

dodatkowych warunków

Składowe przekształcenia jednorodne opisane macierzą Aito 4 przekształcenia podstawowe:

obrotu wokół osi zi-1 o kąt θiprzemieszczenia wzdłuż osi zi-1 o diprzemieszczenia wzdłuż osi xi-1 o aiobrotu wokół osi xi o kąt αi


),(),(),(),( 111 iiiiiiiii xRotaxTradzTrazRotA αθ ⋅⋅⋅= −−−

yi-1

zi-1

xi-1

xi

zi'

zi

yi'

yi

di

ai

αi

θi


Manipulator SCARA RRPR

Człon Nr θ d a α Zakres ruchu

1 θ1 v 0 a1 0 -120o÷120o

2 θ2 v 0 a2 π 0o÷150o

3 0 d3 v 0 0 0.1 m ÷0.3 m

4 θ4 v 0 0 0 -180o÷180o

x0

y0

z0

x1

y1z1

x2y2

z2

x3y3

z3

x4y4

z4


( )( )

o

o

1801

0

01

0

cossin

421421

421

412412

412412

=

−

=

=

=

−+=

−+−+

=

+−

=

arctg

arctg

arctgSSCCSCCSarctg

ψ

θ

θθθθθθθθθ

φφ

θ

ψ

=

=−

−

=

arctgNN

arctg N

N

arctg OA

y

x

z

z

z

z

1 2

x a C a Cy a S a Sz d

= += += −

1 1 2 12

1 1 2 12

3

04

12 4 12 4 12 4 12 4 1 1 2 12

12 4 12 4 12 4 12 4 1 1 2 12

3

00

0 0 10 0 0 1

T

C C S S C S S C a C a CS C C S C C S S a S a S

d=

+ − + +− − − +

− −

oe

x x x x

y y y y

z z z z

N O A PN O A PN O A P

τ =

0 0 0 1

Wyznaczenie współrzędnych kartezjańskich SCARA


x4

y4

z4

x2y2

z2

x0

y0

z0

x1y1

z1

x3

y3

z3

x5

y5

z5

Manipulator RRR RR

Link No.

θ d a α

1 θ1 v d1 0 -90o

2 θ2 v 0 a2 0

3 θ3 v 0 a3 0

4 θ4 v 0 0 90o

5 θ5 v d5 0 0


Link No. θ d a α

1 θ1 v d1 0 -90o

2 θ2 v 0 a2 0

3 θ3+90o v 0 0 90o

4 θ4 v d4 0 -90o

5 θ5 v 0 0 90o

6 0 0 0 α6 v

6 0 d7 0 0

manipulator RRRRRR(RRR+RPY)

x0

y0

z0

x1y1

z1

x2y2

z2

x3

y3

z3

x4

y4z4

x5

y5

z5

x6

y6

z6

x7

y7

z7


Nr członu θ d a α

1 0 d1 v 0 0

2 0 0 a2 v 90o

3 0 d3 v 0 0

T

ad

d3

2

3

1

1 0 00 0 10 1 00 0 0 1

=− −

Ramię kartezjańskie PPP

x0

x1

x2

x3

y0

y1

y2

y3

z0

z1

z2

z3


Ramię cylindryczne PRP

x0

y0

z0

x1

y1

z1

x2

y2

z2

x3

y3

z3


1 0 d1 v 0 0

2 θ2 v 0 a2 -90o

3 0 d3 v 0 0

T

C S a C d SS C a S d C

d3

2 2 2 2 3 2

2 2 2 2 3 2

1

00

0 1 00 0 0 1

=

− −+

−



1 θ1 v d1 0 -90o

2 θ2 v 0 0 90o

3 0 d3 v 0 0

Ramię sferyczne RRP

T

C C S C S d C SS C C S S d S S

S C d d C3

1 2 1 1 2 3 1 2

1 2 1 1 2 3 1 2

2 2 1 3 200 0 0 1

=

−

− +

x0

y0

z0

x1y1

z1

y2

z2

x3

y3

z3



1 θ1 v d1 0 -90o

2 θ2 v 0 a2 0

3 θ3 v 0 0 90o

T

C C S C S a C CS C C S S a S C

S C d a S3

1 23 1 1 23 2 1 2

1 23 1 1 23 2 1 2

23 23 1 2 200 0 0 1

=

−

− −

Ramię antropomorficzne RRR

x0

y0

z0

x1y1

z1

x2y2

z2

x3

y3

z3


Konwencja orientowania osi chwytaka

Wektory: n – normalny (xe)o – orientacji (ye)a – zbliżenia (ze)

aon =×

(Fu)


Kiść Eulera (typ RBR)


x3

y3

z3

x4

y4z4

x5

y5

z5

x6

y6

z6


4 θ4 v d4 0 -90o

5 θ5 v 0 0 90o

6 θ6 v d6 0 0

36

4 5 6 4 6 4 5 6 4 6 4 5 6 4 5

4 5 6 4 6 4 5 6 4 6 4 5 6 4 5

5 6 5 6 5 4 6 5

0 0 0 1

T

C C C S S C C S S C C S d C SS C C C S S C S C C S S d S S

S C S S C d d C=

− − −+ − +

− +

Kiść Eulera (typ RBR)


Kiść RPY (typ RBB)


x3

y3

z3

x7

y7

z7

x4

y4z4

x5

y5

z5

x6

y6

z6

Kiść RPY (typ RBB)


4 θ4 v d4 0 -90o

5 θ5 v 0 0 90o

6 0 0 0 α6 v

6 0 d7 0 0

( )( )3

6

4 5 4 5 6 4 6 4 5 6 4 6 7 4 5 6 4 6

4 5 4 5 6 4 6 4 5 6 4 6 7 4 5 6 4 6

5 5 6 5 6 4 7 5 6

0 0 0 1

T

C C C S S S C C S C S S d C S C S SS C S S S C C S S C C S d S S C C S

S C S C C d d C C=

− + ++ − −

− +


y3

z3

x3

y4

z4

x4

y5

z5

x5

y6

z6

x6

Kiść RPY (typ BBR)


4 θ4 v 0 0 0

5 0 0 0 α5 v

6 θ6 v d6 0 0

36

4 5 6 4 6 4 5 6 4 6 4 5 6 4 5

4 5 6 4 6 4 5 6 4 6 4 5 6 4 5

5 6 5 6 5 6 5

0 0 0 1

T

S C S C C S C C C S S S d S SC C S S C C C C S S C S d C S

S S S C C d C=

− + − −+ − − −


xd

xr

y0

z0

ydzd

yr

zr

Pd

Pr

∆P

Ocena różnicy pozycji

=

1000

0

dzdzdzdz

dydydydy

dxdxdxdx

d PAONPAONPAON

T

Zadana pozycja:

Osiągnięta pozycja:

=

1000

0

rzrzrzrz

ryryryry

rxrxrxrx

r PAONPAONPAON

TRóżnica położenia:

dr PP∆P −=

Różnica orientacji:

rT

drd

rd RRRRR 000

0 == ϕdr, θdr, ψdr

2222 rdrdrd AAOONN ×+×+×=∆Φ

drdrdr φθψ ++=∆Φ1

kidum02

Documents