Robotok drekt geometrája. A gyakorlat célja Drekt geometra feladatot megvalósító osztály mplementálása. A megvalósított függvénycsomag tesztelése egy Stanford kar végberendezése pozícójának meghatározásához. 2. Elmélet bevezető 2. Robotok leírása A soros robotok geometra szempontból nytott knematka láncok. A mechanka struktúra nem más, mnt egy n+ elemből álló mechanka rendszer, ahol ()-val jelölk a robot bázsát, melyhez a vlágkoordnáta-rendszer vagy a bázsrendszer van rögzítve, melynek jelölése OXYZ. A több n elem képez a robot mozgatható részét, és különkülön ezek a struktúra knematka elemenek ll. a robot karjanak s nevezendők. Jelölésük - a bázshoz hasonlóan - (), ahol =l n. Mnden elemhez egy saját koordnáta rendszer csatolódk, {}=OXYZ. Ezeket, a robot mozgó vagy mobl vonatkoztatás rendszerenek s nevezzük, és ezek segítségével alkothatjuk meg a robotok geometra, knematka és dnamka modellezését. Az említett karok csuklókkal vannak összekötve, melyekhez mnden esetben hajtás van társítva. A tovább vzsgálatok leegyszerűsítése végett a csuklók mechanka szempontból deáls csuklóknak lesznek tekntve: - nncs alakváltozás a csuklóban a belső érntkezésekből adódóan (merev csukló); - véges és végtelenül ks mozgások esetén s a szabadságfokok száma megegyezk (nncs játék a csuklóban).
. Ábra. Az R és a T típusú csuklók vázlatos ábrázolása Ezeket a csuklókat (R és T) vázlatosan az. ábra szernt tudjuk jelöln. A csukló az alsó végével az {-} elemhez kapcsolódk, lletve teknthető mnt az {}-edk karhoz tartozó csukló, így ehhez a csuklóhoz kapcsolható a kar vonatkoztatás rendszere. Az ábrán fel van tüntetve a q csukló változó s, mely természetesen csuklóhoz és karhoz kötött az ndexen keresztül. A q határozza meg, hogy mekkora az {-} és az {} karok között relatív elmozdulás, és a változó dmenzója lehet szög, lletve hossz mértékegység annak függvényében, hogy R vagy T típusú csuklóról van szó. A fenteket fgyelembe véve kjelenthető, hogy ha egy robotnak n karja van, akkor mndegykhez hozzárendelhető egy csukló változó, melyeket összesítve robotparamétereknek s nevezhetünk. A q változókat egy mátrxba s foglalhatjuk, mely a robotparaméterek vektorjaként smeretes: q [ q, ] T n vagy q ( nx) ( nx) q n ()
Az így meghatározott vektor a robot egy konfgurácóját képvsel, a csuklóváltozókat tömörít. Ha ezek a változók értéke zérus elemek, akkor az () egyenlet sajátos alakuvá válk - a robot kezdet, zérus konfgurácóját tartalmazza, egy csuklóban sncs elmozdulás: T [ q, n] vagy ( nx) ( nx) (2) Az említett konfgurácós vektorral párhuzamosan bevezethető a koordnáta oszlopmátrx, mely egy test helyzetét írja le a derékszögű, 3 dmenzós vonatkoztatás rendszerbe: [ x, ; ] T j j m m n X (3) ( nx) ahol az m a vonatkoztatás térben lehetséges lletve khasznált szabadság fokok számát képvsel. Fontos megjegyezn, hogy habár a térben hat szabadság foka lehet egy szabad testnek, ha a feladatunk úgy kívánja, akkor m=4 lesz. A hat lehetséges mozgásból csak 4- et használunk (pl. 3 haladó és forgó mozgást). Egy robot használata során az elsődleges feladat annak meghatározása, hogy egy bzonyos robot konfgurácóhoz mlyen karaktersztkus pont koordnáták tartoznak, lletve ennek fordítottját s meg kell határozn (főleg vezérlés esetén elengedhetetlen). Egyenletekbe öntve az előbb kjelentést, mondhatjuk, hogy keressük az X vektort (3), ha smert a robotparaméterek vektora, vagy k kell számtanunk a oszlopvektort (), ha adott a mozgatandó test térbel helyzete. Az első esetben a robot mehanka struktúrájának drekt geometra modellezéséről beszélhetünk, míg a fordított rányú számítást nverz vagy fordított geometra modellezésnek nevezzük. Egy robot struktúráját három fő részre oszthatjuk:.a pályageneráló rész, am nem más, mnt a térbel pozícót megadó mechanzmus, am állhat úgy R mnt T csuklókkal összekapcsolt karokból, 2. a test rányítását megadó mechanzmus: mvel ez egy, kettő vagy három tengely körül forgást gényel, a használt csuklók kzárólag csak R típusúak lehetnek, 3. a szerszámbefogó rész, am rögzít a mozgatandó terhet ll. szerszámot. Mvel ez utóbbnak működése nem befolyásolja a robot és a teher mozgását, csak ezek relatv helyzetét rögzít, eltekntünk a vzsgálásától. Egy un. karaktersztkus ponttal helyettesítjük, melyet P-vel fogunk jelöln, és ezt az n.-k karhoz rözítettnek tekntjük.
Hogy az első két részt matematkalag skeresen modellezzük, egy általános geometra mátrxot vezetünk be, mely meghatározza a mechanka struktúrát a zérus konfgurácóban. Ez egy (n+)x6-os mátrx, melynek hat oszlopa és anny sora van ahány csuklója, ll. az utolsó sor a karaktersztkus pontot határozza meg. Az -edk sor első három eleme a csukló középpontjának helyzetvektor tengelynek () ( k ) () ( ) p komponensen, míg az utolsó három tag a csukló rányítását határozza meg három ránykosznusz segítségével. Természetesen ezek a vektorok a vlágkoordnátarendszerben értendőek, ezért a bal-felső ndex nullát mutat, lletve a zéros konfgurácót jellemzk (ezért jelenk meg zárójelben a zérus érték): T T () () M p k n () g ( n) x6 (4) A homogén transzformácós mátrx: Matemetka szempontból egy vektort az un. homogén módon s fel lehet írn. Bevezetjük a mátrxnak fogunk nevezn. Ennek alakja: T (4x4) jelölést, amt homogén transzformácós T (4x4) R (3x3) f (x3) p w (3x) (5) ahol R egy rotácós mátrx, p helyzetvektor lletve f egy perspektvkus transzformácós vektor, melynek vzsgálatunk során állandó értéket adunk: [ ]. A w egy léptéktényező és w= értékkel lesz ellátva. A TT homogén transzformácós mátrx az elcsúsztatást képvsel: T T I3 r (4x4) (6) A TR a rotácót megjelenítő homogén transzformácós mátrx: T R R (4x4) (7)
Tehát a homogén transzformácós mátrx, amely szorzás útján megvalósítja az átmenetet az {} rendszerből az {-} rendszerbe: I 3 r R R r T T T TR (4x4) (4x4) (4x4) (4x4) (8) A következő egyenlet segítségével meg lehet határozn a P karaktersztkus pont helyzetét a vlágkoordnáta rendszerben smerve az {n} rendszerbel helyzetét, lletve z ezt követő ennek fordítottjára ad lehetőséget. n n n n P T T T T P T P 2 n n n 2 P T T T T P T P n n (9) () 2.2. Drekt geometra feladat A geometra modellezés a két vektor között összefüggés megállapítását jelent: X () Ennek érvényessége két megszorítás elmélet teljesítését feltételez:. statkus feltétel: a () egyenlet független az dőtől 2. szlárdság feltétel: mnden knematka elem véges számú alkatrészből áll, és ezek relatív helyzete nem változk. Ezenkívül a csuklók s deáls csuklóknak vannak tekntve. Két féle modellezésről lehet beszéln - drekt geometra modellezés (DGM): X f - nverz vagy fordított geometra modellezés (IGM): f X Denavt-Hartemberg paraméterek használata: Az eddgekben hat paraméter volt használva, hogy leírható legyen egyk kar máskhoz vszonyított helyzete. A Denavt- Hartenberg (DH) paraméterek segítségével ugyanez megvalósítható, vszont ezek száma csak négy. Legyen a 3. ábrán látható két egymást követő kar, és vzsgálva legyen szokásos módon az. kar helyzete az -l. -hez képest. A relatív helyzetet a négy paraméterrel adhatjuk meg:
[a-;α-;d;θ] Mndegyk meghatározása a 3. ábráról olvasható le: - a- a két tengely ( k - és k ) közös merőlegesének hossza, - α- a két tengely ( k - és k ) között szög, - d a k tengely két közös merölegese között szög, a k tengelyre mérve, - θ - a k tengely két közös merölegese között szög. Annak függvényében, hogy az. csukló R vagy T tpusú, a q csukló-változó más paramétert kell befolyásoljon. Ha R csukló szerepel a két kar között, akkor q (melyet ebben az esetben szög mértékegységben mérünk) a θ elemet befolyásolja. 3. Ábra. Denavt-Haztenberg paraméterek Ha tarnszlácós csukló köt össze a két kart, akkor a q dmenzója hosszmérték lesz, és természetesen akkor a d paramétert befolyásolja. Általánosítva az egész robot struktúrára = n : a ; ; d ; q ; n, ha R a ; ; d q ; ; n, ha T Hogy ezeket a paramétereket be tudjuk építen homogén transzformácós mátrxba, a struktúra koordnátarendszeret kellő képen kell kválasztan. Az első lépés a rendszerek középpontjanak (O) a megválasztása kell legyen: (2)
k k, ha a a O C tetszőleges pont ha k k a k, ha a ( ), (3) Ezek után a z, x és y tengelyek meghatározása következk: z x k a a, ha a z z, ha k k y z x (4) (5) (6) A {} és az {n} rendszerre a következő megkötések érvényesek: O O ; z k n On xn k n (7) Ha meghatározzuk a rendszereket a DH paraméterekkel, akkor ezek értéke s kszámíthatók: a-;α- a távolság lletve a szög a z z tengelyek között az x mentén d;θ a távolság lletve a szög az x x tengelyek között a tengely mentén Meghatározva a paraméterek értéket, mndegyket, egyenként egy homogén transzformácós mátrxba lehet foglaln: z a c s TR ( x, ) TT ( x, a ) s c c s s c TR ( z, ) TT ( z, d ) d (8)
Az eredő transzformácós mátrx nem lesz más, mnt ezek szorzata: T T ( x; ) T ( x; a ) T ( z; ) T ( z; d ) (9) R T R T Az (9) egyenlet kdolgozása után skeresen átalakítható a robot bármely mozgó rendszeréhez tartozó pont helyzete a vlágkoordnáta rendszerbe. 3. A mérés menete 3. Az Armadllo könyvtár függvényenek megsmerése Az Armadllo C++ alatt megírt függvény csomag, amely segítségével C++ környezetben nagyon könnyen lehet mátrxműveleteket végezn. A könyvtár ngyenesen letölthető és könnyen lleszthető C++ alatt fejlesztett tervekhez. A könyvtár bevezet mátrx és vektor típusokat, ezekekkel operátorok használatával lehet műveleteket elvégezn. Néhány mátrx típusok: - Általános mátrx: Mat<t> m(n, m) általános mátrx deklarálása, n sorok, m oszlopok száma, t a típus lehet nt, float double, stb. - Row<t> r(n) sorvektor, n az elemek száma - Col<t> v(n) oszlopvektor, n az elemek száma - Eye<t>(n,m) egységmátrx, nem felülírható típus Néhány fontosabb művelet: - értékátadás: m=m; m=i - mátrx elemenek megadása: m(,j)=2,3; r=m(2,3) - mátrxszorzás: m=m*m2; v=m*v; - elemek számának kelkérdezése: nt n=sze(a); Szorzásnál, értékátadásnál kell vgyázn, hogy a mátrxok soranak és oszlopanak száma a műveletnek megfelelően egyezzenek meg. 3.2 A drekt geometrát megvalósító osztály mplementálása Az osztály a RobotGeometryProj tervben található RobotGeometry.cpp állományban kell megvalósítan a Newmat osztály segítségével. Az osztály már hozzá van csatolva a tervhez. A megvalósítandó függvények: nt RobotGeometry::TranslatonX(double d, Mat<double> &T) Eltolás x mentén. A vsszatérítendő T mátrx egy egységmátrx, amelynek módosítjuk az,4 elemét d értékkel nt RobotGeometry::TranslatonY(double d, Mat<double> &T) Eltolás y mentén. A vsszatérítendő T mátrx egy egységmátrx, amelynek módosítjuk a 2,4 elemét d értékkel
nt RobotGeometry::TranslatonZ(double d, Mat<double> &T) Eltolás z mentén. A vsszatérítendő T mátrx egy egységmátrx, amelynek módosítjuk a 3,4 elemét d értékkel nt RobotGeometry::RotatonX(double a, Mat<double> &T) Elforgatás x körül. A vsszatérítendő T mátrx egységmátrx, amelynek a bal felső 3X3 blokkját az alább mátrxxal módosítjuk: cosa sn a Rot ( z, a) sn a cosa nt RobotGeometry::RotatonY(double a, Mat<double> &T) Elforgatás y körül. A vsszatérítendő T mátrx egységmátrx, amelynek a bal felső 3X3 blokkját az alább mátrxxal módosítjuk: cosa sn a Rot( z, a) sn a cosa nt RobotGeometry::RotatonZ(double a, Mat<double> &T) Elforgatás z körül. A vsszatérítendő T mátrx egységmátrx, amelynek a bal felső 3X3 blokkját az alább mátrxxal módosítjuk: Rot( z, a) cosa sn a sn a cosa nt RobotGeometry::DHMatrx(double theta, double d, double a, double alpha, Mat<double> &T) Denavtt Hartenberg mátrx egy csuklóra: A függvényben meghívjuk a RotatonZ-t a thetara, a TranslatonZ-t a d-re, a TranslatonX-et az a-ra, a RotatonX-et az alpha-ra, amjd a kapott 4 mátrxot összeszorozzuk és vsszatérítjük a T-ben. nt RobotGeometry::DrectGeometry(nt DOF, Col<double> Theta, Col<double> D, Col<double> A, Col<double> Alpha, Mat<double> &T) A drekt geometra feladat megoldása. A DOF (Degree Of Freedom) a robot szabadságfoka. A tovább függvény argumentumok oszlopvektorok, amelyek a teljes robotra tartalmazzák a theta, d, a, alpha paramétereket. A dmenzójuk meg kell egyezzen a robot szabadságfokával. A függvényben DOF-szor meghívjuk a DHMatrx függvényt az oszlopvektorok megfelelő elemere, majd a kapott mátrxokat összeszorozzuk és vsszatérítjük a T mátrxban.
3.3 A Stanford kar drekt geometrája Legyen a 4 Ábrán látható a Stanford kar és a csuklókhoz hozzárendelt paraméterek. A kar Denavt Hartenberg paramétere az. Táblázatban találhatóak. 4. Ábra: Stanford kar. Táblázat: A Stanford kar Denavt Hartenberg paramétere d a d= /2 2 /2 d2= -/2 3 /2 d3= A RobotGeometryProj.cpp állomány man függvényében hozzunk létre egy RobotGeometry obektumot, majd töltsünk fel 4 sorvektort az Táblázat szernt. A feltöltött vektorokkal hívjuk meg a DrectGeometry függvényt. Írassuk k az eredményül kapott mátrxot, a 4. Ábra alapján ellenőrzzük az eredmény helyességét. 4. Kérdések és feladatok. Határozzuk meg analtkusan a Stanford kar végberendezésének x, y, z koordnátáját, majd teszteljük az eredményeket a programban kapott értékekkel. 2. Oldjuk meg analtkusan a Stanford kar drekt geometra feladatát a Denavt Hartenberg táblázat alapján, majd teszteljük a megoldást a program kapott értékekkel. 3. Módosítsuk úgy a programot, hogy a mátrxok szorzását saját függvénnyel oldjuk meg.