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

最新推荐文章于 2022-02-04 14:33:56 发布

Jason.Li_0012

最新推荐文章于 2022-02-04 14:33:56 发布

阅读量4k

点赞数 6

分类专栏：《概率机器人》笔记激光SLAM 文章标签：自动驾驶

本文链接：https://blog.csdn.net/weixin_45929038/article/details/122776379

版权

激光SLAM 同时被 2 个专栏收录

13 篇文章 12 订阅

订阅专栏

《概率机器人》笔记

6 篇文章 8 订阅

订阅专栏

测距仪的似然域模型

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

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

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

模型建立

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

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

在这里插入图片描述

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

噪声一：测量噪声模型

对于测量过程中引起的噪声采用高斯分布进行建模，设在地图环境 $m$ 中与测量坐标 $\begin{bmatrix}X_{z_t^k}&Y_{z_t^k}\end{bmatrix}^T$ 最近的障碍物之间的欧氏距离为 $d i s t$ ，则测量噪声可建模为均值为0，方差为 $\sigma_{hit}^2$ 的高斯分布：
$p_{hit}(z_t^k|x_t,m)=\varepsilon_{\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{cases}1\quad&z=z_{max}\\0\quad&其他\end{cases}$

噪声三：随机噪声模型

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

似然域模型

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

在这里插入图片描述
其表达式如下：
$p(z_t^k|x_t,m)=\begin{bmatrix}z_{hit}\\z_{max}\\z_{rand}\end{bmatrix}^T\cdot\begin{bmatrix}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{bmatrix}$
式中，混合权数 $z_{hit}、z_{max}、z_{rand}$ 和为1：
$z_{hit}+z_{max}+z_{rand}=1$

模型算法

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

系统输出： 测量数据的可能性 $q$
$\begin{aligned} &Algorithm\quad likelihood\_field\_range\_finder\_model(z_t,x_t,m):\\ 1:&\qquad q=1 \\ 2:&\qquad for\enspace all\enspace k\enspace do \\ 3:&\qquad\qquad if z_t^k \neq z_{max}\\ 4:&\qquad\qquad\qquad X_{z_t^k}=X+X_{k,sens}\cos\theta-Y_{k,snes}\sin\theta+z_t^k\cos(\theta+\theta_{k,sens})\\ 5:&\qquad\qquad\qquad Y_{z_t^k}=Y+Y_{k,sens}\cos\theta+ X_{k,snes}\sin\theta+z_t^k\sin(\theta+\theta_{k,sens})\\ 6:&\qquad\qquad\qquad dist=\min_{X_d,Y_d}\Biggl\{\sqrt{(X_{z_t^k}-X_d)^2+(Y_{z_t^k}-Y_d)^2}\Bigg\vert \langle X_d,Y_d\rangle\enspace occupied\enspace in\enspace m\Biggr\}\\ 7:&\qquad\qquad\qquad q=q\cdot \Bigl(z_{hit}\cdot prob(dist,\sigma_{hit})+\frac{z_{rand}}{z_{max}}\Bigr)\\ 8:&\qquad return\quad q \end{aligned}$
第一行，定义变量 $q$ 存储整组数据 $z_t$ 的概率