概率机器人：测距仪的似然域模型

测距仪的似然域模型

似然域模型（Likelihood field model）克服了波束模型的局限性，能够在混乱小空间内，得到较为光滑的后验并大幅提高计算效率。

似然域模型是一种特设（ad hoc）算法，无合适的物理解释。

将障碍物检测的似然描述为地图坐标的函数，称为似然域。

模型建立

首先，模型将传感器扫描终点$z_t$映射至地图的全局坐标系（Map坐标系）。

假设机器人在时刻$t$的位姿$x_t={\left[\begin{array}{ccc}X& Y& \theta \end{array}\right]}^T$，传感器相对于机器人坐标系（Footprint坐标系）的坐标为${\left[\begin{array}{cc}{X}_{k,sens}& Y_{k,sens}\end{array}\right]}^T$，传感器波束相对机器人航向角的偏角为${\theta }_{k,sens}$，传感器的测量终点为$z_t^k$，如图所示：

则通过2D平面坐标变换公式，可以得到传感器相对地图坐标系的坐标${\left[\begin{array}{cc}{X}^{{}^{\prime }}& Y^{{}^{\prime }}\end{array}\right]}^T$表达式
$$\left[\begin{array}{c}{X}^{{}^{\prime }}\\ Y^{{}^{\prime }}\end{array}\right]=\left[\begin{array}{c}X\\ Y\end{array}\right]+\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]\left[\begin{array}{c}{X}_{k,sens}\\ Y_{k,sens}\end{array}\right]$$
随后，考虑测量终点$z_t^k$在地图坐标系中的坐标${\left[\begin{array}{cc}{X}_{z_t^k}& Y_{z_t^k}\end{array}\right]}^T$：
$$\left[\begin{array}{c}{X}_{z_t^k}\\ Y_{z_t^k}\end{array}\right]=\left[\begin{array}{c}{X}^{{}^{\prime }}\\ Y^{{}^{\prime }}\end{array}\right]+z_t^k\left[\begin{array}{c}\cos (\theta +{\theta }_{k,sens})\\ \sin (\theta +{\theta }_{k,sens})\end{array}\right]$$
带入传感器坐标，可以得到：
$$\left[\begin{array}{c}{X}_{z_t^k}\\ Y_{z_t^k}\end{array}\right]=\left[\begin{array}{c}X\\ Y\end{array}\right]+\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]\left[\begin{array}{c}{X}_{k,sens}\\ Y_{k,sens}\end{array}\right]+z_t^k\left[\begin{array}{c}\cos (\theta +{\theta }_{k,sens})\\ \sin (\theta +{\theta }_{k,sens})\end{array}\right]$$
需注意，当测距仪传回的测量值$z_t^k=z_{max}$时，表明存在异常噪声，该坐标在物理世界无意义，似然域模型将丢弃该读数。同波束模型一样，似然域模型考虑了三种噪声模型：

噪声一：测量噪声模型

对于测量过程中引起的噪声采用高斯分布进行建模，设在地图环境$m$中与测量坐标${\left[\begin{array}{cc}{X}_{z_t^k}& Y_{z_t^k}\end{array}\right]}^T$最近的障碍物之间的欧氏距离为$dist$，则测量噪声可建模为均值为0，方差为${\sigma }_{hit}^2$的高斯分布：
$$p_{hit}(z_t^k|x_t,m)={\epsilon }_{{\sigma }_{hit}}(dist)$$
式中，标准差${\sigma }_{hit}$为模型的固有参数。

如下左图，机器人在包含三个障碍物（灰色）的地图环境下进行测量，得到的测量点高斯似然如右图所示，图中越黑的地方存在障碍物的可能性越小；虚线表示传感器轴方向：

概率密度$p_{hit}$可由传感器轴正交归一化的似然域得到，如下所示为概率$p_{hit}(z_t^k)$的曲线。该曲线表明传感器测量到三个障碍物对应曲线上$o_1、o_2、o_3$所在位置。

噪声二：失败噪声模型

当传感器返回$z_t^k=z_{max}$时，认为传感器测量失败，返回最大量程值$z_{max}$。假定此时具有非常大的似然，则可用点群$p_{max}$进行建模：
$$p_{max}(z_t^k|x_t,m)=I(z=z_{max})=\{\begin{array}{ll}1& z=z_{max}\\ 0& 其他\end{array}$$

噪声三：随机噪声模型

由于传感器在测量过程中，可能会出现无法解释的噪声现象，用均匀分布$p_{rand}$对其进行建模：
$$p_{rand}(z_t^k|x_t,m)=\{\begin{array}{ll}\frac{1}{z_{max}}& 0\le z_t^k<z_{max}\\ 0& 其他\end{array}$$

似然域模型

由此，同波束模型一样，采用混合参数混合三种噪声，得到似然域模型如图所示：

其表达式如下：
$$p(z_t^k|x_t,m)={\left[\begin{array}{c}{z}_{hit}\\ z_{max}\\ z_{rand}\end{array}\right]}^T\cdot \left[\begin{array}{c}{p}_{hit}(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]$$
式中，混合权数$z_{hit}、z_{max}、z_{rand}$和为1：
$$z_{hit}+z_{max}+z_{rand}=1$$

模型算法

系统输入： 完整的一组测量数据$z_t$、机器人位姿$x_t$、地图信息$m$

系统输出： 测量数据的可能性$q$
$$\begin{array}{rl}& Algorithmlikelihood\_field\_range\_finder\_model(z_t,x_t,m):\\ 1:& q=1\\ 2:& forallkdo\\ 3:& if{z}_t^k\ne z_{max}\\ 4:& X_{z_t^k}=X+X_{k,sens}\cos \theta -Y_{k,snes}\sin \theta +z_t^k\cos (\theta +{\theta }_{k,sens})\\ 5:& Y_{z_t^k}=Y+Y_{k,sens}\cos \theta +X_{k,snes}\sin \theta +z_t^k\sin (\theta +{\theta }_{k,sens})\\ 6:& dist=\underset{X_d,Y_d}{\min }\left\{\sqrt{(X_{z_t^k}-X_d{)}^2+(Y_{z_t^k}-Y_d{)}^2}\right|⟨X_d,Y_d⟩occupiedinm\}\\ 7:& q=q\cdot (z_{hit}\cdot prob(dist,{\sigma }_{hit})+\frac{z_{rand}}{z_{max}})\\ 8:& returnq\end{array}$$
第一行，定义变量$q$存储整组数据$z_t$的概率

第二行，循环检测组内每个测量值$z_t^k$

第三行，判断是否为最大值，并抛弃最大输出

第四行~第五行，计算测量点在地图坐标系上的坐标值

第六行，计算地图信息上距离该测量点最近的障碍物的欧氏距离

第七行，混合高斯分布与均匀分布的噪声模型

最终，返回整组数据的准确性，模型的内参$\Theta =\{z_{hit},z_{max},z_{rand},{\sigma }_{hit}\}$可用极大似然估计进行标定。

似然域模型的优缺点

欧氏距离的光滑性，致使机器人位姿$x_t$的微小改动将仅对分布结果$p(z_t^k|x_t,m)$存在较小的影响，模型具有光滑性；

同时，模型的与计算采用二维空间代替了波束模型中的三维空间，降低了计算量。

与此同时，似然域模型存在如下缺点：

• 未考虑环境异常噪声$p_{short}$，由此不能对人或动态清晰建模
• 射线投影被近邻函数代替，不能确定抵达某点路径是否被障碍物拦截
• 未考虑地图不确定性，不能处理地图上未知高度或未探测区域。

针对上述缺点，可将地图占用值扩展分为三类：占用、闲置、未知。当测量位置$z_t^k$处于地图未知区域时，假定其概率用均匀分布建模：$p(z_t^k|x_t,m)=\frac{1}{z_{max}}$