(笔记)（4）AMCL包源码分析 | 传感器模型与sensor文件夹

 

AMCL包接收地图，建立运动模型使得粒子可以跟着机器人一起运动，建立观测模型对模拟机器人运动的粒子进行打分，并且建立相关的tf变换信息，最后输出位姿。接收地图部分在上一讲已讲解完毕，这一讲讲述运动模型和观测模型对AMCL位姿输出的影响机制。

图1. AMCL包sensor文件夹展开

sensor文件夹主要表达了两个模型，一个是运动模型，根据机器人运动类型，选择相应的运动模型，用来将粒子一起运动(平移，旋转)，得到粒子样本的位姿；另一个是观测模型，根据选定的观测更新模型，对给定的位姿计算权重。

在amcl_sensor.h定义基类AMCLSensor 和基类AMCLSensorData,为两个派生类amcl_laser和amcl_odom提供统一接口。

1.传感器模型介绍

先来看AMCLSensor类，主要功能是根据运动模型更新粒子滤波器，根据观测模型初始化滤波器，根据观测模型更新粒子滤波器，仅定义运动模型里的位姿。从这里可以看到，运动模型和观测模型与粒子滤波器联系紧密。

// Base class for all AMCL sensors
class AMCLSensor
{
  // Default constructor
  public: AMCLSensor();
         
  // Default destructor
  public: virtual ~AMCLSensor();

  // Update the filter based on the action model.  Returns true if the filter
  // has been updated.
  public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);

  // Initialize the filter based on the sensor model.  Returns true if the
  // filter has been initialized.
  public: virtual bool InitSensor(pf_t *pf, AMCLSensorData *data);

  // Update the filter based on the sensor model.  Returns true if the
  // filter has been updated.
  public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);

  // Flag is true if this is the action sensor
  public: bool is_action;

  // Action pose (action sensors only)
  public: pf_vector_t pose;
};

再来看AMCLSensorData类，主要功能是把AMCLSensor类实例化的对象放在AMCLSensorData类里：

// Base class for all AMCL sensor measurements
class AMCLSensorData
{
  // Pointer to sensor that generated the data
  public: AMCLSensor *sensor;
          virtual ~AMCLSensorData() {}

  // Data timestamp
  public: double time;
};

2. 运动模型

接下来浓墨重彩地介绍运动模型，我们就amcl_odom.h 和amcl_odom.cpp展开讲述。

值得一提的是，amcl_odom.h的头文件引用使用了pf文件夹下的pf_pdf.h。不过这也没什么可紧张的，下一讲我们会介绍pf文件夹。

#include "amcl_sensor.h"
#include "../pf/pf_pdf.h" 

选择运动模型，这一般是根据机器人的运动类型来决定，比如我的机器人是差分驱动，那么我一般是选择差分运动模型odom_model_diff，如果我的机器人是全向驱动，那么就选择全向运动模型odom_model_omni。

//里程计运动模型
typedef enum
{
  ODOM_MODEL_DIFF,
  ODOM_MODEL_OMNI,
  ODOM_MODEL_DIFF_CORRECTED,
  ODOM_MODEL_OMNI_CORRECTED
} odom_model_t;     

使用继承了AMCLSensorData基类的AMCLOdomData派生类定义运动模型的数据结构。这里定义里程计的位姿pose以及里程计位姿pose的微小改变。

//里程计数据结构
// Odometric sensor data
class AMCLOdomData : public AMCLSensorData
{
  // Odometric pose 里程计位姿
  public: pf_vector_t pose;
  
  //里程计位姿改变
  // Change in odometric pose
  public: pf_vector_t delta;
};

接着定义里程计运动模型，使用AMCLOdom类来表示，它继承了AMCLSensor基类。主要的功能是定义不同运动模型的输入参数，以及通过运动来更新粒子滤波器。

 // Odometric sensor model
class AMCLOdom : public AMCLSensor
{
  // Default constructor
  public: AMCLOdom();

  public: void SetModelDiff(double alpha1, 
                            double alpha2, 
                            double alpha3, 
                            double alpha4);

  public: void SetModelOmni(double alpha1, 
                            double alpha2, 
                            double alpha3, 
                            double alpha4,
                            double alpha5);

  public: void SetModel( odom_model_t type,
                         double alpha1,
                         double alpha2,
                         double alpha3,
                         double alpha4,
                         double alpha5 = 0 );

  // Update the filter based on the action model.  Returns true if the filter
  // has been updated.
  public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);

  // Current data timestamp
  private: double time;
  
  // Model type
  private: odom_model_t model_type;

  // Drift parameters
  private: double alpha1, alpha2, alpha3, alpha4, alpha5;
};

进入amcl_odom.cpp里，几个小函数用来处理角度：normalize函数和angle_diff函数。

//normalize函数
static double
normalize(double z)
{
  return atan2(sin(z),cos(z));//atan2 返回以弧度表示的y/x的反正切；返回方向角
}

//angle_diff函数
static double
angle_diff(double a, double b)
{
  double d1, d2;
  a = normalize(a);
  b = normalize(b);
  d1 = a-b;
  d2 = 2*M_PI - fabs(d1);
  if(d1 > 0)
    d2 *= -1.0;
  if(fabs(d1) < fabs(d2))
    return(d1);
  else
    return(d2);//注意这里要么返回d1要么d2;只是为了返回一个约束在0-2pi之间的角度
}

总览amcl_odom.cpp，这里需要设置运动模型，以及将运动模型实例化。关于运动模型的设置，使用UpdateAction函数，它的输入是粒子滤波器pf以及AMCLSensorData类型的data。

//通过运动更新粒子滤波器，其输入为粒子滤波器pf和AMCLSensorData提供的data
bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data) 
{
  //在此使用AMCLOdomData类 创建一个里程计数据模型 ndata ：只有pose ,delta
  AMCLOdomData *ndata;
  //实例化,来源于传入的AMCLSensorData的AMCLOdomData
  ndata = (AMCLOdomData*) data;
  //创建新的粒子sample集
  pf_sample_set_t *set;
  //这里设定粒子sample集由传入的粒子滤波器的粒子sample集和它的当前粒子sample集
  set = pf->sets + pf->current_set;
  //这里定义旧的位姿，即X(t-1)，使用简单的vector加减运算
  pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);
  //选择运动模型的类型，这里主要介绍差分运动模型
  switch( this->model_type )
  {
  //差分运动模型
  case ODOM_MODEL_DIFF:
  {
    //设置运动采样模型，见《概率机器人》
    // Implement sample_motion_odometry (Prob Rob p 136)
    double delta_rot1, delta_trans, delta_rot2;
    double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
    double delta_rot1_noise, delta_rot2_noise;
    
    //避免两个位姿之间靠得太近
    // Avoid computing a bearing from two poses that are extremely near each
    // other (happens on in-place rotation).
    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]);//使用angle_diff函数
    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);
    
    //设置里程计的运动噪声
    // We want to treat backward and forward motion symmetrically for the
    // noise model to be applied below.  The standard model seems to assume
    // forward motion.
    delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
                                fabs(angle_diff(delta_rot1,M_PI)));
    delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
                                fabs(angle_diff(delta_rot2,M_PI)));
    //遍历粒子sample集，对粒子sample集里的每个sample对象的pose进行更新
    for (int i = 0; i < set->sample_count; i++)
    {
      //新建一个粒子sample,用于接收粒子sample集的sample对象
      pf_sample_t* sample = set->samples + i;
      
      //使用pf_ran_gaussian函数采样生成位姿的微小变化
      // Sample pose differences 使用pf_ran_gaussian()产生delta_*_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));
      //对粒子sample里的pose进行更新;得到新的sample
      // 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;//先产生 sample->pose.v[2]再计算得到上面两个
    }
  }
  break;
  }
};

要问这个粒子的更新算法原理是什么，答案就在《概率机器人》第5章 机器人运动。

图2.基于里程计信息的采样算法

总的来说，运动模型的作用就是更新粒子sample的位姿。

3.观测模型

好的，下面开始介绍观测模型，该观测模型具体在amcl_laser.h和amcl_laser.cpp实现。

值得一提的是，amcl_laser.h的头文件引用使用了map文件夹下的map.h。这是为什么呢？稍后解释。

#include "amcl_sensor.h"
#include "../map/map.h"

观测模型对应的测距仪是激光传感器，我们需要设置激光模型的相关参数：

// Laser model params
  //
  // Mixture params for the components of the model; must sum to 1
  private: double z_hit;//具有局部测量噪声的正常范围，对应高斯分布模型，满足正态分布
  private: double z_short;//意外对象引起，对应指数分布
  private: double z_max;//检测失败时，会返回最大值，满足点群分布
  private: double z_rand;//随机测量，即产生了无法解释的测量，满足分布在完整传感器测量范围的均匀分布
  //
  // Stddev of Gaussian model for laser hits. 高斯模型 
  private: double sigma_hit;
  // Decay rate of exponential model for short readings. 指数模型
  private: double lambda_short;
  // Threshold for outlier rejection (unused)
  private: double chi_outlier;

关于测距仪的传感器模型的组成，见图3所示，参考《概率机器人》第6章 机器人感知。

图3 测距仪传感器模型的组成：注意这里c)为点群分布，不是均匀分布！

选择观测模型，观测模型有好几种，有波束模型，似然域模型， 极大似然域模型。

typedef enum
{
  LASER_MODEL_BEAM,
  LASER_MODEL_LIKELIHOOD_FIELD,
  LASER_MODEL_LIKELIHOOD_FIELD_PROB
} laser_model_t;

使用AMCLLaserData类定义观测模型的数据结构，其中包含的基本成员是激光的测量值，最大测量距离，激光发出的射线数目。

// Laser sensor data
class AMCLLaserData : public AMCLSensorData
{
  public:
    AMCLLaserData () {ranges=NULL;};
    virtual ~AMCLLaserData() {delete [] ranges;};
  // Laser range data (range, bearing tuples)
  public: int range_count;
  public: double range_max;
  public: double (*ranges)[2];
};

接下来使用AMCLLaser类定义观测模型，先是定义了AMCLLaser的默认构造函数，它的输入为最大的激光射线发出的数目以及map_t格式的地图。

// Default constructor
AMCLLaser::AMCLLaser(size_t max_beams, map_t* map) : AMCLSensor(), 
						     max_samples(0), max_obs(0), 
						     temp_obs(NULL)
{
  this->time = 0.0;
  this->max_beams = max_beams;
  this->map = map;

  return;
}

值的注意的是 AMCLLaser类里包含了map_t格式的地图以及用于与里程计运动模型产生的位姿做转换的laser_pose。

  private: laser_model_t model_type;

  // Current data timestamp
  private: double time;

  // The laser map 注意 AMCLLaser类包含map,且是私有成员变量
  private: map_t *map;

  //注意 AMCLLaser类包含laser_pose,且是私有成员变量；用于与里程计模型产生的位姿做转换
  // Laser offset relative to robot 
  private: pf_vector_t laser_pose;
  
  // Max beams to consider
  private: int max_beams;

在AMCLLaser类里，我们可以设置不同的观测模型的参数，这里选择似然域模型并设置它的相关参数。注意这里还使用了map_update_cspace函数，更新cspace_distance.

void 
AMCLLaser::SetModelLikelihoodField(double z_hit,
                                   double z_rand,
                                   double sigma_hit,
                                   double max_occ_dist)
{
  this->model_type = LASER_MODEL_LIKELIHOOD_FIELD;
  this->z_hit = z_hit;
  this->z_rand = z_rand;
  this->sigma_hit = sigma_hit;
  map_update_cspace(this->map, max_occ_dist);//这里导入地图，更新cspace distance
}

我们需要根据观测模型的数据来更新滤波器，因此定义UpdateSensor函数设置激光观测模型。

// Apply the laser sensor model设置激光观测模型
bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data) //输入为粒子滤波器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;
}

使用SetLaserPose函数设置激光的位姿，使用选定的观测模型（这里选用LikelihoodFieldModel）来计算给定位姿的权重。

//似然域模型计算给定位姿的权重，其输入为粒子sample集set以及AMCLLaserData类型的的data
// Determine the probability for the given pose
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;
  //创建粒子sample
  pf_sample_t *sample;
  //创建粒子sample的位姿
  pf_vector_t pose;
  pf_vector_t hit;

  self = (AMCLLaser*) data->sensor;

  total_weight = 0.0;

  //遍历传入的粒子sample集
  // Compute the sample weights
  for (j = 0; j < set->sample_count; j++)
  { 
    //这里接收到粒子sample的每个sample对象
    sample = set->samples + j;
    //存储接收的每个sample对象的位姿
    pose = sample->pose;
    
    //考虑激光的位姿与机器人的相对关系
    // Take account of the laser pose relative to the robot
    //从局部转换到全局;laser_pose(a)与里程计运动模型得出的位姿(b)的对应转换关系,最后得到位姿(c)
    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;

    step = (data->range_count - 1) / (self->max_beams - 1);

    // Step size must be at least 1
    if(step < 1)
      step = 1;
    //遍历激光ranges范围内存储的所有距离值
    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.转到map坐标系下的i,j
      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
       //将map坐标系下的i,j转换成map坐标系下的index
        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的权重
    sample->weight *= p;
    total_weight += sample->weight;
  }
  //返回权重
  return(total_weight);
}

关于似然域模型，参考《概率机器人》第6章 机器人感知，算法如图4所示。

图4.利用与最近物体的欧式距离计算测距仪似然的算法

另外比较重要的rellocTempData函数，其根据粒子sample数目的最大值和max_obs进行重新分配。

//新的样本，新的max_obs
 void AMCLLaser::reallocTempData(int new_max_samples, int new_max_obs){ 
  if(temp_obs){
    for(int k=0; k < max_samples; k++){
      delete [] temp_obs[k];
    }
    delete []temp_obs; 
  }
  max_obs = new_max_obs; 
  max_samples = fmax(max_samples, new_max_samples); 

  temp_obs = new double*[max_samples]();
  for(int k=0; k < max_samples; k++){
    temp_obs[k] = new double[max_obs]();
  }

4.总结

从上面的介绍来看，里程计的运动模型对传入的粒子sample集的每个sample的位姿进行更新，激光传感器的观测模型对传入的粒子sample集的每个sample的位姿计算出权重。

因为是观测模型，所以需要知道机器人所在的位置，精确到栅格地图（栅格单元），才能计算出距离障碍物的距离。因此AMCLLaser类里定义了map_t格式的map，调用了map_update_cspace函数来更新cspace distance以及定义了激光测距仪的末端的位姿laser_pose。

使用pf_vector_coord_add 函数 将激光的位姿laser_pose转换到里程计运动模型产生的粒子sample的位姿所在的坐标系。这里就有运动模型与观测模型的关联了。

  /*来自amcl_laser.cpp

  */
// Take account of the laser pose relative to the robot
//从局部转换到全局;laser_pose(a)与这是里程计位姿(b)的对应转换关系,最后得到位姿(c)
pose = pf_vector_coord_add(self->laser_pose, pose);

关于pf_vector_coord_add函数：

/*来自pf_vector.cpp*/
// Transform from local to global coords (a + b)
pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b)
{
  pf_vector_t c;

  c.v[0] = b.v[0] + a.v[0] * cos(b.v[2]) - a.v[1] * sin(b.v[2]);
  c.v[1] = b.v[1] + a.v[0] * sin(b.v[2]) + a.v[1] * cos(b.v[2]);
  c.v[2] = b.v[2] + a.v[2];
  c.v[2] = atan2(sin(c.v[2]), cos(c.v[2]));
  
  return c;
}

还有，从里程计运动模型的采样算法来看，存在一个旧的位姿，这个旧的位姿如何产生呢？

// Apply the action model 设置运动模型 
bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data) //更新运动，其输入为粒子滤波器pf和AMCLSensorData提供的data
{
  AMCLOdomData *ndata;//在此使用AMCLOdomData类 创建一个里程计数据模型 ndata ：只有pose ,delta
  ndata = (AMCLOdomData*) data;//实例化？来源于传入的AMCLSensorData的AMCLOdomData

  // Compute the new sample poses 创建新的sample集，其实是用于接收传入的pf里的粒子sample集
  pf_sample_set_t *set;
  set = pf->sets + pf->current_set;
  //这个是旧的位姿，使用简单的vector加减
  pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);

 

转自：4.AMCL包源码分析 | 传感器模型与sensor文件夹 - 知乎 (zhihu.com)