Simultaneous Localization and Mapping

Apr. 16 2016 by wacoder Tags Bayesian Estimation

The simultaneous localization and mapping (SLAM) problem asks if it is possible for a mobile robot to be placed at unknown location in an unknown environment and for the robot to incrementally build a consistent map of this environment while simultaneously determining its location within this map.

Formulation and Structure of the SLAM Problem1

Preliminaries

Consider a mobile robot moving through an environment taking relative observation of a number of unknown landmarks using a sensor located on the robot. At a time instant k, the following quantities are defined.

\(x_k\) the state vector describing the location and orientation of the vehicle
\(u_k\) the control vector, applied at time \(k-1\) to drive the vehicle to a state \(x_k\) at time \(k\)
\(m_i\) a vector describing the location of the \(i\)th landmark whose true location is assumed time invariant
\(z_{ik}\) an observation taken from the vehicle of the location of \(i\)th landmark at time \(k\)
\(\textbf{X}_{0:k}=\{x_0, x_1,\dots,x_k\}\) the history of vehicle locations
\(\textbf{U}_{0:k}=\{u_0, u_1,\dots,u_k\}\) the history of control inputs
\(\textbf{m}=\{m_1, m_2,\dots,m_n\}\) the set of all landmarks
\(\textbf{Z}_{0:k}=\{z_1, z_2,\dots,z_k\}\) the set of all landmark observations

In probabilistic form, this simultaneous localization and mapping (SLAM) problem requires that the probability distribution

\begin{equation} P(x_k,m|\textbf{Z}_{0:k}, \textbf{U}_{0:k}, x_0) \end{equation}
be computed for all time \(k\). In general, a recursive solution to the SLAM problem is desirable. Starting with an estimate for the distribution \(P(x_{k-1}, m|\textbf{Z}_{0:k-1}, \textbf{U}_{0:k-1})\) at time \(k-1\), the joint posterior, following a control \(u_k\) and observation \(z_k\), is computed using Bayes theorem.

The observation model describes the probablity of making an observations \(z_k\) when the vehicle location and landmark locations are known and is generally described in the form

\begin{equation}\label{Eqn:ob_model} P(z_k|x_k,m) \end{equation}

The motion model for the vehicle can be described in terms of probablity distribution on state transitions in the form

\begin{equation}\label{Eqn:mo_model} P(x_k|x_{k-1},u_k) \end{equation}

The SLAM algorithm is now implemented in a standard two-step recursive prediction (time-update) correction (measurement-update) form:

Time-update
\begin{equation}\label{Eqn:timeupdate} P(x_k, m|\textbf{Z}_{0:k-1},\textbf{U}_{0:k},x_0) = \int{P(x_k|x_{k-1},u_k) P(x_{k-1},m|\textbf{Z}_{0:k-1},\textbf{U}_{0:k-1},x_0)dx_{k-1}} \end{equation}
Measurement Update
\begin{equation}\label{Eqn:measureupdate} P(x_k, m|\textbf{Z}_{0:k},\textbf{U}_{0:k},x_0) = \frac{P(z_k|x_k,m)P(x_k,m|\textbf{Z}_{0:k-1},\textbf{U}_{0:k},x_0)}{P(z_k|\textbf{Z}_{0:k-1},\textbf{U}_{0:k})} \end{equation}
Equation (\ref{Eqn:timeupdate}) and (\ref{Eqn:measureupdate}) provide a recursive procedure for calculating the joint posterior \(P(x_k,m|\textbf{Z}_{0:k},\textbf{U}_{0:k},x_0)\) for the robot state \(x_k\) and map \(m\) at time \(k\) based on all observation \(\textbf{Z}_{0:k}\) and all control inputs \(\textbf{U}_{0:k}\) up to and including time \(k\).

Rao-Blackwellized Filter

The joint SLAM state may be factored into a vehicle component and a conditional map component:
\begin{equation} P(\textbf{X}_{0:k}, m|\textbf{Z}_{0:k},\textbf{U}_{0:k},x_0) = P(m|\textbf{X}_{0:k},\textbf{Z}_{0:k})P(\textbf{X}_{0:k}|\textbf{Z}_{0:k},\textbf{U}_{0:k},x_0) \end{equation}

The general form of R-B particle filter form SLAM is as follows. We assume that, at time \(k-1\), the joint state is represented by \({\{{w^{(i)}_{k-1}}, {\textbf{X}^{(i)}}_{0:k-1},P(m|{\textbf{X}^{(i)}_{0:k-1}},\textbf{Z}_{0:k-1}})\}^N_{i}\).

1)
For each particle conditioned on specific particle history, and draw a sample from it
\begin{equation} x^{(i)}_{k} \sim \pi(x_k|\textbf{X}^{(i)}_{0:k-1},\textbf{Z}_{0:k},u_k) \end{equation}
This new sample is joined to the particle history \(\textbf{X}^{(i)}_{0:k} =\{\textbf{X}^{(i)}_{0:k-1},x^{(i)}_k\}\).

2)
Weight samples according to the importance function
\begin{equation} w^{(i)}_k = w^{i}_{k-1} \frac{P(z_k|\textbf{X}^{(i)}_{0:k}, \textbf{Z}_{0:k-1})P(x^{(i)}_k|x^{(i)}_{k-1},u_k)}{\pi(x^{(i)}_k|\textbf{X}^{(i)}_{0:k-1},\textbf{Z}_{0:k},u_k)} \end{equation}
The numerator terms of this equation are the observation model and motion model.
\begin{equation} P(z_k|\textbf{X}_{0:k},\textbf{Z}_{0:k-1}) = \int{P(z_k|x_k,m)p(m|\textbf{X}_{0:k-1},\textbf{Z}_{0:k-1})dm} \end{equation}

3)
If necessary, perform resampling.

4)
For each particle, perform an EKF update on the observed landmarks as a simple mapping operation with known vehicle pose.

References

  1. Durrant-Whyte, Hugh, and Tim Bailey, Simultaneous localization and mapping: part I. Robotics & Automation Magazine, IEEE 13.2 (2006): 99-110.. ↩

  • Site Map