沉默杀手
源码分析(五)测量模型
|激光SLAMamcl和粒子滤波|
Word count: 2.1k|Reading time: 9 min

测量模型主要用来做scan-match。

光束模型比较古老，实际已经不怎么使用，它的缺陷比较明显：在杂乱的环境(非结构化环境clutter)中，输入的小变化会导致输出的大变化。amcl中实现了这个模型，但是可以选择用还是不用

似然场模型不仅适应于结构化和非结构化的环境，而且在任何环境中的期望值对位姿都是平滑的，还可以通过查表的得到栅格的似然度。

工具函数和类

1
2
3
4
5
6
7
8
9
10
11
// 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;
}

laserReceived 中的准备工作

这一段是紧接着laserReceived中运动模型的准备工作

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
// 刚启动amcl时不执行,机器人移动后执行, update the filter
if(lasers_update_[laser_index])   // 在运动模型的准备工作里赋值true
{
    AMCLLaserData ldata;
    // 成员sensor是指向传感器的指针
    ldata.sensor = lasers_[laser_index];
    // 动态数组ranges的大小，可以用 rostopic echo --noarr /scan 获得
    ldata.range_count = laser_scan->ranges.size();

    
    // 雷达在base_link坐标系的最小和最大偏角
    tf::Quaternion q;
    // 获得雷达的扫描起始角度，例如思岚A2的是-3.124，带时间戳和坐标系名称
    q.setRPY(0.0, 0.0, laser_scan->angle_min);
    tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp,
                                      laser_scan->header.frame_id);
    // angle_increment为角度分辨率，例如思岚A2的是0.017， inc_q: 起始角度+角度分辨率
    q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment为);
    tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp,
                                      laser_scan->header.frame_id);
    try
    {      // 获得两个角度在base_link坐标系中的Quaternion
      tf_->transformQuaternion(base_frame_id_, min_q, min_q);
      tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
    }
    catch(tf::TransformException& e)
    {
      ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
               e.what());
      return;
    }
    // 现在是base_link中的起始yaw和偏航角分辨率，单位rad
    double angle_min = tf::getYaw(min_q);
    double angle_increment = tf::getYaw(inc_q) - angle_min;

    // 把角度映射到[-pi ~ pi]的范围
    angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;
    ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment);


    // Apply range min/max thresholds, if the user supplied them
    // 来自参数 laser_max_range 和 laser_min_range， 准备后面使用
    if(laser_max_range_ > 0.0)
        ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
    else
        ldata.range_max = laser_scan->range_max;
    double range_min;
    if(laser_min_range_ > 0.0)
        range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
    else
        range_min = laser_scan->range_min;


    // 二维数组，AMCLLaserData 的析构函数会释放这段内存
    // 每行第1个元素是激光射程，第2个是相对机器人的夹角
    ldata.ranges = new double[ldata.range_count][2];
    ROS_ASSERT(ldata.ranges);
    for(int i=0;i<ldata.range_count;i++)
    {
      // amcl还没有最小射程的概念，如果range的值不正常，小于最小值，则映射为最大值 
      // 雷达数据的赋值
      if(laser_scan->ranges[i] <= range_min)
          ldata.ranges[i][0] = ldata.range_max;
      else   // 射程的正常赋值，来自话题scan
          ldata.ranges[i][0] = laser_scan->ranges[i];
          // Compute bearing 夹角的赋值
          ldata.ranges[i][1] = angle_min + (i * angle_increment);
    }

    // 核心函数， 更新传感器数据
    lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);
    lasers_update_[laser_index] = false;  // 完成更新传感器数据，标志位变为false

    pf_odom_pose_ = pose;

}


UpdateSensor函数的调用过程：

1
2
3
bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)
// pf_sensor_model_fn_t是函数指针的类型
pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data);

涉及两个函数： LikelihoodFieldModel 似然域模型，pf_update_sensor 粒子更新

LikelihoodFieldModel 函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
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;

    // 传感器局部坐标经过三角变换映射到全局坐标系下，与激光雷达数据匹配
    pose = pf_vector_coord_add(self->laser_pose, pose);
    p = 1.0;

    // 预计算似然域，离散栅格化，《P.R》Page 130, 其实是对sigma和Zmax的赋值
    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);
    if(step < 1)  // 步长至少为1
      step = 1;

  	// 利用与最近物体的欧氏距离计算激光模型似然，对所有特征（激光点）进行遍历
    for (i = 0; i < data->range_count; i += step)
    {
      // 距离和角度的赋值，根源在上面的准备工作里
      obs_range = data->ranges[i][0];  
      obs_bearing = data->ranges[i][1];

      //忽略极大的range，rostopic echo可看到有很多range是inf
      if(obs_range >= data->range_max)
        continue;
      // Check for NaN
      if(obs_range != obs_range)
        continue;

      pz = 0.0;
      //  激光点通过两次转化到全局坐标系，  map  base_link  lase
      // 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);

      // 将激光数据在栅格地图中进行离散化, 获得激光点在栅格地图的坐标
      // 我理解的查表就是这里
      int mi, mj;
      mi = MAP_GXWX(self->map, hit.v[0]);
      // 计算函数在map_cspace.cpp的 map_update_cspace 中实现遍历计算
      // 该函数是被AMCLLaser::SetModelLikelihoodField调用
      mj = MAP_GYWY(self->map, hit.v[1]);  //距离和角度格栅
      
      // 找到距离激光点最近的障碍，距离值是提前计算好的
      if(!MAP_VALID(self->map, mi, mj))
        z = self->map->max_occ_dist;   //不在地图范围内的当成最大距离
      else
        // 这个就是我们需要的最近距离，相当于《P.R》130页的dist
        z = self->map->cells[MAP_INDEX(self->map, mi, mj)].occ_dist;


      // 高斯模型，130页的算法第8行
      // 计算Zt似然域的算法，在障碍物点周围用正态分布和均匀分布计算似然域，是关于dist的函数
      pz += self->z_hit * exp(-(z * z) / z_hit_denom);  //高斯测量
      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
      // 注意： 实际使用的公式和书上的不同
      p += pz*pz*pz;  	//将每个激光点的似然域累加起来（多大范围）
    }
    // sample->weight 在之前的pf_alloc和pf_init里赋值为 最大粒子数的倒数
    sample->weight *= p;    // 概率等于权重
    total_weight += sample->weight;
  }
  return(total_weight);
}

大致的思想是这样:

不解的地方：

测量模型的最后使用的公式是p += pz*pz*pz，并不是书上的p *= pz，怀疑3个pz对应 [x,y,θ]

sample->weight在之前赋值为 1/max_particles 

1
2
3
// 将激光点的世界坐标转为栅格地图的坐标
#define MAP_GXWX(map, x) (floor( (x - map->origin_x) / map->scale + 0.5) + map->size_x / 2)
#define MAP_GYWY(map, y) (floor( (y - map->origin_y) / map->scale + 0.5) + map->size_y / 2)

公式 step = (laser_range_count - 1) / (max_beams - 1);：
因为时间问题，不对所有点进行似然计算，而是间隔选点，step就是步长.

lser_rang_count可以用rostopic echo --noarr /scan得到,max_beams由自己设置. 如果step小于1,程序里会设置为1.

因为寻找最近障碍物是最耗时的，可以将似然域预先计算好，搜索时直接查表即可。我理解的查表是查激光点在查栅格地图的坐标。

似然域模型的最后重新确定了每个粒子的重要性权重sample->weight

pf_update_sensor 函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
// 根据观测值更新滤波器
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;

  // 粒子总权重
  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;
		}
		// Update running averages of likelihood of samples
		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;
}

代码比较简单，就是196页的AMCL算法的7~11行。 不解的地方是 sample->weight最后又除以了粒子总权重