资讯详情

【依赖二维激光雷达与全局地图的定位算法】amcl（蒙特卡洛）粒子滤波定位算法

系列文章目录

提示：所有文章的目录都可以添加到这里。目录需要手动添加 TODO:写完再整理

文章目录

  • 系列文章目录
  • 前言
  • (0)蒙特卡罗方法原理
  • （1）amcl定位框架介绍
  • （2）amcl定位框架组成
    • （1）ROS-TF坐标转换树【ROS的工具】
    • （2）robot_pose_ekf扩展卡尔曼位置估计算法
    • （3）定位amcl蒙特卡洛定位自适应，adaptive monte carlo localizaion**
  • （3）amcl定位节点的订阅和发布
  • （4）amcl定位功能包用法
    • (1)编写amcl节点相关的launch(amcl_omni.launch)文件
    • (2)配置launch文件参数
    • (3)编写测试launch文件
    • (4)执行 launch文件
  • （5）amcl定位程序源码应用程序只需安装源码
  • （6）amcl的定位效果
    • (1)第一步:算法初始分布
    • (2)第二步:算法收敛:
    • (3)第三步:稳定定位
    • (4)原理Odometry Localization和AMCL Map Localization比较
  • （7）amcl解决定位失败的解决方案(工程经验)
    • 方法1：机器人运行时处理
    • 方法二:机器人停止时处理

前言

认知有限，希望大家多包容，希望能和大家多交流，共同成长！

本文先对做一个简单的介绍，具体内容后续更多，其他模块可以参考我的其他文章

提示：以下是本文的主要内容

(0)蒙特卡罗方法原理

蒙特卡罗的本质是基于大数定律防盗标志–盒子君hzj】，主要包括三步：

  1. 样本采集根据输入的概率密度；
  2. 每个样本通过非线性函数转换；
  3. 概率密度从转换样本中重新计算 . 。

（1）amcl定位框架介绍

amcl节点利用粒子滤波算法实现机器人的全局定位，为机器人导航提供全局定位信息 由机器人定位框架ROS-TF坐标转换树，机器人扩展卡尔位置融合曼算法robot_pose_ekf和自适应蒙特卡洛定位框架amcl如果这三个部分相互独立，我们可以根据自己项目的实际需要防盗标志–盒子君hzj】与不同的定位算法灵活匹配，完成机器人的定位任务

简单来说，就是放很多pose(x, y, [偏航角] )，然后估计下一个pose基于粒子滤波的数据，从地图上看到的最符合雷达的数据（KLD）提供算法SLAM定位基础可以帮助机器人在已知地图上确定自己的位置，并不断跟踪机器人的定位 在这里插入图片描述 MCL(非自适应蒙特卡洛)定位算法采用粒子滤波定位 例子滤波器和浅度解释：一开始，沙子（颗粒）均匀地撒在地图空间，然后通过移动机器人的运动移动颗粒。例如，机器人向前移动1米，所有颗粒也向前移动1米，防盗标志–盒子君hzj】同时，每个粒子的位置将模拟一个传感器（通常是激光雷达），粒子的位置将模拟传感器与真实传感器进行比较，从而赋予每个粒子位置准确性的概率，然后根据这个概率不断产生新的粒子。准确性概率越大，产生粒子的可能性越大。经过多次迭代，所有粒子都会慢慢收敛聚集在一起，估计机器人的确切位置。

：地图、雷达数据、里程计 ：小车pose (x, y,? theta)

（2）amcl定位框架组成

（1）ROS-TF坐标转换树【ROS的工具】

具体看看ros教程

在里程计信息定位和车载传感器定位实现中，机器人坐标系一般采用机器人模型中的根坐标系(base_link 或 base_footprint)，里程计定位时，父级坐标系一般称为 odom，【防盗标记–盒子君hzj】如果通过传感器定位，父级参考系一般称为 map。两者结合使用时，map 和 odom 都是机器人模型根坐标系的父级，不符合坐标变换"单继承"因此，转换关系一般设置为： map -> doom -> base_link 或 base_footprint

（2）robot_pose_ekf扩展卡尔曼位置估计算法

我再开一个博客写，请转战

（3）定位amcl蒙特卡洛定位自适应，adaptive monte carlo localizaion**

. .

（3）amcl定位节点的订阅和发布

订阅话题

/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
        )

（4）amcl定位功能包用法

(1)编写amcl节点相关的launch(amcl_omni.launch)文件

<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>

(2)配置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>

< ```

(3)编写测试launch文件

<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find mycar_nav)/map/$(arg map)"/>
    <!-- 启动AMCL节点 -->
    <include file="$(find mycar_nav)/launch/amcl.launch" />
    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz"/>
</launch>

(4)执行 launch文件

. .

（5）amcl定位程序源码【应用仅仅需要安装源码】

ROS提供了定位（amcl）功能包，【防盗标记–盒子君hzj】来实现二维地图中的机器人定位，在ROS的导航功能包集navigation中提供了 amcl 功能包，用于实现导航中的机器人定位，amcl已经被集成到了navigation包中

mcl功能包源码路径 . . .

（6）amcl的定位效果

（1）第一步：算法初始化分布

（2）第二步：算法收敛

（3）第三步：稳定定位

（4）【原理】Odometry Localization和AMCL Map Localization比较

（7）amcl全局定位失败的解决办法（工程经验）

方法一：在机器人运行时处理

（1）调用global_localization (std_srvs/Empty)服务 通过调用该服务将初始化全局定位，将所有的粒子重新打散随机的分散在地图的空闲地方，可以通过以下命令来调用该服务，当调用后可以发现所有的粒子被重新打散随机分布在地图上，然后使用键盘遥控机器人旋转，这样可以自动重新对机器人进行定位 rosservice call /global_localization “{}”

（2）调用request_nomotion_update (std_srvs/Empty)服务 该服务是手动的来更新粒子并发布新的粒子，【防盗标记–盒子君hzj】可以使用如下命令来调用服务执行更新粒子的操作，该服务一般需要多次调用才能逐渐看到粒子收敛的效果 rosservice call /request_nomotion_update “{}”

（3）清理代价地图 rosservice call / move_base / clear_costmaps“{}”

（4）机器人再进行旋转重定位

方法二：在机器人停止时处理

（1）运行rosparam dump amcl.yaml /amcl/ （2）修改amcl.yaml文件 将参数"initial_pose_x"、“initial_pose_y”、“initia;_pose_a”注释掉。 （3）重新启动机器人 . .

标签： at三维激光传感器

锐单商城拥有海量元器件数据手册、 IC替代型号，打造 电子元器件IC百科大全！

锐单商城 - 一站式电子元器件采购平台