Roland Siegwar Margaria Chli Paul Furgale Marco Huer Marin Rufli Davide Scaramuzza ETH Maser Course: 151-0854-00L Auonomous Mobile Robos Localizaion II
ACT and SEE For all do, (predicion updae / ACT), (measuremen updae / SEE) endfor Reurn Localizaion II
3 Map Represenaion Coninuous Line-Based a) Archiecure map b) Represenaion wih se of finie or infinie lines Localizaion II
4 Map Represenaion Eac cell decomposiion Eac cell decomposiion - Polygons Localizaion II
5 Map Represenaion Approimae cell decomposiion Fied cell decomposiion Narrow passages disappear Localizaion II
6 Map Represenaion Adapive cell decomposiion Eercise: how do we implemen an adapive cell decomposiion algorihm? Localizaion II
8 Map Represenaion Topological map A opological map represens he environmen as a graph wih nodes and edges. Nodes correspond o spaces Edge correspond o physical connecions beween nodes Topological maps lack scale and disances, bu opological relaionships (e.g., lef, righ, ec.) are manained node (locaion) edge (conneciviy) Localizaion II
9 Map Represenaion Topological map London underground map Localizaion II
13 Probabilisic Map Based Localizaion Probabilisic Map Based Localizaion Localizaion II
14 Soluion o he probabilisic localizaion problem A probabilisic approach o he mobile robo localizaion problem is a mehod able o compue he probabiliy disribuion of he robo configuraion during each Acion (ACT) and Percepion (SEE) sep. The ingrediens are: 1. The iniial probabiliy disribuion p ( ) 0. The saisical error model of he propriocepive sensors (e.g. wheel encoders) 3. The saisical error model of he eerocepive sensors (e.g. laser, sonar, camera) 4. Map of he environmen (If he map is no known a priori hen he robo needs o build a map of he environmen and hen localize in i. This is called SLAM, Simulaneous Localizaion And Mapping) Localizaion II
18 Illusraion of probabilisic bap based localizaion Iniial probabiliy disribuion p ( ) 0 p z ) ( Percepion updae bel( ) p( z ) bel( ) Acion updae p z ) ( Percepion updae bel( ) p( z ) bel( ) Localizaion II
19 Illusraion of probabilisic bap based localizaion Iniial probabiliy disribuion p ( ) 0 p z ) ( Percepion updae bel( ) p( z ) bel( ) Acion updae p z ) ( Percepion updae bel( ) p( z ) bel( ) Localizaion II
0 Illusraion of probabilisic bap based localizaion Iniial probabiliy disribuion p ( ) 0 p z ) ( Percepion updae bel( ) p( z ) bel( ) Acion updae p z ) ( Percepion updae bel( ) p( z ) bel( ) Localizaion II
1 Illusraion of probabilisic bap based localizaion Iniial probabiliy disribuion p ( ) 0 p z ) ( Percepion updae bel( ) p( z ) bel( ) Acion updae p z ) ( Percepion updae bel( ) p( z ) bel( ) Localizaion II
Markov Localizaion Probabilisic Map Based Localizaion: Markov Localizaion Localizaion II
3 Markov localizaion Markov localizaion uses a grid space represenaion of he robo configuraion. For all do, (predicion updae), (measuremen updae) endfor Reurn Localizaion II
4 Markov localizaion Le us discreize he configuraion space ino 10 cells Suppose ha he robo s iniial belief is a uniform disribuion from 0 o 3. Observe ha all he elemens were normalized so ha heir sum is 1. Localizaion II
5 Markov localizaion Iniial belief disribuion Acion phase: Le us assume ha he robo moves forward wih he following saisical model This means ha we have 50% probabiliy ha he robo moved or 3 cells forward. Considering wha he probabiliy was before moving, wha will he probabiliy be afer he moion? Localizaion II
6 Markov localizaion Acion updae The soluion is given by he convoluion (cross correlaion) of he wo disribuions,, * Localizaion II
8 Markov localizaion Percepion updae Le us now assume ha he robo uses is onboard range finder and measures he disance from he origin. Assume ha he saisical error model of he sensors is: This plo ells us ha he disance of he robo from he origin can be equally 5 or 6 unis. Wha will he final robo belief be afer his measuremen? The answer is again given by he Bayes rule:, Localizaion II
9 Markov Localizaion Case Sudy Grid Map Eample : Museum Laser scan 1 Couresy of W. Burgard Localizaion II
30 Markov Localizaion Case Sudy Grid Map Eample : Museum Laser scan Couresy of W. Burgard Localizaion II
31 Markov Localizaion Case Sudy Grid Map Eample : Museum Laser scan 3 Couresy of W. Burgard Localizaion II
3 Markov Localizaion Case Sudy Grid Map Eample : Museum Laser scan 13 Couresy of W. Burgard Localizaion II
33 Markov Localizaion Case Sudy Grid Map Eample : Museum Laser scan 1 Couresy of W. Burgard Localizaion II
34 Kalman filer Localizaion Probabilisic Map Based Localizaion: Kalman Filer Localizaion Localizaion II
35 Kalman filer Localizaion Assumpions and properies Assumpions Linear or linearizable sysem Robo belief, moion model, and measuremen model are affeced by whie Gaussian noise Oucome Guaraneed o be opimal Only μ and Σ are updaed during he acion and percepion updaes Localizaion II
37 Kalman Filer Localizaion Illusraion Acion (ACT) Percepion (SEE) Localizaion II
38 Inroducion o Kalman filer heory A Gaussian disribuion is repsened only by is firs and second momens: mean μ and variance σ and is indicaed by N(μ,σ ) When he robo configuraion is a vecor, he disribuion is a mulivariae Gaussian represened by a mean vecor μ and a covariance mari Σ Localizaion II
40 Inroducion o Kalman filer heory Applying he heorem of oal probabiliy Le 1, be wo random variables which are Independen and Normally disribued Le y be a funcion of 1, Wha will he disribuion of y be? Localizaion II
41 Inroducion o Kalman filer heory Applying he heorem of oal probabiliy The answer is simple if f is linear If 1, are independen and normal, he oupu is also a Gaussian wih If 1, are vecors wih covariances Σ 1, Σ respecively, hen Localizaion II
47 Inroducion o Kalman filer heory Applying he Bayes rule bel( ) p( z ) bel( ) Here, we wish o demonsrae ha he produc of wo Gaussian funcions is sill a Gaussian Le now q denoe he posiion of he robo. Le p 1 (q) be he robo belief resuling from he Acion updae (i.e., ) p( z ) Le p (q) be he robo belief from he observaion (i.e., ) bel ( ) We wish o show ha p 1 and p are Gaussian funcions, heir produc is also a Gaussian Localizaion II
48 Inroducion o Kalman filer heory Applying he Bayes rule By formalizing his, we wan o show ha if we have hen heir produc is also Gaussian p1( q) p( q) N( q, ) Addiionally, we wan o find an epression of he mean value and variance of he new Gaussian as a funcion of he mean values and variances of he inpu variables Localizaion II
49 Inroducion o Kalman filer heory Applying he Bayes rule p1( q) p( q) From he produc of wo Gaussians, we obain p ( ) p ( q) 1 q Localizaion II
50 Inroducion o Kalman filer heory Applying he Bayes rule p1( q) p( q) From he produc of wo Gaussians, we obain p ( ) p ( q) 1 q As we can see, he argumen of his eponenial is quadraic in q, hence is a Gaussian. We now need o deermine is mean value and variance ha allow us o rewrie his eponenial in he form Localizaion II
51 Inroducion o Kalman filer heory Applying he Bayes rule By rearranging he eponenial, we ge Localizaion II
5 Inroducion o Kalman filer heory Applying he Bayes rule Where he mean value q can be wrien as Localizaion II
53 Inroducion o Kalman filer heory Applying he Bayes rule Where he mean value q can be wrien as Localizaion II
54 Inroducion o Kalman filer heory Applying he Bayes rule Where he mean value q can be wrien as And he variance can be wrien as Localizaion II
55 Inroducion o Kalman filer heory Applying he Bayes rule Where he mean value q can be wrien as And he variance can be wrien as Localizaion II
56 Inroducion o Kalman filer heory Applying he Bayes rule By rearranging he erms, he epressions of he mean value and variance can also be wrien as Kalman gain The resuling variance is smaller han he inpu variances. Thus, he uncerainy of he posiion esimae has shrunk as a resul of he observaion Even poor measuremens will only increase he precision of he esimae. This is a resul ha we epec based on informaion heory. Localizaion II
Inroducion o Kalman filer heory Equaions applied o mobile robos One-Dimenional Case N-Dimensional Case Localizaion II 59 ), ( 1 u f 1 1 u u f f ) ( z z 4 z Acion Updae (or Predicion Updae) Percepion Updae (or Measuremen Updae) Acion Updae (or Predicion Updae) Percepion Updae (or Measuremen Updae) ), ( 1 u f T u u T F F Q F F P P 1 ) ( ) ( 1 o R P P P R P P P P ) ( 1 NB: The new mean value is closer o he one of he wo esimaes ha has smaller uncerainy The new uncerainy is smaller han he wo iniial uncerainies
Inroducion o Kalman filer heory Equaions applied o mobile robos One-Dimenional Case N-Dimensional Case Localizaion II 60 ), ( 1 u f 1 1 u u f f ) ( z z 4 z Acion Updae (or Predicion Updae) Percepion Updae (or Measuremen Updae) Acion Updae (or Predicion Updae) Percepion Updae (or Measuremen Updae) ), ( 1 u f T u u T F F Q F F P P 1 NB: The new mean value is closer o he one of he wo esimaes ha has smaller uncerainy The new uncerainy is smaller han he wo iniial uncerainies K T K K P P R) ( -1 P P K ) ( z P R
5 88 Kalman Filer Localizaion Markov versus Kalman localizaion Markov PROS localizaion saring from any unknown posiion recovers from ambiguous siuaion Kalman PROS Tracks he robo and is inherenly very precise and efficien CONS However, o updae he probabiliy of all posiions wihin he whole sae space a any ime requires a discree represenaion of he space (grid). The required memory and calculaion power can hus become very imporan if a fine grid is used. CONS If he uncerainy of he robo becomes o large (e.g. collision wih an objec) he Kalman filer will fail and he posiion is definiively los Localizaion II