AMCL介绍

AMCL介绍

AMCL( Adaptive Mentcarto Localization)，自适应蒙特卡洛定位，基于粒子滤波器的定位算法，与gmapping的定位方法类似，但主要实现定位功能。

MCL：蒙特卡洛定位介绍
MCL算法流程：

1. 随机生成粒子，粒子权重相等。更常用是位置初始化可以设为(0,0,0)。
2. 根据运动模型估计新时刻位姿
3. 根据观测模型及观测值，更新该时候粒子携带位姿的权重
4. 重采样，调整粒子分布，收紧粒子(去掉可信度低的粒子，增加新的)
5. 计算粒子权重的均值和方差，得到最佳位置估计。
6. 回到第2步，进行下一帧的扫描匹配。
   
   MCL问题：

• 权值退化：如果任由粒子权值增长，只有少数粒子的权值较大，其余粒子的权值可以忽略不计，变成无效粒子，因此需要引入重采样。需要衡量粒子权值的退化程度。
• 粒子多样性丧失：通常我们会舍弃权值较小的粒子，代之以权值较大的粒子。这样会导致权值小的粒子逐渐绝种，粒子群多样性减弱，从而不足以近似表征后验密度。从而出现正确粒子被去掉的可能性，而后出现粒子的奇异性丧失，总体表现就是粒子的权重均值偏低。

改进算法：AMCL
AMCL 结合自适应（Augmented_MCL）和库尔贝克-莱不勒散度采样（KLD_Sampling_MCL）算法

• Augmented_MCL：
  在机器人遭到绑架的时候，它会在发现粒子们的平均分数突然降低了，这意味着正确的粒子在某次迭代中被抛弃了，此时会随机的全局注入粒子（injection of random particles）。
  
  算法流程上看，augmented_MCL算法最显著的区别就是引入了四个参数用于失效恢复：
  $w_{slow}$：长期似然平均估计
  $w_{fast}$：短期似然平均估计
  ${\alpha }_{slow}$：长期指数滤波器衰减率
  ${\alpha }_{fast}$：短期指数滤波器衰减率
  $0<{\alpha }_{slow}<{\alpha }_{fast}$
  失效恢复的核心思想是：当正确粒子偶然被舍弃，通过重新随机注入粒子的方法，可以重新恢复正确的估计。
  实现思路：测量似然的一个突然衰减（短期似然劣于长期似然）象征着粒子质量的下降，这将引起随机采样数目的增加。
  $w_{avg}$粒子的平均权重，当粒子质量下降时，平均权重随之下降，$w_{slow}、w_{fast}$也会随之下降，但是显然$w_{fast}$下降的速度要快于$w_{slow}$——这由衰减率决定，因此随机概率$p=1-\frac{w_{fast}}{w_{slow}}$会增大，随机粒子数目增加。而当粒子质量提高时，粒子短期权重要好于长期，随机概率小于0，不生成随机粒子。

• KLD_Sampling_MCL：
  动态调整粒子数，当机器人定位精确时(粒子都集中在一块)，减少粒子数，当定位不精确时，增加粒子数。
  在栅格地图中，看粒子占了多少栅格。占得多，说明粒子很分散，在每次迭代重采样的时候，允许粒子数量的上限高一些。占得少，说明粒子都已经集中了，那就将上限设低。bin应该是指栅格直方。一个bin对应一个栅格，用来统计有多少粒子在这个栅格中。k则是粒子占有的网格数，根据k值调整粒子数。
  

代码结构解析

AMCL接口定义介绍链接

http://docs.ros.org/lunar/api/amcl/html/annotated.html

参考代码地址

https://github.com/ros-planning/navigation.git

代码解析：
amcl的文件结构：

主要实现文件是：amcl_node.cpp，包含整体的流程。

pf下文件是关于粒子滤波器的结构定义等，比如粒子的class结构，粒子的存储kdtree等
map下是关于地图结构的定义等，amcl是接收指定的map信息，来实现定位，接收到map message后存储及地图相关的处理在这个文件夹下。
sensor下则是 里程计的结构，雷达扫描数据结构等

以下为amcl_node.cpp 的main函数：

main主要任务：
node创建及初始化;
参数解析，判断是不是基于数据集bag文件的运行。
amcl的功能实现是在AmclNode()的初始化中。

int main(int argc, char** argv)
{
  ros::init(argc, argv, "amcl");
  ros::NodeHandle nh;
  // Override default sigint handler
  signal(SIGINT, sigintHandler);
  // Make our node available to sigintHandler
  amcl_node_ptr.reset(new AmclNode());
  if (argc == 1)
  {
    // run using ROS input
    ros::spin();
  }
  else if ((argc >= 3) && (std::string(argv[1]) == "--run-from-bag"))
  {
    if (argc == 3)
    {
      amcl_node_ptr->runFromBag(argv[2]);
    }
    else if ((argc == 4) && (std::string(argv[3]) == "--global-localization"))
    {
      amcl_node_ptr->runFromBag(argv[2], true);
    }
  }
  // Without this, our boost locks are not shut down nicely
  amcl_node_ptr.reset();
  // To quote Morgan, Hooray!
  return(0);
}

AmclNode()的初始化

AmclNode主要任务：
完成参数param的解析
发布pub topic：“tf”、 “amcl_pose”、“particlecloud”
订阅sub topic：“tf”、scan_topic_、“map”/“static map”
提供服务service：global_localization、request_nomotion_update、set_map

AmclNode::AmclNode() :
... ...
  private_nh_.param("laser_min_range", laser_min_range_, -1.0);
  private_nh_.param("laser_max_range", laser_max_range_, -1.0);
  private_nh_.param("laser_max_beams", max_beams_, 30);
  private_nh_.param("min_particles", min_particles_, 100);
  private_nh_.param("max_particles", max_particles_, 5000);
... ...
  base_frame_id_ = stripSlash(base_frame_id_);
  global_frame_id_ = stripSlash(global_frame_id_);
  //init the pose
  updatePoseFromServer();
  cloud_pub_interval.fromSec(1.0);
  //tfb is the tf Broadcaster
  tfb_.reset(new tf2_ros::TransformBroadcaster());
  // tf_ use to buffer the scan msg                                                                                          tf_.reset(new tf2_ros::Buffer());
  //tfl is the tf Listener                                                                                                   tfl_.reset(new tf2_ros::TransformListener(*tf_));
  //publish the topic "amcl_pose"
  pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true);
  //publish the topic "particlecloud"
  particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 2, true);
  //create the service of "global_localization". globalLocalizationCallback is init the pfs
  global_loc_srv_ = nh_.advertiseService("global_localization",
                                         &AmclNode::globalLocalizationCallback,
                                         this);
  //create the service of "request_nomotion_update,"nomotionUpdateCallback force samples updates without motion reveived
  nomotion_update_srv_= nh_.advertiseService("request_nomotion_update", &AmclNode::nomotionUpdateCallback, this);
  set_map_srv_= nh_.advertiseService("set_map", &AmclNode::setMapCallback, this);
  //subscribe scan topic,MessageFilter used to filter and buffer the scan message util scan could be tramform to odom_frame
  laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100);
  laser_scan_filter_ =
          new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_,
                                                             *tf_,
                                                             odom_frame_id_,
                                                             100,
                                                             nh_);
  //when received scan msg, laserReceived be called
  laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,
                                                   this, _1));
  //subscibe topic "initialpose",initialPoseReceived used to init pose that service received,if this not be call, then use map origin to init pose by globalLocalizationCallback
  initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);
  //subscribe topic map, get the map, mapReceived will be called
  //handleMapMessage is used to deal with map message
  //when map msg received, the pf, odom and laser be created and inited
  if(use_map_topic_) {
    map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this);
    ROS_INFO("Subscribed to map topic.");
  } else {
  ... ...
  }

tf2_ros::MessageFilter的使用是将msg缓存，直到能够转换到指定的坐标系，如上是将scan msg缓存在tf_指定区域，直至msg可以被转换到odom_frame_id_。此时laserReceived被调用。

当接收到全局map信息后，mapReceived被调用，处理map msg及完成pf(滤波器)、odom(里程计)、laser等结构的创建及初始化

void AmclNode::mapReceived(const nav_msgs::OccupancyGridConstPtr& msg)
{
  if( first_map_only_ && first_map_received_ ) {
    return;
  }
  handleMapMessage( *msg );
  first_map_received_ = true;
}
//free_space_indices store the free cell index
//alloc the pfs
//new the AMCL odom and set the model of robot
//new the AMCL_laser, this stores map ptr,max num of particles likelyhood type
void
AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg)
{
  // if the config is being writen, stock and wait
  boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
  ... ...
    map_ = convertMap(msg);
#if NEW_UNIFORM_SAMPLING
  // Index of free space
  free_space_indices.resize(0);
  for(int i = 0; i < map_->size_x; i++)
    for(int j = 0; j < map_->size_y; j++)
      if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1)
        free_space_indices.push_back(std::make_pair(i,j));
#endif
  // Create the particle filter
  pf_ = pf_alloc(min_particles_, max_particles_,
                 alpha_slow_, alpha_fast_,
                 (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
                 (void *)map_);
  // Instantiate the sensor objects
  // Odometry
  delete odom_;
  odom_ = new AMCLOdom();
  ROS_ASSERT(odom_);
  odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
  // Laser
  delete laser_;
  laser_ = new AMCLLaser(max_beams_, map_);
  ROS_ASSERT(laser_);
  if(laser_model_type_ == LASER_MODEL_BEAM)
    laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_,
                         sigma_hit_, lambda_short_, 0.0);
  else if(laser_model_type_ == LASER_MODEL_LIKELIHOOD_FIELD_PROB){
  ... ...
  } 
}

接收到scan的消息，laserReceived处理并判断是否需要及完成更新及重采样。

scan处理函数完成任务：
计算雷达相对于机器人车体base坐标系的安装laser pose
获取里程计的估计信息，判断是否需要更新
更新完成后，完成重采样

// if the laserpose in base frame is not inited,then get the laser pose in base frame
// get the odom frame
// determine whether update pf or not
// resamping after update
void AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{
  // laser_scan_frame_id is the name of lase_scan_frame in string
  // get the lase_scan_frame name in string
  std::string laser_scan_frame_id = stripSlash(laser_scan->header.frame_id);
  ... ...
      //laser_pose is the laser pose in base frame,this is a constant
    //laser_pose is the transform form base_laser to base
    geometry_msgs::PoseStamped laser_pose;
    try
    {
      this->tf_->transform(ident, laser_pose, base_frame_id_);
    }
    ... ...
  // Where was the robot when this scan was taken?
  // get the robot pose in odom frame
  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_))
   ... ...
   // 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_;
    //m_force_update used to temporarily let amcl update samples even when no motion occurs
    update = update || m_force_update;
    ... ...
    // Use the action data to update the filter
    //利用运动学模型更新粒子位姿
    odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
    ... ...
    
    // If the robot has moved, update the filter
    if(lasers_update_[laser_index])
    ... ...
    // UpdateSensor use the observe mode to compute the sample's weight
    //使用观测模型来更新粒子权重
    lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);
    ... ...
    // Resample the particles
    if(!(++resample_count_ % resample_interval_))
    {
      pf_update_resample(pf_);
      resampled = true;
    }
    pf_sample_set_t* set = pf_->sets + pf_->current_set;
    

重要的结构：
粒子结构定义amcl/include/pf/pf.h

//单个粒子：代表位姿，权重
// Information for a single sample
typedef struct
{
  // Pose represented by this sample
  pf_vector_t pose;
  // Weight for this pose
  double weight;
} pf_sample_t;
//粒子簇结构，一个位姿节点的粒子簇
// Information for a cluster of samples
typedef struct
{
  // Number of samples
  int count;
  // Total weight of samples in this cluster
  double weight;
  // Cluster statistics
  pf_vector_t mean;
  pf_matrix_t cov
  // Workspace
  double m[4], c[2][2];
} pf_cluster_t;
// 粒子集，kdtree存储，包含多个粒子簇，多个位姿节点粒子簇
// Information for a set of samples
typedef struct _pf_sample_set_t
{
  // The samples
  int sample_count;
  pf_sample_t *samples;
  // A kdtree encoding the histogram
  pf_kdtree_t *kdtree;
  // Clusters
  int cluster_count, cluster_max_count;
  pf_cluster_t *clusters;
  // Filter statistics
  pf_vector_t mean;
  pf_matrix_t cov;
  int converged;
  double n_effective;
} pf_sample_set_t;
// 粒子滤波器结构体，包含了配置信息
typedef struct _pf_t
{
  // This min and max number of samples
  int min_samples, max_samples;
  // Population size parameters
  double pop_err, pop_z;
  // 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;

参考

《Probabilistic_Robotics》
http://wiki.ros.org/amcl
amcl介绍