赞
踩
之前博文《ROS学习笔记之——Navigation Stack》已经学习过了ROS中的导航包了,本博文来学习一下amcl定位与gmapping
目录
在导航中,不可缺少的便是地图与定位。需要知道机器人当前的位置坐标(定位),然后根据已经建立好的地图,通过路径规划来规划出合理的路径
gmapping的作用是根据激光雷达和里程计(Odometry)的信息,对环境地图进行构建,并且对自身状态进行估计。因此它得输入应当包括激光雷达和里程计的数据,而输出应当有自身位置和地图。
源码的地址: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消息。在每个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 tree是由很多的frame之间TF拼接而成。那么TF tree是什么类型呢?如下:
这里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。
源代码: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

通过下面命令也可以查看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
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。