Robot Localization
Table of Contents
1. Robot Localization 简介
Robot Localization 是指在已知的地图中，确定 Robot 的位置，所以 Localization 又称为 Position Estimation 或者 Position Tracking.
基本上，所有基于概率的 Robot Localization 算法都源于 Bayes Filter 或其变种。
注：Bayes Filter 是通用的状态估计方法，除用于估计定位外，还可以估计其它状态。
Bayes Filter 的基本递推式如下：
\[{\color{red}{bel(x_t)}} = \eta \, p(z_t \mid x_t) \int p(x_t \mid x_{t-1}, u_t) {\color{red}{bel(x_{t-1})}} \, \mathrm{d} x_{t-1}\]
本文主要参考：
Probabilistic Robotics, by Sebastian Thrun, Chapter 7 Mobile Robot Localization: Markov and Gaussian
Probabilistic Robotics, by Sebastian Thrun, Chapter 8 Mobile Robot Localization: Grid And Monte Carlo
1.1. Local VS. Global Localization
There are two types of localization: local and global.
- Local techniques aim at compensating odometric errors during robot navigation. They require that the initial location of the robot is approximately known and they typically cannot recover if they lose track of the robot’s position.
- Global techniques can localize a robot without any prior knowledge about its position, i.e., they can handle the kidnapped robot problem, in which a robot is kidnapped and carried to some unknown location.
2. Robot Localization 算法
2.1. Markov Localization
Markov Localization 算法直接基于 Bayes Filter 算法。在定位问题中，地图（常用 \(m\) 表示）是已知输入，故可得到 Markov Localization 算法的框架如图 1 所示。
Figure 1: Markov Localization
2.2. EKF Localization
Markov Localization 使用概率分布 \(bel(x_t)\) 来表达位置的估计，这很通用，但却很低效。 EKF Localization 不用概率分布，而使用均值 \(\mu_t\) 和协方差 \(\sum_t\) 来表示位置的估计，而且地图中需要有路标(Landmark)。 EKF Localization 算法如图 2 所示。
EKF Localization 算法只能用来线性高斯系统（即其 Motion model 和 Measurement model 都必须是线性的，噪声必须是高斯噪声）。
Figure 2: EKF Localization
说明 1：上面算法中，第 8-22 行是 EKF 的“Measurement Update”过程。外层的 for 循环是根据 \(t\) 时刻每一个 feature 的观察值进行相应更新，内层的 for 循环加上第 16 行是用最大似然估计法求解 correspondence variable（这个变量标记 feature 观察值和地图中 feature 的关系）。
说明 2：关于上面 EKF Localization 算法中所使用的 Motion model 和 Measurement model，以及 EKF Localization 的推导请参考：Probabilistic Robotics, by Sebastian Thrun, 7.4 EKF Localization, 7.5 Estimating Correspondences
2.3. Grid Localization
Grid Localization 使用 Histogram Filter（离散 Bayes Filter 应用于连续状态空间时就称为 Histogram Filter）来对机器人位置进行估计。Grid Localization 算法如图 3 所示。
Figure 3: Grid Localization, a variant of the discrete Bayes filter
2.4. Monte Carlo Localization (MCL)
Monte Carlo Localization 使用 Particle Filter 对机器人位置进行估计。Grid Localization 算法如图 4 所示。
Figure 4: MCL, or Monte Carlo Localization, a localization algorithm based on particle filter
MCL 改进方案一：
我们知道，Particle Filter 中权重小的粒子在重抽样时可能消失（称为 Particle Deprivation Problem）。这可以导致 MCL 不能工作，一种解决办法是加入一些随机粒子，这种思路应用到 MCL 算法中称为 Augmented_MCL 算法，如图 5 所示。
Figure 5: An adaptive variant of MCL that adds random samples
MCL 改进方案二：
另外一个 MCL 算法的改进变种是不固定粒子数，让粒子数量可变。称为 KLD-sampling 算法，如图 6 所示。
Figure 6: KLD-sampling MCL with adaptive sample set size