ERK'2021, Portorož, 458-461 458 Kombinirana metoda za antropomorfno teleoperiranje robotske roke z nosljivimi inercialnimi senzorji Domen Ulbl 1 1 Fakulteta za elektrotehniko, računalništvo in informatiko, Univerza v Mariboru E-pošta: domen.ulbl@.um.si Combined Method of Antropomorphic Robotic Arm Teleoperation using inertial Sensors Abstract. One useful way of robotic arm control is gesture based teleoperation. In such cases it is desirable that the robot moves in an antropomorphic way and closely mimics human motion. In our project we used three Myo sensor bracelets to capture human arm motion, which was used to control a simulated model of Kinova Mico robotic arm. We presented a combined control algorithm composed from direct mapping of human arm poses to robotic joint coordinates and control of robotic end effector orientation via inverse kinematics calculation. The system was implemented in Matlab, ROS and CoppeliaSim simulation environment. The algorithm performance was tested by orientation comparison of human and robot arm segments. 1 Uvod Uporaba robotov se vsako leto širi in posega v nova področja človekovega življenja. Pri tem je velikokrat potrebno, da robote nadzirajo ljudje brez specializirane izobrazbe. Eden izmed alternativnih načinov nadzora robota je teleoperacija robota, kjer uporabnik vodi robota na daljavo. Uporaba kretenj pri interakciji z roboti je koristna, ko roboti sodelujejo z ljudmi v okoljih, ki so v prvi vrsti namenjena za ljudi. V takšnih okoljih želimo, da je gibanje robotov t. i. antropomorfno, gibanje robota naj torej sledi človeškemu gibanju. Kretnje uporabnika lahko služijo kot navodilo za direktno teleoperacijo. Cilj je razviti svoj sistem za vodenje robotske roke na podlagi kretenj človeške roke. Glavni problem je zasnovati algoritem, ki bo upošteval razlike v kinematični zgradbi človeške in robotske roke. V tem članku gibanje človeške roke zajamemo z nosljivimi inercialnimi senzorji. Algoritem vodenja za antroporfmno teleoperiranje smo zasnovali za šest-osno robotsko roko. Testiranje smo opravili na modelu robotske roke Kinova Mico [1], ki ima specifično kinematično strukturo. Zaradi razlik v geometriji in kinematični zgradbi lahko človeška roka izvede gibe, ki jim takšen šest-osni robot ne more popolnoma slediti. Z zasnovo algoritma vodenja želimo to razliko premostiti tako, da se bosta konfiguracija robotskega mehanizma in lega konca mehanizma čim bolj ujemali s konfiguracijo in lego konca človeške roke. 2 Robotska roka Kinova Mico Robotska roka Mico, kanadskega podjetja Kinova, je namenjena pomoči gibalno oviranim osebam in raziskavam. Gre za šest-osno robotsko roko z dosegom 70 cm. Posebnost je ukrivljeno tri-osno zapestje, pri katerem se rotacijske osi na sekajo v isti točki. Kinematični model robotske roke je podan z Denavit Hartenbergovi parametri, ki so dostopni v viru [1]. Na sliki (Slika 1) je prikazan robot z označenimi koordinatnimi sistemi DH modela. Slika 1: Kinova Mico z označenimi koordinatnimi sistemi in smermi zasuka sklepov (povzeto po [1]) 3 Metoda antropomorfnega teleoperiranja Metoda antropomorfnega teleoperiranja kombinira dva pristopa. Prvi del na podlagi konfiguracije modela človeške roke neposredno določi sklepne koordinate prvih treh segmentov robotske roke. Cilj je zagotoviti čim bolj antropomorfno konfiguracijo robotske roke in tudi primerno položajno ujemanje robotskega zapestja s človeško roko. Drugi del metode implementira vodenje orientacije, ki skrbi, da se orientacija prijemala robotske roke čim bolje ujema z orientacijo dlani človeške roke. 3.1 Poenostavljen kinematični model človeške roke V literaturi [2] je človeška roka modelirana kot redundantna kinematična veriga s 7 prostostnimi stopnjami oz. rotacijskimi sklepi. V tem članku za zajem gibanja človeške roke uporabimo tri senzorske zapestnice Myo [3], ki vključujejo IMU enote, s katerimi zagotavljajo informacijo o lastni 3D orientaciji glede na zunanji referenčni koordinatni sistem   B . Uporabnik ima nameščene tri zapestnice na nadlakti (   1 M ), podlakti (   2 M ) in dlani (   3 M ) (Slika 2). 459 Slika 2: Namestitev zapestnic Myo na roki uporabnika Za opis konfiguracije človeške roke na podlagi zajetih orientacij iz zapestnic in za snovanje metode teleoperiranja smo sestavili pomožni kinematični model človeške roke predstavljen na sliki spodaj (Slika 3). Sestavljen je iz fiksnega baznega člena in treh gibljivih segmentov dolžine 1 L , 2 L in 3 L ter treh sferičnih sklepov 1 J , 2 J in 3 J . Matematično je opisan s homogenimi transformacijskimi matrikami med koordinatnimi sistemi   0 (trup),   1 (nadlaket),   2 (podlaket),   3 (dlan) in dodatnim pomožnim orodnim koordinatnim sistemom   E kot je prikazano na spodnji sliki (Slika 3). Slika 3: Pomožni kinematični model človeške roke Koordinatni sistem   0 je izhodiščni koordinatni sistem trupa. Predpostavljamo, da je orientacija   0 sovpada z orientacijo   B . Vektorji 1 p , 2 p in 3 p so izraženi v koordinatnih sistemih   1 ,   2 in   3   0 p 0 T i i i L   , (1) kjer je 1, 2,3 i  . Relativne lege zaporednih koordinatnih sistemov zapišemo s transformacijskimi matrikami (2), 1 1 1 1 1 R R p 1 R p 0 0 1 i i i i i i i i i i i i                     T , (2) kjer je 1, 2,3 i  . Orientacija   1 M na sliki (Slika 2) sovpada z orientacijo   1 in je glede na   0 izražena z 0 1 R , ki jo lahko direktno uporabimo v (2). Orientaciji zapestnice   2 M oz.   3 M na sliki (Slika 2) sovpadata z orientacijo   2 oz.   3 glede na   0 . Iskani matriki 1 2 R oz. 2 3 R lahko nato izračunamo po enačbi (3): 1 1 0 0 R R R T i i i i     , (3) kjer je 2,3 i  . Pomožni orodni koordinatni sistem   E smo uvedli, da poenotimo lego konca človeške in robotske roke in je fiksno pritrjen na   3 . Celotni poenostavljen kinematični model izraža lego   E glede na   0 in se izračuna kot (4): 0 0 1 2 3 1 2 3 E E     T T T T T . (4) 3.2 Preslikava osnovne poze človeške roke Koti 1  , 2  in 3  določajo zasuke 1., 2. in 3. sklepa robotske roke Kinova Mico, kot je to prikazano na sliki (Slika 1). Njihov doprinos v veliki meri določa pozicijo konca robotske roke. Sklepni koti 4  , 5  in 6  pa bistveno vplivajo na končno orientacijo robotske roke. Če želimo zagotoviti antropomorfno preslikavo in ustrezno položajno ujemanje robotske roke s človeško roko, lahko to dosežemo tako, da na nek optimalen način preslikamo osnovno pozo človeške roke v minimalno konfiguracijo prvih treh segmentov robotske roke. Pravila za določitev sklepnih koordinat 1., 2. in 3. robotske roke lahko določimo neposredno na podlagi primerjave njene kinematične zgradbe s kinematičnim modelom človeške roke. V ta namen na pomožnem kinematičnem modelu definiramo projekcijske kote 1  , 1  , 2  in 2  , ki so grafično predstavljeni na sliki (Slika 4). Slika 4: Projekcijski koti pomožnega kinematičnega modela Pripadajoči enačbi za določitev projekcijskih kotov sta: 1 1 iy ix p tan p i i i     , (5) 1 1 1 iz ix 2 i 2 y t p p n p a i i i i       , (6) 460 kjer je 1, 2 i  . Projekcijske kote 1  , 1  , in 2  potem lahko preslikamo v sklepne koordinate 1  , 2  in 3  robotske roke Kinova Mico. 3.3 Algoritem za kinematično vodenje orientacije Za nastavitev orientacije konca robota potrebujemo algoritem, ki bo orientacijo robotskega konca usklajeval z orientacijo dlani človeške roke. Orientacijska skladnost je izjemno pomembna za praktične manipulacijske naloge pobiranja in odlaganja objektov ipd. V tem članku za zagotavljanje orientacijske skladnosti predlagamo numerični algoritem inverzne kinematike, ki izhaja iz postopka v viru [4]. Orientacijo dlani človeške roke   E izrazimo s kvaternionom h Q , orientacijo konca robotske roke pa izrazimo kot r Q . Z uporabo pravil za kompozicijo rotacij in množenje kvaternionov izračunamo pogrešek orientacije (7),   1 , ) h r Q Q Q          (7) kjer sta   in   skalarni in vektorski del kvaterniona. Ko sta orientaciji enaki ima kvaternion pogreška vrednost   1, 0, 0, 0 Q   . Koncept algoritma kinematičnega vodenja orientacije je podan z enačbo (8): * ( )    θ J θ K e  , (8) kjer je: θ  izračunana (želena) sklepna hitrost robotske roke, * J (θ) generalizirani inverz geometrijskega Jacobijeve matrike robotskega mehanizma, K pozitivno definitna diagonalna matrika in e pogrešek lege v zunanjih koordinatah. Diagonalni elementi matrike K , katerih vrednosti ustrezno izberemo, predstavljajo ojačenje v shemi kinematičnega vodenja. Za izračun generaliziranega inverza Jacobijeve matrike * J uporabimo metodo dušenih najmanjših kvadratov (DLS) [5]: 2 k * T T -1 J = J (JJ + I) . (9) Faktor k tukaj določa hitrost konvergence rešitve in stabilnost v bližini singularnih leg. Visoka vrednost bo zagotovila stabilnost, ampak znižala hitrost konvergence. Pogrešek e ima dimenzije 6 1  in vsebuje samo orientacijski pogrešek iz (7), kot določa naslednja enačba   1 2 3 0 0 0 T        e . (10) V vsakem koraku izvedbe algoritma vodenja 1 k k t t t     iz sklepnih hitrosti θ  izračunamo nove sklepne koordinate z numeričnim integriranjem (11). 1 ( ) ( ) ( ) k k k t t t t     θ θ θ  (11) 3.4 Kombinirana shema vodenja za teleoperacijo Metoda vodenja kombinira direktno preslikavo kotov in algoritem vodenja orientacije. Preko zapestnic pridobimo podatek o orientacijah posameznih delov človeške roke ( M O ). Blok lim. izvede omejevanje spremembe orientacije ( L O ). V bloku IK izračunamo projekcijske kote pomožnega kinematičnega modela   1 1 2 2 , , ,      φ , ki jih v bloku map preslikamo v sklepne koordinate robotske roke ( 1:3 θ ). Na podlagi trenutne orientacije konca človeške roke E O in konca robotske roke R O izračunamo pogrešek orientacije O  . Sledi izračun želenih sklepnih hitrosti z enačbami (8)-(11). Vektor sklepnih hitrosti razdelimo na dva dela. Prvi del 1:3 θ  pomnožimo z t  , kar predstavlja popravek kotov 1:3 θ zaradi zagotavljanja boljše skladnosti orientacije. Drugi del 4:6 θ  pa integriramo, da izračunamo nove sklepne koordinate zapestja 4:6 θ . Cel vektor θ natančno določa lego robotske roke, pozicijo r p in orientacijo r O . Konec robotske roke uporabnik opazuje in jo po potrebi prilagaja s korekcijskim gibanjem svoje roke. Slika 5: Blokovna shema vodenja za teleoperacijo robotske roke 461 4 Rezultati Predlagano metodo smo testirali v hibridnem računalniškem simulacijskem okolju, kjer smo algoritem vodenja implementirali v Matlabu, virtualno simulacijo robotske roke izvedli s programskim paketom CoppeliaSim, nosljive senzorske zapestnice Myo pa priključili preko brezžične Bluetooth povezave in sistema ROS. Testiranje delovanja je obsegalo premik uporabnikove roke v več značilnih poz in opazovanje lege in konfiguracije robotske roke. V simulaciji prikazujemo pomožni kinematični model človeške roke (svetlo sivo-moder), ki sledi izvornim orientacijam zapestnic M O , in rumen model, ki sledi omejevanim orientacijam L O . Na sliki (Slika 6) so predstavljeni različni trenutki v poteku simulacije. Slika 6: Ključne lege na primeru teleoperacije s predlagano shemo vodenja Slika 7: Primerjava orientacij segmentov človeške in robotske roke, predstavljenih z Eulerjevimi ZYX koti. a) nadlaket in 2. člen robotske roke b) podlaket in 3. člen robotske roke c) dlan in robotsko prijemalo Na izseku 1 je uporabnik v procesu inicializacije zapestnic iztegnil svojo roko. Na izseku 2, 3 in 4 testiramo ujemanje orientacije nadlakti oz. podlakti z robotskimi segmenti. Nastavljamo jo z direktno preslikavo kotov. Orientacijo konca robota testiramo v izseku 5 in jo nastavljamo z algoritmom iz en. (8), (9) in (11). V izseku 6 se orientacije 2. in 3. robotskega člena ujemata z orientacijama podlakti in nadlakti, konec robotske roke zaostaja za koncem modela človeške roke. Na sliki (Slika 7) smo izrisali orientacijske Eulerjeve kote ZYX posameznih segmentov človeške (črtkana črta) in robotske roke (polna črta). Na grafu a) vidimo, da se orientacija nadlakti dobro ujema v Y in Z osi, rotacije okoli X osi pa robot zaradi svoje zgradbe ne more posnemati. Orientacija podlakti na grafu b) se dobro ujema le v Y osi, ker je ta poravnana z osjo 3. robotskega sklepa. Orientacija konca robota v c) se dobro ujema z orientacijo človeške dlani. Pri hitrejših spremembah orientacije človeške dlani opazimo počasnejši odziv zaradi hitrosti konvergence algoritma vodenja orientacije. Hitrost odziva izboljšamo z nastavitvijo ojačenja K v (8) in dušenja k v (9). 5 Zaključek Predstavljena metoda za direktno teleoperacijo omogoča izbiro približno antropomorfnih konfiguracij uporabljene robotske roke in sledenje v okviru njenih kinematičnih omejitev. Direktna preslikava kotov iz v grobem in hitro določi osnovno konfiguracijo robotske roke, ki jo z vodenjem orientacije konca natančneje prilagodimo. Nadaljnje delo obsega testiranje metode pri izvedbi praktičnih nalog, kot je na primer pobiranje predmetov, kjer poleg antropomorfne konfiguracije želimo nadzirati tudi pozicijo konca robotske roke. Metoda je zasnovana tako, da bi jo lahko ob manjših prilagoditvah v prihodnje prenesli tudi na 7-osno robotsko roko, ki pa zaradi svoje redundantne kinematične zgradbe omogoča bistveno višjo stopnjo antropomorfnega gibanja. Zahvala Iskreno se zahvaljujem mentorju izr. prof. dr. Alešu Hacetu za strokovno pomoč in vodenje pri pisanju tega članka. Literatura [1] K. Inc., Kinova SDK . User Guide, 2015 [2] A. M. Zanchettin, Kinematic motion analysis of the human arm during a manipulation task. 2010, str. 1-6. [3] T. L. Inc. Myo SDK Manual. 0.9.0. 2013 [4] B. Siciliano, Robotics Modelling, Planning and Control: Springer-Verlag London Limited, 2009. [5] S. R. Buss, Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares methods, University of California, 2009