ROS中阶笔记（七）：机器人SLAM与自主导航—SLAM功能包的使用

文章目录

1 机器人必备条件
- 1.1 硬件要求
- 1.2 深度信息
- - 1.2.1 激光雷达
  - 1.2.2 kinect
- 1.3 里程计信息
- 1.4 仿真环境
2 SLAM功能包的使用方法
- 2.1 gmapping
- - 2.1.1 gmapping 功能包
  - 2.1.2 栅格地图取值原理
  - 2.1.3 gmapping安装
  - 2.1.4 配置gmapping节点
  - 2.1.5 启动gmapping演示(激光雷达)
  - 2.1.6 启动gmapping(kinect)
- 2.2 hector_slam
- - 2.2.1 hector_slam功能包
  - 2.2.2 安装hector_slam
  - 2.2.3 配置hector_mapping节点
  - 2.2.4 启动hector_slam演示
- 2.3 cartographer
- - 2.3.1 cartographer功能包
  - 2.3.2 cartographer安装
  - 2.3.3 启动2D、3D 演示
  - 2.3.4 把cartographer功能包运行到自己的机器人上
  - 2.3.5 启动cartographer仿真
- 2.4 ORB_SLAM
- - 2.4.1 ORB_SLAM功能包
  - 2.4.2 ORB_SLAM2安装
  - 2.4.3 启动单目SLAM示例（基于数据包）
  - 2.4.4 启动AR示例（基于数据包）
  - 2.4.5 启动ORB_SLAM示例（真实摄像头）
  - 2.4.6 启动AR示例（真实摄像头）
3 参考资料

1 机器人必备条件

1.1 硬件要求

（1）差分轮式机器人，可使用twist速度指令控制

$ rosmsg show geometry_msgs/Twistgeometry_msgs/Vector3 linear   # linear：xyz方向上的线速度，单位是m/s;float64 xfloat64 yfloat64 z
geometry_msgs/Vector3 angular  # angular:xyz方向上的角速度，单位是rad/s。float64 xfloat64 yfloat64 z

（2）机器人必须安装激光雷达等测距设备，可以获取环境深度信息。
（3）最好使用正方形和圆形的机器人，其他外形的机器人虽然可以使用但是效果可能不佳。

1.2 深度信息

1.2.1 激光雷达

$ rosmsg show sensor_msgs/LaserScan           # 查看激光雷达消息结构std_msgs/Header headeruint32 seqtime stampstring frame_id
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensitiesangle_min：可检测范围的起始角度； （—180——180度 ）
angle_max：可检测范围的终止角度，与angle_min组成激光雷达的可检测范围；
angle_increment：相邻数据帧之间的角度步长；
time_incremen：采集到相邻数据帧之间的时间步长，当传感器处于相对运动状态时进行补偿使用。
scan_time：采集一帧数据所需要的视觉；
rang_min：最近可检测深度的阈值；
rang_max：最远可检测深度的阈值；
ranges：一帧深度数据的存储数组。
intensities：每个激光点的强度

1.2.2 kinect

Kinect等GRB-D摄像头，也可以通过红外摄像头获取周围环境的深度信息。
depthimage_to_laserscan功能包：将三维点云数据转换为二维激光雷达数据；

<!--depthimage_to_laserscan节点，将点云深度数据转换成激光数据-->
<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan" output="screen"><remap from="image" to="/kinect/depth/image_raw"/><remap from="camera_info" to=/kinect/depth/camera_info"/><remap froam="scan" to="/scan"/><param name="output_frame_id" value="/camera_link"/>
</node>

1.3 里程计信息

$ rosmsg show nav_msgs/odometrypose：机器人当前位置坐标，包括机器人的XYZ三轴位置与方向参数，以及用于校正误差的方差矩阵
twist：机器人当前的运动状态，包括XYZ三轴的线速度与角速度，以及用于校正误差的方差矩阵。
注意：ROS中所有的坐标系都是右手坐标系。

1.4 仿真环境

在视频中给出了一个仿真环境：

$ roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch     # 启动仿真环境# 使用Building Editor创建仿真环境cloister.world

2 SLAM功能包的使用方法

论文参考：https://openslam-org.github.io/gmapping.html

2.1 gmapping

2.1.1 gmapping 功能包

基于激光雷达
Rao-Blackwellized 粒子滤波算法
二维栅格地图
需要机器人提供里程计信息

OpenSlam开源算法
输出地图话题：nav_msgs/OccupancyGrid

$ rosmsg show nav_msgs/OccupancyGrid

2.1.2 栅格地图取值原理

2.1.3 gmapping安装

$ sudo apt-get install ros-kinetic-gmapping

2.1.4 配置gmapping节点

参考： http://wiki.ros.org/gmapping

catkin_ws/src/mbot_navigation/launch/gmapping.launch

<launch><arg name="scan_topic" default="scan" /><node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true"><param name="odom_frame" value="odom"/><param name="map_update_interval" value="5.0"/><!-- Set maxUrange < actual maximum range of the Laser --><param name="maxRange" value="5.0"/><param name="maxUrange" value="4.5"/><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="srr" value="0.01"/><param name="srt" value="0.02"/><param name="str" value="0.01"/><param name="stt" value="0.02"/><param name="linearUpdate" value="0.5"/><param name="angularUpdate" value="0.436"/><param name="temporalUpdate" value="-1.0"/><param name="resampleThreshold" value="0.5"/><param name="particles" value="80"/><param name="xmin" value="-1.0"/><param name="ymin" value="-1.0"/><param name="xmax" value="1.0"/><param name="ymax" value="1.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"/><remap from="scan" to="$(arg scan_topic)"/></node>
</launch>

2.1.5 启动gmapping演示(激光雷达)

分别开启三个终端运行以下命令：

$ roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch    # 启动仿真环境
$ roslaunch mbot_navigation gmapping_demo.launch  # 启动建图节点，灰色地图建成，黑色障碍物
$ roslaunch mbot_teleop mbot_teleop.launch        #启动键盘控制节点

建图完毕，保存地图

$ rosrun map_server map_saver -f cloister_gmapping  # cloister_gmapping是文件名的意思，是自己保存文件名的意思
# 保存的路径在当前/home文件夹下,有两个文件.pgm和.yaml

2.1.6 启动gmapping(kinect)

建图效果不佳（不推荐）

$ roslaunch mbot_gazebo mbot_kinect_nav_gazebo.launch
$ roslaunch mbot_navigation gmapping_demo.launch
$ roslaunch mbot_teleop mbot_teleop.launch

2.2 hector_slam

2.2.1 hector_slam功能包

基于激光雷达
高斯牛顿方法
二维栅格地图
不需要里程计数据
输出地图话题：
nav_msgs/OccupancyGrid

2.2.2 安装hector_slam

$ sudo apt-get install ros-kinetic-hector-slam

2.2.3 配置hector_mapping节点

参数说明可参考：http://wiki.ros.org/hector_slam

catkin_ws/src/mbot_navigation/launch/hector.launch

<launch><node pkg = "hector_mapping" type="hector_mapping" name="hector_mapping" output="screen"><!-- Frame names --><param name="pub_map_odom_transform" value="true"/><param name="map_frame" value="map" /><param name="base_frame" value="base_footprint" /><param name="odom_frame" value="odom" /><!-- Tf use --><param name="use_tf_scan_transformation" value="true"/><param name="use_tf_pose_start_estimate" value="false"/><!-- Map size / start point --><param name="map_resolution" value="0.05"/><param name="map_size" value="2048"/><param name="map_start_x" value="0.5"/><param name="map_start_y" value="0.5" /><param name="laser_z_min_value" value = "-1.0" /><param name="laser_z_max_value" value = "1.0" /><param name="map_multi_res_levels" value="2" /><param name="map_pub_period" value="2" /><param name="laser_min_dist" value="0.4" /><param name="laser_max_dist" value="5.5" /><param name="output_timing" value="false" /><param name="pub_map_scanmatch_transform" value="true" /><!-- Map update parameters --><param name="update_factor_free" value="0.4"/><param name="update_factor_occupied" value="0.7" />    <param name="map_update_distance_thresh" value="0.2"/><param name="map_update_angle_thresh" value="0.06" /><!-- Advertising config --> <param name="advertise_map_service" value="true"/><param name="scan_subscriber_queue_size" value="5"/><param name="scan_topic" value="scan"/></node></launch>

2.2.4 启动hector_slam演示

分别开启三个终端运行以下命令：

$ roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch     # 动一个gazebo仿真节点
$ roslaunch mbot_navigation hector_demo.launch     # hector建图节点
$ roslaunch mbot_teleop mbot_teleop.launch          # 启动一个键盘控制节点

第一个节点启动了gazebo仿真，它使用仿真的雷达给出了一个scan扫描数据；而后下面的节点订阅了这个数据并进行处理，最后得到一个二维栅格地图。

hector这个算法有一个很大的问题，就是在于它的自定位，它利用自身的激光雷达数据进行定位，如果上一次的扫描数据与下一次的扫描数据基本一致的话，它就很难确定自己是否在运动，例如当我给一个足够大的空间，这里有一条没有任何特征点的长廊，长度远远超过激光扫描的最大距离时，hector基本就失去了建图的能力。

2.3 cartographer

2.3.1 cartographer功能包

2016年10月5日，谷歌开源
基于图网络的优化方法
二维或三维条件下的定位及建图功能
设计目的是在计算资源有限的情况下，实时获取相对较高精度的2D地图
主要基于激光雷达
后续会支持更多传感器和机器人平台，同时不断增加新的功能。

2.3.2 cartographer安装

方法一：（新方法）

sudo apt-get updatesudo apt-get install ros-<your ros version>-cartographer*  # 安装全部关于cartographer的包

方法二：原来的方法（有好处）

cartographer功能功能包没有集成到ROS的软件源里面，所以需要采用源码编译方式进行安装。为了不与其他安装包冲突，最好为cartographer专门创建一个工作空间，这里我们创建了一个工作空间catkin_google_ws：

1、安装工具：

$ sudo apt-get update
$ sudo apt-get install -y python-wstool python-rosdep ninja-build

2、初始化工作空间：

$ mkdir google_ws            # 创建工作空间，名称为google_ws
$ cd google_ws
$ wstool init src

3、设置下载地址（挂VPN最好）

在新建的工作空间下输入下边指令：

# 如果这一步错误，见下面方法一、方法二
$ wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall                 $ wstool update -t src

说明：配置Cartographer需要三个包，分别是：Cartographer、Cartographer_ros 和ceres-solver。

方法一：

# 如果无法从google取出代码可以从github提取code：
# 在~/google_ws/src下目录下面ctrl+h打开隐藏文件夹，修改里面第三个包的下载地址： https://github.com/ceres-solver/ceres-solver.git

方法二：

# 输入完第一个命令后需要改文件的下载地址，另起终端输入：$ gedit google_ws/src/.rosinstall# 将最后一个git来源网址由https://ceres-solver.googlesource.com/ceres-solver.git改为https://github.com/ceres-solver/ceres-solver.git       如该果可以挂VPN可以不改。

4、安装功能包依赖

# 安装  proto3.sh
进入protobuf的release版本下载页，link：https://github.com/protocolbuffers/protobuf/releases
# 在google_ws目录下
# protobuf的下载太慢，可以在官网手动下载好，放到cartographer目录下(和src同级，具体操作如下)$ src/cartographer/scripts/install_proto3.sh# 安装 deb 依赖，出现错误可以忽略
$ sudo rosdep init$ rosdep update
$ rosdep install --from-paths src --ignore-src --rosdistro=${kinetic} -y

5、编译功能包

$ catkin_make_isolated --install --use-ninja          # 在google_ws目录下，编译

6、在当前终端，设置环境变量

source install_isolated/setup.bash  #设置环境变量，在当前终端中设置环境变量，只能在当前终端查找

建议把该句source命令直接写入系统的bashrc环境里

$ vim ./bashrc
source install_isolated/setup.bash           # 然后把该句source命令添加到最后一行即可

2.3.3 启动2D、3D 演示

# Download the 2D backpack example bag.
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag# Launch the 2D backpack demo.
roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag# Download the 3D backpack example bag.
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/with_intensities/b3-2016-04-05-14-14-00.bag# Launch the 3D backpack demo.
roslaunch cartographer_ros demo_backpack_3d.launch bag_filename:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag

启动Revo LDS demo演示

启动PR2 demo填示

wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/pr2/2011-09-15-08-32-46.bagroslaunch cartographer_ros demo_pr2.launch bag_filename:=~/Downloads/2011-09-15-08-32-46.bag

2.3.4 把cartographer功能包运行到自己的机器人上

由于传感器的rostopic不同，我们需要对cartographer订阅的名称进行修改，需要在launch文件中进行修改，但是修改后发现没有任何作用，这是因为修改后需要重新按照上面的流程进行编译安装，这个可能是因为在安装的时候其实已经把launch文件安装到系统中了，所以我们调用的时候还是调用系统中的launch文件

1、配置cartographer节点
mbot_navigation/launch/cartographer_demo_rplidar.launch

2、参数配置
mbot_navigation/config/rplidar.lua

2.3.5 启动cartographer仿真

catkin_make_isolated --install--use-ninja
roslaunch mbot_ gazebo mbot_laser_nav_gazebo.launch
roslaunch cartographer_ros cartographer_demo_rplidar.launch
roslaunch mbot_teleop mbot_teleop.launch

2.4 ORB_SLAM

2.4.1 ORB_SLAM功能包

基于特征点的实时单目SLAM系统
实时解算摄像机的移动轨迹
构建三维点云地图
不仅适用于手持设备获取的一组连续图像，也可以应用于汽车行驶过程中获取的连续图像

Raul Mur-Artal，J.M.M.Montiel和Juan D.Tardos于2015年发表在IEEE Transactions on Robotics上

2.4.2 ORB_SLAM2安装

1、安装工具&下载源码：

sudo apt-get install libboost-all-dev libblas-dev liblapack-dev
git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2       # 克隆到home下

2、安装eigen3.2.10

去官网下载：http://eigen.tuxfamily.org/index.php?title=Main_Page解压源码包，并进入目录：mkdir buildcd buildcmake ..makesudo make install

3、编译g2o

cd ~/ORB_SLAM2/Thirdparty/g2o/mkdir buildcd buildcmake .. -DCMAKE_BUILD_TYPE=Releasemake

4、编译DBoW2

cd ~/ORB_SLAM2/Thirdparty/DBoW2/mkdir build && cd build cmake .. -DCMAKE_BUILD_TYPE=Releasemake

5、编译Pangolin

sudo apt-get install libglew-dev git clone https://github.com/stevenlovegrove/Pangolin.gitcd Pangolinmkdir build && cd build cmake ..cmake --build .

6、编译ORM_SLAM

cd ~/ORB_SLAM2mkdir build && cd build cmake .. -DCMAKE_BUILD_TYPE=Releasemake

7、编译功能包（配置ROS环境）

# 首先修改.bashrc文件
$ cd ~
$ gedit .bashrc# 打开.bashrc文件在最后一行加入
source ~/ORB_SLAM2/Examples/ROS/ORB_SLAM2/build/devel/setup.bash$ source ~/.bashrc

$ export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/ggk/ORB_SLAM2/Examples/ROS
$ cd ~/ORB_SLAM2$ chmod +x build_ros.sh
$ ./build_ros.sh

第7步，编译功能包出现的问题：

解决方法：

修改 ~/ORB_SLAM2/Examples/ROS/ORB_SLAM2/CMakeLists.txt,添加 -lboost_system

2.4.3 启动单目SLAM示例（基于数据包）

KITTI：http://www.cvlibs.net/datasets/kitti/eval_odometry.php，是室外，双目

1、首先下载数据包TUM：这个主要是室内，单目和RGB-D

https://vision.in.tum.de/data/datasets/rgbd-dataset/download#freiburg1_desk

在ORB-SLAM2下新建文件夹Data，把测试的数据解压在这里。
TUM数据集分为相机fr1，fr2，fr3，对应TUM1-3.yaml；
一般第一次测试用fr1/xyz这个数据集，这个就是x，y，z方向来回动，用来检测一下系统出没出什么问题。
其他的数据看名字就知道，比如desk就是在桌子附近来回转，room就是在房间里面扫来扫去。
值得注意的是，运行其他数据集的时候，单目不一定能追踪成功，在台式机上能成功的在虚拟机上也不一定能成功，这就需要我们进行一些调整，比如调整初始化需求数量等，这个关系到对SLAM系统的理解。

2、分别打开三个终端运行下面的三个命令：

$ roscore# 进入ORB_SLAM2目录下启动Mono功能节点。
# 看到开启的等待数据的可视化建图界面
$ rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml# 进入数据包所在目录下，运行命令
# /camera/image_raw表示播放的数据要发布的话题名称，此时可以在界面中看到建图的效果
$ rosbag play rgbd_dataset_freiburg1_desk.bag /camera/rgb/image_color:=/camera/image_raw

PATH_TO_VOCABULARY：算法参数文件，在ORB_SLAM2/Vocabulary中，将其中的压缩包解压即可；

PATH_TO_SETTINGS_FILE：相机参数设置文件，需要对camera进行标定产生，也可以使用ORB_SLAM2/Examples/ROS/ORB_SLAM2中已有的设置文件Asus.yaml。

2.4.4 启动AR示例（基于数据包）

$ roscore# 进入ORB_SLAM2目录下启动Mono功能节点。
# 看到开启的等待数据的可视化建图界面
$ rosrun ORB_SLAM2 MonoAR Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml# 进入数据包所在目录下，运行命令
# /camera/image_raw表示播放的数据要发布的话题名称，此时可以在界面中看到建图的效果
$ rosbag play rgbd_dataset_freiburg1_desk.bag /camera/rgb/image_color:=/camera/image_raw

点击界面Insert Cube，看到正方形插入进来，展示AR效果

2.4.5 启动ORB_SLAM示例（真实摄像头）

$ roslaunch mbot_navigation usb_cam_remap.launch# 进入ORB_SLAM2目录下启动Mono功能节点。
# 看到开启的等待数据的可视化建图界面
$ rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml

2.4.6 启动AR示例（真实摄像头）

摄像头要运动起来，来完成地图构建

$ roslaunch mbot_vision usb_cam_remap.launch# 进入ORB_SLAM2目录下启动Mono功能节点。
# 看到开启的等待数据的可视化建图界面
$ rosrun ORB_SLAM2 MonoAR Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml

3 参考资料

2.3 参考资料

Compiling Cartographer ROS

https://google-cartographer-ros.readthedocs.io/en/latest/compilation.html#building-installation

ROS slam-google cartographer编译和demo运行

https://blog.csdn.net/x_r_su/article/details/52927564

Google Cartographer安装教程

https://blog.csdn.net/xmy306538517/article/details/81455625

ubuntu18.04配置cartographer

https://blog.csdn.net/SimileciWH/article/details/82939752

ROS中阶笔记（七）：机器人SLAM与自主导航—SLAM功能包的使用相关推荐

ROS中阶笔记（八）：机器人SLAM与自主导航—机器人自主导航
ROS中阶笔记(八):机器人SLAM与自主导航-机器人自主导航文章目录 1 ROS中的导航框架 1.1 move_base 1.1.1 基于move_base的导航框架 1.1.2 安装 1.2.3 ...
ROS中阶笔记（十）：ROS机器人综合应用
ROS中阶笔记(十):ROS机器人综合应用文章目录 1 ROS机器人综合应用 1.1 PR2 1.2 PR2实践 1.3 TurtleBot 1.3.1 TurtleBot2实践 1.3.2 Tur ...
ROS中阶笔记（六）：机器人感知—机器语音
ROS中阶笔记(六):机器人感知-机器语音文章目录 1 语音识别理论 2 常用语音功能包 3 科大讯飞SDK 3.1 使用前提 3.2 语音听写 3.3 语音合成 3.4 智能语音助手 4 参考资料 ...
ROS中阶笔记（五）：机器人感知—机器视觉
ROS中阶笔记(五):机器人感知-机器视觉文章目录 1 ROS中的图像数据 1.1 二维图像 1.1.1 安装安装usb_cam 1.1.2 显示图像数据 1.2 三维图像(kinect) 2 摄像 ...
ROS中阶笔记（四）：机器人仿真—Gazebo物理仿真环境搭建（重点）
ROS中阶笔记(四):机器人仿真-Gazebo物理仿真环境搭建(重点) 文章目录 1 ros_control 1.1 ros_control安装 2 Gazebo仿真步骤(重点) 2.1 配置物理仿真 ...
ROS中阶笔记（三）：机器人仿真—ArbotiX+rviz功能仿真
ROS中阶笔记(三):机器人仿真-ArbotiX+rviz功能仿真文章目录 01 机器人URDF模型优化-xacro模型文件 1.1 xacro模型文件 1.2 xacro使用方法 1.3 模型显示 ...
ROS中阶笔记（二）：机器人系统设计—URDF机器人建模
ROS中阶笔记(二):机器人系统设计-URDF机器人建模文章目录 01 URDF理论 1.1 什么是URDF 1.2 模型< robot > 1.2.1 < link > e ...
ROS中阶笔记（一）：机器人系统设计—ROS系统下连接外部传感器
ROS中阶笔记(一):机器人系统设计-ROS系统下连接外部传感器文章目录 01 连接摄像头 1.1 ROS下连接usb_cam 1.1.1 前期准备 1.1.2 问题 1.1.3 安装usb_cam ...
ROS中阶笔记（十一）：ROS 2.0
ROS中阶笔记(十一):ROS 2.0 文章目录 1 为什要有ROS 2 1.1 ROS 1的特点 1.2 ROS 1的困境 1.3 ROS 2的曙光 1.4 ROS 2的版本进化 1.5 ROS 2 ...