ros的navigation之———amcl(localization)應用詳解
關於amcl
amcl的英文全稱是adaptive Monte Carlo localization,其實就是蒙特卡洛定位方法的一種升級版,使用自適應的KLD方法來更新粒子,這裡不再多說(主要我也不熟),有興趣的可以去看:KLD。
而mcl(蒙特卡洛定位)法使用的是粒子濾波的方法來進行定位的。而粒子濾波很粗淺的說就是一開始在地圖空間很均勻的撒一把粒子,然後通過獲取機器人的motion來移動粒子,比如機器人向前移動了一米,所有的粒子也就向前移動一米,不管現在這個粒子的位置對不對。使用每個粒子所處位置模擬一個感測器資訊跟觀察到的感測器資訊(一般是鐳射)作對比,從而賦給每個粒子一個概率。之後根據生成的概率來重新生成粒子,概率越高的生成的概率越大。這樣的迭代之後,所有的粒子會慢慢地收斂到一起,機器人的確切位置也就被推算出來了。
這幅圖模擬了一個一維機器人的粒子更新,機器人下面那些想條形碼一樣的豎條就是粒子的分佈了,可以看到粒子隨著機器人的移動與更新會逐漸的收斂到機器人的正確位置上。
mcl演算法步驟圖:
wiki連結
ros中的amcl
ros中使用的就是自適應的蒙特卡洛定位法。
訂閱的主題
- scan (sensor_msgs/LaserScan)
Laser scans. - tf (tf/tfMessage)
Transforms. - initialpose (geometry_msgs/PoseWithCovarianceStamped)
Mean and covariance with which to (re-)initialize the particle filter. - 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。
釋出的主題
- amcl_pose (geometry_msgs/PoseWithCovarianceStamped)
Robot’s estimated pose in the map, with covariance. - particlecloud (geometry_msgs/PoseArray)
The set of pose estimates being maintained by the filter. - 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.
這個代表了你初始化粒子時粒子分佈的一個狀態,注意要把方差設的大一些,要不所有例子上來就是一坨的就沒法玩了。
上圖是粒子的一個初始狀態~我設的方差比較大~所以分佈很大。
上圖是更新了一段時間之後~粒子逐漸趨於穩定
上圖為最終狀態,趨於穩定