Simultaneous Localisation and Mapping. IAR Lecture 10 Barbara Webb

Transcription

1 Simuaneous Locaisaion and Mapping IAR Lecure 0 Barbara Webb

2 Wha is SLAM? Sar in an unknown ocaion and unknown environmen and incremenay buid a map of he environmen whie simuaneousy using his map o compue vehice ocaion = Simuaneous Locaisaion And Mapping Measuremen Posiion Landmark Acion measuremens & acions

3 So far we have been discussing he ocaisaion probem i.e. a map m is known a priori. From a sequence of conro acions U and measuremens Z we can infer he ocaions of he robo X.

4 Compemenary o ocaisaion is he mapping probem: If we knew he ocaion X of he robo (e.g. precise GPS) hen from he measuremens Z we coud infer he map M. E.g. represen environmen by a grid and esimae he (assumed independen) probabiiy ha each ocaion is occupied by an obsace. p( m z : : ) = p( mi z: : i ) Inverse sensor mode

5 Bu can we sove he chicken and egg probem? If we ony know he robo s posiion a 0 use he sequence of acions U and measuremens Z o infer boh he map M and he robo posiions X.

6 Bayesian SLAM

7 Bayesian SLAM Recursive fier for esimaing robo posiions and map Predicion (ime updae) Correcion (measuremen updae) Sensor mode Moion mode 0 0: 0: 0 0: 0: ) ( ) ( ) ( = d u z m P u P u z m P Esimae a previous ime sep Be( - m) ) ( ) ( ) ( 0 0: 0: 0 0: 0: u z m P m z P u z m P =η Be( m) Be( m) Be( m)

8 Bayesian SLAM Bayesian SLAM works because he error beween esimaed and rue andmark ocaion depends mosy on he error in he posiion esimae which impies error is correaed beween differen andmarks. This means knowedge of he reaive ocaion of andmarks can ony improve as more observaions are made. As a consequence accuracy of map and ocaion esimaes wi converge bounded ony by he quaiy of he possibe map.

9 = ) ( 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 θ θ θ θ θ θ θ θ θ θ θ θ L M O M M M M M L L L L L M Basic idea is simpy o incude he map as par of he sae o be esimaed hen appy mehods as before Map wih N andmarks:(3+n)-dimensiona Gaussian Can hande hundreds of dimensions (Eended) Kaman Fier SLAM

10 EKF-SLAM Map Correaion mari 0

11 EKF-SLAM Map Correaion mari

12 EKF-SLAM Map Correaion mari

13 a)-c) Pose uncerainy increases as robo moves Thus each successive andmark ocaion esimae is aso ess cerain Bu in (d) see firs andmark again Uncerainy of a andmark ocaions decreases Pose uncerainy aso decreases

14 EKF SLAM Appicaion

15 EKF SLAM Appicaion odomery esimaed rajecory

16 Parice Fier SLAM SLAM: sae space < y θ map> for andmark maps = < m > for grid maps = < c c c n c c nm > Probem: The number of parices needed o represen he esimae grows eponeniay wih he dimension of he sae space!

17 Souion: Facored Poserior (Landmarks) poses map observaions & movemens SLAM poserior Robo pah poserior andmark posiions

18 Mapping using Landmarks Landmark observaions Robo poses conros 0 u 0 z z u u u - z z Landmark Knowedge of he robo s rue pah renders andmark posiions condiionay independen

19 Facored Poserior Robo pah poserior (ocaizaion probem) Condiionay independen andmark posiions

20 Rao-Backweizaion This facorizaion is aso caed Rao-Backweizaion Given ha he second erm can be compued efficieny parice fiering becomes possibe. Parices represen he disribuion of possibe robo rajecories (he firs erm).

21 FasSLAM Rao-Backweized parice fiering based on andmarks 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

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

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

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