【学习记录】移动机器人定位：栅格与蒙特卡罗
【学习记录】移动机器人定位：栅格与蒙特卡罗
一、 介绍：
两种定位方法：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 p_{k,t} pk,t定义在一个栅格单元 x k x_k xk上，
合法位姿空间：
d o m a i n ( X t ) = x 1 , t ∪ x 2 , t ∪ . . . x k , t domain(X_t)=x_{1,t}\cup x_{2,t} \cup...x_{k,t} domain(Xt)=x1,t∪x2,t∪...xk,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： p ~ k , t = ∑ i p i , t − 1 \tilde{p}_{k,t}=\sum\limits_{i} p_{i,t-1} p~k,t=i∑pi,t−1 motion_model ( m e a n ( x k ) , u t , m e a n ( x i ) ) (mean(x_k), u_t, mean(x_i)) (mean(xk),ut,mean(xi))
4： p k , t = η p ~ k , t p_{k, t}=\eta \tilde{p}_{k,t} pk,t=ηp~k,tmeansurement_model( z t , m e a n ( x k ) , m z_t,mean(x_k),m zt,mean(xk),m)
5： endfor
6： renturn{ p k , t p_{k,t} pk,t}
算法第3行：实现运动模型更新
算法第4行：实现观测模型更新，使用 η \eta η归一化因子进行归一化
三、蒙特卡罗定位
蒙特卡罗定位算法：
1：Algorithm MCL( χ t − 1 , μ t , z t , m \chi_{t-1},\mu_{t},z_t,m χt−1,μt,zt,m):
2: χ ˉ = χ t = ∅ \bar\chi=\chi_t=\emptyset χˉ=χt=∅
3: for m =1 to M do
4: x t [ m ] = x^{[m]}_t= xt[m]=sample_motion_model( μ t , x t − 1 [ m ] \mu_t,x^{[m]}_{t-1} μt,xt−1[m])
5: ω t [ m ] = \omega^{[m]}_{t}= ωt[m]=measurement_model( z t , x t [ m ] , m z_t,x^{[m]}_t,m zt,xt[m],m)
6: χ ˉ t = χ ˉ t + < x t [ m ] , ω t [ m ] > \bar\chi_t=\bar\chi_t+
7: endfor
8: for m =1 to M do
9: draw i with probability ∝ ω t [ i ] \varpropto \omega^{[i]}_t ∝ωt[i]
10: add x t [ i ] x^{[i]}_t xt[i]to χ t \chi_t χt
11: endfor
12: return χ t \chi_t χt
AMCL算法：
1：Algorithm Augmented_MCL( ch t − 1 , μ t , z t , m \ch_{t-1}, \mu_t,z_t,m cht−1,μt,zt,m):
2: static ω s l o w \omega_{slow} ωslow