Studentská tvůrčí a odborná činnost STOČ 2017

Transkript

1 Studentská tvůrčí a odborná činnost STOČ 2017 TELEOPERAČNÉ ŘÍZENÝ HUMANOIDNÍ ROBOT Stefan GRUSHKO VŠB-TUO: 17. listopadu 2172/15, Ostrava 20. dubna 2017 FAI UTB ve Zlíně

2 Klíčová slova: humanoidní, teleoperační, robot, Kinect, V-Rep. Anotace: Článek popisuje technické řešení teleoperačně řízeného humanoidního robotického systému. Jako základ struktury robotu byl využít systém Bioloid s modifikovanou kinematickou strukturou. K získávání údajů o poloze těla operátora je používán senzor Kinect. V článku jsou popsány matematické rovnice použité pro transformaci dat ze senzoru Kinect do pozic jednotlivých servomotorů robotu. Pro model robotu byl vytvořen matematický popis dle Denavit- Hartenbergových principů. Řídící jednotka vypočítává pomocí optimalizovaných zkrácených rovnic v reálném čase těžiště robotu a ověřuje kolize jednotlivých části robotu mezi sebou. V článku je taky popsaná softwarová struktura obou částí systému: robotu a PC aplikace operátora. Všechna softwarová řešení jsou napsaná s použitím programovacího jazyku C#. Pro dynamické simulace systému byl vytvořen podrobný model robotu v aplikaci V-Rep, virtuální simulace robotu získává identické pohybové příkazy jako skutečný robot. 2

3 Obsah 1. Úvod Mechanická část Elektronická část Software Software strana operátora Software strana robotu Real-time simulace systému Závěr Literatura

4 1. Úvod Cílem tohoto projektu bylo vytvoření humanoidního robotického systému ovládaného na dálku pomocí gest operátora. Gesta jsou rozpoznávány pomocí Kinect senzoru. 2. Mechanická část Systém byl rozdělen na dvě části: část robota a část řídícího PC. PC provádí náročné výpočetní operace. Části mezi sebou komunikují přes Bluetooth. Pod část PC spadá samotné PC a senzor Kinect. Mezi části robotu pak patří mechanická struktura, servomotory, senzory a hlavní kontrolér robotu. Dále bude popsána pouze část robota. Robot je založen na stavebnici Robotis Bioloid [1] s modifikovanou kinematickou strukturou (viz. Obr. 1), která dovoluje robotu lepe nápodobovat (opakovat) pohyby lidské horní končetiny. Každá ruka má o jeden stupeň volnosti více. Jedná se o servomotory s ID 19 a 20, které umožňují rotaci rukou v ose Z. Také poloha servomotorů s ID 5 a 6 byla upraveno pro otáčení v ose Y. a) b) Obr. 1 Struktury robotu: a) kinematická; b) reálná. Pro demonstrační účely byl vytvořen nový kryt hlavy a hlavního kontroléru. Obě komponenty byly vytištěny pomocí technologie 3D Rapid Prototyping. 3. Elektronická část Kompletní struktura elektrické části je uvedena na Obr. 2. Jako hlavní řídící jednotka robota bylo použito Netduino Plus 2 s rozšiřující deskou Netduino Dynamixel Shield v2.2 (dále NDS, ten byl vytvořen na Katedře Robotiky VŠB TUO, [2]). Deska, v kombinaci s Dynamixel knihovnou (napsanou na Katedře Robotiky VŠB TUO, [3]) umožňuje ovládat servomotory Dynamixel. Pro měření aktuální orientace v prostoru je robot vybaven analogovým tříosým akcelerometrem. Akcelerometr je umístěn v hlavě robota (Obr. 9 - souřadnicový systém robota, Z osa směřuje nahoru). Servomotory Dynamixel je možné spojovat do řetězové struktury Daisy-chain kdy jsou motory od každé končetiny zapojeny do série a pak připojeny k NDS. Komunikace robotu s PC se uskutečňuje pomocí Bluetooth modulu. 4

5 Obr. 2 Principiální schéma zapojeni. Jako zdroj energie pro teleoperační režim je použita 3s LiPo baterie, která je chráněna proti nadměrnému vybíjení ochranným modulem BMS (Battery Management System). Vytvořený demonstrační rám umožňuje zavěšení robotu v prostoru. Pokud je robot takto zavěšen může být napájen z externího zdroje přímo ze sítě a sériová komunikace je zprostředkována přes USB rozhraní. 4. Software Obr. 3 - Demonstrační stand s robotem, ovládací a vizualizační aplikace Pro ovládání robota přes PC byla napsána řídící aplikace v programovacím jazyku C# v MS Visual Studio IDE. Řídící aplikace je rozdělená do tříd dle funkcionality. Pro získávání dat ze senzoru Kinect byla využita volně dostupná sada nástrojů pro Windows Kinect SDK 1.8. Vzhledem k tomu, že systém byl rozdělen na dvě hlavní část, nemusí mít mikrokontroler robotu vysoký výpočetní výkon a všechny složité algoritmy běží na počítači. Veškeré nastavení pro obě SW části jsou sdílené ve společném souboru nastaveni, který se využívá při kompilaci programů. 4.1 Software strana operátora Aplikace pro PC (viz. Obr. 6) komunikuje s robotem přes sériovou komunikaci prostřednictvím Bluetooth s BaudRate 57600b/s. V případě demonstrace se systém napájí z vnějšího zdroje energie a robot se může spojit drátově s řídicím PC pomocí USB rozraní. 5

6 Obr. 6 - PC aplikace Řídící PC aplikace má dvě záložky: "Sequence" a "Robot", dále horní menu společných příkazů, společnou oblast pro zobrazení stavových hlášek a nastavení komunikace. V záložce "Sequence" může uživatel přidávat, nastavovat a měnit vlastní sekvence. Každá sekvence je vytvořena z jednotlivých póz, které mají svoji vlastní rychlost vykonávaní. Uživatel může přidat novou pózu do jakékoli sekvence pohybu. Zároveň PC aplikace posílá požadavek na aktuální polohu natočení všech kloubů robotu. Po obdržení tohoto požadavku robot získává aktuální polohu každého servomotoru, a pak tyto hodnoty posílá zpátky do počítačové aplikace. Při vykonávání pohybové sekvence PC aplikace pak využívá tyto data ke generování plynulého přechodu od aktuální pozice k pohybové sekvenci. Celkový počet dílčích póz se nastavuje automaticky a závisí na konstantě zadané uživatelem pro každou sekvenci i na minimálním časovém intervalu potřebném pro odesílání příkazů na robota (omezen vzhledem k rychlosti komunikace). Sekvence mohou být uloženy do Sequence.xml souboru. Po spuštění aplikace se všechny uložené sekvence načtou z tohoto souboru. Některé pohybové sekvence nelze smazat např. pohybové sekvence vstávání. V průběhu vykonávání pohybové sekvence indikační systém robotu mění barvu LED diod umístěných na hlavě robotu ze standardní modré barvy na zelenou. Záložka "Robot" zobrazuje aktuální data z akcelerometru robotu, dále aktuální polohu jeho těžiště vůči základnímu souřadnicovému systému, data od senzoru Kinect a také zobrazuje aktuální prostorovou orientaci robotu. Program sbírá data ze senzoru Kinect, vlákno na pozadí přepočítává úhly pro každý servomotor a tyto data a odesílá do robotu s intervalem 20 ms. Kinect pro Windows v1 je schopen rozpoznat pozici až 6 osob, ale pouze dva celé skeletony. Vysílá nalezené skeletony v samostatném vlákně (spolu s vizuální a zvukovou informací). Aplikace PC vybírá skeleton, který je nejblíže k senzoru Kinect a také kontroluje, zda je skeleton ve vhodném rozsahu vzdáleností od středu pracovního prostoru senzoru. Kromě toho na této záložce může uživatel přepínat mezi režimy: automatické vstávaní po pádu, režim zrcadlového pohybu, pohyb pouze rukama a další režimy aplikace. Celková struktura PC aplikace je zobrazena na Obr. 7. Paralelní vlákno, které se spouští s intervalem 20ms vykonává výpočet úhlů každého kloubu skeletonu a posílá data přes sériovou komunikaci do robotu. Při každé iteraci algoritmus ověřuje, jaké úhly se změnily a v případě, že se změnila menší polovina, posila robotu, pouze změně úhly převedené do bajtů. Tato úprava 6

7 byla vytvořena za účelem snížení vytížení komunikační linky a zvyšování celkové frekvence posílaní příkazů. Obr. 7 - SW struktura PC aplikace Lidské tělo má mnohem více stupňů volnosti než kolika disponuje tento robotický systém, proto byl počet stupňů volnosti lidských končetin redukován na pouhé 4 DOF jimiž disponuje robot. Spodní končetina robotu disponuje 6 DOF, avšak k řízení končetiny robotu se využívají pouze 4 DOF. Zbývající dva DOF v kotníku algoritmus udržuje rovnoběžně s hlavní rovinou XY robotu. U všech končetin byl použit stejný algoritmus pro výpočet úhlů. a) b) Obr. 8 - Výpočet úhlu pro 4 DOF rameno: a) přepočet úhlu z dat z Kinectu; b) vzhledem k reálnému rameni robota. Kinect poskytuje polohy X, Y, Z každého kloubu rozpoznaného skeletonu. Z těchto údajů jsou vypočteny (rovnice 2-5) vektory r1, r2, r3, které jsou pak použity pro výpočet polohy každého servomotoru. Výpočet potřebného natočení motorů v lokti a rameni je úhel mezi dvěma vektory (Obr. 8): Nastavení servomotoru v loktech z kosinové věty: 1 1 arcsin r x, (2) r1 r 1 2 arcsin y, (3) r1 2 3 arcsin r x, (4) r2 7

8 kde: φ1 úhel natočení v rameni v ose Y [ ], φ2 úhel natočení v rameni v ose X [ ], φ3 úhel natočení v lokti v ose Z [ ], φ4 úhel natočení v lokti v ose Y [ ], r1 3D vektor od ramene k lokti, r2 3D vektor od lokte k zápěstí, r3 3D vektor od ramene k zápěstí, rnx X komponenta vektoru n, rny Y komponenta vektoru n arccos r3 r2 r 4, (5) 2 r 1 r2 Identické výpočty se aplikují pro druhou ruku a obě nohy robotu. Vzhledem k tomu, že pracovní rozsah servomotorů Dynamixel AX-12A v kloubovém módu je omezen na , každý z vypočtených úhlů se upravuje přidáním nebo odečtením hodnoty z kalibračního úhlů tak, aby byl výsledek vhodný pro kinematickou strukturou robota. Každý kloub robota má své vlastní omezení úhlu, který snižuje pravděpodobnost, že dojde ke kolizi mezi články. Pomocí údajů z akcelerometru umístěného na robotu, dokáže systém rozpoznat 6 různých stavů orientace robotu v prostoru (každé orientaci bylo přiraženo číslo od 0..5 Obr. 9). Pokud dojde k upadnutí na libovolnou stranu, robot automaticky spustí příslušnou pohybovou sekvenci pro vstávání. Obr. 9 - Číslovaní možných variant orientaci robotu v prostoru 4.2 Software strana robotu Hlavní vlákno aplikace Netduino přijímá údaje o aktualizovaných nastaveních servomotorů z PC aplikace. Servomotory Dynamixel umožňují nastavení pozic několika motorů najednou a to pomocí příkazů typu "SYNC". Background-vlákno je zodpovědné za digitalizaci a odesílání dat z akcelerometru do počítačové aplikace. Pro příjem a zpracování dat z analogového akcelerometru byla využita třida C# pro akcelerometr [4] vytvořená na Katedře Robotiky. Celková struktura aplikace je na Obr. 4. 8

9 Obr. 4 - SW struktura na straně robotu Na základě kinematické struktury robotu byly pomocí Denavit-Hartenbergových pravidel vytvořeny transformační matice, které popisuji celý matematický model robotu. Následně dle z tohoto matematického popisu byly vytvořeny zkrácené matematické rovnice pro výpočet těžiště. Nejdříve se vypočítávají těžiště jednotlivých končetin a až potom se vypočítavá celkové těžiště systému dle vzorce 1. Vypočet běží v reálném čase na straně robotu v řídicí jednotce Netduino. Přepočet souřadnic těžiště se spouští při každé změně nastavení polohy motorů a trvá 6ms. Souřadnice se dále posílají do PC aplikace, kde se zobrazují. kde: xt, yt, zt souřadnice těžiště X, Y, Z části, mt hmotnost části. x t, y t, z t = (x t, y t, z t m t ) m t, (1) Za účelem ochrany robotu proti kolizím svých vlastních částí byl doplněn o softwarovou ochranu proti kolizi. Jednotlivé části robotu, u kterých hrozí riziko kolize, byly doplněny o virtuální sféry (Obr. 5), pomocí kterých se zjišťuje kolize mezi součástmi. Na základě celkového matematického modelu robotu byly vytvořeny optimalizované rovnice pro výpočet souřadnic jednotlivých kolizních objemů. Kontrola kolize se provádí v řídicí jednotce a probíhá při každé změně nastavení servomotorů, trvá 5ms. Obr. 5 Zobrazení kolizních objemů 5. Real-time simulace systému Současně s odesíláním dat do robotu jsou ty samá data posílána do softwaru V-Rep [5], kde se vytváří virtuální simulace robota. Struktura simulačního modelu robotu je stejná jako skutečná kinematická struktura systému. 9

10 c) d) Obr Simulační model ve V-Rep: a) visuální; b) dynamický. Pro simulační prostředí byl použit zjednodušený dynamický model. Zjednodušený model nevyžaduje tak velký výpočetní výkon a umožňuje hladší průběh simulace. Simulační model rovněž disponuje možností zobrazení polohy kolizních objemů používaných při kontrole kolizí mezi součástmi. 6. Závěr Práce na systému stále probíhá, ale systém již plní svůj hlavní cíl kopíruje pohyby operátora. V článku je popsaná elektrická a softwarová struktura systému, jeho možnosti a funkce. 7. Literatura [1] Bioloid Kit. Robotis. [online] [cit ]. Dostupné z: [2] JOCHEC, J., GRUSHKO, S., Ing. BOBOVSKÝ, Z. PHD, Ing. BABJAK, J. PHD. Netduino dynamixel shield v2.2. VŠB-TU Ostrava, Fakulta strojní, Katedra robotiky, In: Department of Robotics [cit ]. Dostupné z: [3] GRUSHKO, S., Ing. BOBOVSKÝ, Z. PHD. Dynamická knihovna pro řízení pohonu Dynamixel[online]. In: Department of Robotics [cit ]. Dostupné z: [4] GRUSHKO, S., Ing. BOBOVSKÝ, Z. PHD. Dynamická knihovna pro analogový akcelerometr[online]. In: Department of Robotics [cit ]. Dostupné z: [5] V-Rep: Virtual robot experimentation platform. In: Coppelia Robotics [online] [cit ]. Dostupné z: [6] GRUSHKO, S., Ing. BOBOVSKÝ, Z. PHD. Teleoperated Humanoid Robot. Transactions of the VŠB Technical University of Ostrava, Mechanical Series. 2016, vol. LXII, no. 2, pp , ISSN [cit ]. 10