amcl_laser 似然域--概率计算

1、蒙特卡罗定位主要流程：

1）评分的基础方法是机器人感知模型----测距仪似然域模型。这里的模型采用四类测量误差，小的测量噪声hit、意外对象引起的误差short，以及由于未检测到对象引起的误差max和随机意外噪声rand。

2）通过按照不同权重进行组合上面四类重点测量误差，得到一个混合分布模型

3）通过该方法计算每个粒子，将平面栅格地图转化为似然地图

4）伪代码

伪代码程序用似然域计算测量概率的算法。外循环将各个p (z|x, m) 的值相乘，并假定不同传感器波束的噪声是相互独立的。
第 4 行检查传感器读数是否为最大距离读数，如果是则被舍弃。第 5-8 行处理有趣的情况：会计算x-y 空间中与最近障碍物的距离（第 7 行）。在第 8 行通过将一个正态分布和一个均匀分布混合得到似然结果。(附代码和解析)

amcl_laser.cpp

//混合分布 Z_hit，Z_rand,Z_max
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;
    // sample->pose表示每个粒坐标
    // Take account of the laser pose relative to the robot
    pose = pf_vector_coord_add(self->laser_pose, pose);
    //转换坐标系，与激光雷达数据匹配
    //将传感器局部坐标经过三角变换映射到全局坐标系下,
    //self->laser_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;//无法解释的随机测量的概率分布
   //data->range_count 一帧激光点云数值
    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//计算结果 不是个 数,
      //它用于表示一个本来要返回数值的操作数未返回数值的情况。
      //NaN与任何值都不相等，
      if(obs_range != obs_range)
        continue;

      pz = 0.0;

      // Compute the endpoint of the beam//hit高斯
      hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing)；//x坐标
      hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);//y坐标

      // 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);
}