amcl_demo之机器人定位器——amcl
在amcl_demo的launch文件中，通过如下的形式开启了节点amcl。 首先根据系统环境变量"TURTLEBOT_3D_SENSOR"指定用于定位的传感器，并根据该传感器设定加载amcl的launch文件。此外还定义了关于初始位姿的三个参数"initial_pose_x", "initial_pose_y"和"initial_pose_a"。
<arg name="3d_sensor" default="$(env TURTLEBOT_3D_SENSOR)"/>
<arg name="initial_pose_x" default="0.0"/>
<arg name="initial_pose_y" default="0.0"/>
<arg name="initial_pose_a" default="0.0"/>
<arg name="custom_amcl_launch_file" default="$(find turtlebot_navigation)/launch/includes/amcl/$(arg 3d_sensor)_amcl.launch.xml"/>
有了这些参数之后，我们就可以加载实际的定位器了。
<include file="$(arg custom_amcl_launch_file)">
<arg name="initial_pose_x" value="$(arg initial_pose_x)"/>
<arg name="initial_pose_y" value="$(arg initial_pose_y)"/>
<arg name="initial_pose_a" value="$(arg initial_pose_a)"/>
</include>
通过如下的指令，我们可以查询到定位使用的传感器是asus_xtion_pro。对应的，我们需要加载的amcl的launch文件是asus_xtion_pro_amcl.launch.xml， 但在源代码中这个文件实际上是一个指向amcl.launch.xml的软链接，所以实际加载的是amcl.launch.xml。
$ env | grep TURTLEBOT_3D_SENSOR
TURTLEBOT_3D_SENSOR=asus_xtion_pro
1. amcl.launch.xml
amcl.launch.xml是实际运行节点amcl的脚本， 在一开始，定义了一堆参数。"use_map_topic"是ROS导航栈在1.4.2版本新增的功能，amcl将订阅一个map主题来接收地图数据。否则通过请求map_server的服务来获取地图数据。 "scan_topic"则是激光传感器的扫描数据主题。"initial_pose_x(y)"分别是机器人的初始位置，"initial_pose_a"则是机器人的初始方向角。 "odom_frame_id", "base_frame_id"和"global_frame_id"分别是用于定位的里程计坐标系、底盘坐标系、地图坐标系。
<arg name="use_map_topic" default="false"/>
<arg name="scan_topic" default="scan"/>
<arg name="initial_pose_x" default="0.0"/>
<arg name="initial_pose_y" default="0.0"/>
<arg name="initial_pose_a" default="0.0"/>
<arg name="odom_frame_id" default="odom"/>
<arg name="base_frame_id" default="base_footprint"/>
<arg name="global_frame_id" default="map"/>
然后我们开启amcl节点，在开启的过程中定义了各种运行参数。在代码片段中我们将之省略了， 下面我们翻译官方Wiki中对这些参数的解释。
<node pkg="amcl" type="amcl" name="amcl">
<!-- 定义了各种运行参数param，省略之 -->
</node>
总体上节点amcl的参数可以分为三大类：粒子滤波器配置、激光雷达模型和里程计模型。
表1 粒子滤波配置
| 参数名称 | 数据类型 | 默认值 | 说明 |
|---|---|---|---|
| ~min_particles | int | 100 | 粒子滤波器使用的最小粒子数量。 |
| ~max_particles | int | 5000 | 粒子滤波器使用的最大粒子数量。 |
| ~kld_err | double | 0.01 | 真实分布与估计分布之间的最大误差。 |
| ~kld_z | double | 0.99 | 上标准分位数（1-p），其中p是估计分布上误差小于kld_err的概率。 |
| ~update_min_d | double | 0.2m | 执行一次滤波器更新所需的最小位移。 |
| ~update_min_a | double | \(\frac{\pi}{6}\) | 执行一次滤波器更新所需的最小转角。 |
| ~resample_interval | int | 2 | 重采样间隔，即至少进行了~resample_interval次的滤波器更新之后，才能进行一次重采样。 |
| ~transform_tolerance | double | 0.1s | 推迟发布坐标变换的时间，to indicate that this transform is valid into the future |
| ~recovery_alpha_slow | double | 0.0(disabled) | 慢速平均权重滤波器(slow average weight filter)的指数衰减速率，用于确定添加随机位姿的时机，以达到recover的目的。一般可以取0.001。 |
| ~recovery_alpha_fast | double | 0.0(disabled) | 快速平均权重滤波器(fast average weight filter)的指数衰减速率，用于确定添加随机位姿的时机，以达到recover的目的。一般可以取0.1。 |
| ~initial_pose_x | double | 0.0 | 初始位姿中心的x分量，用作初始位姿的高斯分布的均值。 |
| ~initial_pose_y | double | 0.0 | 初始位姿中心的y分量，用作初始位姿的高斯分布的均值。 |
| ~initial_pose_a | double | 0.0 | 初始方向角，用作初始位姿的高斯分布的均值。 |
| ~initial_cov_xx | double | 0.5 * 0.5 | 初始位姿方差(x*x)，用作初始位姿的高斯分布的协方差矩阵。 |
| ~initial_cov_yy | double | 0.5 * 0.5 | 初始位姿方差(y*y)，用作初始位姿的高斯分布的协方差矩阵。 |
| ~initial_cov_aa | double | \(\frac{\pi}{12}×\frac{\pi}{12}\) | 初始方向角方差(a*a)，用作初始位姿的高斯分布的协方差矩阵。 |
| ~gui_publish_rate | double | -1Hz(disabled) | 发布扫描数据和路径到可视化界面的最大频率，-1关闭此功能。 |
| ~save_pose_rate | double | 0.5Hz | 保存最近一次估计的位姿和协方差到参数服务器中的最大频率，即修改~initial_pos_*和~intial_cov*。-1关闭此功能。 |
| ~use_map_topic | bool | false | 该参数为真时，amcl将订阅一个map主题来接收地图数据。否则通过请求map_server的服务来获取地图数据。 |
| ~first_map_only | bool | false | 该参数为真时，amcl将只使用所订阅主题的第一帧地图数据进行定位，而不再更新地图。 |
表2 雷达模型配置
| 参数名称 | 数据类型 | 默认值 | 说明 |
|---|---|---|---|
| ~laser_min_range | double | -1.0 | 激光扫描的最小距离，该值为-1时，amcl将使用激光传感器上报数据中的最小值。 |
| ~laser_max_range | double | -1.0 | 激光扫描的最大距离，该值为-1时，amcl将使用激光传感器上报数据中的最大值。 |
| ~laser_max_beams | double | 30.0 | 更新滤波器的时候，使用激光扫描束的间隔。一方面可以减少计算量，另一方面也是因为临近的激光数据之间也不是独立的。 |
| ~laser_z_hit | double | 0.95 | 传感器模型中的z_hit部分的混合权重。 |
| ~laser_z_short | double | 0.1 | 传感器模型中的z_short部分的混合权重。 |
| ~laser_z_max | double | 0.05 | 传感器模型中的z_max部分的混合权重。 |
| ~laser_z_rand | double | 0.05 | 传感器模型中的z_rand部分的混合权重。 |
| ~laser_sigma_hit | double | 0.2m | z_hit部分的高斯标准差。 |
| ~laser_lambda_short | double | 0.1 | z_short部分的指数衰减系数。 |
| ~laser_likelihood_max_dist | double | 2.0m | 似然场模型中，对障碍物膨胀的最大距离。 |
| ~laser_model_type | string | "likelihood_field" | 激光传感器模型，可以是"beam"、"likelihood_field"、"likelihood_field_prob"。 |
表3 里程计模型配置
| 参数名称 | 数据类型 | 默认值 | 说明 |
|---|---|---|---|
| ~odom_model_type | string | diff | 里程计模型类型，可以是"diff"、"omni"、"diff-corrected"、"omni-corrected"。 |
| ~odom_alpha1 | double | 0.2 | 机器人运动模型中，旋转部分的旋转期望噪声。 |
| ~odom_alpha2 | double | 0.2 | 机器人运动模型的平移部分的旋转期望噪声。 |
| ~odom_alpha3 | double | 0.2 | 机器人运动模型的平移部分的平移期望噪声。 |
| ~odom_alpha4 | double | 0.2 | 机器人运动模型的旋转部分的平移期望噪声。 |
| ~odom_alpha5 | double | 0.2 | 平移相关的噪声参数(只适用于"omni"类型的里程计)。 |
| ~odom_frame_id | string | "odom" | 里程计坐标系。 |
| ~base_frame_id | string | "base_link" | 机器人底盘坐标系。 |
| ~global_frame_id | string | "map" | 定位系统的全局坐标系。 |
| ~tf_broadcast | bool | true | 设置为false，将不发布从全局坐标到里程计坐标的变换关系。 |
2. amcl运行分析
我们分别在三个终端中运行如下的指令，依次开启Gazebo仿真环境，运行amcl_demo，打开可视化界面rviz，详细可以参见Turtlebot你好!。
$ roslaunch turtlebot_gazebo turtlebot_world.launch
$ roslaunch turtlebot_gazebo amcl_demo.launch
$ roslaunch turtlebot_rviz_launchers view_navigation.launch
通过工具rosnode，我们可以查看节点amcl的运行状态。下面的日志片段描述了amcl发布的主题。
$ rosnode info /amcl
--------------------------------------------------------------------------------
Node [/amcl]
Publications:
* /amcl/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
* /amcl/parameter_updates [dynamic_reconfigure/Config]
* /amcl_pose [geometry_msgs/PoseWithCovarianceStamped]
* /diagnostics [diagnostic_msgs/DiagnosticArray]
* /particlecloud [geometry_msgs/PoseArray]
* /rosout [rosgraph_msgs/Log]
* /tf [tf2_msgs/TFMessage]
其中主题"/amcl/parameter_descriptions"和"/amcl/parameter_updates"是两个用于动态配置节点运行参数的主题。在我们的示例环境中，这两个主题没有订阅者，与amcl的定位算法无关，不再展开。
主题"/amcl_pose"是amcl的输出， 它以[geometry_msgs/PoseWithCovarianceStamped]的消息形式发布带有协方差信息的机器人位姿。 这个消息类型大体上可以分为三个字段："header"是一个通用的消息头，它为每条消息打上了一个时间戳和坐标系。"pose"记录了笛卡尔坐标系下的xyz三轴坐标，并用四元数的方式记录了机器人的姿态。 "covariance"是一个\(6 \times 6\)的协方差矩阵。
主题"/diagnostics"和"/rosout"都与日志和调试相关，不再描述。
"/particlecloud"主题输出的是amcl算法的粒子位姿。右图是对rviz的截图，机器人周围的绿色箭头就是rviz接收到的"/particlecloud"的消息的可视化。每个箭头对应着一个粒子，箭头的方向是粒子的朝向， 位置则是粒子的位置。amcl以参数"initial_pose_x", "initial_pose_y"和"initial_pose_a"为中心，按照正态分布的形式初始化粒子位姿。系统刚开始运行的时候，粒子分布比较松散。 随着机器人运动，我们将可以看到粒子集合逐渐收敛。
主题"/tf"用于描述系统的坐标变换关系。
下面的日志片段列举了amcl订阅的主题。其中"/clock"是gazebo仿真的时钟系统，"/tf"和"/tf_static"描述了坐标变换关系，"/scan"则是激光传感器的扫描数据， "initialpose"是用于重新初始化amcl的粒子位姿的主题。
Subscriptions:
* /clock [rosgraph_msgs/Clock]
* /initialpose [geometry_msgs/PoseWithCovarianceStamped]
* /scan [sensor_msgs/LaserScan]
* /tf [tf2_msgs/TFMessage]
* /tf_static [tf2_msgs/TFMessage]
下面的日志片段则列举了amcl提供的服务。其中"/amcl/get_loggers"和"/amcl/set_logger_level"与系统日志等级有关，"/amcl/set_parameters"看字面意思就是用来设定运行参数的。
Services:
* /amcl/get_loggers
* /amcl/set_logger_level
* /amcl/set_parameters
* /global_localization
* /request_nomotion_update
* /set_map
服务"/global_localization"是一个用于全局定位的服务。当调用该服务之后，amcl将把所有的粒子散布在地图中的空闲空间中，并开始全局定位。"request_nomotion_update"用于手动更新粒子并发布。 "/set_map"用于手动设定新的地图和位姿。
3. 完
amcl在acml_demo中扮演了机器人定位器的角色。它订阅了"/scan"和"/tf"来获取激光传感器的参数和里程计的坐标变换，通过主题"/map"获取环境地图。使用粒子滤波的方式进行机器人定位， 并通过主题"/amcl_pose"和"/particlecloud"发布对机器人位姿的估计以及粒子的位姿。