概率机器人（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 , . . . } z_t=\{z_t^1, z_t^2,...\} zt​={ zt1​,zt2​,...}，并且每个特征都是可区分的，不可区分的在后文。

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$