Joural o Mechaical Sciece ad echology 25 (11) (2011) 2907~2917 www.sprigerli.com/cotet/1738-494x DOI 10.1007/s12206-011-0805-1 A dead recoig localizatio system or mobile robots usig iertial sesors ad wheel revolutio ecodig Bog-Su Cho Woo-sug Moo Woo-Ji Seo ad Kwag-Ryul Bae * Departmet o lectroic gieerig Pusa atioal Uiversity Kumjeog-Gu Busa Korea (Mauscript Received May 9 2011; Revised Jue 27 2011; Accepted July 13 2011) ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- Abstract Iertial avigatio systems (IS) are composed o iertial sesors such as accelerometers ad gyroscopes. A IS updates its orietatio ad positio automatically; it has a acceptable stability over the short term however this stability deteriorates over time. Odometry used to estimate the positio o a mobile robot employs ecoders attached to the robot s wheels. However errors occur caused by the itegrative ature o the rotatig speed ad the slippage betwee the wheel ad the groud. I this paper we discuss mobile robot positio estimatio without usig exteral sigals i idoor eviromets. I order to achieve optimal solutios a Kalma ilter that estimates the orietatio ad velocity o mobile robots has bee desiged. he proposed system combies IS ad odometry ad delivers more accurate positio iormatio tha stadaloe odometry. Keywords: Positioig; Localizatio; Dead recoig; IS; Iertial sesor; Compass sesor; Accelerometer; Gyroscope; Odometry; coder; Mobile robot; Kalma ilter; Orietatio compesatio ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- 1. Itroductio his paper was recommeded or publicatio i revised orm by ditor Keum Shi Hog * Correspodig author. el.: +82 051 510 2460 Fax.: +82 051 515 5190 -mail address: rbae@pusa.ac.r KSM & Spriger 2011 A mobile robot is a automatic machie that avigates i a give eviromet ad recogizes its surroudigs usig may sesors. he study o mobile robots has developed rapidly i various ields such as housewor support elder assistace educatio medical care atioal deese etc. he localizatio o a mobile robot i order to achieve autoomous movemet is a importat techique ad is curretly a importat research ield. here are two geeral methods used or localizatio: relative positioig ad absolute positioig [1]. Relative positioig also ow as dead recoig evaluates the positio o the mobile robot by usig its velocity ad yaw agles measured by ecoders attached to the robot s wheels or by iertial sesors [1-8]. Absolute positioig evaluates the positio o the mobile robot by usig exteral distace measurig systems. Dead recoig estimates a relative positio rom the iitial startig poit iormatio. Geerally it uses a iertial measuremet uit (IMU) or a cotrol variable such as a ecoder ad so does ot deped o exteral sigals [1-12]. hereore dead recoig has advatages i beig simple low cost ad has a easier time i estimatig the positio i real time compared to absolute positioig. I order to measure the positio the absolute positioig method uses the global positioig system (GPS) ultrasoic local positioig systems irared etwor systems radio requecy idetiicatio (RFID) systems etc. [1 4]. GPS caot be used idoors ad has a slow update rate [1 13]. he ultrasoic local positioig systems ad the irared etwor systems are low cost small ad are easy to iterace [1 12-17]. However these methods caot measure ar distaces require additioal istallatio ad have a diiculty i accuracy due to sigal itererece. RFID requires additioal equipmet ad has a high cost. hese absolute positioig methods have a advatage i that they do ot accumulate positioig errors but the overall positioig errors are relatively big. I this paper we discuss the positio estimatio o mobile robots i a idoor eviromet such as a buildig or a actory. We assume that the mobile robot travels o a lat mostly level surace. Whe the mobile robot travels the positio ad the yaw agle o the mobile robot ca be estimated by ecoders attached to the robot s wheels. his method is ow as odometry [1-8]. he ecoders measure the wheel s agular rate o chage. he positio ad yaw agle o the mobile robot are calculated by the rotary agle ad the diameter o the wheel ad the body width o the mobile robot. Whe usig odometry ubouded errors occur due to slippage mismatches i the system parameters measuremet iaccuracies ad oise rom the ecoder sigals [1-8]. Although these er-
2908 B.-S. Cho et al. / Joural o Mechaical Sciece ad echology 25 (11) (2011) 2907~2917 rors are relatively small they do accumulate caused by positio ad yaw agle errors over the log ru. he icliatio o the groud is also a source or errors. hese errors ca be reduced by usig attitude compesatio. A IS updates the orietatio ad positio iormatio automatically without exteral sigals. he IS which cosists o a IMU ad a avigatio computer provides the positio ad orietatio o the mobile robot at a high rate typically 100 times per secod [9-11]. I the mobile robot's iitial iormatio is ow the positio ad orietatio are determied by the itegratio o the acceleratios ad the agular rates [9-11]. However low requecy oise ad sesor es are ampliied due to the itegrative ature o the system [9 18-20]. hat is IS oers a good short term stability but has a poor log term stability. hereore a IS without additioal exteral sigals has ubouded positio ad orietatio errors. By themselves stadaloe odometry ad IS are ot suitable or dead recoig or over log periods o time due to the accumulatio errors. I this paper we combie the odometry ad the IS i order to reduce the accumulatio errors oud i the dead recoig. Although both IS ad odometry have accumulatio errors the itegratio o these two systems will reduce these errors to a acceptable level. I order to achieve a optimal itegrated system a Kalma ilter will be desiged ad used [21]. he mai idea is same as ollowigs: he positio o the mobile robot is estimated by the velocity ad orietatio that are calculated by tri-axial accelerometers ad tri-axial gyroscopes or every 20 Hz period (50 ms). I order to estimate the orietatio or every 4 Hz period (250 ms) the roll ad the pitch are updated usig accelerometer the yaw agle rate is updated usig odometer ad the yaw agle is updated usig the magetic sesor. I order to estimate the positio the odometer reies the velocity measuremet. he aim o this paper is to exted the period o travellig without eedig ay exteral positio data rom a absolute positioig system. 2. he avigatio system he purpose o the proposed system that combies the IS ad odometry is to estimate the orietatio ad positio o the mobile robot with oly a small amout o errors usig the iormatio rom two systems that have dieret eatures. his sectio presets the udametal equatios o the avigatio system ad describes the proposed positioig algorithm. 2.1 he iertial avigatio system he IS cosists o a IMU ad a avigatio computer. he IMU is a assembly o the iertial sesors which iclude tri-axial accelerometers tri-axial gyroscopes ad at least dual axial magetic sesors [9-11]. he avigatio computer operates usig a algorithm that estimates the orietatio velocity ad positio. he tri-axial accelerometers measure the absolute three dimesio acceleratio with respect to the body rame. he icliatio o the mobile robot is evaluated with three orthogoal accelerometers because the tri-axial accelerometers provide very accurate iormatio whe the mobile robot is statioary. I additio sigle ad double itegratio o the acceleratios provide the velocity ad positio respectively. he three orthogoal gyroscopes provide the agular rates regardig the three axes. he itegratio o the agular rates provides the orietatio o the mobile robot. I additio the itegratio o the tri-axial gyroscopes provides the orietatio ot oly whe the mobile robot is statioary but also whe it is movig. 2.1.1 he iitial aligmet algorithm Dead recoig calculates a relative positio rom the iitial positio iormatio ad the iitial orietatio iormatio o the mobile robot. he iitial aligmet process is used to calculate the iitial orietatio rom the outputs o the iertial sesors whe the mobile robot is statioary. We ca assume that the startig poit is the origi. he iitial orietatio ca be calculated with three orthogoal accelerometers ad dual magetic sesors. he acceleratio ( b ) o the body rame is measured at a complete stadstill i the ollowig maer: 0 siθ x g b b b = = = 0 y C C = g cosθsiφ (1) cosθcosφ z g g b where C is the trasormatio matrix rom the avigatio rame to the body rame. is the acceleratio o the avigatio rame ad g is the gravity. φ ad θ deote the roll agle ad the pitch agle respectively. From q. (1) the roll agle ad pitch agle become: 1 g cosθsiφ 1 y φ = ta = ta (2) g cosθcosφ z 1 g siθ 1 θ = ta = ta g cosθ x 2 2 y + z. (3) he yaw agle (ϕ ) may be obtaied rom the earth rotatio agular velocity whe statioary. However it is quite diicult to measure the earth s rotatio agular velocity because it demads gyroscopes with very high resolutios i order to measure this very small agular acceleratio value. he magetic sesor measures the magitude o the earth's magetic ield usig a mageto-resistive elemet. he compass sesor is composed o two magetic sesors with a orthogoal orietatio that sese the horizotal compoets o the earth s magetic ield. he compass sesor ca thereore measure the yaw agle. he dual axial magetic ields ( Hex H ey ) that are measured by the dual magetic sesors provide the magetic orth agle (α ) i the ollowig maer:
B.-S. Cho et al. / Joural o Mechaical Sciece ad echology 25 (11) (2011) 2907~2917 2909 Fig. 1. he earth magetic ield vector. 1 α ta Hey =. (4) Hex here is a dierece betwee true orth ad magetic orth. his agular dierece is ow as the decliatio agle ( λ ). he decliatio varies rom 0 to 30 i most populated regios o the world. hese decliatio values chage slightly over time as the earth s tectoic plate shit. hereore the actual decliatio value is dieret i each locatio. he yaw agle is a right-haded rotatio about true orth. hereore the yaw agle is determied by: λ = ϕ α ϕ = λ + α. (5) Fig. 1 depicts the earth magetic ield vector ad the relatioship betwee magetic orth ad true orth. 2.1.2 he calibratio algorithm he iitial aligmet process determies the iitial orietatio o the mobile robot. he iitial aligmet is very importat because the dead recoig algorithm uses this iitial orietatio i order to update its attitude ad positio [1 9-11]. he accelerometers ad gyroscopes have errors because o sesor misaligmet sesitivity ad oset. I iitial aligmet the orietatio is calculated by the tri-accelerometers. Whe the mobile robot is movig the orietatio is calculated by the itegratios o the agular rates that are measured by the tri-gyroscopes. Also the velocity ad positio are calculated by sigle ad double itegratios o the acceleratios respectively. hereore the es o the accelerometer ad gyroscope will icrease the errors o the orietatio ad the positio. Hece the outputs o accelerometer ad gyroscope must be calibrated i applicatios that require high accuracy such as a mobile robot. I priciple the icliatio o the mobile robot platorm is determied rom the tri-accelerometers however the uow es o the tri-accelerometers aect the accurate measuremet o the icliatio agle. I order to reduce the uow es o the tri-accelerometers we will use dual-axial icliometers. wo orthogoally mouted icliometers measure the small deviatios o the mobile robot platorm at up to ± 30 rom the horizotal plae (x-y plae). he icliatio iormatio that is provided by the dual-axial icliometers is used to cacel the es o each axis o the accelerometer. Uortuately this icliatio iormatio is oly useul whe the mobile robot is statioary sice icliometers are iheretly sesitive to acceleratio. hereore the accelerometer es must be determied beore attemptig to estimate the orietatio velocity ad positio while the mobile robot is movig. Whe the mobile robot is statioary a algorithm that compesates the es o the tri-accelerometers is the ollowig. he compass sesor measures the yaw agle ad the dualaxial icliometers measure the roll ad pitch agle. We will assume that these agles (yaw roll ad pitch) are the real orietatio o the mobile robot. A trasormatio matrix ( C ) that shows the relatioship betwee the body rame ad the avigatio rame becomes q. (6) by usig these agles. cϕ cθ cϕ sθ sφ sϕcφ cϕsθ cφ + sϕsφ C sϕ cθ sϕ sθ sφ cϕcφ sϕsθ cφ cϕsφ b = + (6) sθ cθsφ cθcφ where c= cos ad s = si. he relatioship betwee the acceleratio i the body rame ad the acceleratio i the avigatio rame is: b b = C. (7) Sice the mobile robot is statioary the acceleratios oud i the avigatio rame are oly rom the gravity vector. hat is the acceleratios i the orth ad east directio are zero. he acceleratios that are trasormed with the avigatio rame become the accelerometer es excludig the gravity vector. I order to compesate or the es the outputs o the compass sesor dual-axial icliometers ad the tri-axial accelerometers are sampled or 10 secods; the sampled data are the averaged ad are applied i the ollowig algorithm. measure = = measure (8) D g + measure D measure = = (9) measure D + D g measure ˆ = (10) b
2910 B.-S. Cho et al. / Joural o Mechaical Sciece ad echology 25 (11) (2011) 2907~2917 Fig. 3. he movemet predictio usig rotary agles. Fig. 2. he bloc diagram o the estimatios. v v (15) + 1 = + Δ t + 1 = + Δ t P P v. (16) where ˆ is the acceleratio that removes the es rom the avigatio rame. Ater estimatig the es i the avigatio rame the measured acceleratios i the body rame are evaluated usig b b = C (11) ˆ b b b =. (12) 1 b I q. (11) C = = Cb C b. he -1 superscript idicates a iverse matrix ad the superscript idicates a traspose matrix. ˆb is the acceleratio that removes the es rom the body rame. he iverse matrix o the trasormatio matrix is equal to the traspose matrix sice the trasormatio matrix is orthogoal. he tri-gyroscopes provide the three dimesio agular rate with respect to the body rame. Although the agular rate is reliable over log periods o time it must be itegrated i order to provide a absolute orietatio measuremet. hereore eve small errors i the agular rate geerate ubouded errors. Whe the mobile robot is statioary the agular rates i the body rame must be zero. hereore the measured agular rates are errors. I order to compesate or the es the agular rates are sampled or 10 secods; the sampled data are the averaged. Fig. 2 shows the bloc diagram o the proposed estimatio algorithm. b b ω = ω = ωx ωy ωz (13) b b b ωˆ = ω ω (14) where ω ˆ b is the agular rate vector that removes the es i the body rame. 2.1.3 he positio algorithm hrough ewto's law o motio the velocity o the mobile robot is calculated by usig a sigle itegratio o the acceleratio. he velocity at oe time-step rom the preset will be equal to the preset velocity plus the commaded acceleratio multiplied by the measuremet period ( Δt ). he positio is calculated by usig a sigle itegratio o the velocity. Ater the measured acceleratio trasorms the body rame ito the avigatio rame by applyig the acceleratio o the gravity the equatio o velocity becomes: ( ) + 1 = + b + b Δ t v v C g (17) where v = v v vd deotes the velocity vector o the avigatio rame at time. he positio o the mobile robot becomes: 2 ( ) t 1 P + 1 = + Δ + b 2 + P v t Cb g Δ (18) where P = P P PD deotes the positio vector o the avigatio rame at time. 2.2 he odometry system he rotor ecoder is the system that coverts the agular rate o the rotor ito a digital sigal. Geerally the rotor ecoders are attached oto the wheels o the mobile robot; the wheel's rotary agle is measured by the ecoder. he rotor ecoder geerates pulses while the wheel rotates 360 degrees. I the measured pulses are M couts each wheel's rotary agle becomes: Ml Mr ηl = 2 π ηr = 2π (19) where η l ad η r are the let ad right wheel's rotary agles i radias respectively. M l ad M r are the measured pulses o let ad right ecoders. he velocity positio ad yaw agle o the mobile robot are estimated by usig each wheel's rotary agle. Fig. 3 shows the movemet predictio o the mobile robot rom the wheels rotary agles. he mobile robot s travel distace a ca be expressed i terms o its wheel s radius ( R wheel ) ad each wheel s rotary agle.
B.-S. Cho et al. / Joural o Mechaical Sciece ad echology 25 (11) (2011) 2907~2917 2911 a l = Rwheelη l a r = R wheelη r (20) al + ar a =. 2 (21) he mobile robot s yaw agle rate ( Δϕ ) is calculated by the width o the robot ( d width ) ad the distace travelled by each wheel. a a Δ ϕ = d l r width I this case the robot s rotatio radius ( r ) is: (22) a r =. (23) Δ ϕ Accordig to the cosie law the mobile robot s positio rate ( Δ λ ) ca be expressed as: 2 2 2 2 2 2 cos 2(1 cos ) Δ λ = r + r r Δ ϕ = Δϕ r. (24) I the robot moves i a straight lie the yaw agle rate will be zero. I this case the rotatio radius will become arbitrarily large ad the term i the parethesis q. (24) will become zero. hereore q. (24) must be expaded by usig a aylor series as: 2 4 6 2 2 Δϕ Δϕ Δϕ a Δ λ = 21 1 + + L 2 2! 4! 6! Δ ϕ = + + 2! 4! 6! 8! 2 4 6 2 1 Δϕ Δϕ Δϕ 2 a L. (25) he ollowig is the mobile robot s positio rate trasormed ito the avigatio rame. Δϕ Δ P =Δ λ cos 1 ϕ + 2 Δϕ Δ P =Δ λ si ϕ1 + 2 (26) I roll ad pitch are experieced by the mobile robot the positio rate that compesates the attitude is: Δ Pˆ =Δ P cosθ +Δ siφcos θ P Δ Pˆ =ΔP cos φ. he mobile robot's positio ad yaw directio is deied as: + 1 + 1 (27) P = P +Δ Pˆ P = P +ΔP ˆ (28) ϕ ϕ ϕ. (29) + 1 = +Δ I this case the velocity is a dieretial o the mobile robot s quatity o chage i the positio. v ΔPˆ Δˆ P = v = Δt Δt 3. rror compesatio usig the Kalma ilter (30) Both positioig systems usig ecoders ad that usig iertial sesors have accumulative errors but caused by dieret reasos. I the method usig iertial sesors itegratio o the outputs is the cause o the accumulated errors. I the method usig ecoders systematic ad osystematic errors are accumulated through calculatio. We desig a liear Kalma ilter to combie these eatures. he Kalma ilter is ow to be the most ideal ilter to estimate state variables i a dyamic system [19 21]. 3.1 he orietatio compesatio I the iitial aligmet algorithm the orietatio o the mobile robot is calculated by the tri-accelerometers ad the compass sesor. I the mobile robot is movig it will have accelerated motio that is the tri-accelerometer will measure ot oly the acceleratio o the gravity but also the acceleratio motio o the mobile robot. I this case the attitude has a low accuracy because the roll ad pitch agles are measured by the tri-accelerometers [10 11]. Similarly the compass sesor which measures the yaw agle has errors sice small magetic ield chages ca be caused by metals oud i the exteral eviromet [1]. hereore whe the mobile robot is movig it is diicult to measure the orietatio o the mobile robot usig the iitial aligmet algorithm. he tri-gyroscopes provide the agular rate i the body rame. he tri-gyroscopes are useul or measurig the orietatio o the mobile robot while the mobile robot is movig. However the orietatio must be itegrated usig the agular rates; the gyroscope has drit ad radom wal oise which ca eect these itegratio. So the orietatio estimatio usig oly gyroscopes geerate accumulated orietatio errors. I odometry the yaw agle rate is calculated by q. (22). However the yaw agle rate is iaccurate with a ubouded accumulatio o systematic ad osystematic errors [1-8]. We desig the liear Kalma ilter to estimate the orietatio. he tri-gyroscopes are used as a system iput variables because the tri-gyroscopes have ast respose ad less error covariace but the error is accumulative. he triaccelerometers the compass sesor ad yaw agle rate rom odometry are used as a measuremet variable because these outputs have ot accumulated errors but have big error covariace. 3.2 he positio compesatio Dead recoig is a commo method used to predict the positio o a mobile robot by iteral sesors geerally ier-
2912 B.-S. Cho et al. / Joural o Mechaical Sciece ad echology 25 (11) (2011) 2907~2917 ad yaw agle rates o the mobile robot. he radom variables w ad v represet the process ad measuremet oise respectively. he speciic equatios or the time update (predict) ad the measuremet update (estimate) are [21]: Fig. 4. he bloc diagram o the Kalma ilter. tial sesors ad cotrol variables such as the ecoder. he positio estimatio obtaied by dead recoig has a acceptable accuracy over the short term however it has ubouded errors over the log term. I this study we use iertial sesors ad ecoders attached to the robot s wheels or positioig. he positioig method usig iertial sesors must itegrate the measured tri-acceleratios. his itegratio icreases the ubouded errors i the velocity ad positio o the mobile robot [9 11]. I the positioig method usig the ecoders the velocity ad positio o the mobile robot is calculated by the agular rate o rotor ad the diameter o the wheel. However ubouded errors occur due to the measurig errors oud i the ecoders i.e. the mechaical desig deects o the mobile robots the slippage betwee the wheels ad groud etc. [1-8]. he accurate positioig o a mobile robot usig either the iertial sesors or the ecoders is diicult to achieve due to the ubouded errors oud i each system. Although they both have ubouded velocity ad positio errors a coupled system will reduce the error to a acceptable level. he triaccelerometers are suitable or a system iput variables because the tri-acceleratios oer ast respose ad good short term stability but have big accumulated errors due to a double itegratio. he outputs o orietatio compesated odometry have less error covariace but have slow respose (about ive times per secod). hereore the outputs o orietatio compesated odometry are suitable or a measuremet variable i the liear Kalma ilter. 3.3 he Kalma ilter Fig. 4 shows the bloc diagram o the Kalma ilter used or dead recoig. he liear stochastic dierece equatios or the Kalma ilter i this case are: x = Ax + Bu + w (31) 1 1 1 z = Hx + v (32) where x = φ θ ϕ Δϕ v v is the state variable at time ad are divided ito the velocity orietatio xˆ = Axˆ + Bu (33) K AK A (34) 1 1 = 1 + ( ) 1 ( ) G = K H HK H + R (35) xˆ = xˆ + G z Hx ˆ (36) ( ) K = IG H K (37) where x ˆ deotes the estimated state variable ad x ˆ deotes the predicted state variable. K deotes the estimatio error covariace ad K deotes the predictio error covariace. G deotes the Kalma gai. he estimated ad predicted state variables are a liear combiatio. We assume that the correlatios betwee the velocity ad the orietatio are zero ad that the system matrix is a uit matrix. 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 A = (38) 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 he system iput variables are the outputs o the accelerometer ad the outputs o gyroscope. he correlatios o each iput are zero. he iput variable ad state variable are oly related to the measuremet period ad the trasormatio matrix. b b b b u = ω ω ω ω x y z z (39) B=Δ t I (40) 6x6 b b b where = D C b x y z. he measuremet variable ad the state variable are a liear combiatio. We assume that the correlatios o the measuremet variable are zero ad that the measuremet matrix is a uit matrix. H = I 6 x 6 (41) z = v v φ θ o o a ϕ a Δ ϕ c = o Hx (42) where the o subscript idicates the calculated values rom the odometry ad the a subscript idicates the calculated values rom qs. (2) ad (3). he c subscript is the output rom the compass sesor.
B.-S. Cho et al. / Joural o Mechaical Sciece ad echology 25 (11) (2011) 2907~2917 2913 he process oise covariace is the error covariace that is measured rom the iertial sesors. I we assume that the correlatios o iertial sesors are zero the process oise covariace is the ollowig. he white oise o velocity is related to white oise o acceleratio ad the measuremet period. I the white oise o acceleratio i the avigatio rame is the the oise covariace o velocity i the avigatio rame is: 2 2 2 2 = = Δ v v t = Δt (43) where is the oise covariace o acceleratio. Similarly the oise covariace o orietatio that is sigle itegratio o gyroscope output is: 2 2 2 2 = = Δ agle agle gyro t = gyroδt (44) where gyro is a white oise o gyroscope ad gyro is the oise covariace gyroscope. he process oise covariace is: Δv 0 0 0 0 0 0 Δ v 0 0 0 0 0 0 0 0 0 Δ vφ = 0 0 0 Δ 0 0 (45) v θ 0 0 0 0 Δv ϕ Δvϕ 0 0 0 0 Δv ϕ Δvϕ 2 where Δ v = Δ t Δ v = Δ t Δ v = Δ φ g x t 2 2 Δ v = Δ θ g y t Δ v = Δ ϕ g z t ad = D C.* b C b x y. z x y z are the error covariace measured with the tri-axis accelerometers. g x g y g z are the error covariace measured with the tri-axis gyroscopes. Operator.* is the array multiplier i MALAB. he measuremet oise covariace is the error covariace that is measured with the ecoders ad the compass sesor ad it is the error covariace o the attitude calculated by the accelerometers. I this case we assume that the correlatio betwee the attitude rom accelerometers the output o compass sesor ad the outputs o the ecoders are zero. he measuremet oise covariace is thereore: Rv 0 0 0 0 0 0 Rv 0 0 0 0 0 0 0 0 0 Rφ R = (46) 0 0 0 Rθ 0 0 0 0 0 0 Rϕ 0 0 0 0 0 0 RΔϕ 2 2 Fig. 5. he lowchart o the proposed system. where R v R v ad R Δϕ are the error covariace o the velocity ad yaw agle rates estimated by the ecoders. R φ ad R θ are the error covariace o the attitude calculated by the accelerometer. R ϕ is the error covariace o the yaw agle measured with the compass sesor. Fig. 5 depicts the lowchart o the proposed system. 4. he experimet 4.1 he system coiguratio he mobile robot is propelled by two DC motors. ach motor was equipped with two chael ecoder that measures the rotatig velocity ad rotatig directio o the wheels. A ADIS16354 Aalog Device Ic. iertial sesor module was used to measure the acceleratio ad agular rate. I order to measure the absolute yaw agle we used a Hoeywell HMC6352. he tilt sesor used to compesate or the accelerometer is a SCA100 that uses a dual-axial icliometer made by VI techologies. A MS320F28335 DSP exas Istrumet Ic. was used to calculate the proposed dead recoig algorithm ad to cotrol the mobile robot. Fig. 6 shows (a) the mobile robot (b) the desiged sesor ad motor drive board ad (c) the desiged positioig ad cotrol board. 4.2 he compesatio experimet I order to perorm the statioary accelerometer compesatio we collected 200 samples rom the tilt ad compass sesors or 10 secods ad the averaged collected samples.
2914 B.-S. Cho et al. / Joural o Mechaical Sciece ad echology 25 (11) (2011) 2907~2917 (a) x-axis agular rate (b) x-axis agular rate ater compersatio Fig. 6. he implemeted system. (c) y-axis agular rate (d) y-axis agular rate ater compersatio (a) x-axis acceleratio (b) x-axis acceleratio ater compersatio (e) z-axis agular rate Fig. 8. he agular rate o the body rame. () z-axis agular rate ater compersatio (c) y-axis acceleratio (d) y-axis acceleratio ater compersatio (a) Roll (b) -axis acceleratio ater compersatio (e) z-axis acceleratio Fig. 7. he acceleratio o the body rame. () z-axis acceleratio ater compersatio (c) Pitch (d) e-axis acceleratio ater compersatio he trasormatio matrix was calculated usig these outputs (roll pitch ad yaw) ad was applied to proposed algorithm. Similarly the values or the gyroscope were calculated usig the average value o 200 collected samples. Fig. 7 ad Fig. 8 show the outputs o the tri-axis accelerometers ad the tri-axis gyroscopes or 3600 secods ater the compesatio usig the proposed algorithm. I Fig. 7(d) at the y-axis is show i regards to the groud icliatio. I Fig. 8 the agular rates are almost zero ater the compesatio. Fig. 9 depicts the orietatio o the mobile robot determied by the iitial aligmet algorithm usig the tri-accelerometers ad the compass sesor. I Fig. 9(a) the roll agle is show i (e) Yaw () d-axis acceleratio ater compersatio Fig. 9. he orietatio ad the acceleratio o the body rame. regards to the groud icliatio. I Fig. 9 the acceleratios o avigatio rame are almost zero because we are compesated or the groud icliatio by usig the tilt ad compass sesors.
B.-S. Cho et al. / Joural o Mechaical Sciece ad echology 25 (11) (2011) 2907~2917 2915 (a) Roll rom gyro (b) Roll rom Kalma (c) Pitch rom gyro (d) Pitch rom Kalma (a) stimatio positio by odometry (e) Yaw rom gyro Fig. 10. he CW orietatio. () Yaw rom Kalma (a) Roll rom gyro (b) Roll rom Kalma (b) stimatio positio by Kalma ilter Fig. 12. he CW estimatio positio. (c) Pitch rom gyro (e) Yaw rom gyro Fig. 11. he CCW orietatio. 4.3 he orietatio estimatio experimet (d) Pitch rom Kalma () Yaw rom Kalma I order to perorm the orietatio estimatio the mobile robot was programmed to travel i a 9700 mm diameter circle i the test space. Figs. 10 ad 11 depict the orietatio o the mobile robot i a clocwise (CW) ad couterclocwise (CCW) directio respectively. he pitch agle rom the gyroscope cotiuously decreased because the mobile robot tae accelerated towards the x-axis. he roll agle rom the gyroscopes cotiuously decreased or icreased due to the cetriugal orce geerated by the circular motio o the mobile robot. From the experimet result it ca be observed that the output o gyroscope is heavily aected by the movemet o the mobile robot. hereore the orietatio must be periodically compesated by usig exteral sigal while the mobile robot is movig i log term. From Figs. 10 ad 11 the estimated orietatio usig proposed algorithm has bee experimetally show to compesate or the accumulative error. Also the roll ad pitch rom the proposed algorithm are show to have periodic agles due to the groud icliatio. 4.4 he positio estimatio experimet he mobile robot travelled i a 9700 mm diameter circle. he experimets were perormed usig CW ad CCW circu-
2916 B.-S. Cho et al. / Joural o Mechaical Sciece ad echology 25 (11) (2011) 2907~2917 able 1. he error mea ad stadard deviatio. CW Positio Mea Std. deviatio (4850 4850) Odo (5013 4534) (52.37 36.06) Kal (4946 4785) (265.07 165.51) (9700 0) Odo (8995-928.9) (45.25 48.69) Kal (9479-351.2) (144.14 368.44) (4850-4850) Odo (3157-4345) (40.54 37.52) Kal (4258-4748) (313.55 157.02) (0 0) Odo (368.4 1826) (37.81 50.56) Kal (19.7 635.8) (164.53 283.49) CCW Positio Mea Std. deviatio (-4850 4850) Odo (-4851 4371) (49.87 37.78) Kal (-4955 4799) (250.86 235.83) (-9700 0) Odo (-8696-910.9) (41.02 48.05) Kal (-9476-250.6) (201.1 269.84) (-4850-4850) Odo (-3509-4190) (47.03 42.79) Kal (-4307-4741) (376.93 172.19) (0 0) Odo (-375.3 1777) (39.21 55.36) Kal (1.54 619.9) (173.76 262.45) (a) stimatio positio by odometry lar motios ity times each. Figs. 12 ad 13 depict the positios o the mobile robot. I the igures the circle is the true positio o the mobile robot. I the experimet results the odometry method estimates a smaller circular positio tha true positio because the calculated yaw agle chages aster tha the true yaw agle. Although the result has ot bee provided i this paper the positioig usig oly iertial sesor has show ubouded positio error due to double itegratio. he proposed method delivers a reliable positio with a acceptable estimatio error compared to the method that oly used odometry or iertial sesors. However whereas the proposed method provided a small error it had a large variace. able 1 display the estimatio positio mea ad stadard deviatio at each o the our positios. 5. Coclusios A IS oers a quic ad accurate respose i the short term but the positio estimatio errors are ampliied due to the itegrative ature o the IS low requecy oise ad sesor. he positio estimatio usig ecoders will suer rom egative due to systematic ad osystematic errors ad ubouded positio errors due to the itegrative ature o the rotatig speed. I this paper we preseted mobile robot positio estimatio that combies odometry ad the IS i order to reduce the accumulatio errors iheret i dead recoig. I order to compesate or weaesses oud i each system we desiged a Kalma ilter. he proposed system estimates the orietatio o the mobile robot usig iertial sesors thereore it ca compesate or the icliatio o the groud. Iertial sesors have errors; the proposed method corrected the errors. he proposed method also compesates or the yaw agle errors that geerate positio errors i odometry thereore it has smaller positio errors. Although the proposed system has a comparatively large variace the system shows a reliable level o positio estimatio. I the proposed method the dead recoig period that estimates the positio without ay exteral positio data rom a absolute positioig system has bee exteded. I order to improve the positio estimatio accuracy the system modelig algorithm ad the two systems combiatio algorithm eed urther study. Acowledgmet his wor was supported by the Grat o the Korea Miistry o ducatio Sciece ad echology (he Regioal Core Research Program/Istitute o Logistics Iormatio echology). Reereces (b) stimatio positio by Kalma ilter Fig. 13. he CCW estimatio positio. [1] J. Borestei ad L. Feg Measuremet ad correctio o systematic odometry errors i mobile robots I ras. Robotics ad Automatio 12 (6) (1996) 869-880. [2]. Houshagi ad F. Azizi Accurate mobile robot positio determiatio usig usceted Kalma ilter lectrical ad
B.-S. Cho et al. / Joural o Mechaical Sciece ad echology 25 (11) (2011) 2907~2917 2917 Computer gieerig Caadia Coerece (2005) 846-851. [3] M. Ibraheem Gyroscope-ehaced dead recoig localizatio system or a itelliget waler Iormatio etworig ad Automatio (ICIA) Iteratioal Coerece 1 (2010) V1-67~V1-72. [4] W. S. Moo B. S. Cho J. W. Jag ad K. R. Bae A multirobot positioig system usig a multi-code ultrasoic sesor etwor ad a Kalma ilter Iteratioal Joural o Cotrol Automatio ad Systems 8 (6) (2010) 1349-1355. [5] A. Widyotriatmo ad K. S. Hog avigatio uctio-based cotrol o multiple wheeled vehicles I ras. Idustrial lectroic 58 (5) (2011) 1896-1906. [6] C. aagawa Y. Suda K. aao ad S. aehara Stabilizatio o a bicycle with two-wheel steerig ad two-wheel drivig by drivig orces at low speed Joural o Mechaical Sciece ad echology 23 (4) (2009) 980-986. [7] A. Widyotriatmo B. H. Hog ad K. S. Hog Predictive avigatio o a autoomous vehicle with oholoomic ad miimum turig radius costraits Joural o Mechaical Sciece ad echology 23 (2) (2009) 381-388. [8] J. B. Sog ad K. S. Byu Steerig cotrol algorithm or eiciet drive o a mobile robot with steerable omidirectioal wheels Joural o Mechaical Sciece ad echology 23 (10) (2009) 2747-2756. [9] D. H. itterto ad J. L. Westo Strapdow iertial avigatio techology Secod d. he Istitute o lectrical gieers Uited Kigdom (2004). [10] P. G. Savage Strapdow iertial avigatio itegratio algorithm desig part 1: Attitude algorithm Joural o Guidace Cotrol ad Dyamics 21 (1) (1998) 19-28. [11] P. G. Savage Strapdow iertial avigatio itegratio algorithm desig part 2: Velocity ad positio algorithms Joural o Guidace Cotrol ad Dyamics 21 (2) (1998) 208-221. [12] J. M. Choi S. J. Lee ad M. C. Wo Sel-learig avigatio algorithm or visio-based mobile robots usig machie learig algorithms Joural o Mechaical Sciece ad echology 25 (1) (2011) 247-254. [13]. Hoghui ad J. Moore Direct Kalma ilterig approach or GPS/IS itegratio I ras. o Aerospace ad lectroic Systems 38 (2) (2002) 687-693. [14] C. J. Wu ad C. C. sai Localizatio o a autoomous mobile robot based o ultrasoic sesory iormatio Joural o Itelliget ad Robotic Systems 30 (3) (2001) 267-277. [15] L. Kleema Optimal estimatio o positio ad headig or mobile robots usig ultrasoic beacos ad dead-recoig Proc. o the 1992 I it. Co. o Robotics ad Automatio ice Frace (1992) 2582-2587. [16] C. C. sai A localizatio system o a mobile robot by usig dead-recoig ad ultrasoic measuremets Istrumetatio ad Measuremet echology Coerece I 1 (1998) 144-149. [17]... Bui ad K. S. Hog Soar-based obstacle avoidace usig regio partitio scheme Joural o Mechaical Sciece ad echology 24 (1) (2010) 365-372. [18] J. Vagaay M. J. Aldo ad A. Fourier Mobile robot attitude estimatio by usio o iertial data Proc. o the 1993 I it. Co. o Robotics ad Automatio 1 (1993) 277-282. [19] B. Barsha Hugh F ad Durrat-Whyte Iertial avigatio systems or mobile robots I ras. Robotics ad Automatio 11 (3) (1995) 328-342. [20] G. A. Piedrahita ad D. M. Guayacudo valuatio o accelerometers as iertial avigatio system or mobile robots Robotics Symposium 2006. LARS '06. I 3rd Lati America (2006) 84-90. [21] G. Welch ad G. Bishop A itroductio to the Kalma ilter UC-Chapel Hill. R 95-041 (4) (2006). Bog-Su Cho received a B.S. degree i 2004 a M.S. degree i 2006 ad is curretly worig toward a Ph.D degree at the school o lectrical gieerig Pusa atioal Uiversity Korea. His research iterests iclude oliear cotrol adaptive cotrol robotics system positioig system ad system idetiicatio. systems. Woo-sug Moo received a B.S. degree i 2007 a M.S. degree i 2009 ad is curretly worig toward a Ph.D degree at the school o lectrical gieerig Pusa atioal Uiversity Korea. His research iterests iclude oliear cotrol adaptive cotrol robotics system ad collective itelliget Woo-Ji Seo received a B.S. degree i 2008 ad is curretly worig toward a M.S. degree at the school o lectrical gieerig Pusa atioal Uiversity Korea. His research iterests iclude sigal processig robotics system adaptive cotrol artiicial itelliget ad localizatio. Kwag-Ryul Bae received a B.S. degree i lectrical ad Mechaical gieerig rom Pusa atioal Uiversity Korea i 1984. He received M.S ad Ph.D degrees rom KAIS Korea. He joied urbotech Compay as the head o developmet rom 1989 to 1994. ow he is a proessor at Pusa atioal Uiversity Korea. His research iterests iclude digital sigal processig cotrol systems ad high-speed circuit systems.