Sensor Data Fusion Using Fuzzy Control for VOR-Based Vision Tracking System

Transcription

1 The 200 IEEE/RSJ Internatonal Conference on Intellgent Robots and Systems October 8-22, 200, Tape, Tawan Sensor Data Fuson Usng Fuzzy Control for VOR-Based Vson Tracng System Hyun-l Kwon, Jaehong Par, Wonsang Hwang, Jong-hyeon Km, Chang-hun Lee, M. Latf Anjum, Kwang-soo Km, and Dong-l Dan Cho, Member, IEEE Abstract Ths paper presents a vson tracng system to acheve hgh recognton performance under dynamc crcumstances, usng a fuzzy logc controller. The man concept of the proposed system s based on the vestbulo-ocular reflex (VOR) and the opto-netc reflex (OKR) of the human eye. To realze the VOR concept, MEMS nertal sensors and encoders are used for robot moton detecton. Ths concept turns the camera towards a selected target, counteractng the robot moton. Based on the OKR concept, the targetng errors are perodcally compensated, usng vson nformaton. The fuzzy logc controller uses sensor data fuson to detect slp or collson occurrences. To calculate a headng angle of the camera accurately, the output of the fuzzy logc controller and the vson nformaton from the camera are combned, usng an extended Kalman flter. The proposed vson tracng system s mplemented n a moble robot and evaluated expermentally. The expermental results are obtaned as the tracng and the recognton success rate usng a moble robot. The developed system acheved the excellent tracng and recognton performance durng slp or collson occurrences under dynamc crcumstances. I. INTRODUCTION HE last decade has seen changng trends n the robot Tndustry rangng from ndustral robots to mltary, servce and entertanment robots. Beng utlzed n a wder feld, the robots requre advanced sensors and control technologes to mplement varous functons. Indoor moble robots, such as cleanng robots or servce robots, requre precse navgaton systems and vson systems to perform varous tass. Manuscrpt receved March 0, 200. (Wrte the date on whch you submtted your paper for revew.) Ths wor was supported partly by the R&D program of the Korea Mnstry of Knowledge and Economy (MKE) and the Korea Evaluaton Insttute of Industral Technology (KEIT). [2008-F-037-0, Development of HRI Solutons and Core Chpsets for u-robot] Hyun-l Kwon, Jaehong Par, Wonsang Hwang, Jong-hyeon Km, and Chang-hun Lee are wth Automaton and Systems Research Insttute, Inter-unversty Semconductor Research Center, and School of Electrcal Engneerng and Computer Scence, Seoul Natonal Unversty, Kwana-u, Seoul, 5-742, Korea (e-mal: hwon@snu.ac.r, jaehong@snu.ac.r, wshwang@snu.ac.r, jhm84@snu.ac.r, chlee84@snu.ac.r) Kwangsoo Km s wth Department of Control and Instrumentaton Engneerng, Hanbat Natonal Unversty, Yuseong-gu, Deajeon, , Korea ( e-mal: sm@hanbat.ac.r) Dong-l Dan Cho s wth Automaton and Systems Research Insttute, Inter-unversty Semconductor Research Center, and School of Electrcal Engneerng and Computer Scence, Seoul Natonal Unversty, Kwana-u, Seoul, 5-742, Korea (correspondng author; phone: ; fax: ; e-mal: dcho@ snu.ac.r). There are two poston estmaton methods appled n navgaton systems, absolute and relatve localzaton. The relatve localzaton s realzed through measurements provded by sensors measurng nternal varables of the vehcle. The ncremental encoders are the typcal nternal sensors. These sensors are placed on the wheel axs whch represents the rotaton axs of the vehcle. The dsadvantage of ths method s that errors of each measurement are accumulated. Ths heavly degrades the estmaton of the poston and the orentaton of the vehcle, especally for long and wndng trajectores []. However, the absolute localzaton s based on the sensors measurng parameters of the envronment n whch the robot s operatng. These sensors, whch are sonar and Infrared sensors, are also wdely utlzed for the gudance of autonomous vehcles wth obstacle avodance n unnown envronment [2-3]. The major dsadvantage of the absolute localzaton s ther dependence on the envronmental parameters [4]. The vson nformaton s mportant for object recognton. In the vson tracng area, especally, the vson nformaton from fxed observers s only used to trac a target. There are algorthms to analyze the vson nformaton n the vson tracng. The smallest unvalue segment assmlatng nucleus (SUSAN) and features from accelerated segment test (FAST) algorthms recognze objects by feature pont extractors, whle the normalzed cross-correlaton (NCC), scale nvarant feature transform (SIFT), and speeded up robust feature (SURF) algorthms recognze objects by smlarty of feature ponts. These algorthms strongly recognze varous objects, but f there are obstacles n front of an object, the vson tracng systems fal to trac the target. Also, n order to handle the more mage nformaton, Also, n order to handle more mage nformaton, the system wth hgher performance s necessary [5-9]. The VOR s a reflex of an eye movement that stablzes mage on the retna durng head movement by producng an eye movement n the opposte drecton to head movement, thus preservng the mage on the center of the retna. The OKR allows an eye to follow objects n moton when the head remans statonary. The VOR concept s mplemented usng mcro-electro-mechancal systems (MEMS) nertal sensors and moble robot encoders, whle the OKR concept s mplemented by usng vson nformaton from a camera whch s mounted on the moble robot /0/$ IEEE 2920

2 Recently, the fuzzy control system s extensvely used for mult-sensor data fuson for navgaton systems. The FLEXnav system uses data from fber optc gyroscopes, Corols gyroscopes and accelerometers [0]. The fuzzy expert system uses data from accelerometers, gyroscopes and dgtal compass to estmate robot atttude []. The fuzzy logc adaptve Kalman flter (FLAKF) and the fuzzy modelng usng a global postonng system (GPS) and MEMS nertal measurement unt (IMU) are proposed to obtan locaton nformaton of robots [2-3]. Ths paper proposes a VOR-based vson tracng system usng the fuzzy control. The proposed system s appled wth the navgaton system and the vson-based tracng system. The moble robot navgaton system s derved from the vestbulo-ocular reflex (VOR), and the vson-based tracng system s derved from the opto-netc reflex (OKR) of the human eye. And ths paper also ntroduces a sensor fuson method for slp detecton usng the fuzzy logc. The man purpose of our system s to eep the range of the sght fxed to the target, even when there are slp or collson occurrences durng robot moton. The proposed system s developed, and evaluated to show the stable and the robust tracng and recognton performance durng slp or collson occurrences under dynamc crcumstances. Fg.. VOR-based vson tracng system II. SYSTEM CONFIGURATION The proposed VOR-based vson tracng system s shown n Fg.. It conssts of a moble robot, an artfcal vestbular system (AVS) sensor cluster such as the artfcal vestbular organ to sense lnear and rotaton moton of the robot, a camera for vson nformaton, an actuator module to rotate the camera, and a host PC to run algorthms locatng a target n the center of the mage plane of the camera. It also descrbes how each part of the system communcates wth the host PC. The robot moves n accordance wth the receved commands from the host PC through the USB nterface. The moble robot s encoder and the MEMS accelerometer and the gyroscope sgnals represent the moton nformaton of the moble robot, and are sent to the host PC by RS232 communcaton. The target poston nformaton at the mage plane obtaned by the camera s transferred to the host PC through the USB nterface. The relatve poston of the robot and the camera error angle are calculated by the AVS sensor cluster sgnals and vson nformaton of the camera n the host PC. The host PC transfers the control sgnals to the actuator module to locate the target at the mage plane of the camera by RS232 communcaton. The moble robot (Moble Robot, Customer & Robot Co., LTD.) for the VOR-based vson tracng system s a commonly used platform. The moble robot has two wheels wth encoders. The AVS sensor cluster conssts of an accelerometer (xps5-357, Konx, Inc), a gyroscope (ADIS6255, Analog Devces, Inc.), a mcrocontroller (Atmega62) for sgnal processng, and data communcaton chp (MAXIM233) for RS232 communcaton. In addton, a Fg. 2. Bloc dagram of VOR-based vson tracng system sngle camera (SPC 520NC, PHILPS) s attached to a DC motor (Seres 269, McroMo Electroncs, Inc.) on the center of the robot. Fnally, A Wndows based vsual C program s developed to realze the VOR-based vson tracng system. III. VOR-BASED VISION TRACKING SYSTEM A. VOR-Based Vson Tracng Prncple The bloc dagram of the proposed system s shown n Fg. 2. It s derved from the VOR and OKR of the human eye reflex. The developed system receve the feedforward data of the robot moton nformaton from the AVS sensor cluster, and the vson nformaton of the camera, whch s the dfferental pxel nformaton between the center of the mage plane and the center of the target mage, s used as a feedbac data to rotate the camera. Based on the VOR and OKR concepts, the DC motor s used to rotate the camera to locate the target n the center of the mage plane. B. Camera Headng Angle Usng Robot Moton In the ndoor navgaton system, a camera headng angle for locatng a target at the center of an mage plane s calculated on the bass of robot moton nformaton. The moton and locaton nformaton of the moble robot are depcted usng the Cartesan coordnate system, whch s shown n Fg. 3. The symbols used are presented below. 292

3 Δ d = d d (5) Δ d e, e, e, a, : Movng dstance calculated by accelerometer durng Δ t Δ d : Movng dstance calculated by encoder durng Δ t e, OXY : Global coordnate system O X Y : Reference coordnate system of - step OXY: Reference coordnate system of step P : Robot poston of - step w.r.t OXY P : Robot poston of step w.r.t OXY T : Target poston w.r.t OXY r : Headng vector of - step w.r.t O XY r : Headng vector of step w.r.t OXY Δ d : Movng dstance of the robot durng Δ t The vector r s cosθ snθ r = ( ro P) snθ cosθ Fg. 3. Cartesan coordnate system for vson tracng system P = P +Δ d (2) The camera headng angle r to locate the target at the center of the mage plane s calculated from the vector r. () The movng angle Δ θ s calculated usng the gyroscope and encoder data. The movng angle Δ θ s gven below. Δ θ = ( Ω + ω ) Δ (6) g, g, g, t RINC LINC Δ θe, = (7) D Δ θ g, : Rotatng angle durng Δt calculated from gyroscope data Δ θ : Rotatng angle durng Δt calculated from encoder e, data RINC: Rght wheel ncrement LINC: Left wheel ncrement D: Dstance between rght wheel and left wheel C. Camera Headng Angle Usng Camera Image The camera headng angle to locate the target mage at the center of the mage plane s calculated usng the vson nformaton of the camera. The SURF algorthm s used to recognze the target, and the camera angle error s calculated usng the pn-hole model. The angle error between the center of the mage plane of the camera and the center of the target n mage plane s used as a feedbac sgnal for the controller as shown n Fg. 2. The angle error s shown n Eq. (8), and pn-hole model s shown n Fg. 4. ε The dstance from mage center to target center = tan focal length (8) ( X p x, ) sn θ + ( Y py, )cosθ r = tan ( X px, )cos θ ( Y py, )snθ + (3) The movng dstance Δd s calculated by usng the accelerometer and encoder data, respectvely. The moble robot movng dstance from the ponto to O durng Δt s gven below. 2 2 Δ da, = va, Δ t+ aa, Δ t (4) Fg. 4. Pn-hole model to calculate camera headng angle error 2922

4 IV. SENSOR FUSION METHOD A. Sensor Fuson Concept The bloc dagram for sensor data fuson, whch s the controller part of the proposed system, s shown n Fg. 7. In the controller part, the encoder sgnal (33 Hz) and the sensor sgnal (69 Hz) are synchronzed wth the encoder cycle, and are used to calculate the movng dstance and angle of the moble robot, whch serve as the fuzzy logc controller nput at the encoder cycle. The movng dstance, calculated from the encoders and accelerometer usng Eq. (4) and (5), s combned usng the fuzzy logc rulebase for the movng dstance. The movng angle, calculated from the encoders and gyroscope usng Eq. (6) and (7), s also combned usng the fuzzy logc rulebase for the movng angle. Each of the rulebase s obtaned from the experments of slp and collson occurrences. The output from the fuzzy logc controller s weghted value for the encoders and the nertal sensors. Weghted sum, the state nput for the extended Kalman flter, s gven below. weghted sum Δ d = weghted value Δ denc + (weghted value) Δ d (9) weghted sum Δ θ = weghted value Δ θenc + ( weghted value) Δ θ (0) The camera angle error between the center of the mage and the center of target n the mage plane s used as measurement update. Fnally, the camera headng angle to locate the target n the center of the mage plane s obtaned by the output of the extended Kalman flter. B. Fuzzy Slp Detecton Modelng The moble robot movng n ndoor envronments s placed n dynamc crcumstances and undergoes slp or collson occurrences. In these envronments, f only encoder gyro acc Fg. 7. Bloc dagram of controller part for sensor fuson nformaton s used, the locaton nformaton of moble robots s not correctly obtaned. When nertal sensors are used over long term duraton, t causes accumulatve errors by bas drft and two-step ntegraton. The proposed soluton for ths problem s the fuzzy slp detecton model. The output of the fuzzy slp detecton model s weghted sum of the nertal sensor and encoder data whch s used as nput of the extended Kalman flter. The equaton (4) and (5) show the movng dstance, d, of the moble robot from the encoder and accelerometerr data durng the encoder cycle t, respectvely. The equaton (6) and (7) are the movng headng angle, θ, of the moble robot from the encoder and gyroscope data durng the encoder cycle t, respectvely. Each of the rulebase has been obtaned from experments. Once the nputs and the outputs are dentfed and defned, the relatonshp between them must be establshed. The Mamdan algorthm s used for the fuzzy nference system. The general "f-then" rule structure of the Mamdan algorthm s gven n the followng equaton: R : IF x s A and... THEN y s B (for =, 2,..) () where s the number of rules, x s the nput varable Fg. 8. d Membershp functon Fg. 5. Fuzzy logc controller for sensor fuson Fg. 9. θ Membershp functon (a) d Rulebase (a) θ Rulebase NB: negatve bg, AO: accelerometer only GO: gyroscope only NM: negatve medum AM: accelerometer medum GM: gyroscope medum Z: zero AE: both GE: both PM: postve medum EM: encoder medum, EM: encoder medum PB: postve bg EO: encoder only EO: encoder only Fg. 6. The desgned fuzzy logc rulebase Fg. 0. d, θ weghted value Membershp functon 2923

5 (antecedent varable) and y s the output varable (consequent varable). For the fuzzy logc controller, we translated our nowledge base nto the fuzzy logc rulebase shown n Fg. 6. The membershp functons used as the nput and the output of the system are shown n Fg. 8, 9 and 0. C. Sensor Fuson Usng Extended Kalman Flter In ths secton the headng angle of the camera on the moble robot s estmated by nsertng the output of the fuzzy logc controller n the nput of the dscrete tme extended Kalman flter. The measurement of the actual headng angle of the camera s obtaned by addng the angle from the DC motor encoder and the error angle from the mage processng of the feedbac sgnal. The state model predcts the coordnates of the state state x. x( ) = p ( ) p ( ) θ ( ) T, ~(0, Q) x p ( ) x : Moble robot x-coordnate p ( ): Moble robot y-coordnate y θ ( ) : Moble robot headng angle y ω (2) The state x s nduced from the nput u whch conssts of Δd and Δ θ. The nput u s the output of the weghted sum the fuzzy logc controller. [ ] T u ( ) = Δd Δd Δ θ (3) px( ) = px( ) + ( Δd( ) + ω( ))cos( θ( )) (4) py( ) = py( ) + ( Δd( ) + ω( ))sn( θ( )) (5) θ( ) = θ( ) + ( Δθ( ) + ω( )) (6) The moble robot s x-coordnate, y-coordnate and headng angle are shown n Eq. (4), (5) and (6). It s updated every 0.03 s (33 Hz), whch s the wheel encoder cycle. The measurement model updates the coordnates of the state vector x. The measurement of the actual headng angle, z, of the camera s obtaned by Eq. (3) wth v ~(0, R ). The partal dervatve matrx s obtaned from Eq. (4), (5), (6), and (3). L cosθ 0 0 f = = 0 snθ ω + (8) h ( Y S ) y ( X S ) x (9) H = = x ( X S ) ( ) ( ) ( ) x + Y Sy X Sx + Y Sy h M = = [] (20) v Fg.. Operaton of the extended Kalman flter Fg. shows the operaton of the extended Kalman flter. The flter estmates the process state x at tme and estmate the covarance P of the error n the estmate. The flter then obtans the feedbac from the measurement. Usng the Kalman flter gan K and the measurement z, t updates the state x and the error covarance P. Ths process s repeated as new measurements come n, and the error n estmaton s contnuously reduced [4]. V. EXPERIMENTAL RESULTS To evaluate the proposed vson tracng system, the expermental envronment was setup as shown n Fg. 2. The drvng velocty and the rotaton velocty of the moble robot mounted on our system are 0.2 m/s and 30 deg/s, respectvely. The ntal poston of the moble robot s.8 m away from the target. In the recognton test, the SURF based recognton F 0 Δ ( d + ω )snθ 0 0 f = = 0 ( Δ d + ω )cosθ x + (7) Fg. 2. Expermental envronment 2924

6 Robot trajectory TABLE I EXPERIMENTAL RESULTS Tracng success rate Recognton rate Translatonal moton (±0.4 m) 00 % 98.5 % Rotatonal moton (±90 deg) 00 % 96.7 % Slp crcumstance (Proposed system) 00 % 9.2 % Slp crcumstance 73.6 % 73.6 % (Encoder) Tracng success rate (# of successful tracng / # of mage frame) Recognton rate (# of successful recognton / # of mage frame) program s used. In the case of accurate tracng and recognton, the target appears n the mage of the camera, and a red square s formed on the edge of the target. To verfy the basc performance of the system, we expermented wth lnear and rotatonal moton. For the lnear moton, the robot move forward by 0.4 m, bacward by 0.8 m, and forward by 0.4 m to ntal poston. For the rotatonal moton, the robot rotates clocwse by 90 deg, antclocwse by 80 deg, and fnally clocwse by 90 deg to come to ntal poston. The tracng success rate s 00 %, and the recognton rate s above 95 % for the both moton scenaros. Due to the blurrng effect, the target n some mages cannot be recognzed successfully. Snce the man purpose of our system s to eep the range of the sght fxed to the target under the dynamc crcumstances, we generated the ntentonal collson and slp by eepng an obstacle near the robot as shown n Fg. 2. The obstacle s placed 0. m away from the robot n the forward drecton. The moble robot moves forward by 0. m, colldes wth the obstacle, and suffers slp whle the robot eep movng by 0.4 m. After that the moble robot moves bacward by 0.5 m, and then forward by 0.4 m to ntal poston. In the slp test usng our system, the tracng success rate s 00 % and the recognton rate s above 90 %. When compared wth the slp test usng the encoder only, the tracng success rate and recognton rate of our system are remarably hgh. The evaluaton results, the tracng success rate and recognze rate, are shown n Table. logc controller and the vson nformaton from the camera. The proposed system s developed, and expermented by mountng t on the top of the ndoor moble robot. The experment results show the stable and the robust tracng and recognton performance durng slp or collson occurrences under the dynamc crcumstances. REFERENCES [] Thomas D. L., Optmal Fuson of Sensors, Department of Automaton Techncal Unversty of Denmar, PHDEssay, pp. -9, 998. [2] Green D.N., Sasade J.Z., and Vuovch G.S., Path Tracng Obstacle Avodance and Deal Reconng by an Autonomous Planetary Rover. In: Proc. IEEE Internatonal Conference on Robotcs and Automaton, San Dego, CA, pp , 994. [3] Carell R., Santos-Vctor J., Robert F., Tosett S., Drect Vsual Tracng Control of Remote Cellular Robots. Robotcs and Autonomous Systems Journal, vol. 54, pp , July, [4] Mohamed Jalloul, Lobna Amour, Nabl Derbel, An Effectve Localzaton Method for Robot Navgaton through Combned Encoders Postonng and Retmng Vsual Control. Journal of Automaton, Moble Robotcs & Intellgent Systems, vol. 3, pp., Nov [5] C. Harrs and M. Brady, A Combned Corner and Edge Detector. Proc of the 4th Alvey Vson Conference, pp. 47-5, 988. [6] S. M Smth and J. M Brady, SUSAN A New Approach to Low Level Image Processng, Defense Research Agency, Techncal Report no. TR95SMS, Farnborough, England, 994. [7] E. Rosten and T. Drummond, Machne Learnng for Hgh-Speed Corner Detecton. Proc, of the 9th European Conf. Computer Vson, pp , [8] J. P. Lews, Fast Normalzed Cross-Correlaton. Vson Interface, pp , 995. [9] D. G Lowe, Object Recognton from Local Scale-Invarant Features. Proc. Of the Internatonal Conf. Computer Vson, pp , 999. [0] Lauro Ojeda and Johann Borensten. FLEXnav: Fuzzy Logc Expert Rule-based Poston Estmaton for Moble Robots on Rugged Terran. Proc. of the 2002 IEEE Internatonal Conference on Robotcs and Automaton, pp , May, [] Wang J-H and Gao Y. Fuzzy Logc Expert Rule-Based Mult-Sensor Data Fuson for Land Vehcle Atttude Estmaton. Proc. 9th Int. Commttee on Data for Scence and Technology (CODATA) Conf., Nov, [2] J.Z. Sasade, Q. Wang and M.B. Zeremba. Fuzzy Adaptve Kalman Flterng for INS/GPS Data Fuson. Proc. of the 5th IEEE Internatonal Symposum on Intellgent Control (ISIC 2000), July, [3] W. Abdel-Hamd, T. Abdelazm, N. El-Shemy, and G. Lachapelle. Improvement of MEMS-IMU/GPS Performance Usng Fuzzy Modelng. The Journal of Global Navgaton Satellte Systems, 2006 [4] V. Subramanan, T. F. Burs, W. E. Dxon. Sensor Fuson Usng Fuzzy Logc Enhanced Kalman Flter for Autonomous Vehcle Gudance n Ctrus Groves. Amercan Socety of Agrcultural and Bologcal Engneers, 2009 VI. CONCLUSIONS In ths paper, we developed the VOR and OKR based vson tracng system for the moble robot usng the robot moton nformaton and vson nformaton. The man purpose of our system s to eep the range of the sght fxed to the target, even when there are slp or collson occurrences durng robot moton. In the control scheme, the fuzzy logc controller s used n detectng slp or collson occurrences, and uses sensor data fuson. To calculate the rotaton angle of the camera accurately, the extended Kalman flter estmates the robot moton nformaton usng the output of the fuzzy 2925