概率机器人测距仪波束模型
基本模型
《概率机器人》中针对测距仪，建立了一个波束模型。将测距仪在测量过程中的误差分为四类：测量噪声、意外对象引起的误差、由测距仪物理原理引起的误差和随机意外噪声。
测量噪声
理想情况下，测距仪总是测量位于其测量领域内的最近物体的正确距离。这个测量值是理想意义下准确的，假设由 $z_t^k $ 表示。但是，在实际情况中，由于各种误差的存在，是的我们获取的测量值与理想情况下的值存在一定的误差，在此由 $z_t^{k * } $ 表示，并且用一个均值为 $z_t^{k * } $ 、标准差为 $\sigma_{hit} $ 的高斯分布来建模这个误差的情况。因此，测量的概率有下式给出：
$$
\begin{align}
p_{hit}(z_t^k|x_t, m) & = \eta N(z_t^k; z_t^{k * }, \sigma_{hit}^2) \quad 0 \le z_t^k \le z_{max}\\
N(z_t^k; z_t^{k * }, \sigma_{hit}^2) & = \frac{1}{\sqrt{2 \pi \sigma_{hit}^2}}e^{ - \frac{1}{2} \frac{(z_t^k - z_t^{k * })^2}{\sigma_{hit}^2}}\\
\eta & = (\int_0^{zmax} N(z_t^k; z_t^{k * }, \sigma_{hit}^2) dz_t^k )^{-1}
\end{align}
$$
意外对象引起的误差
由于移动机器人的环境是动态的，而导航的地图 $m$ 是静态的。因此，地图中不包含的对象可能会引起测距仪测量上的误差。最典型的对象就是与机器人在同一空间中的人。处理这类对象的一般方法有2种：1.将这类对象作为状态向量的一部分来处理；2.把他们当做测量噪声进行处理。
在波束模型中，将这类对象当做测量的噪声进行处理。由于这类对象的出现，测距仪探测到的信息会出现与环境特定的距离明显有减少，而且总是只返回距离测距仪最近的距离信息。所以用数学上的指数分布来对他进行描述。
$$
\begin{align}
p_{short}(z_t^k|x_t,m) & = \eta \lambda_{short}e^{-\lambda_{short}z_t^k}\\
\eta &= \frac{1}{1-e^{-\lambda_{short}z_t^{k * }}}
\end{align}
$$
由测距仪物理原理引起的误差
有时环境中障碍会被完全忽略。例如，在声呐传感器遇到了镜面反射时，会经常发生此情况。当用激光测距仪检测到黑色吸光的对象时，或者某些激光系统在明媚的阳光下测量物体时，也会发生检测失败。
传感器检测失败(sensor failure)的典型结果是最大距离测量(max-range measurement)间题：传感器返回它的最大允许值 $z_{max}$ 。由于这样的事件经常发生，那么在测量模型中明确地建立最大测量范围的模型就很必要。
波束模型使用一个以 $z_{max}$ 的点群分布来建立模型:
$$
p_{max}(z_t^k|x_t, m) = I(z=z_{max}) = \left {
\begin{array}{rl}
1 \quad & z=z_{max}\\
0 \quad & 其他
\end{array} \right.
$$
随机意外噪声
最后，测距仪偶尔会产生完全无法解释的测量 (unexplainable measurements) 。例如，当超声波被几面墙反弹或者它们受到不同传感器之间的串扰时，声呐常产生幻读。为了使之简单化，对千这样的测量，这里将使用一个分布在完整传感器测量范围 $[0,z_{max}]$ 的均匀分布来建立模型：
$$
p_{rand}(z_t^k|x_t, m) = \left {
\begin{array}{rl}
\frac{1}{z_{max}} \quad & 0 \le z_t^k \le z_{max}\\
0 \quad & 其他
\end{array} \right.
$$
波束模型的综合
上面给出了波束模型可能遇到的4中噪声，波束模型中，将上面4中噪声通过四个参数： $z_{hit}、 z_{short}、z_{max}、 z_{rand}$ 进行加权平均混合，且 $z_{hit}+z_{short}+z_{max}+z_{rand}=1$，有：
$$
p(z_t^k| x_t,m) = \left (
\begin{array}{ccc}
z_{hit}\\
z_{short}\\
z_{max}\\
z_{rand}
\end{array} \right )^T \cdot \left (
\begin{array}{ccc}
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{array}
\right )
$$
beam_range_finder_model 算法
$$
\begin{align}
\text{1:} \qquad \qquad & \text{Algorithm beam_range_finder_model}(z_t,x_t,m)\\
\text{2:} \qquad \qquad & q=1\\
\text{3:} \qquad \qquad & \text{for } k=1 \text{ to } K \text{ do}\\
\text{4:} \qquad \qquad & \qquad \text{compute } z_t^{k * } \text{ for the measurement } z_t^k \text{ using ray casting}\\
\text{5:} \qquad \qquad & \qquad q = z_{hit} \cdot p_{hit}(z_t^k|x_t,m) + z_{short} \cdot p_{short}(z_t^k|x_t,m) + z_{max} \cdot p_{max}(z_t^k|x_t,m) + z_{rand} \cdot p_{rand}(z_t^k|x_t,m)\\
\text{6:} \qquad \qquad & \qquad q = q \cdot p\\
\text{7:} \qquad \qquad & \text{return } q
\end{align}
$$
模型调参
这个模型调参的有一个通用方法就是：手动调参。手动调参需要基于工程经验来，我没有调过，也不知道怎么调。这里介绍一下《概率机器人》书中提到的调节模型餐宿的算法（adjusting model parameters）。
模型调参算法
$$
\begin{align}
\text{1:} \qquad \qquad & \text{Algorithm learn_intrinsic_parameters}(Z,X,m)\\
\text{2:} \qquad \qquad & \text{repeat until convergence ceitierion satisfied}\\
\text{3:} \qquad \qquad & \text{for all } z_i \text{ in } Z \text{ do}\\
\text{4:} \qquad \qquad & \qquad \eta = [z_{hit} \cdot p_{hit}(z_t^k|x_t,m) + z_{short} \cdot p_{short}(z_t^k|x_t,m) + z_{max} \cdot p_{max}(z_t^k|x_t,m) + z_{rand} \cdot p_{rand}(z_t^k|x_t,m)]^{-1}\\
\text{5:} \qquad \qquad & \qquad \text{caclulate } z_i^{ * } \\
\text{6:} \qquad \qquad & \qquad e_{i,hit} = \eta \cdot p_{hit}(z_t^k|x_t,m)\\
\text{7:} \qquad \qquad & \qquad e_{i,short} = \eta \cdot p_{short}(z_t^k|x_t,m)\\
\text{8:} \qquad \qquad & \qquad e_{i,max} = \eta \cdot p_{max}(z_t^k|x_t,m)\\
\text{9:} \qquad \qquad & \qquad e_{i,rand} = \eta \cdot p_{rand}(z_t^k|x_t,m)\\
\\
\text{10:} \qquad \qquad & z_{hit} = |Z|^{-1} \sum_i e_{i,hit}\\
\text{11:} \qquad \qquad & z_{short} = |Z|^{-1} \sum_i e_{i,short}\\
\text{12:} \qquad \qquad & z_{max} = |Z|^{-1} \sum_i e_{i,max}\\
\text{13:} \qquad \qquad & z_{rand} = |Z|^{-1} \sum_i e_{i,rand}\\
\text{14:} \qquad \qquad & \sigma_{hit} = \sqrt{\frac{1}{\sum_i e_{i,hit}}\sum_i e_{i,hit}(z_i - z_i^{ * })^2}\\
\text{15:} \qquad \qquad & \lambda_{short} = \frac{\sum_i e_{i,short}}{\sum_i e_{i,short} z_i}\\
\text{16:} \qquad \qquad & \text{return } \Theta = \left { z_{hit}, z_{short}, z_{max}, z_{rand}, \sigma_{hit}, \lambda_{short} \right }
\end{align}
$$
模型调参的数学推导
暂时略过。
实际应用考虑
从计算的角度来看，计算所有传感器读数的密度是非常复杂的。因为对每一个波束扫描和每一个可能的姿态都要进行射线投射操作，因此不能总是实时地将整个扫描综合到当前的置信度中。解决这一问题的典型方法就是仅综合所有测量的小子集（如每次激光测距扫描只综合8个等间隔测量，而不是360个）。这种方法有一个种重要的额外好处：因为一次测距扫描中的相邻波束往往不是独立的，这样做，状态估计过程就会变得不易受到相邻测量的相关噪声的影响。
波束模型的局限性
具体来说，基于波束的模型表现出缺乏光滑性(lack of smoothness)。在有许多小障碍的混乱环境中，分布 $p(z_t^k|x_t,m)$ 在 $x_t$, 处是很不光滑的。
缺乏光滑性有两个后果。
- 第一，任何近似置信度表示法都存在错过正确状态的风险，因为邻近的状态可能具有截然不同的后验似然。这就限制了近似的准确性。近似的准确性如果不满足，将会增加后验的结果误差。
- 第二，由于在这样不光滑模型中有大量的局部极大值，因此用爬坡算法找到最可能的状态容易出现局部极小问题。
