Presentation is loading. Please wait.

Presentation is loading. Please wait.

Advanced Mobile Robotics

Similar presentations


Presentation on theme: "Advanced Mobile Robotics"— Presentation transcript:

1 Advanced Mobile Robotics
Probabilistic Robotics: SLAM/FastSLAM Dr. Jizhong Xiao Department of Electrical Engineering CUNY City College

2 Probabilistic Robotics
SLAM/FastSLAM

3 SLAM Problem : Chicken or Egg
Fundamental problems for localization and mapping The task of SLAM is to build a map while estimating the pose of the robot relative to this map. Without a map, robot cannot localize itself Without knowing its location, robot cannot build a map Which needed to be done first? Localization or mapping?

4 SLAM Simultaneous Localization and Mapping
A robot is exploring an unknown, static environment. Given: The robot’s controls (U1:t) Observations of nearby features (Z1:t) Estimate: Map of features (m) Pose / Path of the robot (xt)

5 Why is SLAM a hard problem?
Uncertanties Error in pose Error in observation Error in mapping Error accumulated

6 Why is SLAM a hard problem?
SLAM: robot path and map are both unknown! Robot path error correlates errors in the map

7 Why is SLAM a hard problem?
Robot pose uncertainty In the real world, the mapping between observations and landmarks is unknown Picking wrong data associations can have catastrophic consequences Pose error correlates data associations

8 Data Association Problem
A data association is an assignment of observations to landmarks In general there are more than (n observations, m landmarks) possible associations Also called “assignment problem”

9 SLAM: Simultaneous Localization and Mapping
Full SLAM: Estimates Entire pose (x1:t) and map (m) Given Previous knowledge (Z1:t-1, U1:t-1) Current measurement (Zt, Ut) Estimates entire path and map!

10 Graphical Model of Full SLAM:

11 SLAM: Simultaneous Localization and Mapping
Online SLAM: Estimates Most recent pose (xt) and map (m) Given Previous knowledge (Z1:t-1, U1:t-1) Current measurement (Zt, Ut) Estimates most recent pose and map!

12 Graphical Model of Online SLAM:
Integrations typically done one at a time

13 SLAM with Extended Kalman Filter
Pre-requisites Maps are feature-based (landmarks) small number (< 1000) Assumption - Gaussian Noise Process only positive sightings No landmark = negative Landmark = positive

14 EKF-SLAM with known correspondences
Correspondence Data association problem Landmarks can’t be uniquely identified True identity of observed feature Correspondence variable (Cit) between feature (fit) and real landmark

15 EKF-SLAM with known correspondences
Signature Numerical value (average color) Characterize type of landmark (integer) Multidimensional vector (height and color)

16 EKF-SLAM with known correspondences
Similar development to EKF localization Diff  robot pose + coordinates of all landmarks Combined state vector (3N + 3) Online posterior

17 Iteration through measurements
Mean Motion update Covariance Test for new landmarks Initialization of elements Expected measurement Filter is updated Iteration through measurements

18 FastSLAM Jizhong Xiao Department of Electrical Engineering
The City College of New York

19 Particle Filters Represent belief by random samples
Estimation of non-Gaussian, nonlinear processes Sampling Importance Resampling (SIR) principle Draw the new generation of particles Assign an importance weight to each particle Resampling Typical application scenarios are tracking, localization, mapping …

20 Localization vs. SLAM A particle filter can be used to solve both problems Monte Carlo Localization: state space < x, y, > SLAM: state space < x, y, , map> for landmark maps = < l1, l2, …, lm> for grid maps = < c11, c12, …, c1n, c21, …, cnm> Disadvantage: The number of particles needed to represent a posterior grows exponentially with the dimension of the state space!

21 Some considerations of particle filter
Scale exponentially with dimension Large variables involved to mapping Convergence of particles Choice of Re-sampling Conditionally independent mapping error And more….

22 Dependencies Is there a dependency between the dimensions of the state space? If so, can we use the dependency to solve the problem more efficiently? In the SLAM context The map depends on the poses of the robot. We know how to build a map given the position of the sensor is known.

23 Mapping using Landmarks
. . . Landmark 1 observations Robot poses controls x1 x2 xt u1 ut-1 l2 l1 z1 z2 x3 z3 zt Landmark 2 x0 u0 Knowledge of the robot’s true path renders landmark positions conditionally independent

24 FastSLAM Principle Features are conditionally independent given the robot path Figure 13.3 The SLAM problem depicted as Bayes network graph. The robot moves from pose xt-1 to pose xt+2, driven by a sequence of controls. At each pose xt , it observes a nearby feature in the map m={m1, m2, m3}. This graphical network illustrates that the pose variables “separate” the individual features in the map from each other. If the poses are known, there remains no other path involving variables whose value is not known, between any two features in the map. This lack of a path renders the posterior of any two features in the map conditionally independent (given the poses).

25 Factored Posterior (Landmarks)
poses map observations & movements SLAM posterior Robot path posterior landmark positions Does this help to solve the problem? (Factorization first introduced by Murphy in 1999)

26 Factored Posterior Robot path posterior (localization problem)
Conditionally independent landmark positions

27 Rao-Blackwellization
This factorization is also called Rao-Blackwellization Given that the second term can be computed efficiently, particle filtering becomes possible!

28 Fast SLAM Rao-Blackwellized particle filtering based on landmarks [Montemerlo et al., 2002] Each landmark is represented by a Extended Kalman Filter Each particle therefore has to maintain M EKFs Particle #1 x, y,  Landmark 1 Landmark 2 Landmark M Particle #2 x, y,  Landmark 1 Landmark 2 Landmark M Particle N x, y,  Landmark 1 Landmark 2 Landmark M

29 FastSLAM Algorithm setup

30 Basic Step of FastSLAM

31 Rao-Blackwellization
Landmarks Grid-based map poses map observations movements

32 Factored SLAM Posterior
The mathematic expression of full SLAM posterior: After a long derivation (on page 442), it becomes

33 Advantage of factorization
FastSLAM estimates the path posterior using particle filter, estimate map feature using EKF Because of factorization: Maintain MN+1 filters (M – particles, N features) All filters are low-dimensional Separate EFK for each map features which makes update more efficient than EKF-SLAM Each particle possess its own sets of EKF’s Easy for data association

34 FastSLAM 1.0 Algorithm: N-th feature location (mean, variance) relative to the k-th particle Robot path estimation K-th Particle is denoted as: Step 1: Extend the path posterior by sampling new poses Step 2: Updating the observed feature estimate Step 3: Resembling Uses control ut to sample new robot pose xt Added into a temporary set of particle, along with the path of previous poses

35 FastSLAM 1.0 Algorithm: Step 1: Extend the path posterior by sampling new poses Step 2: Updating the observed feature estimate For n≠ct do not update For n=ct Perceptual model, Gaussian Previous belief, Gaussian Using EKF, approximate the measurement model g by Taylor expansion New mean Kalman Gain New covariance

36 FastSLAM 1.0 Algorithm: Step 1: Extend the path posterior by sampling new poses Step 2: Updating the observed feature estimate Step 3: Resembling Necessity: particles in the temporary set are not distributed according to the desired posterior, (i.e., poses xt driven by most recent control ut, without paying attention to measurement zt). Importance factor = Target distribution / Proposal distribution

37 Re-sampling by Importance Sampler
Target distribution, takes into account of measurement zt, along with the correspondence ct Samples cannot be drawn conveniently from the target distribution f. Instead, the importance sampler draws samples from the proposal distribution g, which has a simpler form. Proposal distribution of the path particles in the temporary set A sample of f is obtained by attaching the weight f/g to each sample x.

38 Re-sampling by Importance Sampler
Importance factor Observed feature Measurement model, Gaussian With covariance:

39 FastSLAM 1.0 Algorithm with known correspondence
See table 13.1 of text book on page 450

40 Weighting particle to Approximate Target Distribution

41 Resampling mismatch FastSLAM 1.0 problem: samples poses based on control ut only, but the accuracy of control is low. Proposal generates a large spectrum of samples, but only a small subset has high likelihood . After re-sampling, only particles within the ellipsoid “survive” with reasonably high likelihood

42 FastSLAM 2.0 Algorithm: FastSLAM 2.0 avoids the problem by sampling poses based on the measurement Zt in addition to control ut Step 1: Extend the path posterior by sampling new poses Step 2: Updating the observed feature estimate Step 3: Re-sampling See math on page 452~453

43 FastSLAM 2.0 Algorithm: FastSLAM 2.0 avoids the problem by sampling poses based on the measurement Zt in addition to control ut Extend the path posterior by sampling new poses Updating the observed feature estimate Re-sampling

44 FastSLAM Algorithm Particle filter approach to the SLAM problem
Maintain a set of particles Particles contain a sampled robot path and a map The features of the map are represented by own local Gaussian Map is created as a set of separate Gaussians Map features are conditionally independent given the path Factoring out the path (1 per particle) Map feature become independent Eliminates the need to maintain correlation among them

45 FastSLAM Algorithm Updating in FastSLAM
Sample new pose update the observed features Update can be performed online Solves both online and offline SLAM problem Instances Feature-based maps Grid-based algorithm

46 Features of Fast-SLAM Each particle can rely on its own, local data association decisions More robust data association decision on per-particle basis. Solves both full SLAM & on-line SLAM problems. FastSLAM 1.0 – less efficient in generating samples FastSLAM 2.0 – more efficient by improved proposal distribution, at the cost of mathematic complexity.

47 Unknown Data Association
Correspondence: maximizing the likelihood of the measurement zt

48 FastSLAM – Action Update
Landmark #1 Filter Particle #1 Landmark #2 Filter Particle #2 Particle #3

49 FastSLAM – Sensor Update
Landmark #1 Filter Particle #1 Landmark #2 Filter Particle #2 Particle #3

50 FastSLAM – Sensor Update
Particle #1 Weight = 0.8 Weight = 0.4 Weight = 0.1 Particle #2 Particle #3

51 FastSLAM - Video

52 FastSLAM Complexity O(N) O(N•log(M)) O(N•log(M))
Constant time per particle Update robot particles based on control ut-1 Incorporate observation zt into Kalman filters Resample particle set O(N•log(M)) Log time per particle O(N•log(M)) Log time per particle N = Number of particles M = Number of map features

53 Data Association Problem
Which observation belongs to which landmark? A robust SLAM must consider possible data associations Potential data associations depend also on the pose of the robot

54 Multi-Hypothesis Data Association
Data association is done on a per-particle basis Robot pose error is factored out of data association decisions

55 Per-Particle Data Association
Was the observation generated by the red or the blue landmark? P(observation|red) = 0.3 P(observation|blue) = 0.7 Two options for per-particle data association Pick the most probable match Pick an random association weighted by the observation likelihoods If the probability is too low, generate a new landmark

56 Results – Data Association

57 Results – Victoria Park
4 km traverse < 5 m RMS position error 100 particles Blue = GPS Yellow = Fast SLAM Dataset courtesy of University of Sydney

58 Results – Victoria Park
Dataset courtesy of University of Sydney

59 Grid-based SLAM Can we solve the SLAM problem if no pre-defined landmarks are available? Can we use the ideas of Fast SLAM to build grid maps? As with landmarks, the map depends on the poses of the robot during data acquisition If the poses are known, grid-based mapping is easy (“mapping with known poses”)

60 Rao-Blackwellization
poses map observations & movements

61 Rao-Blackwellization
poses map observations & movements SLAM posterior Robot path posterior Mapping with known poses

62 Rao-Blackwellization
This is localization, use MCL Use the pose estimate from the MCL part and apply mapping with known poses

63 A Graphical Model of Rao-Blackwellized Mapping
x z u 2 ... t 1 t-1

64 Rao-Blackwellized Mapping
Each particle represents a possible trajectory of the robot Each particle maintains its own map and updates it upon “mapping with known poses” Each particle survives with a probability proportional to the likelihood of the observations relative to its own map

65 Mapping with Known Poses
Mapping with known poses using laser range data

66 Particle Filter Example
3 particles map of particle 3 map of particle 1 map of particle 2

67 Problem Each map is quite big in case of grid maps
Since each particle maintains its own map Therefore, one needs to keep the number of particles small Solution: Compute better proposal distributions! Idea 1: Improve the pose estimate before applying the particle filter

68 Pose Correction Using Scan Matching
Maximize the likelihood of the i-th pose and map relative to the (i-1)-th pose and map map constructed so far robot motion current measurement To compute consistent maps, we apply a recursive scheme. At each point in time we compute the most likely position of the robot, given the map constructed so far. Based on the position hat l-t, we then extend the map an incorporate the scan obtained at time t.

69 Motion Model for Scan Matching
Raw Odometry Scan Matching

70 Mapping using Scan Matching

71 FastSLAM with Scan-Matching

72 Fast SLAM with Scan-Matching
Loop Closure

73 Fast SLAM with Scan-Matching
Map: Intel Research Lab Seattle

74 Intel Lab 15 particles four times faster than real-time P4, 2.8GHz
5cm resolution during scan matching 1cm resolution in final map

75 Intel Lab 15 particles Compared to Fast SLAM with Scan-Matching, the particles are propagated closer to the true distribution

76 Outdoor Campus Map 30 particles 250x250m2 1.75 km (odometry)
20cm resolution during scan matching 30cm resolution in final map

77 Fast SLAM with Improved Odometry
Scan-matching provides a locally consistent pose correction Pre-correct short odometry sequences using scan-matching and use them as input to Fast SLAM Fewer particles are needed, since the error in the input is smaller [Haehnel et al., 2003]

78 Comparison to Standard Fast SLAM
Same model for observations Odometry instead of scan matching as input Number of particles varying from 500 to 2,000 Typical result:

79 Graphical Model for Mapping with Improved Odometry
z k x 1 u' u k-1 ... k+1 2k-1 2k 2 n n·k (n+1)·k-1 n·k+1

80 Further Improvements Improved proposals will lead to more accurate maps They can be achieved by adapting the proposal distribution according to the most recent observations Flexible re-sampling steps can further improve the accuracy.

81 Conclusion The ideas of Fast SLAM can also be applied in the context of grid maps Utilizing accurate sensor observation leads to good proposals and highly efficient filters It is similar to scan-matching on a per-particle base The number of necessary particles and re-sampling steps can seriously be reduced Improved versions of grid-based Fast SLAM can handle larger environments than naïve implementations in “real time” since they need one order of magnitude fewer samples

82 Approximations for SLAM Problem
Local submaps [Leonard et al.99, Bosse et al. 02, Newman et al. 03] Sparse links (correlations) [Lu & Milios 97, Guivant & Nebot 01] Sparse extended information filters [Frese et al. 01, Thrun et al. 02] Thin junction tree filters [Paskin 03] Rao-Blackwellisation (FastSLAM) [Murphy 99, Montemerlo et al. 02, Eliazar et al. 03, Haehnel et al. 03]

83 Thank You

84 FastSLAM 1.0 Algorithm: N-th feature location (mean, variance) relative to the k-th particle Robot path estimation K-th Particle is denoted as: Extend the path posterior by sampling new poses Updating the observed feature estimate Resembling – difference of target and proposal distribution. – importance factor = Target distri. / Proposal distri. Added into a temporary set of particle, along with the path of previous poses For n≠ct do not update Represented by a Gaussian For n=ct Perceptual model


Download ppt "Advanced Mobile Robotics"

Similar presentations


Ads by Google