navigation stack 中amcl 源码解读

amcl运用在地图地位中的算法（见Probabilistic Robotics chapter8 与 基 于多假设跟踪 的移动机器人 自适应蒙特卡罗定位研 究，mcl具体推导过程可参考http://blog.csdn.net/heyijia0327/article/details/40899819）

在熟悉源码的时候需要弄清楚几个结构体所包含的信息

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;


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


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

在源码中，对定位整体上的处理在amcl_node.cpp中，主要流程为：

   1： 首先获取地图  调用 handleMapMessa（） 进行地图转换 ，记录obsta坐标 ，初始化pf_t 结构体等操作

   2： 获取初始位置 调用 handinitialpose()  进而调用 pf_init()处理初始位置坐标

   3： 更新机器人位置坐标 bindLaserRecieved（）

          a： 获取laser对应于baselink的坐标

          b：获取baselink对应于odom的坐标

          c：根据里程计的变化值+高斯噪音 更新 pf_t中samples的内里程计值（运动模型）

                odom->updateAction()

          d：根据当前雷达数据更新各里程计对应的权值weights  （观测模型   p（z|x）=   1/sqrt(2*pi*σ^2)) * exp(-(z-μ  )^2/(2*σ^2))）

               laser_[laser_index]->updateSensor（）

                具体算法见（Probabilistic Robotics chapter6）

               如：LikelihoodFieldModel 模型，算法为：

                     

             e：获取权值最高的坐标点  进行聚类，对高权值得cluster内的点 求均值即为当前机器人所在位置坐标

                    pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov)

             f：更新采样里程计值

                   pf_update_resample(pf_)

                  

                  

          【具体见（Tutorial on Monte Carlo Techniques chapter7）】

       amcl定位与mcl定位的主要区别在于 αslow，αfast 不为0， 会地图上随机采样点（里程计） ，但当地图较大且相似地方区域的情况下，可能会造成机器人坐标跳变