概率机器人（Probabilistic Robotics）笔记 Chapter 7: 移动机器人定位（Mobile Robot Localization）

1. 简介

可以把定位看作坐标系变换问题，即全局地图坐标系与机器人local坐标系的变换。
传感器通常不能直接测量位置，需要从其他数据中推算位置。
通常需要累积一段时间的数据，来进行定位。
定位算法通常针对特定地图表达方式，多种多样。

2. 定位问题分类

局部(local) v.s. 全局(global)

定位问题可按照初始化的信息和运行时的信息分三类，难度递增：

1. 位置追踪（position tracking）：假设初始状态已知。是一个局部问题，因为不确定性是局部的，且被限制在机器人的真实位置周围。
2. 全局定位（global localization）：初始状态未知。这种情况下不能假设位姿误差有界。
3. 绑架问题（kidnapped robot problem）：是全局定位问题的变种，但更难，因为机器人会保持原有的位姿估计，不知道已经不适用了。这对定位失败后的恢复非常重要

静态环境(static) v.s. 动态环境(dynamic)

1. 静态环境：唯一的状态变量是机器人的位姿，有一些优美的数学特性，可以进行高效的概率估计。
2. 动态环境：除机器人外，还有其他物体的位置或形态（configuration）发生改变。对于转瞬即逝的变化，最好当噪声处理；而对于有一定持续性、会影响多帧测量的变化，比如行人、日光、可移动的家具、门等，是最值得关心的。

两种方法应对动态环境：把动态物体加入状态变量中，使状态符合马尔科夫假设，但计算量大、模型复杂；还可以对传感器数据进行滤波，过滤掉未建模的动态信息。

被动()passive) v.s. 主动(Active)

根据定位算法是否控制机器人运动而区分。
被动定位下，机器人的运动并不是为了增强定位效果。主动定位下，运动的目的是减小定位误差或减小把一个定位不好的机器人移动到一个混乱的环境中的代价。
纯主动定位没有用——机器人的作用在于完成任务。有些主动定位技术是在被动定位之上构造的，也有些结合了具体任务。
本章只讲被动，主动在后续章节。

单机器人 v.s. 多机器人

单机器人没有通信的问题。
多机器人利用可以互相探测的关系，一个机器人的置信度可以影响另一个机器人。

3. 马尔科夫(Markov)定位

概率的定位算法都是贝叶斯滤波器的变种。
马尔科夫定位是贝叶斯滤波器在定位问题中的直接应用，可以解决静态环境中的全局定位、定位追踪、绑架问题。

算法：Algorithm Markov-localization($bel(x_{t-1}),u_t,z_t,m$):
1····for all $x_t$ do
2········$\widehat{bel}(x_t)=\int p(x_t|u_t,x_{t-1},m)bel(x_{t-1})dx$
3········$bel(x_t)=\eta p(z_t|x_t,m)\widehat{bel}(x_t)$
4····endfor
5····return $bel(x_t)$

1. 对于定位追踪问题：如果初始状态已知，则用一个窄高斯分布。
2. 对于全局定位问题：如果初始状态未知，则在整个状态空间上均匀分布。
3. 对于其他定位问题：关于机器人未知的不完全信息可以转换为对应的初始置信分布。

4. 马尔科夫定位图示

一维的小机器人看门的例子，不详细记了。

5. EKF定位

EKF定位是马尔科夫定位的特殊情况，即各种东西都是高斯分布，然后用前两阶矩来表达（均值和方差）。

EKF定位假设地图是一系列特征，在时间点$t$，机器人得到一系列特征的观测，每个观测包含距离、方向、身份$c_t^i$，即观测$z_t=\{z_t^1,z_t^2,...\}$，并且每个特征都是可区分的，不可区分的在后文。

5.1 图示

还是机器人一维运动，有三个小门。假设特征点可区分，也就是门都有独立的标签，所以测量模型是$p(z_t|x_t,c_t)$，对$x_t$都是一个窄带高斯，其中$c_t$取值为1、2、3。此外，再假设初始状态已知（窄高斯）。

5.2 EKF定位算法

输入包括初始估计（高斯），控制$u_t$，地图$m$，还有时间$t$观测到的一系列特征$z_t=\{z_t^1,z_t^2,...\}$。输出是新的状态估计的均值和方差。

伪代码不写了，在下一章节中详细说推导。

5.3 数学推导

1. 由运动模型（$x_{t-1},y_{t-1},{\theta }_{t-1},v_t,{\omega }_t$到$x_t,y_t,z_t$）
2. 将其拆分成无噪声和零均值随机噪声两部分，即$x_t=x_{t-1}+g(u_t,x_{t-1})+/Nu(0,R_t)$
3. 线性化，把$g$进行一阶泰勒展开，即$g(u_t,x_{t-1})\approx g(u_t,{\mu }_{t-1})+G_t(x_{t-1}-{\mu }_{t-1})$，即把未知量$x_{t-1}$换成已知量（其均值）${\mu }_{t-1}$。雅克比$G_t=g^{\prime }(u_t,{\mu }_{t-1})$，是一个3x3矩阵（$u_t$是确定已知量，相当于方程的系数，无偏导），对应$x,y,\theta$对于${\mu }_{t,x},{\mu }_{t,y},{\mu }_{t,\theta }$的偏导。
4. 把观测模型分成无噪和噪声两部分，即$z_t^i=h(x_t,j,m)+\mathrm{N}(0{,}_t)$。
5. 线性化，把$h$进行一阶泰勒展开，即$h(x_t,j,m)=h(\overline{{\mu }_t},j,m)+H_t^i(x_t-\overline{{\mu }_t})$。雅克比$H_t^i=h’(\bar{\mu_t},j,m)，得到3x3矩阵。
6. 基于特征的EKF定位每次迭代处理多个观测数据，这依靠条件独立假设，即$p(z_t|x_t,m)={\Pi }_{i}p(z_t^i|x_t,m)$，而在贝叶斯定律下变成了加法（log）。

6. 估计Correspondences

6.1 对未知Correspondences时的EKF定位

应用中不知道测量特征与地图特征的对应关系，因此需要在定位过程中对特征的身份进行估计。最简单的方法是maxmum likelihood correspondence(最大似然对应),即首先确定配对的最可能的值，然后使用。

如果对配对变量有多个假设的可能性相似，那这个方法不好使，但设计系统的时候可以避免，有两个常用技巧：选择非常独特且彼此距离较远的特征，使不容易混淆；还可以保证机器人的位姿不确定性保持在很小的范围。但是这两个方案彼此矛盾，选择特征的间隔是一门艺术。

运动更新与前文EKF相同，不同在于测量更新：首先计算地图中所有地标的一系列量，可以用来判断配对；之后通过最小化测量到的与预期的特征向量之间的Mahalanobis平方距离函数，来选择配对变量。

实际中，机器人只能看见离的很近的很少的地标，然后可以用一些很简单的测试来拒绝掉地图中一大部分不太可能的地标。

算法也可以修改，以应对outliers，如仅使用Mahalanobis距离小于一定阈值的特征。

伪代码待补充

6.2 数学推导

指导思想是找到最大化测量数据可能性的配对，即:
$\widehat{c_t}={\mathrm{argmax}}_{c_t}p(z_t|c_{1:t},m,z_{1:t-1},u_{1:t})$
注意这以之前的$c_{t:t-1}$为条件进行最大化，最大似然估计器假设他们都是正确的。这样可以递增地更新滤波器，但也让滤波器表现脆弱。
由贝叶斯定理和马尔科夫性，可对上式变形，即:
$p(z_t|c_{1:t},m,z_{1:t-1},u_{1:t})=\int p(z_t|c_t,x_t,m)\overline{bel}(x_t)d{x}_t$
也就是变成了最大化右式，得到$\widehat{c_t}$。
这个最大化是以整个$c_t$向量为自变量，所以复杂度随特征向量的大小指数增长。
一种常用的解决手段是对每个特征$z_t^i$分别最大化，即
$\widehat{c_t^i}={\mathrm{argmax}}_{c_t}\int p(z_t^i|c_t^i,x_t,m)\overline{bel}(x_t)d{x}_t$.
而上式中的单个特征的测量概率可由下式计算：
$p(z_t^i|c_t^i,x_t,m)=\eta \exp \{-\frac{1}{2}(z_t^i-h(x_t,c_t^i,m){)}^T{Q}_t^{-1}(z_t^i-h(x_t,c_t^i,m))\}$

然后再对上式进行泰勒展开，再带入上上式的积分中，得到close form的积分结果表达式：

${\hat{c}}_t^i=\arg {\max }_{c_t^i}(z_t^i-h({\bar{\mu }}_t,c_t^i,m){)}^T[H_t{\bar{\Sigma }}_t{H}_t^T+Q_t{]}^{-1}(z_t^i-h({\overline{mu}}_t,c_t^i,m))$

7. Multi-Hypothesis Tracking

当数据无法很好的关联时，有很多方案可以选择，本章介绍Multi-hypothesis Tracking (MHT)算法。这种算法可以用多个（记为$l$个）高斯按不同权重叠加来表征置信概率分布，其中每一个高斯都对应某一种特定的数据关联序列，将其记为$c_{t,l}$。所以我们可以把每一个置信分布成分看作是以一个特定的数据关联序列为条件，即：

$be{l}_l(x_t)=p(x_t|z_{1:t},u_{t:t},c_{t:t},l)$

一种简单但是计算量不可行的思路是，在算法中维护所有的可能的数据关联序列。每一个高斯的权重随时间不停迭代。即我们把关联看作潜在的变量，并且计算每一个高斯组成部分的后验可能性。但是高斯组成部分的数量不断增加，然后会变得不可解。

MHT算法基于上述算法，但控制高斯成分的数量，即当一个成分的权重小于某个阈值时，终止它，所以成分的数量总小于阈值的倒数。

8. Practical Considerations

有几个EKF和MHT的变种，有更高的效率和鲁棒性：

1. Efficient Search：遍历地图中的所有路标并不实用。可以通过一些测试，选取固定数量的路标候选。
2. Mutual Exclusion：算法的一大制约就是假设了特征噪声相互独立。这个假设让我们可以对特征挨个处理，但同时也让我们可以把两个不同特征关联到同一个地标上去，这不符合$i̸=j$→${\hat{c}}_t^i̸={\hat{c}}_t^j$的常理。上式的限制条件被称为mutual exclusion principle in data association。
3. Outliers：我们的算法中还没有对outlier进行处理。我们可以对outlier进行一个无效标记。

EKF和MHT定位只适用于定位追踪问题，并且一般在不确定性较小的情况下比较好用，原因有三个：

1. 单模型高斯不适用于更泛化的全局定位问题。
2. 窄高斯可以降低错误的数据关联的风险。
3. 泰勒展开只有当展开点离自变量取值很近的时候比较准确。例如，如果机器人$\theta$的标准差大于20°，则完蛋。这也解释了为什么用一个很宽的初始高斯分布并不能把MHT用于全局定位。

给EKF设计合适的特征是一门艺术，因为有很多要求：要多，要独特，要离得远。很多环境并没有很多的点地标，所以总的来说，地标越多，工作越好。但地标如果稠密，则应该加上mutual exclusion。

此外，EKF使用特征，也就是只是用了测量带来的一部分信息，也不能处理negative information (即该看见但没看见特征)，因为负信息也是信息。不能处理的原因是因为它引入了非高斯的分布。MHT中可以使用减少权重的方法来处理付信息。

定位的优劣取决于数据关联。后面的章节将会讲更多更复杂的数据关联技巧。