Mérési útmutató Robotkar inverz geometriája (és irányítása)

Átírás

1 BME Irányítástechnika és Informatika Tanszék Mérési útmutató Robotkar inverz geometriája (és irányítása) összeállította: Dr. Kiss Bálint Budapest, 2016 Intelligens robotok és jármu vek laboratórium (VIIIMB03)

2 Tartalomjegyzék 1. Bevezetés 3 2. Robotkarok geometriája 4 3. A pozícionáló szabályozási hurok 7 4. A fejlesztői környezet és a minta diagram 9 5. Ellenőrző kérdések, feladatok és mérési jegyzőkönyv Ellenőrző kérdések Mérési feladatok Mérési jegyzőkönyv

3 1. fejezet Bevezetés A mérés célja, hogy a hallgató egy egyszerű mechanikai struktúra segítségével a gyakorlatban is megismerkedjen a robotkarok geometriájának néhány alapfogalmával, implementálja az inverz geometriai feladatot egy egyszerű, síkbeli esetben, szabályozókat hangoljon a kiszámolt csuklóhelyzetek beállításához és ha marad idő, lehetővé tegye a szerkezet gamepad-del történő mozgatását a világkoordináta-rendszerben. A felhasznált struktúra egy kétszabadságfokú robotkar, amelynek két egymással párhuzamos, függőleges irányú tengelyét egy-egy DC motor hajtja. Ez lehetővé teszi a robotkar kijelölt végpontjának pozicionálását egy vízszintes síkban, a robot munkaterén belül. A mérés további célja, hogy a hallgató megismerjen egy gyors irányítási célú prototípus tervező környezetet, amelyben a funkciókat hatékonyan valósíthatja meg alacsony szintű kódolás nélkül, lehetőleg diagram alapú programozási nyelv felhasználásával. A pozicionálást szabályozási hurkok segítségével végezzük. Az irányított MIMO szakasz (két bemenet és két kimenet) a robotkar, a beavatkozó szervek DC motorok, az érzékelők pedig inkrementális adók. Az útmutató először a robotkarok geometriájával kapcsolatos alapfogalmakat ismétli át és definiálja a direkt és inverz geometria fogalmát. Az egyes tengelyek szöghelyzetét beállító szabályozási hurkok részletesebb leírását tartalmazza a 3. fejezet. A mérési feladatok megoldását egy Matlab-Simulink-Quarc 1 alapú fejlesztői környezetben lehet elvégezni, ennek ismertetése a 4. fejezet feladata. Az elvégzendő mérési feladatok listáját, a felkészülést segítő kérdéseket és a mérési jegyzőkönyvvel szemben támasztott igényeket az 5 fejezet mutatja be

4 2. fejezet Robotkarok geometriája A robotkarok egymáshoz képest elmozdulni képes merev testek láncolataként írhatóak le a 2.1. ábra szerint. A robotkart alkotó merev testek, vagy szegmensek egymáshoz képesti elmozdulása valamely tengely körüli elfordulást vagy egy tengely mentén történő eltolást jelent. A relatív elmozdulásokat az ún. csuklók biztosítják, az elmozdulások értékét pedig a csuklókoordináták (jelölésük q i ) mutatják. Az utolsó szegmens térbeli helyzetét és orientációját a relatív elmozdulások összessége, azaz a q vektor elemeinek értékei határozzák meg. A geometriai viszonyok leírásához a szegmensekhez koordinátarendszereket (kereteket) rögzítünk, amelyek velük együtt mozognak. A szegmenseket és a kereteket sorszámmal látjuk el úgy, hogy a K 0 keretet a robot bázisához (álló szegmenséhez) rögzítjük. Az i. keret K 0 kerethez képesti pozíciójának és orientációjának ismerete így az i. szegmens minden pontjának helyét számíthatóvá teszi. A K i keret és a K 0 keret relatív pozícióját és orientációját az ún. homogén transzformációs mátrix adja meg: T 0,i = [ A 0,i p 0,i 0 1 ], (2.1) ahol A 0,i egy 3 3 méretű orthonormált mátrix, melynek oszlopai rendre a K i keret bázisvektorai, felírva K 0 bázisában, p 0,i, pedig K i keret origójának helyvektora, szintén K 0 bázisa szerint felírva. Vegyük észre, ha adott P pont koordinátáit szeretnénk áttranszformálni az egyik keretből a másikba, akkor ehhez éppen a fenti transzformációs mátrix használható a 2.2. ábra alapján, mivel [ r 0 1 ] = [ A 0,i p 0,i 0 1 Nyílt kinematikai lánc esetén könnyen belátható, hogy T i 1,i éppen q i értékétől függ, tehát ha egy robot m csuklóval rendelkezik, akkor T 0,m homogén transzformációs mátrix a csuklókoordinátákból kapott q vektor függvénye. A q T 0,m hozzárendelést a robot direkt geometriai 1 modelljének nevezzük. Az ellenkező irányú, T 0,m q hozzárendelés elnevezése pedig az inverz geometria. A mérés során egy két mozgó szegmensből és két csuklóból álló robotkart tekintünk, amelynek sémáját a 2.3. ábra mutatja (felülnézetből). A koordinátarendszereket célszerűen úgy vettük fel, hogy K 0 keret z tengelye egyben az 1. csukló forgástengelye és K 1 keret z tengelye pedig a 2. csukló forgástengelye 2. Elemi geometriai összefüggések alapján könnyedén leolvasható, hogy C 1 S 1 0 a 1 C 1 C 2 S 2 0 a 2 C 2 T 0,1 = S 1 C 1 0 a 1 S ] [ 1 angol terminológia szerint forward kinematics 2 Ez a konvenció a robotok esetében az ún. Denavit-Hartenberg konvenció. ] r i 1 (2.2) T 1,2 = S 2 C 2 0 a 2 S , (2.3)

5 ábra. Nyílt kinematikai láncú robotkar. P r i n K 0 i k j r 0 p 0,i l K i m 2.2. ábra. Homogén transzformáció két keret között ábra. A mérés során használt robotkar sémája felülnézetből.

6 6 FEJEZET 2. ROBOTKAROK GEOMETRIÁJA ahol az egyszerűsítés érdekében a C 1 = cos(q 1 ) és S 1 = sin(q 1 ) jelölések kerültek bevezetésre. Mivel a homogén transzformáció lineáris művelet, így azok egymás utáni alkalmazásával kapott transzformáció az elemi transzformációk szorzata, tehát C 12 S 12 0 a 2 C 12 + a 1 C 1 T 0,2 = T 0,1 T 1,2 = S 12 C 12 0 a 2 S 12 + a 1 S , (2.4) ahol pedig a C 12 = cos(q 1 + q 2 ) és S 12 = sin(q 1 + q 2 ) jelöléseket vezettük be. Az inverz geometriai feladatból most tekintsük csak az inverz pozícionáló feladatot, amely a kétszabadságfokú robotkar esetében a p 0,2 q hozzárendelés meghatározását jelenti. Mivel a robot síkban mozog, ezért a hozzárendelés megadásához a (2.4) alapján adódó p x = a 2 C 12 + a 1 C 1 (2.5) p y = a 2 S 12 + a 1 S 1 (2.6) trigonometrikus egyenletrendszert kell megoldani. Amennyiben p 0,2 a robot munkaterén belül található, úgy két megoldás számítható, azaz az inverz geometriai feladat megoldása nem egyértelmű. Egy lehetséges megoldás, ha a két egyenletet a p x a 1 C 1 = a 2 C 12 (2.7) p y a 1 S 1 = a 2 S 12 (2.8) alakra hozzuk, négyzetre emeljük, és összeadjuk őket. Ekkor a jobb oldalon a 2 2 marad és némi átrendezés nyomán egy A cos(q 1 ) + B sin(q 1 ) = D (2.9) alakú egyenletre jutunk, amelynek általános esetben két megoldása számítható. Mindkét q 1 értékre ennek nyomán pedig egy q 2 érték számítható. Ehhez a (2.7)-(2.8) egyenletekből kifejezhető q 1 + q 2 szögösszeg koszinusza és szinusza is, tehát egy atan2 függvénnyel, majd a korábban meghatározott q 1 felhasználásával q 2 meghatározható. A fenti számítások lehetővé teszik, hogy a robot második szegmenséhez rögzített keret kívánt pozíciójából visszaszámoljuk az egyes csuklók ennek megfelelő szöghelyzetét, amely alapjelként szolgál a csuklókat mozgató szabályozási hurkok számára.

7 3. fejezet A pozícionáló szabályozási hurok A robot minden egyes tengelyére külön szabályozási hurkot kell terveznünk. A szabályozót a Simulink segítségével valósítjuk meg. A pozícionáló szabályozási hurkok hatásvázlata a 3.1. ábrán látható. A két szabályozási hurok az alapjeleket az inverz pozícionáló feladatot megoldó blokkból kapja. A robot csuklótengelyeire szerelt inkrementális jeladók szolgáltatják a szabályozások számára az ellenőrző jeleket. A hibajel alapján a szabályzó előállítja a kívánt nyomatékot, amelyet a szervok állítanak elő a csuklókban. A nyomatékok hatására a robot elmozdul, a visszacsatolt elfordulás érték módosítja a hibajelet, amely megfelelően hangolt szabályzó esetében kellő gyorsasággal a nullához tart. A felhasznált DC motorok és a teljesítményelektronika a Maxon cég termékei. A teljesítményelektronika esetünkben áramszabályozást is megvalósít (DC motorok esetében az áram arányos a nyomatékkal). Mivel a beépített áramszabályozási hurkok gyorsak és pontosak, a pozíciószabályozás szempontjából úgy tekinthetjük, hogy a szervo egy arányos tagként viselkedik. A Quanser Q8 adatgyűjtő kártya közvetlenül képes fogadni az inkrementális jeladókból érkező impulzussorozatot. A jeladók két csatornásak, azaz két, egymástól fázisban eltolt impulzussorozatot bocsátanak ki a forgás során. Az impulzussorozatok éleinek detektálásával számlálókat lehet meghajtani, a le- és felfutó élek sorrendje pedig meghatározza a forgási irányt. Esetünkben egy teljes körbeforduláshoz 400 számláló impulzus tartozik. Ugyanakkor a motor tengelye és a csukló tengelye közötti 1 : 3 19 áttétel miatt a csuklótengely egy fordulatához impulzus tartozik, ami igen pontos pozicionálást tesz lehetővé. A teljesítményelektronika számára tengelyenként egy-egy -10, +10 voltos tartományban kiadott feszültségérték felel meg az áram alapjelnek. Ezt a feszültségértéket a Quanser Q8 kártya DA átalakítói állítják elő. A megvalósított szabályzó egy PD vagy PID típusú szabályzó kell legyen. A kétszabadságfokú robot Simulink diagramját a Qunaser I/O elemekkel a 3.2. ábra mutatja. Az ábrán látható erősítések értékét (különösen a inkremensek számát radiánba átváltó erősítésekét) a mérés során kell majd meghatározni. q 1,ref inverz szabályzó 1 pozícionáló + - feladat beavatkozó szerv 1 robot q 1 q 2,ref + - szabályzó 2 beavatkozó szerv 2 robot q ábra. A pozíciószabályozási körök. 7

8 8 FEJEZET 3. A POZÍCIONÁLÓ SZABÁLYOZÁSI HUROK 3.2. ábra. A kétszabadságfokú kar I/O elemei Simulink diagramban

9 4. fejezet A fejlesztői környezet és a minta diagram A laboratóriumi gyakorlat során az oktatási és kutatási környezetben elterjedt Matlab-Simulink-Quanser (Quarc) rendszert használjuk a valós idejű implementációra. A Matlab alapverzióján (2012b) kívül használjuk a Simulink Coder-t és a Quarc valós idejű magot is, ezek megfelelő (egymással együttműködő) verziói a méréskor használt személyi számítógépen telepítve vannak. A hardver- és szoftverelemek együttesét gyors (szabályozó) prototípus tervező (rapid control prototyping) környezetnek nevezzük. A Quarc rendszer felhasználói útmutatója a web-en is elérhető. A gyors prototípus tervező eszközök hardver komponensei egyrészt a programok valós idejű futtatására képes célszámítógép, másrészt olyan I/O csatlakozási lehetőséget biztosító modulok, melyek segítségével a célszámítógép könnyedén csatlakoztatható a legkülönbözőbb folyamatok jeleihez (érzékelőkhöz és beavatkozó szervekhez). A összeköttetési lehetőségek sokszor az iparban használt buszrendszerekre történő csatlakozást is képesek megvalósítani (CAN, Profibus, FlexRay, RS232, stb.). A rendszer szoftver komponensei lehetővé teszik a jelátalakítási műveletek magas szintű (tipikusan diagram alapú) leírását és annak alapján az automatikus kódgenerálást. A generált kód valós időben végrehajtható a célszámítógépen. A jelátalakításnak megfelelő diagram szerkesztése és a kódgenerálás nem feltétlenül a célszámítógépen zajlik. A gyors prototípus tervező rendszer azt is lehetővé teszi, hogy a valós időben futó kód egyes paramétereit (tipikusan azokat, amelyek a jelátalakítást végző elemek viselkedését befolyásolják, például egy PID szabályozó T i időállandóját) újabb kódgenerálási lépés nélkül, futás közben változtassa a fejlesztő. Alapkövetelmény továbbá a legkülönbözőbb jelek és események rögzítése későbbi kiértékelés céljából. Mindezen elemek együttesen segítik a prototípus fejlesztési folyamat lerövidítését és így végső soron egy új termék piacra jutásához szükséges idő (time-to-market) kurtítását. A Quanser cég Q8-as kártyájának beépítésével és a Quarc valósidejű mag telepítésével egy hagyományos asztali PC számítógép válhat a fentiekben leírtaknak megfelelő célszámítógéppé. A valós idejű kódot a PC processzora futtaja a valós idejű mag ütemezésének megfelelően, a nem valós idejű Windows operációs rendszer pedig a háttérben fut csak. A Q8 kártya 8 tengely irányításához elegendő I/O-val rendelkezik. A Quarc szolgáltatásainak megfelelő blokkok beépülnek a Simulink blokk könyvtárába és ezeken a szolgáltatásokon keresztül érhető el a valós időben futó kód is. A Quarc lehetővé teszi, hogy a felhasználó a Matlab számára közvetlenül olvasható.mat állományba menthesse a számára érdekes jeleket, amelyeken így további feldolgozást lehet végezni. Ehhez egy egy speciális nyelő elemet kell alkalmazni: tohostpc A Simulink segítségével definiálhatjuk azokat a jelátalakítási műveleteket, amelyek a méréshez szükségesek. A Simulink szempontjából a Quanser Q8 kártya I/O bemenetei jelforrások, I/O kimenetei pedig nyelők, melyek egy külön Simulink elemkönyvtárban kaptak helyet a 4.1. ábra szerint. A Simulink felhasználói felületéről (QUARC menü) közvetlenül elérhetjük a Simulink Coder szolgáltatásait, amelyek a valós idejű kód generálását és futtatását teszik lehetővé. Csak olyan Simulink diagramból generálható kód, ahol az integrálás fix lépésközű. Célszerű az is, hogy a szimulálási idő végét megadó értéket végtelenre állítsuk (inf). Lehetőségünk van a diagramban Matlab függvényt is használni, amelyet a Simulink 9

10 10 FEJEZET 4. A FEJLESZTŐI KÖRNYEZET ÉS A MINTA DIAGRAM 4.1. ábra. A Quanser Q8 kártyához tartozó nyelők és források ábra. Matlab kódú függvény a diagramban.

11 ábra. Matlab függvény kódja a szerkesztő ablakban ábra. Simulink minta diagram az inverz geometriai feladat kódolásához. Simulink/User-Defined Functions könyvtárában találunk a 4.2. ábra szerint. A függvényben a valós idejű végrehajtáshoz tartozó követelményeket nem sértő kódot használhatunk, amelyet egy külön szerkesztővel hozhatunk létre (c.f ábra). A valós idejű végrehajtási követelményeket sértik többek között a dinamikus adatszerkezetek, a változó dimenziójú tömbök/mátrixok, illetve az iterációk használata. A méréshez rendelkezésre áll egy minta diagram (4.4. ábra). A külső bemenet egy gamepad (4.6. ábra), amelynek két bemenetével kívánjuk mozgatni x és y irányban a robotkar második szegmensének végpontját. A bementek érzékenységük és kis értékkészletük miatt nem közvetlen koordinátákat adnak meg, hanem az elmozdulás adott időegységre eső távolságát. Ezekből a bemenetekből egy Move in workspace and limit check függvény állítja elő a kívánt végpont pozíció vektorát és közben ellenőrzi azt is, hogy az a robotkar munkaterén belülre esik. A pozícióból az inverz geometriai feladat megoldását az Inverse geometry függvény állítja elő, amelynek kódolása szintén a mérési feladat része. A minta diagram másik része (4.5. ábra) mutatja a robot mozgatásának módját. A zárt körben a szabályzó a pályakövetés hibája alapján állítja elő a Tau1 és Tau2 jeleket. kódolása szintén a mérési feladat része.

12 12 FEJEZET 4. A FEJLESZTŐI KÖRNYEZET ÉS A MINTA DIAGRAM 4.5. ábra. Simulink minta diagram az robot meghajtásához ábra. Gamepad a mozgás irányításához a világkoordináta-rendszerben.

13 5. fejezet Ellenőrző kérdések, feladatok és mérési jegyzőkönyv 5.1. Ellenőrző kérdések A felsorolt kérdések segítségével ellenőrizheti előismereteit a mérés megkezdése előtt. Ezek a kérdések (kisebb változtatásokkal) szerepelhetnek a mérés elején írt beugróban is. 1. Definiálja egy robot szegmenseihez rögzített keretek közötti homogén transzformációs mátrixot! Mit értünk direkt és inverz geometriai feladat alatt? 2. Vázolja fel a kétszabadságfokú robotkar felépítését és a szegmensekhez rögzített keretek megválasztását! Adja meg a második szegmenshez rögzített keret origójának koordinátáit a K 0 keret bázisában. 3. Adja meg a pozíciószabályozási hurkok hatásvázlatát! 4. Sorolja fel a Quanser Q8 adatgyűjtő kártyára épülő gyors prototípus tervező rendszer harder és szoftver elemeit! Hogyan állítható elő a valós időben futtatható kód? Milyen korlátozások vonatkoznak a kódba befordítható Matlab függvényekre? 5.2. Mérési feladatok A mérés során az alábbi feladatokat kell elvégezni. 1. A szükséges geometriai paraméterek meghatározása nyomán implementálja és tesztelje Simulinkben a kétszabadságfokú robotkar inverz geometriai feladatát! 2. Implementálja a gamepad-tól érkező jelek feldolgozását végző függvényt, amely biztosítja azt is, hogy a kikért pozíció a robotkar munkaterén belül helyezkedjen el. 3. Határozza meg az motorok tengelyein mért inkremesek számát a szegmensek radiánban mért elfordulásába átszámító erősítések értékét! 4. Állítsa össze a pozíciószabályzási hurkokat! 5. Töltse le a Simulink modell alapján előálló kódot a Quarc valós idejű magra és tesztelje, dokumentálja a működést! 13

14 14 FEJEZET 5. ELLENŐRZŐ KÉRDÉSEK, FELADATOK ÉS MÉRÉSI JEGYZŐKÖNYV 5.3. Mérési jegyzőkönyv A mérési jegyzőkönyv címlapja tartalmazza 1. a mérés címét, 2. a mérést végző hallgatók neveit és Neptun kódját, továbbá a mérőcsoport számát, 3. a mérés dátumát. A jegyzőkönyv ismerteti a mérési feladatok megoldását dokumentálja azok implementálást és a mérési eredményeket, továbbá kitér az utóbbiak értékelésére is. A mérési jegyzőkönyvet az IIT tanszék Oktatási Portáljára ( kell feltölteni a tárgy honlapján a "Hallgatói Publikációk" eszköz segítségével, vagy a mérésvezetőnek kell ben elküldeni. A mérésre adott osztályzat a beugrók és a mérőcsoport által készített mérési jegyzőkönyv értékelése alapján születik.