Obr. 9 Diagram pracovného cyklu robotického manipulátora

Algoritmy riadenia

Jeden pracovný cyklus robota pozostáva z variabilného počtu (cca 20) pohybov. Na posun z bodu do bodu sa využíva plánovanie pohybov pomocou triedy TubeManipulationControl. Väčšina pohybov je lineárna, teda trajektória je priamka. Je to nielen z dôvodu kolmého ukladania skúmaviek, ale aj vzhľadom na nebezpečenstvo rozliatia vzorky určenej na testovanie pri výskyte chyby v riadení.

Veľká väčšina pohybov manipulátora sa naplánuje vopred a počas aplikácie sa tieto predpripravené trajektórie už len vykonávajú. Po uložení každej uzatvorenej skúmavky sú naplánované trajektórie do všetkých pozícií s otvorenými skúmavkami pripravenými na uchopenie. To umožňuje vyberať a ukladať skúmavky náhodne alebo špecifikovať len niektoré pozície, ktoré majú byť zahrnuté do behu programu. Ak proces plánovania zlyhá, používateľ dostane výzvu na opätovné stlačenie tlačidla. Plánovanie trvá niekoľko sekúnd v závislosti od počtu pozícií. Rádovo sa však plánuje niekoľko desiatok až sto trajektórií.

Na obr. 9 je schéma vykonávania jednotlivých trajektórií. Čierne šípky medzi stavmi označujú vopred naplánovanú trajektóriu. Zelené šípky označujú, že medzi stavmi bude vždy potrebné naplánovanie novej trajektórie. Červené šípky označujú riadenie robota bez použitia softvéru MoveIt! Riadenie prebieha publikovaním správ príslušného typu priamo na riadiaci systém robota. Ako je vidno na diagrame, algoritmy tohto riadenia využívajú hodnoty kĺbov robota a namerané dáta zo silovo-momentového senzora. Použitie MoveIt!-u pri uzatváraní skúmavky uzáverom alebo pri jej vkladaní do stojana je nepraktické, keďže by bolo potrebné v závislosti od meraných síl a momentov neustále plánovať, vykonávať a rušiť nové trajektórie. Lepšou metódou riadenia manipulátora je priame nastavovanie lineárnej a uhlovej rýchlosti koncového efektora a jej regulácia na základe spätnej väzby zo silovo-momentového senzora a hodnôt natočenia kĺbov manipulátora.

Otestovali sa dve riadiace štruktúry:

  • joint_trajectory_controller – štandardná riadiaca štruktúra na použitie s MoveIt!-om, ktorá prijíma pozície kĺbov v čase (prípadne rýchlosti ap.),
  • joint_group_velocity_controller – riadiaca štruktúra na riadenie skupiny kĺbov cez nastavovanie ich rýchlosti v čase.

Lepšie výsledky sa dosiahli pri použití rýchlostnej riadiacej štruktúry.

Po odobratí uzáveru zo zásobníka sa robot presunie do polohy nad otvorenou skúmavkou. Pohyb je plánovaný tak, aby bol šiesty kĺb robota zodpovedný za uzavretie skúmavky na svojom zápornom maxime natočenia (cca -2rad). V tomto stave, keď je robot nehybný, dôjde k zavolaniu služby na statické skalibrovanie silovo-momentového senzora s cieľom čo najpresnejšieho merania počas procesu uzatvárania. Riadenie beží v cykle nastavenom na 50 Hz. Po načítaní dát zo senzora je na základe veľkosti sily pôsobiacej v osi z stanovená lineárna rýchlosť koncového efektora v tejto osi smerom nadol. Po detegovaní nárazu (do skúmavky) je táto rýchlosť regulovaná tak, aby sa sila pôsobiaca na skúmavku od chápadla udržala na hodnote približne 10 N – uzáver pri uzatváraní klesá, teda je potrebná korekcia výšky koncového efektora. Od okamihu nárazu sa zároveň nastaví uhlová rýchlosť v osi z, čo spôsobí otáčanie šiesteho kĺbu robota zo záporného maxima -2rad na 2rad. Tým činom dochádza k uloženiu uzáveru na skúmavku. Tento proces trvá tak dlho, kým moment sily na osi z nedosiahne hodnotu 1 Nm, ktorá bola empiricky stanovená ako dostačujúca na uzavretie skúmavky.

Nikdy sa však nepodarí skúmavku uzavrieť skôr, ako príde šiesty kĺb manipulátora na svoje kladné maximum. Vtedy na malú chvíľu dôjde k publikovaniu opačnej hodnoty lineárnej rýchlosti v osi z, čo spôsobí mierne nadvihnutie koncového efektora (približne o 2 mm) a súčasne sa otvorí chápadlo. Potom sa nastaví nulová lineárna rýchlosť v osi z a uhlová rýchlosť v tejto osi opačná, ako bola hodnota pri uzatváraní. Dochádza k pretáčaniu šiesteho kĺbu manipulátora späť na záporné maximum. Po jeho dosiahnutí je opäť prstami chápadla uchopený uzáver a v osi z smerom nadol publikovaná taká lineárna rýchlosť, aby na skúmavku pôsobila sila 10 N. Súčasne sa opäť nastaví uhlová rýchlosť v osi z taká, aby sa obnovil proces uzatvárania.

Uzatváranie a pretáčanie sa takto striedajú dovtedy, kým senzor v osi z nenameria pri uzatváraní moment sily veľkosti >1 Nm. Vtedy je proces považovaný za ukončený a je naplánovaný lineárny pohyb smerom hore. Tu dôjde k ďalšiemu preplánovaniu a vykonaniu pohybu, počas ktorého sa natočí šiesty kĺb do polohy na uloženie skúmavky. Z tohto stavu už možno vykonávať predplánované trajektórie podľa diagramu na obr. 9.

Aj napriek presnému polohovaniu robota a rezerve vyplývajúcej z kónického tvaru skúmavky sa môže stať, že skúmavka nezapadne do výrezu v platni stojana, ale narazí na jeho okraj. Je to spôsobené najmä nedokonalým uchopením uzáveru skúmavky, ako aj voľou a ohybnosťou plastu, čo spôsobuje odchýlku skúmavky od osi z. Táto výchylka, i keď malá, vytvorí pri výške skúmavky 11 cm dostatočnú chybu na znemožnenie presného trafenia výrezu. Takýto montážny problém sa nazýva „kolík v diere“ (angl. peg in hole). Komplikácie montážnych úloh pre neistoty pozícií v pracovnom priestore a geometrie dielov sú hlavným dôvodom nedostatku plne automatizovaných montážnych prístupov v priemysle, najmä automobilovom [4]. V práci [4] je opísaný algoritmus na báze optického systému a šesťosového silovo-momentového senzora. Po tom ako kolík narazí mimo diery a senzor zacíti kontakt, dochádza k rotácii kolíka v osiach x a y, aby sa našla pozícia diery snímaním momentov síl na týchto osiach. Po určení stredu diery je kolík naklonený týmto smerom tak, aby bol jeho spodný okraj vnútri diery. Kolík je potom posúvaný smerom k stredu diery, až kým nepríde ku kontaktu so stenou na druhej strane diery. Tým je zaručené nájdenie diery a dochádza ku korekcii zarovnania a zasunutiu kolíka do diery. V článku [5] je implementovaný iný algoritmus, ktorý po minutí diery kolíkom začína hľadať správnu pozíciu pohybom po Archimedovej špirále. Podobný algoritmus bol implementovaný aj v prípade navrhnutého riešenia.

Pri vkladaní skúmavky do výrezu existuje rezerva h = 1 mm v polomere. Uvažujme chybu polohovania do strany na úrovni maximálne err = 4 mm. Najväčší potrebný polomer špirály r je určený:

Rezerva špirály na úrovni 1 mm:

Zavedením posunu shift = r a prevedením r na základné jednotky:

Zvolili sme nasledujúce parametre: počet okruhov špirály n = 5, rýchlosť pohybu v = 0,001 m/s, frekvencia riadiaceho cyklu rate = 50 Hz. Doba tc, za ktorú vykoná manipulátor jeden okruh, je určená:

Vzorkovanie uhla f je potom určené:

Robot s uchopenou skúmavkou začne pomaly klesať. Po nameraní sily v osi z na senzore (náraz) sa klesanie skúmavky zastaví, resp. skúmavka sa jemne vráti nahor. Prvým krokom algoritmu je posun v zápornom smere osi x o polomer r. V riadiacom cykle sa nachádza počítadlo count = 0 počítajúce počet ubehnutých cyklov riadenia. Pri posune o polomer r v zápornom smere osi x sa do zložky x lineárnej rýchlosti zapíše hodnota Lx = –0,001. Táto správa sa posiela do Twist riadiacemu systému do splnenia podmienky:

Po splnení podmienky je manipulátor v polohe, odkiaľ môže začať vykonávať špirálový pohyb, a to po vynulovaní počítadla count. Veľkosť uhla kružnice φ, na základe ktorého sa počítajú zložky x a y lineárnej rýchlosti:

Nech cc je premenná pre počet uplynulých okruhov a premenná scale s rozsahom (0 – 1) určuje znižovanie amplitúd zložiek rýchlosti na dosiahnutie špirálovej trajektórie:

Zložky x a y lineárnej rýchlosti:

Po splnení podmienky:

Dochádza k ukončeniu okruhu špirály, zvýši sa počítadlo okruhov cc o 1 a vynuluje sa počítadlo count. Začína sa ďalší okruh špirály. Algoritmus sa končí nájdením diery, teda zaznamenaním poklesu sily pôsobiacej v osi z (zmiznutie prekážky) alebo podmienkou cc = n, keď došlo k ukončeniu špirálového pohybu, teda nenájdeniu diery a zlyhaniu algoritmu. Po nájdení diery je skúmavka umiestnená do stojana postupným zatlačením do požadovanej výšky. Následne je preplánovaný pohyb nad skúmavku a z tohto bodu už možno vykonávať predplánované trajektórie podľa diagramu na obr. 9.

Na obr. 10 sa nachádza grafické znázornenie špirálového algoritmu pre tri prípady netrafenia výrezu. V strede sa nachádza špirála, po ktorej sa pohybuje stred spodnej podstavy skúmavky. Čierna kružnica predstavuje výrez, do ktorého má vojsť skúmavka. Modrá kružnica znázorňuje ideálny prípad dokonalého umiestnenia skúmavky do stredu výrezu s rezervou 1 mm okolo. Červená kružnica je reálna poloha skúmavky. Zelený pás je súbor kružníc predstavujúcich skúšané umiestnenia skúmavky. Z obrázku je zrejmé, že pri maximálnej chybe 4 mm algoritmus vždy nájde výrez.

Overenie algoritmov riadenia

Robot je vďaka presnému 3D modelu pracoviska schopný manévrovať v prostredí bez kolízií s okolitými predmetmi. Vygenerovaný IKFast modul nezlyhával a dosahoval dobré výsledky pri riešení inverznej kinematiky. Plánovanie trajektórií vopred sa ukázalo ako dobré riešenie. Niektoré trajektórie, napríklad pohyb smerom k zásobníku uzáverov, sa ukázali ako dosť zložité na plánovanie, čo spôsobovalo neúspešnosť úvodného plánovania. Napriek tomu sa po niekoľkých pokusoch vždy podarilo naplánovať celý súbor trajektórií. Keby sa tento pohyb plánoval v reálnom čase a plánovanie by nevyšlo, došlo by k nechcenému zastaveniu aplikácie.

Občasným problémom, ktorý sa vyskytoval veľmi náhodne, bolo zlyhanie riadenia robota. Dôvodom bola prekročená tolerancia cesty pri vykonávaní pohybu. Tento problém sa však z väčšej miery odstránil zadefinovaním nových cieľových pozícií pri týchto trajektóriách. Väčším problémom bolo nedokonalé upevnenie robota k podstavcu, resp. samotného podstavca k zemi. Pohyby robota spôsobovali silné vibrácie pracoviska a trasenie manipulátora počas vykonávania pohybov. Tieto problémy boli evidentnejšie pri pomalých lineárnych pohyboch.

Algoritmus ukladania uzáveru na skúmavku fungoval takmer bez chyby. Problémy, ktoré sa vyskytli, boli skôr hardvérového charakteru. Išlo najmä o prešmykovanie uzáveru v prstoch chápadla, čo spôsobovali vydraté styčné plochy prstov a uzáveru vzhľadom na dlhodobé používanie. Napriek tomu nie je vhodné pôsobiť na uzáver silou väčšou ako 50 N, keďže dochádza k jeho značnému opotrebovaniu až poškodeniu. Lepším riešením je vystlanie prstov gumou alebo podobným materiálom, aby sa zvýšilo trenie, čo zabezpečí spoľahlivé uzavretie.

Na obr. 11 sú zobrazené tri priebehy sledovaných veličín počas procesu uzatvárania skúmavky. Zachytených je len posledných 26 sekúnd z podstatne dlhšieho priebehu, z toho približne jedenásť sekúnd dochádza k pretáčaniu šiesteho kĺbu na opačné maximum. Tento jav je zvýraznený sivým rámčekom, v ktorom je vidieť takmer nulové hodnoty priebehu sily a momentu, keďže chápadlo vtedy nie je v kontakte so skúmavkou. Moment sily začína rásť až v posledných sekundách uzatvárania. Uzáver sa totiž na skúmavku ukladá hladko, k odporu dochádza až na posledných približne 45° otáčania. Aby nedošlo k prešmykovaniu, s rastom momentu sily klesá uhlová rýchlosť uzatvárania. Do toho momentu sa však môže kĺb otáčať plnou rýchlosťou.

Hľadanie výrezu na umiestnenie skúmavky špirálovým algoritmom sa ukázalo ako veľmi efektívne riešenie. Algoritmus nevyniká rýchlosťou, aplikácia bola zameraná najmä na kvalitu a stabilitu riešenia. Zvýšenie rýchlosti malo za následok neskorú reakciu na pokles sily pôsobiacej na skúmavku a vybehnutie z už nájdeného výrezu späť na jeho okraj. V horších prípadoch detekcia poklesu sily úplne absentovala.

Pri nižšej rýchlosti (1 mm/s) sa dosiahla vysoká úspešnosť nájdenia výrezu. Problematické situácie nastávali, ak prišlo k zachytávaniu výrezov skúmavky určených na zapadnutie do uzatváracej podložky o okraj výrezu. To viedlo k zlému natočeniu skúmavky. Tento problém sa dá korigovať zabrúsením výrezov skúmavky a prechodom na systém jedného výrezu.

Záver

V rámci výskumnej práce bol navrhnutý a implementovaný algoritmus riadenia, ktorý umožnil robotickému manipulátoru uchopovať a prenášať rôzne vzorky, vkladať ich do analyzátorov a bezkolízne sa pohybovať v štruktúrovanom prostredí laboratória. Na riešenie bol použitý robotický manipulátor LWA 4P vyznačujúci sa vysokou obratnosťou. Boli navrhnuté aj zložitejšie algoritmy riadenia manipulátora fungujúcich spolu so silovo-momentovým senzorom. Implementovaný bol algoritmus na uzatvorenie skúmavky a algoritmus špirálového hľadania cesty na jej bezpečné uloženie do stojana. Pri dosiahnutí vysokej úspešnosti možno túto aplikáciu plnohodnotne využiť v laboratóriách. V súčasnosti prichádza k výraznej modifikácii pracoviska. Pracovisko bolo prerobené na pohyblivú platformu, ktorú možno odtlačiť na ľubovoľné miesto poskytujúce dostatočný priestor. Zároveň prišlo k zvýšeniu modulárnosti pracoviska – je rozšírené o platňu, na ktorú možno upevňovať ďalšie prvky. Aktuálne sa pracuje na automatizácii odoberania vzoriek. Vznikne tak naozaj komplexné pracovisko inteligentne riadeného robota použiteľného v laboratóriu.

Poďakovanie

Táto práca vznikla za finančnej podpory Agentúry na podporu výskumu a vývoja (APVV-17-0214, APVV-16-0006, APVV-16-0124) a DIH2 (A Network of Robotics DIHs for Agile Production).

Literatúra

[1] BOYD, J. 2002. Robotic Laboratory Automation. In: Science, 2002, Vol. 295, No. 5554, pp. 517 – 518. ISSN 0036-8075.

[2] GWYNNE, P. – HEEBNER, G. 2013. Laboratory Technology Trends: Advances in Lab Instrumentation. [online]. In: Science, 2013, Citované 3. 1. 2018. Dostupné na: <http://www.sciencemag.org/site/products/labl.xhtml>. ISSN 1095-9203.

[3] Image Areacell::explorerTurnkey Automation for High Content and Cellular Screening, https://www.perkinelmer.com/Content/RelatedMaterials/Brochures/BRO_CellExplorererHighContentCellScreening.pdf.

[4] ABDULLAH. M. W. et al. 2015. An Approach for Peg-in-Hole Assembling using Intuitive Search Algorithm based on Human Behavior and Carried by Sensors Guided Industrial Robot. [online]. In: IFAC-PapersOnLine, 2015, Vol. 48, No. 3. Citované 5. 5. 2018. Dostupné na: <https://www.sciencedirect.com/science/article/pii/S2405896315005340#>. ISSN 2405-8963.

[5] GHALYAN, I. F. J. – PLAPPER, P. – VOOS, H. 2014. Position Identification in Force-Guided Robotic Peg-in-Hole Assembly Tasks. [online]. In: Procedia CIRP, 2014, Vol. 23, Citované 5. 4. 2018. Dostupné na: <https://www.researchgate.net/publication/268225212_Position_Identification_in_Force-Guided_Robotic_Peg-in-Hole_Assembly_Tasks>. ISSN 2212-8271.

Patrik Bakyta
František Duchoň
Marian Vojs
Ľuboš Chovanec
Andrej Babinec
Národné centrum robotiky o.z.
www.nacero.sk