【学习记录】移动机器人定位：栅格与蒙特卡罗

【学习记录】移动机器人定位：栅格与蒙特卡罗

一、 介绍：

两种定位方法：1、栅格定位（Grid Localization）2、蒙特卡罗定位（MCL）

二、栅格定位

在整个位姿空间的栅格分解上使用直方图滤波来近似后验，栅格定位具有多模分布的能力。
离散贝叶斯滤波：
b e l ( x t ) = { p k , t } bel(x_t)= \{ p_{k,t} \} bel(xt​)={pk,t​}
每个概率$p_{k,t}$定义在一个栅格单元$x_k$上，
合法位姿空间：
$$domain(X_t)=x_{1,t}\cup x_{2,t}\cup ...x_{k,t}$$
栅格定位算法：
1：Algorithm Grid_localization( { p k , t − 1 , u t , z t , m } \{ p_{k,t-1}, u_t, z_t, m \} {pk,t−1​,ut​,zt​,m}):
2：  for all k do
3：     ${\tilde{p}}_{k,t}=\sum _i{p}_{i,t-1}$ motion_model$(mean(x_k),u_t,mean(x_i))$
4：     $p_{k,t}=\eta {\tilde{p}}_{k,t}$meansurement_model($z_t,mean(x_k),m$)
5：  endfor
6：  renturn{$p_{k,t}$}
算法第3行：实现运动模型更新
算法第4行：实现观测模型更新，使用$\eta$归一化因子进行归一化

三、蒙特卡罗定位

蒙特卡罗定位算法：
1：Algorithm MCL(${\chi }_{t-1},{\mu }_t,z_t,m$):
2:   $\bar{\chi }={\chi }_t=\varnothing$
3:   for m =1 to M do
4:     $x_t^{[m]}=$sample_motion_model(${\mu }_t,x_{t-1}^{[m]}$)
5:     ${\omega }_t^{[m]}=$measurement_model($z_t,x_t^{[m]},m$)
6:     ${\bar{\chi }}_t={\bar{\chi }}_t+<x_t^{[m]},{\omega }_t^{[m]}>$
7:   endfor
8:   for m =1 to M do
9:     draw i with probability $\propto {\omega }_t^{[i]}$
10:    add $x_t^{[i]}$to ${\chi }_t$
11:  endfor
12:  return ${\chi }_t$
AMCL算法：
1：Algorithm Augmented_MCL(${\mathrm{ch}}_{t-1},{\mu }_t,z_t,m$):
2:  static ${\omega }_{slow}$