(Simultaneous) Localization & Mapping Matteo Matteucci

Transcription

1 Simuaneous Locaiaion & Mapping

2 A Two Layered Approach Map Lower Frequency Goa Posiion Trajecory Panning From where? Higher Frequency Trajecory Trajecory Foowing and Obsace Avoidance Sensors Moion Commands Curren Posiion Where am I?

3 Where Am I? To perform heir asks auonomous robos and unmanned vehices need To know where hey are e.g. Goba Posiioning Sysem To know he environmen map e.g. Geographica Insiues Maps These are no aways possibe or reiabe GNSS are no aways reiabe/avaiabe No a paces have been mapped Environmen changes dynamicay Maps need o be updaed

4 Represenaions Landmark-based [Leonard e a. 98; Caseanos e a. 99: Dissanayake e a. 00; Monemero e a. 00; ] Grid maps or scans [Lu & Miios 97; Gumann 98: Thrun 98; Burgard 99; Konoige & a. 00; Thrun 00; Arras 99; Haehne 0; ]

5 Locaiaion wih known map

6 Mapping wih known poses

7 Simuaneous Locaiaion and Mapping

8 Dynamic Bayes Nework Inference and Fu SLAM u u G G G 3 rajecory L L L 3 L 4 L 5 L 6 map Smoohing : p G : N Z: U :

9 Dynamic Bayes Nework Inference and Onine SLAM u u Moion Mode Sensor Mode G G G 3 pose L L L 3 L 4 L 5 L 6 map Fiering : p G N Z: U: p G: N Z: U: :

10 Techniques for Generaing Consisen Maps Severa echniques have been sudied o obain a consisen esimae of he join probabiiy of pose and map Scan maching EKF SLAM / UKF SLAM Fas-SLAM Parice fier based Probabiisic mapping wih a singe map and a poserior abou poses Mapping + Locaiaion Graph-SLAM SEIFs... We won see he a of hem! Le s sar wih he basics! ;-

11 Discaimer These sides have been heaviy inspired by he eaching maeria kindy provided wih he book: Probabiisic Roboics by Sebasian Thrun Dieer Fo and Wofram Burgard MIT Press 005 Pease refer o he origina source for a deeper anaysis and furher references on he opic

12 Bayes Fiers: Framework Given: Sream of observaions and acion daa u: d { u u } Sensor mode P. Acion mode P u. Prior probabiiy of he sysem sae P. We wan o compue: Esimae of he sae X of a dynamica sysem. The poserior of he sae is aso caed Beief: Be P u u

13 Markov Assumpion p 0 : : u : p p : : u: p u Underying Assumpions Saic word Independen noise Perfec mode no approimaion errors

14 d Be u P P Bayes Fiers u u P u u P Bayes = observaion u = acion = sae u u P Be Markov u u P P Markov d u u P u P P d u u P u u P P Toa prob. Markov d u P u P P

15 Bayes Fier: The Agorihm Be P P u Be d Agorihm Bayes_fier Be d : 0 If d is a percepua daa iem hen For a do For a do Ese if d is an acion daa iem u hen For a do Reurn Be Be ' P Be Be' Be ' Be ' Be ' P u ' Be ' d' How o represen such beief?

16 Bayes Fiers are Famiiar! Be P P u Be d You migh have me his fier aready if you had somehing o do wih: Kaman fiers Parice fiers Hidden Markov modes Dynamic Bayesian neworks Pariay Observabe Markov Decision Processes POMDPs Le have a coser ook a: Discree fiers Kaman fiers Parice fiers

17 Piecewise Consan Approimaion

18 Piecewise Consan Approimaion

19 Discree Bayes Fier Agorihm Agorihm Discree_Bayes_fier Bed : h=0 If d is a percepua daa iem hen For a do For a do Ese if d is an acion daa iem u hen For a do Reurn Be Be ' P Be Be' Be ' ' Be ' Be ' P u ' Be '

20 Impemenaion Tricks Beief updae upon sensory inpu and normaiaion ieraes over a ces When he beief is peaked e.g. during posiion racking avoid updaing irreevan pars. Do no updae enire sub-spaces of he sae space and monior wheher he robo is de-ocaied or no by considering ikeihood of observaions given he acive componens To updae he beief upon robo moions assumes a bounded Gaussian mode; reduces he updae from On o On. The updae by shifing he daa in he grid according o measured moion hen convove he grid using a Gaussian Kerne.

21 Grid-based Locaiaion

22 Tree-based Represenaion Fier compeiy is eponenia in he number of degrees of freedom i can be sped up by represening densiy using a varian of ocrees Efficien in space and ime Mui-resouion

23 Bayes Fiers are Famiiar! Be P P u Be d You migh have me his fier aready if you had somehing o do wih: Kaman fiers Parice fiers Hidden Markov modes Dynamic Bayesian neworks Pariay Observabe Markov Decision Processes POMDPs Le have a coser ook a: Discree fiers Kaman fiers Parice fiers

24 Bayes Fier Reminder Predicion Correcion be p u be d be p be Can we easiy compue hese inegras remind η is an inegra oo in cosed form for coninuos disribuions? Is here any coninuous disribuion for which his is possibe? NO! YES!

25 Gaussians : ~ e p N p - Univariae / / : ~ μ Σ μ Σ μ Σ e p Ν p d Muivariae

26 ~ ~ a b a N Y b ax Y N X Properies of Gaussians ~ ~ ~ N X p X p N X N X Univariae ~ ~ T A A B A N Y B AX Y N X ~ ~ ~ N X p X p N X N X Muivariae

27 Discree Time Kaman Fier Esimaes he sae of a discree-ime conroed process ha is governed by he inear sochasic difference equaion wih a measuremen A C B u A B C n n describes sae evoves from o - w/o conros or noise n describes how conro u changes he sae from o - k n ha describes how o map he sae o an observaion random variabes represening process and measuremen noise assumed o be independen and normay disribued wih covariance R and Q respecivey.

28 Linear Gaussian Sysems Iniia beief is normay disribued: be 0 N 0; 0 0 Dynamics are inear funcion of sae and conro pus addiive noise: A B u p u N ; A B u R

29 Linear Gaussian Sysems: Dynamics T T T R A A B u A be d B u A R B u A be N R B u A N d be u p be ep ep ; ~ ; ~

30 Linear Gaussian Sysems: Observaions Observaions are inear funcion of sae pus addiive noise: C Q C N p ; wih ep ep ; ~ ; ~ T T T T Q C C C K K C I C K be C Q C be N Q C N be p be

31 Kaman Fier Agorihm Agorihm Kaman_fier µ - Σ - u : Predicion: Correcion: Reurn µ Σ u A B T R A A T T Q C C C K C K C K I Highy efficien: Poynomia in measuremen dimensionaiy k and sae dimensionaiy n: Ok n Opima for inear Gaussian sysems Mos roboics sysems are noninear

32 Noninear Dynamic Sysems Mos reaisic roboic probems invove noninear funcions A B u C

33 Noninear Dynamic Sysems Mos reaisic roboic probems invove noninear funcions g u h

34 EKF: Firs Order Tayor Series Epansion Predicion: Correcion: G u g u g u g u g u g H h h h h h

35 EKF Lineariaion

36 EKF Lineariaion

37 EKF Lineariaion 3

38 EKF Agorihm Eended_Kaman_fierµ - Σ - u : Predicion: Correcion: Reurn µ Σ u g T R G G T T Q H H H K h K H K I u g G h H u A B T R A A T T Q C C C K C K C K I

39 Bayes Fiers are Famiiar! Be P P u Be d You migh have me his fier aready if you had somehing o do wih: Kaman fiers Parice fiers Hidden Markov modes Dynamic Bayesian neworks Pariay Observabe Markov Decision Processes POMDPs Le have a coser ook a: Discree fiers Kaman fiers Parice fiers

40 Parice Fiers Represen beief by random sampes Esimaion of non-gaussian noninear processes Mone Caro fier Surviva of he fies Condensaion Boosrap fier Parice fier Fiering: [Rubin 88] [Gordon e a. 93] [Kiagawa 96] Compuer vision: [Isard and Bake 96 98] Dynamic Bayesian Neworks: [Kanaawa e a. 95]

41 Imporance Resamping 57

42 Imporance Resamping w = f/g 58

43 Imporance Resamping wih smoohing 59

44 A Four Legged Eampe 60

45 Invoved Disribuions p p p 3 Waned: p 3

46 This is somehow easy! Draw sampes from p i using he deecion parameers and some noise : on f disribui Targe n k k n p p p p on g : disribui Samping p p p p w : weighs Imporance n k k n p p p p p g f p p p 3

47 Imporance Samping wih Resamping Weighed sampes Afer resamping

48 Locaiaion for AIBO robos

49 draw i from Be draw i from p i u Imporance facor for i : on disribui proposa on arge disribui i p Be u p Be u p p w d Be u p p Be Parice Fier Agorihm g f

50 Parice Fier Agorihm Agorihm parice_fiers - u - : For S 0 i n Generae new sampes For Sampe inde ji from he discree disribuion given by w - i j i Sampe from p u using and w i w S i p S i n w i w i i i i { w } / Compue imporance weigh Updae normaiaion facor Inser u Normaie weighs

51 p Be Be p w Be p Be Sensor Informaion: Imporance Samping

52 Robo Moion Be p u ' Be ' d '

53 p Be Be p w Be p Be Sensor Informaion: Imporance Samping

54 Robo Moion Be p u ' Be ' d '

55 Projec Minerva 7

56 Using Ceiing Maps for Locaiaion

57 Vision-based Locaiaion P h

58 Under a Ligh Measuremen : P :

59 Ne o a Ligh Measuremen : P :

60 Esewhere Measuremen : P :

61 Goba Locaiaion Using Vision

62 Sonar Sensor Mode Urasound Wave An US wave is sen by a ransducer Time of figh is measured Disance is compued from i Obsace coud be anywhere on he arc a disance D The space coser han D is ikey o be free. 78

63 Laser Range Finder Sensor Mode Lasers are definiey more accurae sensors 80 ranges over 80 up o 360 in some modes o 64 panes scanned 0-75 scans/second <cm range resouion Ma range up o m Probems ony wih mirrors gass and mae back. < 000 ~ 6000 ~ >

64 Beam Based Sensor Mode I The aser range finder mode describe each singe measuremen using Measuremen noise Unepeced obsaces 0 ep ma 0 ep ma P hi m b e b ep P unep m e 0 ep oherwise 80

65 Beam Based Sensor Mode II The aser range finder mode describe each singe measuremen using Random measuremen Ma range 0 ep ma 0 ep ma P rand m ma P ma m sma 8

66 Beam Based Sensor Mode III The aser range finder mode describe each singe measuremen using Which in pracice urns ou o be quie accurae! 8 rand ma unep hi rand ma unep hi m P m P m P m P m P T

67 Mone Caro Locaiaion wih Laser Sochasic moion mode Sar Proimiy sensor mode Laser sensor Sonar sensor

68 Sampe-based Locaiaion sonar

69

70

71

72

73

74

75

76

77

78

79

80

81

82

83

84

85

86

87 A Two Layered Approach Map Lower Frequency Goa Posiion From where? Sensors Trajecory Panning Trajecory Higher Frequency Trajecory Foowing and Obsace Avoidance Moion Commands Curren Posiion Where am I? 09

88 Occupancy from Sonar Reurn The mos simpe occupancy mode uses A D Gaussian for informaion abou occupancy Anoher D Gaussian for free space Sonar sensors presen severa issues A wide sonar cone creaes noisy maps Specuar mui-pah refecions generaes unreaisic measuremens Moravec 984 0

89 D Occupancy Grids A simpe D represenaion for maps Each ce is assumed independen Probabiiy of a ce of being occupied esimaed using Bayes heorem P B A PA P A B = P B A P A + P B ~A P~A Maps he environmen as an array of ces Usua ce sie 5 o 50cm Each ces hods he probabiiy of he ce o be occupied Usefu o combine differen sensor scans and differen sensor modaiies

90 Occupancy Grid Ce Updae

91 Occupancy by Log Odds 3

92 Mapping wih Raw Odomery wih known poses

93 Scan Maching Maimie he ikeihood of he i-h pose and map reaive o he i--h pose and map. ˆ arg ma p mˆ [ ] p u ˆ curren measuremen [ ] ˆ m map consruced so far robo moion Cacuae he map according o mapping wih known poses based on he poses and observaions.

94 Scan Maching Eampe

95 SLAM: Simuaneous Locaiaion and Mapping Fu SLAM: p : m : u : Esimaes enire pah and map! Onine SLAM: p m : u: p : m : u: dd... d Esimaes mos recen pose and map! Inegraions ypicay done one a a ime

96 SLAM: Simuaneous Locaiaion and Mapping Fu SLAM: p : m : u : Two famous eampe of his! Uses a inearied Gaussian probabiiy disribuion mode Onine SLAM: p Inegraions ypicay done one a a ime Esimaes enire pah and map! Eended Kaman Fier EKF SLAM Soves onine SLAM probem m : u: p : m : u: dd... d FasSLAM Soves fu SLAM probem Esimaes mos recen pose and map! Uses a samped parice fier disribuion mode

97 EKF-SLAM Map wih N andmarks:3+n-dimensiona Gaussian N N N N N N N N N N N y y y y y y y y y y y N y m Be u B A R B u A N u p ;

98 EKF-SLAM Map wih N andmarks:3+n-dimensiona Gaussian N N N N N N N N N N N y y y y y y y y y y y N y m Be C Q C N p ;

99 Bayes Fier: The Agorihm Be P P u Be d Agorihm Bayes_fier Be d : 0 If d is a percepua daa iem hen For a do For a do Ese if d is an acion daa iem u hen For a do Reurn Be Be ' P Be Be' Be ' Be ' Be ' P u ' Be ' d' correcion predicion

100 N N N N N N N N N N N y y y y y y y y y y y N y m Be Kaman Fier Agorihm Agorihm Kaman_fierµ - Σ - u : Predicion: Correcion: Reurn µ Σ ` u A B T R A A T T Q C C C K C K C K I

101 Cassica Souion The EKF Approimae he SLAM poserior wih a high-dimensiona Gaussian Bue pah = rue pah Red pah = esimaed pah Back pah = odomery

102 EKF-SLAM Map Correaion mari

103 EKF-SLAM Map Correaion mari

104 EKF-SLAM Map Correaion mari

105 Properies of KF-SLAM Linear Case Theorem: [Dissanayake e a. 00] The deerminan of any sub-mari of he map covariance mari decreases monoonicay as successive observaions are made. Theorem: In he imi he andmark esimaes become fuy correaed Are we happy abou his? Quadraic in he number of andmarks: On Convergence resus for he inear case. Can diverge if nonineariies are arge! Have been appied successfuy in arge-scae environmens. Approimaions reduce he compuaiona compeiy.

106 EKF Drawbacks EKF-SLAM works prey we bu... EKF-SLAM empoys inearied modes of noninear moion and observaion modes and so inheris many caveas. Compuaiona effor is demand because compuaion grows quadraicay wih he number of andmarks. Possibe souions Loca submaps [Leonard & a 99 Bosse & a 0 Newman & a 03] Sparse inks correaions [Lu & Miios 97 Guivan & Nebo 0] Sparse eended informaion fiers [Frese e a. 0 Thrun e a. 0] Thin juncion ree fiers [Paskin 03] Rao-Backweisaion FasSLAM [Murphy 99 Monemero e a. 0 Eiaar e a. 03 Haehne e a. 03] Represens noninear process and non-gaussian uncerainy Rao-Backweied mehod reduces compuaion

107 The FasSLAM Idea Fu SLAM In he genera case we have p m P P m However if we consider he fu rajecory X raher han he singe pose. p X m P X P m X In FasSLAM he rajecory X is represened by parices X i whie he map is represened by a facoriaion caed Rao-Backweied Fier P X P m X hrough parices using an EKF M i i j j P m X P m X

108 FasSLAM Formuaion Decoupe map of feaures from pose... Each parice represens a robo rajecory Feaure measuremens are correaed hough he robo rajecory If he robo rajecory is known a of he feaures woud be uncorreaed Trea each pose parice as if i is he rue rajecory processing a of he feaure measuremens independeny poses map observaions & movemens SLAM poserior Robo pah poserior Landmark posiions

109 Facored Poserior: Rao-Backweiaion Robo pah poserior ocaiaion probem Condiionay independen andmark posiions Dimension of sae space is drasicay reduced by facoriaion making parice fiering possibe

110 FasSLAM in Pracice Rao-Backweied parice fiering based on andmarks [Monemero e a. 00] Each andmark is represened by a Eended Kaman Fier EKF Each parice herefore has o mainain M EKFs Parice # y Landmark Landmark Landmark M Parice # y Landmark Landmark Landmark M Parice N y Landmark Landmark Landmark M

111 FasSLAM Acion Updae Parice # Landmark # Fier Landmark # Fier Parice # Parice #3

112 FasSLAM Sensor Updae Parice # Landmark # Fier Landmark # Fier Parice # Parice #3

113 FasSLAM Sensor Updae Parice # Weigh = 0.8 Parice # Weigh = 0.4 Parice #3 Weigh = 0.

114 FasSLAM Compeiy Updae robo parices based on conro u - ON Consan ime per parice Incorporae observaion ino Kaman fiers ON ogm Log ime per parice Resampe parice se ON ogm Log ime per parice ON ogm Log ime per parice N = Number of parices M = Number of map feaures

115 Fas-SLAM Eampe

116 Cogniive Roboics SLAM wih Lasers