ROBOTOK ÉS VIZSGÁLATUK I.
S 05
ÉAÜLT
BME
ÉAGT
ROBOT1 S 05 SEGÉDLET Dr. Pápai Ferenc 2012.
KOORDINÁTA TRANSZFORMÁCIÓK KOORDINÁTA TRANSZFORMÁCIÓK .....................................................................................................................1
1
1.1 FORGATÁS......................................................................................................................................................................1 Az elemi rotációs mátrixok inverze: ...................................................................................................................................2 1.2 R-P-Y SZÖGEK ...............................................................................................................................................................3 1.3 HOMOGÉN TRANSZFORMÁCIÓK ......................................................................................................................................4 A homogén transzformáció szorzat alakja és inverze .........................................................................................................5 A TCP PONT HELYÉNEK SZÁMÍTÁSA ....................................................................................................................6
2
A ROBOT KARRENDSZER DENAVIT-HARTENBERG MÁTRIXAI: ........................................................................................6
2.1 3
IRODALOM .....................................................................................................................................................................8
1 KOORDINÁTA TRANSZFORMÁCIÓK A robotok mozgását felfoghatjuk úgy is, mint a robotkarokhoz rögzített koordinátarendszerek (frame koordinátarendszerek) relatív helyzetének változását. Ennek megfelelően a TCP pont világkoordinátarendszerbeli helyzete a karokhoz rögzített koordinátarendszerek transzformációjával előállítható, ha ismerjük a koordinátarendszerek relatív helyzetét meghatározó időfüggvényeket. A továbbiakban a robot-specifikus koordináta transzformációkat tekintjük át.
1.1
Forgatás
z
A koordinátageometriából ismert módon a z tengely körüli forgatást (5.3. ábra) az
2
z
cos sin 0 R z Rot (z ) sin cos 0 , 0 0 1
1 Θ
y
2
mátrix segítségével írhatjuk le.
Θ y
1
Θ x x
2
1 ábra
D:\_sajáttárgyak\_robot-1\_segédletek\ROBOT1 S 05 Transzformációk.doc
(5.1)
2
~ e1
sin Θ
Θ
e2
cos Θ
~e -sin Θ 2
Θ
e1
cos Θ
Hasonló mátrixok képezhetők az x és y tengelyek körüli forgatásra is, ahol és a koordináta tengelyek körüli elfordulások szöge, így
0 1 R x Rot (x)0 cos 0 sin
0 sin , cos
cos R y Rot ( y ) 0 sin
0 sin 1 0 . 0 cos
(5.3)
(Ry-nál ügyeljen a negatív előjel helyére!) Ha bármelyik két mátrixot összeszorozzuk, akkor a két tengely körüli együttes forgatás mátrixához jutunk: 0 cos sin 0 1 R z R x Rot ( z )Rot ( x) sin cos 0 0 cos 0 0 1 0 sin
0 cos sin cos sin sin cos cos cos 0 sin
sin sin cos sin . (5.4) cos
A forgatás sorrendje nem közömbös. A fenti Rot ( z)Rot ( x) forgatás először egy x tengely körüli szöggel való forgatást, majd második lépésben az eredménynek a z tengely körüli szöggel való elforgatását jelenti. A három forgatási mátrix összeszorzásából három tengely körüli egyidejű forgatás mátrixa adódik: 0 cos sin 0 1 R z R x R y Rot ( z )Rot ( x)Rot ( y ) sin cos 0.0 cos 0 0 1 0 sin
0 cos sin . 0 cos sin
cos sin cos sin sin cos 0 sin sin cos cos cos sin 0 1 0 0 sin 0 cos sin cos cos cos sin sin sin sin cos cos sin sin sin cos sin cos cos cos sin cos cos sin sin cos sin cos cos sin sin cos cos
0 sin 1 0 0 cos
(5.5)
A forgatás sorrendje itt sem közömbös, a fenti forgatás először y tengely, majd x tengely, végül z tengely körüli forgatást jelent.
Az elemi rotációs mátrixok inverze: Az elemi rotációs mátrixok inverzei transzponáltjukkal egyenlő R Tekintsük a z tengely körüli forgatás mátrixát: ROBOT1 S 05 Transzformációk
1
RT
3
cos R z Rot (z ) sin 0 cos 1 Ennek inverze R z sin 0
sin 0 cos 0 0 1 sin 0 cos 0 0 1
Ellenőrizzük le, hogy a rotációs mátrix és inverzének szorzata valóban egységmátrixot eredményez. 2 2 0 0 cos sin 0 cos sin 0 cos sin R z R z 1 sin cos 0 sin cos 0 0 cos 2 sin 2 0 0 0 1 0 0 1 0 0 1 1 0 0 0 1 0 E 0 0 1
Ha az inverz-mátrix szerkezetét megfigyeljük, belátható, hogy az inverz-mátrix ugyanazon tengely körüli, de ellenkező irányú forgatásnak felel meg.
1.2
R-P-Y szögek
z Θ; R y β; P α;
Y x
ábra ROBOT1 S 05 Transzformációk
Az orientáció jellemzésének egy másik módja a csavarás (Roll), billentés (Pitch) és forgatás (Yaw) szögek használata. Az 5.4. ábrán lévő szögjelöléseket alkalmazva, és az R-P-Y sorrendnek megfelelően összeszorozva R (z), R (y), R (x) mátrixokat;
4 cos sin 0 cos RPY (, , ) R z R y R x Rot ( z )Rot ( y )Rot ( x) sin cos 0 0 0 0 1 sin cos cos sin cos sin
sin cos sin 1 0 cos sin sin 0 cos 0 cos 0 sin
0 sin cos
cos cos sin cos sin
sin cos cos sin sin cos cos sin sin sin cos sin
sin sin cos sin cos cos sin sin sin cos cos cos
0 sin 1 0 1 0 0 cos 0 cos 0 sin
0 sin cos
(5.6) forgató mátrixhoz jutunk, amely az 5.5. ábra szerinti forgatást eredményezi.
z
2
z
z 1 Θ
y
2
β
z
z 4
3 y
β
2
y 2
α
z
3
y 4
3 α
Θ y
3
1 β
Θ x x
y
x
2 x
1
2
3
x
3 x
4
Homogén transzformációk
1.3
z z
1
Tekintsük az ábrán lévő x1; y1; z1 és x2; y2; z2; koordinátarendszer P (x1P; y1P; z1P) és P (x2P; y2P; z2P) pontja közötti összefüggést az alábbi bázisfüggetlen alakban:
2 y
y
r
2
2P
P
2
r1 r2 p
z 2P x r
p e
3
1 y
e
e
y 1P
2
1 x
1P
ábra
x
1
ROBOT1 S 05 Transzformációk
1
2P x
2
r1 r2 z 1P p
A P pont koordinátái az ’1’ koordinátarendszerben A P pont koordinátái a ’2’ koordinátarendszerben A ’2’ koordinátarendszer origójának helyvektora (az ’1’-es koordinátarendszerben)
5 Legyenek továbbá e1; e2; e3 az x2; y2; z2 koordinátatengely irányú egységvektorok az i; j; k bázisában. A fenti bázisfüggetlen alak e1; e2; e3 ismeretében az alábbi formában írható fel:
x 1 y e e 2 1 1 z 1
x 2 e 3 y 2 p z 2
(5.8)
Írjuk fel a fenti mátrixegyenletet az alábbi alakban:
x1 y e 1 1 z1 0 1
e2 0
e3 0
x2 p y 2 A 1 z 2 0T 1
x2 p y 2 1 z 2 1
(5.9)
amelyből megállapíthatjuk, hogy az első három egyenlete azonos az előzőeken felírt mátrixegyenlettel, az utolsó
x1 egyenlete pedig, az 1 1 azonosság, így a két mátrixegyenlet ekvivalens. A fentiek alapján az y 1 vektor z 1 x 1 y 1 homogén koordinátás alakjának az 1 értékű negyedik koordinátával kiegészített vektort nevezzük. z1 1 A homogén transzformáció szorzat alakja és inverze Amennyiben a homogén transzformáció A-val jelölt minora egy forgató mátrix, akkor a homogén transzformáció a következő alakban írható fel:
R H T 0
p 1 .
Ahol R a forgató mátrix (3,3) méretű, p eltolásvektor (3,1) méretű, 0T pedig (1,3) méretű sorvektor (zérusvektor). A H mátrix szorzat alakban:
E H T 0
p R 1 0T
0 1
Az első tényező transzlációt, a második tényező rotációt fejez ki. A hipermátrixok szorzási szabályát alkalmazva látható, hogy a szorzat alak valóban visszaadja az eredeti H mátrixot:
E H T 0
p R 1 0T
0 E R p 0 T 1 0T R 1 0T
E0 p R T 0T 0 1 1 0
p 1
A szorzat mindkét tényezője nemszinguláris. A homogén transzformáció inverzének meghatározásához használjuk fel a szorzat alakot:
H H
1
1
E T 0
p R 1 0T
R 1 T 0
R 1p 1
0 1
1
R T 0
1
0 E 1 0T
p 1
1
R 1 T 0
0 E T 1 0
p R 1E 0 0T 1 0T E 1 0T
R 1p 0 1 0T p 1 1
Egyszerű visszaszorzással ellenőrizhető, hogy a homogén koordinátákkal kifejezett elemi rotáció és az elemi eltolás inverze valóban egy ellenkező irányú forgatás illetve egy ellenkező irányú eltolás. ROBOT1 S 05 Transzformációk
6 Táblázatosan összefoglalva:
Eredeti alak
Inverz
Rotáció
Eltolás
Rotáció+eltolás
R 0T
0 1
E 0T
p 1
R H T 0
R 1 0 T 1 0
E 0T
p 1
R 1 H 1 T 0
p 1
R 1p 1
Szorzat alak
E H T 0
p R 1 0T
0 1
R 1 H 1 T 0
0 E T 1 0
p 1
2 A TCP PONT HELYÉNEK SZÁMÍTÁSA A robot TCP pontja helyének számítására legcélravezetőbb a homogén transzformációk használata. Tekintsük az alábbi ábrán látható két szabadságfokú síkbeli robotot.
2.1
A robot karrendszer Denavit-Hartenberg mátrixai:
A robot karrendszer Denavit-Hartenberg mátrixait továbbiakban: D-vel jelöljük. A Denavit-Hartenberg mátrixok a transzformációkat a homogén koordinátákkal írják le, továbbá azt a konvenciót tartják, hogy két egymást követő szabadsági fok közötti transzformáció mindig úgy történik, hogy a csukló által biztosított forgatási szabadsági fok a z tengely körüli pozitív irányú forgatás legyen.
D D Dc cos sin sin cos 0 0 0 0
0 0 1 0
0 1 0 0 0 cos 0 0 sin 1 0 0
0 sin cos 0
a cos sin cos b sin cos cos s 0 sin 1 0 0
sin sin cos sin cos 0
a cos b sin a sin b cos s 1
Itt D D () a robot szabadsági foka miatti transzformáció mátrixa homogén koordinátákkal kifejezve, a (theta) változó függvénye. D C a konstrukció transzformációs mátrixa (x tengely körüli forgatás és eltolás, ez csak konstans adatokat tartalmaz. A feladatban szereplő robot esetében a TCP szerszámpontra vonatkozó eredő D mátrix egy hat tényezős szorzat:
D14 D12D 23D34 D12DC12D23DC 23D34DC 34 A konstrukciós és robotcsukló forgatási mátrixok: A theta változók szinuszait s-sel, koszinuszait c-vel jelöljük.
ROBOT1 S 05 Transzformációk
7
x1 x1
X3 x1 x1
x1 x1 x1 x1 x1 x1
Az 1.-ből a 2. koordinátarendszerbe való transzformáció mátrixa x tengely körüli: 1 90 -os forgatás és z irányban L1-gyel való eltolás. Csuklóforgatás z tengely körül :
D 12
c1 s 1 0 0
s 1 c1 0 0
0 0 1 0
0 0 0 1
D C12
1 0 0 c 1 0 s 1 0 0
0 s 1 c 1 0
0 1 0 0 L1 0 1 0
0 0 0 0 1 0 1 0 L1 0 0 1
Az 2.-ből a 3. koordinátarendszerbe való transzformáció mátrix x irányban L3-al való eltolás:
D 23
c 2 s 2 0 0
s 2 c 2 0 0
0 0 1 0
0 0 0 1
D C 23
1 0 0 0
0 L3 0 0 1 0 0 1
0 1 0 0
A 3.-ból a 4. koordinátarendszerbe való transzformáció mátrixa x irányban ’–L4’-gyel való eltolás:
D 34
c 3 s 3 0 0
s 3 c 3 0 0
0 0 1 0
0 0 0 1
D C 34
1 0 0 0
0 L4 0 0 1 0 0 1
0 1 0 0
Az eredő D mátrix 6 mátrix szorzata.: D14 c1 s1 0 0 1 s 1 c1 0 0 0 0 0 1 0 0 0 0 1 0 0 Elvégezve a beszorzást
0 0 0 c 2 0 1 0 s 2 1 0 L1 0 0 0 1 0
s 2 c 2 0 0
0 0 1 0
0 1 0 0 0 0 1 0
0 1 0 0
0 L3 c 3 0 0 s 3 1 0 0 0 1 0
s 3 c 3 0 0
D14 D 12 D 23 D 34 c1 s 1 0 0
0 s1 0 c1 1 0 0 0
0 c 2 0 s 2 L1 0 1 0
s 2 c 2 0 0
ROBOT1 S 05 Transzformációk
0 L3 c 2 c 3 0 L3 s 2 s 3 1 0 0 0 1 0
s 3 c 3 0 0
0 L4 c 3 0 L4 s 3 , 1 0 0 1
0 0 1 0
0 1 0 0 0 0 1 0
0 1 0 0
0 L4 0 0 1 0 0 1
8 D14 D 12 D 24 c1 s 1 0 0
0 s1 0 c1 1 0 0 0
0 c 2 c 3 0 s 2 c 3 L1 1
s 2 s 3 c 2 s 3 0 0
c 2 s 3 s 2 c 3 s 2 s 3 c 2 c 3 0 0
0 L4 c 2 c 3 L4 s 2 s 3 L3 c 2 0 L4 s 2 c 3 L4 c 2 s 3 L3 s 2 1 0 0 1
D14 D 12 D 24 c1 (c 2 c 3 s 2 s 3 ) c1 (c 2 s 3 s 2 c 3 ) s1 s (c c s s ) s (c s s c ) c 2 3 2 3 1 2 3 2 3 1 1 s 2 c 3 c 2 s 3 s 2 s 3 c 2 c 3 0 0 0 0
c1 ( L4 c 2 c 3 L4 s 2 s 3 L3 c 2 ) s1 ( L4 c 2 c 3 L4 s 2 s 3 L3 c 2 ) L4 s 2 c 3 L4 c 2 s 3 L3 s 2 L1 1
x4 x1 y y 4 1 Ha a 4. koordinátarendszerben adott v 4 vektornak az 1. koordinátarendszerbeli v 1 koordinátáit z4 z1 1 1 kívánjuk meghatározni, akkor a v 1 D14 v 4 kifejezést kell kiértékelni. Itt természetesen D14 D14 (1 , 2 , 3 ) az ízületi koordináták függvénye. Amennyiben a keresett vektor éppen a robot TCP pontja, mely a 4. koordinátarendszer origójában van , vagyis
0 0 helyvektora itt v TCP4 , akkor úgy kapjuk meg a TCP pont világkoordináta-rendszerbeli pontjait, hogy 0 1 a v TCP1 D14 v TCP4 , mátrix-vektor szorzatot értékeljük ki, ez pedig a v TCP4 speciális értéke miatt (szerszámpont a 4. koordinátarendszer origójában van) nem más, mint a D14 mátrix utolsó oszlopa.
v TCP1 D14 v TCP4
c1 ( L4 c 2 c 3 L4 s 2 s 3 L3 c 2 ) s ( L c c L s s L c ) 4 2 3 4 2 3 3 2 1 L4 s 2 c 3 L4 c 2 s 3 L3 s 2 L1 1
Ebből a TCP pont koordinátái:
x(1 , 2 , 3 ) cos 1 ( L4 cos 2 cos 3 L4 sin 2 sin 3 L3 cos 2 ) y (1 , 2 , 3 ) sin 1 ( L4 cos 2 cos 3 L4 sin 2 sin 3 L3 cos 2 ) z (1 , 2 , 3 ) L4 sin 2 cos 3 L4 cos 2 sin 3 L3 sin 2 L1
Megjegyzés: Amennyiben a TCP ponthoz tartozó orientációkat is figyelembe kívánjuk venni, akkor a v TCP4 vektort (4,4)-es egységmátrixszá kell bővíteni, és az eredményül kapott mátrix bal felső (3,3) minora fogja tartalmazni az orientációt kifejező egységvektorokat.
3 IRODALOM [1] Kulcsár Béla: Robottechnika [2] Beni, G. - Hackwood, S.: Recent Advances in Robotics. John Wiley & Sons 1985. [3] Rózsa Pál: Lineáris algebra és alkalmazásai. Műszaki Könyvkiadó Budapest, 1976. -.ROBOT1 S 05 Transzformációk
