400 likes | 832 Views
Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics) The SLAM Problem Given Robot controls Nearby measurements Estimate Robot state (position, orientation) Map of world features
E N D
Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)
The SLAM Problem • Given • Robot controls • Nearby measurements • Estimate • Robot state (position, orientation) • Map of world features
Indoors Undersea Underground Space SLAM Applications Images – Probabilistic Robotics
Outline • Sensors • SLAM • Full vs. Online SLAM • Marginal Slam • Multirobot marginal slam • Example Algorithms • Extended Kalman Filter (EKF) SLAM • FastSLAM (particle filter)
Types of Sensors • Odometry • Laser Ranging and Detection (LIDAR) • Acoustic (sonar, ultrasonic) • Radar • Vision (monocular, stereo etc.) • GPS • Gyroscopes, Accelerometers (Inertial Navigation) • Etc.
Sensor Characteristics • Noise • Dimensionality of Output • LIDAR- 3D point • Vision- Bearing only (2D ray in space) • Range • Frame of Reference • Most in robot frame (Vision, LIDAR, etc.) • GPS earth centered coordinate frame • Accelerometers/Gyros in inertial coordinate frame
A Probabilistic Approach • Notation:
Full vs. Online classical SLAM • Full SLAM calculates the robot pose over all time up to time t given the signal and odometry: • Online SLAM calculates the robot pose for the current time t
Full vs. Online SLAM Full SLAM Online SLAM
Classical Fast and EKF Slam • Robot Environment: • (1) N distances: mt ={d(xt,L1),….,d(xt,LN)}; m measures distances from landmarks at time t. • (2) Robot pose at time t: xt. • (3) (Scan) Measurements at time t: zt • Goal: Determine the poses x1:T given scans z1:t,odometry u1:T, and map measurements m.
EKF SLAM (Extended Kalman Filter) • As the state vector moves, the robot pose moves according to the motion function, g(ut,xt). This can be linearized into a Kalman Filter via: • The Jacobian J depends on translational and rotational velocity. This allows us to assume that the motion and hence distances are Gaussian. We can calculate the mean μ and covariance matrix Σ for the particle xt at time t.
Outline of EKF SLAM • By what preceded we assume that the map vectors m (measuring distance from landmarks) are independent multivariate normal: Hence we now have:
Conditional Independence • For constructing the weights associated with the classical fast slam algorithm, under moderate assumptions, we get: • We use the notation, • And calculate that:
Problems With EKF SLAM • Uses uni-modal Gaussians to model non-Gaussian probability density function
Particle Filters (without EKF) • The use of EKF depends on the odometry (u’s) and motion model (g’s) assumptions in a very nonrobust way and fails to allow for multimodality in the motion model. In place of this, we can use particle filters without assuming a motion model by modeling the particles without reference to the parameters.
Particle Filters (an alternative) • Represent probability distribution as a set of discrete particles which occupy the state space
Particle Filters • For constructing the weights associated with the classical fast slam algorithm, under moderate assumptions, we get: (for x’s simulated by q)
Resampling • Assign each particle a weight depending on how well its estimate of the state agrees with the measurements • Randomly draw particles from previous distribution based on weights creating a new distribution
Particle Filter Update Cycle • Generate new particle distribution • For each particle • Compare particle’s prediction of measurements with actual measurements • Particles whose predictions match the measurements are given a high weight • Resample particles based on weight
Particle Filter Advantages • Can represent multi-modal distributions
Problems with Particle Filters • Degeneracy: As time evolves particles increase in dimensionality. Since there is error at each time point, this evolution typically leads to vanishingly small interparticle (relative to intraparticle) variation. • We frequently require estimates of the ‘marginal’ rather than ‘conditional’ particle distribution. • Particle Filters do not provide good methods for estimating particle features.
Marginal versus nonmarginal Particle Filters • Marginal particle filters attempt to update the X’s using their marginal (posterior) distribution rather than their conditional (posterior) distribution. The update weights take the general form,
Marginal Particle update • We want to update by using the old weights rather than conditioning on the old particles.
Marginal Particle Filters • We specify the proposal distribution ‘q’ via:
Marginal Particle Algorithm • (1) • (2) Calculate the importance weights:
Updating Map features in the marginal model • Up to now, we haven’t assumed any map features. Let θ={θt} denote the e.g., distances of the robot at time t from the given landmarks. We then write • for the probability associated with scan Zt given the position x1:t. • We’d like to update θ. This should be based, not on the gradient , but rather on the gradient, .
Taking the ‘right’ derivative • The gradient, is highly non-robust; we are essentially taking derivatives of noise. • By contrast, the gradient, is robust and represents the ‘right’ derivative.
Estimating the Gradient of a Map • We have that,
Simplification • We can then show for the first term that:
Simplification II • For the second term, we convert into a discrete sum by defining ‘derivative weights’ • And combining them with the standard weights.
Estimating the Gradient • We can further write that:
Gradient (continued) • We can therefore update the gradient weights via:
Parameter Updates • We update θ by:
Normalization • The β’s are normalized differently from the w’s. In effect we put: • And then compute that:
The Bayesian Viewpoint • Retain a posterior sample of θ at time t-1. • Call this (i=1,…,I) • At time t, update this sample:
Multi Robot Models • Write for the poses and scan statistics for the r robots. • At each time point the needed weights have r indices: • We also need to update the derivative weights – the derivative is now a matrix derivative.
Multi-Robot SLAM • The parameter is now a matrix (with time being the row values and robot index being the column. Updates depend on derivatives with respect to each timepoint and with respect to each robot.