Model šestiosého robotu v prostředí Matlab

Transkript

1 VYSOKÉ UČENÍ TECHNICKÉ V BRNĚ FAKULTA ELEKTROTECHNIKY A KOMUNIKAČNÍCH TECHNOLOGIÍ ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Model šestiosého robotu v prostředí Matlab AUTOŘI PRÁCE : Petr Boháč Tomáš Fábry Ivo Faltus VEDOUCÍ PRÁCE: Ing. Tomáš Florián ROK: 2012

2 Teorie: Jazyk VRML 97 Virtual Reality Modeling Language Původně byl jazyk VRML navržen pro potřeby grafického zobrazení obsahu na webových stránkách. Jazyk byl upraven pro tvorbu interaktivních 3D scén a standardizován normou ISO/IEC :1997, proto se tento jazyk jmenuje VRML97. Tento jazyk je otevřený a pro uživatele relativně jednoduchý, proto byl vybrán do nástrojů MATLAB/Simulink. Souřadný systém tohoto jazyka se ovšem od prostředí MATLAB liší. V jazyku VRML osa Y směřuje nahoru a osa Z směřuje k nám, zatímco v prostředí MATLAB osa Z směřuje nahoru a osa Y směřuje od nás. Transformace je snadná, provádí se prohozením souřadnic. Stejně jako VRML má orientovaný souřadný systém toolbox SimMechanics. Velikosti jsou ve VRML udávány v metrech, úhly v radiánech a čas v sekundách. Obrázek 1, Okno parametrů modelu Pro zabudování 3D modelu do Simulinku se využívá bloček VR Sink, je součástí knihovny Simulink 3D Animation. Kde v sekci Source file vybereme soubor modelu s příponou wrl, zde C3_virtualni_model.wrl. Poté se automaticky vyplní pole Description (na základě informace v poli Title uzlu WorldInfo ve VRML souboru) A v pravé části dialogu se objeví strom představující zjednodušenou strukturu právě asociovaného virtuálního světa. U uzlů, jejichž vlastnosti je možné ovládat, je červený trojúhelník. Tyto uzly je možné rozkliknout, zpřístupní se nám buď seznam polí, jejichž hodnoty můžeme nastavovat ze Simulinku, anebo seznam podřízených uzlů. Po rozkliknutí uzlu C3_Arm2 se nám zobrazí seznam vlastností, které můžeme u tohoto objektu (typu Transform) nastavovat. V tomto případě nás budou zajímat pole translation, rotation a center, představující 1

3 polohu, natočení a střed otáčení kloubu. Po zaškrtnutí těchto polí se v simulinku u tohoto bloku objeví vstupy pro tyto hodnoty. Základní typy objektů V jazyku VRML je definováno několik uzlů (nodes), ty lze rozdělit na různé skupiny. Například skupina pro popis geometrie objektů, pak pro určení jejich povrchu, zdroje světla atd. sady. Základními geometrickými prvky jsou: Text, Jehlan, Kostka, Válec, Koule, sada bodů, a indexované Model lze upravovat, buď v běžném textovém editoru, nebo s využitím V-realm Builderu z MATLABu. Pozn.: Přípona souborů s modely je *.wrl Obrázek 2, Okno V-Realm Builderu #VRML V2.0 utf8 WorldInfo { title "Red Sphere" DEF Sphere1 Shape { appearance Appearance { material DEF Red Material { ambientintensity shininess diffusecolor geometry DEF GeoSphere1 Sphere { radius Ukázka kódu vytvářejícího červenou kouli převzato z Wikipedie Model robota C3 Model využívaný k simulaci pohybu skutečného robota jsme vytvořili z originálních souborů firmy Epson na soubor byl převeden do jazyku VRML pomocí programu Solidworks. Při převodu byly vyřazeny skryté detaily. Vytvořený model je však stále 2

4 příliš náročný na simulaci, proto byl zjednodušen programem Vizup. Verze Vizup 1.8 je ke stažení zdarma, jen ji je třeba spouštět v režimu pro win98. Vypracování: Pro možnost simulace pohybu robota byl vytvořen pomocí robotického toolboxu model robota. Tento model je vytvořen v m-filu EPS_K.m Zdrojový kod: %d - dlzka v smere osi klbu (twist), v ose z %a - dlzka kolmo na os klbu %alfa - uhol nasledujuceho klbu voci aktualnemu klbu % theta d a alfa inicializacni pozice L(1) = Link([ pi/2 0 0]); arm_1=seriallink(l,'name','arm_1'); L(2) = Link([ pi/2]); arm_2=seriallink(l,'name','arm_2'); %připojení jednotlivých částí k sobě L(3) = Link([ pi/2 0 0]); arm_3=seriallink(l,'name','arm_3'); L(4) = Link([ pi/2 0]); arm_4=seriallink(l,'name','arm_4'); L(5) = Link([ pi/2 0]'); arm_5=seriallink(l,'name','arm_5'); L(6) = Link([ ]); arm_6=seriallink(l,'name','arm_6'); EPSONC3=SerialLink(L,'name','Epson C3'); %L(1:6) jednotlivé části 6osého robota 3

5 Obrázek 3, Ukázka robota s popsanými částmi Simulace Simulace robota se provádí pomocí bloku simulinku, který je propojený s ovládacím oknem vytvořeným v GUI. Model simulinku je zobrazen na obr.3 a je popsán níže. Obrázek 4, Simulační model (1-ruční nastavení úhlu kloubů modelu robota, 2-část zprostředkovávající komunikaci s robotem, 3- část výpočtů parametrů pro nastavení pozic natočení modelu robota, 4- blok virtual-toolboxu obsahující model robota 4

6 Část 1 V této části se dá pomocí 6ti sliderů nastavit úhel natočení jednotlivých kloubů. Část 2 Tato část obstarává pomocí s-funkce komunikaci s robotem a to jak zasílání příkazu o žádané poloze, tak vyčítání aktuální polohy robota. Popis sfuntmpl: Při prvním spuštění se naváže TCP/IP komunikace s robotem a pomocí příkazu INIT se zapnou motory robota global t; t=tcpip(' ',2000); set(t,'outputbuffersize',3000) fopen(t); fprintf(t,'!init\n'); fscanf(t) Poté je cyklicky, každých 200ms volána funkce, která na začátku vyšle robotovy příkaz pro zjištění úhlu natočení jednotlivých kloubů. Tyto úhly jsou poté zpracovány a odeslány na výstup s-funkce. Z této cyklické funkce je také zasílán požadavek pro natočení robota do určité polohy. Odesílání polohy lze provést pomocí vytvořeného rozhraní GUI, které se spustí při otevření modelu. V GUI můžeme model spustit, poté nastavit hodnoty X,Y,Z a úhlu natočení koncového bodu a poté je odeslat. Příjem dat: fprintf(t,'?curposj\n'); %odeslání požadavku informace o natočení kloubů for k=1:50 ; end %delay pro zaslání A=fscanf(t,'<CurPosJ: %f %f %f %f %f %f'); %Čtení aktuální pozice %Čtení dokud není v A aktuální pozice while size(a)~=6 A=fscanf(t,'<CurPosJ: %f %f %f %f %f %f'); end A=A/180*pi; %Přepočet z radiánů na stupně Odesílání: send=sprintf('!go\\n%d %d %d %d %d %d\\n',x,y,z,ux,uy,uz); %vytvoreni retezce fprintf(t,send); %poslání příkazu for k=1:50 ; end %delay fscanf(t) %vyčti potvrzení o pohybu Část 3 parametry pro nastavení pozic jednotlivých částí modelu Nastavení pozice a úhlu jednotlivých částí modelu probíhá pomocí nastavení tří konstant : center vektor X,Z,Y určijící vztažný bod rotace dílu translation vektor X,Z,Y určijící translaci dílu vůči bodu {0,0,0 rotation vstupem je 4 složkový vektor určující rotaci kolem bodu X,Y,Z a úhel rotace Hodnoty vektoru traslace [X,Z,Y] získáváme pomocí vytvořeného robota EPSONC3 a jeho částí arm_1 až arm_6. Hodnoty jsou vyčítány z bločku T2xyz z knihovny robotik toolboxu. Virtual reality toolbox 5

7 má však přehozené souřadnice Z,Y oproti robotik toolboxu, proto je potřeba tyto výstupy prohodit, navíc je potřeba změnit znaménko u jedné z os zde osa X. Obrázek 5, Vytvoření translačního vektoru pro část 5 Hodnoty vektoru rotace získáváme výpočtem z natočení aktuálního + všech předcházejících úhlů natočení. Výpočet probíhá přes vytvoření rotačních matic (blok Rotation Angles to Direction cosine matrix z toolboxu Aerospace) a poté je z těchto matic vytvořen zmíněný vektor pomocí bločku Rotation Matrix to VRML Rotation (také z toolboxu Aerospace). Př. Napřiklad pro 5. část robota probíhá rotace takto: 1. rotace podle osy y o úhel natočení 1. kloubu 2. rotace podle osy z o úhel natočení 5. kloubu 3. rotace podle osy x o úhel natočení 4. kloubu (twist) 4. opět rotace podle osy z o úhel daný natočením 2. a 3. kloubu Obrázek 6, Vytvoření rotačního vektoru pro část 5 6

8 Obrázek 7, Porovnání skutečného robota (vpravo) a jeho simulace (vlevo) 7

9 Použitá literatura: 1. Dokumentace k produktu Simulink 3D Animation, dostupné na: 2. DANĚK, Jan Vizualizace dynamických systémů v prostředí virtuální reality, dostupné na: 3. PETER, Corke. Robotics, vision and control: fundamental algorithms in Matlab. 1st ed. New York: Springer, 2011, 570 s. ISBN Epson C3 Compact 6-Axis Robot Manual. Dostupné na: 8