Introduction to Mobile Robotics Summary

Transcription

1 Inroducion o Mobile Roboics Summary Wolfram Burgard Cyrill Sachniss Maren Bennewiz Diego Tipaldi Luciano Spinello

2 Probabilisic Roboics 2

3 Probabilisic Roboics Key idea: Eplici represenaion of uncerainy using he calculus of probabiliy heory Percepion = sae esimaion Acion = uiliy opimizaion 3

4 4 Bayes Formula evidence prior likelihood = = = = y P P y P y P P y P y P y P y P

5 Simple Eample of Sae Esimaion Suppose a robo obains measuremen z Wha is Popenz? 5

6 Causal vs. Diagnosic Reasoning Popenz is diagnosic. Pzopen is causal. Ofen causal knowledge is easier o obain. coun frequencies! Bayes rule allows us o use causal knowledge: P open z = P z open P open P z 6

7 7 = d Bel u P z P η Bayes Filers u z u P u z u z P =η Bayes z = observaion u = acion = sae z u z u P Bel = Markov u z u P z P =η Markov = d u z u P u P z P η = d u z u P u z u P z P η Toal prob. Markov = d z z u P u P z P η

8 Bayes Filers are Familiar! Bel = η P z P u Bel d Kalman filers Paricle filers Hidden Markov models Dynamic Bayesian neworks Parially Observable Markov Decision Processes POMDPs 8

9 Sensor and Moion Models P z m P ' u 9

10 Moion Models Robo moion is inherenly uncerain. How can we model his uncerainy? 0

11 Probabilisic Moion Models To implemen he Bayes Filer we need he ransiion model p u. The erm p u specifies a poserior probabiliy ha acion u carries he robo from o.

12 Typical Moion Models 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. 2

13 Odomery Model Robo moves from yθ o ' y' θ '. Odomery informaion. u = δ δ δ ro ro2 rans δ rans = 2 ' + y' y δ = aan2 y' y ' θ δ ro = θ θ δ ro2 ' ro 2 δ ro2 yθ δ ro δ rans ' y' θ ' 3

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

15 Beam-based Sensor Model Scan z consiss of K measuremens. z = { z z2... z K Individual measuremens are independen given he robo posiion. } P z m = K k = P z k m 5

16 Beam-based Proimiy Model Measuremen noise Unepeced obsacles 0 z ep z ma 0 z ep z ma P hi z m = η 2πb e z z 2 b 2 ep P unep z m = λz η λ e z < z ep 0 oherwise 6

17 Beam-based Proimiy Model Random measuremen Ma range 0 z ep z ma 0 z ep z ma P rand z m = η z ma P ma z m = η z small 7

18 8 Resuling Miure Densiy = rand ma unep hi rand ma unep hi m z P m z P m z P m z P m z P T α α α α How can we deermine he model parameers?

19 Bayes Filer in Roboics 9

20 Bayes Filers in Acion Discree filers Kalman filers Paricle filers 20

21 Discree Filer The belief is ypically sored in a hisogram / grid represenaion To updae he belief upon sensory inpu and o carry ou he normalizaion one has o ierae over all cells of he grid 2

22 Piecewise Consan 22

23 Kalman Filer Opimal for linear Gaussian sysems! Mos roboics sysems are nonlinear! Polynomial in measuremen dimensionaliy k and sae dimensionaliy n: Ok n 2 23

24 Eended Kalman Filer Performs a linearizaion in each sep No opimal Can diverge if nonlineariies are large! Works surprisingly well even when all assumpions are violaed! Same compleiy han he KF 24

25 Paricle Filer Basic principle Se of sae hypoheses paricles Survival-of-he-fies Paricle filers are a way o efficienly represen non-gaussian disribuion 25

26 Mahemaical Descripion Se of weighed samples Sae hypohesis Imporance weigh The samples represen he poserior 26

27 Paricle Filer Algorihm in Brief Sample he ne generaion for paricles using he proposal disribuion Compue he imporance weighs : weigh = arge disribuion / proposal disribuion Resampling: Replace unlikely samples by more likely ones 27

28 Imporance Sampling Principle We can even use a differen disribuion g o generae samples from f By inroducing an imporance weigh w we can accoun for he differences beween g and f w = f / g f is ofen called arge g is ofen called proposal Pre-condiion: f>0 à g>0 28

29 Paricle Filer Algorihm. Algorihm paricle_filer S - u - z : 2. S 3. For i = n Generae new samples 4. Sample inde ji from he discree disribuion given by w - i j i 5. Sample from p uusing and i i 6. w = p z Compue imporance weigh i 7. η = η + w Updae normalizaion facor i i 8. S = S { < w > } Inser 9. For = η = 0 i = n u i i 0. w = w /η Normalize weighs 29

30 30 draw i - from Bel - draw i from p i - u - Imporance facor for i : disribuion proposal arge disribuion i z p Bel u p Bel u p z p w = = η = d Bel u p z p Bel η Paricle Filer Algorihm

31 Resampling W n- w n w w 2 W n- w n w w 2 w 3 w 3 Roulee wheel Binary search n log n Sochasic universal sampling Sysemaic resampling Linear ime compleiy Easy o implemen low variance 3

32 MCL Eample 32

33 Mapping 33

34 Why Mapping? Learning maps is one of he fundamenal problems in mobile roboics Maps allow robos o efficienly carry ou heir asks allow localizaion Successful robo sysems rely on maps for localizaion pah planning aciviy planning ec 34

35 Occupancy Grid Maps Discreize he world ino equally spaced cells Each cells sores he probabiliy ha he corresponding area is occupied by an obsacle The cells are assumed o be condiionally independen If he pose of he robo is know mapping is easy 35

36 Updaing Occupancy Grid Maps Updae he map cells using he inverse sensor model Bel [ y ] [ y ] P m z u m + [ y ] P m z u [ y ] P m [ y ] Or use he log-odds represenaion [ y ] m [ ] m = y P m Bel [ y ] [ y ] [ y ] [ y ] B m = log odds m z u B m : = logodds m [ y logodds m ] P odds : = [ y ] P + B m Bel 36

37 Reflecion Probabiliy Maps Value of ineres: Preflecsy For every cell coun hisy: number of cases where a beam ended a <y> missesy: number of cases where a beam passed hrough <y> Bel m [ y ] = his his y y + misses y 37

38 SLAM 38

39 The SLAM Problem A robo is eploring an unknown saic environmen. Given: The robo s conrols Observaions of nearby feaures Esimae: Map of feaures Pah of he robo 39

40 Chicken-or-Egg SLAM is a chicken-or-egg problem A map is needed for localizing a robo A good pose esimae is needed o build a map Thus SLAM is regarded as a hard problem in roboics A variey of differen approaches o address he SLAM problem have been presened Probabilisic mehods ouperform mos oher echniques 40

41 SLAM: Simulaneous Localizaion and Mapping Full SLAM: p : m z: u : Esimaes enire pah and map! Online SLAM: p m z : u: = p : m z: u: dd2... d Inegraions ypically done one a a ime Esimaes mos recen pose and map! 4

42 Why is SLAM a hard problem? Robo pose uncerainy In he real world he mapping beween observaions and landmarks is unknown Picking wrong daa associaions can have caasrophic consequences Pose error correlaes daa associaions 42

43 43!!!!!!!!!! " # $ $ $ $ $ $ $ $ $ $ % &!!!!!!!!! " # $ $ $ $ $ $ $ $ $ % & = N N N N N N N N N N N l l l l l l yl l l l l l l l yl l l l l l l l yl l l l l y yl yl yl y y y l l l y N l l l y m Bel θ θ θ θ θ θ θ θ θ θ θ θ Map wih N landmarks:3+2n-dimensional Gaussian Can handle hundreds of dimensions EKF-SLAM

44 EKF-SLAM Map Correlaion mari 44

45 EKF-SLAM Map Correlaion mari 45

46 EKF-SLAM Map Correlaion mari 46

47 FasSLAM Use a paricle filer for map learning Problem: he map is high-dimensional Soluion: separae he esimaion of he robo s rajecory from he one of he map of he environmen This is done by means of a facorizaion in he SLAM poserior ofen called Rao-Blackwellizaion 47

48 Rao-Blackwellizaion poses map observaions & movemens SLAM poserior Robo pah poserior Mapping wih known poses Facorizaion firs inroduced by Murphy in

49 Rao-Blackwellized Mapping Each paricle represens a possible rajecory of he robo Each paricle mainains is own map and updaes i upon mapping wih known poses Each paricle survives wih a probabiliy proporional o he likelihood of he observaions relaive o is own map 49

50 FasSLAM Rao-Blackwellized paricle filering based on landmarks Each landmark is represened by a 22 Eended Kalman Filer EKF Each paricle herefore has o mainain M EKFs Paricle # y θ Landmark Landmark 2 Landmark M Paricle #2 y θ Landmark Landmark 2 Landmark M Paricle N y θ Landmark Landmark 2 Landmark M 50

51 Grid-based FasSLAM Similar ideas can be used o learn grid maps To obain a pracical soluion an efficienly compuable informed proposal disribuion is needed Idea: in he SLAM poserior he observaion model dominaes he moion model given an accurae sensor 5

52 Proposal Disribuion Approimae his equaion by a Gaussian: maimum repored by a scan macher Gaussian approimaion Sampled poins around he maimum Draw ne generaion of samples 52

53 Typical Resuls 53

54 Robo Moion 54

55 Robo Moion Planning Laombe 99: eminenly necessary since by definiion a robo accomplishes asks by moving in he real world. Goals: Collision-free rajecories. Robo should reach he goal locaion as fas as possible. 55

56 Two Challenges Calculae he opimal pah aking poenial uncerainies in he acions ino accoun Quickly generae acions in he case of unforeseen objecs 56

57 Classic Two-layered Archiecure Planning low frequency map sub-goal Collision Avoidance high frequency sensor daa moion command robo 57

58 Muli-Robo Eploraion Given: Unknown environmen Team of robos Task: Coordinae he robos o efficienly learn a complee map of he environmen Compleiy: NP-hard for single robos in known graph-like environmens Eponenial in he number of robos 58

59 Levels of Coordinaion No echange of informaion Implici coordinaion: Sharing a join map [Yamauchi e.al 98] Communicaion of he individual maps and poses Cenral mapping sysem Eplici coordinaion: Deermine beer arge locaions o disribue he robos Cenral planner for arge poin assignmen 59

60 The Coordinaion Algorihm informal. Deermine he fronier cells. 2. Compue for each robo he cos for reaching each fronier cell. 3. Choose he robo wih he opimal overall evaluaion and assign he corresponding arge poin o i. 4. Reduce he uiliy of he fronier cells visible from ha arge poin. 5. If here is one robo lef go o 3. 60

61 Informaion Gain-based Eploraion SLAM is ypically passive because i consumes incoming sensor daa Eploraion acively guides he robo o cover he environmen wih is sensors Eploraion in combinaion wih SLAM: Acing under pose and map uncerainy Uncerainy should/needs o be aken ino accoun when selecing an acion Key quesion: Where o move ne? 6

62 Muual Informaion The muual informaion I is given by he reducion of enropy in he belief acion o be carried ou uncerainy of he filer uncerainy of he filer afer carrying ou acion a

63 Inegraing Over Observaions Compuing he muual informaion requires o inegrae over poenial observaions poenial observaion sequences

64 Inegral Approimaion The paricle filer represens a poserior abou possible maps map of paricle map of paricle 2 map of paricle 3

65 Inegral Approimaion The paricle filer represens a poserior abou possible maps Simulae laser measuremens in he maps of he paricles measuremen sequences simulaed in he maps likelihood paricle weigh

66 Summary on Informaion Gainbased Eploraion A decision-heoreic approach o eploraion in he cone of RBPF-SLAM The approach uilizes he facorizaion of he Rao-Blackwellizaion o efficienly calculae he epeced informaion gain Reasons abou measuremens obained along he pah of he robo Considers a reduced acion se consising of eploraion loop-closing and placerevisiing acions 66

67 The Eam is Approaching This lecure gave a shor overview over he mos imporan opics addressed in his course For he eam you need o know a leas he basic formulas e.g. Bayes filer MCL eqs. Rao-Blackwellizaion enropy Good luck for he eam! 67