Gray-dynamic EKF for Mobile Robot SLAM in Indoor Environment

Transcription

1 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), () //$ c IEEE 4

2 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)

3 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

4 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)

5 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 row column 4 (a) hm hea=/deg row column 8 75 Smlary 7 (b) hm Smlary Fg 5 he epermen envronmen rao rao rao /s 9 (a) /s (b) /s Fg 6 ao changes of he moble robo saes: (a), (b) y, (c) hea (c) 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 GEKF h IEEE onference on obocs, Auomaon and Mecharoncs (AM) 47

6 error /cm error /cm error /deg G observaon error /s - - (a) G observaon error /s - (b) - G observaon error /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 /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, pp [] 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 , [] 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 , [] 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 , h IEEE onference on obocs, Auomaon and Mecharoncs (AM)