9 概率机器人 Probabilistic Robotics 二值贝叶斯滤波 占据栅格地图 occupancy grid mapping

1 前言

• 如果通过传感器对一个环境中固定状态进行评估，该状态为二值状态（例如判断一扇门的开关状态），那么就需要用到二值贝叶斯滤波
• 二值贝叶斯滤波的一个重要应用就是通过激光雷达建立占据栅格地图，这会在下文中做介绍

2 二值贝叶斯滤波

2.1 理论基础

• 对于一个静态状态，也就是说该状态不随时间变化且没有动作作用于该状态，那么置信度函数可以写为：
  $$be{l}_t(x)=p(x|z_{1:t},u_{1:t})=p(x|z_{1:t})$$
• 二值状态分别为$x,\bar{x}$,那么相应的置信度满足
  $$be{l}_t(x)=1-be{l}_t(\bar{x})$$
• 二值贝叶斯滤波可以应用离散贝叶斯滤波计算，分别比较两种情况的概率，概率较大的状态即为当前状态值
• 但是对于这种只有两种状态的二值状态，有一个更加巧妙和便捷的方法来计算，那就是对这两个状态的概率作比后取对数，通过判断这个对数值的大小，也可以比较出这两种状态的概率大小
  • 这样做的好处还有$l(x)$的值域是$-\infty ,+\infty$, 避免了概率值在$0,1$附近的截断问题
  • 置信度的更新也更加方便，具体的看下文算法
    $$\begin{gathered} \frac{p(x)}{p(\bar{x})}=\frac{p(x)}{1-p(x)} \\ \downarrow \text{取对数} \\ l(x):=\log (\frac{p(x)}{1-p(x)}) \end{gathered}$$

2.2 算法流程

• 下面是二值贝叶斯滤波方法的更新过程：
  • 可以看出算法十分简单，仅一条公式
  • 具体公式如何得来的见下文推导

• 这里用到了一个概率$p(x|z_t)$,被称为逆测量模型(inverse measurement model)；相反$p(z_t|x)$,被称为测量模型或传感器模型
  • 逆测量模型是测量$z_t$函数的概率分布
  • 逆测量模型一般用在传感器模型比较复杂的情况
  • 例如：从图片中判断一扇门是开启还是关闭状态
    1. 这个状态十分简单，但是测量空间很大
    2. 已知测量图片判断门的状态$p(x|z_t)$这个通过适当算法十分容易获得
    3. 但是已知门的状态求含有这扇门的图片的概率分布$p(z_t|x)$就很难求得
• 已知$l(x)$，置信度$be{l}_t(x)$十分容易求得：
  $$\begin{gathered} l(x):=\log (\frac{p(x)}{1-p(x)}) \\ \downarrow \\ be{l}_t(x)=1-\frac{1}{1+\exp (l_t)} \end{gathered}$$

2.3 重要公式推导

• 从算法流程图中可以看出二值贝叶斯滤波仅有一条更新公式
  $$l_t=l_{t-1}+\log (\frac{p(x|z_t)}{1-p(x|z_t)})-\log (\frac{p(x)}{1-p(x)})$$
• 这里为大家推导这个公式的由来
  $$\begin{gathered} \begin{array}{rl}be{l}_t(x)=p(x|z_{1:t})=& p(x|z_t,z_{1:t-1})\\ =& \frac{p(z_t|x,z_{1:t-1})p(x|z_{1:t-1})}{p(z_t|z_{1:t-1})}\\ =& \frac{p(z_t|x)p(x|z_{1:t-1})}{p(z_t|z_{1:t-1})}\\ & \downarrow p(z_t|x)=\frac{p(x|z_t)p(z_t)}{p(x)}\\ =& \frac{p(x|z_t)p(z_t)p(x|z_{1:t-1})}{p(x)p(z_t|z_{1:t-1})}\end{array} \\ \downarrow \text{同理：}x\leftrightarrow \bar{x} \\ be{l}_t(\bar{x})=p(\bar{x}|z_{1:t})=\frac{p(\bar{x}|z_t)p(z_t)p(\bar{x}|z_{1:t-1})}{p(\bar{x})p(z_t|z_{1:t-1})} \\ \downarrow \text{上面两个式子作比} \\ \begin{array}{rl}\frac{p(x|z_{1:t})}{p(\bar{x}|z_{1:t})}=& \frac{p(x|z_t)}{p(\bar{x}|z_t)}\frac{p(x|z_{1:t-1})}{p(\bar{x}|z_{1:t-1})}\frac{p(\bar{x})}{p(x)}\\ =& \frac{p(x|z_t)}{1-p(x|z_t)}\frac{p(x|z_{1:t-1})}{1-p(x|z_{1:t-1})}\frac{1-p(x)}{p(x)}\end{array} \\ \downarrow \text{取对数} \\ \begin{array}{rl}\log \frac{p(x|z_{1:t})}{1-p(x|z_{1:t})}=& \log \frac{p(x|z_t)}{1-p(x|z_t)}+\log \frac{p(x|z_{1:t-1})}{1-p(x|z_{1:t-1})}+\log \frac{1-p(x)}{p(x)}\\ l_t(x)=& \log \frac{p(x|z_t)}{1-p(x|z_t)}+l_{t-1}(x)-\log \frac{p(x)}{1-p(x)}\\ & \downarrow l_0(x)=\log \frac{p(x)}{1-p(x)}\\ l_t(x)=& l_{t-1}(x)+\log \frac{p(x|z_t)}{1-p(x|z_t)}-l_0(x)\end{array} \end{gathered}$$
• 终于得出二值贝叶斯算法中更新公式，那么具体怎么使用呢？ 我们用下一节的占据栅格地图（occupancy grid mapping）为例子给大家讲解如何使用。

3 实例：占据栅格地图（occupancy grid mapping）

• 占据栅格地图应用在地图构建上，它标识出哪地方有障碍物，哪地方是空闲的
  • 这与二值贝叶斯滤波的应用条件十分吻合
    1. 地图中各点的真实状态是不变的
    2. 地图中各点仅有两种状态（障碍物或者空闲）,符合二值性
• 占据栅格地图算法是根据机器人位姿$x$和传感器测量$z$的数据，计算地图$m$的后验概率$p(m|z_{1:t},x_{1:t})$
• 一般把地图平面分割成若干个栅格$m=\{m_i\}$,估计每个栅格的状态
  • 这里约定$p(m_i=1)=p(m_i)$为栅格被占据的概率（即该栅格存在障碍物）
  • $p(m_i=0)$为栅格是空闲的概率 （即该栅格不存在障碍物）

• 那么计算整体地图$m$的后验概率$p(m|z_{1:t},x_{1:t})$就可以转化为估计每个栅格的后验概率$p(m_i|z_{1:t},x_{1:t})$
  $$p(m|z_{1:t},x_{1:t})=\prod _{i}p(m_i|z_{1:t},x_{1:t})$$
• 每个栅格都有二值性，可以使用二值贝叶斯滤波算法

• 上述算法是估计单一栅格被占据的概率，但是栅格占据地图的实例中要估计多个栅格的概率，所以对上述算法做一下简单修改
  1. line 4: 对于传感器探测到的栅格，我们通过二值贝叶斯滤波算法更新改状态
  2. line 6: 对于传感器没有探测到的栅格，该栅格的状态保持不变

• 这里给出上式算法中的计算公式
  $$\begin{array}{rl}{l}_{t,i}=& \log \frac{p(m_i|z_{1:t},x_{1:t})}{1-p(m_i|z_{1:t},x_{1:t})}\\ l_0=& \log \frac{p(m_i=1)}{p(m_i=0)}=\frac{p(m_i)}{1-p(m_i)}\\ inverse\_sensor\_model(m_i,x_t,z_t)=& \log \frac{p(m_i|z_t,x_t)}{1-p(m_i|z_t,x_t)}\end{array}$$

• 那么二值贝叶斯滤波算法是更新公式为

  • 注意：这里的$x_t$是当前机器人的位姿，不是要估计的状态
  • 注意：这里要估计的状态的$m_i$，相当于二值贝叶斯算法流程中的$x_t$
    $$l_{t,i}=l_{t-1,i}+\log \frac{p(m_i|z_t,x_t)}{1-p(m_i|z_t,x_t)}-\frac{p(m_i)}{1-p(m_i)}$$

• 下面我们一起进行一个简单的占据栅格地图的实例计算：这里参考占据栅格地图

  • 初始时刻机器人对外界一无所知
  • 每次同过激光雷达探测外界环境，从探索数据中计算检测到的栅格状态的概率
  • 对于探测到的栅格有障碍物和无障碍物的正确率均为$90\%$
• • $t=0$: 机器人对外界环境一无所知，所以$p(m_i=0)=p(m_i=1)=0.5$
    $$l_0=\log \frac{p(m_i=1)}{p(m_i=0)}=\log \frac{0.5}{0.5}=0$$

• • $t=1$: 激光雷达进行探测
    • 图中红色线经过的栅格为探测到的栅格，也就是需要更新概率的栅格
    • 图中绿色方块被检查出有障碍物，将激光反射回去，其余被检查的栅格无障碍物

• • $t=1$: 根据探测到的数据进行状态更新
    $$\begin{gathered} \begin{array}{rl}\text{有障碍物的栅格：}{l}_{1,i}=& l_{0,i}+\log \frac{p(m_i|z_1,x_1)}{1-p(m_i|z_1,x_1)}-l_0\\ =& 0+\log \frac{0.9}{1-0.9}-0\\ \approx & 2.2\end{array} \\ \begin{array}{rl}\text{空闲的栅格：}{l}_{1,i}=& l_{0,i}+\log \frac{p(m_i|z_1,x_1)}{1-p(m_i|z_1,x_1)}-l_0\\ =& 0+\log \frac{0.1}{1-0.1}-0\\ \approx & -2.2\end{array} \\ \text{没探测到的栅格：}{l}_{1,i}=l_{0,i} \end{gathered}$$

• • $t=2$: 激光雷达进行探测

• • $t=2$: 根据探测到的数据进行状态更新
    $$\begin{gathered} \begin{array}{rl}\text{有障碍物的栅格：}{l}_{2,i}=& l_{1,i}+\log \frac{p(m_i|z_2,x_2)}{1-p(m_i|z_2,x_2)}-l_0\\ =& 0+\log \frac{0.9}{1-0.9}-0\\ \approx & 2.2\end{array} \\ \begin{array}{rl}\text{空闲的栅格：}{l}_{2,i}=& l_{1,i}+\log \frac{p(m_i|z_2,x_2)}{1-p(m_i|z_2,x_2)}-l_0\\ =& \{\begin{array}{c}0+\log \frac{0.1}{1-0.1}-0\approx -2.2\\ -2.2+\log \frac{0.1}{1-0.1}-0\approx -4.4\end{array}\end{array} \\ \text{没探测到的栅格：}{l}_{2,i}=l_{1,i} \end{gathered}$$

• $t=3,4...$：同理

4 参考文献

• Thrun, & Sebastian. (2005). Probabilistic robotics.
• 占据栅格地图