图说slam（一）：amcl定位算法主要流程概念图解

1. 从高斯分布中随机采样，生成初始粒子

以global坐标系为参考坐标系，以初始位姿（默认为0）作为初始粒子分布的均值，从参数服务器中获取位姿的协方差矩阵，实现的函数在AmclNode::handleMapMessage中，关键代码如下：

updatePoseFromServer(); // 从参数服务器更新初始位姿
pf_vector_t pf_init_pose_mean = pf_vector_zero();
pf_init_pose_mean.v[0] = init_pose_[0];
pf_init_pose_mean.v[1] = init_pose_[1];
pf_init_pose_mean.v[2] = init_pose_[2];
pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
pf_init_pose_cov.m[0][0] = init_cov_[0];
pf_init_pose_cov.m[1][1] = init_cov_[1];
pf_init_pose_cov.m[2][2] = init_cov_[2];
pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov); // 以均值0和相应的初始协方差初始化滤波器
pf_init_ = false;

生成后的粒子分布示意如下图所示：

其中，initial_pose在global坐标系下的坐标为(0, 0, 0)

2. 预测粒子位姿

当接收到里程计信息后，从里程计模型中采样，估计粒子群的预测位姿，关键实现函数为AMCLOdom::UpdateAction，示意图如下所示：

如上图所示，经过机器人的运动，蓝色粒子群从里程计的反馈中采样，转移到了黄色粒子群的分布。

里程计反馈的信息是odom_delta，但是由于里程计固有的高斯随机误差，所以实际的位姿变化是(odom_delta - gussian_noise)，而这其中，对于粒子群的每一次计算，gussian_noise的值是不一样的，其值的服从pdf(0, alpha * odom_delata)。

由于里程计估计的误差，参考蓝色粒子分布，预测后的黄色粒子分布更为分散，状态的不确定性提高。

3. 更新粒子位姿

当接收到测量数据（以激光测量为例）后，通过测量模型，将测量数据放到各个粒子的位姿下，判断测量数据发生的可能性，以这个可能性来更新粒子的权重。

以测量似然域模型为例：将激光的测量数据放到各个粒子位姿下，然后计算激光测量的端点与地图最近障碍物的距离，距离越小激光测量数据发生的可能性越大，粒子的权重越大。

代码中的关键实现函数是AMCLLaser::UpdateSensor，示意图如下所示，其中粒子颜色越深，权重越大。

4. 重采样

如果打开了有选择重采样，程序会判断粒子集合权重的方差，方差越大有效的粒子越小，粒子退化越严重，此时则需要进行重采样，否则可以跳过此步骤。

重采样之前，程序会维持粒子群的短期权重（w_fast）和长期权重（w_slow）的平均值，并以max[0, 1-w_fast/w_slow]的概率增加随机粒子，否则，进行低方差重采样算法。

正常情况下，如果没有发生如机器人绑架情况，增加随机粒子的概率很低，暂时先不考虑。重采样之后，粒子的数量不变，权重较小的粒子将被过滤掉，权重较大的粒子将被复制，程序中的关键实现函数是pf_update_resample，示意图如下所示：

其中，颜色越深的位置粒子的数量越多。

5. 将重采样后的粒子放入直方图中

将重采样后粒子的位姿，放入到对应的直方图中。用kd-tree维护直方图的数据结构，直方图的位姿作为key，粒子的权重作为value。关键实现函数是：pf_kdtree_insert。示意图如下，其中，直方图内粒子的数量越多，直方图的颜色越深，代表该直方图的权重越大。

对应amcl中的kd-tree的插入，以距离相差最大的维度作为主轴，每一个kd-tree的节点都存放了直方图的坐标信息，示意图如下：

这里kd-tree的作用主要是在下一步聚类的时候，方便查找相邻的直方图，提高查找的效率。

6. 聚类统计结果

递归查找每个直方图周围9个固定距离（这里使用一个直方图边的尺寸作为距离）是否有包含粒子的直方图，如果有则聚为一类，关键实现函数是：pf_cluster_stats，示意图如下：

其中，明显可见c1为权重最高的类。再次变量粒子群，找到c1中的粒子（实现函数：pf_kdtree_get_cluster），取它们位姿的均值作为最终结果发布。