Introduction
Simultaneous Localization and Mapping (SLAM) is the problem of
using a mobile robot to build a map of its own environment.
$
\def\R{\mathcal{R}}
\def\R{\mathcal{R}}
\def\argmax{\operatorname{argmax}}
$
It's a localization problem in a much bigger state space!- Probabilistic mapping (without worrying about localization)
- SLAM based on particle filters
Why is SLAM hard?
Localization and mapping cannot be separated.- Localization usually requires a map against which to match the robot's observations.
- Mapping usually requires the robot to be localized, so the locations of observed obstacles can be recorded.
In addition, the potentially large errors in sensing and
motion remain important.Mapping with known robot locations
Mapping without localization
What does the mapping problem look like if localization is not an issue?
Occupancy grid representation:
\[
m_\ell = \begin{cases}
1 & \rm{if \: cell \:} \:\ell\: \rm{is\:obstacle} \\
0 & \rm{if \: cell \:} \:\ell\: \rm{is\:free}
\end{cases}
\]
We want the probability that each cell is occupied, given the robot's
position history and sensing history:
\[ P_{\ell,k} = P(m_\ell = 1 \mid x_1,\ldots,x_k, y_1,\ldots,y_k) \]
The indices $\ell$ and $k$ refer to location and time
respectively.The illustration here is an example of an occupancy grid.
Each cell $\ell$ is shaded according to $P_{\ell, k}$, with darker
colors showing higher obstacle probability. Notice that this map has
cells in each of three different probability levels.Updating occupancy grid probabilities
1. Initialization: Start with some prior distribution $P_0$: \[ P_{l, 0} = P_0 \] This could be an naive assumption (say, $P_0 = 0.5$) or from some other partial map. 2. Update, given a new $x_k$ and $y_k$: \[ P_{\ell,k} = \left[ 1 + \left(\frac{P(m_\ell = 0 \mid x_k, y_k)}{P(m_\ell = 1 \mid x_k, y_k)} \right) \left(\frac{1 - P_{\ell,k-1}}{P_{\ell,k-1}} \right) \right]^{-1} \] This depends on:- An observation model, describing the likelihood of a given cell being free or obstacle, given the robot's current location and its observation.
- The occupancy probability, given the history at the previous stage.
The update rule can be derived using using some reasonable
assumptions and some basic manipulations of probability. Details about
this process, including derivations of the update equation, are in
Choset Section 9.2.Observation model example
For an ultrasonic range sensor, the observation model depends on:- the measured distance,
- the angle between the occupancy grid cell and the sensor's emitter, and
- the distance from the occupancy grid cell to the sensor's emitter.
Occupancy Grid Mapping Examples
SLAM with a particle filter
SLAM as a localization problem
We can think of SLAM as a (passive, global, probabilistic) localization problem with a huge state space. Example:- $M = $ space of all maps
- $X = \mathbb{R}^2 \times M$
Have we already solved this?
We might be tempted to use standard localization methods such as particle filters with this state space to compute: \[P(x_1,\ldots,x_k, m \mid u_1,\ldots,u_k,y_1,\ldots,y_k)\] However, in practice, the dimension is too high! Bad news: For a robot moving in an $n \times n$ grid representation for $k$ steps, we get a $n^2+2k$ dimensional state space. (!)The number of particles needed to maintain an accurate
distribution over this space would be truly ridiculous.
Rao-Blackwellization
Big idea that makes particle-filter-based SLAM actually work:Given the path of the robot, we can
efficiently compute the most likely map.
Maintain a particles from the space of robot paths:
\[ P(x_1,\ldots,x_k \mid u_1,\ldots,u_k,y_1,\ldots,y_k) \]
Each particle is a single path for the robot:
\[ s_j = (x^j_1, \ldots, x^j_k) \]
Factoring of the state space into one part that we sample
(the path) and one part that we derive (the map) is
called Rao-Blackwellization.
Particle Filtering for SLAM
Rao-Blackwellized particle filter:- For each particle, compute the most likely map: \[ m(j, k) = \argmax_{m \in M} P(m \mid x^j_1,\ldots,x^j_k,y_1,\ldots,y_k) \]
- Use this map along with the observation model to compute the weight for each particle: \[ w_j = P(y_k \mid x^j_k, m(j,k)) \]
- Resample using these weights as in the standard particle filter.
SLAM Example
The figure shows the paths shown for all particles, but only
shows the map the highest-weight particle.