[人工智能]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_ == LASER_MODEL_BEAM)
  laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_,
                       sigma_hit_, lambda_short_, 0.0);
else if(laser_model_type_ == LASER_MODEL_LIKELIHOOD_FIELD_PROB){
  ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
  laser_->SetModelLikelihoodFieldProb(z_hit_, z_rand_, sigma_hit_,
			laser_likelihood_max_dist_, 
			do_beamskip_, beam_skip_distance_, 
			beam_skip_threshold_, beam_skip_error_threshold_);
  ROS_INFO("Done initializing likelihood field model.");
}
else
{
  ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
  laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
                                  laser_likelihood_max_dist_);
  ROS_INFO("Done initializing likelihood field model.");
}

如代码中所示，根据laser_model_type_ 所描述的测量模型，选择相应的设置函数，完成模型的初始化工作。

三、采样部分代码解析

采样部分的代码位于AmclNode::laserReceived函数中，在每一次接收到激光数据的时候运行。

运动模型

运动模型的算法实现如下图所示：

2~4行：通过里程计读数u_t中包含的x、y、θ变化量，计算包含了噪声的三段位姿变换；
5~7行：为三段噪声建立高斯分布模型，从中获取高斯随机数作为采样值，将前面计算好的三段位姿变换，减去该噪声值，获得更为准确的位姿变换值；
8~10行：更新x_(t-1)的位姿。

运动模型的函数名称是AMCLOdom::UpdateAction，在AmclNode::laserReceived中被调用，输入的参数是里程计统计的位姿变化量delta，关于ODOM_MODEL_DIFF模型，其实现如下：

1. 计算里程计模型的三段位姿变化：rot1、tran、rot2；
   
   代码实现如下，根据三角关系，计算位姿变换的值：

if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] + 
            ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
  delta_rot1 = 0.0;
else
  delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
                          old_pose.v[2]);
delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
                   ndata->delta.v[1]*ndata->delta.v[1]);
delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);

2. 利用高斯分布和噪声参数计算旋转和平移噪声，从传入的里程计变化量中，消除相应阶段的噪声，获得更为准确的三段位姿变化：rot1_hat、tran_hat、rot2_hat；

delta_rot1_hat = angle_diff(delta_rot1,
                            pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise +
                                            this->alpha2*delta_trans*delta_trans));
delta_trans_hat = delta_trans - 
         pf_ran_gaussian(this->alpha3*delta_trans*delta_trans +
                         this->alpha4*delta_rot1_noise*delta_rot1_noise +
                         this->alpha4*delta_rot2_noise*delta_rot2_noise);
delta_rot2_hat = angle_diff(delta_rot2,
                            pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise +
                                            this->alpha2*delta_trans*delta_trans));

3. 更新粒子位姿

// Apply sampled update to particle pose
sample->pose.v[0] += delta_trans_hat * 
        cos(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[1] += delta_trans_hat * 
        sin(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;

测量模型

测量模型默认采样测距仪似然算法，其算法实现如下图所示：

5~6行：通过几何关系，计算全局坐标系下激光测量端点的位姿；
7行：计算激光测量端点和栅格地图中最近障碍物的距离dist；
8行：计算期望概率p(z_t | x_t, m)。

测距仪似然算法的实现函数是AMCLLaser::LikelihoodFieldModel，主要实现如下所示：

1. 计算激光雷达的全局坐标：

pose = sample->pose;
pose = pf_vector_coord_add(self->laser_pose, pose);

self->laser_pose代表激光雷达相对于车体坐标系的偏移，pose为车体在全局坐标系下的位姿，所以，pose + self->laser_pose即为激光雷达的全局坐标。

2. 计算激光测量端点位姿：

hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);

3. 计算测量概率 p(hit)：

// Part 1: Get distance from the hit to closest obstacle. 使用测量噪声参数计算测量概率
// Off-map penalized as max distance
if(!MAP_VALID(self->map, mi, mj))
  z = self->map->max_occ_dist;
else
  z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist; // 激光端点到地图内最近障碍物的距离，即测量值与期望的距离：(x-b)
// Gaussian model
// 高斯函数: f(x) = a * exp((-(x-b)^2) / 2c^2)，a是期望（均值）的概率，b是期望u，c是标准差sigma
// NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
pz += self->z_hit * exp(-(z * z) / z_hit_denom); // self->z_hit: 概率密度函数，期望（均值）的概率

使用高斯函数计算当前测量的概率，其中，期望值是与激光端点最近障碍物的位姿，测量值是激光端点的位姿，标准差c和期望的概率a为可调参数。

4. 计算随机测量概率 p(rand)：z_rand为可调参数，z_rand_mult为全部粒子数量的倒数，因为随机测量概率是均匀分布的。

pz += self->z_rand * z_rand_mult;

5. 计算期望概率：p = ppz，程序中对该算法进行了改良：p += pzpz*pz，其中，pz = p(hit) + p(rand)。该概率模型不计算测量失败的概率，因为测量失败直接舍弃。

四、重采部分样代码解析

低方差重采样

算法实现见章节一所述，下面解析代码关键实现部分：

1. 建立重采样概率累积表：

// Build up cumulative probability table for resampling.
// 译: 建立重采样的累积概率表
// TODO: Replace this with a more efficient procedure
// (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html)
c = (double*)malloc(sizeof(double)*(set_a->sample_count+1));
c[0] = 0.0;
for(i=0;i<set_a->sample_count;i++)
  c[i+1] = c[i]+set_a->samples[i].weight;

2. 重采样

double r;
// 1. 生成1.0~0.0之间的随机数
r = drand48();
for(i = 0 ;i < set_a->sample_count; i++)
{
  // 2. 遍历找到合适的 i
  if((c[i] <= r) && (r < c[i+1]))
    break;
}
assert(i<set_a->sample_count);

// 3. 确定从 集合a 中的重采样的值
sample_a = set_a->samples + i;

assert(sample_a->weight > 0);

// 4. Add sample to list
sample_b->pose = sample_a->pose; // 将重采样的值赋给 集合b

3. 更新存放重采样结果的kd-tree，并重新聚类

五、结果输出

1. 调用函数pf_get_cluster_stats，从kd-tree聚类结果中获取最大的权重以及其均值和协方差矩阵：

for(int hyp_count = 0;
    hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
{
  double weight;
  pf_vector_t pose_mean;
  pf_matrix_t pose_cov;
  if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov)) // 获取将粒子聚类后的结果
  {
    ROS_ERROR("Couldn't get stats on cluster %d", hyp_count);
    break;
  }

  hyps[hyp_count].weight = weight;
  hyps[hyp_count].pf_pose_mean = pose_mean;
  hyps[hyp_count].pf_pose_cov = pose_cov;

  if(hyps[hyp_count].weight > max_weight)
  {
    max_weight = hyps[hyp_count].weight;
    max_weight_hyp = hyp_count;
  }
}

2. 如果最大的权重大于0，发布最大权重粒子类群（cluster）的位姿：

geometry_msgs::PoseWithCovarianceStamped p;
// Fill in the header
p.header.frame_id = global_frame_id_;
p.header.stamp = laser_scan->header.stamp;
// Copy in the pose
p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
tf2::Quaternion q;
q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
tf2::convert(q, p.pose.pose.orientation);
// Copy in the covariance, converting from 3-D to 6-D
pf_sample_set_t* set = pf_->sets + pf_->current_set;
for(int i=0; i<2; i++)
{
  for(int j=0; j<2; j++)
  {
    // Report the overall filter covariance, rather than the
    // covariance for the highest-weight cluster
    p.pose.covariance[6*i+j] = set->cov.m[i][j];
  }
}
// Report the overall filter covariance, rather than the
// covariance for the highest-weight cluster
p.pose.covariance[6*5+5] = set->cov.m[2][2];
pose_pub_.publish(p);

3. 发布odom到map的tf变换

geometry_msgs::PoseStamped odom_to_map;
try
{
  tf2::Quaternion q;
  q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
  tf2::Transform tmp_tf(q, tf2::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
                                        hyps[max_weight_hyp].pf_pose_mean.v[1],
                                        0.0));
  geometry_msgs::PoseStamped tmp_tf_stamped;
  tmp_tf_stamped.header.frame_id = base_frame_id_;
  tmp_tf_stamped.header.stamp = laser_scan->header.stamp;
  // tmp_tf 是base在map坐标系下的坐标，tmp_tf.inverse() 则是map在base坐标系下的坐标
  tf2::toMsg(tmp_tf.inverse(), tmp_tf_stamped.pose);
  // 将base下的坐标转换到odom坐标
  // 这里的odom_to_map并非真的odom-->map，而是反过来map-->odom，底下的latest_tf_会调用inverse函数进行反转
  this->tf_->transform(tmp_tf_stamped, odom_to_map, odom_frame_id_);
}
catch(tf2::TransformException)
{
  ROS_DEBUG("Failed to subtract base to odom transform");
  return;
}

tf2::convert(odom_to_map.pose, latest_tf_);
latest_tf_valid_ = true;
if (tf_broadcast_ == true)
{
  // We want to send a transform that is good up until a
  // tolerance time so that odom can be used
  ros::Time transform_expiration = (laser_scan->header.stamp +
                                    transform_tolerance_);
  geometry_msgs::TransformStamped tmp_tf_stamped;
  tmp_tf_stamped.header.frame_id = global_frame_id_;
  tmp_tf_stamped.header.stamp = transform_expiration;
  tmp_tf_stamped.child_frame_id = odom_frame_id_;
  tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform);

  this->tfb_->sendTransform(tmp_tf_stamped); // 发布 odom 在 map 坐标系下的 tf
  sent_first_transform_ = true;
}

附录：相关参考网站

• ROS amcl 官方网站：
  http://wiki.ros.org/amcl

• 似然域模型参考：
  https://blog.csdn.net/huafy96/article/details/106549974

• 里程计模型参考：
  https://blog.csdn.net/qq_34672671/article/details/105560612
  模型错误指正：https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/
  https://blog.csdn.net/qq_17032807/article/details/97650380

• kd-tree参考：
  https://zhuanlan.zhihu.com/p/112246942
  https://www.jianshu.com/p/ba79ebddd93c

• 高斯函数分析：
  https://blog.csdn.net/qinglongzhan/article/details/82348153