Localization. Mobile robot localization is the problem of determining the pose of a robot relative to a given map of the environment.

Transcription

1 Localizaion Mobile robo localizaion is he problem of deermining he pose of a robo relaive o a given map of he environmen.

2 Taxonomy of Localizaion Problem 1 Local vs. Global Localizaion Posiion racking iniial robo pose is known pose uncerainy is approximaed by a unimodal disribuion (e.g., a Gaussian) uncerainy is local Global localizaion iniial robo pose is unknown canno assume boundedness of he pose error, hus unimodal prob. disribuion is inappropriae Kidnapped robo problem during operaion, robo can ge kidnapped and eleoperaed o some oher locaion more difficul han he global localizaion robo migh believe i knows where i is while i does no

3 Taxonomy of Localizaion Problem 2 Saic vs. Dynamic Environmen Saic environmen: he only variable (sae) is he robo s pose Dynamic environmen: objecs oher han he robo whose locaions or configuraions change over ime, such as people, dayligh, movable furniure, doors Soluion: dynamic eniies migh be included in he sae vecor sensor daa can be filered so as o eliminae he damaging effec of unmodeled dynamics

4 Taxonomy of Localizaion Problem 3 Passive vs. Acive Passive localizaion: he localizaion module only observes he robo operaing Acive localizaion: conrols he robo so as o minimize he localizaion error and/or he coss coasal navigaion direc he robo o move ino a room in a symmeric corridor o eliminae ambiguiy Single-Robo vs. Muli-Robo One robo s belief can be used o bias anoher robo s belief

5 Localizaion: Iconic vs. Feaure-Based Localizaion mehods: Iconic mehod: Use raw sensor daa direcly Would mach sensor readings o daa fused from previous sensor measuremens in occupancy grid Used a lo in curren meric map making Feaure-based mehod: Use feaures exraced from sensor daa E.g., exrac a corner from sonar daa or occupancy grid, hen compare how corner moved in nex sep Concepually similar o disincive places Good for opological map making

6 Coninuous Localizaion and Mapping (Iconic) Use exerocepion sensors (e.g., laser, sonar, vision, ec.) To eliminae problems wih propriocepive echniques (e.g., shaf encoder) Mach curren percepion wih world wih pas observaions (i.e., map) This is NOT easy! Once rue posiion of robo is known wih respec o he map, curren percepion added o he map (called regisraion )

7 Maching Curren Percepion wih Previous Map If robo hasn moved far, a large porion of grid should mach wha is already in he global map BUT: could be error in he sensing The shaf encoders provide a se of possible poses (x, y, θ)?

8 Tradeoffs in Localizaion Localizaion aemps o balance compeing ineress: Localizaion aemps o deermine proper: Frequency of updaes (more could be beer, bu also incur more noise) Tradeoffs beween: Localizing afer every sensor reading Localizing afer n sensor updaes have been fused The choice of n is done by rial and error

9 Feaure-Based Localizaion Two ypes: (Similar o coninuous localizaion and mapping): robo exracs feaure, hen ries o find feaure in nex sensor updae (Exension of opological navigaion): robo localizes iself relaive o opological feaures Sensor uncerainy: sill plays large role Use gaeways, plus opological map, o deermine posiion (or se of possible posiions)

10 Comparison of Two Mehods Shaffer e al. concluded ha: Iconic mehods are more accurae Iconic mehods impose fewer resricions on he environmen Feaure-based algorihms are faser (ignoring he cos of feaure exracion) Feaure-based algorihms can handle poor iniial esimaes well No echnique handles a dynamic environmen Localizaion o a priori map canno allow a large number of discrepancies beween map and curren sae

11 Our Focus: Two Types of Localizaion Problems Global posiion esimaion figure ou where he robo is, bu we don know where he robo sared, given a priori map Local posiion racking figure ou where he robo is, given ha we know where he robo sared

12 Basic Idea of Markov Localizaion A varian of he Bayes filer Bel( x ) P( z x, m) P( x u, x, m) Bel( x ) dx Algorihm Markov_localizion( bel(x - 1), u, z, m): for all x do bel '( x) p( x u, x 1, m) bel( x 1) dx bel( x) p( z x, m) bel '( x) endfor reurn bel( x)

13 Revisi Bayes Filers z = observaion u = acion x = sae ) ( ), ( ) ( dx x Bel x u x P x z P ),,, ( ),,,, ( u z u x P u z u x z P Bayes ),,, ( ) ( 1 1 z u z u x P x Bel Markov ),,, ( ) ( 1 1 u z u x P x z P Markov ),,, ( ), ( ) ( dx u z u x P x u x P x z P ),,, ( ),,,, ( ) ( dx u z u x P x u z u x P x z P Toal prob. Markov ),,, ( ), ( ) ( dx z z u x P x u x P x z P

14 Markov Localizaion Markov localizaion addresses all hree localizaion problems: Posiion racking bel(x 0 ) = 1 if x 0 = x 0 (known iniial posiion) Or Gaussian disribuion cenered around x 0 Global localizaion Uniform disribuion over he space of all legal poses bel(x 0 ) = 1/ X Kidnapped robo

15 Illusraion of he ML Algorihm (1) Key idea: compue a probabiliy disribuion over all possible posiions in he environmen, represening he likelihood ha he robo is in a paricular locaion 1. Iniial posiion unknown: uniform disribuion: bel(x 0 ) 2. Door sensed: raising probabiliies for places close o door muliply is belief bel(x 0 ) by p(z x, m) 3. Moved forward: shifing belief disribuion accordingly moion: p(x u, x -1 )

16 Illusraion of he ML Algorihm (2) 3. Moved forward: shifing belief disribuion accordingly moion: p(x u, x -1 ) 4. Door sensed: raising probabiliies p(z x ) for places close o door, muliplied o curren belief 5. robo s belief afer having moved furher down he hallway

17 Wha does Markov Mean? Markov means he sysem obeys he Markov Propery Markov Propery: he condiional probabiliy of he fuure sae is dependen only on he curren sae and independen of he pas saes For he purpose of robo localizaion: Fuure sensor readings are condiionally independen of pas readings, given he rue curren posiion of he robo We don have o save all he prior sensor daa and apply i each ime we updae beliefs on he robo s locaion

18 Markov Localizaion (Con.) Bel(x) represens he robo s belief ha i is a posiion x Bel(x) is a probabiliy disribuion, cenered on he correc posiion Two probabilisic models are used o updae Bel(x): Acion (or moion model): represens movemens of robo Percepion (or sensing model): represens likelihood ha robo senses a paricular reading a a paricular posiion 1: Algorihm Markov_localizaion( bel( x 1), u, z, m) : 2 : for all x do 3: '( ) (, 1, ) ( 1) bel x p x u x m bel x dx 4 : bel( x) p( z x, m) bel '( x) 5: endfor 6 : reurn bel( x) predicion correcion

19 Common Terminology

20 Probabilisic Sensor Models Beam-based Scan-based Landmarks

21 Sensors for Mobile Robos Conac sensors: Bumpers Inernal sensors Acceleromeers (spring-mouned masses) Gyroscopes (spinning mass, laser ligh) Compasses, inclinomeers (earh magneic field, graviy) Proximiy sensors Sonar (ime of fligh) Radar (phase and frequency) Laser range-finders (riangulaion, of, phase) Infrared (inensiy) Visual sensors: Cameras Saellie-based sensors: GPS

22 Proximiy Sensors The cenral ask is o deermine P(z x), i.e., he probabiliy of a measuremen z given ha he robo is a posiion x. Quesion: Where do he probabiliies come from? Approach: Le s ry o explain a measuremen.

23 Beam-based Sensor Model Scan z consiss of K measuremens. z { z, z2,..., z 1 K } Individual measuremens are independen given he robo posiion. P( z x, m) K k 1 P( z k x, m)

24 Beam-based Sensor Model P( z x, m) K k 1 P( z k x, m)

25 Typical Measuremen Errors of an Range Measuremens 1. Beams refleced by obsacles 2. Beams refleced by persons / caused by crossalk 3. Random measuremens 4. Maximum range measuremens

26 Proximiy Measuremen Measuremen can be caused by a known obsacle. cross-alk. an unexpeced obsacle (people, furniure, ). missing all obsacles (oal reflecion, glass, ). Noise is due o uncerainy in measuring disance o known obsacle. in posiion of known obsacles. in posiion of addiional obsacles. wheher obsacle is missed.

27 Beam-based Proximiy Model Measuremen noise Unexpeced obsacles 0 z exp z max 0 z exp z max P hi ( z x, m) 1 2b e 1 ( zz 2 b 2 exp ) P unexp ( z x, m) e 0 z z z exp oherwise

28 Beam-based Proximiy Model Random measuremen Max range 0 z exp z max 0 z exp z max P rand ( z x, m) 1 z max P max ( z x, m) 1 z small

29 Resuling Mixure Densiy ), ( ), ( ), ( ), ( ), ( rand max unexp hi rand max unexp hi m x z P m x z P m x z P m x z P m x z P T How can we deermine he model parameers?

30 Raw Sensor Daa Measured disances for expeced disance of 300 cm. Sonar Laser

31 Approximaion Maximize log likelihood of he daa Search space of n-1 parameers. Hill climbing Gradien descen Geneic algorihms P( z zexp ) Deerminisically compue he n-h parameer o saisfy normalizaion consrain.

32 Approximaion Resuls Laser Sonar 300cm 400cm

33 Example z P(z x,m)

34 Scan-based Model Beam-based model is no smooh for small obsacles and a edges. no very efficien. Idea: Insead of following along he beam, jus check he end poin.

35 Scan-based Model Probabiliy is a mixure of a Gaussian disribuion wih mean a disance o closes obsacle, a uniform disribuion for random measuremens, and a small uniform disribuion for max range measuremens. Again, independence beween differen componens is assumed.

36 Scan Maching Exrac likelihood field from scan and use i o mach differen scan.

37 Properies of Scan-based Model Highly efficien, uses 2D ables only. Smooh w.r.. o small changes in robo posiion. Allows gradien descen, scan maching. Ignores physical properies of beams.

38 Robo Moion Robo moion is inherenly uncerain. How can we model his uncerainy?

39 Probabilisic Moion Model In pracice, one ofen finds wo ypes of moion models: Odomery-based Velociy-based (dead reckoning) Odomery-based models are used when sysems are equipped wih wheel encoders. Velociy-based models have o be applied when no wheel encoders are given. They calculae he new pose based on he velociies and he ime elapsed.

40 Example Wheel Encoders These modules require +5V and GND o power hem, and provide a 0 o 5V oupu. They provide +5V oupu when hey "see" whie, and a 0V oupu when hey "see" black. Source: hp:// These disks are manufacured ou of high qualiy laminaed color plasic o offer a very crisp black o whie ransiion. This enables a wheel encoder sensor o easily see he ransiions.

41 Dead Reckoning Derived from deduced reckoning. Mahemaical procedure for deermining he presen locaion of a vehicle. Achieved by calculaing he curren pose of he vehicle based on is velociies and he ime elapsed.

42 Reasons for Moion Errors ideal case differen wheel diameers bump and many more carpe

43 Odomery Model Robo moves from x, y, o x', y', '. Odomery informaion. u, ro1, ro 2 rans rans 2 ( x' x) ( y' y aan2( y' y, x' ) ro1 x ro 2 ' ro1 ) 2 ro 2 x', y', ' x, y, ro1 rans

44 Noise Model for Odomery The measured moion is given by he rue moion corruped wih noise. ˆ ˆ ˆ ro1 ro1 1 ro1 2 rans rans ro 2 ro 2 1 ro 2 2 rans 3 rans 4 ro1 ro 2 rans

45 Typical Disribuions for Probabilisic Moion Models ) ( x e x x 0 if ) ( 2 x x Normal disribuion Triangular disribuion

46 Applicaion Repeaed applicaion of he sensor model for shor movemens. Typical banana-shaped disribuions obained for 2d-projecion of 3d poserior. p(x u,x ) x u x u

47 Sampling from Our Moion Model Sar

48 Markov Localizaion s Implemenaion Depending on how one chooses o represen densiy Bel(x), various algorihms are available o implemen Markov Localizaion: Kalman filer boh he moion and he sensor model are described using a Gaussian densiy mainains a single esimae of he robo s posiion Grid-based Markov Localizaion deal wih muli-modal and non-gaussian densiies a a fine resoluion compuaional overhead vs. accuracy Sampling-based mehods densiy is represened by a se of samples ha are randomly drawn from i

49 Implemenaion wih Kalman Filer KF mainains a single esimae of he robo s posiion iniial belief a measuremen (in bold) wih associaed uncerainy belief afer inegraing measuremen ino belief belief afer moion o he righ new measuremen new inegraion

50 Previous Example I s only applicable o posiion racking problems I works well only if he posiion uncerainy is small Muli-hypohesis racking will solve he problem a he cos of increased compuaional complexiy

51 Muli- Hypohesis Tracking Example

52 Grid Localizaion Approximaes poserior using a hisogram filer over a grid decomposiion of pose space A common granulariy in indoor environmen is 15cm for x- and y-dimensions, and 5 degrees for roaional dimension Grid resoluion: Coarse: opological represenaion Fine: meric represenaion

53 Coarse-Grained

54 Grid wih Fixed-Resoluion

55 Example

56 Discree Bayes Filer Algorihm 1. Algorihm Discree_Bayes_filer( Bel(x),d ): If d is a percepual daa iem z hen 4. For all x do For all x do Else if d is an acion daa iem u hen 10. For all x do Reurn Bel (x) Bel' ( x) P( z x) Bel( x) Bel' ( x) Bel'( x) 1 x' Bel'( x) Bel'( x) P( x u, x') Bel( x')

57 Implemenaion (1) To updae he belief upon sensory inpu and o carry ou he normalizaion one has o ierae over all cells of he grid. Especially when he belief is peaked (which is generally he case during posiion racking), one wans o avoid updaing irrelevan aspecs of he sae space. One approach is no o updae enire sub-spaces of he sae space. This, however, requires o monior wheher he robo is de-localized or no. To achieve his, one can consider he likelihood of he observaions given he acive componens of he sae space.

58 Implemenaion (2) To efficienly updae he belief upon robo moions, one ypically assumes a bounded Gaussian model for he moion uncerainy. This reduces he updae cos from O(n 2 ) o O(n), where n is he number of saes. The updae can also be realized by shifing he daa in he grid according o he measured moion.

59 Markov Localizaion (Grid)

60 Markov Localizaion (Grid)

61 Markov Localizaion (Grid)

62 Sampling Based Mehod In which, we represen bel(x) by paricles Esimaion of non-gaussian, nonlinear processes This algorihm is called Mone Carlo Localizaion (MCL) I s applicable o boh local and global localizaion problems

63 Example M populaion of paricles Each paricle has an imporance weigh Increased # of paricles around locaions

64 Properies of MCL No bound o a limied subse of disribuions Increasing # of paricles (M) increases accuracy Trade off: accuracy vs. compuaion Common sraegy: keep sampling unil he nex pair (u, z) has arrived M needs o be high enough

65 Localize Proxy in Player/Sage The LocalizeProxy class is used o conrol a localize device, which can provide muliple pose hypoheses for a robo Allows you o localize he robo using is sensors, such as laser, sonar, or even radio srengh of signal Localizaion drivers will esimae he pose of he robo by comparing observed sensor readings agains a pre-defined map of he environmen E.g., amcl driver, his driver implemens he Adapive Mone-Carlo Localizaion (AMCL) algorihm described by Dieer Fox Mone Carlo Localizaion for Mobile Robos by F. Dellaer, D. Fox, W. Burgard and S. Thrun, AAAI 99.

66 AMCL Driver in Player/Sage The amcl driver mainains a probabiliy disribuion over he se of all possible robo poses, and updaes his disribuion using daa from odomery, sonar and/or laser. Probabiliy disribuion is represened by a paricle filer: Adapive: he number of paricles can be dynamically adjused pose is uncerain # of paricles increases pose is deermined # of paricles decreases radeoff beween speed and accuracy Feaures of simple MCL echniques: unknown robo iniial pose usually converge o correc pose incorrec robo iniial pose, or robo becomes los will no converge o correc pose

67 Mone Carlo Localizaion Developed by Fox, Burgard, Dellaer, Thrun (AAAI 99 aricle) Mone Carlo refers o echniques ha are sochasic / random / non-deerminisic hp://robos.sanford.edu/movies/sca80a0.avi

68 Adaping he Size of he Sample Se # of samples varies based on differen siuaions: During global localizaion: unknown robo s iniial posiion need los of samples During posiion racking: robo s uncerainy is small don need as many samples MCL deermines sample size on he fly Compare P(x) and P(x z) (i.e., belief before and afer sensing) o deermine sample size The more divergence, he more samples ha are kep

69 More Movies from Dieer Fox

70 Summary Robo localizaion using probabilisic models Sensor models Moion models Markov localizaion: 3 differen ways of implemenaion