School of Compuer Scence McGll Unversy Compuer Robo Vson Conference 2010 Ioanns Rekles
Fundamenal Problems In Robocs How o Go From A o B? (Pah Plannng) Wha does he world looks lke? (mappng) sense from varous posons negrae measuremens o produce map assumes perfec knowledge of poson Where am I n he world? (localzaon) Sense relae sensor readngs o a world model compue locaon relave o model assumes a perfec world model Togeher, he above wo are called SLAM (Smulaneous Localzaon and Mappng) Parcle Fler Tuoral, CRV 2010 Ioanns Rekles 2
Localzaon Trackng: Known nal poson Global Localzaon: Unknown nal poson Re-Localzaon: Incorrec known poson (kdnapped robo problem) Parcle Fler Tuoral, CRV 2010 Ioanns Rekles 3
Proprocepve Sensors (monor sae of vehcle-propagae) IMU (accels & gyros) Wheel encoders Doppler radar Nose Exerocepve Sensors (monor envronmen-updae) Cameras (sngle, sereo, omn, FLIR ) Laser scanner MW radar Sonar Tacle Uncerany Sensors Parcle Fler Tuoral, CRV 2010 Ioanns Rekles 4
Bayesan Fler Esmae sae x from daa Z Wha s he probably of he robo beng a x? x could be robo locaon, map nformaon, locaons of arges, ec Z could be sensor readngs such as range, acons, odomery from encoders, ec ) Ths s a general formalsm ha does no depend on he parcular probably represenaon Bayes fler recursvely compues he poseror dsrbuon: Bel x ) P( x Z ) ( T T T 5
Ierang he Bayesan Fler Propagae he moon model: Bel ( x ) P( x a 1, x 1) Bel ( x 1) dx 1 Updae he sensor model: Compue he curren sae esmae before akng a sensor readng by negrang over all possble prevous sae esmaes and applyng he moon model Bel ( x ) P( o x ) Bel ( x ) Compue he curren sae esmae by akng a sensor readng and mulplyng by he curren esmae based on he mos recen moon hsory 6
Moble Robo Localzaon (Where Am I?) A moble robo moves whle collecng sensor measuremens from he envronmen. Two seps, acon and sensng: (X,Y,θ) Predcon/Propagaon: wha s he robos pose x afer acon A? Updae: Gven measuremen z, correc he pose x Wha s he probably densy funcon (pdf ) ha descrbes he uncerany P of he poses x and x? 7
Sae Esmaon Propagaon P( x x, ) 1 Updae P( x x, z ) 1 1 1 8
Tradonal Approach Kalman Fler Opmal for lnear sysems wh Gaussan nose Exended Kalman fler: Lnearzaon Gaussan nose models Fas! 9
Mone-Carlo Sae Esmaon (Parcle Flerng) Employng a Bayesan Mone-Carlo smulaon echnque for pose esmaon. A parcle fler uses N samples as a dscree represenaon of he probably dsrbuon funcon (pdf ) of he varable of neres: S [ x, w : 1 N] where x s a copy of he varable of neres and w s a wegh sgnfyng he qualy of ha sample. In our case, each parcle can be regarded as an alernave hypohess for he robo pose. Parcle Fler Tuoral, CRV 2010 Ioanns Rekles 10
Parcle Fler (con.) The parcle fler operaes n wo sages: Predcon: Afer a moon () he se of parcles S s modfed accordng o he acon model S ( S,, n) where (n) s he added nose. f The resulng pdf s he pror esmae before collecng any addonal sensory nformaon. 11
Parcle Fler (con.) Updae: When a sensor measuremen (z) becomes avalable, he weghs of he parcles are updaed based on he lkelhood of (z) gven he parcle x w P( z x ) w The updaed parcles represen he poseror dsrbuon of he movng robo. 12
Remarks: In heory, for an nfne number of parcles, hs mehod models he rue pdf. In pracce, here are always a fne number of parcles. 13
Resamplng For fne parcle populaons, we mus focus populaon mass where he PDF s subsanve. Falure o do hs correcly can lead o dvergence. Resamplng needlessly, also has dsadvanages. One way s o esmae he need for resamplng based on he varance of he parcle wegh dsrbuon, n parcular he coeffcen of varance: M 2 var( w ( )) 1 cv ( Mw ( ) 2 E ( w ( )) M ESS 14 M 1 cv 2 1 1) 2
Predcon: Odomery Error Modelng Pecewse lnear moon: a smple example. Roaon: Corruped by Gaussan Nose. Translaon: Smulaed by mulple seps. Each sep models ranslaonal and roaonal error. Sngle sep: Small roaonal error (drf) before and afer he ranslaon. Translaonal error proporonal o he dsance raveled. All errors drawn from a Normal Dsrbuon. 15
Odomery Error Modelng 16
Odomery Error Modelng P r e d c o n 17
Odomery Error Modelng P r e d c o n 18
Odomery Error Modelng P r e d c o n 19
Odomery Error Modelng P r e d c o n 20
Predcon-Only Parcle Dsrbuon 21
Ioanns Rekles Propagaon of a dscree me sysem (d=1 sec) w w v y y w v x x v v d d d ) ( sn ) ( cos ) ( 1 1 1 Where s he addve nose for he lnear velocy, and s he addve nose for he angular velocy w v w Parcle Fler Tuoral, CRV 2010 22
Connuous moon example D=1sec Plong 1 sample/sec all he parcles every 5 sec Consan lnear velocy Angular velocy changes randomly every 10 sec 23
Connuous moon example 24
Predcon Examples Usng a PF Pecewse lnear moon (Translaon and Roaon) Command success 70% Sar a [-8,0,0] Translae by 4m Roae by 30 o Translae by 6m 25
Sar [-8,0,0 o ] 26
Translae by 4m 30% sayed 27
Roae by 30 o 30% sayed 28
Translae by 6m 29
Updae Examples Usng a PF 30
Envronmen wh wo red doors (unform dsrbuon) 31
Envronmen wh wo red doors (Sensng he red door) 32
Sensng four walls 33
Four possble areas 34
Cooperave Localzaon Pose of he movng robo s esmaed relave o he pose of he saonary robo. Saonary Robo observes he Movng Robo. Robo Tracker Reurns: <,,> x m es ( k 1) es 35 x y m m m es es x y s s sn cos s s s
Laser-Based Robo Tracker Robo Tracker Reurns: <,,> 36
Ioanns Rekles Tracker Weghng Funcon 2 2 2 2 2 2 2 2 2 2 1 2 1 2 1 e e e W U p d a e 37 Parcle Fler Tuoral, CRV 2010
Example: Predcon 38
Example: Updae 39
Example: Predcon 40
Example: Updae 41
Varaons on PF Add some parcles unformly Add some parcles where he sensor ndcaes Add some jer o he parcles afer propagaon Combne EKFs o rack landmarks 42
Keep n Mnd: The number of parcles ncreases wh he dmenson of he sae space 43
Complexy resuls for SLAM n=number of map feaures Problem: naïve mehods have hgh complexy EKF models O(n^2) covarance marx PF requres prohbvely many parcles o characerze complex, nerdependen dsrbuon Soluon: explo condonal ndependences Feaure esmaes are ndependen gven robo s pah 44
References Ioanns Rekles. A Parcle Fler Tuoral for Moble Robo Localzaon. Techncal Repor TR-CIM-04-02, Cenre for Inellgen Machnes, McGll Unversy, Monreal, Québec, Canada, 2004. Ioanns M. Rekles, Gregory Dudek and Evangelos Mlos. Mul-robo Cooperave Localzaon: A sudy of Trade-offs Beween Effcency and Accuracy. In Proc. of In. Conf. on Inellgen Robos and Sysems, pp. 2690-2695, Lausanne, Swzerland, Oc. 2002. Sequenal Mone Carlo Mehods n Pracce. Arnaud Douce - Nando de Freas - Nel Gordon (eds). Sprnger-Verlag, 2001, ISBN 0-387-95146-6. Isard M. and Blake A. CONDENSATION - condonal densy propagaon for vsual rackng. In. J. Compuer Vson, 29, 1, 5-28, 1998. F. Dellaer, W. Burgard, D. Fox, and S. Thrun. Usng he condensaon algorhm for robus, vson-based moble robo localzaon. In Conf. on Compuer Vson & Paern Recognon, 1999. M. Monemerlo and S. Thrun. Fasslam 2.0: An mproved parcle flerng algorhm for smulaneous localzaon and mappng ha provably converges. In SODA 01: Proc. of he 12 h annual ACM-SIAM symposum on Dscree algorhms, pages 735 744, 2001. Douce, A., de Freas, N., Murphy, K., and Russell, S. 2000. Rao-Blackwellsed parcle flerng for dynamc Bayesan neworks. In Uncerany n Arfcal Inellgence, pp. 176 183. Sm, R.[Rober], Elnas, P.[Panels], Lle, J.J.[James J.], A Sudy of he Rao-Blackwellsed Parcle Fler for Effcen and Accurae Vson-Based SLAM, IJCV(74), No. 3, Sepember 2007, pp. 303-318. Douce, A.; Johansen, A.M.; "A uoral on parcle flerng and smoohng: ffeen years laer". Techncal repor, Deparmen of Sascs, Unversy of Brsh Columba. December 2008. Arulampalam, M.S., Maskell, S., Gordon, N. and Clapp, T. A Tuoral on Parcle Flers for nonlnear/non-gaussan Bayesan Trackng. IEEE Trans. Sgnal Processng, Vol. 50, No. 2, 2002. p.174-188. Sequenal Mone Carlo Mehods Homepage Mone-Carlo Localzaon-n-acon page 45
Quesons For more nformaon on PF: hp://www.cm.mcgll.ca/~yanns/parcletuoral.hml For offlne quesons: yanns@cm.mcgll.ca 46