[定位算法]amcl（自适应蒙特卡罗定位）

最新推荐文章于 2024-11-07 08:56:54 发布

Travis.X

最新推荐文章于 2024-11-07 08:56:54 发布

阅读量1.3k

点赞数

分类专栏：定位

本文链接：https://blog.csdn.net/Travis_X/article/details/85273403

版权

定位专栏收录该内容

2 篇文章 2 订阅

订阅专栏

本文深入解析AMCL（自适应蒙特卡洛定位）算法，包括其核心组件KLD_Sampling_MCL与Augmented_MCL的工作原理，以及在ROS中的实现细节。文章详细介绍了AMCL的订阅与发布话题、服务调用及launch文件参数配置，为机器人定位提供全面指导。

摘要由CSDN通过智能技术生成

AMCL

amcl采用的是结合自适应（增强蒙特卡洛Augmented_MCL）和库尔贝克-莱不勒散度采样KLD_Sampling_MCL（蒙特卡洛定位的一个变种）。
1.KLD_Sampling_MCL随时间改变粒子数，改良了度过初期后的蒙特卡洛大样本集合的资源浪费。两个kld_配置参数就是KLD的参数。对于每次粒子滤波迭代，KLD采样以概率1-δ确定样本数（1-δ就是kld_z配置参数），使得真实的后验与基于采样的近似之间的误差小于ε（ε就是kld_err配置参数）.kld_z=0.99,kld_err=0.05,直方图位大小为15cm*15cm*15°就能取得良好的结果。
2.Augmented_MCL解决的是从机器人绑架或全局定位失效中恢复的问题。两个recovery_alpha_配置参数就是用于失效恢复的。随机采样以max{0.0,1.0-w(fast)/w(slow)}概率增加。如果短期似然劣于长期似然则增加随机采样，这种方法，测量似然的一个突然衰减将引起随机采样的数目增加。w=w+α(Wavg-w)--Wavg当前测量模型的权重，w为短期（w(fast)）或长期(w(slow))平滑估计，α为与w对应的recovery_alpha_参数。

订阅话题

/tf (tf/tfMessage)：订阅了各坐标系转换的话题，用于查询各坐标系的转换。
/initialpose (geometry_msgs/PoseWithCovarianceStamped)：用于（重新）初始化粒子滤波器的平均值和协方差，简单来理解就是先预估计一下机器人的初始位姿。
/amcl/map (nav_msgs/OccupancyGrid)：当在launch文件中设置了use_map_topic为true时，amcl则订阅该话题获取地图，然后使用基于激光来进行定位，当然设置use_map_topic为false时不订阅该话题也是可以的。

发布话题

amcl_pose (geometry_msgs/PoseWithCovarianceStamped)：机器人在地图上带有协方差的位姿估计，这个是话题是整个粒子滤波定位的最终输出结果，该话题输出的位姿信息是根据全局坐标系/map的坐标转换后的位置。
particlecloud (geometry_msgs/PoseArray)：在粒子滤波器维护下的一组粒子位姿估计，可以直接在rviz中显示，查看粒子的收敛效果。
tf (tf/tfMessage)：发布从odom坐标系到map坐标系的转换，当然该odom坐标系可以使用odom_frame_id参数来重新映射为自定义的坐标系名称。
scan (sensor_msgs/LaserScan)：激光雷达信息。

服务

global_localization (std_srvs/Empty)：通过调用该服务将初始化全局定位，将所有的粒子重新打散随机的分散在地图的空闲地方。
request_nomotion_update (std_srvs/Empty)：该服务是手动的来更新粒子并发布新的粒子。
set_map (nav_msgs/SetMap)：amcl可以通过订阅/map话题来获取地图，如果在launch文件中设置use_map_static为false的话，就可以通过调用该服务来获取地图了，效果与订阅/map话题是一样的。

服务调用

static_map (nav_msgs/GetMap)：amcl调用此服务来检索基于激光定位的地图；启动阻止从此服务获取地图。

launch文件参数配置

<launch>
  <!--当设置为true时，AMCL将会订阅map话题，而不是调用服务返回地图。也就是说当设置为true时，有另外一个节点实时的发布map话题，
  也就是机器人在实时的进行地图构建，并供给amcl话题使用；当设置为false时，通过map server，也就是调用已经构建完成的地图。-->
  <arg name="use_map_topic"   default="false"/>
  <arg name="scan_topic"      default="scan"/>                             <!--扫描话题-->
  <arg name="initial_pose_x"  default="0.0"/>                              <!--初始位姿均值x,用于初始化高斯分布滤波器-->
  <arg name="initial_pose_y"  default="0.0"/>                              <!--初始位姿均值y,用于初始化高斯分布滤波器-->
  <arg name="initial_pose_a"  default="0.0"/>                              <!--初始位姿均值（yaw）,用于初始化高斯分布滤波器-->

  <arg name="odom_frame_id"   default="odom"/>                             <!--里程计默认坐标-->
  <arg name="base_frame_id"   default="base_link"/>                        <!--机器人基坐标-->
  <arg name="global_frame_id" default="map"/>                              <!--全局坐标-->

  <node pkg="amcl" type="amcl" name="amcl">
    <param name="use_map_topic"             value="$(arg use_map_topic)"/>
    <!-- Publish scans from best pose at a max of 10 Hz -->
    <param name="odom_model_type"           value="diff"/>                   <!--选择odom模型，diff差速模型，omni全向模型-->
    <param name="odom_alpha5"               value="0.1"/>                    <!--移相关的噪声参数（仅用于模型是“omni”的情况,默认0-->
    <param name="gui_publish_rate"          value="10.0"/>                   <!--扫描和路径发布到可视化软件的最大频率-->
    <param name="laser_max_beams"           value="120"/>                    <!--更新滤波器时，每次扫描中多少个等间距的光束被使用-->
    <param name="laser_max_range"           value="80.0"/>                   <!--最大扫描范围，参数设置为-1.0时，将会使用激光上报的最大扫描范围-->
    <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"/>                   <!--上标准分位数（1-p），其中p是估计分布上误差小于kld_err的概率-->
    <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"/>                    <!--模型的z_hit部分的混合权值-->
    <param name="laser_z_short"             value="0.05"/>                   <!--模型的z_short部分的混合权值-->
    <param name="laser_z_max"               value="0.05"/>                   <!--模型的z_max部分的混合权值-->
    <param name="laser_z_rand"              value="0.5"/>                    <!--模型的z_rand部分的混合权值-->
    <param name="laser_sigma_hit"           value="0.2"/>                    <!--被用在模型的z_hit部分的高斯模型的标准差-->
    <param name="laser_lambda_short"        value="0.1"/>                    <!--模型z_short部分的指数衰减参数-->
    <param name="laser_model_type"          value="likelihood_field"/>       <!--激光模型类型定义，可以是beam, likehood_field, likehood_field_prob-->
    <!-- <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.4"/>                    <!--执行滤波更新前旋转的角度-->
    <param name="odom_frame_id"             value="$(arg odom_frame_id)"/>   
    <param name="base_frame_id"             value="$(arg base_frame_id)"/> 
    <param name="global_frame_id"           value="$(arg global_frame_id)"/>
    <param name="resample_interval"         value="0.5"/>                    <!--在重采样前需要滤波更新的次数-->
    <!-- Increase tolerance because the computer can get quite busy -->
    <param name="transform_tolerance"       value="0.1"/>                    <!--tf变换发布推迟的时间-->
  <param name="recovery_alpha_slow" value="0.0"/>                            <!--慢速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来recover-->
  <param name="recovery_alpha_fast" value="0.0"/>                            <!--快速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来recover-->
    <<param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
    <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
    <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
    <remap from="scan"                      to="$(arg scan_topic)"/>
  </node>
</launch>