AMCL 激光测量模型

一、似然域模型 likelihood_field model

1、原理

       它是一种“特设(ad hoc)”算法，不必计算相对于任何有意义的传感器物理生成模型的条件概率。而且，这种方法在实践中运行效果良好。即使在混乱的空间，得到的后验也更光滑，同时计算更高效。

       主要思想是将传感器扫描的终点映射到栅格地图。计算与栅格地图最近障碍之间的距离。

 

           

 

2、代码

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;

  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;// 随机测量相关，1/雷达最大距离

    step = (data->range_count - 1) / (self->max_beams - 1);// 总角度/间隔大小

    // Step size must be at least 1
    if(step < 1)
      step = 1;
    //遍历激光点数，间隔选点，因为计算时间原因
    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)
      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...
      p += pz*pz*pz;
    }

    sample->weight *= p;
    total_weight += sample->weight;
  }

  return(total_weight);
}

z 越大，pz 越小，粒子权重越小。

二、波束模型 beam model

1、原理

这里的模型采用四类测量误差，所有这些对使模型工作很重要。这四类误差包括小的测量噪声、意外对象引起的误差，以及由于未检测到对象引起的误差和随机意外噪声。因此，期望模型是四个密度的混合，每一中密度都与一个特定类型的误差有关。

1）具有局部测量噪声的正确范围。在一个理想世界中，一个测距仪总是测量位于其测量领域内的最近物体的正确距离。距离

可由射线投射(ray casting)来确定。实际是由当前激光点的位置，在其射线方向上搜索障碍物栅格得到的。这个测量噪声通常由一个窄的均值为、标准偏差为的高斯建模。

测量概率由以下公式给出：

                                          

                                                 

归一化因子为：

                                                       

2）意外对象。移动机器人的环境是动态的，而地图m是静态的。因此，地图中不包含的对象可能引起测距仪产生惊人的短距离——至少与地图相比时。典型的移动机器人就是与机器人共享操作空间的人。处理这类对象的一种方法是将它们作为状态向量的一部分来对待并估计它们的位置；另一种更简单的方法是将它们作为传感器噪声来处理。作为传感器噪声来处理，未建模对象具有这样的特性，即它们会导致比更短而不是更长的距离。

                  

3）检测失败。有时环境中障碍会被完全忽略。例如，在声呐传感器遇到了镜面反射时，会经常发生此情况。当用激光测距仪检测到黑色吸光的对象时，或者某些激光系统在明媚的阳光下测量物体时，也会发生检测失败。传感器检测失败的典型结果是最大距离测量问题：传感器返回它的最大允许值。由于这样的时间经常发生，那么在测量模型中明确地建立最大测量范围的模型就很必要。

        下面将用一个以为中心的点群分布来建立这种情况的模型：

                                                 

4）随机测量。最后，测距仪偶尔会产生无法解释的测量。例如，当超声波被几面墙反弹或者它们受到不同传感器之间的串扰时，声呐常产生幻读。为了使之简单化，对于这样的测量，这里将使用一个分布在完整传感器测量范围[0; ] 的均匀分布来建立模型：

                                                          

                 

                                   

实际考虑

波束模型的局限 

 

2、代码

double AMCLLaser::BeamModel(AMCLLaserData *data, pf_sample_set_t* set)
{
  AMCLLaser *self;
  int i, j, step;
  double z, pz;
  double p;
  double map_range;
  double obs_range, obs_bearing;
  double total_weight;
  pf_sample_t *sample;
  pf_vector_t pose;

  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;

    step = (data->range_count - 1) / (self->max_beams - 1);
    for (i = 0; i < data->range_count; i += step)
    {
      obs_range = data->ranges[i][0];//真实测量距离
      obs_bearing = data->ranges[i][1];//角度

      // Compute the range according to the map
      map_range = map_calc_range(self->map, pose.v[0], pose.v[1],
                                 pose.v[2] + obs_bearing, data->range_max);
      pz = 0.0;

      // Part 1: good, but noisy, hit
      z = obs_range - map_range;
      pz += self->z_hit * exp(-(z * z) / (2 * self->sigma_hit * self->sigma_hit));

      // Part 2: short reading from unexpected obstacle (e.g., a person)
      if(z < 0)
        pz += self->z_short * self->lambda_short * exp(-self->lambda_short*obs_range);

      // Part 3: Failure to detect obstacle, reported as max-range
      if(obs_range == data->range_max)
        pz += self->z_max * 1.0;

      // Part 4: Random measurements
      if(obs_range < data->range_max)
        pz += self->z_rand * 1.0/data->range_max;

      // 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...
      p += pz*pz*pz;
    }

    sample->weight *= p;
    total_weight += sample->weight;
  }

  return(total_weight);
}