AMCL运行流程

因为去年读过一次AMCL的源码，那时候读的时候，并不能完全理解某些模块，如map，求粒子簇的这块内容，所以再看的时候，是以地图为切入点看的，然后就是pf。这两部分弄差不多了，再重头开始整理下流程。

1、从main函数中的new AmclNode()开始

AmclNode::AmclNode() :
        sent_first_transform_(false),
        latest_tf_valid_(false),
        map_(NULL),
        pf_(NULL),
        resample_count_(0),
        odom_(NULL),
        laser_(NULL),
          private_nh_("~"),
        initial_pose_hyp_(NULL),
        first_map_received_(false),
        first_reconfigure_call_(true)
{
  boost::recursive_mutex::scoped_lock l(configuration_mutex_);
/***参数省略**/
  odom_frame_id_ = stripSlash(odom_frame_id_);
  base_frame_id_ = stripSlash(base_frame_id_);
  global_frame_id_ = stripSlash(global_frame_id_);

  updatePoseFromServer();//从参数服务器中获取初始位姿

  cloud_pub_interval.fromSec(1.0);
  tfb_.reset(new tf2_ros::TransformBroadcaster());
  tf_.reset(new tf2_ros::Buffer());
  tfl_.reset(new tf2_ros::TransformListener(*tf_));

  //后验位姿+一个6*6的协方差矩阵（xyz+三个转角）
  pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true);

  //粒子位姿的数组
  particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 2, true);

  //发布服务，获得没有给定初始位姿的情况下在全局范围内初始化粒子位姿；里面有多层函数调用
  global_loc_srv_ = nh_.advertiseService("global_localization", 
                     &AmclNode::globalLocalizationCallback,
                                         this);// 该Callback调用pf_init_model
                                         // 然后调用AmclNode::uniformPoseGenerator
                                         // 在地图的free点随机生成pf->max_samples个粒子

  //发布服务，没运动模型更新的情况下也暂时更新粒子群
  nomotion_update_srv_= nh_.advertiseService("request_nomotion_update", &AmclNode::nomotionUpdateCallback, this);

//发布服务，回调函数中包含handleMapMessage()和handleInitialPoseMessage(req.initial_pose);
//分别处理地图转换和初始位姿信息（这里用高斯来初始化），详细见函数内部说明
  set_map_srv_= nh_.advertiseService("set_map", &AmclNode::setMapCallback, this);

  //订阅服务，根据激光扫描数据更新粒子，laserReceived是粒子滤波主要过程
  laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100);
  laser_scan_filter_ = 
          new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_,
                                                             *tf_,
                                                             odom_frame_id_,
                                                             100,
                                                             nh_);
  laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,
                                                   this, _1));

  //订阅服务，根据rviz中给的初始化位姿
  initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);

  //根据参数，选择是订阅地图话题还是调用静态地图
  if(use_map_topic_) {
    map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this);
    ROS_INFO("Subscribed to map topic.");
  } else {
    requestMap();  //请求静态地图，调用map_server节点
  }
  m_force_update = false;

  //发布服务，动态参数配置器
  dsrv_ = new dynamic_reconfigure::Server<amcl::AMCLConfig>(ros::NodeHandle("~"));
  dynamic_reconfigure::Server<amcl::AMCLConfig>::CallbackType cb = boost::bind(&AmclNode::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);

  // 15s timer to warn on lack of receipt of laser scans, #5209
  //检查上一次收到激光雷达数据至今是否超过15秒，如超过则报错
  laser_check_interval_ = ros::Duration(15.0);
  check_laser_timer_ = nh_.createTimer(laser_check_interval_, 
                                       boost::bind(&AmclNode::checkLaserReceived, this, _1));

  diagnosic_updater_.setHardwareID("None");
  diagnosic_updater_.add("Standard deviation", this, &AmclNode::standardDeviationDiagnostics);
}

按照AmclNode的构造函数，依次执行参数初始化，节点的订阅发布，数据订阅时处理各种函数。如下一一分析。

1、globalLocalizationCallback函数：用到pf_init_model在前面的粒子滤波器初始化解析过，相较pf_init，这里是均匀分布初始化一个滤波器，其他的都一样。

bool
AmclNode::globalLocalizationCallback(std_srvs::Empty::Request& req,
                                     std_srvs::Empty::Response& res)
{
  if( map_ == NULL ) {
    return true;
  }
  boost::recursive_mutex::scoped_lock gl(configuration_mutex_);
  ROS_INFO("Initializing with uniform distribution");
  pf_init_model(pf_, (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
                (void *)map_);//粒子滤波器初始化的时候调用的是均匀分布的位姿模型
  ROS_INFO("Global initialisation done!");
  pf_init_ = false;
  return true;
}

这个函数在一个发布服务里，不明白这么做的意义，后面好好学习下ROS的服务机制，再思考这个问题。

 

2、nomotionUpdateCallback函数:没有运动时，强制更新滤波器，这里函数只是对update的标识参数赋值true。具体的更新操作放在更新函数里。

// force nomotion updates (amcl updating without requiring motion)
bool 
AmclNode::nomotionUpdateCallback(std_srvs::Empty::Request& req,
                                     std_srvs::Empty::Response& res)
{
    m_force_update = true;
    //ROS_INFO("Requesting no-motion update");
    return true;
}

 

3、setMapCallback函数

bool
AmclNode::setMapCallback(nav_msgs::SetMap::Request& req,
                         nav_msgs::SetMap::Response& res)
{
  handleMapMessage(req.map);//地图转换，记录free space，以及初始化pf_t 结构体,实例化运动模型(odom)和观测模型(laser)；
  handleInitialPoseMessage(req.initial_pose);// 根据接收的初始位姿消息,在该位姿附近高斯采样重新生成粒子集
  res.success = true;
  return true;
}

 其中handleMapMessage在前面的文章已经分析过了，就不再说。

 

4、laserReceived函数：不同激光雷达的数据标记、滤波器更新参数初始化、运动更新、观测更新、重采样、发布位姿数据。由于代码较长，只列出运动更新、观测更新、重采样的代码。这部分似然域模型作为测量模型，及重采样的实现原理，参阅《概率机器人》。

运动更新:delta是上一时刻pose和pf_odom_pose的差值

// Apply the action model
bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)
{
  AMCLOdomData *ndata;
  ndata = (AMCLOdomData*) data;

  // Compute the new sample poses
  pf_sample_set_t *set;

  set = pf->sets + pf->current_set;
 // pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);
  
    for (int i = 0; i < set->sample_count; i++) {
        pf_sample_t *sample = set->samples + i;
        sample->pose.v[0] += ndata->delta.v[0];
        sample->pose.v[1] += ndata->delta.v[1];
        sample->pose.v[2] += ndata->delta.v[2];
    }

  return true;
}

 

观测更新：

// Update the filter with some new sensor observation
void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data)
{
  int i;
  pf_sample_set_t *set;
  pf_sample_t *sample;
  double total;

  set = pf->sets + pf->current_set;

  // Compute the sample weights
  total = (*sensor_fn) (sensor_data, set);
  
  if (total > 0.0)
  {
    // Normalize weights  归一化权重
    double w_avg=0.0;
    for (i = 0; i < set->sample_count; i++)
    {
      sample = set->samples + i;
      w_avg += sample->weight;
      sample->weight /= total;  //权重求平均，因为最后算出的total_weight是超过1的
    }
    // Update running averages of likelihood of samples (Prob Rob p196)
    w_avg /= set->sample_count;
    if(pf->w_slow == 0.0)
      pf->w_slow = w_avg;
    else
      pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow);
    if(pf->w_fast == 0.0)
      pf->w_fast = w_avg;
    else
      pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast);
  }
  else
  {
    // Handle zero total
    for (i = 0; i < set->sample_count; i++)
    {
      sample = set->samples + i;
      sample->weight = 1.0 / set->sample_count;
    }
  }

  return;
}

 

重采样：pf_update_resample函数

    if(!(++resample_count_ % resample_interval_))
    {
      pf_update_resample(pf_);
      resampled = true;
    }

// Resample the distribution 重采样分布
void pf_update_resample(pf_t *pf)
{
  int i;
  double total;
  pf_sample_set_t *set_a, *set_b;   //set_a为上周期采样的粒子，set_b为本周期将要采样的粒子
  pf_sample_t *sample_a, *sample_b;

  //double r,c,U;
  //int m;
  //double count_inv;
  double* c;

  double w_diff;

  set_a = pf->sets + pf->current_set;
  set_b = pf->sets + (pf->current_set + 1) % 2;  //这里是粒子集指针的切换

  // 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;//作用在后面

  // Create the kd tree for adaptive sampling
  pf_kdtree_clear(set_b->kdtree);
  
  // Draw samples from set a to create set b.
  total = 0;
  set_b->sample_count = 0;

  w_diff = 1.0 - pf->w_fast / pf->w_slow;  //随着粒子的更新，如果大权重集中在少数的粒子上，
  // 此时和初始的分布差异较大，即w_idff变大，需要重采样；如果是大部分粒子有较大的权重，差异不大，不进行重采样
  if(w_diff < 0.0)
    w_diff = 0.0;

  while(set_b->sample_count < pf->max_samples)
  {
    sample_b = set_b->samples + set_b->sample_count++;//set_b的粒子指针指向set_b的采样数之后的位置，用于添加重采样的粒子

    if(drand48() < w_diff)//随机生成一个粒子，
      sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);
    else
    {
      // Naive discrete event sampler
      double r;
      r = drand48();//0-1的值作为概率
      for(i=0;i<set_a->sample_count;i++)  //通过遍历，找到该随机数对应权重是哪个粒子，r代表的是权重，
      {
        if((c[i] <= r) && (r < c[i+1])) //相当于概率直方图的bin，以权重作bin的范围，可以参考c[i+1] = c[i]+set_a->samples[i].weight;表示区间范围,r在区间范围内的权重，找到i
          break;
      }
      assert(i<set_a->sample_count);

      sample_a = set_a->samples + i;//在原始采样集set_a中找到随机产生的权重所在的粒子i处，取这个地方的指针

      assert(sample_a->weight > 0);

      // Add sample to list
      sample_b->pose = sample_a->pose;//将i处的粒子添加到sample_b中
    }

    sample_b->weight = 1.0;
    total += sample_b->weight;//相当于计数

    // Add sample to histogram
    pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);//加入kdtree中

    // See if we have enough samples yet
    if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
      break;
  }
  
  // Reset averages, to avoid spiraling off into complete randomness.
  if(w_diff > 0.0)
    pf->w_slow = pf->w_fast = 0.0;

  //fprintf(stderr, "\n\n");

  // Normalize weights
  for (i = 0; i < set_b->sample_count; i++)
  {
    sample_b = set_b->samples + i;
    sample_b->weight /= total;//重采样后的权重都是1/n
  }
  
  // Re-compute cluster statistics
  pf_cluster_stats(pf, set_b);

  // Use the newly created sample set
  pf->current_set = (pf->current_set + 1) % 2; //此时set_b成为当前粒子集

  pf_update_converged(pf);

  free(c);
  return;
}

 

 重采样粒子数的限制：pf_resample_limit函数

// Compute the required number of samples, given that there are k bins
// with samples in them.  This is taken directly from Fox et al.
int pf_resample_limit(pf_t *pf, int k)//这里是KLD自适应采样的过程，流程参照prob robot P201，
// k是kdtree的叶子节点个数，表示有几个粒子在一个cluster里面
// n是书上的M_x
{
  double a, b, c, x;
  int n;

  if (k <= 1)//初始时，k较小，采样粒子数是最大采样数
    return pf->max_samples;

  a = 1;
  b = 2 / (9 * ((double) k - 1));
  c = sqrt(2 / (9 * ((double) k - 1))) * pf->pop_z;
  x = a - b + c;

  n = (int) ceil((k - 1) / (2 * pf->pop_err) * x * x * x);

  if (n < pf->min_samples)
    return pf->min_samples;
  if (n > pf->max_samples)
    return pf->max_samples;
  
  return n;
}

AMCL的整体代码基本上就分析完了。总结：

1、地图信息的用途：初始化滤波器，可以均匀或者高斯分布的在地图空白区域撒粒子;似然域模型计算z变量时，取z的最近的障碍物点的距离值。（这点需要看原理）

2、滤波器分运动和观测更新两部分，观测用的是似然域模型，重采样后粒子权重一样。

3、kdtree的生成，是为了方便计算粒子簇，找到权重高的粒子簇位姿的平均值作为机器人的位姿估计。