AMCL代码详解（五）根据激光观测更新粒子权重

上一篇中讲述了里程计在amcl中的作用，算法根据里程计反馈对每个粒子的位姿进行更新。在粒子更新后，则需要根据激光的观测数据对粒子的权重进行更新，这里主要看一下激光电云在amcl中的作用：

1、激光回调函数

激光回调函数在amcl_node.cpp中：

AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)

前面一大半都是各种前置条件处理，包括fram_id判断，第一帧激光处理，粒子未初始化时候的判断等等，这些暂时不细看，跳过到权重更新：

lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);

激光的整个处理都由这个函数执行，传参有两个，一个为结构体代表粒子，也就是每一个可能的机器人位姿。另一个data数据为激光数据

2、模型判断

从UpdateSensor跳转到amcl_laser.cpp中，这里的：

bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)
{
  if (this->max_beams < 2)
    return false;

  // Apply the laser sensor model
  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);

  return true;
}

进行模型的判断，后面需要根据不同的模型进行处理，一般情况下我们使用的默认模型是LikelihoodFieldModel这个。这是一个参数，再launch文件中可以更改

3、权重更新

权重更新是这一章中最重要的地方。它的代码并不是很长，简单做了下注释：

//该函数主要是针对动态障碍物概率去除,选通过判断每个粒子相同方向的激光点,在地图内与障碍物的最近距离,
//是否小于一定距离阈值,并且超过一定数量阈值, 就不进行计算权重,如果在符合范围内就权重叠加.
double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set)
{
  AMCLLaser *self;//类对象
  int i, j, step;//点云序号,粒子序号,一个区域有多少个点
  double z, pz;//障碍物距离,概率值
  double p;//概率对数值
  double obs_range, obs_bearing;//数据距离,和角度
  double total_weight;//总权重
  pf_sample_t *sample;//单粒子
  pf_vector_t pose;//机器人位姿->雷达世界坐标
  pf_vector_t hit;//世界坐标
  // AMCLLaserData : public AMCLSensorData:public  AMCLSensor
  // AMCLLaser : public AMCLSensor    
  // 最后都是在创建对象AMCLSensor
  self = (AMCLLaser*) data->sensor;
  // 初始化权重
  total_weight = 0.0;
  // 计算粒子权重 
  // Compute the sample weights
  for (j = 0; j < set->sample_count; j++)
  {
    sample = set->samples + j;//粒子序号
    pose = sample->pose;      //获取机器人位姿

    // Take account of the laser pose relative to the robot
    //结合机器人相对雷达的坐标,求出雷达在世界的坐标
    pose = pf_vector_coord_add(self->laser_pose, pose);

    p = 1.0;

    // Pre-compute a couple of things
    // 预算噪声协方差和随机概率
    double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;
    double z_rand_mult = 1.0/data->range_max;
    //高斯分布模型,概率分布,取最大距离的概率
    //间隔()默认30个点读取一个点的数量
    step = (data->range_count - 1) / (self->max_beams - 1);

    // Step size must be at least 1
    //算要分成多少个区域
    //至少分一个区域
    if(step < 1)
      step = 1;
    // beam_ind 检测点序号,每个区域为step个粒子
    for (i = 0; i < data->range_count; i += step)
    {
      obs_range = data->ranges[i][0];
      obs_bearing = data->ranges[i][1];

      // This model ignores max range readings点云距离合理判断
      if(obs_range >= data->range_max)
        continue;

      // Check for NaN角度数值合理判断
      if(obs_range != obs_range)
        continue;

      pz = 0.0;

      // Compute the endpoint of the beam
      // 激光点云结合雷达坐标求出点云在世界的坐标
      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);

      // Convert to map grid coords.世界转栅格坐标
      int mi, mj;
      mi = MAP_GXWX(self->map, hit.v[0]);
      mj = MAP_GYWY(self->map, hit.v[1]);
      
      // 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;
        //否则求出该激光点离障碍物最短的距离.可以理解为当前束激光测的距离和地图匹配的距离做差值
        //  但是它实际是在激光点在附近每个方向都进行搜索最近的障碍的距离,取最小值
      // Gaussian model
      // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
      // 障碍权重乘以高斯概率模型
      // z越大说明点距离障碍物越远，则Pz越小，Pz越小则P越小，P越小权重越小，权重越小说明该点为机器人的坐标点的可能性越小
      pz += self->z_hit * exp(-(z * z) / z_hit_denom);
      // Part 2: random measurements
      //随机权重乘以随机概率模型
      pz += self->z_rand * z_rand_mult;

      // TODO: outlier rejection for short readings
      // 概率在正常范围
      assert(pz <= 1.0);
      assert(pz >= 0.0);
      //      p *= pz;
      // here we have an ad-hoc weighting scheme for combining beam probs
      // works well, though...
      //z 越大()距离越远，pz 越小，粒子权重越小
      p += pz*pz*pz;
    }//一个粒子
    
    sample->weight *= p;
    // 每个粒子总权重
    total_weight += sample->weight;
  }

该算法的主要思想有以下几步:

1).将激光点投影到栅格地图下。
2).在该投影点的附近找到离该点最近的障碍物点，计算两者之间的距离。
3).对距离取对数，表示权重，距离越大权重越小。这里的含义是当前点周围一定范围内匹配障碍物，实际的障碍物离点云越远说明当前位姿越不可信
4).将所有点的置信度相加并返回

amcl算法通过这种方式，对每一个可能的机器人位姿进行点云投影，计算点云中每个点距离最近的障碍物的距离，以障碍物距离的对数表示这个点可能为机器人真实位姿的可能性。也就是说，对于一次机器人运动，首先根据里程计信息预加噪声产生一系列可能的机器人位姿，这个位姿是不确定的，它可能表示了机器人的最终状态也可能是一个错误状态。然后根据激光电云进行位置判断，取一个当前可能的机器人位姿，然后将激光电云投影到当前地图下，然后求每一个点到地图上障碍物的最近距离，用对数表示该点的置信度，最后所有点相加得到该点的置信度。通过这种方式可以得到当前所有带噪声的预测位姿中哪个位姿的激光点云与障碍物之间的实际距离最近，也就说明该点的置信度最高。最有可能是实际机器人的点。