AMCL代码详解（一）

AMCL是移动机器人中应用较为广泛的一种定位算法，也算是在ROS中被广泛使用的一种开源算法了。它的核心还算是比较好理解的，主要包括了以下几个部分：

1.位姿初始化

AMCL的初始位姿估计可以来自于两个地方：

第一种是用好粒子滤波算法估计出的机器人位姿p作为last_publishedpose存储备用。

updatePoseFromServer();//从参数服务器中获取初始位姿，这个位姿可以来自于之前节点关闭时存储于服务器中的数据，没有的话默认为原点

第二种是订阅initialpose的pose，经过initialPoseReceived函数的处理。

initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);

如果说initialpose话题没有订阅到的话，算法默认使用的初始位姿就是updatePoseFromServer函数中的原点，比如说我们机器人其实不在原点但是打开节点时会看到所有散布的点都在原点附近：

2.创建粒子滤波器

粒子滤波器的创建方式主要有两种：

第一种是使用高斯模型生成一系列粒子点：

void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)

第二种是使用其他模型生成一系列粒子点：

void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data)

这两者之间的主要区别在于它们粒子的随机分布方式不太一样。

3.根据运动更新粒子位姿

这个的代码主要在amcl_odom.cpp中，它的主要作用是用于粒子的位姿更新。根据不同的机器人模型计算出一系列粒子点的估计位姿。这里应该会用到odom数据。

bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)

4.根据观测模型更新粒子权重

4.1.确认激光模型

激光模型有4种，一般我们使用的是BEAM类型的

if(this->model_type == LASER_MODEL_BEAM)
    pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
  else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD)
    pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data);  
  else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD_PROB)
    pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModelProb, data);  
  else
    pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);

4.2.根据激光来更新粒子模型（权重）

1.选择似然域模型:likelihood_filed 来计算粒子sample集set的权重

total = (*sensor_fn) (sensor_data, set);

2.遍历粒子sample集set的每个sample对象
1）利用粒子sample集sample对象的权重对w_avg赋值
2）粒子集sample对象的权重归一化

3.将w_avg除以粒子集sample数目，得到新的w_avg

5.重采样

算法中给定了一定频率进行重采样，但是采样频率默认值是1所以其实应该是每一次算法都会进行重采样。

void pf_update_resample(pf_t *pf)

5.1.将粒子sample集set_a的粒子sample对象的权重依次取出来

for(i=0;i<set_a->sample_count;i++)
    c[i+1] = c[i]+set_a->samples[i].weight;

5.2.创建kdtree为选择性采样做准备

这里主要用到了kd树的知识，需要补充

pf_kdtree_clear(set_b->kdtree);

5.3.使用随机生成位姿的模型生成sample_b的位姿

sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);

5.4.遍历粒子sample集set_b,将sample_b权重归一化

for (i = 0; i < set_b->sample_count; i++)
  {
    sample_b = set_b->samples + i;
    sample_b->weight /= total;
  }

5.5.重新计算粒子sample集set_b的簇的统计特性

pf_cluster_stats(pf, set_b);

5.6.粒子滤波器更新收敛

 pf_update_converged(pf);

6.发布粒子云

就是rviz上显示的那些带箭头的东西，每一个箭头都代表了一个可能的位姿

7.发布位姿

发布出一个最有可能是机器人的位姿到amcl_pose话题中

这是AMCL的大致流程，细节部分继续补齐。