第六章：机器人感知

6.1 介绍

在概率机器人学中，环境测量模型environment measurement model是继运动模型之后，第二个特定领域domain-specific的模型。测试模型描述了通过传感器测量值生成物理世界的形式化过程。现如今的机器人用了各种各样的传感器，比如说触觉传感器，距离传感器(range sensor)，摄像头。这些模型都依赖于传感器：图像传感器最好是通过投影几何建模，而声纳传感器则最好通过描述声波和它在环境表面的回波来建模。

概率机器人学显式地在传感器测量值上添加噪声，对机器人传感器与生俱来的不确定性建模。形式上，测量模型由一个条件概率$p(z_t | x_t, m)$表示，其中$x_t$是机器人的位姿，$z_t$是在时刻$t$的测量值，而$m$则是环境的地图。尽管在本章中我们主要讨论距离传感器，但是其背后的原则和公式并不局限于这类传感器。实际上其基本原则可以应用到任何类型的传感器上，比如说摄像头或者条码路标检测器等。

机器人通过传感器感知周围的环境，下面我们以一个简单的例子来说明这一点。图6.1(a)所示的移动机器人装备了一个由24个超声构成的圆形传感器阵列，并用之扫描一个走廊的环境。图中用浅灰色表示各个传感器的距离测量，环境地图则被描绘成了黑色。其中大部分的测量都是采集探测方向上最近物体的距离，而有些测量值却没有检测到物体。


(a)	(b)
图 6.1(a)一个使用超声传感器扫描周围环境的机器人。(b)超声传感器的误读，当声波打在镜面上的角度$\alpha$超过了其开度角(opening angle)的一半时，就会产生这种现象。

声纳系统经常不能有效地测量最近物体，一般我们将之解释为传感器噪声。技术上来说，噪声一般都是可以预测的:当超声打倒光滑的表面(比如说墙面)时就会产生镜面反射specular，墙面就相当于一个声波的镜子。如图6.1(b)所示，当声波以一定的角度打在墙面的时候，回波就会沿着法线的另一面传播出去，以至于传感器收不到回声。这种镜面反射的效应通常会导致很大的测量值。产生镜面反射的可能性依赖于很多因素，比如说反射面的材料、入射角度，测量束的开度角,声纳传感器的灵敏度等。此外，还有一些其他误差源，比如说short reading，可能是不同传感器之间相互干扰造成的，或者是机器人附近未建模的物体，比如说人类。

下图6.2中给出了一个典型的激光测距扫描laser range scan的例子。与声纳类似，激光测距仪也是主动发送一个信号并接收其回波来测距，只是它发出信号是一束激光。图6.2中的激光扫描仪基于飞行时间time-of-flight进行测量的，测量的步进角是一度。

图 6.2. 一个SICK LMS激光测距仪获得的典型的激光扫描图像。该图是Freiburg大学的Dirk Hahnel提供的，对一个煤矿的扫描。

尽管我们在2.4.4节中讨论的马尔科夫假设的问题总是存在的，不过一般来说，传感器模型越精确结果越好。实际上，因为物理世界过于复杂，我们基本不可能完全准确地对传感器建模。

一般，传感器的响应特性依赖于我们不希望在一个概率机器人算法中显式表述的变量，比如说，在机器人建图的过程中，我们就不会考虑墙面的材料。从随机过程的角度看来，概率机器人学对传感器的不精确性具有一定的容忍度。因为它用条件概率$p(z_t | x_t)$对测量过程建模，而不是确定性方程$z_t = f(x_t)$，所以模型具有一定的模糊特性可以兼容传感器测量的不确定性。这就是概率学方法相比于传统机器人技术的优势，在实际应用中我们可以使用极为粗糙的模型。但是，在设计概率学模型的时候，必须考虑不同类型的不确定性可能对传感器测量值带来什么样的影响。

当我们请求传感器数据的时候，很多传感器不止提供一个测量数值。比如说，摄像头生成的就是一个亮度、饱和度、颜色的阵列，类似的，距离扫描器通常生成的是整个区域的扫描结果。我们用符号$K$来表示测量数据的纬度，对于一次测量$z_t$，可以写作： $$ \begin{equation}\label{f1} z_t = {z_t^1, \cdots, z_t^k} \end{equation} $$ 其中$z_t^k$对应着一个测量值(比如,一个距离)。我们用各个测量值的概率之积来近似$p(z_t | x_t, m)$ $$ \begin{equation}\label{f2} p(z_t | x_t, m) = \prod_{k=1}^K {p(z_t^k | x_t, m)} \end{equation} $$ 上式存在一个独立性假设independence assumption，认为每个测量束的噪声都是独立的，这正是我们在2.4.4节中讨论的马尔科夫假设。这个假设只在理想情况下才成立。在2.4.4节中我们已经讨论了产生依赖的各种原因，这里重述一下: 人类，常常困扰机器人的一个主要因素；模型$m$的误差，后验概率的近似，等等。现在，我们直接就不关心各种违背独立性假设的因素了，将在以后的章节中回到这个问题上。

6.2 地图

为了更清楚的表述测量值生成的过程，我们需要先了解一下目标环境的描述方式。环境的地图map是一个记录环境中物体以及它们位置的列表。在前一章中我们已经初步接触了地图信息，有时，开发设计机器人运动模型就需要用到地图信息。地图$m$可以形式化的表述为环境中的物体以及它们属性的列表: $$ \begin{equation}\label{f3} m = { m_1, m_2, \cdots, m_N } \end{equation} $$ 其中，$N$是环境中物体的数量，每个$m_n, 1 ≤ n ≤ N$都描述了一个属性。地图通常由两种索引形式，称为基于特征的索引feature-based和基于地址的索引location-based。在基于特征的地图中，$n$是特征索引。$m_n$中记录了特征的各种属性以及其笛卡尔坐标位置。在基于位置的地图中，索引$n$对应着一个特定的位置。在平面地图中，通常用$m_{x,y}$来代替$m_n$，这是为了显式地说明其世界坐标$(x,y)$。

这两种形式地地图各有优缺点。基于地址的地图是volumetric，它为世界中的每个地址都提供了一个标签。Volumetic地图不仅包含了环境中障碍物的信息，还有空地的信息。这这就是其与基于特征的地图的不同之处。基于特征的地图只描述了环境在特定位置上的特征，也可以说是，环境中哪些位置上由障碍物。因此我们更容易调整特征描述地图中障碍物的位置。也正因为如此，在机器人建图领域中，基于特征的地图更为流行。在本书中，我们要讨论这两种形式的地图，在一些特殊的场景中我们还需要从一种形式转换到另一种形式。

本书将在第9章详细地描述占用栅格地图occupancy grid map，它是一种经典的地图表示形式。占用地图是一种基于位置的地图，它的每一个$x$-$y$坐标都是一个二值函数，标志着其对应位置是否被障碍物占用了。对于机器人导航而言占用栅格地图是很棒的表示形式，因为机器人可以很容易根据地图找到一条可行(没有被占用)的路径。

实际上传感器的测量值是对物理世界中的观测，并不是地图中抽象的数据。尽管如此，在本书中我们仍不区分物理世界与地图之间的差异。一般传感器模型都是建立在地图$m$数据上的，因此我们采用同样的符号表示测量值是依赖于地图的。

6.3 距离扫描的波束模型(beam models of range finders)

距离扫描器是机器人上使用最多的传感器之一，因此本章第一个要介绍的就是测距扫描仪的近似物理模型。测距扫描仪测量通过一个波束beam或者测量锥cone来测量附近物体的距离。激光雷达是一个比较精确的距离传感器，常用波束模型建模。超声传感器通常使用测量锥模型。

6.3.1 基本的测量算法

我们的模型有四种类型的测量误差，它们都对模型工作与否有着重要作用。这四种误差是：小的测量噪声，非预期物体所导致的误差，没有检测到物体而导致的误差，随机的未解(unexplained)噪声。因此，目标模型$p(z_t | x_t, m)$是四中误差的混合概率密度函数：


(a). 高斯分布$p_{hit}$	(b). 指数分布$p_{short}$	(c). 均匀分布$p_{max}$	(d). 均匀分布$p_{rand}$
图 6.3. 距离扫描传感器模型的组件。横坐标对应着测量值$z_t^k$，纵坐标表示其似然度。

带有局部测量噪声的正确距离(correct range with local measurement noise)。理想的情况下，只要在测量范围以内，距离扫描仪总是能够准确测得其到最近物体的距离。我们用$z_t^{k_*}$表示测量值$z_t^k$所对应的真实距离。在基于位置的地图中，可以通过波束投射ray casting的形式确定真实距离$z_t^{k_*}$。在基于特征的地图中，通常通过搜索测量锥中最近的特征获得。但是即使传感器正确的测量了其到最近物体的距离，其返回的值也是有误差的。这一误差来自于距离传感器有限的分辨率，大气对于测量信号的影响等等。该噪声通常用一个窄高斯分布建模，其均值为$z_t^{k_*}$标准差为$\sigma_{hit}$，将之记为$p_{hit}$。如图6.3(a)所示，描述了在特定$z_t^{k_*}$下$p_{hit}$的概率密度。

实际上，距离传感器的测量值是有限的，用区间$[0, z_{max}]$表示其测量范围，其中$z_{max}$表示传感器所能测量的最大距离。那么有测量概率： $$ \begin{equation}\label{f4} p_{hit}(z_t^k | x_t, m) = \begin{cases} \eta \mathcal{N}\left(z_t^k; z_t^{k_*}, \sigma_{hit}^2\right) & if\ 0 ≤ z_t^k ≤ z_{max} \\ 0 & otherwise \end{cases} \end{equation} $$ 其中$z_t^{k_*}$是根据当前状态$x_t$和地图信息$m$通过波束投射求得的。 $\mathcal{N}\left(z_t^k; z_t^{k_*}, \sigma_{hit}^2\right)$表示均值为$z_t^{k_*}$标准差为$\sigma_{hit}$的正态分布： $$ \begin{equation}\label{f5} \mathcal{N}\left(z_t^k; z_t^{k_*}, \sigma_{hit}^2\right) = \frac{1}{\sqrt{2\pi \sigma_{hit}^2}} e^{-\frac{1}{2}\frac{(z_t^k - z_t^{k_*})^2}{\sigma_{hit}^2}} \end{equation} $$ 归一化算子$\eta$通过下式计算: $$ \begin{equation}\label{f6} \eta = \left(\int_0^{z_{max}} \mathcal{N}\left(z_t^k; z_t^{k_*}, \sigma_{hit}^2\right) d z_t^k\right)^{-1} \end{equation} $$ 其中变量$\sigma_{hit}$是测量模型固有的一个噪声参数。我们将在后续文章中讨论设定这些参数的策略。
非预期物体(Unexpected objects)。移动机器人的工作环境是动态的，而地图$m$却是静态的。因此，没有在地图中描述的物体将使得距离扫描仪产生意外的超短距离，仅从地图数据上看它是一个不合理的值。比如说，人类就是一种典型的频繁出现在机器人的工作空间中的物体。处理这样物体的一种方法就是，将它们看作是状态向量的一部分并估计它们的位置。还有一种更简单的方法，直接将它们看作是传感器的噪声。未建模的物体将总是使得传感器的测量值小于或者等于根据地图的估计值$z_t^{k_*}$。

随着测量距离的增加，传感器扫描到非预期物体的可能性逐渐降低。假设有两个人以固定的概率出现在传感器的感知范围内，其中一个人到传感器的距离为$z_1$，而另一个人的距离为$z_2$。不失一般性的，令$z_1 < z_2$。传感器更可能返回$z_1$而不是$z_2$。因为只要第一个人存在，传感器的测量值就是$z_1$。要使测量值为$z_2$，除了要保证第二个人存在外，还需要第一个人不出现在工作空间中。

数学上，一般使用指数分布exponential distribution来描述这种情况出现的概率。该分布也有一个参数记为$\lambda_{short}$，这也是测量模型的固有参数。根据指数分布的定义，有如下关于$p_{short}(z_t^k | x_t, m)$的表达式： $$ \begin{equation}\label{f7} p_{short}(z_t^k | x_t, m) = \begin{cases} \eta \lambda_{short} e^{-\lambda_{short} z_t^k} & if\ 0 ≤ z_t^k ≤ z_t^{k_*} \\ 0 & otherwise \end{cases} \end{equation} $$ 与上一种情况一样，因为指数分布的状态空间为$[0,z_t^{k_*}]$，所以我们需要一个归一化算子$\eta$。由于这个区间的概率积分为： $$ \begin{equation}\label{f8} \int_0^{z_t^{k_*}} \lambda_{short} e^{-\lambda_{short} z_t^k} d z_t^k = 1 - e^{-\lambda_{short}z_t^{k_*}} \end{equation} $$ 因此$\eta$取值为: $$ \begin{equation}\label{f9} \eta = \frac{1}{1 - e^{-\lambda_{short}z_t^{k_*}}} \end{equation} $$ 如图6.3(b)所示，随着距离$z_t^k$的增加，概率密度值以指数的形式下降。
错误(Failures)。有时传感器会漏检障碍物。例如声纳设备发生了镜面反射，就很容易出现这种问题。如果激光照射到黑色吸光的物体，或者工作在强烈的日光环境下，那么激光传感器也可能检测不到它们。这种典型的测量错误通常表现为最大测量距离max-range measurements，即传感器返回其所能测量的最大值$z_{max}$。这一现象很常见，所以在设计传感器的测量模型时需要尤其注意。

我们将使用中心为$z_{max}$的point-mass分布对这一现象建模： $$ \begin{equation}\label{f10} p_{max}(z_t^k | x_t, m) = I(z = z_{max}) = \begin{cases} 1 & if\ z=z_{max} \\ 0 & otherwise \end{cases} \end{equation} $$ 这里的$I$是一个指示器函数当参数条件为真时取值为1，为假时取值为0。实际上$p_{max}$不是一个概率密度函数，而是一个离散分布。但这对于我们来说，没有什么影响。因为传感器测量概率的数学模型并不受密度函数缺失的影响。在我们的图示中，只是用一个中心在$z_{max}$的非常窄的均匀分布来表示$p_{max}$，这样我们可以假装有一个密度函数。
随机测量(Random measurements)。距离扫描器偶尔会产生完全不可解释的测量值unexplainable measurements。比如说，经过多次反射的超声，或者不同传感器之间的串扰，都会使得声纳设备产生鬼一样的读数。为了简化模型，通常使用一个遍及整个传感器测量区间$[0, z_{max}]$的均匀分布来对其建模: $$ \begin{equation}\label{f11} p_{rand}(z_t^k | x_t, m) = \begin{cases} \frac{1}{z_{max}} & if\ 0≤ z_t^k < z_{max} \\ 0 & otherwise \end{cases} \end{equation} $$ 图6.3(d)中是这一分布$p_{rand}$的示意图。

我们可以通过加权平均的方式融合这四种不同的分布，令$z_{hit}, z_{short}, z_{max}, z_{rand}$分别为四种参数，并且$z_{hit} + z_{short} + z_{max} + z_{rand} = 1$。有 $$ \begin{equation}\label{f12} p(z_t^k | x_t, m) = \begin{pmatrix} z_{hit} \\ z_{short} \\ z_{max} \\ z_{rand} \end{pmatrix}^T \cdot \begin{pmatrix} p_{hit}(z_t^k | x_t, m) \\ p_{short}(z_t^k | x_t, m) \\ p_{max}(z_t^k | x_t, m) \\ p_{rand}(z_t^k | x_t, m) \end{pmatrix} \end{equation} $$ 对这些密度模型线性组合之后，就得到了如图6.4所示的概率密度函数。其中point-max分布$p_{max}$是一个范围很小的均匀分布。如图所示，在这个组合后的密度函数保留了这四个基本模型的基本特性。

图 6.4. 混合后的概率分布$p(z_t^k | x_t, m)$

算法6.1中的beam_range_finder_model实现了距离扫描器模型。该算法以完整的区域测量值$z_t$，机器人位姿$x_t$和地图$m$作为输入。第2和第7行构成的外层循环，根据公式($\ref{f2}$)将各个传感器光束$z_t^k$的似然度相乘。在第四行使用ray casting的方法计算无噪声的传感器测量值。并在第5行中使用公式($\ref{f12}$)中的密度混合规则计算每一个测量距离$z_t^k$。遍历了$z_t$中的所有传感器测量值$z_t^k$之后，该算法就返回了预期的概率$p(z_t | x_t, m)$。

算法6.1: 计算距离扫描$z_t$的似然度算法。假设扫描过程中各个测量值都是条件独立的。

6.3.2 调整模型内参

目前，我们还没有讨论如何选择传感器模型的参数。这些参数包括混合权重$z_{hit}, z_{short}, z_{max}, z_{rand}$，以及$\sigma_{hit}$和$\lambda_{short}$。我们将所有的模型参数用一个集合$\Theta$表示。显然，传感器测量值的似然度都是关于$\Theta$的函数。接下来我们讨论调整模型参数的方法。

我们可以通过数据来确定模型内参。下图6.5描述了两组从移动机器人上采集的10000个测量数据，它们是移动机器人经过一个办公室环境的数据。这两个散点图都是对接近3m(2.9m到3.1m)的距离进行测量的。左图是声纳传感器的数据，右图则是激光数据。图中，$x$轴表示读数的编号(从1到10000)，$y$轴则是传感器的测量值。这两个传感器的大多数测量值都在真值附近，但是两者的表现却不相同。超声传感器具有更大的测量噪声和错检概率。它总是不能够检测到障碍物，并报告最大值。相比之下，激光的数据就更准确一些。但偶尔也会报告错误的数值。


(a). 声纳数据	(b). 激光数据
图6.5. 在办公室环境下采集的声纳数据和激光数据。真值为300cm，最大量程为500cm。

手动设定这些内在参数$\Theta$往往能够得到一个完美的结果。我们所要做的的工作仅仅是盯着密度结果调整参数，直到满意为止。另外一种更原则化的方法就是，从实际的数据中学习这些参数。这是通过最大化数据集合$Z = {z_i}$在相关位置$X = {x_i}$和地图$m$下的似然度来实现的，其中$z_i$是一个实际的测量值，$x_i$是测量时所在的位置，$m$则是地图。数据$Z$的似然度表示如下： $$ \begin{equation}\label{f13} p(Z | X, m, \Theta) \end{equation} $$ 我们的目标是找到能够最大化似然度的内参$\Theta$。这种最大化数据似然度的算法称为极大似然估计maximum likelihood estimator，简记为ML估计。算法6.2是learn_intrinsic_parameters的伪代码，是对模型内参的极大似然估计。本质上，它就是一种EM算法(Expectation Maximization)，是一种估计ML参数的迭代过程。

算法6.2: 从测量数据中学习基于波束的传感器模型的内参。

这个算法需要一组对内参$\sigma_{hit},\lambda_{short}$比较好的初始化数据。在第3到第9行，它估计了辅助参数:每一个$e_{i,xxx}$都是由"xxx"计算的测量值$z_i$的概率，其中"xxx"是传感器模型的四个方面之一，hit, short, max和random。接着在第10到15行中估计内参，它们是之前计算的期望的函数。调整内参将导致期望发生改变，因此这个算法是一定是通过迭代的形式实现的。实际上这个算法收敛的很快，几次迭代之后基本上就可以给出一个足够好的结果。

图6.6中给出了四个用learn_intrinsic_parameters计算ML测量模型的例子。图(a)为超声传感器的数据，图(b)是激光传感器的数据。它们都是分别在不同距离下的测量和估计。测量数据以直方图的形式给出的。我们可以清晰地看到各个图片之间的差异。更小的$z_t^{k_*}$具有更高的精度。在近距离的情况下，两种传感器模型的高斯部分都要比远距离的窄。此外，激光区域扫描器要比超声的更精确，因此具有更窄的高斯部分和更小的$p_{max}$。另一个值得注意的事情是，short和random测量值相对较高的似然度。这个较大的误差似然度有一定的缺点也有优点。不好的方面是，它降低了传感器读数中的信息量，因为正好测得真值的似然度与随机测量之间的差异很小(???) 好的一方面是，未建模的系统扰动对这个模型的影响不大，比如说一个人在比较长的一段时间内挡住了机器人的去路(译者按:实际上我并不知道这个例子想要说明什么)。

(a). 不同距离下的声纳数据

(b). 不同距离下的激光数据

图6.6. 基于声纳数据和激光数据的光速模型的距离数据近似。左图中是用极大似然方法对图4中的数据估计结果。

如图6.7所示，列举了在实际应用中学习传感器模型的一个例子。图(a)是一个180度的距离扫描。机器人放置在占有栅格地图中的一个指定位置上。图(b)根据最大化朝向$\theta$，将环境地图中的似然度函数$p(z_t | x_t, m)$投影到$x$-$y$平面上。位置越深，机器人就越可能在那里。很容易看到，所有具有高似然度的区域都在走廊上。这就有点意思了，因为扫描出来的结果在几何上与楼道的位置更相似，不像是在房间中。实际上计算出来的概率密度散布在走廊中，说明一次传感器扫描并不能确定机器人的确切位置。这主要是走廊的对称性导致的。实际上后验概率收敛在两个狭窄的条带中，因为我们不知道机器人的方向，每一个条带对应着机器人的一种朝向。


(a). 激光扫描和部分地图	(b). 不同位置上的似然度
图6.7. 感知的概率模型(a)激光距离扫描，投影到请求的地图$m$中， (b)似然度$p(z_t \| x_t, m)$，为所有的位置$x_t$计算了一个概率值，并将之投影到地图中(灰色阴影部分)。对应位置颜色越深，$p(z_t \| x_t, m)$越大。

6.3.3 波束模型的数学推导

和之前所有的数学推导一样，第一次翻译暂时跳过。

6.3.4 一些实际的考虑

实际上，计算所有传感器读数的概率密度是很耗费时间的。比如说，激光传感器在每次扫描都会返回几百个数据，每秒钟还要扫描很多次。而且对每个扫描束都必须处理一个ray-tracing操作，考虑每一个可能的位置，对整个扫描过程进行积分从而得到目前的置信度，所以这个过程通常不能实时完成。解决这一问题的经典方法就是只用所有测量值中的一小部分数据(比如说，使用激光传感器中8个方向上的数据来代替360°的数据)。这种方法有一个很好的特点，剔除一部分测量值整个状态估计过程对于相邻测量的噪声就不那么敏感了，因为相邻的扫描束通常不是独立的。

如果相邻测量的依赖性很强，ML模型可能过拟合，从而得到一个次优解。一个简单的补救方法是用一个较弱版本的$p(z_t^k | x_t, m)^{\alpha}, \alpha < 1$来代替$p(z_t^k | x_t, m)$。其思想就是通过系数$\alpha$降低传感器测量值中的信息(log形式的概率密度为$\alpha \log{p(z_t^k | x_t, m)}$)。另一种可能的方法，就是在应用的上下文中学习这些内在参数：比如说，在移动定位中，可以通过梯度下降法来训练参数，在多次迭代之后获得较好的定位效果。这样的一个多次迭代的技术与这里所说的单次ML估计方法是完全不同的。在实际的实现中它可能得到惊艳的结果Thrun(1998a)。

基于测量束模型的计算瓶颈在ray casting操作上。我们可以通过对ray casting算法的预处理，来显著的降低计算$p(z_t | x_t, m)$的代价。把预处理的结果存到内存中，这样ray casting操作就变成了更快的查表操作。这种思想的一种实现就是将状态空间拆分为更细粒度的三维网格，并为每一个网格预先计算出一个$z_t^{k_*}$。我们已经在第4.1章中讨论过这一思想了。与网格的分辨率有关，这种方法可能需要很多内存。在移动机器人定位中，我们发现以15厘米和2度的分辨率划分网格就可以很好的解决室内定位问题了。它在RAM资源有限的计算机上工作的很好，有效的加速了在线cast ray的过程。

6.3.5 波束模型的限制

基于扫描束的传感器模型，受限于传感器的几何和物理形态，有两个主要的缺点。

具体来说，它缺少平滑性。在有很多零碎物件的杂乱环境中，分布$p(z_t^k | x_t, m)$在$x_t$上会非常不平滑。比如说会议室这样的一个有很多桌子椅子的环境。如第一章所提到的那样，机器人需要能够感知到这些障碍物的腿儿。显然，稍微改变一下机器人的位置$x_t$都会对传感器的扫描束有极大的影响。因此测量模型$p(z_t^k | x_t, m)$关于$x_t$是高度不连续的，尤其是对朝向$\theta_t$而言，因为方向角的很小的改变都会在$x$-$y$平面的距离检测产生很大的偏差。

不够平滑有两个问题。首先，任何近似的置信度表达都有可能漏掉正确的状态，因为真值附近状态的后验似然度可能截然不同。这限制了近似模型的准确性，增加了后验概率的误差。其次，查找最可能状态的爬坡方法将陷于局部极值，因为在这样的不平滑模型中存在着大量的局部极值。

此外，基于波束的模型的计算复杂度很高。针对每个传感器测量值$z_t^k$，计算$p(z_t^k | x_t, m)$时都需要ray casting操作，这是一个复杂度很高的任务。就像我们之前提到的，我们可以通过划分位姿空间为离散的网格，并预先计算一些值来部分解决这一问题。这样的方法用一个离线的初始化手段，提高了算法的运行效率。但是需要的网格数据是很大的，因为它需要覆盖一个很大的三维空间。因此预处理是用大量内存来换取计算时间的方法。

6.4 距离扫描的似然场

6.4.1 基本算法

现在我们要讨论另一种模型，称为似然场模型likelihood field model，它可以克服波束模型的限制。但是这个模型缺乏一个有道理的物理解释。实际上，它是一种自组织ad hoc的算法不需要计算与传感器相关的有意义的生成模型的条件概率。但是，这种方法在实际应用中工作的很好。尤其是它的后验概率，即使是在杂乱的环境中也是很平滑的，而且更高效。

其核心思想是，首先将传感器扫描值$z_t$的端点映射到地图空间中的全局坐标系中。如此一来，我们就需要知道机器人的局部坐标系相对于全局坐标的关系，也就是机器人传感器扫描束$z_k$的起点和指向。令$x_t = \begin{pmatrix}x & y & \theta \end{pmatrix}^T$表示$t$时刻机器人的位姿。为了保持观察世界的二维视角，我们认为传感器相对于机器人的位置是固定的，用局部坐标系$\begin{pmatrix} x_{k,sens} & y_{k, sens}\end{pmatrix}$表示，传感器扫描束的朝向则用相对于机器人朝向的角度$\theta_{k,sens}$来表示。这些数据都是与传感器相关的。如此，通过三角变换关系就可以将测量$z_t^k$的端点映射到全局坐标系中了。 $$ \begin{equation}\label{f32} \begin{pmatrix} x_{z_t^k} \\ y_{z_t^k} \end{pmatrix} = \begin{pmatrix} x \\ y \end{pmatrix} + \begin{pmatrix} \cos{\theta} & -\sin{\theta} \\ \sin{\theta} & \cos{\theta} \end{pmatrix} \begin{pmatrix} x_{k,sens} \\ y_{k,sens} \end{pmatrix} + z_t^k \begin{pmatrix} \cos{(\theta + \theta_{k,sens})} \\ \sin{(\theta + \theta_{k,sens})} \end{pmatrix} \end{equation} $$ 只有当传感器检测到障碍物这个坐标才有意义。如果距离扫描器取得了它的最大值，也就是$z_t^k = z_{max}$，那么这个坐标在物理世界中就没有意义(即使测量值携带了信息)。似然场测量模型直接就抛弃了这些最大量程的读数值。


(a). 实验环境	(b). 似然场
图6.8.(a)有三个障碍物的场景。机器人朝向图的底部，进行了一次测量$z_t^k$，如图中虚线所示。 (b)这种场景下的似然场，颜色越深，所对应的位置就越不可能检测到障碍物。在图6.9中显示了这一传感器测量的概率值$p(z_t \| x_t, m)$。

与之前讨论的扫描光束模型类似，我们假设有三种噪声和不确定性：

测量噪声。测量中的噪声由高斯函数建模。在$x$-$y$空间中，它涉及到查找地图中最近的障碍物。令$dist$表示坐标$\begin{pmatrix}x_{z_t}^k, y_{z_t}^k \end{pmatrix}^T$与地图$m$上最近障碍物之间的距离。那么传感器测量值的概率由一个零均值的高斯噪声模型描述： $$ \begin{equation}\label{f33} p_{hit}(z_t^k | x_t, m) = \varepsilon_{\sigma_{hit}^2}(dist^2) \end{equation} $$ 图6.8a描述了一个地图，在图6.8b中描述了测量点$\begin{pmatrix} x_{z_t^k} & y_{z_t^k} \end{pmatrix}$在2D空间中的高斯似然度。对应的区域越亮，就越有可能用区域扫描器测得一个障碍物。现在可以通过传感器轴向与似然场的交叉(包括归一化)操作获得密度$p_{hit}$，如图6.8中的虚线所示。在图6.9a中示例了该函数。
错误。和之前一样，我们假设最大距离读数有一个相对比较大的似然度。并用point-mass分布$p_{max}$对其建模。
未解的随机测量值。最后，用一个均匀分布$p_{rand}$对感知过程中的随机噪声建模。


(a). $p_{hit}(z_t^k \| x_t, m)$	(b). $p(z_t^k \| x_t, m)$
图6.9.(a)概率$p_{hit}(z_t^k)$是图6.8所描述场景下测量值$z_t^k$的函数。其中传感器扫描束扫过了三个障碍物，分别对应着三个测量点$o1, o2, o3$。(b)传感器概率$p(z_t^k \| x_t, m)$，通过加上两个平均分布得到。

与基于波束的传感器模型一样，似然场的概率$p(z_t^k | x_t, m)$是对这三个分布的加权求和： $$ \begin{equation}\label{f34} z_{hit} \cdot p_{hit} + z_{rand} \cdot p_{rand} + z_{max} \cdot p_{max} \end{equation} $$ 类似的，$z_{hit}, z_{rand}, z_{max}$是三个混合参数。图6.9b中显示了概率$p(z_t^k | x_t, m)$的分布图示。很容易看出，这个分布组合了图6.9a中所示的$p_{hit}$，以及分布$p_{max}$和$p_{rand}$。大部分调整混合参数的方法都可以迁移过来，用到这个新的传感器模型上。可以手动的调整，也可以通过极大似然法学习。如图6.8b所示，描述全局$x$-$y$坐标系中，检测到障碍物的似然度的函数，称之为似然场likelihood field。

算法6.3给出了一个使用似然场计算测量概率的方法。假设不同传感器扫描束的测量噪声是独立的，该算法在外层循环中将各个$p(z_t^k | x_t, m)$相乘，第4行检查传感器读数是否为最大值，若是则舍弃之。第5到8行中处理我们真正感兴趣的数据，其中第7行计算$x$-$y$空间中最近障碍物的距离。在第8行中将之与一个正态分布和一个均匀分布混合，得到这一距离的概率值。函数$\mathbb{prob}(dist, \sigma_{hit})$计算了$dist$在一个均值为0方差为$\sigma_{hit}^2$的正态分布下的概率。

算法6.3: 使用最近邻的欧式距离计算距离扫描器的似然度的算法。函数$\mathbb{prob}(dist, \sigma_{hit})$计算距离测量值在均值为0方差为$\sigma_{hit}$的高斯分布中的概率。

在这个算法中复杂度最高的操作就是第7行，在地图中查找最近邻的物体。为了加速这个搜索，可以预先计算似然场，将计算坐标系中测量值的概率就转换成了一个查表操作。当然，如果使用离散网格，查询的结果只是一个近似，它也可能返回一个错误的障碍物坐标。但是即使使用了一个很粗糙的粒度(moderately course grids)，其作用在概率$p(z_t^k | x_t, m)$上的影响也很小。

6.4.2 扩展

相比于扫描束模型，似然场模型的一个关键优势就是光滑。因为欧式距离是光滑的，机器人位姿$p_x$的微小改变对于分布$p(z_t^k | x_t, m)$而言，影响很小。另一个关键优势是其预处理操作是在2D空间中进行的，而不是3D空间，提高了预处理信息的紧致度。

但是当前的模型有三个关键的缺陷：首先，它没有显式地对人类和其它可能造成short reading的动态特性建模。其次，它认为传感器可以"透视(see through walls)"。这是因为ray casting操作被最近邻函数所替代了，最近邻函数不能确定地图中通往某点的路径被障碍物阻隔了。最后，该方法没有考虑地图的不确定性。特别的，它不能处理未测量unexplored的区域，这样的区域具有很高的不确定性或者说是unspecified。

我们可以通过对基本算法likelihood_field_range_finder_model的扩展，来减弱这些限制的影响。比如说，我们可以把地图占用情况分为三种：occupied, free, unknown。当有传感器测量值$z_t^k$落入未知区域中时，我们就认为其概率值$p(z_t^k | x_t, m)$是一个常数$\frac{1}{z_{max}}$。如此就得到了一个粗糙的概率模型，认定探测空间中所有传感器测量值出现的概率都是一样的。

图6.10显示了一个地图以及它的似然场。这里的$x$-$y$位置的灰度等级表示传感器读数的似然度。读者可能注意到只在地图内部inside应用了最近障碍物的距离，对应着探测的地形。在地图之外似然度$p(z_t^k | x_t, m)$是一个常数。就计算效率而言，在一个精细的二维网格中预先计算最近邻的距离是值得的。

图6.10. (a)San Jose Tech博物馆的占据格点地图，(b)预处理似然场

可视空间的似然场也可以通过最近扫描来定义，它实际上就是一个局部的地图。图6.11中显示了一个这样的似然场，它在对齐单次扫描结果的过程中扮演者重要角色。


(a). 传感器扫描结果	(b). 似然场
图6.11.(a). 俯视角度(a bird's eye perspective)下，传感器的扫描结果，机器人在图的底部，在其前方扫描出了接近180个点。 (b). 根据这个扫描结果生成的似然场，深色的区域检测到障碍物的概率较低，有障碍的地方是白色的。

6.5 基于关系的测量模型

目前已经有大量的描述测量值和地图之间的测量关系的距离传感器模型。一种常见的技术就是地图匹配map matching。它需要用到本书后续章节中要介绍的技术，将扫描结果转换为占据地图。特别的，地图匹配技术将少数扫描值编辑为局部地图local maps，标记为$m_{local}$。图6.12显示了一个占据栅格形式的局部地图。传感器测量模型对比局部地图$m_{local}$和全局地图$m$，两者越相似$p(m_{local} | x_t, m)$就越大。因为局部地图的形式与机器人位置有关，所以该对比操作需要把局部地图中的栅格映射到全局地图的坐标系中。这一映射过程可以通过类似公式($\ref{f32}$)的手段来实现。如果机器人在位置$x_t$上，我们用$m_{x,y,local(x_t)}$来表示局部地图中对应全局坐标$\begin{pmatrix} x & y \end{pmatrix}^T$的栅格单元。如果这两个地图都在相同的坐标系下，它们可以通过地图相关函数来对比： $$ \begin{equation}\label{f35} \rho_{m, m_{local}, x_t} = \frac{\sum_{x,y}(m_{x,y} - \bar{m}) \cdot (m_{x,y,local}(x_t) - \bar{m})} {\sqrt{\sum_{x,y}(m_{x,y} - \bar{m})^2 \sum_{x,y}(m_{x,y,local}(x_t) - \bar{m})^2}} \end{equation} $$ 上式的求和，是对两个地图中的栅格的运算。其中$\bar{m}$是平均地图值： $$ \begin{equation}\label{f36} \bar{m} = \frac{1}{2N} \sum_{x,y} (m_{x,y} + m_{x,y,local}) \end{equation} $$ 其中$N$表示局部和全局地图中重叠的元素数量。相关度$\rho_{m, m_{local}, x_t}$的值域为$±1$。地图匹配算法将下式用作局部地图在全局地图$m$和机器人位置$x_t$下的概率值。 $$ \begin{equation}\label{f37} p(m_{local} | x_t, m) = \max \{ p_{m, m_{local}, x_t}, 0 \} \end{equation} $$ 如果局部地图是通过一次单独扫描$z_t$生成的，就用这个匹配度来取代测量值概率$p(z_t | x_t, m)$。

地图匹配有一些很好的特性，与似然场模型类似，它很容易计算，只是在$x_t$上的概率值不一定是平滑的。通过对地图$m$卷积一个高斯平滑核，可以对似然场近似，并获得平滑特性，然后就可以在平滑的地图中运行地图匹配算法。(译者按:这点不是很理解，刚刚介绍似然场模型的时候还在说它是平滑的，怎么这里就不平滑了?)

地图匹配算法相比于似然场方法的一个主要优势就是它显式地考虑了两幅地图中的自由空间free-space。似然场技术只考虑了扫描的端点，并据此判定空间是否被占用(或者是噪声)。另一方面，很多建图方法所构建的局部地图都超出了传感器的探测范围。比如说，有些技术会构建一个围绕着机器人的圆形地图，覆盖了超出实际传感器测量范围的0.5的区域 (setting to 0.5 areas beyond the range of actual sensor measurements.)。这种情况存在一些风险，地图匹配的结果还覆盖了一些传感器测量范围之外的区域，就好像传感器可以透视"see through walls"一样。很多地图匹配算法都存在这一问题。地图匹配算法的一个更大缺点就是，没有一个合理的物理解释。相关性是地图间归一化的平方距离，而不是传感器的噪声特性。

6.6 基于特征的传感器模型

6.6.1 特征提取

到目前为止，我们讨论的传感器模型都是基于原始测量值的。还有一种建模方法，就是从测量值中提取出特征features。如果我们用函数$f$表示特征提取器，那么从距离测量值中提取的特征表示为$f(z_t)$。大多数特征提取器都是从高维的传感器测量空间中抽取一小部分特征。这种方法的关键优势是极大的降低了计算复杂度。在高维空间中复杂度很高的计算过程，在低维的特征空间中计算效率可以高出几个数量级。

本书并不探索提取特征的具体方法。有很多研究工作为不同的传感器提供了广泛的特征。对于距离传感器，这些特征通常是线、角落、或者是距离扫描中的局部最小值，它们对应着墙壁，角落，或者是障碍物。当使用摄像头进行导航时，对摄像机图像的处理就是机器视觉的范畴了。在机器视觉领域已经提出了大量的图像特征提取方法。流行的特征包括边缘、角、可分辨的模式、以及不同外观的物体。此外在机器人学中，也经常把一些地点定义为特征，比如说楼道、交叉路口等。

6.6.2 路标测量

在很多机器人应用中，特征对应着物理世界中的不同物体。比如说在室内环境中，门柱或者窗框等都可以当作特征；室外环境中，特征可以是树干或者建筑的一角。机器人学中，通常将这些物体称为路标landmarks，用于机器人导航。

处理路标最常见的模型都会假设传感器可以测量路标在机器人局部坐标系中的方位和距离。这样的传感器称为距离和方位角传感器range and bearing sensors。机器人装有这样的传感器，并不是一个很强的假设，任何从距离扫描中提取的局部特征都有距离和方位信息，从视频中提取的视觉特征也是如此。此外，特征提取器可能生成一个路标signature。在本书中，我们假设标记就是一个数值(比如，平均颜色)，也可以是用作记录路标特征类型的整数，或者是描述路标的一个多维向量(比如，高度和颜色)。

令$r$表示距离，$\phi$表示方位，$s$表示路标，特征向量可以通过一个三元组的集合来表示： $$ \begin{equation}\label{f38} f(z_t) = \{ f_t^1, f_t^2, \cdots \} = \left\{ \begin{pmatrix} r_t^1 \\ \phi_t^1 \\ s_t^1 \end{pmatrix}, \begin{pmatrix} r_t^2 \\ \phi_t^2 \\ s_t^2 \end{pmatrix}, \cdots \right\} \end{equation} $$ 每个时刻，机器人辨识出的特征数量都是不同的。但是很多概率机器人算法都假设特征之间是条件独立的，即 $$ \begin{equation}\label{f39} p(f(z_t) | x_t, m) = \prod_i p(r_t^i, \phi_t^i, s_t^i | x_t, m) \end{equation} $$ 如果每个测量值$\begin{pmatrix} r_t^i & \phi_t^i & s_t^i \end{pmatrix}^T$中的噪声对于其它测量值$\begin{pmatrix} r_t^j & \phi_t^j & s_t^j \end{pmatrix}^T$的噪声是独立的，其中$i \neq j$，那么就能应用条件独立性。在条件独立的假设下，我们可以像一些距离测量值模型那样，在一个时刻处理一个特征。这使得应用概率测量模型开发算法的过程更加简单。

现在我们设计一个针对特征的传感器模型。在本章一开始，我们将地图分成了两种：基于特征的feature-based和基于地点的location-based。路标测量模型通常只用于基于特征的地图。读者可能记得，这样的地图由一个特征的列表构成，$m = \{ m_1, m_2, \cdots \}$。每个特征可能具有一个标记和一个地点坐标location coordinate，记为$m_{i,x}$和$m_{i,y}$，就是它在地图的全局坐标系下的坐标。

通过标准的几何定律就可以生成无噪声的路标传感器的测量向量。我们将使用距离、方位、路标的高斯噪声函数来对路标感知过程中的噪声建模。如此构建的测量模型形式化的描述了$t$时刻，第$i$个特征与地图中第$j$个路标的对应关系。根据惯例，机器人位姿记为$x_t = \begin{pmatrix} x & y & \theta \end{pmatrix}^T$，有 $$ \begin{equation}\label{f40} \begin{pmatrix} r_t^i \\ \phi_t^i \\ s_t^i \end{pmatrix} = \begin{pmatrix} \sqrt{(m_{j,x} - x)^2 + (m_{j,y} - y)^2} \\ \mathbb{atan2}(m_{j,y}-y, m_{j,x} - x) - \theta \\ s_j \end{pmatrix} + \begin{pmatrix} \varepsilon_{\sigma_r^2} \\ \varepsilon_{\sigma_{\phi}^2} \\ \varepsilon_{\sigma_s^2} \end{pmatrix} \end{equation} $$ 其中$\varepsilon_{\sigma_r^2},\varepsilon_{\sigma_{\phi}^2}, \varepsilon_{\sigma_s^2}$都是均值为0的高斯噪声，它们的方差分别为$\sigma_r^2, \sigma_{\phi}^2, \sigma_s^2$。

6.6.3 已知关联度的传感器模型

数据关联问题data association problem是距离/方位角传感器的关键。当不能够唯一地识别出路标，就会产生这样地问题，此时关于路标地身份还存在一些不确定性。

在具体实现距离/方位角传感器模型时，我们需要建立特征$f_t^i$和地图中路标$m_j$之间的对应关系，并用一个称为关联度correspondence的变量来表示，记为$c_t^i \in \{1, \cdots, N+1\}$。其中$N$是地图$m$中路标的数量。如果$c_t^i = j ≤ N$，那么$t$时刻观察到的第$i$个特征就对应着地图中的第$j$个路标。换句话说，$c_t^i$是观察到的特征的真实身份。唯一的例外就是$c_t^i = N+1$，观察到的特征与地图$m$中的任何特征都没有关联。这种情况对于处理虚假路标很重要，它也是机器人建图中的一个重要话题，机器人有时会遇到之前没有观测到的路标。

算法6.4用于已知关联度$c_t^i ≤ N$的情况下，计算特征$f_t^i$的概率值的算法。第3和4行计算了路标的真实距离和方位。在第5行中在独立噪声的假设下计算了测量距离和方位的概率。读者可以很容易验证，这个算法是对公式($\ref{f40}$)的实现。

算法6.4: 一个计算路标测量值似然度的算法。这个算法以观测到的特征$f_t^i = \begin{pmatrix} r_t^i & \phi_t^i & s_t^i \end{pmatrix}^T$、特征的真实身份$c_t^i$、机器人的位姿$x_t=\begin{pmatrix} x & y & \theta \end{pmatrix}^T$以及地图$m$做为输入，其输出就是概率值$p(f_t^i | c_t^i, m, x_t)$。

6.6.4 采样位姿

有时我们期望获取具有特征身份$c_t^i$的测量值$f_t^i$所对应的机器人位姿$x_t$。在之前讨论机器人运动模型的章节中已经介绍过这样的采样算法，它也是传感器模型所需要的。比如说在确定机器人的全局位置时，如果能够生成具有传感器测量信息的位置样本，并据此对机器人的初始位置做出猜测，就很有用。

一般情况下，生成对应传感器测量值$z_t$的位姿样本$x_t$是很困难的，对于路标模型实际上有一种高效的采样算法。但是这样的方法还需要一些假设。具体就是，我们必须知道先验概率$p(x_t | c_t^i, m)$。为了简化模型，我们假设它是均匀分布的(通常情况下并不是!!!)。应用贝叶斯公式有: $$ \begin{align}\label{f41} p(x_t | f_t^i, c_t^i, m) & = \eta p(f_t^i | c_t^i, x_t, m) p(x_t | c_t^i, m) \\ & = \eta p(f_t^i | c_t^i, x_t, m) \end{align} $$ 对$p(x_t | f_t^i, c_t^i, m)$采样可以通过求传感器模型$p(f_t^i | c_t^i, x_t, m)$的逆获得。算法6.5描述了采样位姿$x_t$的算法。这个算法很巧妙，因为即使在没有噪声的情形下，通过一次路标观测，有时也不能唯一的确认机器人的位置。实际上，以路标为中心的一个圆上都可能是机器人的位置，这个元的直径就是机器人到路标的距离。这种机器人位姿的不确定性也来自于三维空间中距离和位姿所提供的两个约束。

算法6.5: 对已知身份$c_t^i$的路标测量$f_t^i = \begin{pmatrix} r_t^i & \phi_t^i & s_t^i\end{pmatrix}^T$进行位姿采样的算法。

为了实现一个位姿采样器，我们必须对剩余的自由参数采样，它们确定了机器人在围绕着路标的圆周上的具体位置。这个参数就是算法6.5中的$\hat{\gamma}$，在第3行中被随机选择出来。第4和第5行对测量的距离和方位加入了噪声，利用了高斯函数中对称的对待均值和方差的事实。最后在第6到8行重现了对应于$\hat{\gamma}, \hat{r},\hat{\phi}$的位姿。

图6.13中列举了位置概率密度$p(x_t | f_t^i, c_t^i, m)$以及算法6.5的采样点。这个后验概率被投影到了$x$-$y$空间中，形成了一个围绕着路标的圆环，半径为$r_t^i$表示测量距离。在3-D位姿空间中，将之按照角度$\theta$展开就是一个螺旋。


图6.13. 路标检测模型:(a)机器人在5m和30度方位角(在2D上的投影)检测到路标的位姿后验分布。 (b)根据这一测量值中生成的机器人位姿样本。短线指示了样本的朝向。

6.6.5 更多的考量

这两种基于路标的测量算法都假设已知关联度。在后续的章节中，介绍未知关联度情况下的定位和建图算法时，我们将详细讨论未知关联度的情况。

关于路标的身份，我们还需要多说两句。大多数公开的算法都没有显式地使用外观特征。如果没有提供路标的身份时，那么所有的路标看起来都一样，那么就更难计算关联度了。我们已经在模型中加入了这个标记，因为它是一个很有用的信息，总是可以很容易从传感器测量值中获得。

如上所述，使用特征来代替测量向量的动机是出于计算复杂度的考量，因为管理几百个特征远比几百万个距离测量值更容易。这里描述的模型非常粗糙，而且没有考虑特征背后的物理定律。尽管如此，这种模型在很多应用中表现的也很不错。

需要注意的是，从测量值塌缩到特征值也是有代价的。在机器人学中，特征通常被误认为是测量向量$z_t$的完全统计。令$X$表示感兴趣的变量(比如地图、位姿)， $Y$是一些附加信息，比如历史的传感器测量值，那么如果下式成立，$f$就是$z_t$的充分统计量。 $$ \begin{equation}\label{f42} p(X | z_t, Y) = p(X | f(z_t), Y) \end{equation} $$ 实际上用特征值代替完全的测量值向量将会随时很多信息，这加大了一些的问题的难度，比如说确定机器人是否曾今探索过某个区域的数据关联问题。通过反思很容易理解特征提取的影响，当你睁开眼睛的时候，周围环境的视觉图像可能已经足够告知你的位置了，即使你事先并不知道自己在全局中的位置。另一方面，如果你只检测了某些特定的特征，比如说相对于门框或者窗子的位置，那么你很可能不太能够确定自己在哪里。尤其是在全局信息不充分的情况下，可用的信息就更少了。

随着高速计算机的到来，特征在机器人学领域中的重要性就逐渐下降了。尤其是在使用距离传感器的时候，大多数现有的算法都依赖于密集的测量向量，它们使用密集的基于位置的地图描述环境。但是特征对于教学而言还是很有意义的，它为概率机器人学引入了许多基本的概念，让我们能够合理地处理一些问题。比如说对应关系问题，这些算法也能够处理密集的扫描点。因此，本书中的很多算法一开始都是针对特征描述的，然后才扩展到使用原始传感器测量值的。

6.7 实际的考量

本部分中我们研究了一系列的测量模型。由于距离传感器在机器人学中具有很重要的地位，我们给予其更多的关注。但是，这里所讨论的模型仅仅是众多概率学模型的代表。选择合适的模型，实际上是在物理现实和算法特性之间的权衡。比如说距离传感器的物理模型，在描述机器人位置的概率分布时不具有平滑特性，反过来会给算法带来一些问题。当然，物理现实并不是选择正确传感器模型的唯一标准，算法对模型的适应性也是一种重要的判断准则。

一般来说，模型越准确越好。也就是说，我们从传感器测量值中提取的信息越多越好。基于特征的模型提取的信息相对较少，这是由于特征提取器把高维的传感器测量值映射到了低维的特征空间中。因此基于特征的方法的结果比较次。这一缺点是基于特征的表示方法的高效计算特性所付出的代价。

在调整测量值模型的内参时，人为的扩大不确定度通常很有用。这是由概率学方法的核心限制所导致的，为了使概率学技术更容易计算，我们不得不忽略物理世界中存在的耦合关系，以及产生这些耦合关系的无数潜在因素。因为没有对这些耦合关系建模，对于多个测量结果的融合很快就会产生偏差。这些偏差最终将导致错误的结论，对结果造成负面的的影响。实际上，适当的降低传感器获得的信息是一种较好的常用手段。将测量值映射到低纬度的特征空间就是这样一种方法。但是它面临着上面提到的各种限制。在6.3.4节中所讨论的，通过参数$\alpha$指数化的均匀的减少信息是一个更好的方法，它不会在概率学算法的结果中引入额外的方差。

6.8 总结

本部分讨论了概率学的测量值模型。

首先讨论了距离传感器(特别是激光传感器)的测量值模型$p(z_t^k | x_t, m)$。第一个这样的模型使用ray casting确定特定地图$m$和位置$x_t$下$p(z_t^k | x_t, m)$的分布形状。我们设计了一个混合模型解决能够影响距离测量值的各种不同类型的干扰。
我们还讨论了通过极大似然方法来确定测量模型中内在的噪声参数。因为测量模型是一个混合模型，所以我们设计了一个迭代过程来进行极大似然估计。这是一种期望最大值(expectation maximization, EM)算法的实现。这类算法有两个过程，称为期望过程和最大化过程。在期望过程中计算各种类型的测量噪声期望，在最大化过程中寻找一组最适合当前期望的模型内参。EM算法就在这两个过程中来回切换，迭代地进行估计。
另一种距离传感器的测量值模型是基于似然场的。这种技术使用二维坐标系中最近的距离来对概率值$p(z_t^k | x_t, m)$建模。这种方法能够提供一个平滑的分布$p(z_t^k | x_t, m)$。但是它忽略了自由空间相关的信息，没有考虑到距离测量中的碰撞问题。
第三种测量模型是基于地图匹配的。这种方法根据传感器扫描值建立一个局部地图，并将之与全局地图建立映射关系。这种方法缺少物理解释，但是非常好用。
我们还讨论了如何通过预处理技术来降低实时计算的复杂度。在基于波束的测量模型中，预处理技术是在3D空间中进行的，而似然场技术则只需要2D的预处理技术。
这里列出了一种基于特征的传感器模型。在该模型中，机器人提取附近路标地距离、方位和记信息。基于特征的技术从原始的传感器测量值中提取不同的特征，能够把传感器测量值的维度降低好几个数量级。
在本章的最后，讨论了一些实际的问题，指出了一些在具体实现中可能遇到的坑。

6.9 参考文献

本章只是简单浏览了一下传感器的物理模型。在文献Blahut et al. (1991); Grunbaum et al. (1992); Etter (1996)中提供了更精确的声纳传感器模型。激光雷达的模型可以在文献Rees (2001)中找到。Sahin et al. (1998)通过实验研究了噪声模型。与这些模型相比，本章中所讨论的模型非常粗糙。

早期使用波束模型对距离传感器建模的工作可以参考文献Moravec (1988)。后来Burgard et al. (1996)在移动机器人定位中应用了一个类似的模型。本章所讨论的波束模型和预处理技术最早是由Fox et al. (1999b)提出的。似然场技术最早由Thrun (2001)发布，它与大量的扫描匹配技术(Besl and McKay 1992)有着密切的联系。实际上，它们可以看作是对Konolige and Chou (1999)相关模型的软变形(soft variant)。计算占用栅格地图之间的相似度的方法也很流行。Thrun (1993)计算了栅格地图中各个单元的差异平方和。 Schiele and Crowley (1994)提供了包括基于关联度的方法在内的不同的模型比较方法。Yamauchi and Langley (1997)分析了动态环境中的地图匹配方法的鲁棒性。 Duckett and Nehmzow (2001)将占用地图转换成了可以高效匹配的直方图。

路标的距离和方位角的测量是SLAM技术中老生常谈的问题了。最早可能是由Leonard and Durrant-Whyte (1991)提及的。在早期的研究中，Crowley (1989)设计了一种直线物体的测量模型。

6.10 练习

暂略


(a). 高斯分布\(p_{hit}\)	(b). 指数分布\(p_{short}\)	(c). 均匀分布\(p_{max}\)	(d). 均匀分布\(p_{rand}\)
图 6.3. 距离扫描传感器模型的组件。横坐标对应着测量值\(z_t^k\)，纵坐标表示其似然度。


(a). 激光扫描和部分地图	(b). 不同位置上的似然度
图6.7. 感知的概率模型(a)激光距离扫描，投影到请求的地图\(m\)中， (b)似然度\(p(z_t \| x_t, m)\)，为所有的位置\(x_t\)计算了一个概率值，并将之投影到地图中(灰色阴影部分)。对应位置颜色越深，\(p(z_t \| x_t, m)\)越大。


(a). 实验环境	(b). 似然场
图6.8.(a)有三个障碍物的场景。机器人朝向图的底部，进行了一次测量\(z_t^k\)，如图中虚线所示。 (b)这种场景下的似然场，颜色越深，所对应的位置就越不可能检测到障碍物。在图6.9中显示了这一传感器测量的概率值\(p(z_t \| x_t, m)\)。


(a). \(p_{hit}(z_t^k \| x_t, m)\)	(b). \(p(z_t^k \| x_t, m)\)
图6.9.(a)概率\(p_{hit}(z_t^k)\)是图6.8所描述场景下测量值\(z_t^k\)的函数。其中传感器扫描束扫过了三个障碍物，分别对应着三个测量点\(o1, o2, o3\)。(b)传感器概率\(p(z_t^k \| x_t, m)\)，通过加上两个平均分布得到。