amcl简介

AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置。

amcl已经被集成到了navigation包，navigation安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-navigation

amcl节点说明

amcl 功能包中的核心节点是:amcl。为了方便调用，需要先了解该节点订阅的话题、发布的话题、服务以及相关参数。

订阅的Topic

scan(sensor_msgs/LaserScan)

激光雷达数据。

tf(tf/tfMessage)

坐标变换消息。

initialpose(geometry_msgs/PoseWithCovarianceStamped)

用来初始化粒子滤波器的均值和协方差。

map(nav_msgs/OccupancyGrid)

获取地图数据。

发布的Topic

amcl_pose(geometry_msgs/PoseWithCovarianceStamped)

机器人在地图中的位姿估计。

particlecloud(geometry_msgs/PoseArray)

位姿估计集合，rviz中可以被 PoseArray 订阅然后图形化显示机器人的位姿估计集合。

tf(tf/tfMessage)

发布从 odom 到 map 的转换。

服务

global_localization(std_srvs/Empty)

初始化全局定位的服务。

request_nomotion_update(std_srvs/Empty)

手动执行更新和发布更新的粒子的服务。

set_map(nav_msgs/SetMap)

手动设置新地图和姿态的服务。

调用的服务

static_map(nav_msgs/GetMap)

调用此服务获取地图数据。

参数

~odom_model_type(string, default:"diff")

里程计模型选择: "diff","omni","diff-corrected","omni-corrected" (diff 差速、omni 全向轮)

~odom_frame_id(string, default:"odom")

里程计坐标系。

~base_frame_id(string, default:"base_link")

机器人极坐标系

~global_frame_id(string, default:"map")

地图坐标系

.... 参数较多，上述是几个较为常用的参数，其他参数介绍可参考官网。

坐标变换

里程计本身也是可以协助机器人定位的，不过里程计存在累计误差且一些特殊情况时(车轮打滑)会出现定位错误的情况，amcl 则可以通过估算机器人在地图坐标系下的姿态，再结合里程计提高定位准确度。

里程计定位:只是通过里程计数据实现 /odom_frame 与 /base_frame 之间的坐标变换。 amcl定位: 可以提供 /map_frame 、/odom_frame 与 /base_frame 之间的坐标变换。

实现过程

amcl文件编写

关于launch文件的实现，在amcl功能包下的example目录已经给出了示例

<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
  <!-- Publish scans from best pose at a max of 10 Hz -->
  <param name="odom_model_type" value="diff"/>
  <param name="odom_alpha5" value="0.1"/>
  <param name="transform_tolerance" value="0.2" />
  <param name="gui_publish_rate" value="10.0"/>
  <param name="laser_max_beams" value="30"/>
  <param name="min_particles" value="500"/>
  <param name="max_particles" value="5000"/>
  <param name="kld_err" value="0.05"/>
  <param name="kld_z" value="0.99"/>
  <param name="odom_alpha1" value="0.2"/>
  <param name="odom_alpha2" value="0.2"/>
  <!-- translation std dev, m -->
  <param name="odom_alpha3" value="0.8"/>
  <param name="odom_alpha4" value="0.2"/>
  <param name="laser_z_hit" value="0.5"/>
  <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_likelihood_max_dist" value="2.0"/>
  <param name="update_min_d" value="0.2"/>
  <param name="update_min_a" value="0.5"/>
  <!-- 设置坐标系 -->
  <param name="odom_frame_id" value="odom"/>
  <param name="base_frame_id" value="base_footprint"/>
  <param name="global_frame_id" value="map"/>

  <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文件编写

<launch>

<!-- rviz -->
    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find nav_demo)/config/slam.rviz"/>
<!-- 加载仿真环境 -->
    <include file ="$(find nav_demo)/launch/nav_map_read.launch" />
<!-- 启动amcl -->
    <include file ="$(find nav_demo)/launch/nav_amcl.launch" />

</launch>

启动 Gazebo 仿真环境(此过程略)

启动键盘控制节点

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

启动 launch 文件

添加必要组件

在启动的 rviz 中，添加RobotModel、Map组件，分别显示机器人模型与地图，添加 posearray 插件，设置topic为particlecloud来显示 amcl 预估的当前机器人的位姿，箭头越是密集，说明当前机器人处于此位置的概率越高；

最终现象