Gray-dynamc EKF for Moble obo SLAM n Indoor Envronmen Peng Wang, Qbn Zhang, Zongha hen Deparmen of Auomaon, Unversy of Scence and echnology of hna, Hefe, 6, hna grapesonwang@gmalcom, zqb@malusceducn, chenzh@usceducn Absrac he Gray-dynamc EKF (GEKF) algorhm s proposed o esmae he saes of a moble robo n an ndoor envronmen Frs, he gray predcon heory s adoped o predc he saes of a moble robo and he feaure posons n he envronmen; ne, based on he predcons, a moble robo sysem model s bul dynamcally; hen, he GEKF s used o esmae he moble robo saes and he feaure posons Epermenal resuls show ha he GEKF can acheve almos he same esmaon accuracy wh EKF, whle whou he need of a fed sysem model o mprove he head drecon esmaon accuracy of he moble robo, a head drecon mach algorhm s proposed, and relavely beer resuls are shown by epermens Keywords gray-dynamc EKF, ndoor envronmen, moble robo, sae esmaon I INODUION o localze self accuraely n an ndoor envronmen, a moble robo needs o represen he nformaon of an envronmen n a compac way, normally n he form of eher a merc or a opologcal map Generally, he moble robo localzaon and mappng happen concurrenly he process s named Smulaneous Localzaon and mappng (SLAM) he SLAM problem was frs descrbed by Smh e al []; hen, Leonard e al developed he orgnal dea and summed up as he SLAM problem [] In he SLAM problem, he odomeer nformaon s usually used as a predcon of he moble robo poson, and hen nformaon obaned by oher sensors lke sonar s used o calbrae he predcon errors Because of he relavely low cos and bologcal mechansm smlares, sonar sensors are wdely used for nformaon percepon n SLAM [] Bu here are sll dsadvanages of employng sonar sensors, and one of hem s he nformaon uncerany due o he hgh sonar beam angle o esmae he saes of a moble robo from he unceran nformaon, varous algorhms have been proposed here are manly wo caegores: he Kalman fler (KF) seres and he parcle fler (PF) seres he KF seres are based on he radonal Kalman fler o esmae he saes of a moble robo and he feaure posons Developed KF lke he eended Kalman fler [4], he unscened Kalman fler [5], he fuzzy Kalman fler [6] and he nerval Kalman fler [7] are all eensvely employed n SLAM Alhough he KF seres are wdely appled n SLAM, her dsadvanages are obvous: besdes he heavy compuaon and he requremen of reasonably precse nformaon, anoher problem s ha hey are all dependng on a fed sysem model When a moble robo moves n he unknown envronmen, due o he dynamcs of he envronmen and he uncerany of sensor nformaon, he sysem model s forced o change hus, here s he need o buld he sysem model dynamcally In hs paper, we propose he GEKF algorhm o fulfll he SLAM wh a lmed number of unceran samples and whou a fed sysem model By eplong he advanages of he gray sysem heory n processng small sample, poor nformaon ssues, we use only a lmed number of samples o predc he saes of a moble robo and he feaure posons, and we name he process he predcon sage; hen, sae predcons of he moble robo are appled o buld he sysem model dynamcally, meanwhle, he feaure predcons are used as esmaon of he feaure posons, and we name he process he modelng sage As he model s bul, he GEKF s employed o fulfll he saes esmaon I urns ou ha he GEKF can acheve almos he same sae esmaon accuracy wh EKF, whle whou he need of a fed sysem model In order o mprove esmaon accuracy of he head drecon, a roaonal head drecon machng (hm) mehod s proposed, whch resuls n a beer esmaon of he moble robo head drecon Indoor envronmen epermens are desgned o valdae he effcency of he proposed mehods II ADIIONAL FIXED SYSEM MODEL radonally, knemacal model of a moble robo ndcaed n Fg s bul n a fed, nonlnear equaon X( k + ) = f( X( k), u( k)) + w( k), () where X( k ) s he sae vecor, uk s he npu and wk s he sysem nose ha s normally Gaussan whe nose wh covarance mar Qk and f ( ) s he sae ranson funcon Observaons of he feaures are modeled as Z( k) = h( X( k)) + v( k), () 978--4799--8//$ c IEEE 4
Y W OW X ( k) X ( k+) ΔD k z( k ) ( k ) Fg he knemacal model of a moble robo θ Δθ ( k) θ ( k +) z( k +) f f F(, y ) where Z( k ) s he feaure vecor, vk s he observaon nose ha s normally Gaussan whe nose wh covarance mar ( k ), and h( ) s he observaon funcon onvenonally, f ( ) and h( ) are called he robo sysem model and hey are supposed o be nvarable However, due o he compley of he envronmen and parameer vbraons of he conrol laws, he sysem model s changng On he oher hand, nformaon obaned by he odomeer or sonar sensor s hghly unceran, makng harder o model he sysem drecly o solve hese problems, we propose he gray-dynamc modelng mehod and he GEKF algorhm based on he bul sysem model III GAY-DYNAMI SYSEM MODELING he gray sysem heory was frs proposed by Deng [8] Our prevous work has appled he gray sysem heory n moble robo navgaon [9] and map buldng [] e al Based on our prevous work, he gray-dynamc model of a moble robo s bul n hs paper We begn wh an nroducon of he gray predcon model GM (,), whch means a gray model ha descrbed by a sngle varable dfferenal equaon of frs order More deals can be fnd n [] A Gray predcon he man ask of a gray predcon model s o erac he governng law of a sysem by usng he avalable daa [] Normally, a gray predcon model eplos he Accumulaon Generaon Operaor (AGO) o decrease randomness of he daa, and hen he accumulaed daa are used o deermne Y W y XW parameers of a dfferenal equaon n accordance wh he sandards of leas squares By solvng he dfferenal equaon, we can ge he n-sep ahead predcons of he sae, bu hey are he accumulaed values o oban he prmve sae values, he Inverse Accumulang Generaon Operaor (IAGO) s used o ge he prmve predcon () Wh a sae sequence X = ( n, n+,, ), as shown n Fg, he sae predcon process s as below ) Deermne he wndow sze n () () () () If n, hen X = ( n, n+,, ) ; If < n, he suaon s more complcaed In hs case, here are no enough sae samples o buld he predcon model, bu hs suaon happens only a he begnnng when he accumulaed odomeer errors are small hus, he odomeer readngs, whch are () () () () denoed as D = ( d, d,, d n ), are used o buld he gray predcon model For convenence of represenaon and undersandng, we defne he unfed epresson as X = (,,, ) () () () () () n n+ ) he Accumulaed sequence s X = (,,, ), (4) () () () () n n+ () where m () m =, m = n,, = n ) he generaed mean sequence s where Z = ( z, z,, z ), (5) () () () () n+ n+ z = 5[ + ], m=,,, n () () () m m m 4) Defne aˆ = [ a, b], n whch a and b are parameers of () () he GM (,) model k + azk = b By subsung he correspondng elemens of Z () () and X no he GM (,) model, wh leas square sandard, we can compue â by aˆ = B B B Y, (6) () () () where Y = [,,, ], and n+ n+ y y y y n + y n O W f f F (, y ) z n z z n + z n n+ Fg he moon process of a moble robo z z X W () z n+ () z n+ B = (7) () z In a praccal applcaon, observaon of a sae s generally posve, because eher ndcaes he dsance he moble robo ravels or he dsance beween he moble robo and he feaure hus, he nverse of B B always ess 44 6h IEEE onference on obocs, Auomaon and Mecharoncs (AM)
5) Subsue â no he whenng funcon () () d d + a = b By solvng he dfferenal equaon we ge () he connuous predcon equaon of as a = b a+ ( b a) e (8) () () n Dong dervaon on boh sdes of equaon (8), we ge he () predcon of + as () () () a d d a( n b a) e + = =, (9) () and he correspondng dscree predcon of + s k () () ak k a( k n b a) e + == () B Dynamc sysem modelng Due o he sysem and measuremen errors, s more reasonable o represen he sensor measuremen wh an nerval raher han a sngle value Normally, one measuremen a me of a sae, such as, can be denoed as [ - kσ, + kσ ] o oban σ, we frs use he sensor o produce a se of measuremens, hen σ s defned as he sandard devaon of he measuremens k deermnes he confdence ha he real value of locaes n [ - kσ, + kσ ] Measuremens of saes are all represened as nervals n he res of he paper o model he sysem dynamcally, wh he predcon equaon (), and n + prmve sae observaons = {[ n, n],[ n+, n+ ],,[, ],[, ] }, () X he sysem modelng process s gven ) he sae predcon a me Accordng o (), he upper and lower predcon bound a me s: = a( n b a)ep a( ) = a( n b a)ep a( ) () ) he sae predcon a me + Whle he sae value a me s predced, he wndow sldes one sep ahead and he las n elemens of X s used o predc he upper and lower bounds a me + = a ( b a )ep a n + + + + + + + = a+ ( n+ b+ a+ )ep a+ () ) Modelng of he sae ranson mar hrough equaon () and (), boh he upper and lower bound predcons a me and + can be calculaed, whch are denoed separaely as, and +, + In hs paper, a weghed average of he upper and lower bounds s used o represen he real sae value For convenence, boh weghs are chosen as 5, and we denoe he correspondng weghed mean values a me and + as ˆ and ˆ + he rao of ˆ + and ˆ, whch are denoed as r ˆ ˆ = +, s chosen o represen changes of he sae predcons he sae ranson mar from me o + s hen bul as m = dag r, r,, r Gray-dynamc predcon of he feaure posons As shown n Fg, he moble robo obans dsance beween feaure F and self Normally, he measured dsance s dsurbed by unknown nose, so s beer o employ a dynamc predcon model raher han a fed one Wh he n dsance readngs {[ n+, n+ ],,[, ],[, ] } Z = z z z z z z, (4) he feaure poson predcon s gven Based on he n observaons, he upper and lower bound predcons a me + are z z z z z+ = a+ ( z n+ b+ a+ )ep a+ z z z z z + = a+ ( z n+ b+ a+ )ep a+ (5) Afer he upper and lower bound predcons a me + are calculaed, he correspondng weghed mean value s calculaed hrough zˆ + = 5( z+ + z + ) Wh he sae ranson mar bul and he predcons of he moble robo and he feaure posons, he GEKF algorhm s gven n able IV OAIONAL HEAD DIEION MAHING Due o he wheel slppage and nheren error of he gyroscope, noses ha dsurb he head drecon are normally no Gaussan herefore, he esmaon accuracy of he head drecon s que low [] o mprove he esmaon accuracy, we propose he hm algorhm ) he local map a me here are mulple mehods beng proposed o buld a local envronmen map As map buldng s no he purpose of hs paper, we apply he map buldng mehod n [] drecly o erac he local envronmen map Because sensor daa are represened by nervals n our paper, here are several dfferences: frs, dsance and angle nformaon obaned by sensors are represened by nervals; second, local envronmen feaures are descrbed by nervals, fnal poson of a feaure s gven by a weghed mean of he eraced upper and lower bounds 6h IEEE onference on obocs, Auomaon and Mecharoncs (AM) 45
ABLE I HE GAY-DANAMI EKF ALGOIHM Algorhm : he GEKF algorhm whle n + sae samples and ()-() are used o buld he curren sae ranson mar n feaure observaons and (5) are used o buld he observaon predcon z ˆ+ a me + () + s calculaed hrough ()- () : ˆ () ˆ = f ( ) + he covarance s propagaed hrough he equaon below: P = P + Q + () z + s calculaed hrough (5): ˆ zˆ = h( ˆ ) () () + + he predcon error covarance mar: S = H P H + he Kalman gan: + z + z + K = P S + + z + he updaed sae equaon and he correspondng updaed sae error covarance mar: () () () () ˆ = ˆ + K [ z zˆ ] + + + + + P = P K S K + + + + + = + End whle he mehod n [] and he properes of nervals are combned o buld he local envronmen map We name he map gray qualave map because lne and pon feaures are augmened o areas he gray qualave map bul by he robo a me s shown n Fg ) he local map a me + Durng he me and +, he robo keeps movng Bu due o he dsurbances of varous noses, head drecon would change A me +, he gray qualave map would be bul based on he sensor daa Gray quanave map bul a me + s shown n Fg ) he roaonal head drecon machng o fulfll he roaonal head drecon machng process, a background emplae (B) shown n Fg s bul he background emplae s a vrual square area whch has he same scale wh he local envronmen, and can be subdvded no small grds wh dfferen sze accordng o he praccal applcaon Accordng o he accuracy of he background emplae and he praccal scale of he local envronmen, he number of small grds n each row and column can be couned, whch are denoed as N Normally, here are N o grds n each row or column are occuped by he gray qualave map whle he oher N e grds are empy he rao No N s called he duy rao, whch ndcaes he percenage of occuped small grds n each row or column o avod ambguy, duy rao of he row and column are separaely denoed as and When he local gray qualave maps of me and + are bul, ogeher wh he bul background emplae, he hm Fg A skemac dagram of he hm algorhm s proposed Fg s a schemac dagram of he hm and deals of he hm s gven n able In Fg 4, can be seen ha, alhough he relave angle of he gray qualave maps a me and + s, he row and column raos are no eacly equal Bu as hs happens durng he whole process, doesn nfluence he mach resuls As shown n Fg 4(b)-(c), whle he relave angle s, boh row and column smlares reach he mamum V EXPEIMENS AND ANALYSIS o verfy he valdy of he GEKF and he hm algorhms, a smulaon epermen n he envronmen shown n Fg 5 s conduced he offce s abou m 5m, and he corrdor s around m long and m wde A poneer -DX moble robo equpped wh 6 sonar sensors, a Gyro and an odomeer are used o fulfll he epermen In our epermen, nal sae of he moble robo s (,5,) n he corrdor, and he robo moves cm/s along he as he frs sep of runnng he GEKF algorhm s buldng he sae ranson mar a each samplng me Accordng o he buldng procedure gven, rao changes of, y and hea beween me + and are shown n Fg 6 As shown n Fg 6(a), he rao of sae s greaer han a he begnnng and ends o be as me goes on; hough saes y and hea are supposed no o change, snce here are sll noses, raos of saes y and hea vbrang around, see Fg 6(b) and 6(c) By usng gray predcon heory and he sae ranson mar bul, he GEKF algorhm was conduced o esmae saes of he moble robo radonal EKF algorhm was also conduced o do some comparson work (a), (b) and (c) of Fg 7 are, y and hea esmaon errors of he GEKF and EKF Sandard devaons of he hree saes are gven n able I s easy o conclude from Fg 7 and able ha he GEKF could acheve almos he same esmaon accuracy wh EKF, whle whou he need of a fed sysem model Fg 8 shows he hm algorhm n esmang he head drecon of he moble robo I can be seen ha he esmaon accuracy of he hm algorhm s hgher han boh EKF and he GEKF 46 6h IEEE onference on obocs, Auomaon and Mecharoncs (AM)
ABLE II HE HM ALGOIHM Algorhm : he hm algorhm Buldng he aresan coordnae sysem Buld he robo coordnae sysem, as shown n Fg, and he correspondng gray qualave maps M, M + alculae he duy rao alculae he row and column duy raos a me and +, denoed as, and +, + alculae he head drecon change alculae he smlary vecor he smlary of wo vecors a me and + are calculaed hrough he equaons below: ' ' sm = +, sm = +, where and are fed, whle + and + are calculaed by roang he gray qualave map a me + a ceran angle Δθ Suppose he real change of he robo head drecon s θ, hen k = θ Δθ smlares can be calculaed, consrucng he row and column smlary vecors SIM and SIM k SIM = sm, sm,, sm (,,, k ) SIM = sm sm sm alculae he head drecon changes Fnd he mamum elemen of SIM, SIM and he correspondng orders, denoed as n and n,hen he head drecon changes of he robo beween me and + s θ, and he correspondng error s: Δ h= n Δθ+ n Δθ duy rao duy rao duy rao duy rao 4 hm hea=5/deg 4 6 8 row 4 4 6 8 column 4 (a) hm hea=/deg 4 6 8 row 4 4 6 8 column 8 75 Smlary 7 (b) hm Smlary Fg 5 he epermen envronmen rao rao rao 5 5 4 6 8 /s 9 (a) 8 4 6 8 /s 5 5 5 5 (b) 4 6 8 /s Fg 6 ao changes of he moble robo saes: (a), (b) y, (c) hea (c) 65 6 4 5 6 hea /deg Fg 4 he hm resuls: (a) he relave angle s 5 degree, (b) he relave angle s degree, (c) he smlary (c) ABLE III OMPAISON OF HE SANDAD DEVIAIONS OF HE ESIMAIONS (cm) y(cm) hea(deg) EKF 876 88 95 GEKF 48 64 954 6h IEEE onference on obocs, Auomaon and Mecharoncs (AM) 47
error /cm error /cm error /deg 4 - - G observaon error - 4 6 8 /s - - (a) G observaon error - 4 6 8 /s - (b) - G observaon error - 4 6 8 /s Fg 7 Errors of he moble robo saes: (a) he error of, (b) he error of y, (c) he error of hea error /deg - (c) - G hm error - 4 6 8 /s Fg 8 he hm errors compared o he GEKF and EKF VI ONLUSION hs paper proposed a dynamc sae esmaon mehod named GEKF, n whch boh he moble robo saes and he feaure posons are predced based on he gray predcon heory wh a lmed number of hsorcal daa Accordng o he predcons a me + and, he sae ranson mar s bul dynamcally Based on he sae predcon heory and he bul sae ranson mar, we use he GEKF o esmae saes of he moble robo and he feaure posons Epermen resuls show ha he GEKF could esmae he saes as accurae as EKF whou he need of a fed sysem model On he oher hand, o mprove head esmaon accuracy of he moble robo, we propose a head drecon machng algorhm named hm, whch, hrough epermen, shows a beer esmaon accuracy compared o boh he GEKF and EKF AKNOWLEDGMEN hs paper s suppored by he Naonal Naural Scence Foundaon of hna (Gran No 6757) EFEENES [] Smh, M Self, and P heeseman, Esmang unceran spaal relaonshps n robocs, n Auonomous obo Vehcles, IJ o and GWlfon, Eds New York: Sprnger Verlag, 99, pp 67 9 [] J J Leonard and H F Durran-Whye, "Smulaneous map buldng and localzaon for an auonomous moble robo," n Proceedngs of IEEE Inernaonal Workshop on Inellgen obos and Sysems, Osaka, 99, pp44-447 [] J D ardós, J Nera, P M Newman, and J J Leonard, "obus mappng and localzaon n ndoor envronmens usng sonar daa," he Inernaonal Journal of obocs esearch, vol, pp -, [4] F Marn, Agüero, and J M anas, "Eended Kalman fler populaons for a relable real-me robo self-localzaon," n Proc of he IEEE Inellgen Vehcles Symposum Workshops, Alcala de Henares,, pp-6 [5] S J Juler and J K Uhlmann, "Unscened flerng and nonlnear esmaon," n Proceedngs of he IEEE, vol 9, pp 4-4, 4 [6] F Maía, A Jménez, B M Al-Hadh, D odríguez-losada, and Galán, "he fuzzy Kalman fler: Sae esmaon usng possblsc echnques," Fuzzy Ses and Sysems, vol 57, pp 45-7, 6 [7] G hen, J Wang, and L S Sheh, "Inerval kalman flerng," IEEE ransacons on Aerospace and Elecronc Sysems, vol, pp 5-59, 997 [8] J L Deng, "Inroducon o grey sysem heory," he Journal of Grey Sysem, vol, pp -4, 989 [9] hen, D Dong, Z hen, and H Wang, "Qualave conrol for moble robo navgaon based on renforcemen learnng and grey sysem," he Mederranean Journal of Measuremen and onrol, vol 4, pp-7, 8 [] S L, P Wang, and Z hen, "An Envronmen Model for Moble obo: Gray Qualave Map," obo, vol 4, pp 485-49, [] D Yaoguo, L Sfeng, and Keja, "he GM models ha (n) be aken as nal value," Kybernees, vol, pp 47-54, 4 [] E Kayacan, B Uluas, and O Kaynak, "Grey sysem heory-based models n me seres predcon," Eper Sysems wh Applcaons, vol 7, pp 784-789, [] K Lee, N L Doh, W K hung, S K Lee, and S-Y Nam, "A robus localzaon algorhm n opologcal maps wh dynamc noses," Indusral obo: An Inernaonal Journal, vol 5, pp 45-448, 8 48 6h IEEE onference on obocs, Auomaon and Mecharoncs (AM)