Probabilistic Robotics SLAM

Transcription

1 Probabilisic Roboics SLAM

2 The SLAM Problem SLAM is he process by which a robo builds a map of he environmen and, a he same ime, uses his map o compue is locaion Localizaion: inferring locaion given a map Mapping: inferring a map given a locaion SLAM: learning a map and locaing he robo simulaneously 2

3 The SLAM Problem SLAM is a chicken-or-egg problem: A map is needed for localizing a robo A pose esimae is needed o build a map Thus, SLAM is (regarded as) a hard problem in roboics 3

4 The SLAM Problem SLAM is considered one of he mos fundamenal problems for robos o become ruly auonomous A variey of differen approaches o address he SLAM problem have been presened Probabilisic mehods rule Hisory of SLAM daes back o he mid-eighies (sone-age of mobile roboics) 4

5 The SLAM Problem Given: The robo s conrols Relaive observaions Waned: Map of feaures Pah of he robo 5

6 Srucure of he Landmarkbased SLAM-Problem 6

7 SLAM Applicaions Indoors Undersea Space Underground 7

8 Represenaions Grid maps or scans [Lu & Milios, 97; Gumann, 98: Thrun 98; Burgard, 99; Konolige & Gumann, 00; Thrun, 00; Arras, 99; Haehnel, 0; ] Landmark-based [Leonard e al., 98; Caselanos e al., 99: Dissanayake e al., 200; Monemerlo e al., 2002; 8

9 Why is SLAM a hard problem? SLAM: robo pah and map are boh unknown Robo pah error correlaes errors in he map 9

10 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 0

11 SLAM: Simulaneous Localizaion and Mapping Full SLAM: Esimaes enire pah and map! p( :, m z:, u : ) Online SLAM: p, :, : ) m z u p( :, m z:, u: ) dd2... d Inegraions (marginalizaion) ypically done one a a ime ( Esimaes mos recen pose and map!

12 Graphical Model of Full SLAM: p( :, m z:, u : ) 2

13 Graphical Model of Online SLAM: p (, m z :, u: ) p( :, m z:, u: ) d d2... d 3

14 Graphical Model: Models "Moion model" "Observaion model" 4

15 Techniques for Generaing Consisen Maps Scan maching EKF SLAM Fas-SLAM Probabilisic mapping wih a single map and a poserior abou poses Mapping Localizaion Graph-SLAM, SEIFs 5

16 Scan Maching Maimize he likelihood of he i-h pose and map relaive o he (i-)-h pose and map. { } p( z, m [ ] ) p( u, ) argma curren measuremen map consruced so far m [ ] robo moion Calculae he map according o mapping wih known poses based on he poses and observaions. 6

17 Kalman Filer Algorihm. Algorihm Kalman_filer( µ -, Σ -, u, z ): 2. Predicion: 3. µ A µ Bu T 4. Σ A Σ A R 5. Correcion: T T 6. K ΣC ( C ΣC Q ) 7. µ µ K ( z C µ ) 8. Σ ( I K C ) Σ 9. Reurn µ, Σ 7

18 Eended Kalman Filer Previously Eended Kalman Filer line feaures deeced from range daa Now review eended Kalman Filer for landmark model Digression (wih slighly differen noaion) 8

19 Kalman Filer Componens (also known as: Way Too Many Variables ) Linear discree ime dynamic sysem (moion model) Sae Conrol inpu Process noise F B u G w Sae ransiion Conrol inpu funcion funcion funcion wih covariance Q Measuremen equaion (sensor model) Sensor reading Sae z Sensor funcion H n Noise inpu Sensor noise wih covariance R Noe:Wrie hese down!!!

20 A las! The Kalman Filer Propagaion (moion model): T T G Q G F F P P B u F / / / / Updae (sensor model): T T T P H S H P P P r K S H P K R H P H S z z r H z / / / / / / / / /

21 In words Propagaion (moion model): Updae (sensor model): - Sae esimae is updaed from sysem dynamics - Uncerainy esimae GROWS - Compue epeced value of sensor reading - Compue he difference beween epeced and rue - Compue covariance of sensor reading - Compue he Kalman Gain (how much o correc es.) - Muliply residual imes gain o correc sae esimae - Uncerainy esimae SHRINKS T T G Q G F F P P B u F / / / / T T T P H S H P P P r K S H P K R H P H S z z r H z / / / / / / / / /

22 Y G Linearized Moion Model for a Robo y v X ω The discree ime sae esimae (including noise) looks like his: y From a robo-cenric perspecive, he velociies look like his: From he global perspecive, he velociies look like his: ( V ( V w w ( ω w y V V ) δ cos ω ) ) δ sin δ V y 0 ω y V sin ω V cos Problem! We don know linear and roaional velociy errors. The sae esimae will rapidly diverge if his is he only source of informaion!

23 Linearized Moion Model for a Robo ~ ~ ~ y y y The indirec Kalman filer derives he pose equaions from he esimaed error: In order o linearize he sysem, he following small-angle assumpions are made: ~ ~ sin ~ cos Now, we have o compue he covariance mari Propagaion equaions.

24 Linearized Moion Model for a Robo V R R m m G W F X X w w y V V y ~ ~ 0 0 sin 0 cos ~ ~ ~ 0 0 cos 0 sin 0 ~ ~ ~ ω δ δ δ δ δ From he error-sae propagaion equaion, we can obain he Sae propagaion and noise inpu funcions F and G : From hese values, we can easily compue he sandard covariance propagaion equaion: T T G G Q F F P P / /

25 Sensor Model for a Robo wih a Perfec Map X Y y G L z n n n y z y L L L From he robo, he measuremen looks like his: From a global perspecive, he measuremen looks like: n n n y y z y L L L cos sin 0 sin cos The measuremen equaion is nonlinear and mus also be linearized!

26 Sensor Model for a Robo wih a Perfec Map Now, we have o compue he linearized sensor funcion. Once again, we make use of he indirec Kalman filer where he error in he reading mus be esimaed. In order o linearize he sysem, he following smallangle assumpions are made: ~ ~ sin ~ cos The final epression for he error in he sensor reading is: n n n y y y y y y y L L L L L L L ~ ~ ~ 0 0 ) ( sin ) ( cos cos sin ) ( cos ) ( sin sin cos ~ ~ ~

27 end of digression 27

28 EKF SLAM: Sae represenaion Localizaion 3 pose vecor 33 cov. mari SLAM Landmarks are simply added o he sae. Growing sae vecor and covariance mari! 28

29 , ), ( 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:(32n)-dimensional Gaussian Can handle hundreds of dimensions (E)KF-SLAM

30 EKF SLAM: Building he Map Filer Cycle, Overview:. Sae predicion (odomery) 2. Measuremen predicion 3. Observaion 4. Daa Associaion 5. Updae 6. Inegraion of new landmarks 30

31 EKF SLAM: Building he Map Sae Predicion Odomery: Robo-landmark crosscovariance predicion: (skipping ime inde k) 3

32 EKF SLAM: Building he Map Measuremen Predicion Global-o-local frame ransform h 32

33 EKF SLAM: Building he Map Observaion (,y)-poin landmarks 33

34 EKF SLAM: Building he Map Daa Associaion Associaes prediced measuremens wih observaion? (Gaing) 34

35 EKF SLAM: Building he Map Filer Updae The usual Kalman filer epressions 35

36 EKF SLAM: Building he Map Inegraing New Landmarks Sae augmened by Cross-covariances: 36

37 EKF-SLAM Map Correlaion mari 39

38 EKF-SLAM Map Correlaion mari 40

39 Vicoria Park Daa Se [couresy by E. Nebo] 42

40 Vicoria Park Daa Se Vehicle [couresy by E. Nebo] 43

41 Daa Acquisiion [couresy by E. Nebo] 44

42 SLAM [couresy by E. Nebo] 45

43 Map and Trajecory Landmarks Covariance [couresy by E. Nebo] 46

44 Landmark Covariance [couresy by E. Nebo] 47

45 Esimaed Trajecory [couresy by E. Nebo] 48

46 EKF SLAM Applicaion [couresy by John Leonard] 49

47 EKF SLAM Applicaion odomery esimaed rajecory [couresy by John Leonard] 50

48 Approimaions for SLAM Local submaps [Leonard e al.99, Bosse e al. 02, Newman e al. 03] Sparse links (correlaions) [Lu & Milios 97, Guivan & Nebo 0] Sparse eended informaion filers [Frese e al. 0, Thrun e al. 02] Thin juncion ree filers [Paskin 03] Rao-Blackwellisaion (FasSLAM) [Murphy 99, Monemerlo e al. 02, Eliazar e al. 03, Haehnel e al. 03] 5

49 EKF-SLAM Summary Quadraic in he number of landmarks: O(n 2 ) Convergence resuls for he linear case. Can diverge if nonlineariies are large! Have been applied successfully in large-scale environmens. Approimaions reduce he compuaional compleiy. 53