ROS与navigation教程-amcl

ROS与navigation教程-amcl

说明：

介绍amcl的概念和相关知识。

代码库

参考链接：https://github.com/ros-planning/navigation

概要

amcl是一种机器人在2D中移动的概率定位系统。它实现了自适应（或KLD采样）蒙特卡罗定位方法（如Dieter Fox所述），该方法使用粒子滤波器来针对已知地图跟踪机器人的位姿。

Algorithms

在《Probabilistic Robotics》这书中，提及了许多的参数和算法。推荐读者阅读该书来获得更多相关信息。
其中用到该书的算法有sample_motion_model_odometry, beam_range_finder_model, likelihood_field_range_finder_model, Augmented_MCL, and KLD_Sampling_MCL。
基于目前现有的实现的话，这个node仅能使用激光扫描和扫描地图来工作，但是可以进行扩展以使用其它传感器数据。

Example

在base_scan topic上使用激光数据定位：

amcl scan:=base_scan

Nodes

( 1 ) amcl

amcl节点接收laser-based地图，激光扫描和tf变换信息，并且输出位姿评估。
启动时，amcl根据提供的参数初始化其粒子滤波器。
注意：如果没有设置参数，初始滤波器状态将是以（0,0,0）为中心的适中大小的粒子云。

( 1.1 ) Subscribed Topics

scan ([sensor_msgs/LaserScan][2])

激光扫描。

tf ([tf/tfMessage][3])

tf变换信息。

initialpose ([geometry_msgs/PoseWithCovarianceStamped][4])

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

map ([nav_msgs/OccupancyGrid][5])

当设置use_map_topic参数时，AMCL会订阅此话题来获取用于laser-based定位的地图。New in navigation 1.4.2.

( 1.2 ) Published Topics

amcl_pose ([geometry_msgs/PoseWithCovarianceStamped][6])

机器人在地图上的带有协方差的预估位姿。

particlecloud ([geometry_msgs/PoseArray][7])

粒子滤波器维护的预估位姿集合。

tf ([tf/tfMessage][8])

发布里程（可以通过~odom_frame_id参数重新映射）到地图的变换。

( 1.3 ) Services

global_localization ([std_srvs/Empty][9])

初始化全局定位，其中所有粒子随机分布在地图中的自由空间中。

request_nomotion_update ([std_srvs/Empty][10])

用于手动执行更新和发布更新的粒子。

( 1.4 ) Services Called

static_map ([nav_msgs/GetMap][11])

amcl调用此服务来获取laser-based定位的地图; 从该服务获取地图的启动模块。
amcl calls this service to retrieve the map that is used for laser-based localization; startup blocks on getting the map from this service.

( 1.5 ) Parameters

ROS参数可以使用三种类型来配置amcl节点：verall filter, laser model, 和 odometery model。

Overall filter 参数

~min_particles (int, default: 100)

允许的最少粒子数。

~max_particles (int, default: 5000)

允许的最多粒子数。

~kld_err (double, default: 0.01)

实际分布与估计分布之间的最大误差。

~kld_z (double, default: 0.99)

Upper standard normal quantile for (1 - p), where p is the probability that the error on the estimated distrubition will be less than kld_err.

~update_min_d (double, default: 0.2 meters)

执行更新过滤器操作之前需要进行平移运动。

~update_min_a (double, default: π/6.0 radians)

执行更新滤波器操作之前需要进行旋转运动。

~resample_interval (int, default: 2)

重新采样前需要的滤波器到更新次数。

~transform_tolerance (double, default: 0.1 seconds)

Time with which to post-date the transform that is published, to indicate that this transform is valid into the future.

~recovery_alpha_slow (double, default: 0.0 (disabled))

slow average weight滤波器的指数衰减率，用于决定何时通过添加随机位姿进行恢复。
建议设置为0.001。
Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.001.

~recovery_alpha_fast (double, default: 0.0 (disabled))

fast average weight滤波器的指数衰减率，用于决定何时通过添加随机位姿进行恢复。
建议设置为0.1。
Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.1.

~initial_pose_x (double, default: 0.0 meters)

初始位姿mean（x），用于初始化高斯分布的滤波器。

~initial_pose_y (double, default: 0.0 meters)

初始位姿mean（y），用于初始化高斯分布的滤波器。

~initial_pose_a (double, default: 0.0 radians)

初始位姿mean（yaw），用于初始化高斯分布的滤波器。

~initial_cov_xx (double, default: 0.5*0.5 meters)

初始位姿covariance (x*x) ，用于初始化高斯分布的滤波器。

~initial_cov_yy (double, default: 0.5*0.5 meters)

初始位姿covariance (y*y) ，用于初始化高斯分布的滤波器。

~initial_cov_aa (double, default: (π/12)

初始位姿covariance (yaw*yaw) ，用于初始化高斯分布的滤波器。

~gui_publish_rate (double, default: -1.0 Hz)

指定最大可用多大速率(Hz)扫描并发布用于可视化的路径。
若设置为-1.0，则表示为禁用。

~save_pose_rate (double, default: 0.5 Hz)

指定在 ~initial_pose_* and ~initial_cov_*变量存储的上次预估的位姿和协方差到参数服务器的最大速率 (Hz)。保存的位姿会在后面初始化滤波器时候使用。
若设置为-1.0，则表示为禁用。

~use_map_topic (bool, default: false)

若为true，AMCL将订阅地图话题，而不是进行服务调用来获取其地图。New in navigation 1.4.2.

~first_map_only (bool, default: false)

若为true，AMCL将使用订阅到的第一个地图，不会使用每次更新获取的新地图。New in navigation 1.4.2.

Laser model 参数

注意：无论使用什么混合权重，权重加总应该等于1。beam model使用了所有的4种权重: z_hit, z_short, z_max, 和 z_rand。likelihood_field model仅仅使用了2种: z_hit 和 z_rand。

~laser_min_range (double, default: -1.0)

指定最小的扫描范围。
若设置为-1.0，则表示使用已报告的激光的最小范围。

~laser_max_range (double, default: -1.0)

指定最大的扫描范围。
若设置为-1.0，则表示使用已报告的激光的最大范围。

~laser_max_beams (int, default: 30)

在更新滤波器时，每次扫描中使用多少个均匀分布的beam。

~laser_z_hit (double, default: 0.95)

模型的z_hit部分的混合权重。

~laser_z_short (double, default: 0.1)

模型的z_short部分的混合权重。

~laser_z_max (double, default: 0.05)

模型的z_max部分的混合权重。

~laser_z_rand (double, default: 0.05)

模型的z_rand部分的混合权重。

~laser_sigma_hit (double, default: 0.2 meters)

在z_hit部分模型中使用的高斯模型的标准差。

~laser_lambda_short (double, default: 0.1)

模型中z_short部分的指数衰减参数。

~laser_likelihood_max_dist (double, default: 2.0 meters)

在地图上进行障碍物膨胀的最大距离，用于likelihood_field模型。

~laser_model_type (string, default: "likelihood_field")

需要用到哪种模型， either beam, likelihood_field, or likelihood_field_prob (same as likelihood_field but incorporates the beamskip feature, if enabled). 。

Odometry model 参数

如果~odom_model_type参数设置为“diff”，则使用《Probabilistic Robotics》p136中的sample_motion_model_odometry算法; 正如本书中所定义一样,该模型使用odom_alpha_1到odom_alpha_4的噪声参数。

如果~odom_model_type参数设置为“omni”，那么使用自定义模型用于omni-directional 基座，该模型使用odom_alpha_1到odom_alpha_5的噪声参数。前4个参数类似于“diff”模型，第5个参数用于捕获机器人在垂直于前进方向的位移（没有旋转）趋势。

~odom_model_type (string, default: "diff")

需要使用哪个模型，either "diff", "omni", "diff-corrected" or "omni-corrected"。

~odom_alpha1 (double, default: 0.2)

基于机器人运动旋转分量，来指定里程旋转估计中预期的噪声。
Specifies the expected noise in odometry's rotation estimate from the rotational component of the robot's motion.

~odom_alpha2 (double, default: 0.2)

基于机器人运动旋转分量，来指定里程平移估计中预期的噪声。
Specifies the expected noise in odometry's rotation estimate from translational component of the robot's motion.

~odom_alpha3 (double, default: 0.2)

基于机器人运动平移分量，来指定里程平移估计中预期的噪声。
Specifies the expected noise in odometry's translation estimate from the translational component of the robot's motion.

~odom_alpha4 (double, default: 0.2)

基于机器人运动平移分量，来指定里程旋转估计中预期的噪声。
Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion.

~odom_alpha5 (double, default: 0.2)

Translation-related noise parameter (only used if model is "omni").

~odom_frame_id (string, default:"odom")

里程计使用的坐标系。

~base_frame_id (string, default: "base_link")

移动基座使用的坐标系。

~global_frame_id (string, default:"map")

由定位系统发布的坐标系的名称。

~tf_broadcast (bool, default: true)

但若设置为false，则可防止amcl在全局坐标系和里程计坐标系之间发布tf变换。

( 1.6 ) Transforms

amcl要把进来的激光扫描信息转换到里程坐标系 (~odom_frame_id)。因此，tf树中必定会存在一条从发布激光的坐标系到里程坐标系的路径。
实现细节：在接收到第一次激光扫描时，amcl查找激光坐标系和基座坐标系（~base_frame_id）之间的的变换，并将其永久锁存（latches it forever）。所以amcl不能处理相对于基地移动的激光。
下图显示了使用里程计和amcl的定位的区别。在操作期间，amcl评估了相对于全局坐标系（~global_frame_id）的基座坐标系（~base_frame_id）的变换，但它仅在全局坐标系和里程计坐标系（~odom_frame_id）之间发布变换。本质上，这种变换解释了使用“Dead Reckoning”进行的累计漂移。 AMCL发布的转换可能带有未来时间戳，但是对AMCL来说是有效的。

参考资料

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号