扩展蒙特卡洛定位（AMCL）算法的实现-ROS AMCL包代码阅读笔记

Peiy_Liu

已于 2023-06-28 10:25:36 修改

阅读量2.2k

点赞数 19

分类专栏：机器人/ROS 文章标签：算法笔记

于 2023-06-27 21:12:15 首次发布

本文链接：https://blog.youkuaiyun.com/lpy13699855019/article/details/131425410

版权

机器人/ROS 专栏收录该内容

3 篇文章

订阅专栏

文章详细介绍了ROSAMCL包的实现原理，包括AMCL算法如何解决“绑架”问题，如何通过维护短期和长期平均测量概率来判断并应对定位性能变化。此外，文章还剖析了ROSAMCL包的源代码结构，包括map、pf、sensors模块，以及它们在AMCL算法中的作用，如地图数据结构、粒子滤波更新和传感器模型的实现。最后，文章提到了AMCL节点在实际运行中的表现，展示了算法的有效性。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

AMCL算法的实现-ROS AMCL包代码阅读笔记

AMCL算法原理

AMCL（Augmented Monte Carlo Localization）是扩展版的蒙特卡洛定位（MCL）算法，AMCL算法的伪代码如下。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Wc8Zu4sQ- 在这里插入图片描述

AMCL算法的提出主要是为了解决传统MCL算法的“绑架”问题，解决思路是在每次迭代时启发式地插入随机采样点。

监控传感器的测量概率
$p(z_t | z_{1:t-1}, u_{1:t}, m)$
并将其与平均测量概率联系在一起。在粒子滤波中，用粒子的权重平均值来近似平均测量概率，即
$w_{\text{avg}} = \frac{1}{M} \sum_{m=1}^M w_i^{[m]} \approx p(z_t | z_{1:t-1}, u_{1:t}, m)$

每次迭代时插入多少个随机点？ AMCL维护了一个短期平均测量概率 $w_{\text{fast}}$ 和一个长期平均测量概率 $w_{\text{slow}}$ ，用来在重采样时评估长期和短期内定位性能的好坏，当 $w_\text{fast}$ 很低时，说明短期内定位效果不好，可能发生了绑架，因此就插入随机点来应对这种情况，否则就按MCL里的方法正常重采样。具体地，插入随机点时的采样概率（ $M$ 个重采样粒子为插入的随机点的概率）为
$\max \{ 0.0, 1.0 - \frac{w_\text{fast}}{w_\text{slow}} \}$
以什么概率分布插入随机点？ 可以在地图的可行空间中均匀采样（AMCL的实现方式就是如此），也可以根据观测模型决定采样的分布。

ROS中AMCL包代码分析

ROS的AMCL包属于ROS导航组建navigation的一部分，实现了AMCL算法并提供了利用AMCL进行机器人定位的节点。AMCL包的src目录结构如下

├── src
│   ├── amcl
│   │   ├── map
│   │   │   ├── map.c
│   │   │   ├── map_cspace.cpp
│   │   │   ├── map_draw.c
│   │   │   ├── map_range.c
│   │   │   └── map_store.c
│   │   ├── pf
│   │   │   ├── eig3.c
│   │   │   ├── pf_draw.c
│   │   │   ├── pf_kdtree.c
│   │   │   ├── pf_pdf.c
│   │   │   ├── pf_vector.c
│   │   │   └── pf.c
│   │   └── sensors
│   │       ├── amcl_laser.cpp
│   │       ├── amcl_odom.cpp
│   │       └── amcl_sensor.cpp
│   ├── amcl_node.cpp
│   └── include
│       └── portable_utils.hpp

可见源代码分为map、pf、sensors（对应的头文件目录include的结构与之对应）和include/protable_utils.hpp四个模块以及amcl_node.cpp节点代码。

map模块定义了栅格地图相关的数据结构和功能，包括从图像读入栅格地图或保存地图为图像、按坐标或索引查询地图点的状态等。
pf模块包括AMCL粒子滤波算法的具体实现以及相关的所用到的数据结构，完全由C语言写成。
sensors模块实现了观测模型和运动模型
include/portable_utils.hpp为没有提供srand48()和drand48()接口的操作系统（如Windows）定义了对应接口。

`map`模块

map模块定义了栅格地图相关的数据结构和功能，包括从图像读入栅格地图或保存地图为图像、按坐标或索引查询地图点的状态等。

在include\amcl\map\map.h中定义了代表单个地图栅格（cell）的结构体map_cell_t和代表整张栅格地图的结构体map_t，对应代码如下

// Description for a single map cell.
typedef struct
{
  // Occupancy state (-1 = free, 0 = unknown, +1 = occ)
  int occ_state;

  // Distance to the nearest occupied cell
  double occ_dist;

  // Wifi levels
  //int wifi_levels[MAP_WIFI_MAX_LEVELS];

} map_cell_t;


// Description for a map
typedef struct
{
  // Map origin; the map is a viewport onto a conceptual larger map.
  double origin_x, origin_y;
  
  // Map scale (m/cell)
  double scale;

  // Map dimensions (number of cells)
  int size_x, size_y;
  
  // The map data, stored as a grid
  map_cell_t *cells;

  // Max distance at which we care about obstacles, for constructing
  // likelihood field
  double max_occ_dist;
  
} map_t;

每个cell存储了占据状态（共三种状态：-1代表不占据，0代表未知，1代表占据）和该cell到最近的占据cell的距离。地图则存储了原点、尺度、尺寸、cell数组和最大占据距离。

该模块还提供了一些对地图进行操作的接口，如根据指定点坐标获取对应cell的函数：

// Get the cell at the given point
map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa);

还有比较重要的世界坐标系和地图坐标系之间的转换操作：

// Convert from map index to world coords
#define MAP_WXGX(map, i) (map->origin_x + ((i) - map->size_x / 2) * map->scale)
#define MAP_WYGY(map, j) (map->origin_y + ((j) - map->size_y / 2) * map->scale)

// Convert from world coords to map coords
#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)

世界坐标系的原点对应于图像的中心，以m为单位；地图坐标系原点则对应于图像的定点，以cell为单位。

`sensor`模块

sensor模块定义了MCL算法中用到了传感器模型和对应的数据结构，二者分别继承自amcl_sensor.h中定义的空基类AMCLSensor和AMCLSensorData。

所有继承自AMCLSensor的传感器模型需要实现公共接口：

virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);

即在每一次迭代时如何更新滤波器的状态：对应于运动模型就是根据控制输入更新下一时刻的粒子集；对应于观测模型就是根据观测数据更新下一时刻粒子集的权重。

运动模型AMCLOdom：该类实现了里程计模型，该模型支持四种不同的机器人运动模型，由如下枚举类型定义：
```
typedef enum
{
  ODOM_MODEL_DIFF, // 差分模型
  ODOM_MODEL_OMNI, // 全向模型
  ODOM_MODEL_DIFF_CORRECTED,
  ODOM_MODEL_OMNI_CORRECTED
} odom_model_t;
```
该类用作AMCL算法中的运动模型。

观测模型AMCLLaser：该类实现了激光雷达模型，也由如下枚举类型定义了可选择的三种激光雷达扫描模型：

typedef enum
{
  LASER_MODEL_BEAM, // 波束模型
  LASER_MODEL_LIKELIHOOD_FIELD, // 似然域模型
  LASER_MODEL_LIKELIHOOD_FIELD_PROB // 极大似然域模型
} laser_model_t;

该类用作AMCL算法中的观测模型。

`pf`模块

pf模块是具体实现AMCL定位算法的模块，该模块完全由C语言实现。其主要部分是pf_t结构体、pf_update_sensor和pf_update_resample函数。

pf_t结构体定义可以在include/amcl/pf/pf.h中找到，具体代码如下：

// Information for an entire filter
typedef struct _pf_t
{
  // This min and max number of samples
  int min_samples, max_samples;

  // Population size parameters
  double pop_err, pop_z;

  // Resample limit cache
  int *limit_cache;
  
  // The sample sets.  We keep two sets and use [current_set]
  // to identify the active set.
  int current_set;
  pf_sample_set_t sets[2];

  // Running averages, slow and fast, of likelihood
  double w_slow, w_fast;

  // Decay rates for running averages
  double alpha_slow, alpha_fast;

  // Function used to draw random pose samples
  pf_init_model_fn_t random_pose_fn;
  void *random_pose_data;

  double dist_threshold; //distance threshold in each axis over which the pf is considered to not be converged
  int converged; 

  // boolean parameter to enamble/diable selective resampling
  int selective_resampling;
} pf_t;

其中包括了AMCL算法的主要参数和维护的变量，如粒子集 $\bar{\mathcal{X}}_t$ 与 $\mathcal{X}_t$ 、 $w_\text{slow}$ 与 $w_\text{fast}$ 、 $\alpha_\text{slow}$ 与 $\alpha_\text{fast}$ 等。random_pose_fn是一个函数指针，用来实现添加一个随机点的功能，用户可以遵照其接口自定义随机采样方式，其类型pf_init_model_fn_t的定义如下

// Function prototype for the initialization model; generates a sample pose from
// an appropriate distribution.
typedef pf_vector_t (*pf_init_model_fn_t) (void *init_data);

pf_update_sensor函数实现了AMCL算法的更新粒子权重的操作，其代码如下

// Update the filter with some new sensor observation
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;

  // Compute the sample weights
  total = (*sensor_fn) (sensor_data, set); // 计算粒子权重

  set->n_effective = 0;
  
  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;
      set->n_effective += sample->weight*sample->weight;
    }
    // Update running averages of likelihood of samples (Prob Rob p258)
    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);
    //printf("w_avg: %e slow: %e fast: %e\n", 
           //w_avg, pf->w_slow, 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;
    }
  }

  set->n_effective = 1.0/set->n_effective;
  return;
}

该函数除了更新粒子权重外，还进行了伪代码第10和11行的更新 $w_\text{slow}$ 和 $w_\text{fast}$ 的操作。参数sensor_fn是函数指针，代表一个传感器模型的接口，其类型定义为：

// Function prototype for the sensor model; determines the probability
// for the given set of sample poses.
typedef double (*pf_sensor_model_fn_t) (void *sensor_data, 
                                        struct _pf_sample_set_t* set);

pf_update_resample函数实现了AMCL算法重要的重采样步骤，其代码如下

// Resample the distribution
void pf_update_resample(pf_t *pf)
{
  int i;
  double total;
  pf_sample_set_t *set_a, *set_b;
  pf_sample_t *sample_a, *sample_b;

  //double r,c,U;
  //int m;
  //double count_inv;
  double* c;

  double w_diff; // 随机点概率

  set_a = pf->sets + pf->current_set; // 先验Xt
  set_b = pf->sets + (pf->current_set + 1) % 2; // 重采样后的Xt

  if (pf->selective_resampling != 0)
  {
    if (set_a->n_effective > 0.5*(set_a->sample_count))
    {
      // copy set a to b
      copy_set(set_a,set_b);

      // Re-compute cluster statistics
      pf_cluster_stats(pf, set_b);

      // Use the newly created sample set
      pf->current_set = (pf->current_set + 1) % 2;
      return;
    }
  }

  // Build up cumulative probability table for resampling.
  // TODO: Replace this with a more efficient procedure
  // (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html)
  c = (double*)malloc(sizeof(double)*(set_a->sample_count+1)); // 权重的积分值
  c[0] = 0.0;
  for(i=0;i<set_a->sample_count;i++)
    c[i+1] = c[i]+set_a->samples[i].weight; 

  // Create the kd tree for adaptive sampling
  pf_kdtree_clear(set_b->kdtree);
  
  // Draw samples from set a to create set b.
  total = 0;
  set_b->sample_count = 0;

  // 计算重新采样概率
  w_diff = 1.0 - pf->w_fast / pf->w_slow; 
  if(w_diff < 0.0)
    w_diff = 0.0;
  
  while(set_b->sample_count < pf->max_samples)
  {
    sample_b = set_b->samples + set_b->sample_count++;

    if(drand48() < w_diff)
      sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data); // 添加一个随机点
    else
    {
      // Naive discrete event sampler 离散事件采样
      double r;
      r = drand48();
      for(i=0;i<set_a->sample_count;i++)
      {
        if((c[i] <= r) && (r < c[i+1]))
          break;
      }
      assert(i<set_a->sample_count);

      sample_a = set_a->samples + i;

      assert(sample_a->weight > 0);

      // Add sample to list
      sample_b->pose = sample_a->pose;
    }

    sample_b->weight = 1.0;
    total += sample_b->weight;

    // Add sample to histogram
    pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);

    // See if we have enough samples yet
    if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
      break;
  }
  
  // Reset averages, to avoid spiraling off into complete randomness.
  if(w_diff > 0.0)
    pf->w_slow = pf->w_fast = 0.0;

  //fprintf(stderr, "\n\n");

  // Normalize weights
  for (i = 0; i < set_b->sample_count; i++)
  {
    sample_b = set_b->samples + i;
    sample_b->weight /= total;
  }
  
  // Re-compute cluster statistics
  pf_cluster_stats(pf, set_b);

  // Use the newly created sample set
  pf->current_set = (pf->current_set + 1) % 2; 

  pf_update_converged(pf);

  free(c);
  return;
}

具体步骤与伪代码中基本相同，先计算了随机点概率，然后据此进行重采样。

AMCL定位流程

上述三个模块只是实现了AMCL算法的基本模块和基础工具，真正将算法跑起来用于定位还是在节点代码amcl_node.cpp中定义的AmclNode类中。

AmclNode节点需要接收的消息有：

地图：类型为nav_msgs::OccupancyGrid，话题名称默认为/map；
坐标系：类型为tf2_msgs::TFMessage，话题名称默认为/tf；
激光雷达数据：类型为sensor_msgs::LaserScan，话题名称默认为/scan。

AMCL算法的迭代过程在回调函数AmclNode::laserReceived中执行，每收到一个雷达扫描数据就执行一次，流程及对应代码如下：

获取里程计数据：

// Where was the robot when this scan was taken?
  pf_vector_t pose;
  if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
                  laser_scan->header.stamp, base_frame_id_))
  {
    ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
    return;
  }


  pf_vector_t delta = pf_vector_zero();

  if(pf_init_)
  {
    // Compute change in pose
    //delta = pf_vector_coord_sub(pose, pf_odom_pose_);
    delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
    delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
    delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);

    // See if we should update the filter
    bool update = fabs(delta.v[0]) > d_thresh_ ||
                  fabs(delta.v[1]) > d_thresh_ ||
                  fabs(delta.v[2]) > a_thresh_;
    update = update || m_force_update;
    m_force_update=false;

    // Set the laser update flags
    if(update)
      for(unsigned int i=0; i < lasers_update_.size(); i++)
        lasers_update_[i] = true;
  }

根据里程计数据按照运动模型更新滤波器中粒子集的位姿：

// If the robot has moved, update the filter
  else if(pf_init_ && lasers_update_[laser_index])
  {
    //printf("pose\n");
    //pf_vector_fprintf(pose, stdout, "%.3f");

    AMCLOdomData odata;
    odata.pose = pose;
    // HACK
    // Modify the delta in the action data so the filter gets
    // updated correctly
    odata.delta = delta;

    // Use the action data to update the filter
    odom_->UpdateAction(pf_, (AMCLSensorData*)&odata); // 更新步骤

    // Pose at last filter update
    //this->pf_odom_pose = pose;
  }

根据激光数据按照观测模型更新粒子权重：

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

该函数的具体定义如下：

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

即调用pf_update_sensor，根据所定的不同雷达模型传递不同的模型函数作为第二个参数。

重采样：

// Resample the particles
if(!(++resample_count_ % resample_interval_))
{
  pf_update_resample(pf_);
  resampled = true;
}

在AmclNode中，为滤波器定义的随机点采样函数random_pose_fn为AmclNode::uniformPoseGenerator，是在地图的free space中进行均匀采样，具体定义如下：

pf_vector_t
AmclNode::uniformPoseGenerator(void* arg)
{
  map_t* map = (map_t*)arg;
#if NEW_UNIFORM_SAMPLING
  unsigned int rand_index = drand48() * free_space_indices.size();
  std::pair<int,int> free_point = free_space_indices[rand_index];
  pf_vector_t p;
  p.v[0] = MAP_WXGX(map, free_point.first);
  p.v[1] = MAP_WYGY(map, free_point.second);
  p.v[2] = drand48() * 2 * M_PI - M_PI;
#else
  double min_x, max_x, min_y, max_y;

  min_x = (map->size_x * map->scale)/2.0 - map->origin_x;
  max_x = (map->size_x * map->scale)/2.0 + map->origin_x;
  min_y = (map->size_y * map->scale)/2.0 - map->origin_y;
  max_y = (map->size_y * map->scale)/2.0 + map->origin_y;

  pf_vector_t p;

  ROS_DEBUG("Generating new uniform sample");
  for(;;)
  {
    p.v[0] = min_x + drand48() * (max_x - min_x);
    p.v[1] = min_y + drand48() * (max_y - min_y);
    p.v[2] = drand48() * 2 * M_PI - M_PI;
    // Check that it's a free cell
    int i,j;
    i = MAP_GXWX(map, p.v[0]);
    j = MAP_GYWY(map, p.v[1]);
    if(MAP_VALID(map,i,j) && (map->cells[MAP_INDEX(map,i,j)].occ_state == -1))
      break;
  }
#endif
  return p;
}