8 概率机器人 Probabilistic Robotics 直方图滤波 离散贝叶斯滤波

1 前言

• 直方图滤波适用于非线性滤波，是非参数滤波的一种
• 直方图滤波是离散贝叶斯滤波在连续状态下的实现
  • 离散贝叶斯滤波：离散状态
  • 直方图滤波：连续状态
• 直方图滤波的中心思想：将连续状态分段分解，在对每段做近似，再用离散贝叶斯滤波
  • 直方图滤波 = 离散分解 + 离散贝叶斯滤波
  • 离散分解有很大学问，不同的分段方法对滤波效果影响很大
    • 细分：精度高，计算量大
    • 粗分：精度低，计算量小
  • 理想的离散分解：非线性弱则粗分，非线性强则细分

2 离散贝叶斯滤波算法

• 这里介绍离散贝叶斯滤波算法
• 离散贝叶斯滤波与贝叶斯滤波很相似，只不过前者用在离散状态
• 先给大家回顾一下贝叶斯滤波流程：
  • 大家看到求置信度用到的是积分

• 离散贝叶斯滤波算法
  • 应用于有限状态的情景
  • 把贝叶斯滤波的积分改成求和就是离散贝叶斯算法了

3 直方图滤波

• 直方图滤波 = 离散分解 + 离散贝叶斯滤波

3.1 分解技术

• 将连续状态$X_t$进行分解：
  $$\begin{gathered} dom(X_t)=x_{1,t}\cup x_{2,t}\cup \cdots x_{K,t} \\ {x}_{i,t}\cap x_{k,t}=\varnothing \\ \bigcup _k{x}_{k,t}=dom(X_t) \end{gathered}$$
• 对状态分解主要有两种思想：静态，动态
  • 静态：固定粗细的分解方法，不随变换函数的非线性程度所改变
  • 动态：分的粗细程度自适应变换函数
• 动态分解要优于静态分解，动态分解的方法很多，这里不做介绍，给大家看一下动态与静态的对比：
  • 右下图是原始状态，$x$服从正态分布的连续状态，对它进行离散分解
  • 右上图是非线性变换函数$y=g(x)$
  • 左上图是静态分解，分解间隔是定值，近似效果较差
  • 中上图是动态分解，该分解的粗细程度与状态转移函数$y=g(x)$的非线性程度有关，近似效果较好

3.2 直方图滤波介绍

• 上述介绍完对状态进行分解的方法，这里介绍分解后的计算过程
• 我们分解了很多区间，那么每一段的概率为$p_{k,t}$,那么该区间概率密度:
  $$p(x_t)=\frac{p_{k,t}}{|x_{k,t}|}$$
• 再在每一段区内选一个代表点${\hat{x}}_{k,t}$:
  $${\hat{x}}_{k,t}=|x_{k,t}{|}^{-1}{\int }_{x_{k,t}}{x}_{t}d{x}_t$$
• 有了代表点就可以近似测量和状态转移概率，具体为什么这样近似看下面重要公式推导部分
  $$\begin{gathered} p(z_t|x_{k,t})\approx p(z_t|{\hat{x}}_{k,t}) \\ p(x_{k,t}|u_t,x_{i,t-1})\approx \eta |x_{k,t}|p({\hat{x}}_{k,t}|u_t,{\hat{x}}_{i,t-1}) \end{gathered}$$
• 所以离散贝叶斯滤波中两个公式可以表示为：
  $$\begin{array}{rl}line3:{\bar{p}}_{k,t}& =\sum _{i}p(X_t=x_k|u_t,X_{t-1}=x_i)p_{i,t-1}\\ & =\sum _i\eta |x_{k,t}|p(X_t={\hat{x}}_{k,t}|u_t,X_{t-1}={\hat{x}}_{i,t-1})p_{i,t-1}\\ line4:p_{k,t}& =\eta p(z_t|X_t=x_k){\bar{p}}_{k,t}\\ & =\eta p(z_t|X_t={\hat{x}}_{k,t}){\bar{p}}_{k,t}\end{array}$$

3.3 重要公式推导

• 测量概率近似：
  $$\begin{gathered} \begin{array}{rl}p(z_t|x_{k,t})& =\frac{p(z_t,x_{k,t})}{p(x_{k,t})}\\ & =\frac{{\int }_{x_{k,t}}p(z_t,x_t)d{x}_t}{{\int }_{x_{k,t}}p(x_t)d{x}_t}\\ & =\frac{{\int }_{x_{k,t}}p(z_t|x_t)p(x_t)d{x}_t}{{\int }_{x_{k,t}}p(x_t)d{x}_t}\\ & =\frac{{\int }_{x_{k,t}}p(z_t|x_t)\frac{p_{k,t}}{|x_{k,t}|}d{x}_t}{{\int }_{x_{k,t}}\frac{p_{k,t}}{|x_{k,t}|}d{x}_t}\\ & =\frac{\frac{p_{k,t}}{|x_{k,t}|}}{\frac{p_{k,t}}{|x_{k,t}|}}\frac{{\int }_{x_{k,t}}p(z_t|x_t)d{x}_t}{{\int }_{x_{k,t}}1d{x}_t}\\ & =|x_{k,t}{|}^{-1}{\int }_{x_{k,t}}p(z_t|x_t)d{x}_t\\ & \approx |x_{k,t}{|}^{-1}{\int }_{x_{k,t}}p(z_t|{\hat{x}}_{k,t})d{x}_t\\ & =|x_{k,t}{|}^{-1}p(z_t|{\hat{x}}_{k,t}){\int }_{x_{k,t}}1d{x}_t\\ & =|x_{k,t}{|}^{-1}p(z_t|{\hat{x}}_{k,t})|x_{k,t}|\\ & =p(z_t|{\hat{x}}_{k,t})\end{array} \\ \downarrow \\ p(z_t|x_{k,t})\approx p(z_t|{\hat{x}}_{k,t}) \end{gathered}$$
• 状态转移概率近似：
  $$\begin{gathered} \begin{array}{rl}p(x_{k,t}|u_t,x_{i,t-1})& =\frac{p(x_{k,t},x_{i,t-1}|u_t)}{p(x_{i,t-1}|u_t)}\\ & =\frac{{\int }_{x_{k,t}}{\int }_{x_{i,t-1}}p(x_t,x_{t-1}|u_t)d{x}_{t}d{x}_{t-1}}{{\int }_{x_{i,t-1}}p(x_{t-1}|u_t)d{x}_t}\\ & =\frac{{\int }_{x_{k,t}}{\int }_{x_{i,t-1}}p(x_t|u_t,x_{t-1})p(x_{t-1}|u_t)d{x}_{t}d{x}_{t-1}}{{\int }_{x_{i,t-1}}p(x_{t-1}|u_t)d{x}_t}\\ & \downarrow p(x_{t-1}|u_t)=p(x_{t-1})\\ & =\frac{{\int }_{x_{k,t}}{\int }_{x_{i,t-1}}p(x_t|u_t,x_{t-1})p(x_{t-1})d{x}_{t}d{x}_{t-1}}{{\int }_{x_{i,t-1}}p(x_{t-1})d{x}_t}\\ & =\frac{{\int }_{x_{k,t}}{\int }_{x_{i,t-1}}p(x_t|u_t,x_{t-1})\frac{p_{k,t-1}}{|x_{k,t-1}|}d{x}_{t}d{x}_{t-1}}{{\int }_{x_{i,t-1}}\frac{p_{k,t-1}}{|x_{k,t-1}|}d{x}_t}\\ & =\frac{{\int }_{x_{k,t}}{\int }_{x_{i,t-1}}p(x_t|u_t,x_{t-1})d{x}_{t}d{x}_{t-1}}{{\int }_{x_{i,t-1}}1d{x}_t}\\ & =|x_{k,t-1}{|}^{-1}{\int }_{x_{k,t}}{\int }_{x_{i,t-1}}p(x_t|u_t,x_{t-1})d{x}_{t}d{x}_{t-1}\\ & \downarrow p(x_t|u_t,x_{t-1})\approx p({\hat{x}}_{k,t}|u_t,{\hat{x}}_{k,t-1})\\ & \approx \eta |x_{k,t-1}{|}^{-1}{\int }_{x_{k,t}}{\int }_{x_{i,t-1}}p({\hat{x}}_{k,t}|u_t,{\hat{x}}_{k,t-1})d{x}_{t}d{x}_{t-1}\\ & =\eta |x_{k,t-1}{|}^{-1}p({\hat{x}}_{k,t}|u_t,{\hat{x}}_{k,t-1}){\int }_{x_{k,t}}{\int }_{x_{i,t-1}}1d{x}_{t}d{x}_{t-1}\\ & =\eta |x_{k,t-1}{|}^{-1}p({\hat{x}}_{k,t}|u_t,{\hat{x}}_{k,t-1})|x_{k,t}||x_{k,t-1}|\\ & =\eta |x_{k,t}|p({\hat{x}}_{k,t}|u_t,{\hat{x}}_{k,t-1})\end{array} \\ \downarrow \\ p(x_{k,t}|u_t,x_{i,t-1})\approx \eta |x_{k,t}|p({\hat{x}}_{k,t}|u_t,{\hat{x}}_{i,t-1}) \end{gathered}$$

3.4 实例

• 这里借鉴自动驾驶定位算法(九)-直方图滤波(Histogram Filter)定位的例子
• 例子：1D自动驾驶定位
  • 无人驾驶汽车在一维的宽度为$5m$的世界重复循环
  • 因为世界是循环的，所以如果无人驾驶汽车到了最右侧，再往前走一步，它就又回到了最左侧的位置
  • 车上的传感器可以检测车辆当前所在位置的颜色
  • 传感器对颜色的检测不是$100\%$准确的
  • 汽车以自认为$1m/step$的恒定速度向右运动
  • 车辆运动本身也存在误差，即向车辆发出的控制命令是向右移动2m，而实际的车辆运动结果可能是待在原地不动，可能向右移动1m，也可能向右移动3m

• 数学模型：
• • 机器人只有一个状态变量：位置 $X\in [0,5)$
• • 对状态空间分解（这里采用静态分解）：$x_{0,t}\in [0,1),x_{1,t}\in [1,2),x_{2,t}\in [2,3),x_{3,t}\in [3,4),x_{4,t}\in [4,5)$
• • 对每一个分段区间计算一个代表的值${\hat{x}}_{0,t}=|x_{0,t}{|}^{-1}{\int }_{x_{0,t}}{x}_{t}d{x}_t=0.5,{\hat{x}}_{1,t}=1.5,{\hat{x}}_{2,t}=2.5,{\hat{x}}_{3,t}=3.5,{\hat{x}}_{4,t}=4.5$
• • 车辆自认为在向右运动，每一步运动$u_t=1m$，我们假设存在$5\%$的概率，无人驾驶汽车仍待在原地没动；存在$90\%$的概率车辆在向右移动$1m$；存在$5\%$的概率无人驾驶汽车在向右运动$2m$
    $$\begin{gathered} p({\hat{x}}_t=x|u_t=1,{\hat{x}}_{t-1}=x)=5\% \\ p({\hat{x}}_t=(x+1)mod5|u_t=1,{\hat{x}}_{t-1}=x)=90\% \\ p({\hat{x}}_t=(x+2)mod5|u_t=1,{\hat{x}}_{t-1}=x)=5\% \end{gathered}$$
• • 汽车上的传感器有$90\%$的概率检测正确，$10\%$的概率检测错误
    $$\begin{gathered} p(sensor=Bule|{\hat{x}}_t=0.5,2.5,3.5)=90\% \\ p(sensor=Orange|{\hat{x}}_t=0.5,2.5,3.5)=10\% \\ p(sensor=Bule|{\hat{x}}_t=1.5,4.5)=10\% \\ p(sensor=Orange|{\hat{x}}_t=1.5,4.5)=90\% \end{gathered}$$
• 利用直方图滤波进行车辆定位：
• • 我们用5维向量来描述$t$时刻车辆在$c$的概率
    $$\begin{array}{rl}{p}_t& =(p_{0,t},p_{1,t},p_{2,t},p_{3,t},p_{4,t})\\ & =(p({\hat{x}}_{0,t}),p({\hat{x}}_{1,t}),p({\hat{x}}_{2,t}),p({\hat{x}}_{3,t}),p({\hat{x}}_{4,t}))\end{array}$$
• • 过程：无人驾驶汽车对自己所在位置一无所知，假设它连续三次【向右走一步】-【观测】,三次观测结果分别是orange、blue、orange
• 在计算之前大家想一想连续3次分别检测出orange、blue、orange后应该在哪个位置？其实大家对照上图不难看出应该在${\hat{x}}_{1,t}$处
• 下面我们一步步计算汽车是如何通过【运动】-【观测】过程逐步确认自己的位置在在${\hat{x}}_{1,t}$处
• • $t=0$: 没有任何先验知识，车辆对自己在哪一无所知，所以各个位置的置信度相等
    $$\begin{array}{rl}{p}_0& =(p_{0,0},p_{1,0},p_{2,0},p_{3,0},p_{4,0})\\ & =(p({\hat{x}}_{0,0}),p({\hat{x}}_{1,0}),p({\hat{x}}_{2,0}),p({\hat{x}}_{3,0}),p({\hat{x}}_{4,0}))\\ & =(0.2,0.2,0.2,0.2,0.2)\end{array}$$
• • $t=1$: 向右走$1m$
    $$\begin{array}{rl}line3:{\bar{p}}_{k,1}=& \sum _i\eta |x_{k,1}|p(X_1={\hat{x}}_{k,1}|u_1=1,X_0={\hat{x}}_{i,0})p_{i,0}\\ =& \eta ((0.05,0.9,0.05,0,0)\ast 0.2+(0,0.05,0.9,0.05,0)\ast 0.2\\ & +(0,0,0.05,0.9,0.05)\ast 0.2+(0.05,0,0,0.05,0.9)\ast 0.2\\ & +(0.9,0.05,0,0,0.05)\ast 0.2)\\ =& \eta (0.2,0.2,0.2,0.2,0.2)\\ =& (0.2,0.2,0.2,0.2,0.2)\end{array}$$
• • $t=1$: 检测到环境颜色为orange
    $$\begin{array}{rl}line4:p_{k,1}=& \eta p(z_1=Orange|X_1={\hat{x}}_{k,1}){\bar{p}}_{k,1}\\ =& \eta (0.1,0.9,0.1,0.1,0.9)\ast (0.2,0.2,0.2,0.2,0.2)\\ =& \eta (0.02,0.18,0.02,0.02,0.18)\\ =& (0.04762,0.42857,0.04762,0.04762,0.42857)\end{array}$$
• • $t=2$: 向右走$1m$
    $$\begin{array}{rl}line3:{\bar{p}}_{k,2}=& \sum _i\eta |x_{k,2}|p(X_2={\hat{x}}_{k,2}|u_2=1,X_1={\hat{x}}_{i,1})p_{i,1}\\ =& \eta ((0.05,0.9,0.05,0,0)\ast 0.04762+(0,0.05,0.9,0.05,0)\ast 0.42857\\ & +(0,0,0.05,0.9,0.05)\ast 0.04762+(0.05,0,0,0.05,0.9)\ast 0.04762\\ & +(0.9,0.05,0,0,0.05)\ast 0.42857)\\ =& \eta (0.39048,0.08571,0.39048,0.06667,0.06667)\\ =& (0.39048,0.08571,0.39048,0.06667,0.06667)\end{array}$$
• • $t=2$: 检测到环境颜色为blue
    $$\begin{array}{rl}line4:p_{k,2}=& \eta p(z_2=Orange|X_2={\hat{x}}_{k,2}){\bar{p}}_{k,2}\\ =& \eta (0.1,0.9,0.1,0.1,0.9)\ast (0.39048,0.08571,0.39048,0.06667,0.06667)\\ =& (0.45165,0.01102,0.45165,0.07711,0.00857)\end{array}$$
• • $t=3$: 向右走$1m$
    $$\begin{array}{rl}line3:{\bar{p}}_{k,3}=& \sum _i\eta |x_{k,3}|p(X_3={\hat{x}}_{k,3}|u_3=1,X_2={\hat{x}}_{i,2})p_{i,2}\\ =& \eta ((0.05,0.9,0.05,0,0)\ast 0.45165+(0,0.05,0.9,0.05,0)\ast 0.01102\\ & +(0,0,0.05,0.9,0.05)\ast 0.45165+(0.05,0,0,0.05,0.9)\ast 0.07711\\ & +(0.9,0.05,0,0,0.05)\ast 0.00857)\\ =& (0.03415,0.40747,0.05508,0.41089,0.09241)\end{array}$$
• • $t=3$: 检测到环境颜色为orange
    $$\begin{array}{rl}line4:p_{k,3}=& \eta p(z_3=Orange|X_3={\hat{x}}_{k,3}){\bar{p}}_{k,3}\\ =& \eta (0.1,0.9,0.1,0.1,0.9)\ast (0.03415,0.40747,0.05508,0.41089,0.09241)\\ =& (0.00683,0.73358,0.01102,0.08219,0.16637)\end{array}$$
• 通过$p_{k,3}=(0.00683,0.73358,0.01102,0.08219,0.16637)$可以看出$p({\hat{x}}_{1,t})=0.73358$远大于其他位置，所以此时在${\hat{x}}_{1,t}$处

4 参考文献

• Thrun, & Sebastian. (2005). Probabilistic robotics.
• 自动驾驶定位算法(九)-直方图滤波(Histogram Filter)定位