AMCL源码解析（ROS-noetic）

一、AMCL算法引言

节选自中文版《概率机器人》8.3.5章节，amcl的算法实现如下图所示：

这里稍微对该算法的几个阶段做一些解释：

• 算法输入参数：
  1、X_t-1：上一时刻的粒子集合；
  2、u_t：运动过程中的控制量；
  3、z_t：测量值；
  4、m：粒子数量；

• 初始化阶段
  1、静态变量：Wslow是长期权重，Wfast是代表短期权重，用于重采样时判断是否需要添加新的随机粒子；
  2、粒子集合：Xt（上划线）采样后粒子集合，Xt重采样后粒子集合

• 采样阶段
  
  如上图算法所示，将每个上一时刻的粒子和运动过程的控制量作为参数，输入到运动模型（ODOM_MODEL_DIFF），获取到对当前时刻运动状态的预测x_t；然后，将该预测x_t、测量值z_t以及m作为参数，输入到测量模型（LikelihoodFieldModel），获取对当前时刻运动状态的预测的权重w_t；将预测位姿和预测权重作为粒子，放入粒子集合Xt（上划线）中，完成采样。

• 重采样阶段
  
  首先，更新w_fast和w_slow，其中要求参数：0 <= a_slow << a_fast 。然后对于粒子集合Xt（上划线）中的粒子，按照max{0.0, 1.0-w_fast/w_slow}的概率添加随机粒子到X_t集合中，即如果1.0-w_fast/w_slow小于0，则不添加随机粒子；如果大于0，则以1.0-w_fast/w_slow的结果作为概率，添加随机粒子。如果没有发生添加随机粒子，则进行重要性重采样（采样算法如下图所示），并将采样结果放入到粒子集合Xt中。
  
  其实现原理如下图所示：
  
  其中，c为采样前粒子集合的概率累积表，从第一个粒子开始，逐步累积该粒子的权重；r为固定的[0, 1/M]的随机数，r的值会才采样后按照采样数量(m-1) * 1/M进行递增；当U < c时，则进行采样，将粒子放入到新的粒子集合Xt（上划线）中。
  amcl代码中的实现方式稍有不同，但原理是一样的，其实现如下图所示：
  
  其中，r的值不再是固定的随机数，而是[0, 1]的随机数。判断条件则变为：c[i] < r < c[i + 1]。
  可见，经过了重采样，c[2]粒子被排除了，粒子集中在了c[1]附近，实现了重采样对粒子进行筛选的目的。

下面，针对性解析上述算法的代码实现。

二、初始化部分代码解析

算法的初始化发送在函数AmclNode::handleMapMessage，当接收到地图的时候，完成算法相关的初始化工作，具体内容如下：

滤波器的创建

pf_ = pf_alloc(min_particles_, max_particles_,
               alpha_slow_, alpha_fast_,
               (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
               (void *)map_);

pf_alloc函数主要完成了：

1. 创建两个粒子集合，粒子的初始位姿为0，均匀权重；
2. 对于每个粒子集合，进行kd-tree初始化，树申请了3倍最大粒子数量的节点空间；
   

滤波器的初始化

// Initialize the filter
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); // alg_note: 以均值和协方差为0初始化滤波器
pf_init_ = false;

pf_init函数主要完成了：

1. 根据从参数服务器获取的初始位姿（即初始均值）和协方差矩阵，调整当前正在使用的粒子集合的位姿，使其分布服从高斯分布；
2. 将粒子集合插入kd-tree，重新计算聚类参数；
   

运动模型的初始化

//  alpha1 旋转阶段中角度变化所导致的旋转噪音
//  alpha2 旋转阶段中位置变化所导致的旋转噪音
//  alpha3 平移阶段中位置变化所导致的平移噪音
//  alpha4 平移阶段中角度变化所导致的平移噪音
delete odom_;
odom_ = new AMCLOdom();
ROS_ASSERT(odom_);
odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );

对于ODOM_MODEL_DIFF模型，只用到了alpha1~4四个参数。

测量模型的初始化

delete laser_;
laser_ = new AMCLLaser(max_beams_, map_);
ROS_ASSERT(laser_);
if(laser_model_type_