A PROBABILISTIC MULTIMODAL ALGORITHM FOR TRACKING MULTIPLE AND DYNAMIC OBJECTS

Transcription

1 A PROBABILISTIC MULTIMODAL ALGORITHM FOR TRACKING MULTIPLE AND DYNAMIC OBJECTS MARTA MARRÓN, ELECTRONICS. ALCALÁ UNIV. SPAIN MIGUEL A. SOTELO, ELECTRONICS. ALCALÁ UNIV. SPAIN J. CARLOS GARCÍA, ELECTRONICS. ALCALÁ UNIV. SPAIN. ABSTRACT The work presened is relaed o he research area of auonomous navigaion for mobile robos in unsrucured, heavily crowded, and highly dynamic environmens. One of he main asks involved in his research opic is he obsacle racking module ha has been successfully developed wih differen kind of probabilisic algorihms. The reliabiliy ha hese echniques have shown esimaing posiion wih noisy measuremens make hem he mos adequae o he menioned problem, bu heir high compuaional cos has made hem only useful wih few objecs. In his paper a compuaional simple soluion based on a mulimodal paricle filer is proposed o rack muliple and dynamic obsacles in an unsrucured environmen and based on he noisy posiion measuremens aken from sonar sensors. KEYWORDS: Eended paricle filers, muliple objecs racking, mulimodal probabiliy disribuions, muliple hypoheses, dynamic unsrucured and crowded environmens. 1. INTRODUCTION The origin of he probabilisic esimaors arrives quie early on he fifies, wih he idea of represening he sae vecor o predic is probabiliy disribuion, and applying his idea no only o he area of racking in robo navigaion. There were many advanages in he so-called Bayesian mehod (also known as Sequenial MoneCarlo) from he sochasic ones: he sysem model should no necessary be linear, and he noise coupled o measuremens should no necessarily be Gaussian. The sandard paricle filer (PF) is a sampling weighed represenaion of he Bayesian filer, where each one of he samples aken from he coninuous probabilisic disribuion is called paricle. The se of paricles mus be independen and idenically disribued o achieve a correc approximaion o he coninuous disribuion, bu his can be easily solved by using a large enough number of randomly acquired paricles. These echniques are no eensively used unil he end of he 90s in he area of ineres [1], wih he inroducion of a selecion sep in he PF loop o avoid he degeneraion of he algorihm wih ime [2]. The idea consiss on selecing (or resampling) and muliplying he paricles wih high imporance weighs and rejecing he res. Differen alernaives for his par were also designed [3]. To achieve a muliple objecs racker, differen opions have also been designed during he las years [4]. An iniial soluion is o use a sandard PF o rack each objec bu his is no efficien as i does no work wih a dynamic number of objecs. Some oher soluions include an associaion among he deeced objecs and he paricles of he filer over he ime (JPDAF) [5]. This is no done in he applicaion of ineres as long as hese echniques are no he mos appropriaed here. 2. DESCRIPTION OF THE DESIGNED ALGORITHM The dynamic and muli-objec racker designed for he applicaion menioned is based on a PF, hus in he following paragraphs boh he sandard algorihm and he improvemens made will be described.

2 2.1 The sandard PF The main loop of a sandard PF a ime sars wih a se S = { si / i = 1.. N} of random paricles represening he poserior disribuion of he sae vecor o be esimaed p( x 1 a he previous ime sep (-. These paricles are propagaed by he sysem dynamics o obain a new se S ha represens he prior disribuion of he sae vecor a ime, p( x y 1 ). The weigh of each paricle W = { wi / i =1.. N} is hen obained based on he comparison of he measured oupu vecor and he esimaed one based on he prior esimaions. Applying he seleced resample scheme, a new se S is obained wih he mos probable paricles ha will consiue he new p x y ) a ime. ( The funcionaliy of he algorihm is shown in figure 1. See [6] for a deailed explanaion. 1. Iniialize (only a =0): Obain he sample se S wih N paricles from he poserior disribuion p( x 1 (for -1=-1, his will be done wih he prior disribuion p( r x 0) ) 2. Propagae: Obain he new prior se S wih he model of he sae vecor o esimae: r = f ( x 1, v S = f ( S, V ), ( where v r 1 is he noise signal associaed o he dynamics of he sae vecor model, and from which V is obained as a vecor of sandard random variables wih he saisics ha define v r Imporance Sample: Calculae he imporance weighs of S se from he likelihood p( y ) p( y ) p( W = W 1 r W = W 1 p( y ), (2) q( x0.., y0.. ) r where q x x, y ) ( is he bes proposal approximaion of he poserior disribuion. In mos works his bes approximaion is subsiued by he prior one, so he Eq. (2) can be simplified. 4. Resample: Muliply/rejec samples of S wih high/low imporance weighs respecively o obain a poserior disribuion p( x y ) represened by a new se S wih N paricles. 5. Oupu: The final esimaed vecor sae is usually obained calculaing he mean of he poserior disribuion. 6. Go o sep 2 : The sample se S from he poserior disribuion p x y ) ( a ime is hen used as he new se S for he algorihm implemenaion in he ne sep, a ime +1. : Figure 1. Descripion of he sandard PF funcionaliy. 2.2 The mulimodal PF The sandard PF esimaes quie well he evoluion of any kind of single objec defined by is model, bu as i was already menioned a he inroducion of his paper, i has no been designed o rack a muliple and variable number of hem. To do so, differen soluions depending on he final applicaion have been proposed as i has already been explained in he inroducion. The mos ineresing of hem for he work proposed in his paper is he one presened in [7], because wih a single probabiliy disribuion a variable number of objecs can be racked wih high reliabiliy and wih no need of doing a previous associaion beween he differen measuremens and he paricles of he disribuion. The mos imporan innovaions ha were implemened a [7] o adap a sandard PF o a mulimodal esimaor are he following:

3 Re-iniializaion: In he sandard PF a new appearing objec is no going o be considered unless is sae vecor is close enough o an already exising one, as he oupu vecor does no modify direcly he sample se of he esimaed sae vecor iself bu only heir imporance weighs. To solve his problem a re-iniializaion of he sample se S a each ime sep has o be done, insering on i M samples direcly from he oupu vecor. Wih his modificaion, informaion from he new environmenal configuraion pm ( 1 is combined wih he poserior disribuion p p ( 1 o obain a new expression for i: p( 1 = γ pm ( 1 + (1 γ ) p p ( 1, (3) where γ is he facor ha weighs he disribuion associaion up, and ha is fixed by he relaion beween he M samples insered direcly from he oupu vecor measured a -1 and he oal number of samples (N) in he paricle se S ( γ = M N ). Wih his new iniializaion he single probabiliy disribuion will adap iself over ime o finally represen simulaneously he sae vecor of all he differen objecs ha exis in he environmen a each ime, wihou he risk ha he paricles relaed o new objecs in S disappear in he resample sage. Resample: To inser he new M paricles as menioned, he resample phase is also modified. In his case, only N-M samples have o be seleced from he N exising a he S sample se. The resampling process, as well as he res of he PF algorihm is for he res equal o ha one of he sandard PF. This version of he eended PF works quie well if all objecs are sensed wih more or less he same accuracy, bu he auhors of [7] explain ha if i does no occur (as i can happen easily working wih ulrasound sensors) he sample se may degenerae as he relaed weighs can be much larger for some objecs han for some ohers. To solve his problem differen improvemens have been made o his eended PF in he algorihm proposed in his paper. 2.3 The clusering algorihm A clusering algorihm has been designed o organize he measuremens ha come direcly from he sonar in k deeced objecs. This process is based on a sandard kmeans algorihm [8], bu some improvemens have been included o adap i o is specific use: Sandard kmeans: a. Selec randomly k cenroids for he clusers. b. While he disance from each measuremen is no minimum o is assigned cluser cenroid. c. For 1 o all measuremens: assign i o he cluser whose cenroid is he neares. d. If he disance from he measuremen o any cenroid is bigger han a limi, creae a new cluser, whose cenroid is he measuremen iself (k=k+. e. Recalculae all cluser cenroids using he mean. f. If a cluser is empy or has very few members i is erased (k=k-. Cluser movemen esimaion: Insead of assigning randomly he iniial cenroids, hey are obained from he previous clusering process, so ha he algorihm is shorer as he clusers o find are slighly predefined. The movemen of he cluser can be esimaed calculaing he dynamics of is cenroid. Cluser candidae: When a new cluser is creaed (i has no been calculaed from an iniial cenroid) i is convered ino a candidae ha is no validaed o be useful in he probabilisic

4 algorihm unil i is possible o follow is evoluion wih is relaed dynamics for a variable number of imes. The same process is used o remove a cluser. This mehod ensures he robusness of he probabilisic esimaor o spurious measuremens and so incremens is reliabiliy. The informaion obained from he clusering algorihm is used in differen pars of he PF loop incremening he possibiliies and reliabiliy of he probabilisic esimaor, as follows: A he re-iniializaion sep: Wih a cluser organizaion i is possible o selec he measuremens o be insered in he prior disribuion p( x 1 a he re-iniializaion sep, according o heir preliminary objec assignaion (in general M/k measuremens from each cluser). As newly insered paricles are chosen randomly from groups wih high level concenraion of measuremens heir likelihood is very high from he beginning. This fac prevens from siuaions in which paricles relaed o objecs poorly sensed are erased from he mulimodal disribuion a he resampling sep, as occurred in [7]. The M/k paricles o be insered from each cluser are compleed wih some ohers randomly seleced from is hisory buffer, which conains measuremens assigned o each cluser in previous ime seps and ha are no very disan from is acual cenroid. New paricles aken from he hisory buffer make he esimaion more sable. S N-M paricles from poserior disribuion a -1 (black) M/k new paricles randomly aken from each cluser + is hisory buffer a -1 (whie) S Paricles propagaion wih heir cluser dynamics (cenroid propagaion is also done) Imporance Sampling based on likelihood. The neares cluser cenroid is used o obain each paricle likelihood W S Clusering (whie) from acualized cenroids Members oo disan from he new cenroid are aken ou from cluser hisory buffers (black) Resampling N-M paricles according o heir cluser associaion Figure 2. Descripion of he funcionaliy of he eended PF designed. A he propagaion and imporance sampling sep: The cluser srucure is used o make paricles evoluion and likelihood calculus according heir one mos similar cluser model and oupu vecoespecively. Wih his mehod he prediced sample se S is going o be very close o he real sae vecor, obaining high values for he likelihood funcion p( y ) a he imporance sampling sep, and hus improving he robusness and reliabiliy of he global esimaor. A he resampling sep: The cluser informaion can be used o do a dynamic assignmen of he M-N paricles o resample among he k differen clusers deeced, and according o heir

5 likelihood oo. This fac also prevens from he siuaions of objecs poorly measured whose relaed paricles are removed from he poserior disribuion, as previously menioned. A he oupu sep: The clusering algorihm is execued again a he end of he eended PF, and his ime over he S sample se, using he cenroids of he obained clusers as he final esimaed sae vecor, ha is he oupu of he eended PF a each ime sep. Figure 2 shows he funcionaliy of he eended PF designed, including he clusering algorihm. 3. RESULTS Differen ess have been developed wih a roboic plaform (from AcivMedia Roboics), in a dynamic environmen. The robo has 16 sonar all around is body wih a 3m range. In one of he ess he robo has been wandering around by an unsrucured environmen wih diverse obsacles appearing in he scene, in a manual driving mode. Ne figures 3 and 4 show differen momens of he experimen, each one wih four graphics wih he following meaning: o Lef upper corner: acual 3D densiy funcion of he occupied space represened by he paricle se in he x-y space. o Righ upper corner: represenaion of he robo acual siuaion in he environmen. The circles show validaed clusers cenroids generaed by he oupu clusering process. o Lef lower corner: accumulaed represenaion of he cenroids from he oupu clusers, all along he covered pah. Differen idenified clusers are represened wih differen shapes for heir cenroid poin. The darker line shows he robo posiion all along he pah. o Righ lower corner: acual hisogram of he paricle se sae vecor componens. The r obsacle posiioning model is based on he sae vecor: x = [ y v vy ], and only is wo firs componens are shown here. The differen groups ha can be disinguished a he hisograms would resul in differen clusers (differen disinguished obsacles) a he oupu clusering process. Figure 3. Resuls from he probabilisic racker developed.

6 Figure 4. Resuls from he probabilisic racker developed. 4. ACKNOWLEDGEMENTS This work has been financed by he Spanish adminisraion (Consejería de Educación de la Comunidad de Madrid) hrough Research Projec Enigma, 07T/51/2003 and was developed during a researching period spen a he Cenre of Auonomous Robos in KTH (Sweden) sponsored by he Spanish Sae Secreary of Educaion and Universiy. 5. REFERENCES [1] Isard M. & Blake A. "Condensaion: condiional densiy propagaion for visual racking". Inernaional Journal of Compuer Vision, 29 (, pp [2] Gordon N.J., Salmond D.J, & Smih A.F.M. Novel Approach o Nonlinear/Non-gaussian Bayesian Sae Esimaion. IEE Proceedings-F, 140(2), pp: [3] van der Merwe R., de Freias N., Douce A., Wan E. The Unscened Paricle Filer. Advances in Neural Informaion Processing Sysems. 13, November [4] Oron M. & Fizgerald W. A Bayesian Approach o Tracking Muliple Targes using Sensor Arrays and Paricle Filers. Technical Repor, Cambridge Universiy Engineering Dep. CUED/F-INFENG/TR.403. [5] Schulz D., Burgard W., Fox D. & Cremers A.B. Tracking Muliple Moving Objecs wih a Mobile Robo. Proc. of he IEEE Compuer Sociey Conference on Compuer Vision and Paern Recogniion (CVPR), [6] Douce A. de Freias J.F.G. & Gordon N. Sequenial MoneCarlo mehods in pracice. Springer-Verlag [7] Koller-Meier E.B. & Ade F. Tracking Muliple Objecs using a Condensaion Algorihm. Journal of Roboics and Auonomous Sysems, pp , [8] Kanungo T., Neanyahu N.S. & Wu A.Y. An Efficien k-means Clusering Algorihm: Analysis and Implemenaion. IEEE Transacions on Paern Analysis and Machine Inelligence, Vol 24, nº7, July