博客

ROS学习笔记之——gmapping与amcl

  • Post author:
  • Post category:其他


之前博文《

ROS学习笔记之——Navigation Stack

》已经学习过了ROS中的导航包了，本博文来学习一下amcl定位与gmapping


目录


概述


gmapping 建图


订阅话题


发布话题


里程计误差及修正


TF消息


格式规范


TF树的数据类型


amcl定位


订阅主题


发布主题


参考资料

概述

在导航中，不可缺少的便是地图与定位。需要知道机器人当前的位置坐标（定位），然后根据已经建立好的地图，通过路径规划来规划出合理的路径

gmapping 建图

gmapping的作用是根据激光雷达和里程计（Odometry）的信息，对环境地图进行构建，并且对自身状态进行估计。因此它得输入应当包括激光雷达和里程计的数据，而输出应当有自身位置和地图。


http://wiki.ros.org/amcl

源码的地址：

https://github.com/ros-perception/slam_gmapping

gmapping是一个比较完善的地图构建开源包，使用激光和里程计的数据来生成二维地图。

要采用gmapping需要机器人拥有odometry数据以及laser range-finder。slam_gmapping节点会尝试将输入的激光雷达扫描的数据转换到odom frame下。

slam_gmapping采用sensor_msgs/LaserScan Message（

http://docs.ros.org/en/api/sensor_msgs/html/msg/LaserScan.html

）来构建地图并且发布nav_msgs/OccupancyGrid Message出来（

http://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html

）

注意在地图消息中，存在了原点的pose，包括了位置与方向

订阅话题

1、tf message （

http://docs.ros.org/en/api/tf/html/msg/tfMessage.html

）目的是为了实现laser，base，odometry之间的变换

<the frame attached to incoming scans> → base_link

usually a fixed value, broadcast periodically by a

robot_state_publisher

, or a tf

static_transform_publisher

.

base_link → odom

usually provided by the odometry system (e.g., the driver for the mobile base)


/tf

以及

/tf_static

： 坐标变换，类型为第一代的

tf/tfMessage

或第二代的

tf2_msgs/TFMessage

其中一定得提供的有两个tf，一个是

base_frame

与

laser_frame

之间的tf，即机器人底盘和激光雷达之间的变换；一个是

base_frame

与

odom_frame

之间的tf，即底盘和里程计原点之间的坐标变换。

odom_frame

可以理解为里程计原点所在的坐标系。

2、sensor_msgs/LaserScan Message（

http://docs.ros.org/en/api/sensor_msgs/html/msg/LaserScan.html

）激光雷达数据，类型为

sensor_msgs/LaserScan


/scan

很好理解，Gmapping SLAM所必须的激光雷达数据，而

/tf

是一个比较容易忽视的细节。尽管

/tf

这个Topic听起来很简单，但它维护了整个ROS三维世界里的转换关系，而

slam_gmapping

要从中读取的数据是

base_frame

与

laser_frame

之间的tf,只有这样才能够把周围障碍物变换到机器人坐标系下，更重要的是

base_frame

与

odom_frame

之间的tf，这个tf反映了里程计（电机的光电码盘、视觉里程计、IMU）的监测数据，也就是机器人里程计测得走了多少距离，它会把这段变换发布到

odom_frame

和

laser_frame

之间。

因此

slam_gmapping

会从

/tf

中获得机器人里程计的数据。

发布话题

1、nav_msgs/MapMetaData Message （

http://docs.ros.org/en/api/nav_msgs/html/msg/MapMetaData.html

）这是map_metadata，

2、nav_msgs/OccupancyGrid Message（

http://docs.ros.org/en/api/nav_msgs/html/msg/OccupancyGrid.html

）这是map。感觉跟1的区别是，真在发布的消息，是这个，因为里面还包含了header之类了，除了header外，还有nav_msgs/MapMetaData，就是1了～

3、~entropy (std_msgs/Float64)（

http://docs.ros.org/en/api/std_msgs/html/msg/Float64.html

）这个是代表了机器人位姿分布的不确定性。

在之前博客《

ROS学习笔记之——激光雷达SLAM建图

》中也谈到了，通过运行下面命令即可实现slam建图 

roslaunch turtlebot3_slam turtlebot3_slam.launch

那么具体地，我们来看看这个launch文件的组成（/home/kwanwaipang/catkin_ws/src/turtlebot3/turtlebot3_slam/launch/turtlebot3_slam.launch） 

<launch>
  <!-- Arguments -->
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="slam_methods" default="gmapping" doc="slam type [gmapping, cartographer, hector, karto, frontier_exploration]"/>
  <arg name="configuration_basename" default="turtlebot3_lds_2d.lua"/>
  <arg name="open_rviz" default="true"/>

  <!-- TurtleBot3 -->
  <include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch">
    <arg name="model" value="$(arg model)" />
  </include>

  <!-- SLAM: Gmapping, Cartographer, Hector, Karto, Frontier_exploration, RTAB-Map -->
  <include file="$(find turtlebot3_slam)/launch/turtlebot3_$(arg slam_methods).launch">
    <arg name="model" value="$(arg model)"/>
    <arg name="configuration_basename" value="$(arg configuration_basename)"/>
  </include>

  <!-- rviz -->
  <group if="$(arg open_rviz)"> 
    <node pkg="rviz" type="rviz" name="rviz" required="true"
          args="-d $(find turtlebot3_slam)/rviz/turtlebot3_$(arg slam_methods).rviz"/>
  </group>
</launch>

然后再看看gmapping.launch 

<launch>
  <!-- Arguments -->
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="configuration_basename" default="turtlebot3_lds_2d.lua"/>
  <arg name="set_base_frame" default="base_footprint"/>
  <arg name="set_odom_frame" default="odom"/>
  <arg name="set_map_frame"  default="map"/>

  <!-- Gmapping -->
  <node pkg="gmapping" type="slam_gmapping" name="turtlebot3_slam_gmapping" output="screen">
    <param name="base_frame" value="$(arg set_base_frame)"/>
    <param name="odom_frame" value="$(arg set_odom_frame)"/>
    <param name="map_frame"  value="$(arg set_map_frame)"/>
    <param name="map_update_interval" value="2.0"/>
    <param name="maxUrange" value="3.0"/>
    <param name="sigma" value="0.05"/>
    <param name="kernelSize" value="1"/>
    <param name="lstep" value="0.05"/>
    <param name="astep" value="0.05"/>
    <param name="iterations" value="5"/>
    <param name="lsigma" value="0.075"/>
    <param name="ogain" value="3.0"/>
    <param name="lskip" value="0"/>
    <param name="minimumScore" value="50"/>
    <param name="srr" value="0.1"/>
    <param name="srt" value="0.2"/>
    <param name="str" value="0.1"/>
    <param name="stt" value="0.2"/>
    <param name="linearUpdate" value="1.0"/>
    <param name="angularUpdate" value="0.2"/>
    <param name="temporalUpdate" value="0.5"/>
    <param name="resampleThreshold" value="0.5"/>
    <param name="particles" value="100"/>
    <param name="xmin" value="-10.0"/>
    <param name="ymin" value="-10.0"/>
    <param name="xmax" value="10.0"/>
    <param name="ymax" value="10.0"/>
    <param name="delta" value="0.05"/>
    <param name="llsamplerange" value="0.01"/>
    <param name="llsamplestep" value="0.01"/>
    <param name="lasamplerange" value="0.005"/>
    <param name="lasamplestep" value="0.005"/>
  </node>
</launch>

仿真看看 

export TURTLEBOT3_MODEL=waffle
roslaunch turtlebot3_gazebo turtlebot3_world.launch

再运行 

export TURTLEBOT3_MODEL=waffle
roslaunch turtlebot3_slam turtlebot3_slam.launch

然后查看rqt_graph

可以看到此处有两个tf的数据

查看现有的topic 

/camera/depth/camera_info
/camera/depth/image_raw
/camera/depth/points
/camera/parameter_descriptions
/camera/parameter_updates
/camera/rgb/camera_info
/camera/rgb/image_raw
/camera/rgb/image_raw/compressed
/camera/rgb/image_raw/compressed/parameter_descriptions
/camera/rgb/image_raw/compressed/parameter_updates
/camera/rgb/image_raw/compressedDepth
/camera/rgb/image_raw/compressedDepth/parameter_descriptions
/camera/rgb/image_raw/compressedDepth/parameter_updates
/camera/rgb/image_raw/theora
/camera/rgb/image_raw/theora/parameter_descriptions
/camera/rgb/image_raw/theora/parameter_updates
/clicked_point
/clock
/cmd_vel
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/imu
/initialpose
/joint_states
/map
/map_metadata
/map_updates
/move_base_simple/goal
/odom
/rosout
/rosout_agg
/scan
/statistics
/tb_0/cmd_vel
/tb_1/cmd_vel
/tb_2/cmd_vel
/tf
/tf_static
/turtlebot3_slam_gmapping/entropy

查看节点 

rosnode info /turtlebot3_slam_gmapping

Node [/turtlebot3_slam_gmapping]
Publications: 
 * /map [nav_msgs/OccupancyGrid]
 * /map_metadata [nav_msgs/MapMetaData]
 * /rosout [rosgraph_msgs/Log]
 * /tf [tf2_msgs/TFMessage]
 * /turtlebot3_slam_gmapping/entropy [std_msgs/Float64]

Subscriptions: 
 * /clock [rosgraph_msgs/Clock]
 * /scan [sensor_msgs/LaserScan]
 * /tf [tf2_msgs/TFMessage]
 * /tf_static [tf2_msgs/TFMessage]

Services: 
 * /dynamic_map
 * /turtlebot3_slam_gmapping/get_loggers
 * /turtlebot3_slam_gmapping/set_logger_level


contacting node http://10.79.138.249:35249/ ...
Pid: 4690
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound (58405 - 10.79.138.249:34112) [19]
    * transport: TCPROS
 * topic: /tf
    * to: /turtlebot3_slam_gmapping
    * direction: outbound
    * transport: INTRAPROCESS
 * topic: /tf
    * to: /rviz
    * direction: outbound (58405 - 10.79.138.249:34136) [10]
    * transport: TCPROS
 * topic: /map
    * to: /rviz
    * direction: outbound (58405 - 10.79.138.249:34140) [11]
    * transport: TCPROS
 * topic: /clock
    * to: /gazebo (http://10.79.138.249:42589/)
    * direction: inbound (51256 - 10.79.138.249:49873) [20]
    * transport: TCPROS
 * topic: /tf
    * to: /turtlebot3_slam_gmapping (http://10.79.138.249:35249/)
    * direction: inbound
    * transport: INTRAPROCESS
 * topic: /tf
    * to: /gazebo (http://10.79.138.249:42589/)
    * direction: inbound (51262 - 10.79.138.249:49873) [21]
    * transport: TCPROS
 * topic: /tf
    * to: /robot_state_publisher (http://10.79.138.249:40355/)
    * direction: inbound (40938 - 10.79.138.249:37293) [22]
    * transport: TCPROS
 * topic: /tf_static
    * to: /robot_state_publisher (http://10.79.138.249:40355/)
    * direction: inbound (40940 - 10.79.138.249:37293) [23]
    * transport: TCPROS
 * topic: /scan
    * to: /gazebo (http://10.79.138.249:42589/)
    * direction: inbound (51270 - 10.79.138.249:49873) [24]
    * transport: TCPROS

对于关键的实现点，建议查看gmapping代码

里程计误差及修正

目前ROS中常用的里程计广义上包括车轮上的光电码盘、惯性导航元件（IMU）、视觉里程计，你可以只用其中的一个作为odom，也可以选择多个进行数据融合，融合结果作为odom。通常来说，实际ROS项目中的里程计会发布两个Topic：


  • /odom

    : 类型为

    nav_msgs/Odometry

    ，反映里程计估测的机器人位置、方向、线速度、角速度信息。

  • /tf

    : 主要是输出

    odom_frame

    和

    base_frame

    之间的tf。这段tf反映了机器人的位置和方向变换，数值与

    /odom

    中的相同。

由于以上三种里程计都是对机器人的位姿进行估计，存在着累计误差，因此当运动时间较长时，

odom_frame

和

base_frame

之间变换的真实值与估计值的误差会越来越大。你可能会想，能否用激光雷达数据来修正

odom_frame

和

base_frame

的tf。事实上gmapping不是这么做的，里程计估计的是多少，

odom_frame

和

base_frame

的tf就显示多少，永远不会去修正这段tf。gmapping的做法是把里程计误差的修正发布到

map_frame

和

odom_frame

之间的tf上，也就是把误差补偿在了地图坐标系和里程计原点坐标系之间。通过这种方式来修正定位。（而关于gmapping中如何修正里程计的误差，则需要更多深入的查看代码了～）

这样

map_frame

和

base_frame

，甚至和

laser_frame

之间就连通了，实现了机器人在地图上的定位。

TF消息

格式规范

在上面的描述中，存在一个很关键的点，就是tf消息。在每个frame之间都会有broadcaster来发布消息维系坐标转换。这个消息TransformStampde.msg,它就是处理两个frame之间一小段tf的数据格式．

TransformStamped.msg的格式规范如下： 

std_mags/Header header
        uint32 seq
        time stamp
        string frame_id
string child_frame_id
geometry_msgs/Transform transform
        geometry_msgs/Vector3 translation
                float64 x
                float64 y
                float64 z
        geometry_msgs/Quaternion rotation
                float64 x
                float64 y
                flaot64 z
                float64 w

由上可见，header定义了序号，时间以及frame的名称．接着还写了child_frame，这两个frame之间要做那种变换就是由geometry_msgs/Transform来定义．

Vector3三维向量表示平移，Quaternion四元数表示旋转．像下图TF树中的两个frame之间的消息，就是由这种格式来定义的．odom就是frame_id,baselink_footprint就是child_frame_id.我们知道，一个topic上面，可能会有很多个node向上面发送消息。如图所示，不仅有我们看到的frame发送坐标变换个tf，还有别的frame也在同样的向它发送消息。最终，许多的TransformStamped.msg发向tf，形成了TF树。

故此gmapping中的odom的数据就是发布在了odom frame上，其对应着map frame，相对map frame的变换

TF树的数据类型

上面我们讲了，TF tree是由很多的frame之间TF拼接而成。那么TF tree是什么类型呢？如下：

  • tf/tfMessage.msg
  • tf2_msgs/TFMessage.msg

这里TF的数据类型有两个，主要的原因是版本的迭代。自ROS Hydro以来，tf第一代已被“弃用”，转而支持tf2。tf2相比tf更加简单高效。此外也添加了一些新的功能。

由于tf2是一个重大的变化，tf API一直保持现有的形式。由于tf2具有tf特性的超集和一部分依赖关系，所以tf实现已经被移除，并被引用到tf2下。这意味着所有用户都将与tf2兼容。官网建议新工作直接使用tf2，因为它有一个更清洁的界面，和更好的使用体验。

如何查看自己使用的TF是哪一个版本，使用命令

rostopic info /tf

即可。（其实通过上面看slam_gmapping的节点可以知道我们采用是tf2）

tf/tfMessage.msg或tf2_msgs/TFMessage标准格式规范如下: 

geometry_msgs/TransformStamped[] transforms
        std_msgs/Header header
                uint32 seq
                time stamp
                string frame_id
        string child_frame_id
        geometry_msgs/Transform transform
                geometry_msgs/Vector3 translation
                        float64 x
                        float64 y
                        float64 z
                geometry_msgs/Quaternion rotation
                        float64 x
                        float64 y
                        flaot64 z
                        float64 w

一个TransformStamped数组就是一个TF tree。

amcl定位


http://wiki.ros.org/amcl

源代码：

https://github.com/ros-planning/navigation/tree/noetic-devel/amcl

amcl的英文全称是adaptive Monte Carlo localization，其实就是蒙特卡洛定位方法的一种升级版。而mcl（蒙特卡洛定位）法使用的是粒子滤波的方法来进行定位的。而粒子滤波很粗浅的说就是一开始在地图空间很均匀的撒一把粒子，然后通过获取机器人的motion来移动粒子，比如机器人向前移动了一米，所有的粒子也就向前移动一米，不管现在这个粒子的位置对不对。使用每个粒子所处位置模拟一个传感器信息跟观察到的传感器信息（一般是激光）作对比，从而赋给每个粒子一个概率。之后根据生成的概率来重新生成粒子，概率越高的生成的概率越大。这样的迭代之后，所有的粒子会慢慢地收敛到一起，机器人的确切位置也就被推算出来了。

amcl包将基于laser的地图，laser scans，transform消息，作为输入，输出预测的pose

上面gmapping运行完后，保存了地图，即可通过下面的命令来运行 

export TURTLEBOT3_MODEL=waffle

roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map/simulatuion_map.yaml

一开始先预测位置，给一个pose estimate，可以看到此处可视化的amcl的点是比较散乱的

但是随着运动，就会发现点慢慢更加的密集，机器人通过slam定位对自己的位置的确信度提高了

如果没有做初始的pose估计会怎么样呢？见下图


会发现amcl虽然会收敛，但是最终定位的结果是不对的。

感觉原因在于：

1、一开始撒粒子的位置。如果一开始粒子布于整张地图，这样来看，还是有机会可以不初始化就可以定位到的，只是时间久一些。

2、特征差异性。在实验中可以发现，因为场景是对称的，往左跟往上走的方向遇到的特征很像。

3、amcl中的初始位置，并不需要多精准，只需要大概的，这样粒子也分布于实际位置的附近，再通过amcl来提供高准确度定位

接下来看看rqt

可以看到amcl输入中包含了激光雷达的数据

看看amcl节点的信息 

^Ckwanwaipang@kwanwaipang:~$ 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]

Subscriptions: 
 * /clock [rosgraph_msgs/Clock]
 * /initialpose [geometry_msgs/PoseWithCovarianceStamped]
 * /scan [sensor_msgs/LaserScan]
 * /tf [tf2_msgs/TFMessage]
 * /tf_static [tf2_msgs/TFMessage]

Services: 
 * /amcl/get_loggers
 * /amcl/set_logger_level
 * /amcl/set_parameters
 * /global_localization
 * /request_nomotion_update
 * /set_map


contacting node http://10.79.138.249:46809/ ...
Pid: 11378
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound (46153 - 10.79.138.249:33684) [10]
    * transport: TCPROS
 * topic: /tf
    * to: /amcl
    * direction: outbound
    * transport: INTRAPROCESS
 * topic: /tf
    * to: /move_base
    * direction: outbound (46153 - 10.79.138.249:33718) [11]
    * transport: TCPROS
 * topic: /tf
    * to: /rviz
    * direction: outbound (46153 - 10.79.138.249:33742) [14]
    * transport: TCPROS
 * topic: /particlecloud
    * to: /rviz
    * direction: outbound (46153 - 10.79.138.249:33792) [16]
    * transport: TCPROS
 * topic: /clock
    * to: /gazebo (http://10.79.138.249:38437/)
    * direction: inbound (50248 - 10.79.138.249:48561) [17]
    * transport: TCPROS
 * topic: /tf
    * to: /amcl (http://10.79.138.249:46809/)
    * direction: inbound
    * transport: INTRAPROCESS
 * topic: /tf
    * to: /gazebo (http://10.79.138.249:38437/)
    * direction: inbound (50258 - 10.79.138.249:48561) [18]
    * transport: TCPROS
 * topic: /tf
    * to: /robot_state_publisher (http://10.79.138.249:40505/)
    * direction: inbound (56252 - 10.79.138.249:48215) [19]
    * transport: TCPROS
 * topic: /tf_static
    * to: /robot_state_publisher (http://10.79.138.249:40505/)
    * direction: inbound (56256 - 10.79.138.249:48215) [20]
    * transport: TCPROS
 * topic: /scan
    * to: /gazebo (http://10.79.138.249:38437/)
    * direction: inbound (50270 - 10.79.138.249:48561) [21]
    * transport: TCPROS
 * topic: /initialpose
    * to: /rviz (http://10.79.138.249:37941/)
    * direction: inbound (33284 - 10.79.138.249:55799) [15]
    * transport: TCPROS

订阅主题


  1. scan (sensor_msgs/LaserScan)


    Laser scans.

  2. tf (tf/tfMessage)


    Transforms.

  3. initialpose (geometry_msgs/PoseWithCovarianceStamped)


    Mean and covariance with which to (re-)initialize the particle filter.初始的位置

  4. map (nav_msgs/OccupancyGrid)


    When the use_map_topic parameter is set, AMCL subscribes to this topic to retrieve the map used for laser-based localization.

    实际上初始位姿可以通过参数提供也可以使用默认初始值，我们主要是要将scan（激光）、tf和map主题提供给amcl。

发布主题


  1. amcl_pose (geometry_msgs/PoseWithCovarianceStamped)


    Robot’s estimated pose in the map, with covariance.

  2. particlecloud (geometry_msgs/PoseArray)


    The set of pose estimates being maintained by the filter.（粒子滤波输出的一系列结果）

  3. tf (tf/tfMessage)


    Publishes the transform from odom (which can be remapped via the ~odom_frame_id parameter) to map.

    如果纯粹是为了显示一下机器人的位姿（in rviz）我们只需要tf主题就够了。要在地图上显示机器人的位姿，是通过tf来显示的

通过下面命令也可以查看tf tree 

rosrun rqt_tf_tree rqt_tf_tree

参考资料


https://blog.csdn.net/chenxingwangzi/article/details/49802763


https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/chapter9/9.2.html


https://blog.csdn.net/chenxingwangzi/article/details/50038413


版权声明：本文为gwplovekimi原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
Tags: