Scetfc Jourals Martme Uversty of Szczec Zeszyty Naukowe Akadema Morska w Szczece 1, 3(14) z. 1 pp. 75 8 1, 3(14) z. 1 s. 75 8 Costructo of a system scag the movemet of huma upper lmbs Kaml Stateczy West Pomeraa Uversty of Techology, Isttute of Mechacal Techology 7-31 Szczec, al. Pastów 19, e-mal: kaml.stateczy@zut.edu.pl Key words: vrtual realty, operator terface, CNC mache tool, upper lmb movemet scaer Abstract The paper presets a prototype of a ew put output devce whch s a costructed scaer of huma upper lmb movemet whch, together wth the software layer, creates a ew user terface of CNC (Computer Numercal Cotrol) mache tools. The costructed scaer cotas a sesory system based o accelerato sesors ad gyroscopes whch are dstrbuted wth the key areas o the huma upper lmb. O the bass of the sgals regstered durg the movemet of operator's upper lmbs, basg o the soluto of a smple kematcs model, t s possble to determe the locato ad oretato of operator s hads. A combato of the developed techology wth ready solutos of gloves scag movemets of fgers ad hads provdes a ew tutve put-output devce for the commucato wth the mache. Itroducto The developmet of the fuctoalty of advaced mechatroc systems eforces research o the ew commucato forms betwee the operator ad the mache. Sce maches are ot yet capable of commucatg drectly wth humas, they are equpped wth varous put-output devces costtutg the user terface. The paper presets a costructed system scag the operator s movemets. Ths system, together wth the software layer, creates the terface of a CNC mache tool operator. Ths terface, alog wth the use of vrtual realty, allows for the maual programmg of a mache tool (Fg. 1). Maual programmg s based o movg the body elemets of a mache tool maually by meas of the operator's upper lmbs ad rememberg dvdual postos, whch allows for automatc recostructo of movemets. The operator terface (Fg. ) cossts of put elemets such as oretato sesors ad a 3D Data Glove 4 Ultra o whch there are fve bed sesors oe sesor for each fger. A output elemet s a head-mouted dsplay (HMD) whch s a helmet wth dsplays mouted o the head. Owg to the fact that the HMD s equpped wth two separate A 3D model of real mache tool Output code wtch the algg movemets Fg. 1. Operator terface of a CNC mache tool dsplays capable of dsplayg dfferet mages for each eye, t s possble to obta the three-dmesoalty effect whe the dsplayed mages are trasposed approprately. The etrety s maaged by a custom applcato for a PC. It s resposble for the recept of data, data processg ad stereoscopc vsualsato. Zeszyty Naukowe 3(14) z. 1 75
Kaml Stateczy Iput elemets Oretato sesors Data Glove Mcrocotroller PC Recept of data (USB) Ma applcato module Applcato Vsualsato Output elemets HMD Fg.. Block dagram of the operator terface System scag the operator s movemets To map the movemets of the huma upper lmb the vrtual realty, t s ecessary to kow the oretato of the arm, the forearm ad the had, ad order to map the movemet more accurately, t s possble to addtoally take to accout the oretato of the shoulder ad the back. Oretato sesors There are umerous methods for determg the oretato of a object space, they have dfferet advatages but also dsadvatages, ad therefore, dfferet methods are appled varous felds of techology. Systems based o ertal sesors have the advatage over other systems the way that they are moble ad self-suffcet,.e. they do ot requre the deploymet of addtoal equpmet for measuremets, e.g. cameras ad lghtg whch s ecessary vso systems, trasmtters ad recevers as the case of systems usg frared, radar ad ultrasoc systems, as well as cols the case of magetc systems [1]. Thus, they are ot lmted by a specfc evromet, movemet or locato. I ths paper, author appled a techque usg the accelerato measuremets (cludg gravty) ad agular velocty measured by meas of a accelerometer ad gyroscope respectvely. These sesors made the MEMS techology (Mcro Electro-Mechacal System) are tegrated a complete ertal system ADIS1636. Its advatage s that t has a compesato of the put voltage mpact ad a compesato of the mpact of temperature for each of the bult- sesors []. It s factory-calbrated sestvty, bas ad axal algmet. A gyroscope measures the agular velocty, ad kowg the tal codtos, t s possble to tegrate the measured sgal wth the tme order to obta the oretato. However, a measuremet error leads to a error accumulato of the traslated oretato whch makes the determato of absolute oretato mpossble. A accelerometer measures the earth gravty a absolute system, but there are strog measuremet terfereces durg ts movemet. I order to calculate oretato o the bass of measuremets from the gyroscope ad the accelerometer, author used the oretato flter [3] for the IMU (Iertal Measuremet Ut) mplemetato, whose block dagram s preseted fgure 3. Fg. 3. Block dagram of the oretato flter for a IMU mplemetato [3] 76 Scetfc Jourals 3(14) z. 1
Costructo of a system scag the movemet of huma upper lmbs Ths flter s charactersed wth a low computatoal cost, whch eables ts mplemetato o cheap mcrocotrollers. It acheves a hgher level of accuracy comparso wth the Kalma flter appled for calculatg oretato [3]. The flter was mplemeted o the STM3F13 CBT6 mcrocotroller o the ZL3ARM structo set. Fgure 4 presets the accelerato sesors alog wth the platform o whch the flter s mplemeted. ADIS1636 sesors commucate wth the mcrocotroller by meas of a SPI terface (Seral Perpheral Iterface), sedg the accelerato ad agular velocty values ecessary for calculatg the oretato by meas of the flter. Oretato space s represeted by meas of a quatero. Data s trasmtted to the computer by meas of a USB terface. q s, v cos, s u where 1 u u q cos s u 1 (4) A quatero may be regarded as a represetato of rotato where the vector determed the drecto of axs rotato ad the agle determed the agle by whch the rotato has bee made (Fg. 5). v arccoss u (5) s u Fg. 5. Rotato by a agle aroud the vector u It s possble to traslate the quatero to the rotato matrx uambguously: 1 y z xy zw xz yw xy zw 1 x z yz xw xz yw yz xw 1 x y (6) Fg. 4. Sesors alog wth the computg platform A quatero may be treated as four umbers ad, smlarly to the vector, t dffers the way of performg some arthmetc operatos [4, 5]: w, x, y z w x jy kz q, (1) They are a exteso of complex umbers ad they may be wrtte usg three roots of 1 marked wth the followg symbols:, j, k. If squared, they equal 1: j k 1 () It s ofte represeted as a par of the umber s ad a vector v: where: s w, x, y, z v. q s,v (3) Ut quateros are quateros whose orm s equal to 1, ad they ca be represeted as follows: Determg the posto of segmets I the stadard approach, e.g. robotcs, jot coordates are measured to determe the locato of segmets (Fg. 6),.e. the cofgurato agles of segmets of the moto coecto, = 1... [6]. The, the rotato matrces ad homogeeous trasformato matrces are costructed: 1Θ 1 1Θ P 1 (7) 1 O ths bass, t s possble to determe the posto of segmets: 1... 1 (8) I the desged system scag the huma movemets, the MEMS measuremet system returs the quateros whch descrbe the oretato a global referece system (Fg. 7) Rotato matrces ad homogeeous trasformato matrces are costructed: Θ w, x, y, z (9) Zeszyty Naukowe 3(14) z. 1 77
Kaml Stateczy Segmet + 1 : Segmet : +1 z +1 y +1 P +1 z x +1 y x z y x Fg. 6. Determg the posto of segmets, stadard approach Segmet : Segmet + 1 : +1 z +1 u +1 u y +1 +1 z y P +1 x +1 x z y x Fg. 7. Determg the posto of segmets, modfed approach Θ 1 1 Θ Θ 1 k1 k k1 Θ T 1 Θ P 1 (1) 1 The rotato matrx determed o the bass of the quatero descrbes the oretato the global referece system, thus, there s a eed to traslate the rotato matrx to the matrx relato to the system of partcular segmets. The, as before, t s possble to determe the locato of segmets wth the use of a homogeeous matrx: 1 1 ( ) (11) Skeletal amato Havg aalysed the huma skeletal system, author costructed a smplfed operator model. Fgure 8 presets a smplfed dagram of the huma skeletal system. The etrety has bee mplemeted the costructed vrtual evromet usg the OpeGL graphcs lbrary. For amato of the operator the 3D space, author used a modfed popular method of skeletal amato whch boes are arraged herarchcally formg a tree (a boe has oe paret but t may have may chld boes). I ths way, the ext posto of the boe s determed o the bass of the posto relato to the paret boe, ad therefore, t s ot ecessary to determe the posto of boes the scee referece system. Sce ths method boes are terlked, a chage the boe oretato or boe 78 Scetfc Jourals 3(14) z. 1
Costructo of a system scag the movemet of huma upper lmbs shoulder arm forearm had boes backboe Fg. 8. Smplfed dagram of the huma skeletal system posto results the chage oretato ad posto of all chld boes of a gve boe. As metoed above, oretato sesors provde formato about the oretato of boes the global system, ad therefore, t was ecessary to modfy the gve method, so that the posto of the paret boe could have a fluece o the oretato of a chld boe ad o fluece o the chld boe oretato ( the case of the shoulder, arm ad forearm). Whereas, the bedg of fgers s Puttg the curret matrx o the stack Traslato to the begg of the boe Oretato settg (multplyg the curret matrx by the oretato matrx) Drawg of the boe Traslato to the ed of the boe measured the local system.e. relato to the prevous boe, ad therefore, the uchaged method of skeletal amato s used the. The algorthm s the modfed method of skeletal amato s preseted fgure 9. The sesors have bee deployed o the operator s body takg to cosderato the movemet system of the huma body muscles. Sce t s mpossble to posto the sesors o each boe ad determe ther oretato, author costructed a smplfed model of huma kematcs ad deployed sesors strategc locatos so as to uambguously determe the posto of hads space. It s worth otg that the wrtte system takes to accout the dffereces the structure of dfferet huma bodes ad eables eterg the legths of dvdual boes. I addto, t s possble to coduct perodc calbrato durg whch t s possble to determe the tal oretato of the sesors, as preseted fgure 1. Oretato relato to paret N Multplyg the curret matrx by the trasposed oretato matrx oretato sesors Y Does t have chld boes? N Recursve call Y Takg the curret matrx off the stack Fg. 9. Algorthm of modfed method of skeletal amato Fg. 1. Dstrbuto of the oretato systems Zeszyty Naukowe 3(14) z. 1 79
Kaml Stateczy Coclusos The costructed scaer of the huma upper lmb movemet ca successfully be used applcatos whch volve vrtual realty. The use of oretato sesors based o a MEMS gyroscope ad a accelerometer causes a stuato whch the devato value durg the work of the sesor s moved, thus requrg perodc calbrato of the sesors. Better results ca be acheved wth the use of sesors wth a magetometer alog wth the exteso flter for MARG (Magetc, Agular Rate, ad Gravty) mplemetato, descrbed the paper [3]. The costructed scaer of the huma upper lmb movemet costtutg the usage terface of a CNC mache tool the applcato wth a vrtual techology ad maual programmg ca facltate ad accelerate programmg. Ackowledgemets The research preseted ths paper s part faced by the project of the Polsh Mstry of Scece ad Hgher Educato No. NN 53 43138 ettled Developmet of the Project ad Expermetal Research o the Prototype of the System for Maual Programmg of CNC Mache Tools. Refereces 1. LIBERTY M.: Add Moto Sesg to Your Devce. Embedded Systems Coferece Slco Valley, 4.5.11.. http://www.aalog.com/e/mems-sesors/mems-ertalsesors/ads1636/products/product.html. 3. MADGWICK S., HARRISON A., VAIDYANATHAN R.: Estmato of IMU ad MARG oretato usg agradet descet algorthm. Iteratoal Coferece o Rehabltato Robotcs, 11. 4. MATULEWSKI J., DZIUBAK T., SYLWESTRZAK M., PŁOSZAJ- CZAK R.: Grafka. Fzyka. Metody umerycze. Symulacje fzycze z wzualzacją 3D. Wydawctwo Naukowe PWN, Warszawa 1. 5. DELOURA M.: Perełk programowaa ger. Vademecum profesjoalsty. Tom 1. Wydawctwo Helo,. 6. CRAIG J.: Wprowadzee do robotyk. Mechaka sterowae. Wydawctwo Naukowo-Techcze, Warszawa 1993. 8 Scetfc Jourals 3(14) z. 1