AMCL源码解析

Posted 2023-02-27 等待破茧

转载自：

AMCL源码解析_首飞爱玩机器人的博客-CSDN博客_amcl

AMCL是ros导航中的一个定位功能包。其实现了机器人在2D平面中基于概率方法的定位系统。该方法使用粒子滤波器来针对已知地图跟踪机器人的位姿。
MCL与AMCL的区别

它们最重要的区别应该是重采用过程。AMCL在采样过程中仍然会随机的增加小数量的粒子。这一步骤正式为了解决MCL不能处理的重定位问题。当粒子逐渐聚集，其它地方的粒子将慢慢消失。对于MCL来说，如果此时将机器人搬动到另一个地方。此时原来正确的粒子全都不正确了，而其它地方又没有粒子。因为没有粒子可以近似描述机器人的真实位置，重定位将会失败。而AMCL会保留一部分随机粒子，所以仍然会有粒子慢慢近似机器人的真实位置。关于MCL和AMCL的重要区别下面一篇文章有详尽的描述。
MCL和AMCL的区别
AMCL功能包文件结构

在这里插入图片描述
amcl工程中包括了三个模块和汇总所有模块的节点文件amcl文件。其中map模块维护了一个地图，sensor模块实现了运动模块和观测模型（sample_motion_model_odometry, beam_range_finder_model, likelihood_field_range_finder_model），pf模块实现了粒子滤波器。amcl_node.cpp文件聚合了所有模块的功能，实现了整个AMCL算法流程。下面从amcl_node.cpp开始分析。
AMCL算法流程

amcl算法流程
ps：该图片来自https://blog.csdn.net/weixin_40114832/article/details/78322034
上面为整个AMCL算法流程，其对应于amcl_node.cpp中的laserReceived函数。该函数的执行流程如下：
在这里插入图片描述
下面提取几个主要的执行函数。
通过odom获取当前位置，以作为先验位置。

  // 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],   //获取odom给出的位置
                  laser_scan->header.stamp, base_frame_id_))
 
    ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
    return;
 

   

更新所有粒子位置

    // Use the action data to update the filter
    odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);  //用运动模型更新粒子位置，得到当前时刻的先验位置

  

根据激光数据更新粒子位置权重

lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);//利用激光的数据更新粒子位置的权重

重采样过程

    // Resample the particles
    if(!(++resample_count_ % resample_interval_))//默认间隔两次激光数据重采样一次
   
      pf_update_resample(pf_);//按照一定的规则重采样，包括失效恢复、粒子权重等，然后放到kdtree中，暂时理解为关于位置的二叉树，
      resampled = true;        //然后进行聚类，得到均值和方差等信息，个人理解就是将相近的一堆粒子融合成一个平均值位置
   

发布新采样的粒子组

particlecloud_pub_.publish(cloud_msg);//在全局坐标系下，发布新采样的粒子

   

发布amcl得出的机器人后验位置。到此完成了整个AMCL算法流程

  if(resampled || force_publication)
 
    // Read out the current hypotheses
    double max_weight = 0.0;
    int max_weight_hyp = -1;
    std::vector<amcl_hyp_t> hyps;
    hyps.resize(pf_->sets[pf_->current_set].cluster_count);
    for(int hyp_count = 0;
        hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)//遍历所有的粒子簇，找到权重最大的簇，其平均位置就是机器人的后验位置。到此完成一次定位。
   
      double weight;
      pf_vector_t pose_mean;
      pf_matrix_t pose_cov;
      if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))//这里得到了amcl预测的权重最大的位置
     
        ROS_ERROR("Couldn't get stats on cluster %d", hyp_count);
        break;
     

      hyps[hyp_count].weight = weight;
      hyps[hyp_count].pf_pose_mean = pose_mean;
      hyps[hyp_count].pf_pose_cov = pose_cov;

      if(hyps[hyp_count].weight > max_weight)
     
        max_weight = hyps[hyp_count].weight;
        max_weight_hyp = hyp_count;
     
   

    if(max_weight > 0.0)
   
      ROS_DEBUG("Max weight pose: %.3f %.3f %.3f",
                hyps[max_weight_hyp].pf_pose_mean.v[0],
                hyps[max_weight_hyp].pf_pose_mean.v[1],
                hyps[max_weight_hyp].pf_pose_mean.v[2]);

      /*
         puts("");
         pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f");
         puts("");
       */

      geometry_msgs::PoseWithCovarianceStamped p;
      // Fill in the header
      p.header.frame_id = global_frame_id_;
      p.header.stamp = laser_scan->header.stamp;
      // Copy in the pose
      p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
      p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
      tf::quaternionTFToMsg(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                            p.pose.pose.orientation);
      // Copy in the covariance, converting from 3-D to 6-D
      pf_sample_set_t* set = pf_->sets + pf_->current_set;
      for(int i=0; i<2; i++)
     
        for(int j=0; j<2; j++)
       
          // Report the overall filter covariance, rather than the
          // covariance for the highest-weight cluster
          //p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
          p.pose.covariance[6*i+j] = set->cov.m[i][j];
       
     
      // Report the overall filter covariance, rather than the
      // covariance for the highest-weight cluster
      //p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
      p.pose.covariance[6*5+5] = set->cov.m[2][2];

      /*
         printf("cov:\\n");
         for(int i=0; i<6; i++)
        
         for(int j=0; j<6; j++)
         printf("%6.3f ", p.covariance[6*i+j]);
         puts("");
        
       */

      pose_pub_.publish(p);  //发布了amcl预测的位置
      last_published_pose = p;

   

amcl_node中的其它主要回调函数

初始位置回调函数

initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);

   

当在Rviz中点击2D Pose Estimae按钮就会向“initialpose”话题发布一个初始位置信息。如下所示：
在这里插入图片描述
检测激光数据是否超时的函数

void
AmclNode::checkLaserReceived(const ros::TimerEvent& event)

  ros::Duration d = ros::Time::now() - last_laser_received_ts_;
  if(d > laser_check_interval_)
 
    ROS_WARN("No laser scan received (and thus no pose updates have been published) for %f seconds.  Verify that data is being published on the %s topic.",
             d.toSec(),
             ros::names::resolve(scan_topic_).c_str());
 

它由一个15秒的定时器来调用

  // 15s timer to warn on lack of receipt of laser scans, #5209
  laser_check_interval_ = ros::Duration(15.0);
  check_laser_timer_ = nh_.createTimer(laser_check_interval_,
                                       boost::bind(&AmclNode::checkLaserReceived, this, _1));

 

地图数据获取的两种方式
通过服务请求来获取

AmclNode::requestMap()

 

通过话题来接收地图

AmclNode::mapReceived(const nav_msgs::OccupancyGridConstPtr& msg)

 

其中use_map_topic_参数用来决定使用哪种方式获取地图。first_map_only_参数用于确定是否只使用第一次接收到的地图。
动态参数配置函数

void AmclNode::reconfigureCB(AMCLConfig &config, uint32_t level)

 

传感器模型

该模块中包含基于laser的观测模型和基于odom的运动模型。amcl_sensor.cpp中实现了虚基类，主要用于统一函数接口。amcl_odom.cpp中实现了三轮全向轮和两轮差速的运动模型。这里主要分析两轮差速的运动模块。具体模型算法参考《ProbabilisticRobotics_2006》的136页。
odometry模型

在这里插入图片描述
函数bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)里的ODOM_MODEL_DIFF部分实现了上面的算法。
该模型的基本思想是一个位移分解成一次旋转，平移和第二次旋转。如下图所示：
在这里插入图片描述
代码实现如下（这里还考虑了原地旋转的情况）：

    // 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)//如果原地旋转的情况，定义第一个旋转为0，认为所有旋转都是第二个旋转做的
      delta_rot1 = 0.0;
    else
      delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]), old_pose.v[2]);
    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);
    //函数angle_diff实现了得到两个角度的差值，并取夹角小的差值。

  

通过计算出δrot1, δtrans, δrot2就可以重构出这次位移，然后将这次位移更新到每个粒子上。但在应用到粒子上前算法为每一部分增加了高斯噪声。

      // Sample pose differences  //下面给三部分增加高斯噪声，其中alpha1～alpha4参数会影响噪声的分布
      delta_rot1_hat = angle_diff(delta_rot1,   //pf_ran_gaussian函数实现了一个标准的高斯分布函数，其接收sigma参数
                                  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));

   

最后将添加了噪声的位移应用到每个粒子上。

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

laser模型

amcl_laser.cpp文件中实现了 beam_range_finder_model和 likelihood_field_range_finder_model。这里主要分析likelihood_field_range_finder_model。《ProbabilisticRobotics_2006》的6.4节（p169）描述了该模型。下面这篇文章描述了三种观测模型。
概率机器人——观测模型
在这里插入图片描述
该算法的执行步骤如下：
1.基于机器人本地坐标系获取光束最远端的坐标点。将该坐标点转换到全局坐标系中。这里也需要考虑激光安装位置相对于机器人坐标系的位置。处理的光束数量由max_beams参数确定。这里完成了第5行和第6行的内容。

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

  

2.计算最远端激光点与周围最近障碍物之间的距离。该步骤是通过查表完成的。因为在接收到地图后就已经预先计算了每个栅格到最近障碍物的距离。这里完成了第7行的内容。

      // 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;  //查表获取到最近障碍物的距离

 

3.将距离值带入均值为0的标准高斯分布函数获取概率密度值。上面标亮的的参数是激光传感器固有的模型参数，需要预先进行测量得到。其中Zmax是激光标称的最大测量距离值。这里将每束光得到的概率密度值相乘得到最终的粒子概率密度值，后面进行归一化得到每个粒子的位置概率。这里完成了第8行的内容。

      // 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; //加上测量误差

 

粒子滤波器

该模块中有5个有效的文件。pf_vector.c中实现pose向量的加减等操作。eig3.c中实现了对称矩阵的特征值分解操作。pf_pdf.c中实现了高斯分布函数和基于高斯采样生成一个坐标。
kdtree

pf_kdtree.c维护一个kdtree，它将pose跟其权重保存在一个kdtree中。该文件主要实现了节点插入和查找功能，为实现粒子聚类打下了基础。关于kdtree基础知识的描述参考下面三篇文章：
K Dimensional Tree | Set 1 (Search and Insert)
K Dimensional Tree | Set 2 (Find Minimum)
K Dimensional Tree | Set 3 (Delete)
关于amcl功能包中是如何实现kdtree的，下面这篇文章有详尽的解释。
ROS导航包源码学习2 — 定位
pf

pf.c文件中主要实现了粒子的重采样（resample）和根据重采样得到的粒子集统计后验位置。下面分析一下重采样过程。在《ProbabilisticRobotics_2006》的259页描述了该重采样过程的流程。
1.计算所有粒子的权重平均值。

    // 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 (Prob Rob p258) 259页描述了重定位的过程
    w_avg /= set->sample_count;

   

2.根据权重均值和两个衰减系数计算生成随机粒子的可能性。

    if(pf->w_slow == 0.0)
      pf->w_slow = w_avg;
    else
      pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow);//默认alpha_slow=0.001
    if(pf->w_fast == 0.0)
      pf->w_fast = w_avg;
    else
      pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast);//默认alpha_fast=0.1

  

下面的w_diff即表征生成随机粒子的可能性。其值范围为0~1。

  w_diff = 1.0 - pf->w_fast / pf->w_slow;//当w_fast减小的速率比w_slow更快时，增加的随机粒子数会增加。

   

下面即是生成随机粒子的语句

    if(drand48() < w_diff)//drand48()会生成一个0～1之间的实数。这里用来判断是生成随机粒子还是从之前的粒子集中选出粒子。
      sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);//用高斯分布生成一个随机粒子

 

3.如果不生成随机粒子，将会从之前的粒子集中选出粒子。

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

   

4.将新采样的粒子插入到kdtree中，为后面统计粒子集群信息作准备

    // Add sample to histogram
    pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);//将新采样的粒子插入到kdtree中，为后面统计粒子集群信息作准备

 

5.自适应调整粒子集大小。因为粒子越多就需要更多的计算量。而当粒子收敛了后，只需要少量的粒子就可以追踪机器人的位置。所以这里需要根据机器人定位的置信程度调整粒子集的大小。如果粒子比较分散，就需要增加更多粒子来定位机器人位置。这里使用了《ProbabilisticRobotics_2006》284页描述的KLD-sampling algorithm 。

    // See if we have enough samples yet      //KLD-sampling algorithm p284
    if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))   //这里自适应调整粒子的数量。叶子数量越大表示粒子分得越散，越少表示越聚拢
      break;                                                                      //粒子插入到kdtree的时候，如果要插入的粒子已经存在就只增加粒子的权重

  

6.最后将采样后的粒子集进行聚类并计算每个类的粒子权重和与粒子均值位置。比较每个类的权重大小，选出权重最大的粒子聚类，然后取出其粒子均值状态值（即全局坐标）。这样就得到了AMCL的后验状态值。该步骤的内容主要在pf_cluster_stats函数中执行。

ps：下面是MCL的示例程序，可作为参考。
MCL的示例程序
Map

Map模块中需要特别注意map_cspace.cpp文件中的map_update_cspace函数。它实现了接收地图后计算每个cell离最近的障碍物的距离，即每个cell结构体中max_occ_dist变量存储的值。这个预处理会生成一张距离表。在前面计算粒子权重时使用的LikelihoodFieldModel就使用了这张表。

      // 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;  //查表获取到最近障碍物的距离

   

参考网址：
http://wiki.ros.org/amcl
amcl源码解析
AMCL定位

原文链接：https://blog.csdn.net/shoufei403/article/details/102454304