概率机器人（Probability Robotics）笔记 Chapter 6: 测量（Measurements）

1. 简介

测量模型描述了在物理世界中传感器数据的生成。
概率机器人学将噪声显式地融入传感器测量模型中。
测量模型定义为条件概率$p(z_t|x_t,m)$
对于超声波的噪声：
1.偏大：以特定角度测量光滑平面->反射（specular reflection效应）
2.偏小：sensor之间cross talk
传感器模型越准确越好，但不可能完全准确（太花时间，且外部世界状态变量可能未知）
实际中，非常粗糙的传感器模型也可以工作。这就是概率机器人学的优势
传感器同时有多个数据，假设噪声互相独立，即有$p(z_t|x_t,m)={\prod }_{k=1}^{k}p(z_t|x_t^k,m)$

2. 地图

环境的地图是一个环境中的物体与其位置的表单
地图常以两种方式索引：基于特征，和基于位置。各有优劣
基于位置的描述为Volumetric，标记所有位置，即不仅包含有物体的信息，也包含没物体的信息
基于特征的描述可以更简单的对物体进行移动。
一类经典的基于位置描述的地图为Occupancy Grid Map，适合用来搞Navigation

3. 测距传感器的光束模型

测距传感器可以是光束的（如激光），也可以是锥形的（如超声波）

3.1 基础测量算法

模型包含四种测量误差：较小的测量误差，意料之外的物体引发的误差，传感器探测物体失败，随机的不可解释的误差。

1. 正确测距+局部测量误差：定义$z_t^{k\ast }$为$z_t^k$应测得的真实距离，由$x_t$和$m$决定，则测量值为窄分布的高斯噪声，均值为$z_t^{k\ast }$，标准差为${\sigma }_{hit}$，标记本高斯分布为$p_{hit}$。实际中测量数据必大于0，小于传感器量程，因此高斯分布仅保留$[0;z_{max}]$部分，再进行概率归一化。
2. 意料之外的物体：可导致测量数据非常短。可以将这种物体视作环境状态变量，也可以视作传感器噪声。按后者时，测量比真实距离小。探测到这种物体的概率随距离指数减小，概率分布的参数${\lambda }_{short}$是模型的内参，概率分布函数是${\lambda }_{short}{e}^{-{\lambda }_{short}{z}_t^k}$取$[0;z_t^{k\ast }]$部分，再概率归一化。
3. Failures: 例如以大角度照射光滑平面、照射吸光平面、强光下进行测量。通常返回最大测量值$z_{max}$，所以是在该处的冲击函数分布，实际中建模为一个窄平均分布或窄高斯分布。
4. 随机测量：不可解释，在$[0;z_{max}]$上平均分布

以上四种分布按权重（$z_{hit}$，$z_{short}$，$z_{max}$，$z_{rand}$）线性叠加，四个系数相加为1。

光束测距模型算法：输入为测量$z_t$（多个光束），机器人位姿$x_t$，还有地图$m$。将某一个测量向量的可能性，是所有单个光束可能性的乘积，即假设光束之间独立。具体算法：

1····Algorithm beam range finder model ( $z_t;x_t;m$ ):
2····$q=1$
​3····for $k=1$ to $K$ do
4········compute $z_t^{k\ast }$ for the measurement $z_t^k$ using ray casting
5········$p=z_{hit}\cdot p_{hit}(z_t^k|x_t,m)+z_{short}\cdot p_{short}(z_t^k|x_t,m)$
6··············$+z_{max}\cdot p_{max}(z_t^k|x_t,m)+z_{rand}\cdot p_{rand}(z_t^k|x_t,m)$
7········$q=q\cdot p$
8····return $q$

3.2 调整模型内参

定义模型的内参集合为$\Theta$，包括叠加权重（$z_{hit}$，$z_{short}$，$z_{max}$，$z_{rand}$）和分布参数（${\sigma }_{hit}$与${\lambda }_{short}$。

任何测量数据的概率都是$\Theta$的函数。

可以通过数据确定内参：

1. 可以手调参数，直到感觉符合数据；
2. 也可以从数据中学习参数，即通过调整$\Theta$，最大化$p(Z|X,m,\Theta )$，其中$Z$是数据，$X$是位置，可以用最大似然估计器（ML estimator）：需要比较好的初始${\sigma }_{hit}$与${\lambda }_{short}$估计。算法思想是先估计第$i$个测量值分别由四种情况产生的可能性如$e_{i,hit}=\eta \cdot p_{hit}(z_i|x_i,m)$，再以之计算内参，包括$z_{hit}=|z{|}^{-1}{\Sigma }_i{e}_{i,hit}$等线性权重，还有${\sigma }_{hit}=\sqrt{\frac{1}{{\Sigma }_i{e}_{i,hit}}{\Sigma }_i{e}_{i,hit}(z_i-z_i^{\ast }{)}^2}$与${\lambda }_{short}=\frac{{\Sigma }_i{e}_{i,short}}{{\Sigma }_i{e}_{i,short}{z}_i}$。不断迭代，直到收敛。具体伪代码不再列写。

模型中大的误差（噪声）概率有两面性：坏处是传感器读数传达的信息量更小，好处是这样的模型更鲁棒。

3.3 最大似然估计器（ML Estimator）的数学推导

有心情的时候再好好看吧…先跳啦…

3.4 实践指南

实际中，传感器可能有几百个beam，每秒好几帧数据，不可能都ray-trace然后再把概率乘起来得到一个总的置信度，因此通常只选取几个均匀分布的测量光束，好处是这样的光束更满足相互独立的假设，而相邻的光束通常有相关的噪声。

如果相邻的光束相关性太强，可以把$p(z_t^k|x_t,m)$换成更弱的版本$p(z_t^k|x_t,m{)}^{\alpha }$，其中$\alpha <1$。这样做的原理是减少传感器数据中提取的信息。还可以在实际应用的场景中学习内参，例如可以用多步的梯度下降。

计算光束模型最花时间的步骤是做ray casting。可以提前计算ray casting算法，存在内存里，到时候查表。一种常用的做法是把状态空间结构成细致的三维网格，然后提前算好对不同cell的$z_t^{k\ast }$。在室内移动机器人应用中，通常分辨率可以取15cm和2°。

4. 距离传感器的似然场（Likelihood Fields）

4.1 基础算法

光束模型的优点是与测距传感器的几何和物理紧密相关，但是有两个缺点：

1. 不平滑：在有很多小障碍物的零乱空间中，$p(z_t^k|x_t,m)$对$x_t$可能很不平滑，导致近似的置信度会与真实的不同，也就是对近似的精确度要求更高。且梯度上升（hill climbing）找最大似然状态的方法会陷入局部极值。
2. 计算复杂：ray casting要么要在线算，要么占内存（上一章节）。

另一种模型为似然场模型（likelihood field model），跟物理模型无关，但在零乱空间中更平滑，且计算更快。

首先，已知机器人状态$x_t=(\begin{array}{ccc}x& y& \theta \end{array}{)}^T$与第$k$个传感器与机器人的相对位姿$(\begin{array}{ccc}{x}_{k,sens}& y_{k,sens}& {\theta }_{k,sens}\end{array}{)}^T$，将传感器测量的测量点$z_t^k$转换到世界坐标系下，得到($\begin{array}{cc}{x}_{z_t^k}& y_{z_t^k}\end{array}{)}^T$。只有当传感器检测到障碍物（即测量值不是最大值）时，这些转换的坐标才有意义，所以$z_{max}$数据被扔掉。

与光束模型类似，假设三种噪声源：

1. 测量噪声（与光束模型不同）：对地图上所有点，计算测量出障碍物的可能性$p_{hit}$。这可能性取决于这个点到离他最近的障碍物的距离（类似距离场），然后测量概率是零均值高斯，即$p_{hit}(z_t^k|x_t,m)={ϵ}_{{\sigma }_{hit}^2}(dis{t}^2)$。得到整个地图所有cell的$p_{hit}$后，取与光束相交的部分，再归一化。
2. Failures：返回最大值，有一个类似冲击函数的分布$p_{max}$
3. 随机测量：在$[0:z_{max}]$上的均匀分布$p_{rand}$

测量模型概率即三者以权重$p_{hit}$，$p_{rand}$，$p_{max}$线性叠加。

调整内参的方法与光束模型相同，可手调，也可以用最大似然估计器。

算法：
1····$q=1$
2····for all $k$ do
3········if $z_t^k̸=z_{max}$
4············compute end point in world frame ($x_{z_t^k}$, $y_{z_t^k}$) and dist^2
5············$q=q\cdot (z_{hit}\cdot prob(dis{t}^2,{\sigma }_{hit}^2)+\frac{z_{random}}{z_{max}})$
6····return $q$

计算量最大的一步是找最近的障碍物，得到$dis{t}^2$（不过有距离场计算的包？SDF？）。可以将环境离散化（2D），然后提前计算好，到时候查表。

4.2 扩展

由于欧氏距离平滑，所以似然场得到的结果也比较平滑。

缺点：

1. 模型中没有显式加入动态障碍物（如人）。
2. 认为传感器可以看穿墙，因为不用ray casting了。
3. 不考虑地图的不确定性，不可用于未经探索的区域。不过可以将未知区域的测量概率设为常量$\frac{1}{z_{max}}$，即在未知区域内各种测量的概率相等（问：难道不应该是有障碍物的概率相等，所以测量概率越近越大？答：似然场的方法中，测量概率只与这个点到最近障碍物的距离有关，即仅与这个点的局部几何特征相关，而与光束追踪的历史无关，所以有障碍物等概率与测量等概率是等价的）。

现在的似然场用的是扩展的对称算法。

5 基于相关性的（Correlation-Based）传感器模型

有很多测距传感器模型，测量的是一次measurement和地图之间的相关性。
一个常用的技巧叫map matching，可以将scan转换成occupancy grid。
通常是将少数几个连续的scan叠加为一个局部地图，记为$m_{local}$。
传感器测定模型将$m_{local}$与全局地图$m$作对比，也就是两者越像则$p(m_{local}|x_t,m)$越大。局部地图的cells首先要变换到全局地图坐标系中。
地图相关性函数：
${\rho }_{m,m_{local},x_t}=\frac{{\Sigma }_{x,y}(m_{x,y}-\bar{m})\cdot (m_{x,y,local(x_t)}-\bar{m})}{\sqrt{{\Sigma }_{x,y}(m_{x,y}-\bar{m}{)}^2{\Sigma }_{x,y}(m_{x,y,local(x_t)}-\bar{m}{)}^2}}$

上式中，$\bar{m}$是全局和局部地图的重叠区域的总均值，得到的相关性取值分为是$[-1,1]$。然后，map matching认为$p(m_{local}|x_t,m)=max\{{\rho }_{m,m_{local},x_t},0\}$。

地图匹配法的优点：容易计算（但是结果不平滑）。一种得到似然场的方式就是先给地图做高斯模糊（用高斯核卷积），然后在结果中做地图匹配。相比似然场方法，地图匹配的一大优势就是显式考虑了两个地图中的自由空间，而前者只考虑了对应障碍物的传感器测量点。

地图匹配的缺点：很多建图方法构建的地图超出了传感器测量范围，从而影响地图匹配。地图匹配方法也没有一个物理解释。相关性定义为地图距离平方的归一化，与传感器的噪声模型没有关系。

6 基于特征的传感器模型

6.1 特征提取

前文所讲的传感器模型都是基于传感器的原始数据（raw data）。
定义特征提取函数$f$，则得到的特征是$f(z_t)$。
通常从高维传感器测量数据中提取少量的特征。
一大优势是降低计算复杂度。
对于测距传感器，通常提取线、角、局部极小值，对应墙壁、角落、物体。

6.2 地标测量（Landmark Measurements）

机器人应用中，特征对应物理世界中不同物体，室内室外不同。将这些物理物体称之为地标，表明他们被用于导航。

每一个地标对应的数据包括相对机器人的距离（$r$）和方向($\varphi$)，也包括一个签名（$s$，如种类、颜色、高度等）。

通常假设不同特征条件独立，即$p(f(z_t)|x_t,m)={\Pi }_{i}p(r_t^i,{\varphi }_t^i,s_t^i|x_t,m)$，当不同特征测量的噪声相互独立时成立，此时可以对不同特征进行依次（独立）处理。

基于特征的传感器模型：对距离、方向、签名各有独立的高斯噪声，对于特征地图中第$j$个地标，生成$t$时刻测量的第$i$个特征，模型简单的由几何关系得到，不写了。

6.3 已知配对(Correspondence)时的传感器模型

对一个特征$f_t^i$，用变量$c_t^i$存储它的身份，可以取值为$1$到$N+1$，$N$是地图中地标总数，即如果$c_t^i=j$，则$f_t^i$就对应地图中第$j$个特征。当没有匹配时，$c_t^i=N+1$。

计算得到一个有匹配$c_t^i$的特征$f_t^i$的算法是首先计算应有的距离$\hat{r}$、方向$\hat{\varphi }$和签名$s_j$，然后根据距离和高斯噪声模型得到概率，即$q=prob(r_t^i-\hat{r},{\sigma }_r^2)\cdot prob({\varphi }_t^i-\hat{\varphi },{\sigma }_{\varphi }^2)\cdot prob(r_s^i-\hat{s},{\sigma }_s^2)$

6.4 采样位姿

有时需要对对应某特征$f_t^i$的机器人位姿$x_t$进行采样。在地标模型下，这个采样可以比较高效，但需要额外的假设：需要知道先验概率$p(x_t|c_t^i,m)$。由于距离和方向提供了两个自由度的信息，而机器人状态是三维的，所以需要在第三个维度上采样，同时叠加一个高斯。采样的结果通常是围绕地图上地标的环形分布。

6.5 其他考虑

上述的两种算法都是基于配对已知。
关于特征的签名，大多数算法没有使用，也就是不对地标进行区分。但签名容易提取，且可以提供有价值的信息。

从测量数据中提取特征是有代价的，虽然在研究中通常认为$p(f(z_t)|x_t,m)\approx p(z_t|x_t,m)$，但实际中牺牲了很多信息。
*现在计算机发达，特征方法已经不太重要了。*大多数state-of-the-art的方法都使用稠密的测量数据。

7. 实际考虑

在选择传感器模型时，通常要权衡物理真实度和其他属性，例如平滑性（没有平滑性对后续算法不好，如粒子滤波器）。
总的来说模型越准越好，从传感器中能提取的信息越多越好。
在调模型内参的时候，可以把不确定性放大一些，因为其实没有对所有的噪声源进行建模，导致模型可能会过度自信。所以可以减少从测量中提取的信息，如在3.4节中讲的。