SLAM (Simultaneous Localization and Mapping)

Table of Contents

1 SLAM简介

SLAM stands for Simultaneous Localization and Mapping.

SLAM problems arise when the robot does not have access to a map of the environment, nor does it know its own pose. Instead, all it is given are measurements \(z_{1:t}\) and controls \(u_{1:t}\). The term "simultaneous localization and mapping" describes the resulting problem: In SLAM, the robot acquires a map of its environment while simultaneously localizing itself relative to this map.

SLAM问题的输入是:控制量 \(u_{1:t}\) 和测量值 \(z_{1:t}\) ,想得到的输出是:地图 \(m\) 和机器人路径 \(x_{1:t}\) 。

参考:
Probabilistic Robotics, by Sebastian Thrun, 10 Simultaneous Localization and Mapping
Simultaneous Localisation and Mapping (SLAM): Part I The Essential Algorithms
视觉SLAM漫谈:http://www.cnblogs.com/gaoxiang12/p/3695962.html

1.1 Online SLAM和Full SLAM

Online SLAM是指仅求解地图和当前位姿。即求解:
\[p(x_{t}, m \mid z_{1:t}, u_{1:t})\]

Full SLAM是指求解地图和整个路径。即求解:
\[p(x_{1:t}, m \mid z_{1:t}, u_{1:t})\]

2 EKF SLAM

The EKF SLAM has been the de facto method for SLAM, until the introduction of FastSLAM.

EKF SLAM和EKF Localization算法类似,区别在于EKF SLAM除了估计机器人位姿外,还要估计Landmark的坐标。

我们知道,要使用Kalman滤波器进行估计,需要知道“理论估计值”(在“预测阶段”会使用它)和“测量值”(在“更新阶段”会使用它)。Kalman滤波器可以看作混合“理论估计值”和“测量值”,从而得到一个更接近真实值的估计值。

EKF SLAM要同时估计机器人位姿和Landmark的坐标。对于估计机器人位姿,它的“理论估计值”可以由机器人的Motion Model得到;对于估计Landmark的坐标,它的“理论估计值”可以由下式(摘自:SLAM for Dummies)得到:
\[\begin{bmatrix} \text{range} \\ \text{bearing} \end{bmatrix} = \begin{bmatrix} \sqrt{(\lambda_x - x)^2 + (\lambda_y - y)^2} + v_r \\ \text{atan2}(\lambda_y - y, \lambda_x - x) - \theta + v_{\theta} \end{bmatrix}\]
其中, \(\lambda_x, \lambda_y\) 是Landmark的坐标, \(x,y\) 是机器人坐标估计值, \(\theta\) 是机器人Bearing(Heading Direction)。

参考:
SLAM - Simultaneous Localization and Mapping, by Wolfram Burgard, Diego Tipaldi
SLAM for Dummies
Simulataneous localization and mapping with the extended Kalman filter

2.1 Data Association

从传感器得到了Landmark的数据,但这个数据是哪个Landmark的呢?这就是Data Association所要做的工作。

Data association is the process of associating uncertain measurements to known tracks.

参考:
http://ais.informatik.uni-freiburg.de/teaching/ws10/robotics2/pdfs/rob2-15-dataassociation.pdf

2.2 EKF SLAM: Complexity

Cost per step: quadratic in \(n\), the number of landmarks: \(O(n^2)\)
Total cost to build a map with \(n\) landmarks: \(O(n^3)\)

参考:http://ais.informatik.uni-freiburg.de/teaching/ss09/robotics/slides/j_slam.pdf

3 FastSLAM

如果直接把粒子滤波器应用于SLAM算法中,则描述地图时维度太大,计算上不可行。比如,有一个 \(20m \times 10m\) 的地图,且需要的分辨率为 \(5cm \times 5cm\) ,则这个地图中共有 \(400 \times 200 = 80,000\) 个单元,那么有 \(2^{80,000}\) 个可能的地图!参考:http://people.eecs.berkeley.edu/~pabbeel/cs287-fa12/slides/RBPF.pdf

FastSLAM算法是Rao-Blackwellised粒子滤波器(RBPF)在机器人SLAM问题中的应用。

Rao-Blackwellised粒子滤波器(RBPF, Rao-Blackwellized Particle Filtering)
在高维状态空间中采样时,PF的效率很低。对某些状态空间模型,状态向量的一部分在其余部分的条件下的后验分布可以用解析方法求得,例如某些状态是条件线性高斯模型,可用Kalman滤波器得到条件后验分布,对另外部分状态用PF,从而得到一种混合滤波器,降低了PF采样空间的维数,RBPF样本的重要性权的方差远远低于SIR方法的权的方差,为使用粒子滤波器解决SLAM问题提供了理论基础。而Montemerlo等人在2002年首次将Rao-Blackwellised粒子滤波器应用到机器人SLAM中,并取名为FastSLAM算法。

摘自:http://baike.baidu.com/view/2238505.htm

The Rao-Blackwellized Particle Filtering (RBPF) is a combination of the particle filter and the Kalman filter, where each particle has a Kalman filter associated to it.

RBPF参考:
Rao-Blackwellized Particle Filters and Localization: http://www2.informatik.uni-freiburg.de/~grisetti/teaching/ls-slam/lectures/pdf/ls-slam-06-rbpfloc.pdf
Thomas Schön, Fredrik Gustafsson, and Per-Johan Nordlund. “Marginalized Particle Filters for Mixed Linear/Nonlinear State-Space Models”. IEEE Transactions on Signal Processing, 53(7):2279-2289, Jul. 2005: http://user.it.uu.se/~thosc112/pubpdf/schongn2005.pdf

FastSLAM参考:
Probabilistic Robotics, by Sebastian Thrun, 13 The FastSLAM Algorithm
http://ais.informatik.uni-freiburg.de/teaching/ss15/robotics/slides/14-slam-fastslam.pdf

3.1 FastSLAM基本思想(混合滤波器)

FastSLAM基于SLAM问题的下面分解形式:
\[p(x_{1:t}, m \mid z_{1:t}, u_{1:t}) = p(x_{1:t} \mid z_{1:t}, u_{1:t}) \cdot p(m \mid x_{1:t}, z_{1:t})\]

上式的推导如下(说明:下面推导中 \(p(m \mid x_{1:t}, z_{1:t}, u_{1:t}) = p(m \mid x_{1:t}, z_{1:t})\) 的理由是估计地图时可以认为 \(x_{1:t}\) 已经包含了 \(u_{1:t}\) 的信息):
\[\begin{aligned} p(x_{1:t}, m \mid z_{1:t}, u_{1:t}) & \stackrel{\text{Chain rule}}{=} p(x_{1:t} \mid z_{1:t}, u_{1:t}) \cdot p(m \mid x_{1:t}, z_{1:t}, u_{1:t}) \\ & = p(x_{1:t} \mid z_{1:t}, u_{1:t}) \cdot p(m \mid x_{1:t}, z_{1:t}) \\ \end{aligned}\]

Feature-based FastSLAM的核心思想是:用粒子滤波器来估计 \(x_{1:t}\) (即每个粒子代表一个可能路径);而对每个粒子(可能路径),使用Extended Kalman Filter来估计地图 \(m\) 。 即用粒子滤波器求解 \(p(x_{1:t} \mid z_{1:t}, u_{1:t})\) ,用Extended Kalman Filter求解 \(p(m \mid x_{1:t}, z_{1:t})\) 。在Feature-based FastSLAM中,地图 \(m\) 可用 \(N\) 个Landmark表示。Feature-based FastSLAM如图 1 所示。

robot_slam_fastslam.png

Figure 1: Feature-based FastSLAM

设有 \(M\) 个粒子,每个粒子所关联的地图有 \(N\) 个Landmark,每个Landmark都用一个Extended Kalman Filter进行估计。Feature-based FastSLAM中一共有 \(NM\) 个Extended Kalman Filter。

3.2 Grid-based FastSLAM

Feature-based FastSLAM = "Particle Filter" + "Extended Kalman Filter"
Grid-based FastSLAM = "Particle Filter" + "Occupancy Grid Mapping (Mapping With Know Poses)"

参考:
robabilistic Robotics, by Sebastian Thrun, 13.10 Grid-based FastSLAM
http://ais.informatik.uni-freiburg.de/teaching/ss15/robotics/slides/15-slam-gridrbpf.pdf

3.3 GMapping(实现并改进了Grid-based FastSLAM 2.0)

Using GMapping, you can create a occupancy grid map from laser and pose data collected by a mobile robot.

GMapping implements the Grid-based FastSLAM 2.0 algorithm, with two main improvements:
(1) Improved proposal distribution
(2) Adaptive resampling

参考:
http://irawiki.disco.unimib.it/irawiki/index.php/ROS_GMapping
http://people.eecs.berkeley.edu/~pabbeel/cs287-fa11/slides/gmapping.pdf

3.3.1 GMapping相关论文

Giorgio Grisetti, Cyrill Stachniss, and Wolfram Burgard: Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters, IEEE Transactions on Robotics, Volume 23, pages 34-46, 2007 (pdf link)
Giorgio Grisetti, Cyrill Stachniss, and Wolfram Burgard: Improving Grid-based SLAM with Rao-Blackwellized Particle Filters by Adaptive Proposals and Selective Resampling, In Proc. of the IEEE International Conference on Robotics and Automation (ICRA), 2005 (pdf link)

4 Graph-based SLAM

SLAM问题可以转换为图优化问题。

参考:
Probabilistic Robotics, by Sebastian Thrun, 11 The GraphSLAM Algorithm
Graph-based SLAM using Least Squares: http://ais.informatik.uni-freiburg.de/teaching/ws11/robotics2/pdfs/rob2-09-least-squares-slam.pdf
A Tutorial on Graph-Based SLAM: http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf
SLAM Back-End, by Giorgio Grisetti: http://kaspar.informatik.uni-freiburg.de/~kitutorial/pdfs/ki-pose-slam.pdf
g2o:A general Framework for (Hyper) Graph Optimization: https://svn.openslam.org/data/svn/g2o/trunk/g2o/doc/g2o.pdf

5 VSLAM (Visual SLAM)

Visual SLAM (Vision-based SLAM) uses camera images to map out the position of a robot in a new environment.

参考:http://frc.ri.cmu.edu/~kaess/vslam_cvpr14/


Author: cig01

Created: <2016-06-26 Sun 00:00>

Last updated: <2017-12-13 Wed 11:56>

Creator: Emacs 25.3.1 (Org mode 9.1.4)