ERK'2019, Portorož, 106-109 106 Daljinsko vodenje robota z inercijskimi merilnimi enotami Gaja ˇ Zumer 1 , Jernej Puc 2 , Janez Podobnik 1 , Marko Munih 1 1 Laboratorij za robotiko, Fakulteta za elektrotehniko, Univerza v Ljubljani 2 Fakulteta za matematiko in fiziko, Univerza v Ljubljani f 1 Trˇ zaˇ ska 25, 2 Jadranska 19g, 1000 Ljubljana E-poˇ sta:fjp4745, gz1671g@student.uni-lj.si,fjanezp, markog@robo.fe.uni-lj.si Abstract We describe a generally applicable approach for intu- itive and remote robot control, which relies on estimat- ing the orientation of inertial measurement units that are placed on the upper and lower part of the user’s arm. The Unscented Kalman filter, a key component in this estima- tion, is briefly discussed. Practical counter-measures to typical problems are considered, as well as suggestions for extending the application. The described approach is evaluated on a practical case, comparing the estimated trajectory to that of the robot and of Optotrak Certus’ reference. 1 Uvod Odkar se je sledenje ˇ cloveˇ skega gibanja pojavilo na ra- ziskovalnem podroˇ cju robotike, se je uveljavilo kot zmo- gljivo orodje pri vodenju robotskih sistemov; primeri upo- rabe se vrstijo od daljinskega upravljanja [1] do enostav- nega uˇ cenja trajektorij [2]. Pri teh je dejanska uporabnost navadno pogojena s sistemom za sledenje (npr. optiˇ cni ali mehanski), ki uporabnika omejuje z zahtevami po pro- storu, neprekinjeni vidnosti sledenega objekta ali drugimi fiziˇ cnimi omejitvami, poleg tega pa so lahko natanˇ cne konfiguracije cenovno nerazpoloˇ zljive [3]. Iz teh razlogov so dostopne, majhne in lahke naprave (ang. microelectromechanical systems - MEMS) postale pomemben merilni sistem na podroˇ cju kinematike in gi- banja [4, 5]. Z uporabo naprav, kot so inercijske merilne enote (IME oziroma ang. IMU - inertial measurement unit), lahko omejitve sistemov za sledenje gibanja po- ljubno omilimo glede na robustnost brezˇ ziˇ cne komuni- kacije. V prispevku je privzeta uporaba IME, ki vsebujejo tri- osni ˇ ziroskop, pospeˇ skometer in magnetometer, saj je na podlagi kotne hitrosti, pospeˇ ska in gostote magnetnega polja lahko z metodami za senzorno fuzijo (kot je npr. Kalmanov filter) ocenjena orientacija objekta. Osrednja ideja aplikacije je povezava lege vrha robota v baznem koordinatnem sistemu z lego dlani v koordina- tnem sistemu rame. V nadaljevanju je opisan in ovredno- ten postopek vodenja robota, ki realnoˇ casno sledi poziciji dlani, izraˇ cunani iz orientacij IME. Postopek omogoˇ ca zmogljivost roˇ cnega vodenja (hand-guiding) z dodano mo- ˇ znostjo vodenja na daljavo. 2 Metodologija Da bi lahko sklepali o primernosti vodenja robota z upo- rabo IME, primerjamo tri razliˇ cne meritve pozicij: meritve referenˇ cnih pozicij dlani, ki jih dobimo z uporabo Optotrak Certus sistema, s pomoˇ cjo Kalmanovega filtra izraˇ cunane ˇ zelene pozicije robota iz meritev IME in prebrane (dejanske) pozicije robota. Slika 1 prikazuje vse potrebne elemente merilnega sistema in povezave med njimi. Najpomembnejˇ si je pro- gram napisan v programskem jeziku Python, ki skrbi za uspeˇ sno in usklajeno komunikacijo med ostalimi elementi ter istoˇ casnim raˇ cunanjem ˇ zelenih pozicij z uporabo Kal- manovega filtra. Slika 1: Diagram komponent potrebnih za opravljanje meritve in vrste povezav med njimi. 2.1 Kinematika Poloˇ zaj rame p rama definiramo kot koordinatno izhodi- ˇ sˇ ce [0; 0; 0] T globalnega koordinatnega sistema, ki je po- ravnan s koordinatnim sistemom robota. ˇ Ce poznamo dolˇ zini nadlahti‘ nad in podlahti‘ pod ter njuni orientaciji R nad inR pod , p dlan izraˇ cunamo kot: p komolec = p rama +R nad v nad ; p dlan = p komolec +R pod v pod ; pri ˇ cemer sta vektorja nadlahtiv nad in podlahtiv pod de- finirana kot [‘ nad ; 0; 0] T in [‘ pod ; 0; 0] T za konfiguracijo na Sliki 2. 107 Slika 2: Razporeditev IME po roki uporabnika s pripadajoˇ co vektorsko obliko, ki doloˇ ca kinematiˇ cne lastnosti sistema. 2.2 UKF (Unscented Kalman filter) Med algoritmi za senzorno fuzijo je Kalmanov filter med najbolj uveljavljenimi. Slednji v vsakem trenutku oceni verjetnostno porazdelitev spremenljivk dinamiˇ cnega sis- tema glede na zaporedje meritev z znano statistiˇ cno na- pako. V obravnavanem primeru se z integracijo kotne hi- trosti po ˇ casu oceni orientacijo v danem trenutku (predic- tion), nato pa na podlagi meritev pospeˇ ska in magnetnega polja oceno dopolni glede na ujemanje z referenˇ cnimi po- lji (update). Orientacija je interno obravnavana v kva- ternionski obliki; kvaternioni, ki predstavljajo razˇ siritev kompleksnega prostora na ˇ stiri dimenzije, se v sploˇ snem uporabljajo zaradi prednostnih matematiˇ cnih lastnosti [6, 7]. Za nelinearne probleme, kot je ocenjevanje orienta- cije, predpostavke osnovnega filtra niso izpolnjene, zato je v ta namen uporabljena njegova razˇ siritev, t.i. Unscen- ted Kalman filter (UKF), ki jih zadovoljivo aproksimira. Ker je teoretiˇ cno ozadje filtra preobˇ sirno za podrobno obravnavo v tem prispevku, je bralec napoten k upora- bljenem viru [8]. Medtem ko lahko‘ nad in‘ pod enostavno izmerimo za vsakega uporabnika pred inicializacijo, seR nad inR pod dinamiˇ cno posodabljata glede na ocenjeni orientaciji IME. Orientacijo v diskretnem ˇ casun predstavimo z enot- skim kvaternionom q m;n = [q r q i q j q k ]; ki se posodablja glede na merjeno kotno hitrost! m;n , kot jo merim-ti IME, po znani enaˇ cbi 1 [6]: q m;n+1 =q m;n + Z _ q m;n dt; Z _ q m;n dt = 1 2 q m;n [0! m;n ] T t; (1) kjer je oznaka za kvaternionski oz. Hamiltonov pro- dukt. Oboje je kompenzirano s korakom korekcije ob po- sodobitvi ocene Kalmanovega filtra. Da oceno popravimo, projiciramo orientacijski kva- ternion v meritveni prostor, tj. rotiramo referenˇ cna vek- torja polj (teˇ znostno in magnetno), nakar je ocena primer- ljiva z rezultati meritev. Definicija vektorja teˇ znostnega polja je dokaj sploˇ sna, zato za normirani primer zadoˇ sˇ ca, da gledamo le z-komponento projekcije, medtem ko je vektor referenˇ cnega (Zemljinega) magnetnega polja teˇ zje definirati za vsako lokacijo posebej. Poleg tega je z njim doloˇ cena usmeritev globalnega koordinatnega sistema, za- to bi ˇ zeleli, da je definirano v skladu s specifiˇ cno aplika- cijo (npr. poravnano s koordinatnimi osmi upravljanega robota). V ta namen preko enaˇ cbe h ref =R T h inverzno rotiramo merjeno magnetno polje h nazaj v re- ferenˇ cnoh ref ter ga po preureditvi proglasimo za novo re- ferenco ^ h [7]. Najbolj smiselno je, da doloˇ cimo velikost (normo) horizontalne komponente in jo razdelimo med x;y-osi glede na ˇ zeleno deklinacijo (kotz -rotacije med referenˇ cnim poljem in baznim koordinatnim sistemom); ta mora biti eksperimentalno doloˇ cena (preverjena) pred vsako inicializacijo: h hor =k[h refx ;h refy ]k; ^ h x =h hor cos; ^ h y =h hor sin: Vertikalna komponenta ^ h z v tem primeru ostaja enaka kot pri h ref . Ko imamo referenco znano, je projekcija v meritveni prostor ponovna rotacija referenˇ cnega polja: h =R ^ h: Matematiˇ cni modeli, ki opisujejo stanje in prehodno delovanje Kalmanovega filtra, so lahko poljubno komple- ksni (in raˇ cunsko zahtevni). Za obravnavani primer je najpreprosteje, da se orientaciji IME na nadlahti in pod- lahti ocenjujeta soˇ casno s svojim filtrom, za stanje posa- meznega filtra pa se vzame pripadajoˇ ci orientacijski kva- ternion. 2.3 Vodenje robota in komunikacija Uporabljen je 6-osni robot Epson PS3-AS00 s krmilni- kom RC170. Na strani robota se izvaja program, napisan v pro- gramskem jeziku SPEL, ki skrbi za komunikacijo in vo- denje robota. Z raˇ cunalnikom komunicira po Ethernet protokolu (TCP/IP), pri ˇ cemer se na robota poˇ silja trenu- tno izraˇ cunano pozicijo dlani. Ukazu za linearni premik, ki se izvaja v zanki, je dodan naˇ cin CP (ang. continuous path control), ki zagotavlja zvezno gibanje vrha robota. 2.4 Optotrak Certus Z Optotrak kamero so bile izmerjene referenˇ cne pozicije dlani in rame. Vzpostavljena je bila UDP komunikacija s Simulink shemo na raˇ cunalniku, ki je bila namenjena shranjevanju meritev. Da so izmerjene pozicije dlani podane relativno glede na globalni koordinatni sistem (in ne koordinatni sistem merilne naprave - sistema Optotrak Certus), se od njih odˇ steje pozicijo ramep rama , ki v opisani aplikaciji sluˇ zi kot izhodiˇ sˇ ce globalnega koordinatnega sistema. 108 3 Rezultati Sledeˇ ci rezultati so bili pridobljeni s kroˇ znim gibanjem dlani v ravninix-y glede na globalni koordinatni sistem (gl. sliko 2). Na sliki 3 so za vsako koordinatno os pri- kazani grafi referenˇ cnih pozicij dlani (Opto), ˇ zelenih po- zicij vrha robota (IME) in prebranih pozicij vrha robota (Robo) v odvisnosti od ˇ casa. Slika 3: Grafi referenˇ cnih pozicij dlani merjenih z Optotrakom (Opto), ˇ zelenih pozicij vrha robota (IME) in dejanskih pozicij vrha robota (Robo) v odvisnosti od ˇ casa. Zavoljo preglednosti ordinate grafov niso v istem merilu. Napake po oseh - pov- preˇ cne vrednosti absolutnih razlik med referenˇ cnimi pozicijami dlani (Opto) in dejanskimi pozicijami robota (Robo) - so pri- merljive in znaˇ sajo9:5mm, 13:3mm, in8:9mm z ozirom na osix,y inz. Iz primerjave ˇ zelenih pozicij robota (IME) z njego- vimi dejanskimi pozicijami (Robo) je razviden ˇ casovni zamik, ki nastane zaradi komunikacije med raˇ cunalnikom in robotom ter premika robota. Pri primerjavi poteka re- ferenˇ cnih (Opto) in izraˇ cunanih pozicij dlani (IME) je iz grafov vidna razlika, ki nakazuje na pomanjkljivosti raˇ cunske metode in kinematiˇ cnega modela roke, slabe kalibracije IME, neugodne vplive iz okolja (npr. vpliv bliˇ zine kovinskih objektov na meritve magnetometra), itd. Najveˇ cje odstopanje se kaˇ ze med referenˇ cnimi pozicijami dlani (Opto) in pozicijami robota (Robo), kar je posle- dica prej omenjenih pomanjkljivosti. V tabeli so podane izraˇ cunane povpreˇ cne vrednosti omenjenih napak in pri- padajoˇ ci standardni odkloni. Osnovna statistiˇ cna analiza napak je grafiˇ cno prikazana na sliki 4. Tabela 1: Povpreˇ cne vrednosti in standardni odkloni napak (norm razlik) med izraˇ cunanimi pozicijami (IME), izmerje- nimi pozicijami robota (Robo) in izmerjenimi pozicijami dlani (Opto). Primerjane pozicije povp. vrednost [mm] std [mm] IME-Robo 8.7 5.6 Opto-IME 13.6 6.3 Opto-Robo 20.5 7.3 Slika 4: ˇ Skatliˇ cni diagram za tri razliˇ cne reprezentacije napak - med ˇ zelenimi pozicijami in dejanskimi pozicijami robota (IME- Robo), referenˇ cnimi pozicijami dlani ter ˇ zelenimi pozicijami ro- bota (Optotrak-IME) in med referenˇ cnimi pozicijami dlani in dejanskimi pozicijami robota (Optotrak-robot). Kakovost vodenja je odvisna od veˇ cjega ˇ stevila spre- menljivk in dejavnikov, zaˇ cenˇ si s fiziˇ cnimi omejitvami izbranega robota (konfiguracija sklepov, ˇ stevilo osi, de- lovni prostor, maksimalne hitrosti in pospeˇ ski, itd.). Med izvajanjem meritev je bila nastavljena minimalna hitrost premika robota na 900 mm=s. Doloˇ cena je bila ek- sperimentalno z namenom, da pri tej robot ni preveˇ c zao- stajal za premiki dlani, saj je potrebno upoˇ stevati ˇ casovni zamik pri komunikaciji. Smiselno bi bilo dinamiˇ cno pri- lagajanje hitrosti robota glede na izmerjene kotne hitrosti premikanja dlani, vendar v primerjavi z zagotovljeno mi- nimalno vrednostjo ne bi priˇ slo do izraza. Mirovanje predstavlja dodaten problem. Zaradi trese- nja roke robot sledi ˇ sumni trajektoriji, ˇ ceprav bi ˇ zeleli, da miruje na mestu. V tem primeru bi dinamiˇ cno prilagaja- nje hitrosti prav tako priˇ slo v poˇ stev. Za kompenzacijo tresljajev bi lahko ponovno upora- bili Kalmanov filter, ki bi se v svojih fazah zanaˇ sal na razliˇ cne pristope, od doloˇ cenega dinamiˇ cnega modela do izraˇ cuna na podlagi orientacij IME, ali celo razˇ siril kon- cept senzorne fuzije in uporabljal podatke kamere ali Op- totraka. V vsakem primeru v sistem vnesemo nekaj za- mika, hkrati pa dodaten UKF prinaˇ sa veˇ c zapletov in na- redi model precej zahtevnejˇ si. Druga ideja za kompenza- cijo tresljajev temelji na predpostavki, da se slednji naj- bolj izrazijo v ˇ casu mirovanja, medtem ko so med giba- njem manj problematiˇ cni. Predlagana reˇ sitev je uporaba sigmoidne funkcije S(x) = 1 1 +e x ; ki je razˇ sirjena s parametri za natanˇ cnejˇ se nastavljanje v S(x) = S max 1 +e s (x x0) ; S(x)2 (0; 1); katere namen je zagotoviti zvezen prehod med konstan- tno preslikavof(x) = 0 in linearno funkcijog(x) =x: f(x) +S(x) (g(x) f(x)) = x 1 +e s (x x0) : 109 Tako bo glede na velikost premika ta bolj ali manj zaduˇ sen. Uvedemo prag blaˇ zenja d, od katerega naprej ˇ zelimo, da je premik zaznan v celoti. Nastavimox 0 = d 2 ter strmino prehodas kot s = 2 log 1 Emax 1 d : S tem doseˇ zemo, da bo nezveznost na pragu enaka E max = S(0) = 1 S(1). Odloˇ camo torej med glad- kostjo prehodne funkcije in napako nezveznosti. Tako iz- peljemo enaˇ cbo, s katero zagotovimo stabilizacijo v ˇ casu mirovanja in glajenje tekom izvajanja premika: p 2 =p 1 + p e s(k pk d 2 ) + 1 : Primer take prenosne funkcije je funkcija s predpisom f(x) = x 1 +e 200(x 0:05) ; x2 (0;1); katere potek je prikazan na sliki 5. Slika 5: Graf funkcije f(x). Med pomembnimi lastnostmi robota je tudi naˇ cin in hitrost komunikacije, ki sta lahko veˇ cja povzroˇ citelja na- pak. Za zagotavljanje zveznega gibanja je namreˇ c klju- ˇ cnega pomena visoka frekvenca izmenjave podatkov. Za obravnavano konfiguracijo se je izkazalo, da se hitrost ko- munikacije obˇ cutno zmanjˇ sa, ko med vodenjem poteka tudi branje pozicije vrha robota. Na podlagi pridobljenih meritev je bil izraˇ cunan pri- bliˇ zen ˇ cas, ki preteˇ ce med poˇ siljanjem ˇ zelene pozicije, izvedbo premika ter branjem in posredovanjem nove po- zicije robota na raˇ cunalnik. Veˇ cino ˇ casa, ki znaˇ sa 0:45 s, se porabi za dejanski premik in branje nove pozicije vrha robota. Pri izraˇ cunu je bila uporabljena kriˇ zna korelacija med ˇ zeleno in dejansko trajektorijo robota, ki ju je bilo potrebno predhodno interpolirati, saj posamezne meritve niso bile enakomerno ˇ casovno vzorˇ cene. Robot UR5 bi bil s staliˇ sˇ ca hitrosti komunikacije primernejˇ si (Ethernet (TCP/IP) komunikacija s priˇ cakovano hitrostjo 500 Hz), vendar predstavlja druge omejitve pri vodenju, ki oteˇ zuje- jo aplikacijo. 4 Zakljuˇ cek Predstavljena metoda omogoˇ ca daljinsko vodenje robota z inercijskimi merilnimi enotami prek sledenja vodilni trajektoriji, ki jo opisuje dlan upravljalca. V odenje je re- alnoˇ casno in intuitivno, a hkrati nenatanˇ cno v primerjavi z diskretnimi alternativami. Omejitve opisanega postopka so povezane z zmoglji- vostjo robota (hitrost in naˇ cin komunikacije, hitrost pre- mikanja, konfiguracija sklepov), natanˇ cnostjo kalibracije IME, napakami zaradi tresljajev rok in ˇ suma v meritvah uporabljenih senzorjev, itd. V primerih, kjer je natanˇ cnost kritiˇ cnega pomena, je torej ˇ se vedno zaˇ zelena in potrebna natanˇ cnost eksplicitnega vnosa. Uporabnost metode je tako odvisna od aplikacije; primer bi bilo hitro uˇ cenje trajektorij, ki se v naknadnih procesih zgladijo in ure- dijo, kot sestavni del hibridnega vodenja ali zahtevnejˇ se realnoˇ casno upravljanje na daljavo [1]. Pristop je zlahka razˇ sirljiv in nosi potencial za razliˇ cne aplikacije. Med predlogi za nadaljnji razvoj je hkratna uporaba obeh dlani za boljˇ si nadzor nad poloˇ zajem in ro- tacijo robotskega vrha. Alternativno bi druga dlan obliko- vala kretnje nauˇ cenih ukazov, prepoznavne npr. z Dyna- mic time warping ali kompleksnejˇ simi algoritmi. Zahvala ˇ Studija je bila opravljena v sklopu raziskovalnega pro- grama ˇ st. P2-0228, ki ga je sofinancirala Javna agencija za raziskovalno dejavnost Republike Slovenije iz drˇ za- vnega proraˇ cuna. Literatura [1] D. Whitney, E. Rosen, D. Ullman, E. Phillips, S. Tellex. ROS reality: a virtual reality framework using consumer- grade hardware for ROS-enabled robots. IEEE/RSJ Inter- national Conference on Intelligent Robots and Systems (IROS), 2018. [2] M. Ortiz-Salazar, A. Rodriguez-Linan, L. M. Torres- Trevino, I. Lopez-Juarez. IMU-based trajectory generation and modelling of 6-DOF robot manipulators. International Conference of Mechatronics, Electronics and Automotive Engineering, 2015. [3] N. Miller, O. C. Jenkins, M. Kallmann, and M. J. Matric. Motion capture from inertial sensing for untethered huma- noid teleoperation, International Conference of Humanoid Robotics, pp. 547–565, 2004. [4] O. J. Woodman. An introduction to inertial navigation. Uni- versity of Cambridge, Computer Laboratory, Technical Re- port UCAM-CL-TR-696. Avgust, 2007. [5] H. Qiang, S.Zhang. Applications of IMU in Humanoid Ro- bot. Humanoid Robotics: A Reference. Springer Nether- lands, 2019. [6] K. Feng, J. Li, X. Zhang, C. Shen, Y . Bi, T. Zheng, J. Liu. A New Quaternion-Based Kalman Filter for Real-Time Attitude Estimation Using the Two-Step Geometrically- Intuitive Correction Algorithm. Sensors (Basel). Septem- ber, 2017. [7] S. O. H. Madgwick, A. J. L. Harrison, R. Vaidyanathan. Estimation of IMU and MARG orientation using a gradient descent algorithm. IEEE International Conference on Re- habilitation Robotics. ˇ Svica, 29. junij - 1. julij, 2011. [8] R. R. Labbe Jr. Kalman and Bayesian Filters in Python. Maj, 2018.