關於amcl

amcl的英文全稱是adaptive Monte Carlo localization，其實就是蒙特卡洛定位方法的一種升級版，使用自適應的KLD方法來更新粒子，這裡不再多說（主要我也不熟），有興趣的可以去看：KLD。
而mcl（蒙特卡洛定位）法使用的是粒子濾波的方法來進行定位的。而粒子濾波很粗淺的說就是一開始在地圖空間很均勻的撒一把粒子，然後通過獲取機器人的motion來移動粒子，比如機器人向前移動了一米，所有的粒子也就向前移動一米，不管現在這個粒子的位置對不對。使用每個粒子所處位置模擬一個感測器資訊跟觀察到的感測器資訊（一般是鐳射）作對比，從而賦給每個粒子一個概率。之後根據生成的概率來重新生成粒子，概率越高的生成的概率越大。這樣的迭代之後，所有的粒子會慢慢地收斂到一起，機器人的確切位置也就被推算出來了。

ros中的amcl

ros中使用的就是自適應的蒙特卡洛定位法。

訂閱的主題

1. scan (sensor_msgs/LaserScan)
   Laser scans.
2. tf (tf/tfMessage)
   Transforms.
3. initialpose (geometry_msgs/PoseWithCovarianceStamped)
   Mean and covariance with which to (re-)initialize the particle filter.
4. map (nav_msgs/OccupancyGrid)
   When the use_map_topic parameter is set, AMCL subscribes to this topic to retrieve the map used for laser-based localization. New in navigation 1.4.2.
   實際上初始位姿可以通過引數提供也可以使用預設初始值，我們主要是要將scan（鐳射）、tf和map主題提供給amcl。

釋出的主題

1. amcl_pose (geometry_msgs/PoseWithCovarianceStamped)
   
   Robot’s estimated pose in the map, with covariance.
2. particlecloud (geometry_msgs/PoseArray)
   The set of pose estimates being maintained by the filter.
3. tf (tf/tfMessage)
   Publishes the transform from odom (which can be remapped via the ~odom_frame_id parameter) to map.
   如果純粹是為了顯示一下機器人的位姿（in rviz）我們只需要tf主題就夠了。

tf tree

我們需要將base_link，odom，map三個frame連線起來。

上圖是我自己的tf連線圖

example

<?xml version="1.0"?>
<launch>
   <!-->
  <node pkg="beginner_tutorials" type="talker" name="talker"/>
   <-->
  <node pkg="map_server" type="map_server" name="map_server" args="/home/zqq/map.yaml"/>

  <!-- amcl node -->
  <node pkg="amcl" type="amcl" name="amcl" output="screen">

  <remap from="scan" to="scan"/>
  <!-- Publish scans from best pose at a max of 10 Hz -->
  <param name="use_map_topic" value="true"/>
  <param name="odom_model_type" value="omni"/>
  <param name="odom_alpha5" value="0.1"/>
  <param name="transform_tolerance" value="0.5" />
  <param name="gui_publish_rate" value="10.0"/>
  <param name="laser_max_beams" value="300"/>
  <param name="min_particles" value="500"/>
  <param name="max_particles" value="5000"/>
  <param name="kld_err" value="0.1"/>
  <param name="kld_z" value="0.99"/>
  <param name="odom_alpha1" value="0.1"/>
  <param name="odom_alpha2" value="0.1"/>
  <!-- translation std dev, m -->
  <param name="odom_alpha3" value="0.1"/>
  <param name="odom_alpha4" value="0.1"/>
  <param name="laser_z_hit" value="0.9"/>
  <param name="laser_z_short" value="0.05"/>
  <param name="laser_z_max" value="0.05"/>
  <param name="laser_z_rand" value="0.5"/>
  <param name="laser_sigma_hit" value="0.2"/>
  <param name="laser_lambda_short" value="0.1"/>
  <param name="laser_lambda_short" value="0.1"/>
  <param name="laser_model_type" value="likelihood_field"/>
  <!-- <param name="laser_model_type" value="beam"/> -->
 <param name="laser_min_range" value="1"/>
<param name="laser_max_range" value="5"/>
  <param name="laser_likelihood_max_dist" value="2.0"/>
  <param name="update_min_d" value="0.2"/>
  <param name="update_min_a" value="0.5"/>
  <param name="resample_interval" value="1"/>
  <param name="transform_tolerance" value="0.1"/>
  <param name="recovery_alpha_slow" value="0.0"/>
  <param name="recovery_alpha_fast" value="0.0"/>


</node>

</launch>

這是我自己的一個launch檔案，分別呼叫了map_server節點和amcl節點，map_server節點讀取了一個我自己之前用gmapping製作的地圖（詳細教程：戳這裡），之後呼叫amcl節點，訂閱了scan鐳射主題，設定了一些引數，引數詳細作用：戳這裡。
注意一定要將tf tree設定好！！坑了大部分人的都是這個tf。

tf顯示的就是當前的機器人位姿。
注意：要將rviz的fixed frame設成map，因為map才是global_frame_id。

2015-11-27增補

今天才發現了rviz居然連粒子都可以顯示，顯示讓我對amcl粒子更新有了更深刻的理解。
首先在引數表裡面有幾個比較重要的引數。
~initial_pose_x (double, default: 0.0 meters)
Initial pose mean (x), used to initialize filter with Gaussian distribution.
~initial_pose_y (double, default: 0.0 meters)
Initial pose mean (y), used to initialize filter with Gaussian distribution.
~initial_pose_a (double, default: 0.0 radians)
Initial pose mean (yaw), used to initialize filter with Gaussian distribution.
~initial_cov_xx (double, default: 0.5*0.5 meters)
Initial pose covariance (x*x), used to initialize filter with Gaussian distribution.
~initial_cov_yy (double, default: 0.5*0.5 meters)
Initial pose covariance (y*y), used to initialize filter with Gaussian distribution.
~initial_cov_aa (double, default: (π/12)*(π/12) radian)
Initial pose covariance (yaw*yaw), used to initialize filter with Gaussian distribution.

這個代表了你初始化粒子時粒子分佈的一個狀態，注意要把方差設的大一些，要不所有例子上來就是一坨的就沒法玩了。

上圖是粒子的一個初始狀態~我設的方差比較大~所以分佈很大。

上圖是更新了一段時間之後~粒子逐漸趨於穩定

上圖為最終狀態，趨於穩定