slam导航之AMCL算法

主要内容来自ROS_Robot_Programming。建议多看几次，熟悉一下里面的例子。
1.目的：就是确定机器人在特定环境的位置，即在地图中得到x, y和θ。

2.完成过程
a. MCL计算机器人所在位置的概率。t时刻的位置和方向Xt(x, y, θ)。距离传感器到t时刻为止获得的距离信息记为Z={z0, z1, z2…….,zt}。编码器到t时刻为止获得的机器人的运动信息记为U={u0,u1,u2……,ut}.下面计算belief(使用贝叶斯更新公式的后验概率）。
bel(Xt) = p(Xt|Z0..t, U0..t)
b. 机器人可能具有硬件误差，因此建立传感器模型和移动模型，再执行贝叶斯滤波器的预测和更新。先是预测阶段，利用机器人的移动模型P（Xt | Xt-1,Ut-1）,上一个位置上的概率bel(Xt-1), 编码器获得的移动信息U。计算下一时刻的机器人位置bel’(Xt).
bel’(Xt) = ∮p(Xt | Xt-1, Ut-1)bel(Xt-1)dXt-1
c. 矫正步骤，利用传感器模型p(Zt | Xt), 前面求得的概率bel’(Xt)和归一化常数ηt。可以求得基于传感器信息提高准确度的当前位置的概率bel(Xt)
bel(Xt) = ηP(Zt | Xt)bel’(Xt)

3.以上可以得到当前位置的概率bel(Xt), 用粒子滤波器生成N个粒子来估计位置。
a.抽样，这里使用前一个位置的概率bel(Xt-1)中的机器人的移动模型P(Xt | Xt-1, Ut-1)来提取新的样本集合Xt’。样本Xt’中的第i个样本Xt’. 距离传感器获得的距离信息Zt和归一化常数ηp来计算权重值Wt。
Wt = ηP(Zy | Xt’)
b.重权重采样过程中，我们使用样本Xt’和Wt’来创建N个新的样本（粒子）集合Xt。
Xt = {Xj | j=1 … N} ~ {Xt’ , t’}