BME Irányítástechnika és Informatika Tanszék – www.iit.bme.hu
Mérési útmutató Robotkar inverz geometriája (és irányítása)
összeállította:
Dr. Kiss Bálint Budapest, 2016
[email protected]
Intelligens robotok és járm˝uvek laboratórium (VIIIMB03)
Tartalomjegyzék 1. Bevezetés
3
2. Robotkarok geometriája
4
3. A pozícionáló szabályozási hurok
7
4. A fejleszt˝oi környezet és a minta diagram
9
5. Ellen˝orz˝o kérdések, feladatok és mérési jegyz˝okönyv 5.1. Ellen˝orz˝o kérdések . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2. Mérési feladatok . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3. Mérési jegyz˝okönyv . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2
13 13 13 14
1. fejezet
Bevezetés A mérés célja, hogy a hallgató egy egyszer˝u 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˝u, síkbeli esetben, szabályozókat hangoljon a kiszámolt csuklóhelyzetek beállításához és ha marad id˝o, lehet˝ové tegye a szerkezet gamepad-del történ˝o 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˝oleges irányú tengelyét egy-egy DC motor hajtja. Ez lehet˝ové 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˝o környezetet, amelyben a funkciókat hatékonyan valósíthatja meg alacsony szint˝u kódolás nélkül, lehet˝oleg 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˝ok pedig inkrementális adók. Az útmutató el˝oszö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-Quarc1 alapú fejleszt˝oi környezetben lehet elvégezni, ennek ismertetése a 4. fejezet feladata. Az elvégzend˝o mérési feladatok listáját, a felkészülést segít˝o kérdéseket és a mérési jegyz˝okönyvvel szemben támasztott igényeket az 5 fejezet mutatja be.
1
http://www.quanser.com/products/QUARC
3
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˝o 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 qi ) 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 K0 keretet a robot bázisához (álló szegmenséhez) rögzítjük. Az i. keret K0 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 Ki keret és a K0 keret relatív pozícióját és orientációját az ún. homogén transzformációs mátrix adja meg: " # A0,i p0,i , (2.1) T0,i = 0 1 ahol A0,i egy 3 × 3 méret˝u orthonormált mátrix, melynek oszlopai rendre a Ki keret bázisvektorai, felírva K0 bázisában, p0,i , pedig Ki keret origójának helyvektora, szintén K0 bázisa szerint felírva. Vegyük észre, ha adott P pont koordinátáit szeretnénk áttranszformálni az egyik keretb˝ol a másikba, akkor ehhez éppen a fenti transzformációs mátrix használható a 2.2. ábra alapján, mivel " # " #" # r0 A0,i p0,i ri = (2.2) 1 0 1 1 Nyílt kinematikai lánc esetén könnyen belátható, hogy Ti−1,i éppen qi értékét˝ol függ, tehát ha egy robot m csuklóval rendelkezik, akkor T0,m homogén transzformációs mátrix a csuklókoordinátákból kapott q vektor függvénye. A q 7→ T0,m hozzárendelést a robot direkt geometriai1 modelljének nevezzük. Az ellenkez˝o irányú, T0,m 7→ q hozzárendelés elnevezése pedig az inverz geometria. A mérés során egy két mozgó szegmensb˝ol és két csuklóból álló robotkart tekintünk, amelynek sémáját a 2.3. ábra mutatja (felülnézetb˝ol). A koordinátarendszereket célszer˝uen úgy vettük fel, hogy K0 keret z tengelye egyben az 1. csukló forgástengelye és K1 keret z tengelye pedig a 2. csukló forgástengelye2 . Elemi geometriai összefüggések alapján könnyedén leolvasható, hogy C2 −S2 0 a2 C2 C1 −S1 0 a1 C1 S1 C1 0 a1 S1 S2 C2 0 a2 S2 , T1,2 = (2.3) T0,1 = 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1 1 2
angol terminológia szerint forward kinematics Ez a konvenció a robotok esetében az ún. Denavit-Hartenberg konvenció.
4
5
2.1. ábra. Nyílt kinematikai láncú robotkar.
P ri n r0 l Ki p0,i
k
K0
i
m
j
2.2. ábra. Homogén transzformáció két keret között.
2.3. ábra. A mérés során használt robotkar sémája felülnézetb˝ol.
6
FEJEZET 2. ROBOTKAROK GEOMETRIÁJA
ahol az egyszer˝usítés érdekében a C1 = cos(q1 ) és S1 = sin(q1 ) jelölések kerültek bevezetésre. Mivel a homogén transzformáció lineáris m˝uvelet, így azok egymás utáni alkalmazásával kapott transzformáció az elemi transzformációk szorzata, tehát C12 −S12 0 a2 C12 + a1 C1 S12 C12 0 a2 S12 + a1 S1 , T0,2 = T0,1 T1,2 = (2.4) 0 0 1 0 0 0 0 1 ahol pedig a C12 = cos(q1 + q2 ) és S12 = sin(q1 + q2 ) 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 p0,2 7→ 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ó px = a2 C12 + a1 C1
(2.5)
py = a2 S12 + a1 S1
(2.6)
trigonometrikus egyenletrendszert kell megoldani. Amennyiben p0,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˝u. Egy lehetséges megoldás, ha a két egyenletet a px − a1 C1 = a2 C12
(2.7)
py − a1 S1 = a2 S12
(2.8)
alakra hozzuk, négyzetre emeljük, és összeadjuk o˝ ket. Ekkor a jobb oldalon a22 marad és némi átrendezés nyomán egy A cos(q1 ) + B sin(q1 ) = D (2.9) alakú egyenletre jutunk, amelynek általános esetben két megoldása számítható. Mindkét q1 értékre ennek nyomán pedig egy q2 érték számítható. Ehhez a (2.7)-(2.8) egyenletekb˝ol kifejezhet˝o q1 + q2 szögösszeg koszinusza és szinusza is, tehát egy atan2 függvénnyel, majd a korábban meghatározott q1 felhasználásával q2 meghatározható. A fenti számítások lehet˝ové 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˝o szöghelyzetét, amely alapjelként szolgál a csuklókat mozgató szabályozási hurkok számára.
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˝orz˝o jeleket. A hibajel alapján a szabályzó el˝oállítja a kívánt nyomatékot, amelyet a szervok állítanak el˝o 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˝oen hangolt szabályzó esetében kell˝o 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˝ujt˝o kártya közvetlenül képes fogadni az inkrementális jeladókból érkez˝o 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 19 · 3 · 400 impulzus tartozik, ami igen pontos pozicionálást tesz lehet˝ové. 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˝o. 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˝osítések értékét (különösen a inkremensek számát radiánba átváltó er˝osítésekét) a mérés során kell majd meghatározni.
inverz pozícionáló feladat
q1,ref + -
szabályzó 1
q1 beavatkozó szerv 1
q2,ref + -
robot
szabályzó 2
q2 beavatkozó szerv 2
3.1. ábra. A pozíciószabályozási körök. 7
robot
8
FEJEZET 3. A POZÍCIONÁLÓ SZABÁLYOZÁSI HUROK
3.2. ábra. A kétszabadságfokú kar I/O elemei Simulink diagramban
4. fejezet
A fejleszt˝oi 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˝u implementációra. A Matlab alapverzióján (2012b) kívül használjuk a Simulink Coder-t és a Quarc valós idej˝u magot is, ezek megfelel˝o (egymással együttm˝uköd˝o) 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˝o (rapid control prototyping) környezetnek nevezzük. A Quarc rendszer felhasználói útmutatója a web-en is elérhet˝o. A gyors prototípus tervez˝o eszközök hardver komponensei egyrészt a programok valós idej˝u futtatására képes célszámítógép, másrészt olyan I/O csatlakozási lehet˝osé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˝obb folyamatok jeleihez (érzékel˝okhöz és beavatkozó szervekhez). A összeköttetési lehet˝oségek sokszor az iparban használt buszrendszerekre történ˝o csatlakozást is képesek megvalósítani (CAN, Profibus, FlexRay, RS232, stb.). A rendszer szoftver komponensei lehet˝ové teszik a jelátalakítási m˝uveletek magas szint˝u (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˝oben végrehajtható a célszámítógépen. A jelátalakításnak megfelel˝o 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˝o rendszer azt is lehet˝ové teszi, hogy a valós id˝oben futó kód egyes paramétereit (tipikusan azokat, amelyek a jelátalakítást végz˝o elemek viselkedését befolyásolják, például egy PID szabályozó Ti id˝oállandóját) újabb kódgenerálási lépés nélkül, futás közben változtassa a fejleszt˝o. Alapkövetelmény továbbá a legkülönböz˝obb jelek és események rögzítése kés˝obbi 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˝o soron egy új termék piacra jutásához szükséges id˝o (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˝u mag telepítésével egy hagyományos asztali PC számítógép válhat a fentiekben leírtaknak megfelel˝o célszámítógéppé. A valós idej˝u kódot a PC processzora futtaja a valós idej˝u mag ütemezésének megfelel˝oen, a nem valós idej˝u Windows operációs rendszer pedig a háttérben fut csak. A Q8 kártya 8 tengely irányításához elegend˝o I/O-val rendelkezik. A Quarc szolgáltatásainak megfelel˝o blokkok beépülnek a Simulink blokk könyvtárába és ezeken a szolgáltatásokon keresztül érhet˝o el a valós id˝oben futó kód is. A Quarc lehet˝ové 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˝o elemet kell alkalmazni: toHostPC A Simulink segítségével definiálhatjuk azokat a jelátalakítási m˝uveleteket, 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˝ok, melyek egy külön Simulink elemkönyvtárban kaptak helyet a 4.1. ábra szerint. A Simulink felhasználói felületér˝ol (QUARC menü) közvetlenül elérhetjük a Simulink Coder szolgáltatásait, amelyek a valós idej˝u kód generálását és futtatását teszik lehet˝ové. Csak olyan Simulink diagramból generálható kód, ahol az integrálás fix lépésköz˝u. Célszer˝u az is, hogy a szimulálási id˝o végét megadó értéket végtelenre állítsuk (inf). Lehet˝oségünk van a diagramban Matlab függvényt is használni, amelyet a Simulink 9
10
˝ KÖRNYEZET ÉS A MINTA DIAGRAM FEJEZET 4. A FEJLESZTOI
4.1. ábra. A Quanser Q8 kártyához tartozó nyel˝ok és források.
4.2. ábra. Matlab kódú függvény a diagramban.
11
4.3. ábra. Matlab függvény kódja a szerkeszt˝o ablakban.
4.4. á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˝u végrehajtáshoz tartozó követelményeket nem sért˝o kódot használhatunk, amelyet egy külön szerkeszt˝ovel hozhatunk létre (c.f. 4.3. ábra). A valós idej˝u 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˝o 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˝oegységre es˝o távolságát. Ezekb˝ol a bemenetekb˝ol egy Move in workspace and limit check függvény állítja el˝o a kívánt végpont pozíció vektorát és közben ellen˝orzi 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˝o, 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˝o a Tau1 és Tau2 jeleket. kódolása szintén a mérési feladat része.
12
˝ KÖRNYEZET ÉS A MINTA DIAGRAM FEJEZET 4. A FEJLESZTOI
4.5. ábra. Simulink minta diagram az robot meghajtásához.
4.6. ábra. Gamepad a mozgás irányításához a világkoordináta-rendszerben.
5. fejezet
Ellen˝orz˝o kérdések, feladatok és mérési jegyz˝okönyv 5.1.
Ellen˝orz˝o kérdések
A felsorolt kérdések segítségével ellen˝orizheti el˝oismereteit a mérés megkezdése el˝ott. 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 K0 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˝ujt˝o kártyára épül˝o gyors prototípus tervez˝o rendszer harder és szoftver elemeit! Hogyan állítható el˝o a valós id˝oben 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˝o jelek feldolgozását végz˝o 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˝osí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˝oálló kódot a Quarc valós idej˝u magra és tesztelje, dokumentálja a m˝uködést! 13
˝ O˝ KÉRDÉSEK, FELADATOK ÉS MÉRÉSI JEGYZOKÖNYV ˝ FEJEZET 5. ELLENORZ
14
5.3.
Mérési jegyz˝okönyv
A mérési jegyz˝okönyv címlapja tartalmazza 1. a mérés címét, 2. a mérést végz˝o hallgatók neveit és Neptun kódját, továbbá a mér˝ocsoport számát, 3. a mérés dátumát. A jegyz˝okö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˝okönyvet az IIT tanszék Oktatási Portáljára (http://edu.iit.bme.hu) 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˝onek kell emailben elküldeni. A mérésre adott osztályzat a beugrók és a mér˝ocsoport által készített mérési jegyz˝okönyv értékelése alapján születik.
