ZÁKLADY ROBOTIKY Transformace souřadnic

ZÁKLADY ROBOTIKY Transformace souřadnic Ing. Josef Černohorský, Ph.D.

TECHNICKÁ UNIVERZITA V LIBERCI Fakulta mechatroniky, informatiky a mezioborových studií

Tento materiál vznikl v rámci projektu ESF CZ.1.07/2.2.00/07.0247 Reflexe požadavků průmyslu na výuku v oblasti automatického řízení a měření, který je spolufinancován Evropským sociálním fondem a státním rozpočtem ČR

Transformace souřadnic Reflexe požadavků průmyslu na výuku v oblasti automatického řízení a měření

Z ákladní ot ázky pro tento blok? Základní otázky Změny souřadného systému Matice rotace Přepočet vektoru Posunutí systému Homogenní transformace

• Jak zapsat bod, jehož polohu znám v rámci jednoho souřadného systému v jiném systému – Pootočeném systému – Posunutém systému – Pootočeném a posunutém systému

• Jak zapsat změnu vektoru v rámci jednoho systému – Posun vektoru – Pootočení vektoru – Posun i pootočení


P řipomenutí sou řadných syst é mů Připomenutí souřadných systémů Změny souřadného systému Matice rotace Přepočet vektoru Posunutí systému Homogenní transformace

θ2

θ1

θ3

x, y, θ

Kloubový vs. Globální souřadný systém


Vyj ádření zm ěny sou řadného syst ému Vyjádření změny souřadného systému Změny souřadného systému

YA ZB

Matice rotace Přepočet vektoru Posunutí systému

XA

Z

A

Homogenní transformace

• Bod AP zobrazený v A bod BP zobrazený v B • Vzájemný vztah „orientací“ se dá popsat maticí rotace


Zp ět do 2D Zpět Změny souřadného systému Matice rotace Přepočet vektoru

YA YB A

P

XB

Posunutí systému Homogenní transformace

XA

• Bod AP zobrazený v A pokud známe jeho souřadnice v B • Vzájemný vztah „orientací“ se dá popsat rovnicemi


Zp ět do 2D Zpět Změny souřadného systému

YA

Matice rotace Přepočet vektoru Posunutí systému Homogenní transformace

YB

xa = xb cos ϕ − yb sin ϕ ya = xb sin ϕ + yb cos ϕ ⎡ xa ⎤ ⎡cos ϕ ⎢ y ⎥ = ⎢ sin ϕ ⎣ a⎦ ⎣

− sin ϕ ⎤ ⎡ xb ⎤ ⎢ ⎥ ⎥ cos ϕ ⎦ ⎣ yb ⎦

A

P

XB

XA


Vyj ádření zm ěny sou řadného syst ému Vyjádření změny souřadného systému Změny souřadného systému

YA

Matice rotace

YB

Přepočet vektoru Posunutí systému

XA

ZB

Homogenní transformace Z

A

XB

A

XB= R ⋅ XB A B

B

⎡ r11 ⎢ A B R = ⎢ r21 ⎢⎣ r31

r12 r22 r32

r13 ⎤ ⎥ r23 ⎥ r33 ⎥⎦

• Bod AP zobrazený v A bod BP zobrazený v B • Vzájemný vztah „orientací“ se dá popsat maticí rotace


Jednotliv é sloupcov é vektory R Jednotlivé sloupcové A

Změny souřadného systému

YA

YB

Matice rotace

XB= R ⋅ XB A B

B


⎡0 ⎤ A YB = BAR ⋅ ⎢⎢1⎥⎥ ⎢⎣0⎥⎦

XA

ZB


A

XB

A B

[

R = XB A

⎡1⎤ A X B = BAR ⋅ ⎢⎢0⎥⎥ ⎢⎣0⎥⎦

A

YB

A

ZB

]

⎡0 ⎤ A Z B = BAR ⋅ ⎢⎢0⎥⎥ ⎢⎣1⎥⎦


B do A a co A do B? Změny souřadného systému

YB

Matice rotace Přepočet vektoru Posunutí systému

Z

A

XB

Homogenní transformace A B

R =

[

A

XB

A

YB

A B

B A

T

R= R

A B

R −1 = BAR T

Vlastnost ortonormální matice ⎡ B X AT ⎢ B T A Z B = ⎢ YA ⎢ BYAT ⎣

]

⎤ ⎥ ⎥= ⎥ ⎦

[

B

XA

B

YA

B

]

T

Z A = ABR T


P říklad 1 Příklad Změny souřadného systému Matice rotace Přepočet vektoru Posunutí systému Homogenní transformace

• Jednoduchá rotace mezi dvěma souřadnými systémy (B do A) a zpět


Syst ém rotovaný kolem Xosy Systém Změny souřadného systému

YA

Matice rotace Přepočet vektoru

XB

Y A

ZB

Z


B

Posunutí systému

XA

⎡1 0 0 ⎤ ⎢0 0 − 1⎥ A R = B ⎢ ⎥ ⎢⎣0 1 0 ⎥⎦

⎡ X B ⋅ X A ⎤ ⎡1 ⋅1⎤ ⎡ r11 ⎤ A X B = ⎢⎢ X B ⋅ YA ⎥⎥ = ⎢⎢1 ⋅ 0⎥⎥ = ⎢⎢r21 ⎥⎥ ⎢⎣ X B ⋅ Z A ⎥⎦ ⎢⎣1 ⋅ 0⎥⎦ ⎢⎣r31 ⎥⎦ ⎡YB ⋅ X A ⎤ ⎡1 ⋅ 0⎤ ⎡ r12 ⎤ A YB = ⎢⎢ YB ⋅ YA ⎥⎥ = ⎢⎢1 ⋅ 0⎥⎥ = ⎢⎢r22 ⎥⎥ ⎢⎣ YB ⋅ Z A ⎥⎦ ⎢⎣1 ⋅1⎥⎦ ⎢⎣ r32 ⎥⎦ ⎡ Z B ⋅ X A ⎤ ⎡ 1 ⋅ 0 ⎤ ⎡ r13 ⎤ A Z B = ⎢⎢ Z B ⋅ YA ⎥⎥ = ⎢⎢1 ⋅ −1⎥⎥ = ⎢⎢r23 ⎥⎥ ⎢⎣ Z B ⋅ Z A ⎥⎦ ⎢⎣ 1 ⋅ 0 ⎥⎦ ⎢⎣ r33 ⎥⎦


Z A do B Změny souřadného systému

YA


XB

Y A

ZB

Z


B

Posunutí systému

XA

⎡1 0 0 ⎤ ⎢0 0 1 ⎥ B R = A ⎢ ⎥ ⎢⎣0 − 1 0⎥⎦

⎡ X A ⋅ X B ⎤ ⎡1 ⋅1⎤ ⎡ r11 ⎤ B X A = ⎢⎢ X A ⋅ YB ⎥⎥ = ⎢⎢1 ⋅ 0⎥⎥ = ⎢⎢r21 ⎥⎥ ⎢⎣ X A ⋅ Z B ⎥⎦ ⎢⎣1 ⋅ 0⎥⎦ ⎢⎣r31 ⎥⎦ ⎡YA ⋅ X B ⎤ ⎡ 1 ⋅ 0 ⎤ ⎡ r12 ⎤ B YA = ⎢⎢ YA ⋅ YB ⎥⎥ = ⎢⎢ 1 ⋅ 0 ⎥⎥ = ⎢⎢r22 ⎥⎥ ⎢⎣ YA ⋅ Z B ⎥⎦ ⎢⎣1 ⋅ −1⎥⎦ ⎢⎣ r32 ⎥⎦ ⎡ Z A ⋅ X B ⎤ ⎡1 ⋅ 0⎤ ⎡ r13 ⎤ B Z A = ⎢⎢ Z A ⋅ YB ⎥⎥ = ⎢⎢1 ⋅1⎥⎥ = ⎢⎢r23 ⎥⎥ ⎢⎣ Z A ⋅ Z B ⎥⎦ ⎢⎣1 ⋅ 0⎥⎦ ⎢⎣ r33 ⎥⎦


Z B do A a A do B Změny souřadného systému

YA


XB

Y A

Z


B

Posunutí systému

ZB

XA

⎡1 0 0 ⎤ ⎢0 0 − 1⎥ A R = B ⎢ ⎥ ⎢⎣0 1 0 ⎥⎦ ⎡1 0 0⎤ ⎢ 0 0 1 ⎥ = AR T B R = A ⎢ ⎥ B ⎢⎣0 − 1 0⎥⎦


P říklad 1 - zzávěr ávěr Příklad Změny souřadného systému Matice rotace Přepočet vektoru Posunutí systému Homogenní transformace

• Prostá matice 3x3, ortonormální, inverzní matice je rovna transponované


Speci ální ppřípady řípady rotace X,Y,Z Speciální Změny souřadného systému Matice rotace Přepočet vektoru Posunutí systému Homogenní transformace

0 ⎡1 RX (θ ) = ⎢⎢0 cos θ ⎢⎣0 sin θ ⎡ cos ϑ RY (ϑ ) = ⎢⎢ 0 ⎢⎣− sin ϑ ⎡cos ζ RZ (ζ ) = ⎢⎢ sin ζ ⎢⎣ 0

0 ⎤ − sin θ ⎥⎥ cos θ ⎥⎦

0 sin ϑ ⎤ 1 0 ⎥⎥ 0 cos ϑ ⎥⎦ − sin ζ 0⎤ cos ζ 0⎥⎥ 0 1⎥⎦


P řepočet obecn ého vektoru Přepočet obecného Změny souřadného systému

YA


XB

P

A

ZB

XA

Z


Y

B

Posunutí systému

⎡ B X A ⋅B P ⎤ ⎡ B X TA ⎤ ⎢B B ⎥ ⎢B T⎥B A P = ⎢ YA ⋅ P ⎥ = ⎢ Y A ⎥ ⋅ P ⎢ B Z A ⋅B P ⎥ ⎢ B Z T ⎥ ⎣ ⎦ ⎣ A⎦

A

P = BAR ⋅B P


Posunut řadného syst ému Posunutíí sou souřadného systému Změny souřadného systému Matice rotace Přepočet vektoru Posunutí systému Z

B


Z

A

A

P = APP + BP


Obecn á transformace (Posun+Rotace) Obecná Změny souřadného systému

YB

Matice rotace

YA

XB

B

A

P

P

Přepočet vektoru

A

Posunutí systému

A

Pp

XA


A

ZB

⎡ A P ⎤ ⎡ BA R A PP ⎤ ⎡ B P ⎤ ⎥⋅⎢ ⎥ ⎢ ⎥=⎢ ⎢⎣ 1 ⎥⎦ ⎢⎣ 0 0 0 1 ⎥⎦ ⎢⎣ 1 ⎥⎦

P = R⋅ P + PP A B

B

A


Homogenn Homogenníí transformace Změny souřadného systému

YB

YA



B

P

XB

P

A

XA

Pp

A

ZB Z

Posunutí systému

A

⎡ P⎤ ⎡ PP ⎤ ⎡ P ⎤ R ⎥⋅⎢ ⎥ ⎢ ⎥=⎢ ⎢⎣ 1 ⎥⎦ ⎢⎣ 0 0 0 1 ⎥⎦ ⎢⎣ 1 ⎥⎦ A A B P( 4 x1) = BT( 4 x 4 ) ⋅ P( 4 x1) A

A B

A

B


A do B a co zp ět? B do A zpět? Změny souřadného systému

YB

Matice rotace

YA

Přepočet vektoru

B

P

A

XA

Posunutí systému

Pp

A

ZB Z


A

XB

P

A A ⎡ ⎤ P R P A B T = ⎢ ⎥ B ⎢⎣ 0 0 0 1 ⎥⎦ B A

R = BAR −1 = BAR T

A T A A T ⎡ ⎤ R P − ⋅ R B P B A −1 B T T = = ⎢ ⎥ A B 1 ⎢⎣ 0 0 0 ⎥⎦

A B

P −1 ≠ BAP T


P říklad 2 Příklad Změny souřadného systému Matice rotace Přepočet vektoru Posunutí systému Homogenní transformace

• Dva systémy pootočené jako v předchozím příkladě, navíc posunuté • Určit výslednou transformační matici


Syst ém rotovaný kolem Xosy + posun Systém Změny souřadného systému

YA XB

Matice rotace B

ZB

Y

Přepočet vektoru

XA

Z


A

Posunutí systému

⎡1 0 0 ⎤ ⎢0 0 − 1⎥ A R = B ⎢ ⎥ ⎢⎣0 1 0 ⎥⎦

⎡0 ⎤ ⎢ ⎥ A PP = ⎢3⎥ ⎢⎣1⎥⎦


Syst ém rotovaný kolem Xosy + posun Systém Změny souřadného systému

⎡1 0 0 ⎤ ⎢ ⎥ A 0 − 1⎥ B R = ⎢0 ⎢⎣0 1 0 ⎥⎦

YA XB

Matice rotace B

ZB

Y

Přepočet vektoru

XA

Posunutí systému

Z

A


⎡1 ⎢0 A ⎢ T = B ⎢0 ⎢ ⎣0

0 0 0 −1 1 0 0 0

0⎤ 3⎥⎥ 1⎥ ⎥ 1⎦

⎡0 ⎤ ⎢ ⎥ A PP = ⎢3⎥ ⎢⎣1⎥⎦


P řepočet bodu P pomoc ční matice T Přepočet pomocíí transforma transformační Změny souřadného systému

YA XB

Matice rotace B

ZB

Y

Přepočet vektoru

XA

Posunutí systému

Z

A


⎡1 ⎢0 A ⎢ T = B ⎢0 ⎢ ⎣0

⎡0 ⎤ ⎢ ⎥ B P = ⎢1⎥ ⎢⎣1⎥⎦ 0 0 0 −1 1 0 0 0

0⎤ 3⎥⎥ 1⎥ ⎥ 1⎦

⎡?⎤ ⎢ ⎥ A P = ⎢?⎥ ⎢⎣?⎥⎦


P řepočet bodu P pomoc ční matice T Přepočet pomocíí transforma transformační Změny souřadného systému

YA XB

Matice rotace B

ZB

Y

Přepočet vektoru

⎡1 ⎢0 A ⎢ T = B ⎢0 ⎢ X ⎣0 A

Posunutí systému

Z

A


⎡0 ⎤ ⎢ 2⎥ A P =⎢ ⎥ ⎢ 2⎥ ⎢ ⎥ ⎣1 ⎦

0 0 0⎤ 0 − 1 3⎥⎥ 1 0 1⎥ ⎥ 0 0 1⎦

⎡0 ⎤ ⎢ ⎥ B P = ⎢1⎥ ⎢⎣1⎥⎦

⎡ AP ⎤ A ⎡ B P ⎤ ⎢ ⎥ = BT ⋅ ⎢ ⎥ ⎢⎣ 1 ⎥⎦ ⎢⎣ 1 ⎥⎦


P říklad 2 - zzávěr ávěr Příklad Změny souřadného systému Matice rotace Přepočet vektoru Posunutí systému Homogenní transformace

• Lze pracovat „po částech“ vyjádříme vzájemnou rotaci dvou systémů, vektor posunutí • Zapíšeme homogenní tvar • Rozšíříme bod o 1 • Přepočteme • Použijeme první tři komponenty (poslední 1 zahodíme)


St ále stejný sou řadný syst ém Stále souřadný systém Změny souřadného systému Matice rotace Přepočet vektoru Posunutí systému Homogenní transformace

• Jak v jeho rámci vyjádřit posun a pootočení vektoru • Pomůže mi to co už vím?


Rotace vektoru v rrámci ámci sou ř. sys souř. sys.. Změny souřadného systému Matice rotace

P

Přepočet vektoru

XA

Posunutí systému A

P´

Z


P′ = R ⋅ P

YA

P′ = RK (θ ) ⋅ P


Rotace kolem osy X Změny souřadného systému Matice rotace

P

Přepočet vektoru

XA P´

Z

A


P′ = RK (θ ) ⋅ P

YA

0 ⎡1 ⎢ RK (θ ) = RX (θ ) = ⎢0 cos θ ⎢⎣0 sin θ

0 ⎤ ⎥ − sin θ ⎥ cos θ ⎥⎦


Rotace kolem osy X o 36 °52’ 36°52’

Přepočet vektoru Posunutí systému Homogenní transformace

⎡?⎤ ⎢ ⎥ ′ P = ⎢?⎥ ⎢⎣?⎥⎦

P′ = RK (θ ) ⋅ P

YA

P XA P´

A

Matice rotace

⎡0 ⎤ ⎢ ⎥ P = ⎢ 2⎥ ⎢⎣1 ⎥⎦ Z

Změny souřadného systému

0 ⎤ ⎡0 ⎤ ⎡0 ⎤ ⎡1 0 ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ P′ = ⎢0 0,8 − 0,6⎥ ⎢2⎥ = ⎢1 ⎥ ⎢⎣0 0,6 0,8 ⎥⎦ ⎢⎣1 ⎥⎦ ⎢⎣2⎥⎦


Posunut Posunutíí vektoru, homogenn homogenníí def def.. Změny souřadného systému

P′ = P + Q

Matice rotace

P′ = T ⋅ P


Z

A


⎡1 ⎢0 T =⎢ ⎢0 ⎢ ⎣0

0 0 q1 ⎤ ⎥ 1 0 q2 ⎥ 0 1 q3 ⎥ ⎥ 0 0 1⎦


Sekvence transformac transformacíí T = T⋅ T

A C

Změny souřadného systému Matice rotace

A B

B C

B

Přepočet vektoru Posunutí systému Homogenní transformace

T⋅ T⋅ T =1

A B

B C

C A

A

⎡ R⋅ R PB + R ⋅ PC ⎤ T =⎢ ⎥ 1 ⎢⎣ 0 0 0 ⎥⎦

A C

A B

B C

A

A B

B

PB ↔C = BAR ⋅B PC


Souvislost s angul árním robotem angulárním Změny souřadného systému

T = T⋅ T⋅ T⋅ T⋅ T⋅ T

0 6

Matice rotace

1 0

2 1

3 2

4 3

5 4

6 5

• Na čem závisí hodnoty T

Přepočet vektoru

– Na rozměrech jednotlivých ramen (konstanty) – Úhlech natočení jednotlivých os (proměnné)!!!


http://www.servosystems.com/staubli_rx130.jpg


Pod ěkování Poděkování Změny souřadného systému Matice rotace Přepočet vektoru Posunutí systému Homogenní transformace

Tento materiál vznikl v rámci projektu ESF CZ.1.07/2.2.00/07.0247 Reflexe požadavků průmyslu na výuku v oblasti automatického řízení a měření, který je spolufinancován Evropským sociálním fondem a státním rozpočtem ČR.

ZÁKLADY ROBOTIKY Transformace souřadnic

Recommend Documents