BUDAPESTI MŰSZAKI EGYETEM VILLAMOSMÉRNÖKI ÉS INFORMATIKAI KAR AUTOMATIZÁLÁSI ÉS ALKALMAZOTT INFORMATIKAI TANSZÉK
ROBOTVEZÉRLŐ SZOFTVERRENDSZERÉNEK FEJLESZTÉSE Diplomaterv
Készítette:
Prohászka Zoltán Konzulens:
Bézi István adjunktus
Budapest, 2002.
1
Tartalmi összefoglaló Tanszékünkön (az IIT tanszékkel közösen) évek óta folyik egy fejlesztés, melynek keretében a meglévő NOKIA PUMA-560-as manipulátorhoz egy teljesen dokumentált, nyitott rendszerű vezérlő HW-SW együttes jött létre. A munka majdnem befejezett, az elért eredmények lehetővé teszik a robotkar összetettebb, fejletebb irányításainak kipróbálását, fejlesztését. Az én feladatom az eddig kialakult rendszer analízise, hibáinak felderítése, és megoldása volt. Én már egy többé-kevésbé jól működő rendszert kaptam kézhez. Ennek a rendszernek már minden szükséges alkotórésze megvolt, így elődeimtől eltérően nem teljesen új programrészeket kellett implementálnom, hanem a meglévőeket kellet javítanom, módosítanom. A rendszer együttműködésének megismerése után több tucat javítandó részt találtam. Ezek közül a világkoordinátás mozgatások javítását tartottam a legsürgősebb komoly feladatnak. Ez a funkció elméletileg meg volt valósítva, a gyakorlatban azonban szinte minden mozgatás durva, hibás robotmozgatáshoz vezetett. A feladatot melyrehatoló átalakítással lehetett csak orvosolni. Mielött ehez nekifoggtam volna, a jobb áttekinthetőség érdekében még megoldottam pár egyszerűbben javíthó problémát. A dolgozat tartalmazza az elkészült rendszer ismertetését (2. fejezet), a megoldott problémák és megoldásaik részletes leírását (3. fejezet), illetve az idő hiányában meg nem oldott hibák és hasznos fejlesztési feladatok ismertetését (4. fejezet). Remélem a diplomaterv segítségére lesz a rendszert továbbfejlesztő hallgatóknak.
2
Abstract This thesis describes my contribution to the development of a fully documented robot controll system, which is a common project of the division of Automation & Applied Informatics (AUT.) and the division of Controll & Informatics (IIT). Both division operates under the Department of Informatics & Electric Engineering (Villamosmérnöki és Informatikai Kar, V.I.K.) of the Technical University of Budapest (Budapesti Műszaki Egyetem, BME). The aim of this multi-year development is to achive a basic robot controll HW-SW system, which can drive our NOKIA-PUMA-560 manipulators, and makes us able to quickly implement advanced robot controll algorithms and complex robot operations. The project is almost finished. My work was to analyse the system, detect errors, and solve as many of them as I can. The system harited to me was more-or-less well-functioning. All parts of the system existed already. Instead of the prior graduates, I have had not to implement new parts of the software, but to correct and modify existing ones. I found several errors and mistakes while worked on the understanding of the cooperation of the system. I thought the fixing of world-coordinate movements to be the most urgent problem between the serious ones. This function was implemented theoretically, but almost every usage of it lead to false movements and to the override of the manipulator. This error was correctable only by deep re-organisation. For the better overview, I solved several minor problems before starting to fix this major error. Chapter #2 describes the operation of the final software-system. The corrected problems can be found in chapter #3, with the detailed explanation of the solutions. Chapter #4 contains detected, but not corrected problems, and examples of features to implement in the future. I hope this book will help the work of students continuing the project.
3
Tartalomjegyzék 1
.fejezet: Bevezetés ................................................................................................... 8 1.1.1 Bevezetés a projektbe................................................................................ 8 1.1.2 Diplomamunkám stílusáról......................................................................... 8 1.2 A robotvezérlő rendszer elemei....................................................................... 10 1.2.1 A hardver architektúra.............................................................................. 10 1.2.2 A robot ..................................................................................................... 11 1.2.2.1 Robot vs. félkarú rabló ...................................................................... 13 1.2.3 A BME robotvezérlő ................................................................................. 14 1.2.3.1 Az ARC kártyák................................................................................. 14 1.2.3.2 Kicsit a megvalósított, illetve jövőbeni kommunikációról................... 15 1.2.3.3 QNX,neutrino: ................................................................................... 15 1.2.3.4 A robotvezérlő processzorainak számításteljesítményéről ............... 15 1.2.4 A szoftverrendszer fejlesztéséről ............................................................. 17 1.2.4.1 A szoftverrendszer fejlesztésének története ..................................... 17 1.2.4.2 Új fejlesztők figyelmébe .................................................................... 18 1.2.5 Az egyes platformokra való fordításról ..................................................... 19 1.2.6 A szoftverrendszer megismeréséhez ajánlott linkek ................................ 20 2 .fejezet: A jelenlegi (utánam maradó) SW rendszer............................................... 21 2.1 Bevezetés ....................................................................................................... 21 2.1.1 Robot paraméterek 1 ............................................................................... 21 2.2 A TMS szerepe ............................................................................................... 21 2.3 Az IEX feladata ............................................................................................... 22 2.3.1 A 4-féle kommunikáció............................................................................. 22 2.4 A Host programrendszere ............................................................................... 25 2.4.1 A kommunikációhoz kapcsolódó részek: arcrun, arcproc, robot.c ........... 26 2.4.2 A Host programjainak forrásállományai, közös modulok ......................... 26 2.4.3 A Host QNX-ütemezési beállításai ........................................................... 28 2.4.4 A szabályozásban kulcsfontosságú dynjac processz............................... 28 2.4.5 A robotot mozgató processzek: pathgen és pendant ............................... 28 2.4.6 Pendant-arps kommunikáció.................................................................... 29 2.4.7 Pathgen-pendant kizárás a szemaforon keresztül ................................... 30 2.4.8 A pathgen................................................................................................. 30 2.4.9 Pathgen arps kommunikáció.................................................................... 31 2.4.10 A rendszert elindító arps processz ........................................................... 31 2.4.11 Az arps működés közben, kommunikációja a console és messenger folyamatokkal ......................................................................................................... 32 2.4.12 A console ................................................................................................. 33 2.4.13 A messenger............................................................................................ 33 2.4.14 A kalibráció, a pendant működésének függése a kalibrációs állapottól ... 33 3 .fejezet: A robotvezérlő szoftverrendszeren elért fejlesztési eredményeim ............ 35 3.1 A robot laborban a diplomatervezés megkezdéséig végzett munkám összefoglalása ........................................................................................................... 35 4
3.2 A diplomatervező félév elejéig felmerült, addigra megoldott problémák ......... 36 3.2.1 Az egymásrahatás, a 2000 tavaszán levő állapotokról, a csukló egymásrahatás miatti lavináról, D-H alak, zeroready, szinguláris helyzet, virtuális csuklók, valódi motorszögek, az SI mértékegységek ............................................. 36 3.2.2 A robotparaméterek körüli huzavona ....................................................... 40 3.2.3 ROBOP .................................................................................................... 41 3.2.4 A Relécsere, féklazító vezeték átforrasztása ........................................... 42 3.2.5 A sebességszabályozó telítése, különböző paraméterek, a paraméterek TMS-re történő kötelező feltöltése, ennek hiányosságai........................................ 42 3.2.6 A Host OS-ei, installálás .......................................................................... 43 3.2.7 A 2ndary IDE controller miatti probléma .................................................. 44 3.3 A Diplomatervezési időszakra tervezett, illetve ekkor megoldott feladatok ..... 47 3.3.1 A Platformok, Visual C ............................................................................. 48 3.3.2 Pathgen egyszerűsítése, annak előnyei................................................... 48 3.3.3 Az osztott memóriák új tagjai, szerepük................................................... 49 3.3.4 A log fájl készítést segítő debug.c modul, használata és a robot_nomove define 50 3.3.5 A pendant sebességlimiteinek összehangolása, pendant mozgatások anticiklusból ciklusba rendezése ............................................................................ 50 3.3.6 Puma.c és geometr.c egyesítése............................................................. 51 3.3.7 MYMATH.C módosítása .......................................................................... 51 3.3.8 A Rodrigez képletről................................................................................. 51 3.3.8.1 A probléma ....................................................................................... 52 3.3.8.2 A megoldás ....................................................................................... 54 3.3.9 SZFÉRA Tesztek: konfiguráció-kezelése, jacobi-nullázódás-kezelés, interrupt: break-nobreak ......................................................................................... 58 3.3.10 Az inverz geometria ................................................................................. 60 3.3.10.1 Bevezetés ......................................................................................... 60 3.3.10.2 Több konfiguráció léte, említés a különbözőségükről ....................... 60 A konfigurációváltás problémája, SZFÉRA és a régi rendszer összehasonlítása62 3.3.10.3 Követelmények az invgo felé a különbözőfajta hívások esetén ........ 65 3.3.10.4 Konfigurációkódolás előnyei, követelményei (getconfig()) ................ 65 3.3.10.5 Nearest eset szükségessége j456 esetében és súlyozott nearest előnyei j1-j2 esetben........................................................................................... 65 3.3.10.6 Az invgeo rutin, a probléma geometriai megoldása .......................... 67 3.3.10.7 Konfiguráció-bitmezőket maszkoló hívások: pathgen, pendant ........ 72 3.3.10.8 Arps konfigurációállítást figyelembevevő hívás: csak GO-hoz.......... 73 3.3.10.9 Egy előforduló, de nem kijavított hiba ............................................... 73 3.3.11 A sebességtúllépés .................................................................................. 73 3.3.12 A kommentezés eddigi szűkösségéről, a pótolt megjegyzésekről ........... 74 3.3.13 PC-LAB-DOUT- regiszter több példányban létezett (pendant és arps globális változóját SHM-be kellett rakni)................................................................. 75 3.4 Az RT feladatok által támasztott követelmények vs. objektum orientáltság .... 75 4 .fejezet: A fejlesztés közben felmerülő hibák, hiányosságok, valamint továbbfejlesztési feladatok, és megoldásuknak lehetséges módjai............................... 77 4.1 Bevezetés ....................................................................................................... 77 4.2 A rendszerben levő hibák, hiányosságok ........................................................ 78 5
4.2.1 4.2.2 4.2.3 4.2.4 4.2.5 4.2.6
A QNX alkalmazásai rendesen telepítendőek (ismétlés) ......................... 78 A dinamikus modell Coriolis tagja javítandó (ismétlés) ............................ 78 Az init hiányossága, a robot megtörése ................................................... 78 A Cntrl-break valamint pendant-emergency-stop után lefagy a program . 79 A deinit hiányossága, kétszeri indítás szükségessége............................. 79 A TMS szabályozásában a limittúllépések figyelését vissza kell kapcsolni! 79 4.2.7 A nagy időzítésű mozgató-üzenetek a rendszer teljes blokkolásához vezetnek. ................................................................................................................ 79 4.2.8 TMS-re a deginc-átváltási konstansokat is fel kell tölteni (ismétlés) ........ 80 4.2.9 Az arps processzben sok megengedhetetlen utasítás nincs tiltva kalibrálatlan állapotban........................................................................................... 80 4.2.10 Pendanttal való mozgatás sebességugrásos........................................... 81 4.2.11 Interrupt megoldása. Utalás a szféra teszt eredményre (Ismétlés) .......... 81 4.2.12 Az arps.c UseConsole() hívásában a where parancs általi kiíratás hibás 82 4.2.13 A makefile hibás....................................................................................... 82 4.2.14 A pályatervezés hibái ............................................................................... 83 4.3 A továbbfejlesztés lehetséges feladatai .......................................................... 84 4.3.1 A pályatervezés gyorsítása (a robot és nem az algoritmus sebességét illetőleg) 84 4.3.2 A szoftverrendszerünk architekturális és szoftvertechnológiai szemszögből 88 4.3.3 Héjszerkezet architektura, SW rendszer újjátervezése............................ 88 4.3.4 HOST-CONTROL .................................................................................... 89 4.3.5 C-for robot függvények............................................................................ 90 4.3.6 \arc.ini olvasása (ismétlés)....................................................................... 92 4.3.7 Grafikus felület. Időzítési viszonyok analízise, ha a photon is fut ............ 92 4.3.8 Robotszimuláció, egy makefálj kellene és egy main include minden processz számára .................................................................................................. 92 4.3.9 Analízis bármely processzor, processz kiesésére.................................... 93 4.3.10 Analízis bármely HW elem (vezeték) meghibásodására, jelzésük megoldásai ............................................................................................................. 93 4.3.11 A kalibrációval kapcsolatos hardver meghibásodás elleni védekezési módszerek.............................................................................................................. 93 4.3.12 Kalibrációs görbe felvétele az arps-en belül............................................. 93 4.3.12.1 Ready-helyzet felvétele az orosz manual-ban leírt folyamat alapján (move joints-al) ................................................................................................... 93 4.3.12.2 Kalibrációs fájl felvétele és mentése................................................. 94 4.3.13 Kalibrációs potméter jelenek eltérése a tacho-által számított jelhez képest, ennek folyamatos figyelése, warning üzenettel, ha túl nagy az eltérés az eredetitől, inkremens impulzusvesztés monitorozása ............................................................. 94 4.3.14 Robotparaméterek identifikálása.............................................................. 94 4.3.14.1 C-for robottal könnyebben implementálható a geometriai modell visszamérése ...................................................................................................... 94 4.3.14.2 Áram (2048)-nyomaték átváltás, szimulált rugó (csak P szabályozó)95 4.3.14.3 Súrlódás lemérése, modellezése. (a*v^2+b*v+c) alakkal.................. 95
6
5 6 7
4.3.14.4 Dinamikus modell mérése (csak p-szabályozó, gyenge p-taggal, periodusidő mérése (csuklóváltozók loggolása)) ................................................ 96 . Összefoglalás....................................................................................................... 97 . Függelék............................................................................................................... 98 . Irodalomjegyzék ................................................................................................. 101
7
1 .fejezet: Bevezetés 1.1.1 Bevezetés a projektbe A BME-n oktatási célokra több tanszék és kar használ PUMA 560 robotokat, melyeket a Tungsram rendszerváltáskor leépített robotikai leányvállalatától vásárolt az egyetem. Ezek az eredeti vezérlőszekrényekkel (SZFÉRA-36) érkeztek, melyek orosz replikái egy még korábbi amerikai vezérlőnek. A Villamosmérnöki és Informatikai kar AUT és IIT (régebben FSZ) tanszékén majdnem egy évtizede folyik a fejlesztés (OTKA 756), hogy egy teljesen saját fejlesztésű hardver és szoftver együttessel lecseréljük a régi vezérlőszekrényt. A kutatás-fejlesztés célja, hogy egy bármilyen robotikai fejlesztés számára nyitott, jól dokumentált rendszert kapjunk. A HW rendszer elkészültéig csak egy előzetes SW fejlesztésre volt lehetőség. Amikor 1996 tavaszára elkészült a vezérlő, akkor lett lehetőség a szoftverek fejlesztésének felgyorsítására, kipróbálása a HW-en, és a roboton. A projektbe való bekapcsolódásomkor az AUT tanszéken már működött a rendszer, de még voltak benne hibák, olyanok is, melyek miatt nem lehetett a robotot biztonságosan üzemeltetni. Az időközben IIT névre keresztelt FSZ tanszéken az egyik inkrementális adó cseréje volt folyamatban, így az ottani, kissé eltérő rendszert nem állt módomban működés közben megismerni, tesztelni. A szoftverrendszer (mindkét változata) kizárólag hallgatók munkája. A projekt volumene igen nagy. Vállalati körülmények között történő megvalósítása esetén is jelentős energiákat kellene a különböző fejlesztők közötti kommunikációra fordítani. Az egyetemünkön megvalósult elhúzódó ütemű fejlesztés esetében a kommunikáció csak egymást szorosan követő évfolyamok között lehetséges, de egy év állás megszakítja a verbális tapasztalatátadás folyamatát. Marad tehát az írásban rögzített, egyirányú kommunikáció. Az érdemi munka megkezdése előtt a bekapcsolódó hallgatónak jelentős időt kell szentelnie arra, hogy alaposan megismerje az eddig elért eredményeket, és még inkább az eddig előfordult nehézségeket, és azok megoldásait.
1.1.2 Diplomamunkám stílusáról Munkám során igen sokat kellett elődeim diplomamunkáit forgatnom. A kommentekben szűkölködő forráskódon kívül ez volt az egyedüli támpontom a megoldások mikéntjével szemben felmerülő kérdéseim megválaszolására. Ezeknek a munkáknak a stílusán természetesen érződik az az elvárás is, miszerint az egyetemi diplomamunkának, mint első komoly tudományos értekezésnek tudományos hangvételűnek kell lennie. Ez sajnos nem a legalkalmasabb arra, hogy az utódok számára a legfrappánsabban lehessen kifejezni egy-egy fontosabb gondolat részleteit. Rendszerünk fejlesztésében résztvevők diplomamunkája kettős célú, egyrészt vizsgamunka, másrészt dokumentum a későbbi fejlesztők számára. Az első funkció alapján azok az elvárások, hogy hitelt érdemlően bizonyítsam, komoly munkát végeztem a fejlesztést illetőleg, illetve, hogy minden döntés meghozatalakor
8
kamatoztattam az eddig megszerzett tudásomat és programozási tapasztalatomat. A második funkció egy-egy problémát illetőleg teljesen más fogalmazást kíván. A döntést igénylő részeknél szintén fontos leírni, mit miért csináltam a megvalósult módon, de elég annyit leírnom, hogy szerintem miért volt ez a legjobb megoldás, és nem kell döntésemet kimerítően alátámasztani elméleti háttérrel. Ez azért lényeges, mert néha nem is lehet ezt megtenni, míg néha a döntés helyességének részletezése helyett hasznosabb lehet egyéb fejlesztési tapasztalatok megosztása az utókorral. Jelen munkát elsősorban kézikönyvnek szánom, és könnyű eligazodás érdekében néhány olyan formai megoldást is alkalmazok, ami eltér a standard diploma stíluskövetelményétől. A diplomamunkákkal szemben támasztott követelmény még, hogy külső szakértő számára is teljesen érthető legyen. Ez a diplomamunka nem hiszem, hogy mint különálló értekezés sokat mondó lehet egy kívülálló robotikus számára, mint ahogy azok a programsorok, melyeket megírtam, javítottam, sem alkotnak egészet elődeim munkája nélkül. Ennek ellenére a bevezetésben megismétlem azt, ami az összes eddigi vonatkozó diplomatervben elhangzott már, arra az esetre, ha valaki egyedülálló dolgozatként kívánja ezt a munkát olvasni, megérteni. Még egy meggondolás: a legtöbb tudományos munkát forgatva az irodalmi hivatkozásoknak a következő az alakjuk: [46] 1456-2711.old. Ilyenkor a hivatkozás megtekintéséhez még az irodalomjegyzéket is át kell böngészni. Az eddigi dokumentációkban az is zavaró, hogy különböző kötetekben különböző sorszámmal szerepel ugyanaz a hivatkozott mű. Amennyiben kerülöm a hivatkozások kódszámmal történő rejtjelezését, és szerző, cím, old. alakot használok, jelentős lapozgatástól kímélem meg az olvasót. A már említett fejlesztési segédletként való alkalmazás esetén különösen fontos, hogy a diplomamunkák egymásra hivatkozása gyorsan követhető legyen. A humor alkalmazása az oktatásban fontos dolog. Az olyan egyetemeken, ahol a nagyobb létszámú előadások oktatóit inkább pedagógiai érzék, mint tudományos fokozat alapján választják meg, fontos szempont a jó humorérzék. ( Persze a megfelelő szintű tárgyi tudás, jártasság alapkövetelmény.) Egy-egy felejthetetlen vicc a tananyag megjegyzését segíti, emellett a bóbiskolókat ráébreszti, hogy mégis van miért odafigyelni. A katonai-, rendőri-, illetve gyász-jelentés az, amiben a vidám hangvétel megengedhetetlen.
9
1.2 A robotvezérlő rendszer elemei 1.2.1 A hardver architektúra A HW a következő főbb egységekből áll: • Robotkar, motorok, fékek, szöghelyzet jeladók, megfogó. • PWM áramszabályozó motorhajtás. • Két db. ARC kártya az inkrementális adók jeleinek fogadására, és a motoráram alapjelek kiadására. Ezekhez az IO-perifériákhoz kapcsolódik egy TMS DSP processzor, mely jó FPU teljesítménnyel rendelkezik. Egy IEX processzor is van minden kártyán, mely az eddigi és várhatóan a jövőbeni feladatokban is csak átjátszóként funkcionál a TMS és az ISA busz között. • Egy PC-Lab kártya a fékek kezelésére, vészjelek fogadására, a kalibrációs potméterek analóg jeleinek a mérésére, a digitális IO csatornák kezelésére. • RS-232-n csatlakozó kézivezérlő (Nokia gyártmány). • Host számítógép. ISA buszbővítésen csatlakozik a két ARC és a PC-Lab kártyához, com2 porton a kézivezérlőhöz. Jelenleg Celeron600-as processzora van. Ez a konfiguráció FPU sebességben messze meghaladja az eredetileg tervezett 486 DX kapacitását, de a TMS-ek együttes sebességén is felül teljesít. Emiatt a feladatkiosztások kissé módosultak a kezdeti tervekhez képest.
10
1.2.2 A robot A mi állatorvosi lovunk egy NOKIA gyártmányú PUMA (Programmable Universal Machine for Assembly) 560 típusú szerelőrobot (ábra). Mechanikai alkatrészek rakodására a legalkalmasabb talán. 0.1 mm pontos, kb. akkora térben képes munkát végezni, mint egy helyben álló ember. 2 kg tömegű alkatrészek mozgatására szól a specifikációja az eredeti vezérlővel használva. A robot 6 rotációs (azaz forgó) csuklóval rendelkezik. Az ilyen robotokat sokkal macerásabb irányítani, mint azokat, amelyeknek több transzlációs (csúszó, lineáris) csuklói vannak. Ezért alkalmas kiváltképp a robotirányítási nehézségek elszenvedtetésére. Mozgó részei együttesen nehezebbek, mint 30 kg. A BME fejlesztésű vezérlőszekrénnyel ezt a vastömeget igen fürgén képes mozgatni. Ez alkalmassá teszi arra, hogy egy-egy programhiba bekövetkeztekor lezajló tranziensek során a laborban tartózkodók izgalmi szintje annyira megemelkedjen, hogy soha többet ne menjenek bekapcsolt, fejlesztés alatt álló szoftverű robot közelébe. Az ábra arányaihoz tartozik, hogy a függőlegesen kinyújtott kar (2-es és 3-as szegmens) együttes hossza 90 cm. Ebből a családból (PUMA) léteznek még nagyobbak, a BME GK-nál van is egy jól megtermett példány mutatóba. (Nekik szintén van az 560-as típusból is, azon szintén végeznek robot-programozási gyakorlatokat.) A robot állítólag 120.000 üzemórát képes karbantartás(!) nélkül dolgozni. Karbantartás során a zsírzószemeken keresztül zsírozni kell, be kell állítani a fogaskerék hézagokat az állítócsavarokkal és lefuttatni a kalibrációs programot. Robotunk (test) felépítését szokás a félkarú emberi felsőtesthez hasonlítani.
11
(ábra: a PUMA-560 robot)
12
1.2.2.1
Robot vs. félkarú rabló
Lássuk hát az analógiát. • A robot 1. csuklójának tengelye függőleges. Ennek mozgatása megfelel az emberi felsőtest csípőből való elfordításának. • A 2. tengely vállmagasságban levő mindig vízszintes tengely. Ennek mozgatása olyan, mintha a 3 szabadságfokú emberi gömb-vállizületet csak a két vállat összekötő tengely körül forgatnánk. • A 3. csukló teljesen analóg az emberi könyökkel. • A 4-5-6-os csukó egy ponton megy át, így hasonlatos az emberi csuklóizülethez. A 4-es csukló tengelye (majdnem) párhuzamos az alkarral (3-as szegmens). Ennek mozgatása megfelel az emberi alkar (orsó és sing csont) csavarásának. Viszont. A robot 5-6-os izülete mechanikai konstrukciós okokból az Euler-kézcsuklónak nevezett geometriát valósítja meg. Az ember kéz csuklója nem véletlenül eltérő, RPY-geometriájú. Az Euler-kézcsukló ugyanis amilyen egyszerű mechanikai kúpkerekes áttételezést tesz lehetővé, annyira problémás a fogaskerekeket kímélő irányítása az RPY-hoz képest. Ha az embernek is Eulerféle lenne a csuklója, akkor nemhogy a szerszámhasználat során lettek volna bajok, illetve a fáról történő lemászás során, hanem már a fáramászás sem ment volna. • A robot csuklója még egy mechanikai érdekességet tartogat, ami szintén az egyszerű kúpkerekes konstrukció következménye. A 4-5-6 csuklók között áttételezett egymásra hatás van. Erről bővebben a későbbiekben írok. Előljáróban annyit, hogy mechanikai konstrukciós okokból a motorok nem a látszólagos csuklószögeket befolyásolják, hanem három olyan tengelyt, melynek lineáris kombinációjából adódik ki a 4-5-6-os látszólagos elfordulás. Szerencsére ez a probléma robotmodellezés bármely szintjén (geometriai, kinematikai, vagy dinamikai) egy egyszerű lineáris transzformációval áthidalható. Robotunk környezettel történő leglényegesebb kölcsönhatása az, hogy mozgatja magát, azaz a hat darab csuklóját. A csuklókat elekromotorok forgatják, az alsó 3 csuklóét 3 egyforma nagyobb, a kézizületeket 3 egyforma kisebb motor. Szöghelyzetjeladók adnak visszajelzést a csuklók tényleges elfordulásának mértékéről. Ezek az érzékelők igen pontosak, de csak relatív elmozdulást mérnek (differenciális jellegűek), ezért az abszolút csuklóhelyzet meghatározásához segéd potmétereket használunk. Ezek pontatlanok, de a megfelelően programozott együttes működés csak az előnyöket egyesíti. (Az oldalkocsival, illetve a robogóval szöges ellentétben, előbbi ugyanis az autó, utóbbi a roller hátrányait egyesíti a motor hátrányaival.) Ezeket a potmétereket kalibrációs potmétereknek nevezzük. Az én fejlesztési stádiumomban már kielégítően működtek a vonatkozó programrészek, így ezekről csak kevés szó fog esni. A robot izületeit feszültségmentes állapotban záró fékekkel rögzíteni lehet, hogy kikapcsolt állapotban ne essen össze a robotkar. Ezen kívül a robot képes bármilyen a kezére szerelt szerszámmal nyomatékot és értelmet adni annak, hogy azt képes igen pontosan és gyorsan különböző pozíciókba vinni. Jelenleg egy pneumatikus, elektromosan működtethető kétujjú megfogó van rajta.
13
1.2.3 A BME robotvezérlő A robot a BME fejlesztésű robotvezérlő szekrényhez kapcsolódik. A motorokat analóg PI áramszabályozású PWM erősítők mozgatják. ±30V-os feszültség tartományban mozoghat a kimenetük, az áramszabályozás pedig a nagyobb motorokra ±7A-t, a kisebbekre ±3.5-et próbál beállítani. Ezek a határok az alapjelekre vonatkoznak, tehát azt jelenti, hogy ennél nagyobb áramok is előfordulhatnak a kimeneten. Az analóg áramszabályozás bemenő áram alapjeleit az ARC kártyák adják ki. Ezekre vannak kötve a szöghelyzet (inkrementális) jeladók is. Egy ilyen kártya 4 csukló áram és inkerementális jeleit képes kezelni. Jelen kiépítésben 2 kártya kezel 3-3 csuklót. Ezek az ARC (Advanced Robot Controller) kártyák BME fejlesztésűek. Lényegében Tevesz Gábor munkái. Tervezésük a teljes robot-vezérlő tervezésével egyszerre, azzal összhangban készült. A robot fennmaradó vezetékei egy Advantech gyártmányú kártyára vannak bekötve. Ezt mi fejlesztők csak PC-lab kártyának hívjuk. Erre futnak be a kalibrációs potméterek analóg kimenetei, a robotvezérlőre beköthető 8 optocsatolt digitális bemenet, és ez adja ki a 8 digitális kimenetet, melyekből 4 hasznosítható általános célra, kettő a megfogót működteti, egy a fékek elengedését, egy pedig a hajtást engedélyezi. A robotvezérlőnek van egy betanítópultja, ez RS-232 porton csatlakoztatható a Host alaplapjához. Ezt szokás kézivezérlőnek, illetve az angol teach-pendant kifejezés után pendant-nak nevezni. Bár ’pendant’ alatt általában a kézivezérlőt kezelő processzt szoktuk érteni. A robotvezérlőn belüli részek egy összekötő panelen kapcsolódnak egymáshoz. Ennek a tervezése is egy régi diplomamunka keretében zajlott. A kapcsolási rajz megtalálható a laborban, hasznos lehet az egyes jelek ponált-negált voltának kiderítésekor. A javításokról, nem egyértelmű dolgokról [Donát Tamás, Diplomaterv, 1996. 3.Fejezet: A hardveren végzett módosítások 35-40. old.] olvashatunk. 1.2.3.1
Az ARC kártyák
Egy ARC kártyán két processzor van. Egy párhuzamos lebegőpontos számításra képes TMS dsp a robot felöli oldalon, és egy minimális 386-os PC alaplapot is magában rejtő embedded Intel 386 EX processzor (továbbiakban IEX). A TMS buszára kapcsolódnak a szöghelyzet jeladók µprocesszoros illesztését végző tacho-controllerek, az áram alapjeleket kiadó DA-konverterek, illetve az IEX-el való kommunikáció céljából egy 4-kiloszavas DualPort RAM (továbbiakban DPR-D mint Dsp),és egy egyszerű szinkronizációs logika. Ez jelenleg nem használt, bár érdemes lenne a két TMS megszakítását szinkronizálni. A TMS feladata nagyjából a tengelyenként szeparalható számítási algoritmus elvégzése, az ehhez szükséges kommunikáció megvalósítása, illetve az eredeti feladatkiosztás alapján még egyéb számítások is. Az IEX processzorok csak kommunikációszervező szerepet töltenek be, lévén számításra nem igazán használhatóak, mert nincs FPU-juk. Az IEX buszára kapcsolódik a DPR-D-n kívűl egy másik DP-RAM az ISA-busz csatlakozóra kötve (DPR-I mint Isa), egy eprom a 14
bootoláshoz, és a TMS bootoltatásához, valamint egy CAN-busz logika, amely nincs minden kártyán fizikailag kiépítve, így a szoftverrendszer konkrét megvalósítása hanyagolni kénytelen az ebben rejlő lehetőségeket. 1.2.3.2
Kicsit a megvalósított, illetve jövőbeni kommunikációról
Jelen megvalósításban működés közben az IEX-ek szerepe a DPR-I-k összekötése a DPR-D-kel. Sajnos nem lehet olyan átlátszó kommunikációt megvalósítani (Huszti Ákos diplomaterve 22-33.old), mintha csak egy DP-RAM lenne az ARC kártyán. A csuklókat mozgató ARC kártyák egy PC ISA buszára kapcsolódnak, így ennek a PC-nek (továbbiakban Host) a processzora hangolja össze működésüket, illetve végzi a magasabb szintű robotirányítást. A Host-buszára köthető a jövőben a hatkomponensű erőmérő illesztését végző kártya, de a hálózati kártyáját a valós idejű működés zavarása nélkül kezelni tudó QNX operációs rendszer képessé teszi más, QNX-et futtató PC-kel való együttműködésre is. A BME robotkezet irányító PC is várhatóan így fog kommunikálni a Hostal. 1.2.3.3
QNX,neutrino:
A Hoston futó QNX operációs rendszer 32-bites védett módban fut, multitaszkos, preemptív ütemezésű valós idejű (RT) OS. Gyors processzközti kommunikációt tesz lehetővé. Négyféle kommunikációt használhatunk és használunk is alatta: üzeneteket, hírvivőket, jeleket, és osztott memóriát. A kézfogás alapú kommunikáció (üzenetek) várakoztatná a legtöbbet a résztvevő processzeket, de ez is több százezer szinkronizálásra képes egy másodperc alatt. A QNX időzítőjének a legkisebb időfelbontása sajnos csak egy ezredmásodperc, pedig ennél nagyságrendekkel gyorsabban képes taszkot váltani. A jövőben valószínűleg áttér a projekt a QNX új üdvöskéjére, a Neutrinora, amelynek a legnagyobb erőssége a szálak igen gyors ütemezése. A témába mélyült önálló laboros hallgatók (Halmai Zsuzsanna és Varga László) tapasztalatai szerint nem igazán újrabeléptethetőek a könyvtári függvényei, illetve nehézkes ezeket a problémákat orvosolni. Ezért az áttérés során javasolt megtartani a több processzes szerkezetet, nem alkalmazni a szálak (threads) gyors ütemezhetőségét, és csak a kommunikációs változásokat implementálni. Az alternatív Real Time operációs rendszerek és a QNX verzióinak összehasonlításával, időzítési analízisével foglalkozik Pindzsulya Béla szintén ebben a félévben készült diplomaterve. 1.2.3.4
A robotvezérlő processzorainak számításteljesítményéről
A robot irányítás számításigényes folyamat. Ez a mai számítási teljesítmények fajlagos árához viszonyítva már nem probléma, de a kutatás indulásakor rendelkezésre álló eszközök sebessége mellett a feladatot csak több processzor összehangolt párhuzamos működésével lehetett megoldani. (Mi sem bizonyítja jobban a rohamos
15
fejlődést, mint hogy az IBM-et perelték, mivel 1996-ban eladott 2 Gflop feletti "szuper" számítógépeket Oroszországba az USA külügyminisztérium engedélye nélkül. 1999-ben már elérték ezt a sebességet az utcai boltokban árult intell és AMD processzorok. (4flop/clk@500Mhz)). Az eredetileg tervezett rendszerben megvolt a számítási feladatok elosztásának tervezete is. Időközben kétszer is le lett cserélve a Host-nak nevezett PC. Az eredeti 486-dx2-50 10-20 Mflopos teljesítményéhez képest ma 600 Mflop feletti teljesítményt tud a Host processzora elvégezni. A rendszer többi alkatrésze egyedi, ezek cseréje nem lett volna ilyen olcsón elvégezhető. Így az eredetileg elosztott számítási teljesítményű rendszerből egy központosítandó rendszer lett. Maradt viszont a teljes HW kiépítés, az eredeti kommunikációs tervek szerint optimális, de a változásokkal együtt is jól használható HW együttes.
16
1.2.4 A szoftverrendszer fejlesztéséről 1.2.4.1
A szoftverrendszer fejlesztésének története
A szoftverrendszeren eddig dolgozók listája, tevékenységeik röviden (önálló labor munkákat nem említek külön): • 1993: • Palotás Zoltán: Az ARPS nyelv fordítóprogramja MS-DOS alá. Diplomaterv, FSZ. • Nagy Norbert: Az ARPS nyelv interpretere, MS-DOS. Diplomaterv, FSZ. • 1995: • Vörös István: A készülő HW élesztése, tesztprogramjai, a 93-as ARPS eredmények kísérleti adaptálása QNX alá. Diplomaterv, AUT. • 1996: • Donát Tamás: HW áramköri módosítások, PC-LAB illesztőfüggvényei, az ARC kártya felé szükséges kommunikáció definiálása az IEX meglévő, karakteres gyűrűs pufferes kommunikációjának a használatával, a kézivezérlő funkciói, kalibrálóprogram. Diplomaterv, AUT. • Korcsmár Attila: Egykori Controller Teszt Program fejlesztési eredményei*, Tevesz Gábor MS-DOS-os ARC monitor programjának QNX, illetve QNXWindows alá való implementálása, a TMS processzor első szabályozóprogramjai. Diplomaterv, AUT. *Ez tematikában kapcsolódik ugyan a robotvezérlőhöz, de konkrét összefüggés nincs a kettő között. A robotvezérlő rendszerről ez a fejezet nem tartalmaz információt. • Jász Gábor: Az átírási bonyodalmak miatt az ARPS újraírása QNX alá, az eddigi eredmények felhasználásával, a pályatervezés koncepciója. A robot konkrét mozgatása tesztlehetőség híján kidolgozatlan maradt. Diplomaterv, FSZ. • 1995-1998 Klatsmányi Péter: Az FSZ tanszék szoftverének továbbfejlesztése. Doktori disszertációja beadatlan. FSZ. • 1997: • Ludvig László: a BME robotkéz mechanikája. Doktori Munka. (A robot kéz inverz mechanikáját 3-motorral mozgatva kevesebb probléma lenne, mint a 4 motor direkt mozgatásával) • Vas Gábor: A BME-ROBOTKÉZ Irányító rendszere QNX-Windows alá. Diplomaterv, FSZ. • 1999:
17
• Huszti Ákos: A programozási beállítások (make-fájlok, fordítási opciók) megfontolásai jelen fejlesztés szempontjából. A kezdeti kommunikációs módszer mellett három féle új, gyors kommunikáció implementálása. A Puma 560 specifikus részek kifejlesztése. A TMS dupla pályatervezése, fejlettebb szabályozási algoritmusok implementálása. Az RMAC elméleti nehézségei a gyakorlatban. Diplomaterv, AUT. • Somlói Dániel: Az ARPS továbbfejlesztése, a pathgen processz implementálása, kézivezérlő működésének megvalósítása új processzben (pendant), a Hoston futó processzek együttműködése. Diplomaterv, AUT. • 2000: • Sárközy Ferenc: Robot vizuális megjelenítése photon alá, ütközésdetektálás, robotszimuláció. Diplomaterv, AUT. • Lovasi Péter: Az előző éves eredmények javítása, allign algoritmus, inverz geometria több konfigurációban, geometriai erők kiadása csuklóelengedés esetén, program futtatás hibáinak javítása, messenger processz. Diplomaterve jelenleg beadatlan. AUT. • Fodor Gergely: TMS-processzoros 6 komponensű erő-nyomatékmerő illesztése, HW kidolgozása. Az AUT és FSZ programrendszer összehasonlítása, fejlesztési irányok megfontolása. Doktoranduszi beszámolók, AUT. 1.2.4.2
Új fejlesztők figyelmébe
A fejlesztésbe bekapcsolódó új hallgató számára szerencsés, ha a következő lépéseket megteszi: • Átolvassa az elmélet megfelelő részeit. (Robot geometria, dinamika, pályatervezés interpolációs megoldásai, eddig megvalósított irányítások (DCS,CTM,RMAC) elmélete. A BME vezérlő architektúrája. A QNX operációs rendszer valós idejű programozása, ütemezése, kommunikációs megoldásai.) • Tájékozódik, hogy az aktuális rendszerben melyik hallgató eredményei találhatóak meg, illetve, hogy a rendszer melyik része kinek a műve, és milyen alapokra épül. Ezt szinte memorizálás szerűen érdemes bevágni. • Megismeri és megérti az eddig elkészült szoftverrendszer forráskódját. Azok számára, akik csak a rendszer egy kis részét kívánják tökéletesíteni, célszerűbbnek tűnhet csak a megfelelő részek tanulmányozása. Tapasztalataim szerint ez ahhoz vezet, hogy a nem megismert részeket úgy fogja kerülgetni a programozó, mint macska a forró kását, még akkor is, ha rendszertechnikailag jobb megoldást eredményezne, ha az egész rendszert érintő módosításokat választana a fejlesztő. • Ismerje meg minden fejlesztési platformon való munka mikéntjét. (fájlkezelők, editorok, fordítók használata, stb.) Jelen pillanatban 4 platformon folyik a fejlesztés: • QNX alatt fordítás QNX-re, a Host processzoron futó processzek számára, • QNX alatt fordítás DOS-ra, az IEX programjának előállítása. • DOS (Windows DOS shell) alatt fordítás a TMS-dsp-re, 18
• valamint Windows alatt a MS-VisualC I.D.E-jének a használata szerkesztésre és szintaktikai ellenőrzésre. Melegen ajánlhatom az eredeti orosz dokumentáció átlapozgatását mindenkinek. A szöveg természetesen nem érthető benne, de az ábrákból, táblázatokból, példaprogramok alapján sok lényeges dologra derült fény az eddigiekben. Amúgy a dokumentáció fontosabb részeinek létezik magyar fordítása. A csukló-egymásrahatás megértésében nagy segítséget jelentettek a robot belső szerkezetét mutató ábrák, és ebből a dokumentációból derül az is ki, hogy a robot szállításánál használt fekete hengerek használhatóak a pontos kalibrációs diagramm felvételénél is. Sajnos a sokat vadászott fogaskerék-fogszámok nincsenek benne említve, habár az egyes tengelyeken levő tömítőgyűrűk méretei, cikkszáma, és egyéb nélkülözhetetlen adatok természetesen adottak. Erről jut eszembe, hogy a másik fekete folt a dokumentáltságot illetőleg a hajtásszabályozók esetében van. Találhatunk a laborban kapcsolási rajzot a rögzítési furatok átmérőjével, a beszerelésre került trimmer potenciométerek cikkszámával, de a laborban levő dokumentumok alapján nem lehet kideríteni se a PWM frekvencia értéket, se a szabályozó paramétereket. (Bármely egyéb kaszkád szabályozás paraméterének meghatározásához elméletileg jó lenne ezeket ismerni. Gyakorlatilag a lineáris modell borul. A fizikai áramkörök miatt a modellbe beveendő szaturációs (korlátozó) jellegű elemek a különböző szintű szabályzóhurkok egymásra hatásától való eltekintésre kényszerítenek.) A PWM frekvencia állítólag 16 kHz körül van. Emiatt a szimuláció ciklusidejét nem érdemes 1 ms-nál gyorsabbra venni. Jó lenne tudni az áramszabályozás beállási idejét.
1.2.5 Az egyes platformokra való fordításról A Host programrendszere QNX alatt fordul. Bárhová áthelyezhető az ‘aktualis’ könyvtár. Ezen belül található meg a ‘host’, az ’iexforras’ és a ’tms’ könyvtár. A host programjai mind a ’host’ könyvtár alatt vannak, kivéve a ’com.h’-t, mely a ’tms’ könyvtárban van. A ’host’ könyvtárban van egy ‘makefile’ nevű makefile, ez tartalmazza az összes fordítási beállítást. A szoftverrendszer a ’host’ könyvtárban kiadott ‘make all’ parancsra fordul. QNX alatt hasznos a QNX-Commander (qc) használata. Ez külső editorként hívja a vedit nevű programot. Mindkettő belső menüből konfigurálható, és a konfiguráció fájlba menthető. A ‘Fatfsys’ program teszi láthatóvá a DOS-os partíciókat. Ezekbe a legpraktikusabb ‘cd /dos/c’ parancsal belépni, mert a ‘/dos’ könyvtárra kiadott listáztatás megseekeli a floppyt, és hibajelzést ad, ha nincs benne. A watcom debugger ‘wd’-re, a dissassembler ‘wdisasm’-re indul, de ez utóbbi paraméterezését jobb a felhasználói kézikönyvből kinézni. A régi pentium gépen (most középső) fel van installálva a photon. Ez ‘ph’ begépelésére indul. Nagyon hasznos a QNX, illetve Clibrary on-line segédlete. Ez egy könyvszerű ikonnal indítható. A robotszimulátor fordításáról [Sárközy Ferenc, 2000:Diplomaterv]-e tartalmaz információt. A TMS fordításához a könyvtárát át kell másolni MS-DOS partícióra. Egy sima MSDOS bootolása (vagy windowsban ‘újraindítás MS-DOS módban’) után a következő parancsokat kell kiadni: ‘\tms_sys\startup’, ‘cd tms-forrás-könyvtár’, ‘tms_env’, ‘do3’.
19
Ezek után QNX alatt a tmspos.out visszamásolható a ‘tms’ könyvtárba, majd a ‘host’ könyvtárában levő ‘TMSPOS.OUT’ arhiválása után átmásolandó oda is. Nagybetűsre kell átnevezni, különben a loader (arcrun) nem találja meg. Az IEX-re nem fordítottam, annyit tudok róla, hogy QNX alatt kell csinálni, és a makefile-jába bele van írva, hogy 32 bites DOS alá készül. A kész, futtatható állományt szintén nagybetűvel kell a ‘host’ könyvtárba átmásolni a régi verzió arhiválása után.
1.2.6 A szoftverrendszer megismeréséhez ajánlott linkek A következő fejezetben ismertetem a BME robotvezérlő AUT tanszéken működő szoftverrendszerét. A fejezet végighalad a rendszer egészén, és közben olyan részek működésére is kitérek, melyeket alkotójuk már ismertetett saját diplomatervében. Ebben a bekezdésben ennek a fejezetnek és a következőnek a tematikája szerint felsorolom a hivatkozásokat. Ez a lista a szoftverrendszer kapcsolódásai alapján rendezi az eddigi fejezeteket, így előfordulhat, hogy két kötet közötti ide-oda váltogatást javasol, ha a témák kapcsolódása ezt kívánja. A BME vezérlő tervezési aspektusairól olvashatunk: [Lantos-könyv, 8. fejezet 262.269.old.] A hardver architektúráról bővebben [Tevesz Gábor oktatási segédlet] -ében találunk ábrakat, ezeket engedélyével a függelék tartalmazza. Az arc kártyák működéséről és programozásáról szól (teljes részletességgel): [Tevesz Gábor: ARC robotvezérlő kártya felhasználói leírás]. A QNX operációs rendszer kommunikációs megoldásait az photon alatti on-line kézikönyvből lehet megismerni. [Bézi István Párhuzamos és valósidejű rendszerek segédletének 6. fejezet]-e magyarul foglalkozik a QNX-el. [Donát Tamás,1996,2.fejezet 6.-34.old.]: A hardver [Huszti Ákos,1999:9.-21.old.]: A fordítóprogramok használata [Huszti Ákos,1999:49.-67.old.]: A TMS, az IEX és a Host processzor szerepe [Korcsmár Attila, 1996: 4. fejezet,35.-48.old.] A TMS-re fejlesztett programok [Huszti Ákos,1999:22.-33.old.]: A kommunikáció A com.h forrás (tms/com.h) áttanulmányozása is segít a kommunikáció megértésében. [Huszti Ákos,1999:8.old]: Az arcrun folyamat. [Somlói,1999,45.-52.old.]: A host processzeinek együttműködése [Somlói,1999,31.old.]: A pathgen működése [Somlói,1999,37.-40.old.]: A pendant működése [Donát Tamás,1996,60.-65.old.] A kézivezérlő kezelése [Somlói,1999,2.,3.,4.6.,6. fejezetek]: Az Arps-ről bővebben
20
2 .fejezet: A jelenlegi (utánam maradó) SW rendszer 2.1 Bevezetés A szoftverrendszer ismertetését működés alapján teszem. Először megismertetetem, hogy normál robotmozgás közben mi zajlik le a robotvezérlőn belül, a megfelelő részeknél kitérve azokra a részletekre, ami nem tartozik a normál működéshez (inicializáció, kalibráció, vészleállás.) Mindezek előtt érdemes megemlíteni egy két technikai részletet a robotkarról, a hardver és a szoftver kapcsolatáról.
2.1.1 Robot paraméterek 1 A szoftverrendszer szempontjából a robot IO vezetékeinek működésen kívül igen fontosak a robot dinamikai és geometriai paraméterei. Az eddig felbukkant több ellentmondásos forrásból sikerült kiszűrni a legvalószínűbb értékeket, amivel a rendszer úgy-ahogy pontosan működtethető. A projekt fontos állomása lesz, amikor a megbízható működést log-fájlba menteni képes rendszer adatai alapján saját mérési eredményekkel is össze tudjuk hasonlítani az eddig szerzett információkat. A jelenlegi modellek körüli huzavonára a későbbiekben még visszatérek. Amit tudni kell a robotról: melyik tengelyen hány osztású inkrementális adó helyezkedik el, mekkorák az áttételek tengelyenként, mik a lényeges geometriai méretek, illetve a dinamikus modell számításához szükséges paramétereket. A szoftver ismertetésénél visszatérek arra, hogy melyik modul mely adatokkal számol.
2.2 A TMS szerepe Normál robotműködés esetében a szöghelyzet jeladók az ARC kártyák tachokontroller regisztereinek értékét változtatják az elfordulással összhangban. Egy, a TMS inicializálásakor 1 ms-ra felprogramozott timer interrupt elindítja a TMS megfelelő IRQ kezelőjét. Ez kiolvassa a tachoregiszterek értéket, a 16 bites regiszterek túlcsordulási problémáját lekezeli, majd a beállított szabályozási algoritmust meghívja. Ez (lejjebb részletezve) kiszámítja a kiadandó digitális áram-alapjelet, azt a DA-konverteren keresztül érvényre juttatja, a szabályozás által érintett változókat és a csuklóváltozókat a DPR-D-nek egy részébe másolja (erre a kommunikációs résznél fontos lesz visszatérni, mert gyorsítani kellene a jelenleg használt megoldáson), valamint nyugtatja a watch-dog timert. A szabályozó algoritmus 3 féle lehet. Decentralizált kéthurkos kaszkád szabályozás (DCS), kiszámított nyomatékok módszere (CTM), illetve bármilyen egyéb Host-on történő szabályozás esetén a vett áramjelek kiadása. Ez jelenleg az RMAC kódot viseli, mert ez volt az első (és egyetlen) Hoston implementált szabályozás. DCS és CTM szabályozás bemenetei az aktuális csukló pozíción és sebességen kívül a pozíció, 21
sebesség és gyorsulás alapjel. (Utóbbi kettő csak CTM-hez.) Ezeket a szabályozó algoritmus onnan nyeri, hogy neki a TMS-en fut egy negyedfokú csuklóinterpolációs pályatervezése, melynek szimulációs lépéseit is a tacho-interrupt végzi. A szabályozási algoritmusok globális bemenő paraméterei a szabályozó paraméterek. Mivel már volt probléma a TMS-en levő, és a Host-által ismert paraméterek eltéréséből, beleírtam a programba, hogy a szabályozás addig nem hajlandó elindulni, amíg minden paraméter nem lett a Host-által ismert értékűre állítva.* A szabályozási algoritmusok kezelik a sebesség, ill. gyorsulástelítésből adódó problémát. Az általam örökölt verziókban valamiért ki van remelve a túl nagy sebesség, illetve pozícióhiba miatti vészleállítás. Elvileg vissza lehetne kapcsolni, de a sebességhatárt legalább 10%-kal az elméleti határ fölé kellene állítani. A TMS-en ezenkívül végtelen ciklusban fut egy üzenetfogadó switch() elágazás. Ezen keresztül lehet azokat a globális változókat állíttatni a Host által küldött üzenetekkel, amelyek befolyásolják a robot-mozgás-szabályozás működését. Így lehet új pozícióértékeket küldeni a pályatervezés számára, de vannak a robotmozgás és a szabályozás belső állapotát lekérdező üzenetek is. Természetesen megtalálhatóak az Init() és Exit() jellegű üzenetek, valamint a kalibrációs rutinok, mint nem normál működést indító üzenetek is. *A radián-inkremens konverzió állandóira ez még nincs megvalósítva.
2.3 Az IEX feladata Az IEX processzor mindig is a postás szerepét töltötte be, lévén nincs FPU-ja, így nem sok robotirányítási feladatra használható. Mivel a multiprocesszoros rendszerekben a kommunikáció is szűk keresztmetszet tud lenni, ezért az azt megvalósító áramkörök szerepe igen hangsúlyos. Ezért jön jól, hogy a robotvezérlőben külön µprocesszorok foglalkoznak a feladattal. Az IEX sem multiprogramozott, sőt rajta sem fut igazán operációs rendszer. Egy időzítő és egy szerver jellegű (más processzorok által rangatható) megszakítása van, valamint egy végtelen ciklusa a legalacsonyabb prioritású kommunikáció bonyolítására. Mint már említettem, az IEX jelen kiépítésben két Dual Port Ram közé van bekötve. Ennek a kettőnek a virtuális összekötését végzi. A jelenlegi kommunikációs formát Huszti Ákos építette ki. Sajnos az ő diplomatervben ez a sarkalatos rész inkább deklaráló, mint magyarázó stílusban van leírva [Huszti Ákos,1999:22-33.old,com.h]. Nekem csak pár nekifutás után keletkezett annyira tiszta képem az egész rendszerről, hogy időzítési kérdésekben döntést merjek hozni a lehetséges megoldások között. Úgy érzem hasznos, ha a saját szavaimmal is leírom az ő munkájának eredményeit, így a jövőbeni fejlesztők több forrásból meríthetnek információt ennek a fontos résznek a megismeréséhez.
2.3.1 A 4-féle kommunikáció
22
A legrégebbi kommunikációs fajta Tevesz Gábor DOS-os arcmonitor programjáig visszanyúló gyűrüs pufferes karakterbázisú kommunikáció. Mindkét DP-RAM-ban szükséges hozzá egy-egy azonos méretű puffer, egy írás pointer, és egy olvasás pointer. Az egyik oldalon elküldött szövegek megjelennek a másik oldalon, ha időközben nem telt be a buffer. Az IEX fő végtelen ciklusa az átjátszást kb. így oldja meg: • While(1){ • If(dprd_canget()&&dpri_canput) dpri_putch(dprd_getch()); • If(dpri_canget()&&dprd_canput) dprd_putch(dpri_getch()); • } Ezt ma a TMS szöveges hibaüzenetei használják, ezzel töltődik fel a TMS programja, és a TMS programjába szúrható debug célú print-üzenetek is így juthatnak el a Host-ra. Erre a kommunikációra emlékeztet Huszti Ákos által megvalósított és legszívesebben használt üzenetbázisú kommunikáció abból a szempontból, hogy az üzenetek vételi oldalán szintén egy gyűrűs pufferbe kerülnek. Ez a kommunikáció lehetővé teszi, hogy a robotvezérlőben futó bármely processz tetszőleges hosszúságú üzenetet küldhessen bármely másik processzor bármely processzének. Természetesen a TMS processzoroknak csak egy processzük van jelenleg, míg az IEX processzorok nem funkcionálnak se feladóként, se vevőként. A következő említendő üzenetfajta a periodikus DP-RAM tükrözés. Ennek lényege, hogy a DP-RAMokban lefoglalhatunk olyan területeket, amelyek tartalmát az IEX a timer interruptja bekövetkezésének (1ms jelenleg) ütemében másolja át a megadott irányban a másik DPR-be. Ezt ’Blokk másolás periodikusan’ , illetve ’periódikus blokk másolás’ néven emlegetem a forráskód megjegyzéseiben. A két előző kommunikáció átmenete a ’blokk másolás kérésre’ típusú módszer. Ezt szükség esetén kérni kell az IEX-től, és az a kérést egyszeri alkalommal hajtja végre. Ez jelenleg nem használt, pedig a Host-on futó szabályozások implementálásához sokkal jobb időzítéseket nyújthat, mint a jelenlegi periodikus másolásos megoldás. Ez a módszer lenne alkalmas az igazán gyors kommunikáció megvalósítására. Sajnos azonos prioritású a standard üzenetkezeléssel, ezért várakoznia kell az éppen kiszolgálás alatt levő kérés befejeződésére. Ez nem biztos, hogy olyan nagy baj, szerintem az IEXl parancsvégrehajtására várakozás nem jelentős a mostani rendszerben. Igazából ki kellene mérni az egészet. A következő bekezdésben kénytelen vagyok hivatkozni a még nem tárgyalt arcproc folyamatra. Ez a processz a Host processzoron fut, és a többprocesszoros kommunikációnak a külön folyamatban megvalósítandó funkcióit végzi el a Host CPUján. Lássuk a fejlettebb megoldásokat egy kicsit bővebben: Az IEX-ek mindkét oldalára kapcsolódó DP-RAM-okban van egy command, egy státusz mező (word), valamint egy rángatható interrupt . Az összes Huszti féle kommunikáció úgy működik, hogy a command nullázódásának kivárása után a command mezőbe irható az IEX-el végrehajtandó akció kódja, majd az interrupt kiváltandó. Az IEX-el végrehajtandó akciók paramétereit a DPR-Ii megfelelő részére kell előzőleg felmásolni. Az üzenetkezelés oszthatatlansága biztosított, mivel minden
23
üzenetet küldeni kívánó processz az arcproc-on keresztül üzenget, amiből csak egy példány létezik. Így a már kitöltött paraméterek nem íródhatnak felül a command?=0 várakozás alatt. Az IEX csak a teljes akció végrehajtása után nullázza a command mezőt, és a státusz mezőben jelez vissza a végrehajtás eredményességéről. Az üzenetek lehetnek a következő félék: • üzenetkezelő mechanizmus inicializálása (ARC kódszám tudomásul vétele), • exitálás, • kérés üzenet továbbítására, • blokkmásolás kérésre. Minden típusú akció elsődleges paraméterei számára fix DPR-en belüli címeket definiált Huszti Ákos. Az olyan akciók, amelyek az önálló akciót kívánnak a címzett oldalán is (interrupt kérést) (pl. az üzenetküldés), várakozást tartalmaznak mind a túloldali akció megkezdése előtt, mind azután. Szerintem az utólagos várakoztatást meg lehetne szüntetni, ha nem törődnénk a túloldal státusz-visszajelzésével. Ezt alátámasztja az, hogy az üzenet küldője ezt sehol sem vizsgálja. Elvileg a túloldalon azonnal beüt a kiszolgáló interrupt, ha a TMS a címzett. A mostani rendszerben a TMS nem küld üzenetet, így a Host sohasem címzett. Ha mégis megtenné, úgy az arcproc IRQ-handlerje sem várakozna semmire a kérés kiszolgálása alatt. Üzenet küldéséhez a meg kell várni az IEX command szavának nullázódását, ki kell tölteni az üzenet DPR-en belüli elejét és hosszát mutató paraméterszavakat a DPRben. Ha a címzettet implicit tartalmazza a parancs kódja, akkor az kiadható, különben a címzett-paramétert is írni kell. Ezután jöhet a parancs elindítása az interrupttal. Az üzenetküldés végét (command=0) csak akkor kell megvárni, ha kíváncsiak vagyunk a státuszra. Amennyiben a TMS-nek küldünk a Host-ról üzenetet, úgy az IEX megvárja a TMS DP-RAM interruptjának készenlétét, kiváltja az interruptot. Ez az interrupt kimásolja az üzenetet, ha van rá helye, ha nincs, eldobja, és a státuszban nem 0 értékkel jelez vissza. Ezek után azonnal visszatér. Az üzeneteket érkezési sorrendben fogja a TMS végtelen ciklusa fontossági sorrend nélkül lekezelni, de erre már nem vár az IEX. Ezután az IEX a Host oldalára másolja a státusz szót, és command mező nullázásával jelzi, hogy kész az újabb akció (üzenet) végrehajtására. Ha a Host kap(na) üzenetet, akkor az arcproc processz interrupt handlere ütemeződik. Ez a processz minden fogadni óhajtó processz számára külön gyűrűspuffert tart fent. A kapott üzenetet a címzetthez tartozó buffer telítettségétől függően vagy átnyalja, vagy eldobja, és egy QNX Trigger(proxi) hívás után jelzi az IEX felé az újbóli készenléti állapotot. A proxi indítás nem blokkol, az adott processz elvileg akkor fog ütemeződni, ha az interrupt handler visszatér, és minden magasabb prioritású ütemezésre váró processz blokkol. A blokkmásolás kérésre kommunikáció egyetlen szépséghibája, hogy érvényre jutásához kisebb várakozásra kényszerül, annak ellenére, hogy a Hoston történő szabályozáshoz ez lenne a leggyorsabb kommunikáció. Sajnos a Host-ról jelenleg nem használható, mivel az oszthatatlan IEX akciókiváltás miatt ezen akciónak is az arcprocon keresztül kell történnie. Jelenleg ez nincs implementálva, az arccom.c fájlba kell felvenni a kliens által hívandó függvényt, ennek egy send() hívást kell tartalmaznia az arcproc felé, az arcproc.c-be az üzenet kezelését kell felvenni, és az akció továbbítását az IEX felé.
24
A periodikus blokkmásolás esetében az IEX timer interruptja végignézi a Host2TMS, illetve TMS2Host tömböket, melyik irányba hány, és mekkora memóriaterületet kell másolni, illetve, hogy van-e szemaforkérés ezekre. Véleményem szerint a szemaforhasználat az esetek jó részben felesleges holtidőket hoz csak a rendszerbe ennél a fixen egyirányú kommunikációnál. Jelenleg CBUP-nak hívjuk azt a DPR-eken belüli memóriaterületet, amely az alacsony szintű csuklóprocesszoroktól szállít információt a magas szintű irányítást végző Host processzor folyamatai felé. Ebben Lantos-féle jelöléssel többek között a következők vannak benne: q[i],qd[i],qa[i],qad[i],qe[i],qde[i],I[i]. CBDOWN-nak hívjuk azt a memória területet (és struktúrát), amely a dynjac processz által számított kinematikai ill. dinamikai paramétereket viszi a TMS-ek felé. A periodikus blokkmásoláshoz szükséges változók az IEX globális változói. Egy új buffer létrehozása csak az IEX felől kérhető. Az iexpass.c modul az IEX programjának elindulásakor inicializálja a két mostanában használt buffert. Szintén ennek a modulnak az exitáló kódja szünteti meg a bejegyzéseket.
2.4 A Host programrendszere A Host programrendszerének 1999-es állapotáról olvashatunk Somlói Dániel [Somlói,1999,45-old-52.old] diplomatervében. A Host programrendszerének teljes feltérképezését önálló laboratórium keretében egy évvel ezelőtt végeztem (2000 őszén). Erről készült is egy sokat mondó ábra, ami nem tartalmazza az időközben eszközölt változtatásokat. Ennek az ábrának a frissített változata található itt meg. A robotvezérlő szoftverrendszer teljes mértékben kihasználja a QNX real-time operációs rendszer által nyújtott valós idejű ütemezési és kommunikációs szolgáltatásokat. A szoftverrendszer részeként a Hoston jelenleg 9 folyamat fut. Ebből 2 csak az inicalizáláskor kap szerepet, ráadásul ugyanaz az imageük (gépi-kódjuk). Ezek az ARC kártyák processzorait indító arcrun processzek. 2 másik processz működésére csak a blokkolás elkerülése érdekében van szükség. Ezek a console és a messenger. A maradék 5 processz vesz részt érdemlegesen a robot irányításában. Az arcproc illeszti az ARC kártyákon megvalósított kommunikációt a QNX operációs rendszerhez. A dynjac processz prioritása jelenleg nincs emelve, periodikusan ütemezett, és a rendszeresen frissülő különböző csuklókról jövő adatokat alakítja emészthető formába, valamint a CTM algoritmust támogatja. Jelenleg ki van ’remelve’ a TMS CTM szabályozójából, hogy használja ezeket az adatokat, mivel a 33MHz elégnek bizonyult arra, hogy mindkét TMS processzor a saját maga részére számolja ki a szükséges adatokat. A pendant processz a COM porton lógó kézivezérlőt kezeli. Mivel ennek vészleállító gombja van, ezért nem szabad sohasem blokkoltatni, valamint magas prioritáson kell futtatni. Amennyiben övé a robot mozgatásának a joga, úgy arcproc-on keresztül mozgatja a robotot. Az arps processz ’preemptív’ módon elveheti tőle a vezérlést. Ilyenkor üzenetet küld a pendant-nak, melyet az creceive()-el vesz (feltételes, nem blokkoló üzenetfogadás), ha odaér a megfelelő ponthoz. Az arps-nek meg kell várnia, míg a pendant visszajelez, hogy tudomásul vette a vezérlés elvételét (ez egy egyszerű
25
reply()-al történik). A vészleállítás a szervó hajtások azonnali tiltását és a fékek meghúzását vonja maga után, valamint értesít minden egyéb processzt erről. Jelenleg ez után, valamint ctrl-brk után a szoftverrendszer sajnos még lefagy. A pathgen processz valósítja meg a Lantos könyvben leirt negyedfokú, folytonos gyorsulásos pályatervezést. Egy szemaforral zárják ki kölcsönösen a pendantal magukat a robot együttes mozgatása miatti problémák alól. Az arps processz indítja a rendszert, spawn()-olja az összes másik folyamatot, majd belekezd a parancs-sor jellegű interpreter működésbe. Ennek keretén belül memóriában tárolt programot is tud futtatni. A robotot direkt nem mozgatja, csak a pathgen-nek küldött üzeneteken keresztül. A robot digitális kimeneteit, és a megfogót közvetlenül irányítja. A megfogó állításara a pendant is képes. Ehhez a témakörhöz tartozó ábrákat tartalmaz a függelék, melyeket méretük miatt nem lehetett ide beilleszteni. Lássuk hát sorra az egyes processzeket részletesen:
2.4.1 A kommunikációhoz kapcsolódó részek: arcrun, arcproc, robot.c Az arcrun program a Tevesz Gábor féle DOS-os arcmonitor QNX-es, Illetve QNX Windows-os utódainak az utódja. Lényegében egy csendes módban futó arcmonitor. [Huszti,1999 8.old.]. Része az arcmonit.c forrásmodul is. Igen kulturált hibakezelések közepette elindítja az ARC-kártyákat, és öntesztet hajt rajtuk végre. Az arcproc folyamat a kommunikációt taglaló részben elhangzott funkciókat látja el. Két, az IEX-ek által hívható interrupt handler rutinja van. Ezek üzenet fogadásakor lépnek életbe. Emellett egy végtelen ciklusban várakozik új, a Host proccesszei felől érkező üzenetküldési kérésekre. Ezeket kiszolgálja, és folytatja a várakozást. Az üzenetekről történő értesítést az interrupt handler proxy-val végzi. Egy így értesített folyamatnak üzenetet kell küldenie az arcproc számára, melyben kéri az üzenet továbbküldését. Furcsa, hogy a karakterbázisú kommunikációt nem fogadja. Jelenleg nem használt, de így debug jellegű printeket sem igazán lehet a TMS kódjába beszúrni. A robot.c modul teszi lehetővé egy processz számára, hogy a függvényein keresztül kultúrált robotmozgatási, szabályozási funkciókat lásson el. Ebbe tartoznak a robotot inicializáló hívások, a szabályozok beállítása, a robot számára mozgatás előírása azáltal, hogy új pályapontot küldünk el neki. A kalibrációt végző rutinok is ebben vannak. Ez a modul tulajdonképpen üzenetek kitöltését tartalmazza, amelyeket elküld az arcproc számára, mely a TMS-eknek küldi tovább ezeket. Az igazi funkciók a TMS forráskódjában vannak implementálva. Emellett a robot.c modulon keresztül lehet kezelni a robot digitális IO-ját (a megfelelő függvények a pclab.c függvényeit hívjak).
2.4.2 A Host programjainak forrásállományai, közös modulok
26
A robot.c modul része mind a három robot állapotát befolyásoló processznek: az arpsnek, a pendantnak és a pathgennek. Ezek közül az arps csak a digitális IO-t és a kalibrációt kezeli rajta keresztül, míg a pathgen csak mozgatja a robotot. (Normál működésben, mivel a robot_start() hívás az arps része, mely az összes inittel kapcsolatos funkció elvégzését igényli.) Ennek a modulnak a tárgyalásával elérkeztem arra a pontra, hogy a szoftverrendszerről egy ideig ne, mint egymással kommunikáló processzekről beszéljek, hanem a forrásfájlok szerepét taglaljam. A forrás fájlok a jelenlegi munkakönyvtárakban a következő struktúrában helyezkednek el: • • •
Iexforras/: iexpass.exe forrásfájljai. Tms/ : a TMS-éi. Host/: a Host programrendszere.
Ezen belül az include fájlok mind az include/ alatt vannak. (A make fájl megírása így egyszerűbb.) Minden processz számára van egy könyvtár a csak benne használt forrásmodulok számára. Az arps-nek két könyvtára van arps_main/ és arps_func/ néven. Az arps_func/ tartalmazza az interpreter által értelmezett utasításokat végrehajtó függvényeket, míg az arps-main/ a többi forrásfájlt. A common könyvtár alatt vannak azok a .c forrásfájlok amelyek több processznek is részei. Ez alatt találhatóak: • robot.c, a robotfunkciók kezelésére, arps, pendant, pathgen része. • geometr.c a robotmodellekkel kapcsolatos függvényeket tartalmazza, arps, pendant, pathgen része. • mymath.c, a vektor műveleteket tartalmazza, arps, pendant, pathgen része. • debug.c, mellyel minden processz saját log-fájlba írathat vektor mennyiségeket. • puma.c, mely törlendő, összes függvényét a geometr.c-be raktam (nagy kavarodás volt). • arc.c: Az ARC kártyák fizikai eléréséhez szükséges rutinok és címek vannak benne. • pclab.c: mely a PC-Lab kártyára kötött részek elérését implementálja, szükséges a kalibrációhoz és a digitális IO kezeléshez. Csak a robot.c hívja a függvényeit. • robotfake.c : A robotszimulátor számára íródott. • arccom.c : Az arcproc processz szolgáltatásait ennek a modulnak a rutinjaival lehet elérni. • arcmonit.c : Ez a fájl csak az arcrun processz része. Külön említést érdemel a com.h, melyet Huszti Ákos úgy írt meg, hogy azonos lehessen mindhárom platformra fordított forrásban. [Huszti 24.old] Jelenleg a TMS\ könyvtárban van az igazi, a másik két platform megfelelő fájlja erre tartalmaz egy linket (egy include-ot). Azért pont a TMS-é az igazi, mert a TMS-könyvtár DOS partícióra másolandó a fordítás előtt. A robotszimdir/ alatt található forrás a makerobszim makefájl futtatásakor van csak felhasználva. Ekkor egy olyan processz jön létre, ami helyettesíti a jelenleg a TMS-eken futó programot, így amennyiben az arcproc minden TMS-nek címzett üzenetet ennek 27
küld el, úgy hatékonyan szimulálható a robot a vezérlőszekrény bekapcsolása nélkül. Munkám alatt nem próbáltam futtatni.
2.4.3 A Host QNX-ütemezési beállításai Az ütemező a QNX default beállításaival fut, nem állítja a rendszer. Ez egy adaptív ütemezési algoritmust jelent, lehet, hogy érdemes lenne FIFO-ra átkapcsolni. A következő processzek futnak megemelt prioritáson: • Pendant: indulás+6 • Messenger : indulás+4 • Pathgen: indulás+3
2.4.4 A szabályozásban kulcsfontosságú dynjac processz A dynjac processz jövőbeni esetleges szerepéről szól az utolsó fejezet Host-Control bekezdése. Jelenleg 10-es (indulási) prioritáson fut. Egy 4ms-os ütemezésű timer küld neki proxit, mire blokkolt állapotból futásba kezd. A CBUP-okból kiolvassa a pozíció, illetve a sebesség bemenet értékét és ezt a joint_shm osztott memóriába írja. Ezeket az értékeket csak ő írja. A szabályozás hibáját átlagoló kódrészlet is van benne, ez a paraméterek állításánál fontos lehet. Az értékek exportálása után kiszámolja az összes dinamikai-kinematikai robot-jellemzőt. Itt megemlítem, hogy a Coriolis erőt számító rész cserélendő a Stanford-cikkben levőre (a geometr.c modulban). A CTM számításhoz szükséges dinamikus modell integerre konvertált értékeit a CBDOWN-okba másolja, de a CTM szabályozási algoritmusok nem ezt használják, hanem a saját maguk által használtat. Meg kell jegyeznem, hogy a csukló-egymásrahatás nincs benne a modellben, illetve, hogy a modell H mátrixának alsó 3*3-as része azt sugallja, hogy az inkább a valódi mechanikai, mintsem a virtuális geometriai csuklóváltozókra vonatkoznak. Így a TMS-eken jónak tekinthető a modell, ha a Hoston akarjuk felhasználni, akkor az egymásrahatás miatti lineáris transzformációt rá kell ereszteni. A számítások végeztével dynjac végtelen ciklusának elején levő receive() hívás után blokkolt állapotban várakozik proxijának beütésére.
2.4.5 A robotot mozgató processzek: pathgen és pendant Pendant: Az eredeti vezérlőszekrényhez szállítottak egy szintén Nokia gyártmányú betanítópultot, mely a Host PC alaplap COM portjára van dugva. (rs-423 vs. rs-232: [Donát,1996. 60. old.]) A kézivezérlőt (teach pendant) kezelő pendant processz megalkotása Somlói Dániel diplomamunkájának volt a része. [Somlói,1999,37-40. old.]. A kézivezérlő és a robotprogramozást megvalósító arps együttműködését az eredeti szféra vezérlő által megvalósított módon implementálta. A kézivezérlő feladata a robot gyors mozgatása a számokkal előre nem meghatározható helyzetekbe. Erre akkor van 28
szükség, amikor a robotnak új környezetben való munkavégzéshez kell meghatározó helyzeteket megtanulnia. Általában a robotot vagy az arps interpreter, vagy a kézivezérlő irányítja. A kettő együttműködésének, egymásra hatásának két esete van: Az lteach utasítás esetében, amikor lényegében a kézivezérlőé a robot irányítása, az arps dolga az, hogy a kézivezérlő megfelelő gombjának magnyomásakor az aktuális helyzetet új pontként eltárolja az interpreter memóriájában. A másik hatás a két magas szintű robot-mozgatás között, hogy az alacsonyszintű mozgatást befolyásoló állapotváltozókat (base, tool koordinátarendszer értéke, geometriai konfiguráció) csak az arps-en keresztül lehet megváltoztatni, de állapotuk a kézivezérlő által kiadott bizonyos mozgatások kimenetelére is hatással van. Megoldottam, hogy ezeket az állapotváltozókat ne lehessen a robot mozgása közben változtatni. A világkoordinátás mozgáshoz szükség van a robot abszolút pozíciójának az ismeretére. Ezért ezekre a kézivezérlő csak kalibrált állapotban hajlandó. A robot kalibrált állapotát is szebb lenne a geometriai osztott memóriában tárolni. Jelenleg ezt a változót mindkét érintett processz saját globális változójaként tárolja. Mivel csak az arps képes kalibrációra, ezért a két változó közötti megfeleltetést meg lehetett oldani azzal, hogy a sikeres kalibráció végén egy üzenettel jelzi az arps a pendant számára az állapot változását. A pendant processz induláskor fájl-ként megnyitja a COM portot, és 65 ms-ra felprogramoz egy timert, ez fogja periodikusan kihozni a blokkolt állapotból. Ilyenkor elküldi a kézivezérlővel kijelezhető állapotváltozók tartalmát, lekérdezi a pendant gombjainak és sebességállító regiszterének az állapotát, és végrehajtja a gombok által kiváltott akciót. A blokkolt állapotba való visszatérés (receive( timer-proxi);) előtt megnézi, hogy az arps küldött-e módváltást eszközölő üzenetet. Általában bármilyen arps akcióra (interpreter parancs végrehajtás vagy program futatása) azonnal elengedi a robotot és WAIT módba vált. WAIT módból COMP módba vált vissza, ha az arps ismét tétlen, azaz egy konzol parancsra várakozik. A forráskódban az üzenet feldolgozásának implementálása a végtelen ciklus elején helyezkedik el. Ezután következik az időzítőre való várakozás, a COM port kommunikáció, majd a kért akció végrehajtása : (ha nem WAIT vagy PROGRAM-RUN módban van) vagy módváltás, vagy a robot mozgatása az aktuális módnak megfelelően. A csuklók elengedése esetén a SZFÉRA vezérlőt újra kell kalibrálni. Mivel nálunk a tacho interruptok működését a FREE mód nem befolyásolja, ezért erre nincs szükség. A robotot csak akkor szabad mozgatnia, ha övé a robot_mozgás_joga szemafor. Erre persze csak feltételesen várhat, mert a kézivezérlőn levő vészleállító gombot illik a lehető leggyakrabban (65 ms) lekérdezni. Vészleállításkor egyből kiadja a PC-ab kártya által a megfelelő hardver jeleket, majd ezután értesíti a szoftverrendszer többi egységét. A szoftverrendszer fejlesztése alatt tanácsosabb a vezérlőracken levő vészleállítót használni, mivel a pendant-é szoftveres, a QNX esetleges fagyása esetében nem jut szóhoz. Amikor a szoftverrendszernek egy kipróbált verziója fut, akkor meglehetősen biztonságos a használata.
2.4.6 Pendant-arps kommunikáció A pendant és az arps processz kétirányú kommunikációja teljesen szétválasztott. Mindkét irányú kezdeményezés esetében megoldott, hogy a pendant ne blokkoljon. Ha
29
az arps kezdeményez üzenetet, az rendszerint a pendant állapotának megváltoztatására irányul. Ilyenkor az arps a send hívás után blokkolódik, a pendant max. 65 ms elteltével feltételes (blokkolásmentes) Creceive() hívással veszi az üzenetet, befejezi a robot mozgatását, illetve a robotot az arps számára mozgatható állapotba hozza, ezek után egy reply() hívással jelez vissza az üzenet vételének és feldolgozásának megtörténtjéről. Az arps ekkor elkezdheti saját parancsa futtatását. Hiba esetén a kommunikáció kezdeményezője a pendant processz. Négy esetben van szükség erre: a vészleállítás megtörténtekor, az rs-232 kommunikáció megszakadásakor, a program leállító OFF gomb megnyomásakor, illetve az lteach parancs használata esetében a tárolandó pozíció jelzésére. A blokkolás-mentes kommunikációt Lovasi Péter oldotta meg. Létrehozta a viszonylag magas (indulás+4) prioritású messenger processzt, mely a pendant minden üzenetére egyből válaszol, így biztosítva a pendant blokkoltságának gyors megszűnését. A messenger tárolja a legutolsó az arps-nek szánt üzenetet, osztott memóriában jelzi annak létezését. Az arps az osztott memória pollingolása után egy üzenettel kéri a neki szánt levél továbbküldését. Mivel a pendant egy karakteren kódolja a négyféle üzenetet (S: Emergency Stop, E: rs-232 Error, O: OFF gomb, P: lteach tanítás), így egy osztott memóriás byte-tal és egy proxy küldésével is meg lehetne valósítani a kommunikációt, amely nem blokkolná a pendantot arra az időre sem, amíg a messenger ütemeződik.
2.4.7 Pathgen-pendant kizárás a szemaforon keresztül A robot alsóbb szintű irányítása által kínált szolgáltatásokat a pathgen és a pendant használja. Azért, hogy ezek biztosan ne irányíthassák egyszerre a robotot, illetve, hogy a Descartes mozgást befolyásoló állapotváltozóikat ne lehessen állítani a robot mozgatása közben. Ez a geometriai osztott memóriában helyezkedik el a base, tool mátrixokkal együtt. A pathgen processz és a mátrixokat állító arps blokkolva várakozik erre a szemaforra, ha nem az övé, míg a pendant feltételesen próbálja foglalni, és addig nem hajt végre semmilyen akciót, amíg ez nem járt sikerrel. Ha az arps a robot elengedésére szólítja fel a pendantot, akkor az a szemafort is elengedi, amennyiben sikerült megszereznie.
2.4.8 A pathgen A pathgen processz a [Lantos-könyv (4. Fejezet, 139-169. old.)] által ajánlott negyedfokú pályatervezési algoritmust valósítja meg mind csukló, mind Descartes interpoláció esetében. Az arps interpreter csak a pathgen által nyújtott robotmozgatási szolgáltatásokon keresztül mozgatja a robotot. (Kivétel persze a kalibráció, amely nem minősül a robot igazi mozgatásának, legfeljebb megmozdításának lehetne nevezni.) A processz Somlói Dániel keze közül került ki. [Somlói,1999, 23,31-36.old] Az inverz geometria konfigurációival kapcsolatos problémákat kezeli, az inverz geometriát megoldó geom_invgeo_rad() függvényt az egyenes-interpoláció által megkívánt módú működésre felparaméterezve hívja.
30
2.4.9 Pathgen arps kommunikáció A pathgen processz üzenetekben veszi a legújabb pontelőírásokat az arps-től. A pályatervezés új pont érkezésekor indul el. Ha nem ezé a processzé a robot_mozgás_joga szemafor, akkor blokkolva várakozik annak felszabadulására. Első lépésben kiszámítja a gyorsító, állandó sebességű, és lassító szakaszok polinom paramétereinek az értéket, ezután indítja az interpolációs ciklust. Somlói Dániel fontosnak tartotta az arps processz folyamatos működését, mert a vészleállítást követően ő értesít minden egyéb programot, mely a robotvezérlő bármely processzorán fut. Így a pathgen igyekszik nem várakoztatni az arps-t, az általa kapott teljesítetlen pontelőírásokat memóriájában tárolja. Ennek az arps-interrupt beütésekor isszuk meg a levét. A rendszerről levő ismereteim szerint az arps processz nyugodtan blokkolható lenne, ha a teljes rendszer indítása és lelövése egy kifejezetten nem blokkolható processz feladata lenne. Ez lehetne akár a messenger, akár a dynjac. Amennyiben a dynjac szabályozó processzé lépne elő, úgy mindenképpen őt kellene elsőnek értesíteni a vészhelyzetekről. A pathgennek csak a következő pontelőírást kellene átvenni az arps-től, azt is csak a lassító szakasz megkezdése előtt. Így egy interrupt rutin lefolyása után az arps-nek csak egy mozgást kellene újra elküldenie. [Somlói Dániel diplomatervének 50. oldalán] ír az arps-blokkolás elkerülésének az okáról. Így utólag könnyen látszik, hogy az interrupt-bemenetek egy másik processz általi pollingolása egyszerűbb megoldáshoz vezetett volna. De 1999-ben még nem létezett a messenger processz, és a dynjac is kísérleti stádiumban volt. Egy teljesen új, inicializáló-felügyelőlelővő processz létrehozása is alternatív megoldást jelentene. Ezzel mind a messenger, mind a console feladata szükségtelenné válna, így a processzek száma csökkenhetne. Szoftvertchnológiai megfontolásból is jobb lenne a rendszer-indítási és felügyeleti szerepkört elvenni az interpretálást végző processztől. Jelen megvalósításban lehetetlen a mozgás közbeni interrupt utáni megfelelő folytatás. Az AUT tanszéken tartott mérési feladat az interrupt utáni pont újbóli kiértékelését igényli. Ezt a SZFÉRA-36 rendszere meg is teszi. A Somlói féle verzióban külön-külön volt az egyenes illetve a csukló interpoláció teljes ciklusa lekódolva. Ezt olyan mértékben vontam össze, amennyire csak lehetett. Az interrupt működését is Somlói Dániel interpretálta: [Somlói, 1999 15. old.]
2.4.10 A rendszert elindító arps processz A szoftverrendszer a QNX konzolon az aktuális programrendszer Host könyvtárából indul. Itt az arps futtatható állományt kell indítani. (Jelenleg ./arps begépelésével, mert az aktuális könyvtár nem része a path-nak.) Ez a processz indítja az összes többit, illetve a közös modulokban (robot.c, geometr.c, stb.) ő hívja az összetettebb inicializáló és kilepő függvényeket, míg a többi processz ezeknek a moduloknak a bejelentkezés jellegű inicializáló rutinjait hívja. Az arps processz jelenlegi állapotában Jász Gábornak [Jasz,1996,X-Y.old] és Somlói Dánielnek tulajdonítható. Induláskor létrehozza a csukló-osztott memóriát (joint_shm), a robot-start() és a geometr_start() hívásokkal létrehozatja a két modulban implementál funkciók működéséhez szükséges elemeket. A robot_start() hívás indítja az arcrun processzt, 31
létrehozza az arcproc-ot, és elindítja a dynjac folyamatot és a csuklók szabályozását bekapcsolja. Ezek után az arps main() a control-break hatását átirányítja saját kezelőrutinjára, indítja a console, pathgent, pendant, messenger processzeket, elindítja a robot szabályozását, végtelen ciklusba kezd (UseConsole() hívás), majd annak kiszállásakor fordított sorrendben töröl mindent. A programrendszer leállásának menete két hibát is tartalmaz: a cntrl-break megnyomása fagyáshoz vezet, illetve a deinit olyan állapotban hagy valamit a rendszerben, hogy az első újraindítás arc-init failed hívással kiszáll, de a második már sikeres. A cntrl-break utáni holt állapot másik QNX-konzolról lelőhető, ekkor az arps és a console processzeket kell killelni. Egy ilyen QNX-általi lelövés után az első újraindítás sikeres. Ezeknek az okát nem derítettem ki.
2.4.11 Az arps működés közben, kommunikációja a console és messenger folyamatokkal Inicializálás után az arps is végtelen ciklusba kezd, a messenger processz által állított közös memóriabeli változó pollingolásával fogadja a neki címzett üzeneteket. Ha ez egy utasítás a konzolról, akkor meghívja a parancssor-feldolgozó rutinját, ez szükség esetén indítja az interpretert. Ha ez az üzenet a betanítópultot kezelő pendant processztől jött, akkor kezeli a különleges esetet. Végeredményben ennyiből áll a ciklusa. A különböző típusú arps utasítások végrehajtó törzsét tartalmazó források az arps-func könyvtárban vannak. Az arps-main könyvtár forrásfájlai között van a főciklust tartalmazó program (arps.c), a parancssorok feltördelését végző str(ing)proc(essing).c, a memória dinamikus kezelését megvalósító memproc.c, a robot mozgató üzeneteket a pathgen felé postázó (move_to() függvénnyel hívható) robproc.c, a kombinált pontok transzformálását végző loc(ation)proc(essing).c, illetve a processz globális változóinak (pl. utasitás-táblázat) feltöltését tartalmazó global.c. Véleményem szerint igen nagy munka lett ennek az interpreternek a kódolásába fektetve, de a speciális robotmozgató utasításaitól eltekintve egy commodor basic által nyújtott nyelvi szolgáltatásokra képes csak. A mai C-fordítókhoz szokott programozók számára különösen szembeötlő a memóriakezelés nehézsége, tömb változók hiánya, struktúrák hiánya. A programozható interrupt túlmutat a commodore basic szintjén, de a függvények, szubrutinok (programok) nem paraméterezhetőek, matematikai függvényeket nem használhatunk, sőt, a kombinált pontok programozása is sokkal kényelmesebb és egyértelműbb lenne koordináta-keret kombináló függvények alkalmazásával. Mindezek a ’hiányosságok’ semmiképpen sem róhatóak a processzt kódoló hallgatókra. Az említett hiányosságok pótlására még legalább ennyi, ha nem több munkát kellene az arps interpreter-forditó továbbfejlesztésére fordítani. Éppen ezért, és a hatkomponensű-erőmérő installálása után bevezetendő hibrid pozícióerőirányítás által igényelt nyelvi bővítések beágyazása miatt javaslom a robotprogramozási nyelvnek a C-nyelvbe történő beágyazását. Erről a későbbekben írok bővebben. Emiatt a terv miatt nem mélyedtem bele az arps processz megértésébe, hibáinak a feltárásába. A jelen verzió működőképes, akár rekurzív szubrutinhívásra is alkalmas [Somlói, 1999, 18-20. oldal], a külső editorral szerkeszthető programok és az elmentett robot-helyzetek gond nélkül tölthetőek be vele. Az interrupt programozása ütközik a fentebb említett nehézségekbe. A probléma nem kerülhető el narrow
32
mozgatások használatával, mert igaz, hogy ilyenkor az arps minden mozgatás után várakozik, lehetővé téve a recovery-t, de a várakozás közben nem pollingolja a digitális bemeneteket, így a megszakítás nem juthat érvényre.
2.4.12 A console A console processz kisajátítja az aktív QNX-konzolt. A QNX alatt megvalósított getch(), stb. függvény, mint minden más OS jellegű szolgáltatás, a mikrokernel folyamatközti kommunikációs megoldásait használja, így tulajdonkeppen egy sendreceive-reply hármas valósul meg a fájlkezelő-processzel együttműködve. Emiatt a console blokkolódik a gets() hívást követően. A console processz feladata az arps megóvása a blokkolástól a vészleállítás után szükséges üzenetszórás megvalósíthatósága miatt. A console processz eredeti működéséről Somlói,1999,4748.oldalon olvashatunk. Ez még a régi verzió működését tárgyalja, Lovasi Péter messenger processzének közbeiktatása kicsit megváltoztatta a folyamatokat.
!!! printf(“”,...) hívások helyett fprintf(stderr,””,...) hívások használandóak, mert ezek tartalma azonnal a képernyőre kerül, míg a normál printf valószínűleg a console blokkoltsága miatt várakozni kényszerül. !!!! 2.4.13 A messenger Ezt a processzt Lovasi Péter hozta létre. Szerepe a pathgen által az arps féle küldendő üzenetek miatti blokkolás elkerülése. Mint már említettem egy osztott memóriabeli byte-tal és proxi használatával is megoldható lenne ez a rész. Az arps felé érkező üzeneteket tárolja, és osztott memóriabeli változóval jelzi az ARPS felé, hogy jött üzenete (akár a pendant-tól, akár a console-tól). A pendant felől jövő 4-féle kód belefér az osztott memóriába, ezért az üzenet lekérdezését az arps csak akkor kezdeményezi, ha a console-tól jött egy parancssor. Ekkor az arps-részéről egy send hívás adandó ki, minek hátasára a messenger reply()-al küldi a raktározott üzenetet.
2.4.14 A kalibráció, a pendant működésének függése a kalibrációs állapottól Mivel a robot inkrementális adókból olvassa a pozícióját, szükséges minden bekapcsolás után a kalibráció elvégzése. Ebben az esetben a SZFÉRA működését követi. A pendant processzbe beleírtam, hogy nem hajlandó Descartes koordinátában mozogni kalibrálatlan állapotban. Eddig csak a kalibrációs állapotot jelezte ki, valamint kiindult abból a feltételezésből, miszerint a robot bekapcsoláskor ready helyzetben volt. Az arps interpereter ezt még mindig megteszi. Azaz hajlandó eltárolt pontokba csuklóinterpolációval elmenni kalibrálatlan állapotban. Szerencsére Descartes 33
mozgásra nem hajlandó. A megfelelő megoldás az lenne, ha csak parancssorba irt relatív csuklómozgatást, valamint kézivezérlő csuklómozgást engedélyezne. Tiltani kellene a GO pontnév típusú mozgást, az lteach utasítást, a program futtatását kalibrálatlan állapotban. A kalibrációval Somlói Daniel foglalkozott behatóbban. [Somlói, 1999,41-44. old.] A robot-start függvénybe beleírtam, hogy megpróbálja a körülbelüli abszolút pozíciót meghatározni a robot megmozdítása nélkül. Ez úgy veszi, mintha nullimpulzuson állna a robot, és a kalibrációs fájl tartalma szerinti feltételezett csuklókoordinátákkal indítja a tachométereket. Ez a megoldás csökkenti a kalibrálatlan állapotban még hibásan megengedett utasítások végrehajtása miatti hibákat. A kalibrációs adatokat felvevő segédprogram jelenleg nincs meg a winchesteren. A forrás állományok megtalálhatóak a diplomaterv lemezmellékleteken. Jobb lenne pótolni a segédprogramot két arps utasítással. Az egyik a szállításkor használt rögzítődarab segítségével segíthetne pontosan ráállni a ready helyzetre, míg a másik feltételezve, hogy ready helyzetben áll a robot, felvehetné a csuklónkénti kalibrációs diagrammot, és a paraméterként kapott nevű fájlba írhatná azokat.
34
3 .fejezet: A robotvezérlő szoftverrendszeren elért fejlesztési eredményeim 3.1 A robot laborban a diplomatervezés végzett munkám összefoglalása
megkezdéséig
1999 őszen kezdem meg szakirányú tanulmányaimat az informatikus szakon. Alfa szakirányom intelligens Robotok, béta szakirányom intelligens rendszerek lett. Pindzsulya Béla évfolyamtársammal a tanév tavaszi félévében már önálló laboratóriumi munka keretében az AUT tanszéken ismerkedtünk a QNX operációs rendszer kommunikációs megoldásainak időbeni lefolyásával, programozásával. Ekkor készítette diplomatervét Sárközy Ferenc és Lovasi Péter. 2000 tavaszán észrevettem, hogy a BME-vezérlőre kötött robot sokkal lassabban mozog, mint a SZFÉRA által irányított. Lovasi Péter rámutatott, hogy nincs szándékos, fejlesztés miatti lassítás bekapcsolva, ezért valószínűsíthető volt, hogy a szembeötlő sebességkülönbség valami hibának tulajdonítható. A probléma okát sikerült kiderítenünk. Bővebben a robotparaméterekről szóló bekezdésben ecsetelem a problémát. Második féléves önálló laboratóriumi munkám során (2000 ősz) feltérképeztem a Host processzor meglévő programrendszerét, kiemelve a választott kommunikációs megoldásokat. Emellett beleírtam a szoftverrendszerbe a csukló egymásrahatás figyelembe vételét. Ekkoriban merült fel az egész rendszer átsrukturálása, de mivel nem lett volna rá elegendő időnk és energiánk, inkább nem vágtunk bele. Az akkor megvitatott problémákat Fodor Gergő Doktoranduszi beszámolójában írta le. Ezek közül azokat a változtatásokat, amiket be lehet építeni a meglévő rendszerbe, megkíséreltem megvalósítani. (A külső processzból hívható robotműveletek (C for robot), egységes mértékegységek a szoftverrendszeren belül és a Hoston történő szabályozás). 2001 tavaszán nem diplomáztam még, de bejártam a laborba dolgozni. Kiderítettem a mozgások végen levő sebességugrások okát, valamint egyszerűsítettem a pathgen processz interpolációs ciklusát. Megtálaltam a TMS illetve IEX forrásmodulok legfrissebb változatát a könyvtár-dzsungelben. Pindzsulya Bélával beüzemeltük a tanszék (AUT) által beszerzett új robotvezérlő Host PC-t. A robotvezérlő bármely platformon való kipróbálása érdekében 4 OS-t próbáltunk telepíteni a gépre. A 8.4 milliárd (pontosan 8 Giga) byte (24-biten előjelesen ábrázolható legmagasabb szám) kálváriájának megmászása után ez sikerült is. (A merevlemezt kb 5-ször partícionáltuk újra, alkalmanként általában a 4. operációs rendszer telepítése után mondott csütörtököt a master boot record.) 2001 őszén kezdtem meg a munka java részét. A rendszer futtatása, tesztelése során felmerült egy sor probléma, melyek alapján elterveztem a féléves munkát.
35
3.2 A diplomatervező félév megoldott problémák
elejéig
felmerült,
addigra
3.2.1 Az egymásrahatás, a 2000 tavaszán levő állapotokról, a csukló egymásrahatás miatti lavináról, D-H alak, zeroready, szinguláris helyzet, virtuális csuklók, valódi motorszögek, az SI mértékegységek A 1999 tavaszi félévnek a vége felé az eredeti dokumentáció gépészeti rajzait szemlélve feltűnt, hogy PUMA 560 három kisebb motorja nem direkt a csuklószögeket befolyásolják, hanem az egyszerű mechanikai konstrukció miatt az elmozdulások a motorelfordulások kombinációjából adódnak ki. Pontosabban a 4-es motor direkt mozgatja a 4-es csuklót. Az ötös és a hatos csukló fogaskerekei két kardántengellyel kapcsolódnak a motorjaikhoz. Ha a 4. csuklóval együttforgó nézőpontból szemléljük a robotkart, akkor álló 5-ös és 6-os motor esetében úgy néz ki, mintha a 4-es csukló elforgatásakor az 5-6-os motortengelyek (és a kardántengelyek vége) ellenkező irányban pont ugyanennyit mozdult volna el. Ezért a 4-es csukló hatása az 5-ös, 6-os csuklóra -1/v5, illetve -1/v6, ahol v5 és v6 a az 5-ös, illetve a 6-os áttétel. A 6-os csukló egy alaphelyzetben lefelé álló kúpfogaskerékkel kapcsolódik egy, ez 5-ös tengelyen szabadon (a 6-os motor által forgatott) elforduló nagyobb kúpfogaskerékhez. Ha a 6-os motor áll, (és a 4-es is), akkor egyrészt a nagyobb 5-ös tengelyen csapágyazott kúpfogaskerék is állva marad, és az 5-ös csukló elforgatása a kis, 6-os tengelyen levő kúpfogaskeréknek a nagyobb álló keréken való végiggördítését eredményezi. Ezért a 6os tengely az 5-ös csukló alfa szögű elforgatásával alfa*kiskerékfogszám/nagykerékfogszám arányban fog elfordulni.
36
Az egymásrahatás leírható két 3*3-as mátrix-szal. A virtuális (csuklók ténylegesen elvárt) szögeket q-val, a valódi (motorelfordulás leosztva az áttétellel, a motoroknak kiadandó ) syögeket i-vel jelölve a két mátrix a következő:
*
[i 4
1 − 1 v 4 − 1 v6 1 − 1 v56 1
i5 i 6] [q 4
q5
q6
]
1 1 v 4 * 1 [q 4 q5 q6] [i 4 i5
1 1 + v6 v56 ⋅ v 4 1 v56 1 i6 ]
v1= 62.35; v2=107.36; v3= 53.69; v4= 76.01; v5= 71.91; v6= 76.73; v56 ≈ 5.8; Az áttételek értékei és az egymásrahatási mátrixok (sorvektoros alakban) Ahol v4,v5,v6 az adott csuklók áttételezése (a robotparaméterek része), A v56 érték a 6-os csukló második (kúp)fogaskerék-kapcsolatának az áttétele. Ez utóbbit nem találtuk meg se hiteles, se kétes dokumentációban, így mérések alapján lett 5.8-ra állítva. Ez a probléma megoldatlan volt Lovasi Péter ismeretei szerint, és a robot mozgása is ezt mutatta. Az egymásrahatás problémáját sikerült következő szeptemberben egyszerűen javítani, de rámutatott a különböző csukló-vektor reprezentációk kuszaságára. A robotban a csuklószögek reprezentációja több kategória szerint is osztályozható: •
Mértékegység: radián, fok, vagy inkremens érték.
•
Nulla helyzet: Ready vagy Denavit-Hartenberg alaphelyzet (ábra)
•
Egymásrahatás alapján: csuklószögek vagy motorszögek.
37
(ábra: zero-ready vs. D-H nullhelyzet) Szerencsére a lehetséges 12 kombináció nem fordult elő mind, de lényegesen több konverzió volt a kelleténél, és egyes rutinokról csak figyelmes olvasással lehetett kideríteni, hogy melyik reprezentációt hansználják. Ezeket a problémákat teljesen csak az utolsó félévben tudtam kijavítani. A jelenlegi kiosztás: A TMS processzor motorszögekben számol (még a dinamikus modell esetében is), és inkremens értékekben. (CTM szabályozás által igényelt dinamikus modellnek D-H nulla helyzet, radián mértékegység és az egymásrahatás tekintetében inkább 38
motorszögek a bemenete. Ennek a számításakor egy konverziót végez. A konverzió paraméterei még nincsenek a Host-al egyeztetve). A TMS csuklóábrázolásának nulla helyzetét a kalibráció állítja be. Ez az arps feladata. Így nem véletlen hogy a TMS az arps-sel megegyezően zero-ready alakot használ kalibrált állapotban. A pathgen és a pendant radián mértékegységet használ. Főként deriválási megfontolásból, a felesleges szorzók elkerülése érdekében. Belső ábrázolásuk szintén zero-ready alakú. A Hoston az egymásrahatás tekintetében mindenki csuklószögeket ábrázol. Mivel a radián-inkremens átváltás (és fordítottja) olyan processzorok közötti kommunikáció (Host és TMS) esetében szükséges, amikor az egymásrahatás miatti konverzió is szükséges, így ezek egy rutinban végeztetnek el. A robot.c modul legalján levő robot_inc_2_rad(), rad_2_inc() függvények vannak ma leginkább használatban. Az arps a felhasználó miatti kommunikáció könnyítésére fokokban ábrázolja a csuklószögeket, emellett a másik két ábrázolási szempontban követi a HOST-ra általánosan jellemző ábrázolást (zero-ready és csuklóelfordulás). Az ehhez szükséges konverziókat (rad2deg,deg2rad) az arps végzi, jól kivehető módon. A Hoston egy helyen van még eltérő ábrázolás, ez a robotmodellek direkt számításához szükséges. A direkt geometriát számító dg_theor_06() függvény (geometr.c) az input csuklóvektort D-H null helyzetbe transzformálja, feltölti a s1,c1,s2,c2, stb. globális változókat, majd elvégzi a visszatranszformálást. A kinematikai-dinamikai modellek bemenetei a sinus-cosinus értékek. A [Stanford cikk] dinamikai modelljébe valószínűleg nem számolták bele az egymásrahatást. Valószínű, hogy ezen csuklók tengelyre képzett tehetetlenségi nyomatékának legnagyobb részét a motorinerciák teszik ki. Ha szemügyre vesszük a H mátrix alsó 3*3-as részét, akkor kijelenthetjük, ez a modell használandó, ha motorszög– reprezentációval dolgozunk, és az inverz egymásrahatási mátrixszal beszorzott alakot kell a Hoston használni. Az inverz geometriai feladat egyből zero-ready alakot ad vissza. A csuklóvektorok null-értékének létezik még egy változata. Ez pedig az alsó 3 csukló esetében abszolút szinguláris, azaz teljesen kinyújtott helyzet. Ebben a helyzetben csak az x-tengely menti sebességelőírás teljesíthető. Ennek a speciális helyzetnek az inverz geometriai probléma konfigurációváltásával kapcsolatban van jelentősége. Az arps részéről szükség van a geometriai modell direkt és inverz függvényeire. Erre a célra a geometr.c-ben a dirgeom_degree_mm(), illetve az invgeo_degree_mm() hívások használhatóak. Innen látszik, hogy az ARPS szintén (gépész) felhasználói szempontok miatt mm-ben számol, a többi processz méterben. Az megörökölt programrendszer mértékegységek tekintetében is hagyott kívánnivalót maga után. A távolságot mm-ben számolta, az időt ms-ban. így a sebesség mm/ms lett, a gyorsulás pedig mm/ms^2. A numerikus deriválások és integrálások esetében a szimulációs idővel osztani, illetve szorozni kell. Mivel a TMS ütemezése milliszekundumos, így a szimulációs idő 1 egységre (ms-ra) adódik. Emiatt a kódból (a szabályozó hurkokból) hiányoznak ezek a konstans szorzások. Ez csak akkor jelent bajt, ha a TMS ütemezése megváltozik. Akkor a dynjac processzben átírt módon kell a hibát kiküszöbölni: globális dt illetve dtrec (dt reciproka) konstansokat inicializálni, ezt használni az időzítő felprogramozásakor, és ezekkel szorozni a szimulációs lépésekben. (Ha értékük 1.0, akkor valószínű, hogy se a watcom, se a TMS fordítója nem fogja a felesleges szorzást végrehajtani.) A Host programrendszeren az SI
39
mértékegységekre átírtam a programrendszert. Egyedüli kivétel ez alól az arps processz, mert a nyelv specifikációja szerint mm-ben méri a távolságot.
3.2.2 A robotparaméterek körüli huzavona A robot áttételeit először szögmérés segítségével határozták meg. [Donát, 27. Old]. Időközben előkerült egy cikk, amelyet Donát a Saint Louis Egyetemről származónak jelöl, és ebben a BME mérésekkel 1% pontosságig egyező paramétereket találtak. Sajnos az orosz dokumentáció a tengelyekre nézve csak annyi információt közöl, amennyi Donát 28. Old található. A maximális szöggyorsulás oszlop értékei első ránézésre szembeötlőek voltak, hiszen a 0.9 fok/s2 érték alapján a 2. csukló 90 fokos elforgatásának min. 20 másodpercig kellene tartania. Mint később kiderült, az orosz dokumentáció műszaki fordítása közben csúszott hiba az adatokba, az eredeti dokumentumban cirill betűvel rad/s 2 mértékegység szerepel. Ebben a 2-es számjegy a 2. lapaljai megjegyzésre utal. A rad-ot grad-nak olvasták, így lett radián/sec-ból fok/s2, illetve maximális sebességből maximális gyorsulás. Ezt a problémát Lovasi Péterrel együtt próbáltuk orvosolni, és ekkor derült ki, hogy a szoftverrendszerben legálabb három-négy helyen vannak sebességlimitek definiálva. Ezt az állapotot mostanra sikeresen megszüntettem. A robot.c limitei jutnak érvényre minden modulban. Huszti Ákos diplomatervében közöl egy D-H modellt, melynek d3 paramétere igen csak eltér a valószínűleg 149mm-es értéktől. A D-H paraméterek kinyerhetőek a SZFÉRA-36 vezérlőszekrény ROM-jából, ha ready helyzetben kiadunk egy where parancsot, illetve a 3-as csukló 90-fokos forgatása után úgyszintén. A D-H paramétereket illetőleg én leginkább ezeknek az adatoknak hiszek. Fodor Gergely a BME könyvtárában rábukkant a következő cikkre: [Armstrong-Khatib-Burdrick,Stanford AI-lab,Stanford University:The Explicit Dynamic model and Inertial Parameters of the PUMA 560 ARM(továbbiakban Stanford-cikk)], mely az eddig rendelkezésre álló dokumentumok közül a leginkább hitelt érdemlő módon foglalkozik a kérdéses paraméterek meghatározásával. Egy eltérés van csak a D-H paramétereket illetőleg, a d3 paraméter 149.101 helyett náluk 150.01-re jön ki. Én inkább a ROM-nak hiszek, így most az van a programban. Dinamikus paramétereket a Stanford cikk is közöl, a szerzők igen szellemesen minden inerciamérést periodusidő-mérésre vezettek vissza, melynek pontossága a megszámlált periódusok számával tetszőlegesen növelhető, és nem igényel méregdrága berendezéseket. A szoftverben jelenleg e cikk alapján adódó értékek szerepelnek. Nem kell ügyelni arra, hogy az ő módosított D-H modelljükben odébb van a 2. csuklóhoz rögzített koordinátakeret, mert ez csak a gravitációs tömegközéppontot befolyásolná, de a gravitációs modell érzéketlen a 2. csukló tömegközéppontjának 2. tengely menten történő eltolására. A Robot modellel kapcsolatban még pár dolgot meg kell említeni: • -A BME arps rendszerben történeti okokból a csuklópozíciók=0 alak a readyhelyzet (ezt a kódban zero-ready alaknak nevezem), míg a SZFÉRA-36 vezérlő a ready helyzetre a D-H alakhoz képesti eltérést írja ki: 0,-90,90,0,0,0. • -A dinamikus modellbe célszerű belevenni a súrlódást, mert jelentős nagyságú. • -A gravitációs modell alkalmazása free-zett csuklók esetében működött akkor is [Lovasi Péter], amikor a g1 tag 37.2-es tagja előtt nem volt ott a negatív előjel. Ez
40
azért lehetett, mert a csuklóváltozók D-H alakba transzformálása helytelen volt. Q_d-H[1]=Q[1]-PI/2 helyett +PI/2 volt. Javítottam a hibát, q1-re és q3-ra teljesen teszteltem. A 2. csukló megtartását csak függőlegeshez közeli helyzetben teszteltem le. • -A coriolis tag az implementált modellben Huszti Ákos műve. Lantos Béla többször kifogásolta, hogy a [0][0] ([1][1]) indexű tagnak elméletileg 0-nak kellene lennie. A Stanford-cikkben közölt modellben ez annyi is. Jelenleg nem javítom ki a forráskódokat, mivel a tesztelésre nem lesz már időm. A probléma kijavítását meghagyom annak, aki kellő időt tud szánni a CTM szabályozás javítgatására. Amennyiben ezt valaki megteszi, ügyeljen arra, hogy a modell mind a Host geometr.c moduljában, mind a TMSpos.c modulban javítandó, és újrafordítandó! Fontosnak tartom megemlíteni, hogy a Lantos-könyvben közölt geometriai modell úgy számol, mintha a3 nulla lenne. Valamint, hogy a kódban szereplő modellben d6 néven benne van a megfogó felszerelési pont 456 tengelyközépponttól mért távolsága. Ezt a tool koordináta rendszer z-koordinatájához való hozzáadásával is lehetne pótolni, de történeti okokból nem így van. Ha valaki a későbbiekben a két modellt összeveti, ezt a két eltérést fogja találni. Amúgy a kódban a direkt geometriai alakot D-H alakban számoljuk, míg a jelenleg implementált geometriai alapú inverz geometriai függvény egyből zero-ready alakot ad vissza.
3.2.3 ROBOP Amikor 2000 őszen Fodor Gergővel és Márton Lőrinccel tanakodtunk a két szoftverrendszer-verzió közös jövőjéről, többek között szóba került a vektor és mátrixműveletek kuszasága. A robot-modellek körüli bizonytalanság mellett kapóra jött, hogy Fodor Gergő talált egy lineáris algebrai libraryra épülő robot-libraryt, amibe bele volt írva a többek között a PUMA 560-as modellje is. Fodor Gergő összevetette ezt a Matlab robotikai toolboxának modelljével, valamint a stanford-cikkel is, és megállapította egyezésüket. Ennek ellenére ennek az objektum-orientált mátrix-könyvtárnak a bevezetését messzemenőleg elleneztem mindig. A problémám a konkrétan választott roboplibraryval van. Ez ugyanis egy általános lineáris algebrai library-ra épül, ami c++-os módon dinamikus memóriakezeléssel kezeli a tetszőleges méretű mátrixokat. Egy olyan könyvtár bevezetése szerintem is olvashatóbbá, átláthatóbbá tenné a forráskódot, ami képes a c++-ban bevezetett operátor-overloadra, de mindenképpen fix méretű vektorokat és mátrixokat használó objektumok kellenek. Nem véletlenül írtam meg a mymath.c-ben egy-két ilyen rutin fix méretű (3*3, 4*3 (4*4 homogén)) változatait. Annak idején, egy kizárólag ekkora méretű mátrixokat és vektorokat használó grafikai projekt indulásakor a lineáris algebrai műveletek implementálásához minden lehetséges megoldást kipróbáltam, megnéztem a fordított gépi kódot. Ezek alapján jelentem ki, hogy robotikai alkalmazás esetén nem szabad változó méretű vektor, illetve mátrix osztályt használni. Az olvashatóság és a sebesség preferenciája alapján a megoldás az anticiklusos, fixen 3, illetve 6 dimenziós operator-overload függvények és a hasonló jellegű assembler makrók között van. Jelen esetben a c++ fordító hiánya perdöntő, így maradnak a függvényhívások. A forráskód olvashatósága miatt jól jönne néhány
41
tipusdefinició: 6-D integer vektor, 6-D float vektor, 3-D float vektor, 3*3 float mátrix, stb. Ha lesz cpp fordítónk, az operátor-overload előtt ezeket be kell majd vezetni.
3.2.4 A Relécsere, féklazító vezeték átforrasztása A robotvezerlő-szekrény összekötő paneljén állítódik elő az inkrementális adok 5V-os tápfeszültsége. Ha ez elszáll, az a robot biztos karosodásához vezet. Ezért erről a tápfeszültségről egy relét működtetünk, melynek áramkimaradása esetén lekapcsolnak a hajtások és a féklazító mágnesek is áram nélkül maradnak. Az eredeti relé gyenge minőségű volt, és sor került a cseréjére mindkét robotvezérlőn. Mivel a fékvezetékre nem volt ráírva, hogy negált (fék-elengedő) működésű, ezért a technikus rossz helyre forrasztotta be. Emiatt a robot bekapcsolása után nem engedtek ki a fékek, illetve a robot sárga lámpája nem kezdett villogni. Az átforrasztást az AUT tanszék vezérlőjén végrehajtottam, az IIT vezérlője még mindig nincs megjavítva. Ha a panel hátlapja (forrasztási oldala) felöl nézzük a relét, úgy a jobb alsó csatlakozóra kötött fékvezetéket kell a felette levő (jobb felső) csatlakozóra átforrasztani.
BRAKE Release
Servo ENABLE
(bekötési ábra)
3.2.5 A sebességszabályozó telítése, különböző paraméterek, a paraméterek TMS-re történő kötelező feltöltése, ennek hiányosságai Az egymásrahatás kiküszöbölése után a csuklókoordinátákban levő pályatervezés tesztelésébe fogtam. Alacsony sebességek mellett szépen mozgott a robot, de speed 42
250-es parancs után mindig egy rándulással fejezte be a mozgást. Szemre (és sajnos fülre) is megállapítható volt, hogy sebességugrásos a mozgás. Úgy viselkedett, mintha a lineáris szakasz után egy pillanat alatt megállítaná a robotot. Először arra gyanakodtam, hogy a pathgen processz időzítési gondokba keveredett. Hosszas log-fájl elemzés után derült ki, hogy a mozgás-végi csattanás a sebességszabályozó telítődése miatt van. Azaz, hogy a pályatervezés nagyobb sebességű alapjelet enged meg, mint amekkorát a szabályozó hajlandó kiadni. Ez úgy alakult ki, hogy a szabályozóban a régebbi lassabb, míg a pályatervezésben az új, gyorsabb konstansokat használta a rendszer. A TMS forrását előbányáztam a könytárlabirintusból, és megtaláltam benne az illető konstansok inicializálását. Először kerülgettem a TMS-ről való fordítás kipróbálását [Huszti 9. old-tól], ezért HEXA editorral próbáltam az integer konstansokat átírni. Ez sikerrel is járt, de a robot mégsem működött helyesen. Gondoltam, elírtam valamit (igazából nem), ezért kipróbáltam egy backup után, hogy sikerül-e TMS-re fordítanom DOS alatt. Könnyebben ment, mint gondoltam, egyáltalán nem érte meg a HEXA editorral szenvedni. De a fordítás után sem ment szépen a robot. Többszöri próbálkozás után azt gyanítottam, valaki felülírja ezeket a limitváltozókat, mivel még extrém lassú paraméterek esetén sem változott a robot mozgása. Megtaláltam a TMS forrásában a set-dcs-param üzenet törzsét. Ez írja felül majdnem az összes, TMS forrásállományban is értéket kapó paramétert. A Hoston implementáltam, hogy a senddcs-param hívás is ugyanabból a változóból vegye a csuklónkénti sebességlimitet, így egyel csökkent a forráskódokban tárolt sebességlimit definíciók száma. (Büszkén jelenthetem ki, hogy sikerült ezt a számot 1-re csökkenteni mostanára.) Jelenleg a robot.c modulban levő értékek a mérvadók. Emiatt a szívás miatt döntöttem úgy az utolsó félévben, hogy a TMS-eket nem hagyom elindulni addig, amíg nincs minden robotparaméter egyeztetve a Host-al. Úgyhogy a paraméterek TMS-forrás-beli inicializálását ki lehet törölni, nehogy mást is megtévesszen. Azért forráskód kommentjeibe is le van ez írva. A szabályozások megkövetelik, hogy a TMS kártyán konvertáljunk inkremensekről radián értékre. A TMS-en levő konverziós konstansok a legelső BME-mérésekből származnak, így nem azok, amikkel a Host számol. Ezeket a konstansokat is a robot.cben definiált értékekkel kellene felülírni. Ehhez bővíteni kell a dcs-param struktúrát. Ugyanígy a nyomaték-DA-egység átváltási konstansok (axis_curtorq[]) is beállítandóak lennének. Ezek után az egész szoftverrendszerben már csak két modul tartalmazna paramétereket: a robot.c modul és a geometr.c modul. (A dinamikus modell TMS-en levő megismétlését nem számítva.) Az nem baj, hogy az egymásrahatás a TMS-en levő inc-rad konverzióban nincs benne, mivel a TMS nem csukló, hanem motorszögekben szabályoz.
3.2.6 A Host OS-ei, installálás A Host processzor másodszori cseréje 2000 őszén merült fel, mikor a kereskedelemben kapható legolcsóbb processzorok is elérték az 1 Gflopos teljesítményt (2 FP alapművelet /clk@500MHz), illetve a megvásárolható merevlemezek mérete lehetővé tették, hogy az összes számításba jövő op. rendszert egyetlen gépre telepíthessük. Eredetileg egy 2 isa-slotos alaplapot terveztünk, mivel a buszbővítő ház
43
kártyája egyet elfoglal, és még egy maradt volna tartaléknak, de végül is csak egy 1 ISA-s kivitel érkezett. Félő volt, hogy az új energiatakarékos szabványok mellett nem biztos, hogy lesz elegendő áram a buszbővítő meghajtó-latch IC-k működtetéséhez. (a régi PC parallel portján jól működő HW kulcs áramköröket az új alaplapok nem látják el elegendő ’táp’-árammal.) Szerencsénkre az ISA szabvány nem jutott arra a sorsra, hogy a régi, energiatakarékosságot nem túlzásba vivő gépeken kipróbált és megbízhatóan működő perifériák a túlzottan takarékoskodó újabb alaplapokban nem működnek. A primary master HDD jelenleg 4 operációs rendszert tartalmaz: Egy 4.25-ös QNX-et, egy Neutrinot, egy Linuxot, amire lehet RT-linux kernelt fordítani a jövőben, és egy Windows98-at. Boot loadernek a linux lilo-boot programját kellett használnunk, mivel csak ez tudja a linuxot elindítani abban az esetben, ha a linux swap partíció logikai partíciónak van definiálva. Sajnos a rendelkezésünkre álló verzió nem tudott 8.4 Gbyte felettről bootolni, így a leghátsó partíció 12 GB-os. A neutrínónak lenne ilyen boot loadere, de az nem ismeri a linuxnak ezt a speciális elindítását. A neutrinó installálásával küszködnünk kellett egy kicsit, mire rájöttünk, hogy csak akkor hajlandó installálni magát, ha saját maga hozhatja létre a partícióját. Ez nem lenne különösebb gond, ha nem csak a következő 4 féle méret közül lehetne választani: rendelkezésre álló lemezterület, annak a fele, ötöde illetve tizede. Amikor egyéb fdisk jellegű programmal előre létrehoztuk neki a megfelelő kódú partíciót, egyszerűen nem ismerte fel. Időközben a Windows alá is felkerült a neutrinónak az a változata, amely egy partíción tud élni a Windowsal. Ez a két installálási mód sebességének összehasonlíthatósága miatt volt fontos. Sajnos nem emlékszem a jelenlegi installációt adó pontos metódusra, de a partíciók végleges kiosztásának és a megfelelő master boot loadernek a megtalálása volt az igazi probléma. A Qnx partícóra szinte semmilyen program nem lett felinstallálva, a régi gép wincseszterének tartalma lett átmásolva az 5 lemezes install után. Ezért nem működik a photon sem a gépen. Az is csoda, hogy a C fordító megfelelően működik rajta. Szerencsés lenne, ha idővel valaki felrakna rá minden programot. Időközben az emeleti hálózatot koax-ról UTP-re cserélték. Mivel a régi pentium gép hálókártyájának csak koax kimenete volt, az átmenetileg kiesett a hálózatból. Táblázat: A Host winchesterének partíciói: A partíció típusa Lilo boot Mérete és %karakter MB-ban, ban QNX Q 1004 4% Neutrino N 1757 6% Linux (Ext.Dos) L 5240 18% Win98 (Pri.Dos) D 20623 72%
3.2.7 A 2ndary IDE controller miatti probléma A Qnx sikeres Installálása után nem működött elsőre a robotvezérlő. Az AUT tanszéken a 10-es és a 15-ös IRQ-ra vannak jumperolva a kártyák. A 15-ös
44
megszakítás viszont a 2ndary IDE Controllerhez tartozik újabban. Elsőre úgy sikerült elindítani a robotot, hogy a BIOS-ban kikapcsoltuk a második vezérlőt. (E mellett a PnPmenüben Legacy-ISA opcióra kell állítani a 10-es és a 15-ös megszakítást.) Szóba jöhetett volna még a kártyák átjumperolása, de a legtöbb régi programba (pl. PotCal) bele vannak fordítva fixen a HW-re jellemző aktuális beállítások. Még az arps rendszerben is #define-al vannak ezek beállítva, annak ellenére, hogy régóta létezik a programkönyvtárban egy arc.ini fájl, de ezt egy modul sem olvassa. A legszebb megoldás az lenne, ha a gyökér könyvtárban keresné ezen túl minden program ezt a konfigurációs állományt. Így egy szükséges átjumperelés esetén csak egy helyen kell ezt módosítani, és minden program rendesen működne. A régi programok közül a potcal program az, amit kellene még használni. Sajnos ez nincs meg egyik gép egyik partícióján sem. (Donát Tamás 1996-os munkájának 66. Oldalán, valamint Huszti Ákos 1999. 66,69. Old. olvashatunk róla). A szükséges fejlesztésekről szóló fejezetben részletesen visszatérek a problémára. A mostani Host PC -ben egy 28 Gbyte-os winchester van fixen beépítve primary master meghajtónak. A primary IDE controller slave meghajtója egy cd-rom olvasó, de beépítettünk egy HDD dokkoló rack-et is. Ez a secondary ide controllerre van dugva. Jelen BIOS beállítások mellett Windows alatt működik a rack, míg QNX alatt megy a robotvezérlő. Táblázat: A fontosabb BIOS opcióik mostani beállítása Standard CMOS setup: HDD-s: Primary master (28 GB): Primary slave (CD): Secondary master (Rack): Secondary slave: Bios Features setup: Virus warning: CPU internal cache: CPU L2 cache ECC: ... Gate A20 option: Memory ECC: ... VGA Palette snoop: OS Select for DRAM>64MB: Shadow: ...
User Auto Auto None
... ... ...
LBA Auto Auto
Disabled Enabled Enabled Fast Disabled Disabled Non-OS2 Enabled Disabled Disabled Disabled Disabled ...
Chipset features setup:
45
Bank timing 0/1: Bank timing 2/3: Bank timing 4/5: SDRAM cycle length: Dram Clk:
AGP aperture: AGP 2x: USB: USB keyboard: Power management: ... Interrupts: ... 10: ... 15:
SDRAM 8/10 ns SDRAM 8/10 ns SDRAM 8/10 ns 3 Host Clk Disabled Disabled Disabled Disabled Disabled 64 MB Enabled Enabled Disabled
Disabled Disabled
PnP/PCI config: PnP OS: Resources ctrld. by: ... Mindegyik: Kivéve: 10: 15:
Legacy ISA Legacy ISA
Integrated Peripherals: IDE channel 0: IDE channel 1: Alatta 8 db:
Enabled Enabled AUTO
Serial Port:
NO MANUAL PCI/ISA PnP
3f8/ IRQ 4
!!! FONTOS !!! Nem érdemes kipróbálni, mit reagál a rendszer, ha be van dugva a rackbe egy winchester, és elindítjuk a robotvezérlőt. Minden Robot-működtetés előtt ki kell húzni a dokkolófiókot, és úgy kell felbootolni a rendszert !!!
46
3.3 A Diplomatervezési időszakra tervezett, illetve ekkor megoldott feladatok A 2001 őszi félév elején a rendszerben levő következő, szerintem javítandó dolgokat tartottam számon: • Vészleállás a csukló-véghelyzetekhez való túl gyors közeledéskor (eredetileg a TMSre terveztem) Probléma: a robotnak lehet olyan vezérlőjelet kiadni, hogy bár nem menne ki az engedélyezett tartományból magától, de a lendület ütközésbe viszi. Megoldás: csuklókoordinátákban egy térengedélyt adni, mindig megnézi, hogy arányos, 6d egyenes (csuklókban) mentén való vészfékezés esetén a szakasz érint-e tiltott zónát, ha igen, vészfékezés. • C-for robot library Probléma: a robotprogramozási nyelv változóhasználata nehézkes, lehetőségei korlátozottak. Megoldás: C_for Robot library létrehozása. • Inverz Rodriguez probléma megoldása Probléma: A már említett grafikai munkám keretében jött elő az implementált inverz Rodriguez képlet diszkussziójának hiányossága. Megoldás: A robotirányító rendszerben is le kell cserélni a képletet az alkalmazott új megoldásra, valamint el kell végezni az új megoldás matematikai analízisét. • Inverz geometria miatti konfigurációváltások kezelése, hiba javítása, üzembiztos egyenes vonalú mozgások. Probléma: Egyenes vonalú mozgatáskor a robot iszonyatos dolgokat produkál. Megoldás: Tisztázni az inverz geometriai konfigurációk miatt létező problémát, definiálni a kezelésének optimális megoldásait, ezt implementálni, tesztelni, debuggolni. • Csuklóváltozók mértékegységeinek egységesítése különböző hívási felületek esetében. Ahol lehet SI egységek, a deriválási szorzók elhagyása miatti jövőbeni hibák elkerülése érdekében Probléma: A programrendszerben az egyes fizikai mennyiségek ábrázolása kaotikus. Megoldás: egységesíteni az ábrázolási módokat, SI mértékegységek alkalmazása lehetőség szerint. Ha szükség van több eltérő reprezentáció használatara, úgy a különböző ábrázolások határvonalait a szoftverrendszer határaihoz kell igazítani. • Diplomamunkákból a vonatkozó kommentek átmásolása a forrásokba, modulok fejléceinek beszédes felkommentezése a forrásmodul szerepét illetőleg. Probléma: A szoftverrendszer szűkölködik a rutinok, modulok, processzek szerepét magyarázó kommentekben. Megoldás: mivel ezek az információk szépen tagolva
47
megtalálhatóak a diplomatervekben, így ezek drag&drop technikával a megfelelő modulba másolandóak. Ezek közül a C-for robot library csak tervezés szintjen valósult meg. A Kommentezés pedig nem lehet befejezett, bár néhány helyen elég bőbeszédűre sikeredett.
3.3.1 A Platformok, Visual C A 3 régebbi platform fordítási problémáiról olvashatunk: [Huszti Akos,1999, 9-21.old.]. Az eddigi forráskód editálás a QNX alatti vedit editorral történt. Ennek C-specifikus funkciója a zárójel párra ugrás (Go to matching brace). Ha photon grafikus rendszer alatt fejlesztünk, lehetőség van a nagyobb ablakméret használatára, de az új gépre a photon nem lett még installálva. Mivel a Microsofttól megszokott szoftverminőséghez képest lenyűgöztek az MS VisualC szerkesztőjének képességei, úgy döntöttem, a nagyobb lélegzetvételű változtatásokat otthon nem QNX, hanem Windows alatt gépelem be. Mivel a roboton történő kipróbálás előtt rengeteg dolgot kell ellenőrizni, így egy jobb környezet előnyei nagyobbak annál a hátránynál, minthogy a teljes rendszer nem futtatható. A módosított függvények tesztelhetőek és debuggolhatóak egy egyszerű tesztprogram keretében. Létrehoztam VC 6.0-alatt a szoftverrendszert leíró projekt-fájlokat. Valamint a szükséges QNX include fájlokból dummy-verziókat kellett elkészíteni, mivel az eredeti QNX fájlok összevesznek a VC include fájljaival. Az inverz geometriai problémák magoldásának implementálása közben igen jól működött ez a módszer. Melegen ajánlom mindenkinek ezt a megoldást. A syntax-highlight-nak nevezett funkció jelentősége akkor tűnt csak fel, amikor kezdtem sejteni, hogy a csak vedit-tel dolgozó elődök valószínűleg azért kerülték a megjegyzéseket, mert így is alig lehet megtalálni a 25 soros ablakban a keresett kódrészleteket. VC 6.0 következő funkciói segítettek igen a munkámat: Bookmarks(F2), Complete word(ctrl-space), függvény-deklaráció-kijelzés, struktúra-tag lista, valamint a file-view ablak általi könnyű navigálás a forrásállományok között. Ha valaki szeret a kódolásra koncentrálni, és nem az edittor nehézségeivel akar foglalkozni, használja ezt a megoldást. Jelenleg a benti gépen is működik ez a módszer. A működtetés menete a következő. Windows-t bootolni, szerkesztés, szintaktikai hibák javítása VC-ben. Robot Be. QNX bootolás. Fatfsys. Forrás másolása (felülírás) window partícióról QNX partícióra. Make all. Futtatás. Log-fájlok. tanulmányozása, stb. Kisebb változtatás QNX alatt. Nagyobb változáshoz érdemes a robotot kikapcsolni, és Windowst-bootolni.
3.3.2 Pathgen egyszerűsítése, annak előnyei Már a 2000-es tavaszi félévben belekezdtem a pathgen egyszerűsítésébe. Egy backup után átírtam, hogy a pathgenben a negyedfokú interpolációnak ne legyen két külön ciklusa a Descartes-, illetve a csuklóinterpoláció esetére. Az interpolációs konstansok feltöltését meghagytam különbözőnek, de az egyenes mozgás együtthatóit is a csuklóinterpolációs ciklus által használt változókba írattam bele. Ezek után a csuklóinterpolációs ciklusba beleírtam, hogy a kimeneti 6 dimenziós (az eddigiekben 48
csuklóértékekként értelmezett) vektoron egyenes mozgás esetén végezzen el egy direkt-Rodrigez, egy elforgató, és egy inverz geometriai függvényhívást. Az így adódó 6d vektorral felülírandó az interpoláció kimenete. Ezután törölhettem az egyenesinterpoláció ciklusát, mivel a két verzió közötti különbségeket beleírtam az első ciklus feltételes végrehajtású ágaiba. Akkoriban a konfigurációváltás miatti problémák kijavítása előtt ezt a módosítást nem tudtam alaposan tesztelni. A pathgen minden ciklusában folyamatosan figyeli az arps esetleges üzenetét, hogy ezt elraktározhassa. A sok egyező kódrészletet makro-ba raktam (neve:MESSAGE_SWITCH, de lehetne függvény is, annyira nem sebességkritikus). Ez a pathgen.c elején található. Ma már az a véleményem, hogy az interpreter funkciót szét kell választani az inicializáló-felügyelő funkciótól, és a nyelvi funkciókat ellátó processzt a pathgennek blokkolnia kell. Csak akkor szabad a következő pontot átvennie, amikor az előző lassítási szakaszába érne. Ez az interrupt-recovery miatt fontos. Az egyenes mozgás hibajavítása után (ekkorra az eddigi összevonások le lehettek tesztelve) az inicializáló részeket is összevontam annyira, amennyire lehetséges. A pathgen kódja így rövidebb, olvashatóbb, és az azonos interpolációs képletek nincsenek többszörösen kódolva benne.
3.3.3 Az osztott memóriák új tagjai, szerepük A Hoston eredetileg 3 osztott memória tartomány volt 3 saját szemaforral ellátva. A dynjac.shm létrehozását, illetve írását kitöröltem, mert a Host-on nem olvasta semelyik processz sem az értékeket, és még a hibrid erő-pozíció irányítás nyelvi kiterjesztésének kódolása után is csak a szabályozásnak lesz szüksége a kinematikai és a dinamikai adatokra. A geometr.shm nem használt értékeinek használatát megvalósítottam, az ebben levő szemafort átneveztem robot-mozgás-jogá –ra, mivel az itteni értékek megváltoztatását meg kellett akadályozni a robot mozgatása alatt. Így a szemafor birtoklása ezt az új értelmet kapta. A robotot mozgató két processz (pathgen és pendant) ezen a szemaforon keresztül is óvja a robotot az egyidejű közös mozgatástól. A joint-shm-be új elemeket raktam. Így meg lehet különböztetni az utoljára kiolvasott pozícióértékeket az utoljára kiadott mozgás-végponttól (last-q-a). A robot mozgatását mindig az előző mozgás végső alapjelétől kell folytatni. Ha a tényleges pozíciótól folytatnánk, az a szabályozójelek ugrásához vezetne. Bizonyos esetekben szükséges a tényleges pozícióval felülírni az előző mozgás végét jelentő mezőt (amikor a robot nem normál mozgatással mozdult meg: bekapcsolás, kalibráció, illetve free használata után). Vannak processzek, melyek várakoznak mindkét SHM (SHared Memory) szemaforára. A deadlock elkerülését biztosítja, hogy a joint-shm szemaforának birtoklása rövid idejű, és ezalatt biztos nem kezd el a másik szemaforra várakozni a program. Ez a holtpontelkerülésnek az a módszere, amikor az erőforrások foglalásához fix sorrendet rendelünk. Pindszulya Béla szerint a neutrínó már nyelvi szinten is támogatja ezt a megoldást. Amúgy a rövid, biztosan lefutó shm-elérések oszthatatlanságát gyorsabb a megszakítások tiltásával (_enable() és _disable()) biztosítani. Az osztott memóriákban levő struktúrák definíciója a megfelelő include-fájlokban (comm.h és geometr.h) minden elemhez beszédes kommenteket tartalmaz.
49
3.3.4 A log fájl készítést segítő debug.c modul, használata és a robot_nomove define A szoftverrendszer fejlesztése sokkal lassabb, mint egyéb alkalmazásé, mivel a hibák kiirtása teljesen más módszereket kíván, mint egy mezei szoftver esetében. Minden nagyobb, új programrészt érdemes külön tesztprogrammal tesztelni a roboton való próba előtt. Jó példa erre a csak Windows platform alatt létező geom-test alkalmazás, mely a direkt geometria kimenetével meghívja az inverz geometriát. Emellett a konfigurációkódolás illetve lekérdezés konzisztenciáját is ellenőrzi. Sokkal több időbe telt volna, ha az ezáltal kiderült hibákat log-fájlok tartalma alapján kellett volna kideríteni. (A forráskod érdemi része megtalálható a geometr.c vége felé. Nincs igazi kimenete az alkalmazásnak, debuggerrel futtatandó.) A rendszer egész működését igénylő futtatások közbeni hibákat csak log-fájlok alapján lehet kideríteni. A debug.c modulban implementált funkciók segítségével minden processz saját fájlba könnyedén loggolhat 1,3,6,9 illetve 12 dimenziós vektorokat (utóbbi kettő 3*3-as illetve 4*3-as mátrix loggolasára). Két define-al kapcsolható, hogy az üzenetek mely része menjen a képernyőre, illetve a fájlba. Alkalmazására példa a pathgen.c-ben található. A rendszert érdemes úgy kipróbálni, hogy a robot nem mozog. Erre két lehetőség van. A robotszimulátor fordításával (makerobszim makefájl) a robotvezérlőt nem kell bekapcsolni, az ARC és a PC-Lab kártya működését egy segéd processz szimulálja. Ekkor a watcom debugger is használható. Sárközy Ferenc diplomamunkája eredményeképpen photon alatt grafikusan megjeleníthető a robot az aktuális, szimulált pozícióban. A szimuláció nem dinamikai, a kiadott alapjel azonnali beállását feltételezi. Az időzítések éles kipróbálására is szükség van egyes módosítások után. Ilyenkor jobb a robot mozgását tiltani. E célból létrehoztam a robot.c modul elején egy define-t, (robot_nomove), mellyel a kiadott robot pozíció mindig 0 lesz. A szimuláció ebben az esetben nem tökéletes, ugyanis a lekérdezett pozíció a valódi lesz, azaz 0 ±szabályozási hiba. Mindennemű robotmozgás tiltva van ilyenkor, a kalibráció is. Eredményesen lehetett ezt a módszert ütemezési problémák kiderítésére használni.
3.3.5 A pendant sebességlimiteinek összehangolása, mozgatások anticiklusból ciklusba rendezése
pendant
A pendant processz által kiadott robotmozgatások szintén saját sebességlimitekkel számoltak eredetileg. A robot-mozgás-joga szemafor, illetve a megváltozott inverz geometriai viselkedés kódolása közben kicsit javítgattam ezen a modulon is. A sebességlimiteket most már a robot.c-modulból veszi. Az egyes mozgatási módok kezelése hosszú anticiklusokkal volt megoldva. Ezeket az olvashatóság érdekében lerövidítettem, igazi ciklusokká tettem őket. Az egész diplomatervben ajnároztam az anticiklus előnyeit, és mégis itt az ellenkezőjét teszem. Pont azért, mert ez a rész nem sebességkritikus, másrészt az anticiklusos megoldás annyi helyet foglalt el, hogy a 50
tényleges algoritmust alig lehetett kiolvasni a sorok közül. A vektorműveletek esetében pedig az anticiklusos megoldásaim egy-egy függvény törzsét duzzasztják csak. Ezek alapműveleteket valósítanak meg. Megértésükhöz támpont a matematika.
3.3.6 Puma.c és geometr.c egyesítése A Huszti Ákos által írt puma.c modult csak a dynjac processz tartalmazta. A direkt geometria ebben is implementálva volt, természetesen a geometr.c-ben levőtől eltérő paraméterekkel. (Ráadásul a képlet sem volt a geometr.c-vel megegyező, és különböző nullaszög reprezentációt használtak.) Úgy gondoltam, hasznosabb, ha a robotunk geometriájábol adódó összes kódrészlet egy modulban van. Ez a geometr.c lett. A puma.c és a puma.h fájlokat teljesen megszüntettem. A robotmodellek által használt csuklóreprezentáció az egymásrahatásról szóló bekezdésben lett részletezve.
3.3.7 MYMATH.C módosítása Mymath.c: Begyorsítottam egy-két vektor-mátrix függvényt, specializáltam 3*3-as esetre. Az anticiklus (for ciklus és ciklusváltozó tömbindexek helyett kódrészlet ismétlése különböző, de konstans tömbindexekkel) alkalmazásával csökkenteni lehet az adategymásrahatás miatti pipeline lassulást. Az inverz Euler függvény is tartogatott egyszerűsítési, gyorsítási lehetőségeket. Többek között egy felesleges szinusz-koszinusz híváspárt, egy numerikusan instabil equabout(1) tesztet tartalmazott. A 487-esnél újabb FPU-knak létezik egy fsincos nevezetű utasítása, ami egyszerre számítja ki a szinusz és a koszinusz értéket. Robotikai alkalmazásakor ez igen hasznos. Mivel minden trigonometrikus függvény kiértékelése előtt egy lebegőpontos maradékszámítást kell végezni 2PI hányadossal (ennek reciproka természetesen az FPU ROMjába lehet égetve a gyorsítás érdekében), de ennek a műveletnek, valamint a síknegyed meghatározásának megismétlését lehet megspórolni vele. Pentium órajeltáblázatok szerint egy trigonometrikus függvény időigénye min. 16 clk, míg a két érték együttes kiszámítása 17clk [Fókuszban a Pentium]. Épp ezért létrehoztam a mysincos makrót, amely pointerbemeneteken keresztül használhatja ezt az utasítást. Sajnos már elég rég programoztam az FPU-t watcom inline assembler alatt, így segédlet nélkül nem sikerült a megfelelő paraméterezést eltalálnom. Szerencsére időközben előhalásztam a régi watcomos FPU assembler kódjaimat, és a Watcom User’s Guide segítségével sikerült lefordítanom. A linker viszont nem látta az inline függvényt, másik függvénybe ágyazva a sok push és pop elveszi a sebességnövekedést. Így most egy külön függvény pótolja az assembler makrót, de nem érdemes használni.
3.3.8 A Rodrigez képletről
51
A sokfele reprezentáció közül, amivel a 3 dimenziós orientáció jellemezhető,( RPY, Euler, OAT, Rodrigez, kombinált Rodriguez) deriválási szempontok miatt a kombinált Rodriguez (egységvektor, mint forgástengely beszorozva a szög nagyságával) emelkedik ki a többi közül. Nem létezik számára szinguláris orientáció, azaz olyan helyzet ami a reprezentáns egy egész tartományával ábrázolható. A kombinált Rodriguez reprezentációban csak a 360 fokos körbefordulási periodicitás jelentkezik. Ez egy gömbhélyszerűen ismétlődő periodicitást jelent. A pálya-interpoláció derivált folytonossági megkötései miatt ott egyértelműen ez alkalmazandó, de szerintem minden más esetben is előnyös a használata, mint belső reprezentáció. Felhasználói szempontból viszont nem alkalmas orientáció bevitelre. (majdnem minden itt említett reprezentáció kényelmetlen felhasználói szempontból. Egy grafikai munka keretében alkalmam nyílt kipróbálni mindegyiket, és végül a világ koordinátarendszer tengelyeihez képesti pillanatnyi forgatás megadása tűnt a leghasználhatóbbnak.) Ennek a reprezentációnak az előnye, hogy numerikusan stabil képlettel számítható az inverze. Ez a számítás nem része a tananyagnak, a könyvben közölt képlet diszkussziója nem tér ki a sign(0) esetre, ami ténylegesen előfordul. 3.3.8.1
A probléma
A direkt kombinált-Rodriguez képlet. Adott egy térbeli vektor. Legyen ez v. Keressük azt a mátrixot, amelyet megkapunk, ha az egységmátrixot elforgatjuk v vektor egyenese körül v hosszának megfelelő szöggel (radián). Nullvektor esetén nem definiálható a tengely, de mivel a forgatás szöge is nulla lenne, így egyértelmű, hogy függetlenül a választandó tengely irányától, az egységmátrixot kapnánk eredményül. Így a nullvektor nem jelent szinguláris esetet. (Míg ha nem kombinált Rodriguezképlettel dolgozunk, ott nincs így.) Vizsgáljuk meg a problémát numerikus szempontból. Tegyük fel, hogy fixpontos számokkal számolunk, és a legkisebb felbontás 0.001. Szeretnénk egy igen kis szögű elfordulást megadni, de ennek a tengelyét elég pontatlanul kapja meg a leképező függvény, hiszen a hossza be lett szorozva az igen kis szög értékével. Magyarul: mennél kisebb az elforgatás mértéke, annál bizonytalanabb a tengely iránya. Viszont hiába is lenne a tengely pontosan megadva, mikor a kimenő mátrix koordinátái szintén csak 0.001 egység pontosan adottak. Azaz a tengely egységvektorát akár egy fixpontos kombinált Rodriguez vektorból, akár a kimeneti mátrixból akarjuk kinyerni, kis szögeknél kb. ugyanakkora lesz a numerikus bizonytalanság. Matematikusan fogalmazva, kis szögekre a kimenő mátrix koordinátáit deriválva a kombinált bemeneti vektor koordinátáinak változására, nagyjából 1-et kapunk (biztos nincs fél nagyságrend eltérés). Lássuk ezt algebrailag: h= sqrts(v*v), t=v/h, C=cos(h); S=sin (h); M=C*I+(1-C)*(tοt)-S*t×. [Lantos Béla: Robotok irányítása:34.old.] Ha h tart 0-hoz, akkor:
52
és
C=1,(1-C)=0,S=h;
− h * tz h * ty 1 M=I-h*t×= h * tz 1 − h * tx − h * ty h * tx 1 De h*t×=v×, Így − vz vy 1 M= vz 1 − vx = I-v× , ha h majdem 0. − vy vx 1 Innen ordít, M numerikus bizonytalansága azonos v numerikus bizonytalanságával. Az elfordulás reprezentációkra jellemző 360-os körülfordulási probléma persze nem kerüli el a kombinált Rodriguez képletet sem. A bemenetre (v) nézve gömbhélyszerűen a sugarat illetőleg 2PI periodikus a probléma. Az inverz probléma megoldása során érdemes felírni újra a mátrixot, majd az elemek kombinálásával kifejezni az eredeti bemenő, illetve közbülső változókat. A könyvben közölt megoldás először C értékének meghatározásával kezdi, mivel ordít a képletről, hogy C=0.5*(átlóösszeg-1). Ezután jön S^2 meghatározása. Innen kérdéses, hogy előbb a tengelyt érdemes-e meghatározni, és a tengely választott iránya ismeretében dönteni S=±gyök(S^2) előjelét illetőleg, vagy S-t eleve pozitívra választani, és az alapján határozni meg tx,ty és tz előjelet. T vektor koordinátáinak a négyzetei nyerhetőek ki aritmetikailag egyszerűen a képletből, így a gyökvonás pozitív-negatív megoldását illetőleg egy szignum függvény döntene. Ezt a megoldást leprogramoztam egy grafikai munka keretében, amely során kiderült, hogy bizonyos esetekben belefut a program a signum(0) esetbe. Ilyenkor sem a pozitív, se a negatív megoldás mellett nem lehet elkötelezni a programot, mivel lehet, hogy tx pozitív lenne, és ty negatív. Ez akkor fordul elő, ha az egységmátrixot valamelyik bázistengely körül ±90-fokkal elforgatjuk, és az eredménnyel hívjuk meg az inverz Rodriguez algoritmust. φ=± ±∏
−1 1 1 t= [0 0 1] 1 − 1 1
1 1 −1 t= [0 1 0] − 1 1 1
1 − 1 1 t= [1 0 0] 1 1 −1
53
(képlet: az eredeti algoritmus által hibásan kezelt bemenetek) 3.3.8.2
A megoldás
Ekkor vettem elő a tengely meghatározásának azt a geometriai módját, ami a probléma első tárgyalásakor (előadáson) is egyből eszembe jutott. A következő vektorműveleteket térbeli műveletként érdemes követni, bár koordinátákra bontva is eljuthatunk a megoldáshoz, de úgy kicsit bonyolultabb. Ha a bemeneti mátrixot úgy kaptuk, hogy az egységmátrixot elforgattuk egy tengely köré, akkor bármely, a mátrixhoz, mint koordináta-kerethez rögzített pont által a forgatás során leírt körívnek a tengelyre merőleges síkban kell feküdnie.
(ábra) Így a bázis-egységvektorok végpontjai által leírt körívek is 3 ilyen párhuzamos síkon fekszenek. A kapott mátrix bázisvektoraiból kivonva az egységmátrix bázisvektorait, a kapott vektorok (a körívek húrjai) is merőlegesek lesznek a tengelyre.
(ábra) Bármelyik kettőt vektoriálisan összeszorozva, majd normálva megkaphatjuk a tengelyt. Numerikusan stabilabb, ha páronként vektoriálisan szorozzuk őket, (persze a sorrendre figyelve), utána összeadjuk (kombináljuk) a részeredményeket, és azután 54
normálunk. Ha a probléma geometriai elrendezését a tengely axonometrikusan síkbavetítve (végtelen távoli nézőpontból) szemléljük,
irányából
(ábra) akkor amilyen háromszög csillagzatot rajzolnak az egységmátrix bázisai, vele egybevágót alkotnak az elforgatott mátrix bázisvektorainak vetületei. A különbség (húr) vektorok végpontjait az origóba (a tengelyhez) tolva szintén velük geometriailag hasonló csillag alakot kapunk. Amennyiben a tengely párhuzamos az [1,1,1] síkkal, úgy a vetületen a bázisvektorok egy egyenesre esnek, így a szimpla összegzés eredményeképp nullvektort kaphatunk. Pont ezért a részeredményeket úgy kell kombinálni, hogy amennyiben nem egy irányba mutatnak (skalárszorzatuk kisebb, mint 0 (de itt lehet akár kisebb egyenlő is), akkor az ellentettet adjuk hozzá a halmozott eredményhez. Ez az algoritmus is egy szignum jellegű függvényt rejt magában: v3 a,b,c; //bemenet, a húrok vektorai v3 t; //kimenet, a tengely iránya v3 v; //munka v=aXb; t=v; v=bXc; t+=v*sign(tT*v); v=cXa; t+=v*sign(tT*v); t=norm(t); De nem érzékeny a signum argumentuma = 0 esetre. Másrészről ez a képlet csak akkor eredményezhet 0 vektort, ha minden részeredmény 0, ami akkor fordulhat csak elő, ha a három húr vetülete párhuzamos. Mivel helyzetük geometriailag hasonló a bázisvektorok vetületével, ez azt kívánná, hogy a három bázisvektor vetülete párhozamos legyen, azaz egy, a tengellyel párhuzamos síkba essenek. Ez pedig addig biztos nem fog előfordulni, amíg az ortonormált bázisvektorok determinánsa nem 0.
55
Ezzel a módszerrel megvan a tengely, de kérdéses az iránya. Mármint, hogy ha a tankönyv módszerével a szinusz értékét pozitívnak (vagy akár fixen negatívnak) választva, negálni kell-e a tengely koordinátáit. A problémát fel lehet fordítva tenni, miszerint adott irányultságúnak véve a tengely egyenesén az egységvektort, kell-e negálni a színusz értékét. (vagy akár az arkusz színusz szögét). Mivel kombinált Rodriguez kimeneti eredményt akarunk, teljesen mindegy, hogy az összeszorzandó szöget (ill. szinuszát) negáljuk-e előbb, majd aztán szorozzuk be vele az egységvektort, vagy az utóbbit negáljuk először. Az általam választott megoldásban a szinuszt negálom a feltétel függvényében, hívok egy atan2 függvényt, majd szorzok a szöggel. Ha esetleg nem kombinált, hanem normál (szög+egységvektor) Rodriguez inverz a feladat, úgy az utolsó művelet elmaradhat. Lássuk a problémát, amely felett átsiklottam: Adott egy 3D ortonormált mátrix, az egységmátrixhoz képesti elforgatásának egyenese, és a forgatási szög koszinuszán (mely előjel-információt nem hordoz lévén páros függvény, (Taylor sorában is csak a páros tagok szerepelnek)) kívül szinuszának a négyzete. Meg kell állapítani, hogy az egységvektorral adott tengelyegyenes és a szinusz közül kell-e az egyiket negálni. A módszer egyszerű. Elméletben kiszámítjuk a direkt Rodriguez képletet a negálatlan és a negált részeredményekre, kivonjuk a mátrix elemeket az eredeti bemeneti mátrixból. A kérdés, hogy melyik adja a kisebb négyzetes hibát. Természetes, hogy a mátrixoknak csak a szinuszos tagot tartalmazó elemeit kell vizsgálni. Legyen t vektor a tengely egyenesének egységvektora, és az e vektor olyan, hogy e.x=m.w-m.s;e.y=m.r-m.z;e.z=m.y-m.u;
x u r ahol a mátrix = y v s alakú. z w t Ekkor a hibanégyzetek a két megoldásra: if ((e.x-2*t.x*S)^2+(e.y-2*t.y*S)^2+(e.z-2*t.z*S)^2 < //pozitív S (e.x-2*t.x*-S)^2+(e.y-2*t.y*-S)^2+(e.z-2*t.z*-S)^2 ) //negatív S S=S; else S=-S; ,azaz a pozitív, vagy a negatív szinusz adja a kisebb hibát? A feltétel kifejtve: if( e.x^2+4*t.x^2*S^2 -4*e.x*t.x*S + ....y-ra és z-re... < e.x^2+4*t.x^2*S^2 +4*e.x*t.x*S + ...y-ra és z-re.. azaz: S negálandó, ha 0>S*(e smul t) ha S eleve pozitív, akkor s*=-1 ha 0>e smul t 56
Milyen érdekes, hogy ennél a megoldásnál egy osztás, egy atan2, két gyökvonás, És három előjel-összehasonlítást kellet végezni. (Bár ebből egy elhagyható lenne, csak a pontosabb eredmény miatt tartottam meg. ) Az eredeti képlet egy osztást, egy atan2-t, négy gyökvonást, és fixen három előjelösszehasonlítást igényel. Az érdekesség az, hogy a szükséges műveletekből alig lehetett valamit megspórolni. Az eredmény mindenesetre az, hogy a numerikus hibákra érzéketlen képletet kaptunk. Az algoritmus (a forráskód megjegyzéseiben nélkülöznöm kellett az ékezeteket, mert a különböző op. rendszerek eltérő karakterkódokat használnak): typedef struct{ double x,y,z;}v3; v3 a,b,c,t,e; double f,C,S,sn; //C,S : C ketszerese S ketszerese //a tengely meghatarozasahoz szukseges, hogy melyik egysegvektor csucsa milyen iranyba mozdult(fordult) el az egysegmatrix megfelelo csucsahoz kepest. a.x=m[0]-1; a.y=m[1]; a.z=m[2]; b.x=m[3]; b.y=m[4]-1; b.z=m[5]; c.x=m[6]; c.y=m[7]; c.z=m[8]-1; //a tengely ezekre a vektorokra meroleges. ha kepezzuk paronkent a //vektorszorzatukat, es osszeadjuk oket(a pontossag novelese erdekeben) // akkor az eredeti tengelyell parhuzamos iranyt kapunk. //szimpla osszegzezs eseten null vektor nem csak akkor johet ki, ha az elforgatas 0, //hanem akkor is, ha a tengely parhuzamaos az [1,1,1] sikkal. //emiatt az osszeadas helyett kivonast kell alkalmazni olyankor, amikor a reszeredmenyek //ellentetes iranyuak. vv3multv(&a,&b,&t); //t=aXb vv3multv(&b,&c,&e); //e=bXc f=t.x*e.x+t.y*e.y+t.z*e.z; if(f<0) //nem egy iranyba mutatnak { t.x-=e.x;t.y-=e.y;t.z-=e.z; } //t=t-e; else { t.x+=e.x;t.y+=e.y;t.z+=e.z; } //t=t+e; 57
//inentol mar csak a numerikus stabilitasert van a szamitas, el lehetn hagyni. vv3multv(&c,&a,&e); //e=cXa f=t.x*e.x+t.y*e.y+t.z*e.z; if(f<0) //nem egy iranyba mutatnak { t.x-=e.x;t.y-=e.y;t.z-=e.z; } //t=t-e; else { t.x+=e.x;t.y+=e.y;t.z+=e.z; } //t=t+e; //eddig
f=t.x*t.x+t.y*t.y+t.z*t.z; //f=abs(t)^2. nem alkalazok fn. hivast, mert az lassu if(f>0) { f=1/sqrt(f); C=m[0]+m[4]+m[8]-1; //Cosinusz ketszerese:atlok osszege minusz 1. ez a konyvben is igy van. e.x=m[5]-m[7];e.y=m[6]-m[2];e.z=m[1]-m[3]; S=sqrt(e.x*e.x+e.y*e.y+e.z*e.z); //Szinusz ketszerese sn=e.x*t.x+e.y*t.y+e.z*t.z; if(sn<0) S=-S; *phi=myatan(S,C); //myatan-hoz es atan-hoz a fuggoleges koord. kell elore. az a sinus. T[0]=t.x*f;T[1]=t.y*f;T[2]=t.z*f; } else { T[0]=1.0;T[1]=T[2]=0.0; *phi=0.0; }
3.3.9 SZFÉRA Tesztek: konfiguráció-kezelése, kezelés, interrupt: break-nobreak
jacobi-nullázódás-
A félév során felmerült egy sor probléma, melyek megvalósítása során nem volt egyértelmű, hogy a különleges szituációkat hogyan kell a programrendszernek elkerülnie. Az ilyen esetekben megnéztem, az adott problémára hogyan reagál a régi vezérlő. Ezek a problémák:
58
• Mit csináljon a robot, ha egyenes vonalú mozgás közben túllépné a csuklósebességlimiteket? • Pontosan mit jelentenek az eredeti arps nyelv konfigurációi, a konfiguráció-módosító utasításoknak mikor van szerepük, és mit reagál a robot azokra a mozgatásokra, melyek elméletileg sem teljesíthetőek? • Interrupt esetén mit befolyásol a nobreak kapcsoló, az interrupt bekövetkeztének milyen hatása van a mozgásra, annak kétszer (háromszor) deriválható jellegére? • Mennyi ideig mozog a robot, ha nagyon közeli pontba mozdítjuk pályatervezéssel? Az első működés kipróbálása nem igenyelt különösebb faradozást, hamar kiderült a SZFÉRA-ban alkalmazott algoritmus. A régi vezérlő nem monitorozza a limittúllépést, kiadja a túlzott sebességeket a hajtásprocesszorok számára. Ebben a kérdésben a BME vezérlő jobb a SZFÉRA-nál. A kézivezérlővel való mozgatáskor kiadott alapjel mindkét vezérlőben sebességugrásos, ha ez a limittúllépés hibájával párosul, úgy igen nagyot rúghat a robot. (Ready helyzetben a kézivezérlő WORLD módjában nagy sebességű –Z mozgatást nem jó gyakran végrehajtatni.) A konfigurációkkal kapcsolatos tesztek az inverz geometriai feladat elméleti hátterének a fényében mutatják igazi jelentőségüket. Ezeket a teszteket így az inverz geometriai részben ismertetem. Az interrupt. A mostani programrendszerben az interrupt megvalósítása nem tökéletes. Pontosan definiálni kellett, mi a teendő break-es, illetve nobreakes esetben, ha beüt az interrupt feltétele. Break-es esetben az aktuális pozícióban sebességugrással megáll a robot, az interrupt azonnal hívódik. Az interrupt rutin végrehajtása után a rutin utáni pozícióból megy a soron következő pontba a robot. Ha egy pontot elkerülő negyedfokú görbe interpolációja már elkezdődött, úgy az a pont már végrehajtottnak számít, és a következő célpontba megy majd a robot. A pontok helyzetét újra kiértékeli az arps rendszer, így a (pont) változók interrupt alatti megváltoztatása hatással van rájuk. (Ez az AUT tanszék mérési feladata esetében igen lényeges.) A pont elértségének a feltételét lehetne akár úgy is módosítani, hogy az elkerülő interpoláció felének elérésekor tekinti a rendszer a pontba vezető mozgás végét. NOBREAK-os interrupt a beütése után utasítja a pályatervezést, hogy a következő pontelőírásban álljon meg (Break-es mozgást forszíroz), ezt a pontot elértnek veszi, végrehajtja az interrupt rutint, majd folytatja a program futtatását. (ciklusos, nem break-es mozgás esetében elmegy a következő pontba, de ott már nem áll meg.) Ennek a tesztnek a loggolását a függelékben közlöm. A pathgen pályatervezése a [Lantos-könyv] szerinti módszerrel igyekszik elkerülni a gyorsulástúllépést. A program indulásakor kiszámítja, mekkora idő kell ahhoz, hogy a robot maximális gyorsulással maximális sebességre gyorsítson fel. Minden mozgás gyorsító fázisa ennyi ideig fog tartani. Ennek hátránya, hogy közeli pontokba mozgatva a robotot, lassabb mozgást kapunk, mint amire valójában képes lenne a robot. Az eredeti vezérlőszekrény (speed 300-as beállítással) 1°-os mozgást kb fele annyi idő alatt hajt végre, mint egy 4°-ost, és negyed annyi idő alatt, mint egy 16°-os elmozdítást. Ezen a téren még érdemes lenne módosítani az algoritmuson.
59
3.3.10 Az inverz geometria 3.3.10.1
Bevezetés
Ebben az alfejezetben az inverz geometriai problémát veszem górcső alá. Egyik legfontosabb feladatomnak tartottam, hogy kijavítsam az egyenes mozgatások végrehajtásakor szinte biztosan felmerülő hibát. A GOS, MOVES utasítások, illetve a kézivezérlő WORLD, TOOL módjaiban végrehajtott mozgatások majdnem minden esetben a robotkar hadonászásához vezettek. Már kezdetben is indokolatlan konfiguráció-váltást sejtettem a hiba okozójaként. A hiba javításához komoly elméleti megfontolásokra volt szükség, amelyek végül elvezettek a mostani algoritmushoz. 3.3.10.2
Több konfiguráció léte, említés a különbözőségükről
A rotációs csuklókkal rendelkező robotok inverz geometriai feladata több megoldást is visszaadhat egy konkrét elérendő térbeli helyzethez. A megoldásokat megoldásfán ábrázoljuk. Más robotkarokról nincs tapasztalatom, így a PUMA 560 megoldásfájának lényeges tulajdonságai, melyek befolyásolják a probléma kezelését, lehet hogy más geometriájú robot esetén egyáltalán nem teljesülnek. A mi megoldásfánk három szinten ágazik el. Minden ilyen elágazás két döntési alternatívát tartalmaz. Így 2^3, azaz 8 féle különböző megoldás létezik minden megoldható térbeli helyzethez. A geometriailag nem megoldható helyzetek túl közeliek vagy túl távoliak, de ha van megoldás, akkor nyolc van. (Elméletileg, gyakorlatilag a csuklószögek nem vehetnek fel bármilyen valós értéket, így pár megoldás kiesik. Fontos momentum lesz , hogy ezt a joint_range()-nek nevezett gyakorlati tesztet az algoritmus melyik pontján végezzük el.) Tehát a nyolc megoldást 3 szempont szerint lehet osztályozni, és minden szempont szerint kétféle eset lehet. Ezeket a szempontokat konfigurációs problémának is hívom. Így megkülönböztetem a váll, a könyök és a csukló konfigurációs problémáját. A váll problémája az 1-es és a 2-es, a könyöké a 2-es és a 3-as, a csuklóé a 4-es, 5-ös, 6-os csuklók értékét befolyásolja közvetlen. Így ezeket szoktam 1-2, 2-3, 4-5-6 problémának is nevezni. Az arps rendszer J2, J3, ill. J5 néven nevezi őket. A probléma analizálásakor jelentős szerepet játszott a konkrét robotkar geometriájából adódó megoldás-elágazások különbözősége. A váll, a könyök és a csukló kétféle választása jelentősen különbözik egymástól. Minden elágazási problémához tartozik egy olyan helyzet, melyben a két alternatíva egyezik. A váll esetében ez a teljesen függőleges kar esete, a könyök esetében ez a teljesen kinyújtott (illetve behajlított, de ez a gyakorlatban nem fordul elő) könyök, míg a csukló esetében szintén a teljesen kinyújtott helyzet az ilyen. Ezeket szinguláris helyzeteknek nevezzük. A három probléma viselkedésének különbsége legjobban abban nyilvánul meg, hogy a szinguláris helyzet milyen kivételes szituációkat tartogat. Például. A váll szinguláris helyzete az, amikor a 4-5-6 tengelyközéppont pont a 2. tengely felett van (könyök lehet akár előre, akár hátra behajlítva). Egy ilyen helyzeten való világkoordinátás, 2. tengelyre merőleges áthaladáskor a józan paraszti ész szerint a csípő (1-es csukló) helyben maradhatna. A robotvezérlő viszont nem így mozgatja, de az nem jelent problémát. A könyök szinguláris helyzeten nem lehet egyenes mozgatással áthaladni, de ha egyenes 60
mozgás végpontja ez, akkor a sebességtúllépés-védelemnek be kell kapcsolnia. A csukló szinguláris helyzetén végtelenféleképpen lehet világkoordinátás mozgatással áthaladni. Ha itt nem a józan paraszti ész szerint elvárt módon mozgunk, akkor az már bajokhoz vezet. Remélem sikerült elöljáróban érzékeltetnem a probléma összetettségét, valamint azt, hogy alaposan analizálni kell őket. Az eredeti arps nyelv fogalmai egy eleinte ígéretes útra tereltek, mert leginkább a könyök szinguláris helyzete járt az eszemben, és ez az út a könyök problémáját a legjobban oldja meg. Az új algoritmus tesztelésének vége felé derült ki, hogy a csukló problémája mindenképpen másfajta bánásmódot kíván, és a váll problémájához is jobban illene egy másik módszer. Így az eredeti arps konfigurációkezelési filozófiája determinálta gondolataimat, és mire a probléma feletti teljes áttekintés birtokába jutottam, már annyira készen volt az algoritmus, hogy nem írtam új, az arps-től kicsit eltérő filozófiájú verziót helyette. Az birtokomba jutott módszerek más geometriájú robotok világkoordinátás mozgatásához akkor használhatóak, ha igaz, hogy minden elágazási probléma minden addigi megoldás-ágát ugyanannyi ágra ágaztat el. Azaz, ha a megoldások minden probléma szempontjából úgy osztályozhatóak, hogy minden osztály azonos számú elemet tartalmaz, és a többi elágazás szerint ugyanígy továbbosztályozható. (A megoldások száma tehát M1*M2*.*Mn képlettel számítható, ahol Mi az i. konfigurációs probléma elágazásainak a száma.)
(ábra:4 külonbozö konfigurációjú megoldás egy térbeli helyzetre)
61
A konfigurációváltás problémája, SZFÉRA és a régi rendszer összehasonlítása A felmerült új elméleti problémák megoldásához egyeztettem konzulensemmel, Bézi Istvánnal, és arra jutottunk, hogy mindenekelőtt vizsgáljuk meg az eredeti rendszer (SZFÉRA) reakcióját a kérdéses mozgásokban. Problémás mozgások:
SZFÉRA reakciók:
Régi BME reakciók
Egyenes vonalú mozgatás ready-be, nem teljesen szinguláris pozíció, de a jacobi mátrix értékei kicsik. (inverze túl nagy)* Egyenes vonalú mozgatás tanított pontba, (lehet akár ready helyzetben) Egyenes vonalú mozgatás precíz pontba, melynek geom. Konfigurációja eltér az aktuális helyzetétől. Egyenes vonalú mozgatás szinguláris, Azaz meghatározhatatlan konfigurációjú pozícióból precíz pontba. Egyenes vonalú mozgatás olyan precíz pontba, aminek a konfigurációja megegyezik a mostanival.
nem volt hajlandó gos-al readybe menni.
túlzottan gyorsak a csuklók
túl nagy csuklóseb.
Szintén túl nagy csuklóseb. 'use joint motion' Iszonyatos error robotmozgás
hajlandó mozogni
hajlandó mozogni
hajlandó mozogni
hajlandó mozogni
* Az javított BME szoftver nem megy ready-be GOS-al, ha nem a default konfigurációban áll a robot. (pl. Ready után .m j 3,60). Lehet, hogy a teszt alatt is ugyanezért nem mozdult meg a SZFÉRA. Elöljáróban említem meg, hogy a BME szoftver jelenlegi (javított) verziója a SZFÉRAval megegyező módon működik. Azaz annál jobban, mert túlzott csuklósebesség értékek esetén visszavesz arányosan minden csukló sebességéből. A működés tervezésénél nem a SZFÉRA rendszer másolása volt a célom. Az összes felmerülő elméleti problémán végigrágtam magamat, és igyekeztem mindegyikre megoldást találni. Volt, amelyik egyszerűre sikerülhetett, de volt, ahol első ránézésre kicsit bonyolult lett a legegyszerűbb megtalált megoldás. Az, hogy a rendszer a konfigurációs kérdésekben a SZFÉRA vezérlővel azonos módon viselkedik, azt mutatja, hogy ezekkel a problémákkal az eredeti rendszer fejlesztői is megküzdöttek, és ők is hasonló
62
megoldásokat találtak a problémákra. Az elméletből ugyanis egyértelműen levezethető, mely mozgásfajtákat milyen konfiguráció állások esetén szabad csak megengedni. Az eredeti arps nyelv a különböző inverz geometriai megoldás-elágazásokra a problémánként kétféle megoldást különböző névvel illeti. Ezek a nevek nincsenek mindig összhangban a pontos a geometriai jelentésükkel. Az 1-es és a 2-es csuklót befolyásoló elágazást J2 problémának hívja, és ’RIGHT, LEFT’ (váll jobbos, váll balos) kódokkal különbözteti meg a pozitívabb, illetve negatívabb csuklóértékekhez tartozó elrendezéseket. A 2-es és a 3-as csuklót érintő problémát a ’J3’ kóddal illeti, és ’UP, DOWN’ szavakkal (könyök fent, ill. lent) különbözteti meg a két megoldást. A 4-es, 5-ös és 6-os csuklókat érintő inverz-Euler probléma kódja J5 és ’positive, negatíve’ értékek osztják kétfelé a lehetséges helyzetek 3 dimenziós teret. Ennek az elágazási problémának a szétválasztást fedik egyedül az elnevezések. A váll jobbos konfiguráció esetében el lehet vinni úgy a robotot, hogy inkább egy balkezes emberre emlékeztet a helyzete. A könyök fent konfiguráció-elnevezés is fedhet olyan helyzetet, amiben a könyök lejjebb van, mint az ugyanazon térbeli helyzetnél könyök-lent konfigurációnál felvett ’testtartás’ esetén. Éppen ezért szerencsésebbnek éreztem a positive-negative elnevezést használni mind a 3 elágazás esetében. Az 1-2 , a 2-3 és a 4-5-6 megoldási elágazásokat én is rendre J2,j3,j5 konfigurációnak hívom Arps nyelvi szinten. A robot mozgatására négyféle alesetet kell megkülönböztetni kettő kétválasztású szempont szerint. Végezhetjük a mozgást csukló-, vagy Descartes-interpolációval, illetve a végpontot megadhatjuk csukló-, vagy Descartes koordinátákkal. A kiindulási pozíció a robot aktuális helyzete, ennek mindig a csuklókoordinátákban adott leírását kell figyelembe venni. • Amikor mind az interpoláció, mind a véghelyzet megadása csuklókoordinátákban történik, akkor az inverz geometria nem megoldandó feladat, így a konfigurációk problémája sem jut szóhoz. • A többi három esetből a legegyszerűbb az, amikor csuklókoordinátákban interpolálódik a mozgás, de a véghelyzet térbeli pozíciókent adott. Ilyenkor a lehetséges inverz geometriai megoldások (8 db) közül el kell hagyni azokat, amelyek csuklóértékei a robot által nem elérhetőek, illetve választani kell közülük. Az arps nyelv konfiguráció-módosító utasításai csak ebben az esetben számítanak (számíthatnak), akkor is csak a következő mozgás idejéig. Ilyen módosító-utasítások híján a robot a default konfigurációban (j2 right, j3 down, j5 pozitíve) keresi a megoldást. Ebből is látszik, hogy az inverz geometria minden elágazásának címkézhetőnek kell lennie, még pedig úgy, hogy differenciálisan közeli bemenetek azonos kódú (címkehármasú) megoldása szintén közeli legyen. Erre csakis a ’negatívabb-pozitívabb érték a két megoldás közül’ című címkék módszer használható. Ez a követelmény vezetett az arps által is használt a konfiguráció-kódolásos módszer implementálásához, majd öszvéresítéséhez. • Egyenes (világ-koordinátákban interpolált) mozgás esetében nem lehet számításba venni a felhasználó konfigurációval kapcsolatos kívánságát. A mozgás elején levő konfigurációt ugyanis determinálja a robotkar helyzete. A következő interpolációs pont megoldásának ehhez a lehető legközelebb kell lennie. Ez kétféleképpen nyerhető: vagy a 8 konfiguráció közül megkeressük az előzőhöz legközelebbi csuklóértékeket, vagy a kiindulási pozícióból kinyerjük a rá jellemző konfiguráció-
63
kódot, és az inverz geometriát utasítjuk, hogy csak ebben keressen megoldást. (Ennek előnye, hogy egyszer kell tesztelni, és utána a kinyert kód elküldhető az inverz geometriai rutinnak, hogy az egész mozgás alatt csak azon az egy megoldási ágon keressen megoldást.) A mozgások többségére jellemző, hogy csak abban a konfigurációban szabad menet közben (és a véghelyzetben) tartózkodni, amiben a kiinduláskor volt a robot. (Akár a legközelebbi módszer, akár a konfigurációs kód alapján keressük a megoldást). A zárójel előtti állítás akkor nem igaz, ha a mozgás közben szinguláris pozíción halad át a robot. Ilyenkor ugyanis a 456, illetve az 12 elágazás esetében célszerű konfigurációt váltani. (A 23 elágazás szinguláris helyzetén (teljesen kinyújtott kar) nem lehet egyenes mozgatással áthaladni egy valódi robottal.) Amennyiben mindig nearest módszer szerint végezzük az interpolációt, úgy ez meg is történik. (Ekkor mozgunk a józan paraszti ész szerint elképzelt legegyszerűbb módon.) Ez a leglényegesebb különbség a konfigurációkódolásos, illetve a nearest módszer közötti működésben. A 456-elágazás problémája nem kódolható. Ennek az elágazásnak a tekintetében mindig a nearest módszert kell alkalmazni, ha az inverz geometria megoldásválasztása mozgatás közbeni interpolációhoz kell. Ennek a ténynek a felismerésekor már elköteleztem magamat a konfiguráció-kódolás módszere mellett, annak többi része kimerítően le lett tesztelve, így az 12 konfiguráció-elágazás még mindig konfiguráció-kódolással működik. Ennek az elágazásnak az esetében ez nem vezet a robot túlterheléséhez. A több megoldás létezéséből adódó programhibák eredetileg onnan fakadtak, hogy ‘az előző megoldáshoz képest legközelebbi csuklóértékek’ szerinti választás algoritmusa a csuklóhatárértékek ellenőrzése után lett lefuttatva. Így ha az előzővel azonos konfigurációban már nem létezett gyakorlatilag érvényes megoldás (Az elméletileg nem érvényesek a ’too-far’ ill., ’too-close’ kategóriába esők, a gyakorlatilag nem érvényesek a csukló-limitekbe ütközők.), akkor átváltott automatikusan arra a konfigurációra, ami a legközelebbi, és gyakorlatilag is megengedett. Ez természetesen az egyenes pálya elhagyását, és durva robotmozgást eredményezett. Ilyenkor az interpoláció időlépése (60 ms) alatt próbált konfigurációt válltani (30-90 fokos csuklómozgatásokat végrehajtani) a robot. A csuklópozíció-tartomány ellenőrzését csak a konfigurációs probléma megoldása után szabad elvegezni. A legegyszerűbb megoldás, ha a legközelebbi konfiguráció kiválasztásakor nem törődünk a gyakorlati határértékekkel, és csak arra az egy konfigurációra ellenőrizzük azokat, amely mellett már döntöttünk. Ha ez nem ad jó megoldást, hibával kell visszatérnünk. (Kivéve, ha az inverz geometriai feladatot nem Descartes koordinátás interpoláció miatt hívtuk, hanem pl. egy térbeli pont lehetséges csuklóértékeinek meghatározása a cél.) A mozgást hibajelzéssel azonnal (sebességugrással) be kell fejezni. Ez a hiba is abba az irányba vezetett engem, hogy inkább ne a nearest módszer, hanem a konfiguráció-kódolás irányába haladjak.
64
3.3.10.3
Követelmények az invgo felé a különbözőfajta hívások esetén
Az eddig tárgyaltakból kitűnik, hogy az inverz geometria megoldásválasztási stratégiája nagyban függ attól, hogy milyen programrésznek van szüksége az eredményre. Ennek megfelelően a különböző programrészeknek a rutint különböző programkapcsolóval kell hívniuk. Ha csuklóinterpolációs mozgás esetén kell kideríteni az egy térbeli ponthoz tartozó csuklóhelyzeteket, akkor a 8 megoldás közül az algoritmus a felhasználó kénye kedve szerint választhat. Ha a kért kódú megoldás a robot mechanikája által nem elérhető, akkor azt ki kell írni, ha igen, akkor el kell vinni a robotkart a megfelelő a pozícióba. Így egy olyan konfiguráció, aminek helyzete távol esik a jelenlegitől, szép lassú csuklómozgatással elérhető. A robot végberendezése persze görbe pályát fog leírni. Ha világkoordinátás mozgatáshoz kell az inverz geometriai feladatot megoldani, akkor követelmény az egész mozgás alatt a pálya megtartása, és a kiindulási helyzet determinálja, hogy melyik az az egy konfiguráció a 8 közül, amit az egész mozgatás alatt tartani kell. 3.3.10.4
Konfigurációkódolás előnyei, követelményei (getconfig())
Amikor azt hittem, hogy a probléma ennyire egyszerű, a legjobbnak a 8 megoldás egyértelmű kódolása tűnt. Emellett szükség volt egy olyan rutinra, ami adott csuklópozícióból kinyeri a konfiguráció kódját. Az inverz geometriai rutint módosítani kellett úgy, hogy egy programkapcsolótól függően vagy fixen egy megoldást keres, vagy megmondja, hogy a 8 megoldás közül melyik teljesíthető a csuklótartományokat figyelembe véve. Ezt implementáltam, tesztprogramot írtam az inverz-direkt geometriai rutin konzisztenciájának a tesztelésére, a két inverz megoldási mód kódmegfeleltetésének, illetve a konfigurációkód-kinyerő rutin (geom_getconfig()) ellenőrzésére. Emellett arra is jutott időm, hogy az SZFÉRA rendszerrel ellentétben az algoritmus Descartes mozgatás esetén is engedje kijönni a robotot a szinguláris konfigurációkból mindkét megoldás irányába. Ehhez a valid_configurations_within_tolerance() függvényt is meg kellett írni. Ez a konfiguráció-kódolásos módszer egyszerűnek tűnt, és ennek körvonalazódásakor még nem ismertem a régi nearest módszer implementálásában rejtőző hiba egyszerű megkerülhetőségét. Ebben a módszerben jelentős mértékben előrehaladtam, amikor kiderült, hogy a 4-5-6 csukló esetében muszáj a nearest módszer alkalmazása. 3.3.10.5 Nearest eset szükségessége j456 esetében és súlyozott nearest előnyei j1-j2 esetben Bézi Istvántól sokszor hallottam már, hogy a SZFÉRA az ötös csukló esetében valamiért mindig a közelebbi megoldást választja. Ugyanis a 4-5-6 csukló esetében
65
előírható két olyan világkoordinátában (Rodriguez képlet szerint) egyenes mozgásnak számító pálya, amely a kiindulási térbeli helyzetbe viszi vissza a robotot, de ekkor az ellenkező kódú konfigurációban fognak állni a csuklók. (5-ös csukló átbillentése a túloldalra és közben a 6-os csukló +180-fokos forgatása. Ezután a 4-es csukló –180 fokos forgatása következzen.) A problémát úgy tudom érzékeltetni, ha rábeszélem az olvasót, hogy feledkezzünk meg a hatos csuklóról, és ábrázoljuk a 4-es és az ötös csukló helyzetét egy hengerkoordinátarendszer fix sugarú palástján. A henger tengelyére merőleges sugár forogjon együtt a 4-es szegmenssel, míg a függőleges koordináta az 5-ös csukló meghajlításának a szöge legyen. (-110 foktól +110 fokig). Ezen a hengeren a z=0 kor reprezentálja a végtelen sok szinguláris pozíciót. (5-ös csukló pont függőlegesen felfelé áll.) Az ettől feljebb levő palástrészekre mondja az arps, hogy J5 positive, és az alsó félpalástra, hogy J5 negative. A függőleges irányú mozgások világkoordinátában végrehajthatóak, sőt két egymás felett levő pont által jelképezett pozíció közötti világkoordinátás mozgáskor így kell interpolálni. Az azonos magasságú pontok közötti világkoordinátás mozgás a robot csuklójának helyzetét jelképező pontot a hengeren vízszintesen körbe viszi.
(ábra) Egy két vízszintes és két függőleges szakaszból (körívből) álló meghajlított téglalap négy sarokpontja által egyértelműen meghatározott. Ha ezeken GOS utasítással körbekörbe visszük a robotot, akkor a reprezentáns pontnak a téglalap oldalain kell mozognia. Nem tudjuk semmilyen konfiguráció-kódolási képlettel úgy kettéosztani ezt a hengerpalástot, hogy ilyen téglalap jellegű mozgással ne lehessen az egyik konfigurációból a másikba áttérni.
66
E probléma felismerésekor az inverz geometriai feladatot át kellett írni, hogy a csukló konfigurációs problémája esetében mindig a közelebbi megoldást keresse. (kivéve GO térbeli pont típusú utasításnál, ahol lehet kérni a távolabbi konfiguráció felvételét.) Ennek implementálása után gyorsan beleírtam a rutinba (invgeo()), hogy a teljesen szinguláris helyzetben a 4-es és a 6-os csukló által felvehető végtelen pozíció közül válassza ki azt, amelyik az előző megoldáshoz képest a legkisebb négyzetes eltérést adja. 3.3.10.6
Az invgeo rutin, a probléma geometriai megoldása
Az inverz geometriai feladat a PUMA geometriája esetében szeparálható. Az első rész az alsó három csuklóérték meghatározása a kiadódó 4-5-6 tengelyközéppont helyzetéből, míg a második egy inverz-Euler számítás a megfelelő mátrixszorzás után. Az első lépés megoldható geometriai, illetve analitikai úton is. A geometriai megoldás szerintem sokkal szemléletesebb, és a megoldásfa elágazásainak oka és mikéntje jobban kivethető belőle. A konfiguráció-kódolás természetes módon biztosított, hiszen a több megoldás alakja: J1=alfa+a*Beta, j2=+a*gamma-b*delta. J3=b*q3; ahol a=±1 és b=±1 a konfigurációkódoktól függően. Az algoritmushoz szukséges konstansok: #define DH_A2 0.431812 #define DH_D3 0.149101 //ide kellene a cikk szerint meg 1 milimeter. Ez az ertek van viszont beprogramozva a SZFÉRA vezerlobe #define DH_A3 -0.020311 // A Lantos konyvben levo modellek ugy szamolnak, mintha ez a tag 0 lenne. (ranezesre nem latszik a roboton) #define DH_D4 0.43306 #define DH_D6 0.0562698 #define DH_H3 0.433536 //a 3.szegmens hossza sqrt(a3^2+d4^2) double d4_2 = DH_D4*DH_D4; double a2_2 = DH_A2*DH_A2; double a2h3recfel = 0.5/(DH_A2*DH_H3); double a2_per_h3 = DH_A2/DH_H3; double d3_2 = DH_D3*DH_D3; double h3 = DH_H3; //a 3.szegmens hossza sqrt(a3^2+d4^2) double h3_2 = DH_A3*DH_A3+DH_D4*DH_D4; //(a3^2+d4^2) double h23 = (DH_H3+DH_A2); //a 2. es 3. szegmens hossza double h23_2 = (DH_H3+DH_A2)*(DH_H3+DH_A2); //a 2. es 3. szegmens hosszanak a negyzete. double alfa3 = 0.046866; //a 3. es az 5. tengelyt osszekoto egyenes (sik) es a 4. tengely altal bezart szog radianban.
67
//atan2(-a3,d4) És egyes változók: double RVizsz_2; //p(px,py,pz) viszintes vetulete hosszanak negyzete double Rtotal_2; // p hosszanak negyzete double r23_vizsz; //2.3. szegmens behajlitott vizszintes vetulete double r23_2; //2.3. szegmens behajlitott hosszanak negyzete Az algoritmus: //(px;py;pz)-be a 4-5-6 tengely közös metszéspontjának pozícióját beleírjuk. RVizsz_2=px*px+py*py; //a vektor vizszintes vetuletenek a negyzete Rtotal_2=RVizsz_2+pz*pz; //a vektor hosszanak a negyzete if(Rtotal_2>d3_2+h23_2) { invsolgeom_code=IG_TOO_FAR; fprintf(stderr,"TOO_FAR:Rtotal:%f,max:%f,px:%f,py:%f,pz:%f\n",sqrt (Rtotal_2),sqrt(d3_2+h23_2),px,py,pz); return 0; } //a 4-5-6 tengelykozeppont //messzebb van az origotol (1-2 tengelykozeppont), mint az 1. tengely //oldalratolasa es a 2.3. szegmens legnagyobb eggyuttes hossza altali //derekszogu 3szog atloja.
if(RVizsz_2
0;}
//
alfa=myatan(py,px); //felulnezetben az origot a 456 pontal osszegoto vektor szoge az x,y=1,0 vektorhoz kepest. r23_vizsz=sqrt(RVizsz_2-d3_2); beta=myatan(r23_vizsz,d3); //az 1. tengely(szegmens) mekkora szoget zar be a fenti vektorral. //q1=alfa+-beta r23_2=(Rtotal_2-d3_2); //a megoldasban a 2.3 szegmens behajlitott hossza. if(r23_2<=(h3-a2)*(h3-a2)) {invsolgeom_code= IG_TOO_CLOSE;return 0;} //a 2.es a 3. szegmens minimalis hossza d4-a2.
68
//egyenloseget azert nem engedek meg, mert ugyus magaba nyulna a robot. cos_q3=-((h3_2+a2_2-r23_2)*a2h3recfel); //cosinus tetel a 2. es a 3. szegmensre q3=acos(cos_q3); //pozitiv. gamma=myatan(r23_vizsz,pz); //a 2.3. szegmens altali eltolast jelkepezo szakasz (rajzon v23) szoge a fuggolegestol. delta=myatan(sqrt(1-cos_q3*cos_q3),a2_per_h3+cos_q3); //a 2. szegmens es v23 szoge, cosinusz tetel. //q2=gamma+-delta; //ha eddig van megoldas, akkor negyfele megoldas van. cout=0; if(!multiple) //csak egy megoldast keres; //milyen opcioval hivodott a rutin, hany megoldast keres? {qo=q;} else{qo=m_q;} //mutltiple solutions if(qo==NULL) {invsolgeom_code=-100;return 0;}//biztos ami biztos
for(i=0,j=0;i<4;i++) if(multiple||i==(configin&3)) { sz2=((i&J2_MASK)==J2_NEGATIVE?-1:1); sz3=((i&J3_MASK)==J3_NEGATIVE?-1:1); qo[0+j]=alfa+beta*sz2-FEL_PI; //-felPI, mert az x,y=0,1 vektor a 0 irany. beta*sz es nem -beta*sz, mert ha a ket lehetseges allas kozul a 2. csuklo pozitivabb, akkor az 1. csuklo is pozitivabb. qo[1+j]=gamma*sz2-delta*sz3; qo[2+j]=sz3*q3+alfa3; ... //innentől jön az orientációs rész kiszámítása, inverz-Euler feladat, stb. }
69
A vonatkozó ábrák:
(ábra:A teljesen szinguláris helyzet, és a j12 szinguláris helyzetén áthaladó mozgás véghelyzete összevetve a legkissebb elmozdulást adó megoldással)
70
(ábra:A váll, a könyök, illetve a csukló szinguláris helyzete, és az ezekben el nem érhető sebességkomponensek)
(ábra:A megoldások tere (px;py;pz) vektorra nézve)
71
(ábra: Az egyes változók, mint geometriai vektorok térbeli helyzete) Ez a megoldás nekem mindig jobban tetszett, mint az algebrai úton levezethető változat. A megoldásfa elágazásait egyértelműen lehet kódolni, míg az algebrai levezetés esetében a A*cos(α)+B*sin(α)=D egyenlet két megoldása felcserélődhet, mivel köztük a sinus, illetve a cosinus értékének nagysága között kell hogy válasszunk. Persze ennek az egyenletnek is lehetne geometriai megoldást adni: atan2(A,B)±acos(D/√(A^2+B^2)) *, mely után a kódolás egyértelmű lenne. A szoftverben implementált rutin az Euler csukló szinguláris állásában a végtelen lehetséges megoldás közül a legkissebb négyzetes hibát adót választja ki. Ehhez szükség van az előző megoldás ismeretére, de ez amúgyis bemenő paramétere a teljes inverz-Euler problémának, mert az egyébként lehetséges két megoldás közül nearest módszerrel muszály választani a sima működés érdekében. *Itt atan2-argumentumainak cos-sin sorrendet véve alapul. (Ez szerintem közelebb áll a megszokott vízszintes-függőleges koordináta sorrendhez.) 3.3.10.7
Konfiguráció-bitmezőket maszkoló hívások: pathgen, pendant
Az inverz geometria rutin két helyről hívódik úgy, hogy az eredmény Descartes mozgatáshoz lesz felhasználva. Ezek a részek a pathgen.c illetve a pendant.c modulokban találhatóak meg. A pendant működése kicsit egyszerűbb. A TOOL, illetve WORLD módok közbeni mozgatásoknál lekérdezi, hogy az adott pozíció mely konfigurációk esetén érhető el. (A szinguláris helyzetek több konfigurációban érhetőek el, és ezt kezeli a program.) Ez egy 72
8 bites kóddal jelez vissza, egy bit egyes, ha az aktuális pozíció Descartes mozgatással elérhető az adott konfigurációban. Ezek után az elérendő következő pozíció világ koordinátáival meghívja az inverz geometriai rutint, melyet több megoldás keresésére utasít. Ez is egy 8-bites visszatérési értékkel jelzi a szóba jöhető konfigurációkat. A két kódot logikai AND műveleten áthajtva megkapható, hogy van-e közös konfiguráció. Ha igen, akkor az egyik ilyenben elmozdítja a robotot, ha nem, akkor sebességugrással megáll. A pathgen ezt a maszkoló tesztet elvégzi az aktuális pozícióra (B0 pont, csuklókoordinátákkal adott, valid_configurations_within_tolerance() hívás), illetve a B (elkerült pont) és a C (mozgás vége) pontokra. (Több megoldást kerestetve hívja az invgeo()-t). Ha nincs közös megoldás, akkor C vektorba betölti B értékét, és folytonos mozgatással megáll B-ben. Ha van, akkor kinyeri annak a kódját, és az interpolációs ciklus alatt az inverz geometriai rutinnal csak az ilyen kódú konfigurációban levő megoldást keresteti meg. Ha valamely köztes pontban már nincs ilyen kódú megoldás (invgeo() hibával tér vissza), akkor kénytelen sebességugrással azonnal megállni. 3.3.10.8
Arps konfigurációállítást figyelembevevő hívás: csak GO-hoz
Az inverz geometriai rutint az Arps processz hívja olyan célból, hogy egy világkoordinákban adott pont csuklókoordinátáit megtudja. (Egy majdani csuklóinterpolációs mozgatáshoz.) Ilyenkor az elmélet nem tartalmaz megkötést a választandó konfigurációra, ezért az Arps szintén egy megoldás keresésével bízza meg az inverz geometriai rutint, de ennek a kódját nem az aktuális helyzet adja, hanem a felhasználó óhaja. Ezt a felhasználó a konfiguráció-módosító utasításokkal közölheti. Az eredeti rendszer ezeket csak egy mozgás erejéig veszi figyelembe, de a mienkbe az még nincsen beleírva, hogy mozgatás után írja felül a változókat az alapértelmezéssel. 3.3.10.9
Egy előforduló, de nem kijavított hiba
Ha a robot Ready helyzetéhez képesti (-200;200;-200) pontból (400;0;0) irányba mozgattam egyenes mozgatással a robotot, akkor az X=0 sík keresztezésekor leállt a robot. Hasomló leállás fordul elő, ha kb. speed 300-as vagy nagyobb sebesség mellett egyenes pályán mozgatjuk a robotot szinguláris helyzet közelében. Ilyenkor a hajtások áramtalanítása után megszűnik a blokkoltság. Valószínű, hogy akkor lép fel a probléma, ha túl nagy a TMS-nek már elküldött mozgások összegzett hátralévő ideje. Mostanáig nem volt alkalmam alaposabban letesztelni, remélem a diplomaterv védésekor bővebben be tudok majd a probléma okáról, lehetséges javításáról számolni.
3.3.11 A sebességtúllépés Az 5-ös csukló szinguláris pozíciójában egyes nem csuklókoordinátában interpolált mozgások sokkal durvább mozgást adnak, mint a 2-es csukló szing. konfigurációjában. Ez azért van, mert a világ-csukló függvénynek a 2-es csukló szing. pontjában csak 73
végtelen sebessége van, míg az 5-ös csuklónak véges pozícióugrása. Az ilyen eseteket a robot felügyeleti szoftverének kell figyelnie, de a robotot mozgató részeknek sem igazán lenne szabad ilyen mozgásokat előírni. Megoldás: egyenes vonalú mozgás esetén monitorozni a limittúllépést, és az eredeti pályán, de lassabban végigmenni. Tehát a robottól nem lehet elvárni az ilyen pontok környezetében sebességelőírt mozgást. Megelőző megoldás: a robot szinguláris konfigurációba csak csuklókoordinátákban interpolálva vihető, és hozható ki. A probléma csak világkoordinátás mozgás esetén lép fel, mert csuklómozgásnál a sebesség betartása egyszerűen megoldható. A SZFÉRA vezérlő nem kezeli a problémát, hajlandó ezekben az esetekben kiadni a túlzott csuklósebességeket. Ilyenkor a fogaskerekek felsüvöltenek. Miután az inverz geometriai konfigurációk megfelelő kezelését beleírtam a robotot mozgató két processzbe (pendant és arps), a konkrét robotmozgatás kiadása előtt beraktam a sebesség ellenőrzést. Ez limittúllépés esetén megnyújtja a mozgás előírt idejét. A ciklusváltozó (időt ábrázol) tartalma nem változik. A későbbiekben a robot.c-ben létrehoztam a robot_point_check_rad() függvényt, mely a robot-point függvény meghívása előtt elvégzi az ütközésdetektálást, a sebességtúllépés tesztet, felülírja a közös memóriában az utolsó pont értékét, elvégzi a radián-inkremens átváltást (egymásrahatás számolásával együtt), illetve a dynjac-által életbeléptethető tiltás (joint_shm move_enabled tagja) ellenőrzését. Ezeket az előzetes dolgokat a robot-point összes hívása előtt el kellett végeztetni. Ezután a robot-point hívásokat kiváltottam az új megoldással. A robot-point, illetve robot-droppoint hívásokat csak különleges esetekben szabad hívni, (kalibrációs görbe felvételekor, illetve break-es interrupt esetén.
3.3.12 A kommentezés megjegyzésekről
eddigi
szűkösségéről,
a
pótolt
A forráskódba minden modul elejére odaírtam a szerepét röviden. Ha ez a modul egy processz main() függvényét is tartalmazza, akkor a processz szerepéről is írtam egy kicsit. Ezekbe a megjegyzésekbe beleírtam, hogy melyik diplomaterv melyik oldalán találhatnak az érdeklődők bővebb leírást. A robot.c modul függvényeinek leírása majdnem sorrendben megvolt Huszti Ákos vonatkozó fejezetében. Ezeket egy az egyben át lehetett másolni. Az egyes processzek, modulok szerepét taglaló bekezdéseket igyekeztem a forrásfájlok elejére is átmásolni. A sarkalatos, magyarázatra szoruló részekhez is másoltam leírást, ha találtam, egyéb esetben megírtam a megjegyzést. Az eredeti megjegyzések amúgy egy-egy függvényen belüli eligazodáshoz megfelelő mértékűek voltak. Az egyes változók szerepe szinte mindenütt kimerítően kommentezve volt. Csak a nagyobb egységek megértését segítő kommentek hiányoztak. A legtöbb komment angolul volt megfogalmazva, de itt-ott egy-egy változónév, kommentezés magyarul lett megírva. Én szívesebben használok magyar kommenteket, és a diplomatervekből átvett leírásokat sem állt szándékomban önszorgalomból lefordítani. Jelenleg a kommentek jó része magyarul van.
74
3.3.13 PC-LAB-DOUT- regiszter több példányban létezett (pendant és arps globális változóját SHM-be kellett rakni) A pendant vészleállításának bemutatásakor szóba jött, hogy a digitális IO utolsó kiküldött jelét minden processz saját globális változóban tárolta. Ez inkonzisztenciához vezethetett volna, bár az arps-en kívül csak a pendant állította ezt, és az is a megfogót nyitogatta. A hibát javítottam. Az, hogy a pendant open gombjának megnyomásakor nem ment nulla ki az enable és a brakes vezetékekre, úgy volt megoldva, hogy a robot_open-close hívások fixen enable=1, brakes=0 értekkel felülírták az eddigi kimenetet. Ezt a hibát elég későn javítottam, előfordulhat, hogy az eddigi fejezetekben erről, mint még meglévő hibáról írok. Mint már említettem, probléma lehetett volna abból, ha az arps éppen a digitális kimeneteket állító programot futtatott volna. Ilyenkor ugyanis az arps globális robot_doutreg-jében tárolt érték alapján határozta meg az új kiadandó outputot, és visszakapcsolhatta a vészleállítás által kikapcsolt robotot. Ha ezután értesült volna az arps a vészleállásról, a robot bekapcsolt állapotban maradt volna még egy ideig.
3.4 Az RT feladatok által támasztott követelmények vs. objektum orientáltság Amikor a c++ nyelv új fordítóit kereskedelmi forgalomba hozták, akkor nem igazán törődtek az objektum orientált programok hatékony fordításával. Így ezek a c fordítók (de a maiak is) c stílusban írt kódot igen hatékonyan tudnak optimalizálni, felismerik pl. a matematikai képletekben a konstans kifejezéseket, és ezek kiszámítását fordítás alatt végeztetik el, nem programfutáskor. Emellett a c++ stílusú kód sebességoptimalizált fordítására nem fektettek súlyt, hiszen a kezdeti időben a legtöbb olyan alkalmazás, amelynek a c++ stílusra nagy szüksége volt, nem volt sebességorientált. Leginkább a felhasználói oldal erősítésére használtak az új technikát, és ezek a programok idejük nagy részét úgy is a felhasználóra való várakozással töltötték. Ha megnézzük egy erősen c++ stílusú program lefordított kódját, igen sok tovább optimalizálható részt találunk benne. Még a mai legjobb fordítók sem képesek felismerni a this pointer konstans értékét, és egy befordított inline függvény használatakor a [this+0x0c] típusú címzéseket ténylegesen kiszámolja a gép. (más kérdés, hogy a processzorokat azóta keményen a c++ jellegű műveletekre optimalizálják). Ha régi, C-s #define-os makrót használunk, úgy gyorsabb kódot érhetünk el, mint inline függvények használatával. Ez a sebességkülönbség leginkább tömeges műveleteknél lenne jelentős, pl. nagy ciklusok vektorműveleteiben. Sajnos operator-overload esetében makrót nem lehet használni, így marad az inline által behozott lassulás. (Az inline az átadott paramétereket megduplázza a memóriában. Osztály esetében a copy konstruktor is meghívódik, ha az nem default. Pedig az adott elem a hívó rutin memóriájában már létezik. A copykonstruktor hívása csak akkor indokolt, ha az inline függvény módosítja az argumentumát. Hiába deklaráljuk const típusúra a paramétereket, a másolás ekkor sem marad el.)
75
A kezdetben megálmodott osztály-könyvtárak, újrafelhasználható szoftver IC-k gyűjteményének a fordítókhoz árult standard lista, és egyéb osztályokon kívül a legtipikusabb képviselője az MFC. Ennek használatával villámgyorsan lehet olyan felhasználói felületű programot létrehozni, aminek nemcsak kinézete, de működése is az adott OS programjai esetében megszokott és elvárt stílusú lesz. Az objektum orientált stílus jobb hiba-elkerülő képességét hozza is a rendszer, viszont az adott osztály egy típusán éppen tilalom alatt álló művelet elvégzésének megakadályozása érdekében minden hívás többszörösen áthajtódik az egyes ellenőrzésekért felelős modulokon. A standard Windows alkalmazások esetében ez nem jelentett túl nagy problémát. Ha ezeknek a programoknak a sebessége kritikus is volt, az nem a többi programokkal közös részeket érintette, hanem az adott programra jellemző egyedi részeket. Ezeket az illető szoftvercég saját hatáskörében is tudta optimalizálni. (Példái lehetnek ennek a Photoshop 2, illetve konvolúciós műveleteknél 4 dimenziós belső ciklusai, melyek valószínűleg optimalizálva vannak minden színábrázolási formára, és nem tartalmaznak a milliós ciklusok legbelsejében pixelszín-kiolvasó osztályhívást.) A játékprogramok területén (akkoriban legnagyobb gazdasági erőt képviselő RT alkalmazásterület) viszont nem nyert teret a Windows. Ha a Microsoft nem tesz valamit az OOP és a RT stílus közelítése érdekében, akkor biztos vesztes lett volna, és a mai PC-s játékok még mindig Dos4gw alatt futnának. A választott megoldás a DirectX bevezetése volt. Ennek a filozófiája nagyjából a következő. Ahelyett, hogy minden rendszerhívás esetében a hívás paramétereitől szükséges elágazásokat futás időben értékelje ki minden osztály-könyvtár (DLL), szükség esetén a hívást egy alacsonyabb szintű DLL felé irányítva, az összes ellenőrzés az alkalmazásba belefordított, (de nem módosítható, így a specifikációkat mindenképpen teljesítő) függvényekkel történik, melyek többszörös hívások helyett egyből a konkrét periféria driver programját hívják. Hogyan kapcsolódik a DirectX megoldása a robotvezérlőhöz? Képzeljük el, hogy egy alacsony szintű processz által tiltani szeretnénk bizonyos esetekben a magasabb szintű processzek robot-mozgatását. Az eredeti c++-os felelősségmodell szerint ehhez minden robotmozgás hívást át kell hajtani ezen a processzen. A függvény magjában egy if kiértékelése esetében egy feltételes továbbhívás lenne. Ez QNX üzenetekkel megvalósítva a teljes ciklusban kettővel több taszkváltást eredményez, mintha a magasabb szintű processzek általi hívásokba belefordítanánk, hogy ők tesztelik a feltételt. Ehhez meg kell kapniuk az alacsonyabb processz megfelelő státusz változóját. Ha ezt csak olvashatóvá tesszük a magasabb szint számára, akkor egy osztott memóriás elhelyezés nem igényel szemaforhasználatot. Igazi OOP stílus megkövetelné, hogy az osztott memória osztály a megfelelő címen egy uniót tartalmazzon. Egyik eleme sima változó lehetne, másik ennek const változata. A const változatot public-nak, míg a normált private-nak deklarálva biztosított, hogy fordítás idejű hibajelzést kapjunk, ha valaki megsérti a protokollt. A robot-point-check-rad() hívás dynjac-általi blokkolhatósága így van megvalósítva, persze nem c++, csak c kódolással. Összefoglalva, az OOP nyakló nélküli használata RT alkalmazás esetében nem megengedhető. Az egyes OOP előnyök használatakor erősen oda kell figyelni, hogy az OOP kód hátrányától, azaz a lassúságtól megkíméljük programunkat. Ellenkező esetben, amit nyerünk a várható hibák előfordulásának csökkentésével, azt elveszítjük az időzítési viszonyok esetleges, de durva kimenetelű felborulásával.
76
4 .fejezet: A fejlesztés közben felmerülő hibák, hiányosságok, valamint továbbfejlesztési feladatok, és megoldásuknak lehetséges módjai 4.1 Bevezetés A szoftverrendszer abban az állapotban, ahogy én az előttem dolgozóktól megkaptam, működőképes volt. A legtöbb robotművelet úgy-ahogy bemutatható volt rajta. Kötelességemnek éreztem ezt a tulajdonságot megőrizni, és csak olyan fejlesztésekbe, átalakításokba belefogni, amelyeket az elejétől a végéig be tudok fejezni a kiszabott idő alatt. Az új programrészek tesztelése időigényes, egy-egy becsúszott hibát nagyon nehéz megtálalni, mivel a rendszer valós idejű debuggolására nincs mód. A log-fájlok segítenek egy kicsit, de nagyon sok futtatás, és újrafordítás kell ahhoz, míg pont azokat a változókat loggoltatja az ember, amelyek rámutatnak a hiba okára. Ezért hiába tártam fel jóval több javítani valót annál, amit végül meg is valósítottam, mégis egy sor minimális kódolást igénylő változtatást kellett megoldatlanul hátrahagynom, mert a javítás utáni tesztelés közben esetlegesen felmerülő hibák várható időigénye túlmutatott a kereteimen. Ebben a fejezetben az ilyen megmaradt változtatandó dolgokat részletezem. Ha abba bocsátkozom, hogy a megoldás módjára is javaslatot teszek, akkor igyekszem minden felmerülhető nehézséget végiggondolni. Ha valaki kedvet kapna egy ilyen rész javításához, de nincs még a rendszerről eléggé átfogó képe, jól jöhetnek ezek a javaslatok. Ha viszont megvan a saját konkrét elképzelése a javítást illetőleg, válassza azt, mert az ember a saját maga döntése miatti kellemetlenségeket sokkal tovább képes elviselni, mint a máséit. Előre is elnézést kérek, de egy-két fontosabb hibát szükségesnek tartottam a jelenlegi működést bemutató résznél részletezni, és ennek ellenére ezeket itt is meg fogom ismételni. Ekkor utalok arra, hogy erről vagy más szavakkal írtam már, vagy hogy az adott bekezdést egy az egyben megismétlem. Ezt azért érzem szükségesnek, hogy az összes, most is látható hiányosság kimerítően részletezve, egybegyűjtve benne legyen ebben a fejezetben. Ezúttal hozzájárulok ahhoz, hogy ha valaki egy ilyen problémát megold, akkor a tanszéki diplomaterv példányban a ’javítva’ megjegyzést ceruzával bejegyezze az oldalszélre, de feltétlen jelölje meg a dokumentációt (oldalszámmal), amelyben a megvalósult megoldást részletezi.
77
4.2 A rendszerben levő hibák, hiányosságok 4.2.1 A QNX alkalmazásai rendesen telepítendőek (ismétlés) A Qnx partícióra szinte semmilyen program nem lett felinstallálva, a régi gép winchesterének tartalma lett átmásolva az 5 lemezes install után. Ezért nem működik a photon sem a gépen. Az is csoda, hogy a C fordító megfelelően működik rajta. Szerencsés lenne, ha idővel valaki felrakna rá minden programot.
4.2.2 A dinamikus modell Coriolis tagja javítandó (ismétlés) A Coriolis tag az implementált modellben Huszti Ákos műve. Lantos Béla többször kifogásolta, hogy a [0][0] ([1][1]) indexű tagnak elméletileg 0-nak kellene lennie. A Stanford-cikkben közölt modellben ez annyi is. Jelenleg nem javítom ki a forráskódokat, mivel a tesztelésre nem lesz már időm. A probléma kijavítását meghagyom annak, aki kellő időt tud szánni a CTM szabályozás javítgatására. Amennyiben ezt valaki megteszi, ügyeljen arra, hogy a modell mind a Host geometr.c moduljában, mind a TMSpos.c modulban javítandó, és újrafordítandó! Ügyeljen arra is, hogy a két modellnek eltérőnek kell lennie. A csuklóegymásrahatás miatti lineáris transzformációt (virtuális csuklók -> valódi motorszögek) rá kell ereszteni a TMS-en implementált modellre.
4.2.3 Az init hiányossága, a robot megtörése Egy vészleállítást követően (régi Descartes interpoláció monitorozása közben kellett leállítani), amikor nem ready, hanem erősen oldalranyúló helyzetben volt a robot, megkíséreltem újrabootolás nélkül elindítani a rendszert. Sajnos a Hoston futó programrendszer nem vette észre az ARC-kártyák működésképtelenségét, és kiadta a fék elengedés jelet úgy, hogy nem ment ki áram jel a hajtásnak. A standard inicializálás logikája kihasználja a pc-lab kártya kimenetei Power-on utáni állapotát. Valószínű, hogy ebben volt a hiba. A probléma megoldandó a méréseken való biztonságos használhatóság érdekében. A busz bővítő kártya tápellátó és reset mechanizmusát sem ismerem, nem tudni, hogy ilyen működéskor van-e tápkimaradás az ARC és a PC-lab kártyákon, illetve, hogy mikor kapnak reset jelet. A robot a csuklója alatt sérült. A legfrappánsabb megoldás az lenne, ha fék elengedés esetében (induláskor) már elindítaná a pozíciószabályozást alacsony áramkorlátokkal (alsó 3 motornak max 5%), és egy bizonyos pozícióhiba esetén vészleállna. Jelenleg a csuklók elengedésekor kiadott gravitációs kompenzálás fogja a Robotot, habár a legnagyobb tag előjele helytelen volt. Erre az esetre is hasznos lenne egy csuklósebességmonitorozó algoritmus, amely kikapcsolná a csuklóelengedést.
78
A hajtások áramtalaníthatóak mind a vészleállítógombbal, mind a középső kulcsos kapcsolóval. Ilyen áramtalanításkor tovább világítanak az R,S,T vezetékek, és a bővítő PC-házon is tovább ég a táp-led. Jó lenne, ha ilyen részleges áramtalanítás után képes lenne a szoftverrendszer bootolás nélkül újraindulni. Ennek teszteléséhez jobb alápárnázni a robotot, és ready helyzet helyett teljesen alátámasztott helyzetben hagyni a robotot. A szoftverrendszer úgy is elindul, ha nincs a hajtások árama bekapcsolva. Ez hiba, bár egyes dolgok tesztelésénél jól jöhet.
4.2.4 A Cntrl-break valamint pendant-emergency-stop után lefagy a program Az említett két esetben a rendszer normál működése megszakad, még kilépni sem lehet az arps-ból. Másik QNX konzolról a bennmaradt processzek lelőhetőek, illetve a processzállapotok analizálhatók.
4.2.5 A deinit hiányossága, kétszeri indítás szükségessége Az előző bekezdésben tárgyalt eset azért is furcsa, mert az arps-ból való szabályos kilépés esetén minden első újraindítás arps-com init failed üzenettel kiszáll, és csak a második próbálkozás sikeres. Ha a rendszer fagyása után (előző bekezdés) egy másik konzolról lelövöm a megmaradó console-t és az arps-t, akkor a következő indításra elsőre elindul a rendszer. Úgy látszik a QNX kill jobban eltakarítja az arps maradványait, mint saját maga deinit-je.
4.2.6 A TMS szabályozásában a limittúllépések figyelését vissza kell kapcsolni! Nem nagy kódolási, fordítási feladat, de alapos tesztet kíván a megfelelő limitek tapasztalati meghatározásához. Sajnos nincs már rá időm.
4.2.7 A nagy időzítésű mozgató-üzenetek blokkolásához vezetnek.
a
rendszer
teljes
A túlzott csuklósebességek elkerülésének következtében a TMS-eknek elküldött pontelőírások időtartama meghaladhatja ez eddig általános 60 ms-ot. Mennél nagyobb sebesség van beállítva, annál nagyobb értékek fordulnak elő, de az értékek maximumára szinguláris helyzet közelében kell számítani. A próbafuttatások alkalmával 100-as sebesség mellett egyszer sem, míg 200-as sebesség mellett igen gyakran történt meg, hogy szinguláris pozíció közelében, egyenes mozgatás közben megállt a 79
programrendszer. Ekkor a robot sebességugrással megáll. (Legalábbis eddig mindig így történt.) A vészleállító gomb megnyomása után bizonytalan időn belül a blokkoltság megszűnik. Másik QNX konzol indításakor ez általában bekövetkezik. Szerintem a kommunikációs láncba csúszik valami hiba. Elég körülményes lesz ezt debuggolni, a log-fájl írás mellett ajánlanám az osztott memóriák kibővítését, ebbe egy-két állapotváltozó tükrözését, és egy külön konzolon futtatható SHM-monitor alkalmazását. Nem tartom kizártnak, hogy a hiba egyik oka a TMS, illetve az IEX programjában keresendő.
4.2.8 TMS-re a deginc-átváltási konstansokat is fel kell tölteni (ismétlés) A szabályozások megkövetelik, hogy a TMS kártyán konvertáljunk inkremensekről radián értékre. A TMS-en levő konverziós konstansok a legelső BME-mérésekből származnak, így nem azok, amikkel a Host számol. Ezeket a konstansokat is a robot.cben definiált értékekkel kellene felülírni. Ehhez bővíteni kell a dcs-param struktúrát. Ugyanígy a nyomaték-DA-egység átváltási konstansok (axis_curtorq[]) is beállítandóak lennének. Ezek után az egész szoftverrendszerben már csak két modul tartalmazna paramétereket: a robot.c modul és a geometr.c modul. (A dinamikus modell TMS-en levő megismétlését nem számítva.) Az nem baj, hogy az egymásrahatás a TMS-en levő inc-rad konverzióban nincs benne, mivel a TMS nem csukló, hanem motorszögekben szabályoz.
4.2.9 Az arps processzben sok megengedhetetlen utasítás nincs tiltva kalibrálatlan állapotban Csak a relatív csuklómozgatást és a kalibrációt szabad megengedni. Lásd Szoftverrendszer ismertető (2.) fejezet kalibrációs (utolsó) bekezdés. Ennek ismétlése: Az arps interpreter bekapcsolás után feltételezte, hogy a robot ready helyzetben van. A robot_start függvény egy előzetes kalibrációt hajt végre, egy nullimpulzusnyi pontossággal tudja a robot abszolút helyzetét. Az arps hajlandó eltárolt pontokba csuklóinterpolációval elmenni kalibrálatlan állapotban. Szerencsére Descartes mozgásra nem hajlandó. A megfelelő megoldás az lenne, ha csak a parancssorba irt relatív csuklómozgatást, valamint kézivezérlő csuklómozgást engedélyezné. Tiltani kellene a GO pontnév típusú mozgást, az lteach utasítást, és a program futtatását kalibrálatlan állapotban, illetve minden, nem megengedhető utasítást. Vigyázzunk, ne tiltsunk túl sok mindent. Például programok betöltését engedélyezni érdemes, hogy a robot működésképtelensége esetén is lehessen ezek szintaktikáját ellenőrizni. Azaz ne tiltsunk semmit fölöslegesen.
80
4.2.10 Pendanttal való mozgatás sebességugrásos A pendant robotmozgatása a mozgató gombok lenyomása után azonnal a beállított sebességgel egyező sebességű mozgatást ír elő. Ez sebességugráshoz, azaz hatalmas gyorsuláshoz vezet. Még csuklókoordinátákban történő mozgatásnál is rúg a robot, ha nagy sebességre van a pendant állítva. Azonban Descartes koordinátás mozgatás esetében szinguláris konfiguráció problémás irányból való közelítésekor, illetve elhagyásakor bármekkora is a beállított sebesség, a robot egyből maximum sebességű alapjelet kap. A megoldás egyik része az lenne, hogy a sebességtúllépést kezelő rész (robot.c robot_point_check_rad() függvénye) az aktuális sebességszorzót is figyelembe venné. (Pathgen esetében ez a speed sebességszorzó, míg pendant esetében a pulton állított sebesség. Ezt a robot_point_check_rad() függvénynek plusz paraméterben kéne átadni.) Ez programból való irányítás mellett is csak akkor süvöltetne a robot fogaskerekeket, ha speed kb. 300 fölé van állítva. A fontosabb teendő, hogy a pendantnak a folyamatosan lenyomva tartott mozgatógomb hatására fokozatosan kellene a beállított sebességet elérnie. A gomb elengedését előre jelezni nem lehet. Így elengedés után tanácsosabb jóval nagyobb fékezőgyorsítást kiadni, mint felgyorsuláskor. A kezelőt a mozgás megkezdésekor levő lassú felgyorsulás emlékezteti arra, hogy a gomb felengedése után nem fog egyből megállni a robot. Így biztosabb, hogy az elérni kívánt pont előtt engedi el majd a gombot, de a robot a nagyobb fékezőgyorsulás miatt a vártnál hamarabb fog megállni. A megengedett gyorsulás lehet a sebességtől függően állandó, illetve azzal arányosan változó. Utóbbi állandó idejű gyorsításhoz, illetve fékezéshez vezet, ezt is jól meg lehet szokni. Az állandó fékezőgyorsulás használata kicsit veszélyes, lévén minél gyorsabb sebesség van beállítva, annál tovább (távolságban négyzetesen) fog mozogni a robot a gomb felengedése után. Fékezéshez mindenképpen állandó idejű lassítást javaslok. A problémát nem kell túldimenzionálni, így azzal, hogy ez a megoldás gyorsulásugrásos lesz, nem érdemes foglalkozni, mivel a gyorsulásugrás elkerülését leprogramozni alapjel szinten túl komplikált. (Hasonló problémák legegyszerűbb megoldásához a következő módszer használható: Egy virtuális szabályzókört és szimulátort kell lére hozni, mely szaturációs (korlátozó) tagot tartalmaz a gyorsulás deriváltjára (’rúgás’-ra) nézve. Az ebből kijövő tényleges pozíció (és sebesség) jeleket lehet alapjelként továbbküldeni.)
4.2.11 Interrupt megoldása. Utalás a szféra teszt eredményre (Ismétlés) A pathgen processz tárolja az arps-tól kapott mozgás-kérelmeket. Ez szinte lehetetlenné teszi az interrupt utáni helyreállítást. Ezt a pathgen az arps blokkolásának elkerülése érdekében teszi. Egy lehetséges megoldásról részletesen írtam már a szoftverrendszert ismertető fejezet pathgen-arps kommunikációt tárgyaló bekezdésében. Ennek Ismétlése: Somlói Dániel fontosnak tartotta az arps processz folyamatos működését, mert a vészleállítást követően ő értesít minden egyéb programot, mely a robotvezérlő bármely 81
processzorán fut. Így a pathgen igyekszik nem várakoztatni az arps-t, az általa kapott teljesítetlen pontelőírásokat memóriájában tárolja. Ennek az arps-interrupt beütésekor isszuk meg a levét. A rendszerről levő ismereteim szerint az arps processz nyugodtan blokkolható lenne, ha a teljes rendszer indítása és lelövése egy kifejezetten nem blokkolható processz feladata lenne. Ez lehetne akár a messenger, akár a dynjac. Amennyiben a dynjac szabályozó processzé lépne elő, úgy mindenképpen őt kellene elsőnek értesíteni a vészhelyzetekről. A pathgennek csak a következő pontelőírást kellene átvenni az arpstől, azt is csak a lassító szakasz megkezdése előtt. Így egy interrupt rutin lefolyása után az arps-nek csak egy mozgást kellene újra elküldenie. [Somlói Dániel diplomatervének 50. oldalán] ír az arps-blokkolás elkerülésének az okáról. Így utólag könnyen látszik, hogy az interrupt-bemenetek egy másik processz általi pollingolása egyszerűbb megoldáshoz vezetett volna. De 1999-ben még nem létezett a messenger processz, és a dynjac is kísérleti stádiumban volt. Egy teljesen új, Inicializaló-felügyelő-lelővő processz létrehozása is alternatív megoldást jelentene. Ezzel mind a messenger, mind a console feladata szükségtelenné válna, így a processzek száma csökkenhetne. Szoftvertchnológiai megfontolásból is jobb lenne a rendszer indítási és felügyeleti szerepkört elvenni az interpretálást végző processztől. Jelen megvalósításban lehetetlen a mozgás közbeni interrupt utáni megfelelő folytatás. Az AUT tanszéken tartott mérési feladat az interrupt utáni pont újbóli kiértékelését igényli. Ezt a SZFÉRA-36 rendszere meg is teszi. Az interrupt működését Somlói Dániel interpretálta: [Somlói,1999 15.old.] A C-for-robot bekezdésben (lejjebb) is szól pár sor a probléma lehetséges javításáról. Ez a rész az interrupt kezelésére tesz javaslatot arra az esetre, ha C-for-robot library-t használó program fut. Ügyeljünk arra, hogy interrupt után a következő pont megismétlése előtt az arps számolja újra a pont helyzetét. (A SZFÉRA is megteszi ezt, és ez a funkció kell az Aut. tanszéken tartandó meréshez!)
4.2.12 Az arps.c UseConsole() hívásában a where parancs általi kiíratás hibás Az strproc.c-ben levő Printf() függvényt használta. Ebben nem fprintf(stderr,...) hanem sima printf hívás volt. Javítottam, de a kiíratás sajnos nem folyamatos. (Talán ha a Receive()-helyett Creceive()-lenne a UseConsole()-ban, akkor az megoldaná a problémát. )
4.2.13 A makefile hibás A makefile függőségeket leíró része csak a .c állományokat tartalmazza, a header állományokat nem. Így egy-egy include fájl változása nem biztos, hogy a következő fordításkor érvényre jut. Az osztott memóriák struktúrája tipikusan header fájlokban van deklarálva. Több alkalommal is előfordult, hogy egy-egy új tag bevezetése után nem minden vonatkozó processz fordult újra, és így a szemaforokat különböző helyen levőnek hitték. Ez a rendszer blokkolásához, azaz sem-blokked állapotokhoz vezetett. 82
Ezt egy új QNX konzol indításával (ctrl-alt-2 után bármely billentyű) és a ‘ps’ paranccsal meg lehet nézni. Ilyenkor a ‘host/obj’ könyvtár törlése segít a problémán. A legjobb lenne a makefile-t javítani.
4.2.14 A pályatervezés hibái Nem teljesen megoldott, hogy ha a pathgen csuklóinterpolációról vonalinterpolációra vált menet közben, vagy fordítva, akkor is pozíció, sebesség és gyorsulás folytonos legyen a mozgást. Van egy kódrészlet az egyenes szakasz végén, amely Descartes interpoláció esetén a pillanatnyi csuklósebességet eltárolja. Ezt felhasználja a csuklóinterpoláció eleje. Így GOS-GO átmenet megoldott. A Descartes interpoláció maga számítja az indulási sebességet prevv=(B-B0)/Tau, ám ez nem egyezik meg a tényleges sebességgel. Ez jelenleg sebességugráshoz vezet. A Lantos-féle negyedfokú interpolációnak van egy speciális feltétele a végpontok pozíciókülönbségeit, illetve a végsebességek összegeit illetőleg. pdiff=(vbe+vki)*tau, mert pdiff=p(tau)-p(-tau)=2*a1*tau (mindem más tag kiesik, a3=0), illetve a1=0.5*(vbe+vki) [Lantos-könyv, bemutatását.
148.-149.old]
tartalmazza
az
említett
negyedfokú
interpláció
Ez a gyorsulásprofil szimmetriája miatt teljesül, és viszont: ennek teljesülése biztosítja, hogy a végpontokban 0 legyen a gyorsulás. Emiatt nem lehet tetszőleges bemeneti sebességet előírni az interpolációnak, ha B0,B és C (p(-tau),p(0),p(+tau)=B+(C-B)/t*tau is) adott. 1. kiút: feladjuk a speciális GOS-GO ill. GO-GOS átmenetekre a gyorsulás folytonosságát, (GOS-GO-nal ez így van) és nem 'szabályos' sebességet írunk elő kezdősebességnek. (GO-GOS-esetre bele kell írni egy numerikus deriválót (csuklósebességből -> Descartes) vagy a Jacobi -mátrix technikát kell használni. Az előbbi egyszerűbb, de a konfiguráció-megtartásra ügyelni kell. 2. kiút: folytonos mozgásnál egyszerre interpoláljuk az előző mozgás lassító szakaszat a következő mozgás gyorsító szakaszával, majd az eredményeket csuklókoordinátákban szuperponáljuk. Az egyszerre interpoláció jelen esetben a legegyszerűbben úgy oldható meg, hogy a Descartes illetve a csukló interpoláció idejét (STR-TAU-t és FREE-TAU-t) kötelezően egyenlővé tesszük. Érdemes az interpoláció szimulációs időlépését (FREE_TS és STR_TS) minden esetben egyenlővé tenni.
83
4.3 A továbbfejlesztés lehetséges feladatai 4.3.1 A pályatervezés gyorsítása (a robot és nem az algoritmus sebességét illetőleg) Itt említeném meg, hogy ennek a pályatervezésnek a legnagyobb korlátja az, hogy a minimális mozgás ideje 2Tau, mely jelenleg kb. 1.2 sec. Ha igen rövid távolságokra szeretnénk mozogni, akkor ez túl hosszú idő. Egy másik robotikai munkában, ahol a lehető leggyorsabb mozgás volt a legfőbb követelmény, az volt a kívánalom, hogy határozzuk meg azt az optimális interpolációt, ami két egyenes szakaszról úgy kanyarintja át a robotot, hogy az a két szakasz metszéspontjának R sugarú környezetében tér csak le a pályáról. A feladat nem Real Time megoldható, csak közelíthető. Amennyiben valakinek kedve támad a tökéletesíteni a jelenlegi interpolációt, és ebben az irányban gyorsítani a robot munkavégző képességén, akkor a köv. módszer szerintem analizálandó alternatíva lehet: • A negyedfokú, folytonos gyorsulásos interpolációból kell kiindulni. • Az együttinterpolációt alkalmazni minden esetben, nem csak GOS-GO átmenetnél. • A lassító-gyorsító szakaszokon csak a felét szabad felhasználni a rendelkezésre álló csuklógyorsulásnak. A másik felét meg kell hagyni az együtt gyorsító következő pontnak. (ez az a megkötés, amiben eltér az optimumtól, de RT számítható, és egyszerűen programozható is.) • Meg kell határozni minden lassító-gyorsító (negyedfokú) szakaszra a lehetséges legrövidebb végrehajtási időt (tau) úgy, hogy Amax(-tau
Ci -> Ci ponthármassal megy, míg a szuperponált gyorsítás (B(i+1)-Ci) -> (B(i+1)-Ci) -> (B1(i+1)-Ci) értékekkel.
84
Levzetés: negyedfokú szimmetrikus, általunk használt polinom felső korlátjának közelítése teljes négyzetű alak keresésével és megfelelően választott iterációs módszerrel, mely a Newton-Raphson módszerből indul ki, de a Taylor-soros közelítésre is emlékeztet. Amint említettem, szükség lesz az interpoláció inverzének a meghatározásához. Az interpolációs polinom 0 sebességről indulva fix alakú. Ha az interpoláció ideje tau, és az egyenes szakasz sebessége v1_1 (vektor), valamint a kiindulási, illetve a végső gyorsulás 0, úgy a negyedfokú polinom mind az öt együtthatója meghatározható. Figyelem! A Lantos-könyvben közölt interpoláció úgy működik, ha –tau-tól +tau-ig tart a mozgás. A fent vázolt esetben t1=0-tól t1=tau-ig mennénk. Ebben a levezetésben fölöslegesen kellene a tau^2, tau^3 tagokkal számolni. Legyen t = t1/tau, és végezzük az interpolációt ebben a változóban. Így a pozíció t1 szerinti deriváltja nem lesz egyenlő a t-szerinti deriváltjával, ezért t-ben deriválva nem dp/dt1=v1_1, hanem v1= dp/dt= v1_1*dt1/dt= v1_1*t1/t= v1_1*tau sebességet kell előírni a t=1 (t1=tau pontban). A gyorsulások amúgy tau^2-esen aránylanak az eredetihez. Ezzel a transzformációval (új változó bevezetése, Petőfi-módszer) lényegesen egyszerűbb a levezetés. Lássuk az interpoláció együtthatóinak meghatározását: p(t)=at^4+bt^3+ct^2+dt+e; v(t)=dp/dt=4at^3+3bt^2+2ct+d; a(t)=dv/dt=12at^2+6bt+2c; A kötött feltételek: p(0)=0; v(0)=0; p(1)-nem kötött. v(1)=v1 és a(0)=a(1)=0 a folytonos gyorsulás biztosítása miatt. Most derül ki, miért volt jó a t=t1/tau trafó alkalmazása. A feltételeket behelyettesítve: 0=p(0)=a*0+b*0+c*0+d*0+e => e=0; 0=v(0)=4*a*0+3*b*0+2*c*0+d => d=0; 0=a(0)= 12*a*0+6*b*0+2*c=0 => c=0; 0=a(1)= 12*a+6*b; b=-0.5*a; v1=v(1)=4*a+3*b+2*c+d= 4a+3b+2c+d=4a+3b; =2.5a; => a=0.4v1; b=-0.2v1; A módszer általános esetre az, hogy az interpolációs függvény, illetve deriváltjai számára képzett előírásokból vektort képzünk képzeletben (p0,v0,a0,a1,v1). Minden előírás az együtthatók lineáris kombinációjából áll elő (ha az előírások helyét (esetünkben 0 és 1) fixnek vesszük). Ekkor egy mátrixszorzás írható fel a kettő kapcsolatára. 12 4 1 6 3 −1 1 0,5 0,75 − 1,66 1 * * 1 1 2 1 1 0,25 − 0,33 1 − 0,5 1
85
[a
b c d e][ p0 v0 a0 a1 v1] (képlet: A paraméter-mátrix és inverze)
[ p0
v0 a0 a1 v1][a b c d
e]
Ezt a mátrixot invertálva kifejezhetjük az együtthatóvektort (a,b,c,d,e) a feltételek függvényében. Nagyobb interpolációkhoz (pl. bicubic felület), ha a feltételek helye fix, érdemes a mátrixot MATLAB-bal invertáltatni. Ha az együtthatóvektornak az előírások helyétől való függőségére is kíváncsiak vagyunk (nem fix pozíciójú előírások), akkor a mátrix változókat is tartalmaz. Ekkor a mátrixot szimbolikusan kell invertáltatni. Tehát adott egy polinomunk: p=0.4v1*t^4-0.2v1*t^3, amelyet csak a 0-1 tartományban használunk, kíváncsiak vagyunk, mennyi idő az, amikor p eléri az R távolságot. Ha R>=p1, úgy a lineáris szakaszon kell keresni a megoldást (p1=0.2*v1). Az nem nagy ügy. Ha r=p(t), ha 0=2; 2AB>=-1; B^2+A(2C+E)>=0; B(2C+E)>=0 C^2+EC+F>=-R’. Ez az egyenlőtlenség-rendszer nagyon sok polinomot enged meg. Nem determinál minden együtthatót. Bizonyos megoldásokat választva a megengedettek közül csalódva tapasztaltam, hogy az eltérés túl nagy lett. Azért nem volt felesleges ez a gondolatmenet, mert ennek a módszernek egy átkombinált változatát a későbbiekben fogjuk használni. Elindultam hát a következő úton: Ax^4+Bx^2+C alakban keresek olyan polinomot, mely felülről közelíti 2X^4-x^3-t. Ebből csak -x^3-t kell közelíteni, mert a negyedfokú tag tisztán ábrázolható. Közelítsük –x^3-t felülről, azaz x^3-t alulról. A 0-1 intervallumban x^4 <= x^3. A közelítés hibáját megkaphatjuk, ha a különbség deriváltjának nullahelyét megkeressük. 4x^3-3x^2=0 4/3x * x^2=0 x=0 vagy ¾-ed. Minket az utóbbi megoldás érdekel, ekkor a hiba nagysága kb. egy tized (27/256). –x^3 közelítésére tehát –x^4 jó
86
választás. Az eredeti 2x^4-x^3-t tehát x^4-el fogjuk felülről közelíteni. Megoldjuk az x^4=R’ egyenletet. Az így kapott x(t) elfogadható eredményt adhatna. A legnagyobb távolságbani eltérés (0.1) esetében a függvény deriváltja 27/16-od. Ezzel leosztva megkapjuk az időbeni eltérést: az optimális interpoláció-kezdés maximum 1/16-od tau-t fog késlekedni. Ezzel az eredménnyel elégedettek is lehetünk, de mivel egy nagyon frappáns iterációs eljárást találtam az eredmény igen gyorsan konvergáló pontosítására, hadd legyen szabad azt is megosztanom az olvasóval. Akit nem érdekel a téma, nyugodtan ugorja át. Az olyan inverz függvények közelítésére, melyek maguk analitikusan nem számíthatóak, de inverzük (az eredeti függvény) igen, Newton, illetve Raphson kitalált egy gyors konvergens eljárást. A példát az x^0.5 függvényen mutatom be. Keressük y=H^0.5-t. Legyen az első közelítés y0. Legyen yv a helyes megoldás. A problémát átírjuk, y^2=H egyenlet megoldását keressük. A közelítés hibáját a vízszintes tengelyen nem ismerjük. Ha pontosan ismernénk, akkor a megoldást is tudnánk. A függőleges tengelyen levő hiba H-y0^2. A függvény deriváltja ebben a pontban 2y0. A megoldást közelíthetjük, ha az érintővel elmetszük az y=H egyenest. Y1=y0+(H-y0^2)/2y0. Ez lokálisan lineáris függvény esetében pontos közelítést adna. Mennél kevésbé lineáris a [yv y0] intervallumon a függvény, annál rosszabb arányaiban a közelítés. Viszont a közelítés hibájának csökkenésével a függvénynek egyre inkább lineáris jellege lesz. A közelítés hibája kb. így alakulhat: 0.1; 0.01; 0.0001; 0,00000001; stb. (pontosan: 0.1 –0.0062 –0.000027 –5E-10). Amit fontos észrevennünk, hogy minden interpolációs lépes esetén a hiba negatív. Ez azért van, mert az y^2 függvény felfelé görbül, így minden egyenes közelítés a függvény alatt fogja metszeni az y=H egyenest. Az eredeti feladatot pont ezért standard N-R módszerrel egyszer sem iterálhatjuk, mert a felülről közelítés alulról közelítésre váltana, mivel a mi 2x^4-x^3 függvényünk is felfelé görbül. Éppen ezért az egyenes közelítés helyett olyan polinom közelítést kell alkalmazni, mely invertálható, és a tényleges megoldás és a közelítés közötti intervallumban felülről közelíti az eredeti függvényünket. Tudjuk, hogy a közelítés (legyen x’) mindig <xv, a valódi megoldás. Így x0<x<xv szakaszon kell a felülről közelítést teljesíteni. Keressünk egy olyan Ax^4+Bx^2+C alakú függvényt, mely az x=x0 pontban egyenlő 2x^4-x^3-el, és attól jobbra nagyobb nála. A nagyobb relációt megint úgy biztosítjuk, hogy a közelítő függvény minden deriváltját nagyobb értékűvé tesszük, mint az eredeti függvény deriváltjai. Ezt a negyedik deriváltig kell végig ellenőrizni. Az eredeti függvény, és deriváltjai: 2x^4-x^3 8x^3-3x^2 24x^2-6x 48x-6 48 A közelítő fny: Ax^4+Bx^2+C 4Ax^3+2Bx 12Ax^2+2B 24Ax 24A Ha az alsó sornak tagonként kisebb-egyenlőnek kell lennie a felső sornál, akkor az A=2 választás mellett B>=-1.5 adódik. B-t –1.5-nek véve C=1.5x^2-x^3. Ha most megoldjuk az 2x^4-1.5x^2+(1.5*x0^2-x0^3)=R’
87
egyenletet, akkor xv újabb közelítéséhez juthatunk, mely még mindig alulról fogja xv-t közelíteni. Ennek az iteraciónak a sebessége még rohamosabb, mint a NewtonRaphson módszeré, mivel nem egyenes, hanem negyedfokú illeszkedést használ. Az így megkapott xv-t (x0-t) csak az első lineáris transzformació inverzén kell áthajtanunk, mivel a második egyszerűsítés csak függőlegesen skálázta függvényeinket (és a távolság bemenetet). Ha úgy számítjuk, hogy tau (Lantos könyv szerinti paraméterezéssel 2*tau) idő alatt gyorsulna v1_1 sebességre, akkor xv*tau idő múlva még biztos R sugáron belül jár a robot. Az előző szakasz lassításának vége előtt xv*tau idővel már elkezdhetjük a következő, szuperponált kigyorsítást, feltéve, ha a lassitó fázis ekkor már R sugáron belül jár. Ha nem, akkor ezt még meg kell várni.
4.3.2 A szoftverrendszerünk architekturális és szoftvertechnológiai szemszögből Valójában a szoftverrendszer fejlesztése arról szól, hogy megismerjük, hogy a robotirányítás matematikájának, operációs rendszer és hardver architekturális kritérumjainak elmélete és a gyakorlat között még mennyi áthidalandó apró probléma húzódik. Némelyik ilyen problémát fércszerűen, némelyiket gyökerekig hatoló átszervezéssel oldottunk meg. De nem a megoldások mikéntje, illetve megléte a fontos, hanem a tapasztalat, hogy ezek a problémák léteznek. Mert a szoftverrendszereket nem árt időnként újraírni, főleg akkor, ha a fejlesztés alatt olyan tudás birtokába jut a csapat, aminek hiánya a rendszer eredeti tervezésekor nem tette lehetővé a legjobb megoldás megválasztását, a precíz szoftvertechnológiai tervezést. Amennyiben ez megvalósul, úgy a kommunikációs diagrammok elkészítéséhez valószínű, hogy nagy segítség lenne az UML használata. A rendszert fejlesztő hallgatóknak mindig megvolt a lehetőségük arra, hogy a felmerülő problémák közül válasszanak, melyikkel foglalkoznának szívesen, és azt milyen módon óhajtják megoldani. Így sokkal inkább magáénak érezhette mindenki a munkáját, és az egész rendszert, mintha egy előre elkészített rendszertervnek egy konkrét szeletét kellett volna elkészítenie. Bár az utóbbi eset szakmailag jobb megoldásokat eredményezhet. Az ellentmondás (önkéntes alapú fejlesztés szívhez közel állása, de kaotikus jellege vs. tervszerű munka) azon alapszik, hogy a rendszertechnikai döntéseket azoknak célszerű meghozni, akik az implementációt is végzik majd, és nálunk az időbeli eltolódás miatt ez nem lehetséges. Ha a problémát egy most bekapcsolódó hallgató szemszögéből nézzük, neki teljesen mindegy, hogy egy történeti okokból éppen így alakult rendszer logikájához kell-e igazodnia, vagy egy előre kitűzött rendszertervhez.
4.3.3 Héjszerkezet architektura, SW rendszer újjátervezése A rendszer szofvertechnológiailag nincs megtervezve. Egy ilyen kritikus működésű alkalmazásnak igen jól jönne az a szolgáltatás alapú héjszerkezet, ami pl az ISO-OSI hálózati protokoll filozófiája. A jelen rendszerben két egyértelműen definiált interfész felület van, a robot IO-HW vezetékei, illetve az ARPS nyelvtől a felhasználó által 88
elvárható működés és működtetés. A kettő között helyezkedik el a programrendszerünk. Jót tenne a rendszer egészének, ha lenne a későbbiekben két közbülső felület is. Mivel a robot IO vezetékei az ISA buszon levő különböző kártyákra kapcsolódnak, így a legbelső robot-funkciókat szolgáltató egységnek kell tartalmaznia a Host processzoron futó részt is. (Pl. autonóm vészleállítást nem igazán várhatunk az ARC kártyák szolgáltatásában, mert a másik kártyát és a PC-lab kártyát is összehangoltan kell működtetni. Az ARC kártyáktól csak vészhelyzet jelzését várhatjuk, mint szolgáltatás, valamint a vészleállás rájuk eső funkcióinak elvégzését.) Egy ilyen belső magtól a következő funkciókat várhatjuk el: önteszt, inicializálás, kulturált újraindítható leállás, robot kalibrációja, robotmozgás szabályozása. Egy másik szint lehetne a robot programozási nyelv és a robotot mozgató pályatervezés között. Itt már egy magasabb szinten lehetne definiálni az elvárt robotmozgásokat, és a mozgás állapotát (hibáját) is sokkal többrétűbb módon lehetne visszajelezni. Egyszóval, egy kis objektum-orientált jellegű strukturáltság kellene a rendszerbe. Értem ezalatt az egyes részek olyan leválasztását, hogy a kapcsolódó egységek részéről teljes egésznek látszódjanak. Egy ilyen egységnek léteznének hívható funkciói és állapotváltozói, melyektől függene a működés eredménye. Az objektum orientált programozási stílusban megszokott módon kellene biztosítani azt, hogy pl. robotmozgás közben nehogy valaki szabályozót váltson, illetve, hogy pályainterpoláció közben se válthasson senki se robot konfigurációt. Az egyes részek a jelenlegi tapasztalatok alapján tervezhetőek lennének. Jelenleg egyes hívások a hierarchiában több lépcsőt átugranak, megnövelve ezzel a sebességet, de csökkentve a megbízhatóságot.
4.3.4 HOST-CONTROL A lehető leggyorsabb Host-on futó szabályozáshoz a következőket kell megtenni: dynjac processz legyen a legnagyobb prioritású a Host-on. (Ez lesz a szabályozó processz.) A TMS tacho interruptjának az inkrement-sebességek átlagolása után azonnal a CBUP-ba kell másolni a q, ill qd értékeket, valamint az új értékeket jelző 1est. Ha szabad az IEX TMS-ről hívható interruptja, akkor blokkmásolás kérésre akcióval át kell másoltatni ezt a DPRI-CBUP-ba, majd olyan üzenetet küldeni a Hoston a dynjacnak, mely nem várakozik a státusz visszatérésére. Ehhez dynjacnak be kell jelentkeznie az arc-procba, mint fogadó processznek. A tacho interruptnak meg kell vizsgálnia, hogy a DPR-D CBDOWN-ban az új áramjeleket jelző szó 1-es e. Ha igen érvényre kell azokat juttatni, a új áramokat jelző szót és egy belső watchdogot nullázni. Ha a rutin elején sikertelen volt a BLK_CPY, akkor újra meg kell kísérelni azt, és az üzenet küldést. Amennyiben a Host-controll watchdog lejár (eléri az 5 ms-ot), akkor a DCS szabályozást be kell kapcsolni, és állandó alapjelet kell kiadni. A dynjac process ütemezése jelenleg 4 ms-onként történik. Ezt 1-2 ms sebességre kell feltornászni. A Send-Receive-Reply mérések alapján egy régebbi gépen is több 100.000/s taszkváltást produkált a QNX. 1 ms alá nem mehetünk, mert ez a QNX legkisebb ticksize-a. A dynjac processznek ugyanazt a proxit kell elküldenie az arcprochoz való bejelentkezéskor, amivel a timer-event is értesíti periódikusan. Minden proxy érkezésnél meg kell vizsgálnia, hogy mindkét DPRI-CBUP-ban frissültek-e a q, ill qd
89
értékek. Ha igen, az új értékeket jelző flag-et nullázza. A bejövő sebesség és pozícióadatok alapján döntse el, nincs-e vészhelyzet, ha igen, fékezzen gőzerővel, ha nem, számítsa újra a szabályozást, és a kiadandó áramjeleket az új értéket jelző flag-el együtt másolja a DPRI-CBDOWN-okba. Ezek után neki is feltételes BLK-CPY-t kell kérnie. (Az arcproc.c-ben illetve az arccom.c-ben a feltételes block copy-t, és a várakozásosat is implementálni kell.) Meg kell emellett vizsgálnia a CBUP-okban, hogy a TMS-ekben nem járt-e le időközben a watchdog-timer. Szemaforokat nem érdemes használni, se a periodikus, se a kérésre történő másolásnál, mivel a kommunikáció egyirányú, és bármilyen olvasás közbeni felülírás szempontjából stabil ez a megoldás. Mivel mind a jelenlegi CBUP, illetve CBDOWN buffer is sokkal több adatot tartalmaz, mint amennyi a HOST-CONTROLL megvalósításához feltétlen szükséges, érdemes lenne HCUP és HCDOWN néven kisebb memória területeket definiálni (akár ezeken belül). A felpörgetett sebességű periodikus másolásnál csak ezeket kellene másolni. (Az interrupt rutin két módban fut: vagy minden megszakításkor másolja a kisebb buffereket, vagy minden n-edik megszakításkor a nagyobbakat. De meglehet úgy is oldani, hogy minden megszakításkor másolja a teljes HC-buffereket, illetve a többi buffernek mindig másik és másik kisebb részét.) Jelenleg az iex a 8-bites memcpy-t használja. Amennyiben csak word-aligned bufferhatárokat engedne meg, úgy a word-os memcpyt lehetne használni, és kétszeres sebességet elérni. (A DP-RAM-ok 16 bites szervezésűek) Az IEX periodikus másoló interruptját 1ms-nál valamivel gyorsabbra (0.1-0.3 ms) kell állítani. Egy 25-Mhz-es processzornak néhány tucat buszciklust igénylő másolás nem tarthat 25000 órajelbe. Amennyiben az IEX TMS-t kiszolgáló interruptja el volt foglalva, úgy a magasabb prioritású periodikus másolás mindenképpen érvényre jut.
4.3.5 C-for robot függvények Az arps nyelv sok programszervező, és egyéb utasítást tartalmaz. A C-for-robot library-nak csak a robottal kapcsolatos részét kell tartalmaznia, de ezt sem érdemes egy-az egyben implementálni. Mint már említettem a kombinált pontok transzformációja segédfüggvényekkel kell, hogy történjen. Egyes állapotváltozók kapcsolását végző utasítások helyett is szebb lehet az állapotváltozót paraméterként átadni. (pl mozgás break-es lesz-e.) Így a programsorokról is első ránézésre látszik, melyik utasítás lesz break-es, és melyik nem. Itt közlöm azoknak a függvényeknek a listáját, amit egy robotlibrarynak szerintem érdemes tartalmaznia. Lista: A C-for robot library által célszerűen implementált függvények, változók: typedef v3,m33,m43,v6; void set_speed(double); m43 here(); v6 here#(); void base(v3 poz,double phi); void tool(m43); void go(m43,int brake);
90
void go#(v6,int break); void gos(m43,int brake); void gos#(v6,int break); void move(v3,int brake); void moves(v3,int brake); void tmove(v3,int brake); void tmoves(v3,int brake); void m_j(int,float,int brake); void open(); void close(); void out(int,int); int in(int); void wait_in(int); void incall(int,void (*handler)(),int nobreak); //NULL függvény kikapcsolja a megszakítást char get_configuration(); void set_configuration(char); void tolerance(int) //NARROW / WIDE Mátrix manipulációs függvények: m43 invert(m43); m43 shift(m43); m43 base_frame(m43 base,m43 rel); //mátrix szorzás m43 scale(m43,double); //pozíció szorzódik float distance(v3); v3 position(m43); m33 orientation(m43); m43 combine(v3,m33); m43 near(float z); //GO_NEAR egyszerű implementálásához m43 align(m43); m33 align(m33); m43 xyzoat(v6); m33 rotate(int axis,double phi); A függvények törzsét send-del érdemes megvalósítani. Kalibráló utasításra nincs szükség, mivel a rendszert csak akkor célszerű külső processzből vezérelhető módba kapcsolni, ha a robot már kalibrált. Az interrupt bejegyeztetéséhez egy rutin nevét kell paraméternek átküldeni, illetve az interrupt vezeték számát, valamint a nobreak paramétert. A felügyelő processz feladata a digitális input tesztelése. Break-es interrupt esetén az ARC-kre küldött pontokat töröltetni kell, az aktuális pozícióban sebességugrással vagyunk kénytelenek megállni. Ezután a pathgen és az arps processzeket értesíteni kell az interruptról, miközben az új pontelőírások elküldése már tiltva van. Miután ez a két processz visszajelezett, a robot mozgatását újra lehet engedélyezni. Ha fut egy C-for-robotos processz, akkor annak ezen a ponton kell
91
szignált küldeni. A C-for-robot library inicializáló függvénye tartalmazza a sig-handler()-t. Ez az arps értesítése után meghívja az incall()-hívásban megadott rutint.
4.3.6 \arc.ini olvasása (ismétlés) Az arps rendszerben is #define-al vannak a perifériák IO címei és interrupt sorszámai beállítva, annak ellenére, hogy régóta létezik a programkönyvtárban egy arc.ini fájl, de ezt egy modul sem olvassa. A legszebb megoldás az lenne, ha a gyökér könyvtárban keresné ezentúl minden program ezt a konfigurációs állományt. Így egy szükséges átjumperelés esetén csak egy helyen kell ezt módosítani, és minden program rendesen működhetne továbbra is.
4.3.7 Grafikus felület. Időzítési viszonyok analízise, ha a photon is fut Ha a Hoston futtatunk mást is a robot működése közben, akkor érdemes a rendszert indító processz prioritását jól felemelni még minden másik gyermek-processz spawn()olása előtt. Így az egész szoftverrendszer prioritása megemelkedik. Sárközy Ferenc grafikus ábrázolása szépen mutatna egy külön ablakban. [Sárközy Ferenc, 2000:Diplomaterv] A szabályozás és a folytonosan változó robotparaméterek kijelzésére is szebb lenne egy új ablak, (ha nem is photon alá, de egy másik QNX konzolon) és azon lehetne egy-két dolgot (pl. a közös memóriák tartalmát) folyamatosan monitorozni.
4.3.8 Robotszimuláció, egy makefálj kellene és egy main include minden processz számára A robotszimulációs rendszer jelenleg külön make-fájllal fordítandó. Egységesebb lehetne a rendszer, ha lenne egy globális include-fálj, melyben define-os kapcsolókkal lehetne állítani, hogy éppen milyen opcióval fordulnak a programok. A mostani robotszimuláció csak kinematikai, azaz az adott előírásokat adott időn belül teljesíti a robot. A dinamikus modell invertálásával lehetőség lenne dinamikai szimulációra is. Mivel H invertálása szeparálható, így biztos, hogy elmenne akár valós időben is. A korrekt szimuláció esetében feltétlen kell valamiféle zajt adni az áramjelekhez, illetve a fogaskerék-súrlódás minél realisztikusabb modellezése is javít a rendszeren. Nem ismerem a QNX hangprogramozásának rejtelmeit. Grafikus szimuláció esetén a PC-speaker használatával (periférián elérhető, de az időzítő IC-re van kötve, ami lehet, hogy a QNX miatt kevésbé babrálható) a sebességek még valószerűbben érzékeltethetőek. Ha jól tudom, Kozma Péter e féléves önálló laboros munkája egy, az interneten keresztül mozgatható NET-Robot volt. Az ő munkáját érdemes lehet Sárközy Ferenc munkájával házasítani.
92
4.3.9 Analízis bármely processzor, processz kiesésére 4.3.10 Analízis bármely HW elem (vezeték) meghibásodására, jelzésük megoldásai 4.3.11 A kalibrációval kapcsolatos hardver meghibásodás elleni védekezési módszerek A kalibráció alatt a szabályzó-algoritmusok élnek, ha azokba a limittúllépésekre való leállást visszaírja valaki, akkor az a robot ütközéses állapotában is védi a berendezést a károsodástól. A kalibráció maximum két körülfordulásnyit forgatja az inkremens adókat, így ha a nullimpulzus vezetékek elszakadnak, hibajelzéssel tér vissza az utasítás. Az inkremens adók vezetékeinek szakadása esetén viszont a szabályozás csak az említett limitfigyelés kommentezett állapotának megszüntetése esetén nem jelent veszélyt a robotra, de amúgy is a szabályozó paramétereinek erősségétől függ, hogy egy hibás inkrementális adó miatt visszajelzett álló pozíció esetén a szabályozás áramjelei milyen sebességre gyorsítják a robotot, amíg a pozícióhiba (ebben az esetben egyezne az alapjellel) eléri a limitet. Ebben az esetben mennél lassabb a kalibráció közbeni sebesség alapjel, annál veszélyesebb a meghibásodás. Lehetőség lenne a Host felügyeleti processze számára, hogy a PC-Lab kártyát pollingolva a kalibráló potenciométerek által jelzett elmozdulást összehasonlítsa az inkremens adók értékeivel. Kalibrált állapotban az abszolút pozíciók összehasonlításával még biztonságosabbá tehetné az inkrementális vezetékek szakadása elleni védelmet. Ha az indulás után a fékek kiengedése előtt kis amplitúdójú négyszögjelet eresztenénk a motorokra, lehet, hogy tesztelhető lenne a vezetékek hibátlansága. Mennél kotyogósabb a fékek házhoz való rögzítése, annál valószínűbben működőképes ez a teszt. Egy robot gépészeti tervezésekor érdemes ezt a szempontot figyelembe venni. Sajnos a robotunk inkerementális adóinak jelei nem szimmetrikus kivezetésűek, így nincs lehetőség a tachocontroller elektronika által a vezetékszakadás érzékelésére. Ez a módszer azonban a fotodiódák meghibásodását nem képes jelezni. Ha már a témánál vagyunk, megemlítendő, hogy a robot összekötő panelon ki lett cserelve egy relé, melynek feladata a kimeneti áramok tiltása és a féklazító elektromágnesek feszültségtelenítse abban az esetben, ha az inkrementális adók tápfeszültsége kiesik. Sajnos erről a szoftverrendszer nem kap visszajelzést, és a robotvezérlőn sem jelzi egy lámpa sem ennek a tápfeszültségnek a meglétét, vagy a hiányát.
4.3.12 Kalibrációs görbe felvétele az arps-en belül 4.3.12.1 Ready-helyzet felvétele az orosz manual-ban leírt folyamat alapján (move joints-al) Az orosz kézikönyv fordításában megtálalható az parancssorozat, amivel ready helyzet egyértelműen felvehető. A robot szállításakor össze van csukva. Ilyenkor a 93
megfogó helyére egy fekete henger van szerelve (6-os csavarokkal). Ennek a vashengernek a túlvégét a robot megfelelően összecsukott helyzetében a talphoz lehet rögzíteni egy kb. 19-es külső méretű anyával. Az manualban levő move joints utasítások ready helyzetbe viszik a robotot, ha a kiindulási helyzet a szállítási pozíció volt. Ezt az utasítássorozatot akár arps-programként is le lehet tárolni. Így nem kell az interpreterhez sem hozzányúlni. 4.3.12.2
Kalibrációs fájl felvétele és mentése
Ennek az új utasításnak mindenképpen programozva kell lennie. Első lépésben rá kell kérdeznie, hogy tényleg ready helyzetben áll-e a robot. (Kiírhatná, hogy ő mit tud a jelenlegi csuklószögekről.) Igenlő válasz után 0 értéket kell minden változóba tölteni (tacho-load parancs). Ezután egy robot_cal() hívással ráállhat a következő 0impulzusra. Ezek után a robot motorjainak (és nem a csuklóinak (egymásrahatás)) a görbéjét egyenként kell felvenni. A mozgatásokat inkremensben, egymásrahatás számolása és ütközésellenőrzés nélkül kell kiadni (robot_point()) Az egyes motorokat el kell vinni a limiteken belüli legnegatívabb nullimpulzushoz. Majd a nullimpulzusokon (egy inc-per-rev- nyi mozgásokkal) végighaladva menteni kell a megfelelő potméter értékét a memóriába. A kalibrációs fájl formátuma a robot-calload() függvény törzséből deríthető ki.
4.3.13 Kalibrációs potméter jelenek eltérése a tacho-által számított jelhez képest, ennek folyamatos figyelése, warning üzenettel, ha túl nagy az eltérés az eredetitől, inkremens impulzusvesztés monitorozása A PC-Lab kártyán az AD átalakítók működtetése folyamatosan, interruptal történik. Jelenleg a kapott értékeket csak kalibráció esetén használjuk, pedig vészeset monitorozására kivallóan alkalmas. Az aktuális inkremensértékből gyorsabb számítással (egyszerű táblázatolvasás a lineáris leképezés előtt) kaphatjuk meg az elvárt analóg értéket mint fordítva. Ezért a hibát analóg feszültségben számoljuk. Zaj nélküli AD-k esetében 10-20 inkremens eltérés esetén is baj van. Mivel a Sample&Hold AD zaja ennél várhatóban nagyobb, és inkább feszültségben, mint inkremensben állandó a nagysága, úgy az összehasonlítást a várt és a valódi feszültségekkel jobb megtenni. Az inkerementális adók hibája csak így monitorozható.
4.3.14 Robotparaméterek identifikálása 4.3.14.1 C-for robottal könnyebben implementálható a geometriai modell visszamérése
94
A robot áttételeit a legegyszerűbben úgy lehet lemérni, ha a robot megfogójára vízszintmérőt szerelünk. Nagyjából 90 fokos mozgatásokkal kell felvenni a pontos helyzeteket. Az 1-es csukló áttétele nem mérhető ezen a módon, viszont az 5-ös 6-os egymásrahatás egy megfelelő mérési elrendezéssel igen. Persze a legpontosabb a robot szétszerelése, és a fogaskerekek megszámlálása lenne. A jól védett mechanika miatt ez több hónapig is eltarthat. Ha a robot megfogójára valami tű szerű alakzatot rögzítünk, és ezt jól kimérhető (1-11,3-4-5 oldalarányú) térbeli háromszögek csúcsaiba visszük úgy, hogy valami puhább anyagban nyomot hagyjon (kartonpapír, stb.), akkor elegendő számú mérés esetén a várt és a tényleges eredményekből következtethetünk a D-H robotmodell paramétereinek és az áttételek tényleges értékeire. 4.3.14.2
Áram (2048)-nyomaték átváltás, szimulált rugó (csak P szabályozó)
A konkrét áramalapjel-egység<->nyomaték átváltás lemérése a hatkomponensű erőnyomatékmérő üzembehelyezése után gyerekjáték lesz. Az alsó 3 és a felső 3 motor azonos, mivel az eredeti nyomatékértékek arányosak az áttételekkel. Addig jó módszer, ha DCS szabályozó mellett nulla I taggal kis P együtthatóval lemérjük, adott kiterítés mellett mekkora erővel indul meg a robot kifele, illetve befele. A kettő különbsége a súrlódás kétszerese. Ügyeljünk a gravitáció hatásaira, a P tag ne legyen olyan kicsi, hogy összecsukolhasson a robot. A gravitáció miatti erőket ügyes mérési elrendezéssel vegyük fel! Egyszerre csak egy csuklót rugóztassunk. Az erőmérő hosszú kötélre legyen kötve, nem menjünk közel a robothoz bekapcsolt állapotban. A gravitáció hatását az 1es csuklóhoz hasonló módon elliminálhatjuk, ha a robotot a padló helyett a falra csavarozzuk. Minimum 12-es átmenőcsavarokat használjunk! A fúrógépet ne felejtsük a nagy átmerő miatt alacsony fordulatszámra állítani. Mivel időigényes a folyamat, legjobb ha a feladatot a robottal végeztetjük el éjszaka, off-line. A robotprogram indítása előtt feltétlen kérjük a témavezető oktató engedélyét. A műveletet lehetőleg a V-2 épületben végezzük, annak helyreállítása kevésbé terheli ösztöndíjunkat. 4.3.14.3
Súrlódás lemérése, modellezése. (a*v^2+b*v+c) alakkal
A robotnak állandó sebességű mozgása során menteni kell a kimenő áramalapjelet. A gravitációs modellt elliminálva az eredményekből minden csuklóra felvehetünk egy súrlódás-sebesség görbét. Feltehetjük, hogy a gravitációs modell minden 560-as pumában azonos, illetve hogy a stanford-cikk mérési módszerei alapján ezt ők igen pontosan tudták lemérni. A jelenleg nyüstölt robot hangján hallatszik, hogy pár ezer izzót odébbpakolt már, így a fogaskerekei különböző szögekben különbözőképpen vannak elkopva. Emiatt a használt robotok súrlódása nemcsak sebesség, de pozíció függő is.
95
4.3.14.4 Dinamikus modell mérése (csak p-szabályozó, gyenge p-taggal, periodusidő mérése (csuklóváltozók loggolása)) A szimulált rugó módszert a stanford-cikk közli. Ha a DCS szabályozásban tiltjuk az I tagot, és viszonylag kicsi P együtthatót választunk, akkor súrlódásmentes esetben harmonikus rezgőmozgással válaszolna a robot az alapjelugrásokra. Az aktuális pozíció értékeket periodikusan log-fájlba menthetjük, így az egységugrás-válaszból identifikálhatóak a lényeges rendszerjellemzők. (akár a súrlódás is).
96
5 . Összefoglalás A robotvezérlő rendszerünk jelen formájában alkalmas a robot kísérleti üzemeltetésére, új algoritmusok kipróbálására. A rendszer nem 100%-osan megbízható, így a méréseket továbbra is a СФЕРА vezérlőn kell tartani. Mindkét tanszék robotprogramozási mérési feladatának végzésekor azt tapasztaltam, hogy a vezérlő megbízhatósága miatt a robot munkaterébe időnként - praktikussági okokból egy-egy emberi végtag, néha egy fej is bekerül a robot mozgása alatt. Kiforrott szoftverű vezérlő esetében ez nem baj, egyes robotgyártó cégek prospektusában is láthatóak hasonló helyzetek. A kísérleti rendszer működésekor azonban fontos betartani, hogy a bekapcsolt manipulátorra úgy tekintsen minden jelenlevő, mint amely bármelyik pillanatban egy 1kW-os, 6 szabadságfokú húsdarálóvá válhat egy programihiba ritka előfordulásakor. A rendszer belső állapotválltozói memóriába, vagy akár közvetlen fájlba menthetők, így off-line idenfikáció számára az adatok nagy mennyiségben előállíthatóak. A hátrahagyott feladatok nagyrésze kissebb kaliberű, egyes tervezett alkalmazásokhoz nincs szükség mindnek a javítására. A robotkéz együttes vezérléséhez a C-for Robot library implementálandó, ennek egyetlen komolyabb momentuma az interrupthoz szükséges átszervezés. Amíg interruptot nem kell használni, addig az alap C-for Robot funkciók implementálása elegendő a két QNX-es PC összekötésére. A hatkomponensű erőmérő illesztése adatgyűjtés (identifikáció) szempontjából gyorsan megoldható, irányítási algoritmusban való felhasználásához a Host-Conrtoll funkciót jó előzetesen megvalósítani. Az on-line grafikus vizualizáció időzítési problémák nélkül úgy valósítható meg, ha Sárközy Ferenc programját egy másik PC futtatja, és az QNX hálózaton kap információt a csuklók helyzetéről. Az elvégzett feladat szépséghibája, hogy a világkoordinátás mozgatás tesztelése közben időnként valamilyen fatális időzítési hibába fut a rendszer. Szerencsére ekkor a robotkar azonnal megáll. Ez a hiba valószínűleg a kommunikációs alrendszerben rejtőzött, és csak most jött elő. Remélem, hogy a rendszert ismertető részek könnyebbé teszik a bekapcsolódó hallgatók munkáját, és így hamarabb tudnak az érdemi munkára összpontosítani. Egy vagy két hibajavító stílusú diplomaterv után az alaprendszer elérheti a teljesen letisztázott állapotot, és lehetőség nyílik a komplex robotikai kutatások elindítására. Olyan fordulópont lesz ez a fejlesztésben, mint amikor elkészült a teljes HW együttes.
97
6 . Függelék Szféra arps tesztprogramok: A D-H paraméterek kinyerése: >.M J 3,90 >.M J 1,-90 >HERE REDI >.M J 1,-90 >HERE FENT1180 >LL
FENT FENT5 FENT1180 HAJL3 HAJL3M5 HAJL2M5 HAJL2 HAJL2M3 REDI
x -------149.09 -149.09 149.09 -149.09 -149.09 -149.09 -149.09 -149.09 -20.31
y -------20.31 35.94 20.31 489.31 433.06 864.88 921.13 411.50 149.09
z ------921.13 864.88 921.13 452.13 508.38 76.56 20.31 489.31 921.13
o ------90.000 90.000 -90.000 90.000 90.000 90.000 90.000 90.000 0.000
a ------0.000 90.005 0.000 90.000 0.000 0.000 90.000 0.000 0.000
t ------0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000
a csuklok elotti M jelzi a csuklo minusz 90 fokos hajlitasat.
A SZFERA konfigurációkezelásének tesztje: >HERE #B >HERE B >LL A B C R
x -------20.28 -247.69 -220.28 -20.31
y ------149.09 193.13 149.09 149.09
z ------721.13 686.66 721.13 921.13
o ------0.000 121.888 0.000 0.000
a ------0.000 67.225 0.000 0.000
t ------0.000 -58.112 0.000 0.000
#A #B #C #R
joint 1 ------0.000 0.000 0.000 0.000
joint 2 -------51.888 -72.285 -72.285 -90.000
joint 3 ------13.156 20.764 20.764 90.000
joint 4 ------0.000 90.000 0.000 0.000
joint 5 ------38.732 51.520 51.520 0.000
joint 6 ------0.00 0.00 0.00 0.00
>.M J 6,179 >.M J 6,1 >.HERE B001 >.HERE #B001 >.M J 6,179 Invalid solution: 6 >.M J 6,-179
98
>.M J 6,-2 >.M J 6,-179 >HERE B009 >HERE #B009 >LL A B B001 B009 C R
x -------20.28 -247.69 -247.69 -247.69 -220.28 -20.31
y ------149.09 193.13 193.13 193.13 149.09 149.09
z ------721.13 686.66 686.66 686.66 721.13 921.13
o ------0.000 121.888 121.888 121.888 0.000 0.000
a ------0.000 67.225 67.225 67.225 0.000 0.000
t ------0.000 -58.112 121.888 121.888 0.000 0.000
#A #B #B001 #B009 #C #R
joint 1 ------0.000 0.000 0.000 0.000 0.000 0.000
joint 2 -------51.888 -72.285 -72.285 -72.285 -72.285 -90.000
joint 3 ------13.156 20.764 20.764 20.764 20.764 90.000
joint 4 ------0.000 90.000 90.000 90.000 0.000 0.000
joint 5 ------38.732 51.520 51.520 51.520 51.520 0.000
joint 6 ------0.00 0.00 180.00 -180.00 0.00 0.00
y ------149.09 193.13 193.13
z ------721.13 686.66 686.66
o ------0.000 121.888 121.888
a ------0.000 67.225 67.225
t ------0.000 -58.112 121.888
>.GO READY >.J5 M >.GO B >.GO B001 >.GO #B001 >.GO B001 >.GO READY >.GO B >.GO B001 >.GO #B >.GO B001 >.GO #B001 >.GO READY >.J5 P >.GO #B >.M J 4 ,179 Invalid solution: 4 >.M J 4,-179 >.M J 4,179 >.GO READY >.GO B >.GO READY >.J5 M >.GO B >.GO #B >.GO B >.GO B >.GO READY >.GO B >.J5 M >.GO B >LL A B B001
x -------20.28 -247.69 -247.69
99
B009 C R
-247.69 -220.28 -20.31
193.13 149.09 149.09
686.66 721.13 921.13
121.888 0.000 0.000
67.225 0.000 0.000
121.888 0.000 0.000
#A #B #B001 #B009 #C #R
joint 1 ------0.000 0.000 0.000 0.000 0.000 0.000
joint 2 -------51.888 -72.285 -72.285 -72.285 -72.285 -90.000
joint 3 ------13.156 20.764 20.764 20.764 20.764 90.000
joint 4 ------0.000 90.000 90.000 90.000 0.000 0.000
joint 5 ------38.732 51.520 51.520 51.520 51.520 0.000
joint 6 ------0.00 0.00 180.00 -180.00 0.00 0.00
>.GOS #A Can't go str line,use joint motion >.J5 P >.GOS A Invalid solution: 4 >.J5 M >.GOS A Invalid solution: 4
A mellékletben: A rendszer blokkvázlatának nagyított ábrája Egyes 3d ábrák sztereo kivitelben CD: forrás fájlok, futtatható állományok, diplomatervek, eddigi segédprogramok, és minden egyéb vonatkozó elektronikus anyag. 1 db piros-kék szemüveg a harmadik dimenzió megtekintéséhez
100
7 . Irodalomjegyzék [1]
Lantos Béla: Robotok irányítása Akadémiai Kiadó, Budapest, 1991.
[2]
Bézi István - Pilipár Gábor: Segédlet az ARPS robot programozási nyelv használatához
[3]
Tevesz Gábor: ARC Robot vezérlő kártya felhasználói leírás - 1.0 verzió. BME Automatizálási Tanszék, 1994.
[4] Nagy Norbert: Robotprogramozási nyelv interpretere. Diplomaterv. BME Folyamatszabályozási Tanszék, 1993. [5]
Palotás Z oltán: Robotprogramozási nyelv fordítóprogramja. Diplomaterv. BME Folyamatszabályozási Tanszék, 1993.
[6] Vörös István: Programfejlesztői környezet kialakítása kísérleti robotvezérlő rendszerhez. Diplomaterv. BME Automatizálási Tanszék, 1995. [7]
Jász Gábor: Robotprogramozási nyelv kifejlesztése QNX valósidejű operációs rendszerhez. Diplomaterv. BME Folyamatszbályozási Tanszék, 1996.
[8]
Korcsmár Attila: Teszt és működtető programok fejlesztése célhardverekhez. Diplomaterv. BME Automatizálási Tanszék, 1996.
[9]
Donát Tamás: Kísérleti robotvezérlő élesztése és tesztelése. Diplomaterv. BME Automatizálási Tanszék, 1996.
[10] Huszti Ákos: Robotvezérlő szoftver rendszerének fejlesztése. Diplomaterv BME Automatizálási Tanszék, 1999.
101
[11] Somlói Dániel: Robotvezérlő szoftver rendszerének fejlesztése. Diplomaterv BME Automatizálási Tanszék, 1999. [12] Agárdi Gábor-Hadi János: Fókuszban a Pentium. LSI Oktatóközpont, 1996. [13] Armstrong-Khatib-Burdick: The Explicit Dynamic Model and Inertial Parameters of the PUMA 560 Arm. Stanford AI-Lab, 1986. [14] WATCOM C Library Reference for QNX (2nd edition) WATCOM International Corporation, Canada, 1993 [15] WATCOM C Language Reference WATCOM International Corporation, Canada, 1992
102
