机器人SLAM与自主导航(三)——SLAM功能

目录

    • 一、gmapping
      • 1、gmapping功能包
      • 2、栅格地图取值原理
      • 3、gmapping安装
      • 4、配置gmapping节点
      • 5、启动gmapping演示(深度信息基于激光雷达)
      • 6、启动gmapping(kinect)
    • 二、hector_slam
      • 1、hector_slam功能包
      • 2、安装hector_slam
      • 3、配置hector_mapping节点
      • 4、启动hector_slam演示(深度信息基于激光雷达)
      • 5、打滑现象
    • 三、cartographer
      • 1、cartographer功能包
      • 2、cartographer安装
      • 3、配置cartographer节点
      • 4、启动2D slam demo演示
      • 5、启动3D slam demo演示
      • 6、参数配置
      • 7、启动cartographer仿真
    • 四、Karto_slam
      • 1、Karto_slam概况
      • 2、Karto_slam具体介绍
      • 3、karto_slam总结
    • 五、ORB_SLAM
      • 1、ORB_SLAM功能包
      • 2、ORB_SLAM2安装
      • 3、启动单目SLAM示例(基于数据包)
      • 4、启动AR示例(基于数据包)
      • 5、启动ORB_SLAM示例(真实摄像头)
      • 6、启动AR示例(真实摄像头)

一、gmapping

1、gmapping功能包

机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
输入:1.深度信息 2.IMU信息 3.里程计信息
输出:栅格地图

$ rosmsg show nav_msgs/OccupancyGrid 

2、栅格地图取值原理

机器人SLAM与自主导航(三)——SLAM功能包-编程知识网

3、gmapping安装

$ sudo apt-get install ros-kinetic-gmapping

机器人SLAM与自主导航(三)——SLAM功能包-编程知识网

4、配置gmapping节点

参数说明可参考: http://wiki.ros.org/gmapping
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网

5、启动gmapping演示(深度信息基于激光雷达)

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

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

机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
建图完毕,保存地图

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

机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
查看生成地图文件
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网

6、启动gmapping(kinect)

建图效果不佳(不推荐)

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

二、hector_slam

1、hector_slam功能包

机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
输入:深度信息
输出:栅格地图

2、安装hector_slam

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

机器人SLAM与自主导航(三)——SLAM功能包-编程知识网

3、配置hector_mapping节点

参数说明可参考:http://wiki.ros.org/hector_slam
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网

4、启动hector_slam演示(深度信息基于激光雷达)

机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
第一个节点启动了gazebo仿真,它使用仿真的雷达给出了一个scan扫描数据;而后下面的节点订阅了这个数据并进行处理,最后得到一个二维栅格地图。

5、打滑现象

hector_slam功能包使用时要求机器人移速不能过快,否则运算跟不上会出现打滑
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
hector这个算法有一个很大的问题,就是在于它的自定位,它利用自身的激光雷达数据进行定位,如果上一次的扫描数据与下一次的扫描数据基本一致的话,它就很难确定自己是否在运动,例如当我给一个足够大的空间,这里有一条没有任何特征点的长廊,长度远远超过激光扫描的最大距离时,hector基本就失去了建图的能力。

三、cartographer

1、cartographer功能包

机器人SLAM与自主导航(三)——SLAM功能包-编程知识网

2、cartographer安装

方法一(新方法):

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

方法二:
cartographer功能功能包没有集成到ROS的软件源里面,所以需要采用源码编译方式进行安装。为了不与其他安装包冲突,最好为cartographer专门创建一个工作空间,这里我们创建了一个工作空间catkin_google_ws:
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
cartographer功能包可能出现的问题
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网

3、配置cartographer节点

机器人SLAM与自主导航(三)——SLAM功能包-编程知识网

4、启动2D slam demo演示

机器人SLAM与自主导航(三)——SLAM功能包-编程知识网

5、启动3D slam demo演示

机器人SLAM与自主导航(三)——SLAM功能包-编程知识网

6、参数配置

机器人SLAM与自主导航(三)——SLAM功能包-编程知识网

7、启动cartographer仿真

机器人SLAM与自主导航(三)——SLAM功能包-编程知识网

四、Karto_slam

1、Karto_slam概况

机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
评价:
Karto_SLAM是基于图优化的思想,用高度优化和非迭代 cholesky分解进行稀疏系统解耦作为解。图优化方法利用图的均值表示地图,每个节点表示机器人轨迹的一个位置点和传感器测量数据集,每个新节点加入,就会进行计算更新。
Karto_SLAM的ROS版本,其中采用的稀疏点调整(the Spare Pose Adjustment(SPA))与扫描匹配和闭环检测相关。landmark越多,内存需求越大,然而图优化方式相比其他方法在大环境下制图优势更大,因为他仅包含点的图(robot pose),求得位姿后再求map。

2、Karto_slam具体介绍

整体程序框架
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
从上图中可以看出,其实流程还是相当简单的,slam传统软实时的运行机制,每进入一帧数据,即进行处理,然后返回。
1)扫描匹配
关键词:
兴趣区域:
匹配的方式是scanTomap,兴趣区域就是矩形形状的submap,也可以将这块区域理解为参考模型。
搜索区域:
以里程计估计的位置为中心的一个矩形区域,用以表示最终位置的可能范围,在匹配时,遍历搜索区域,获取响应值最高的位置。
查找表:
对于激光获得的数据信息,以一定的角分辨率和角偏移值进行投影,获取查找表,用以匹配。
Running-scans:
实时维护的局部激光数据链,首末两帧距离在一定距离范围内,且满足一定数据规模,否则需要删除末端数据帧。维护当前局部数据链。
生成submap(因为匹配的方式是ScanToMap)
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
生成查找表:
查找表存在的意义就是相比于暴力匹配,不要每次都重新计算每个激光数据信息,相同角度不同位置的激光数据信息只需要被索引一次。
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
由里程计预测可以得到当前预估的姿态角,真实的姿态角必定在附近;
以一定的分辨率和偏移值对原机器人坐标系下(局部坐标系)表示的激光信息进行不同角度的映射,获得一个包含n个角度的查找表。
其中:
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
查找表->submap(匹配)
移动机器人自身的状态包括 (x,y,θ),通过查找表的方式有效解决了角度的问题,眼下仍需解决位置的问题,采用离散化搜索区域进行定位,并利用多分辨率提高搜索速度。
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
响应值的计算如下描述:将查找表以一定的为宜投到submap上,此时的submap已有running-scans生成,并利用高斯进行模糊,假设总共有n个点被查找表击中,击中的每个点得分不同(高斯模糊的作用),累加得分并除以可以达到的最高得分。
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
查找表->submap(粗匹配->精匹配)
为提高搜索效率,采用多分辨率的方式,即粗匹配时采用较低的分辨率搜索得到候选区域,再对候选区域进行划分得到精确求解。
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
计算节点均值
在每次匹配过程中(粗匹配和精匹配),选取几个拥有最优响应值的位姿状态,取平均值作为匹配结果。
最优响应值为一个,设立一个10^-6作为容忍度,一旦有其他位姿状态的响应值与最优响应值在容忍度内,即为相等;最终的节点为它们的平均值。
迭代:
粗匹配(获得均值)->(作为初值)精匹配(得到最终均值)
计算节点协方差——位置协方差(粗匹配)
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
计算节点协方差——角度协方差(粗匹配)
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
上述协方差存在和理论相背的情况,主要每次计算的分量需要乘上一个响应值。
2)添加顶点和边
添加顶点:即关键帧的位姿
添 加 边: 当前帧与前一帧、running-scans、near-chains建立连接。
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
添加边主要经过三个步骤:
(1)Link to previsous scan
(2)Link to Running scans
RunningScan chain:一定数量且距当前一定距离内的激光数据链。
(3)Link to other near chains
NearChain: 以当前节点开始广度优先的方式从graph中遍历相邻的一定距离范围内所有节点,依据当前id从sensorManager中分别递增与递减寻找一定范围内的chain, 生成nearLinkScans。
前两个步骤都好理解,主要是第三个,以当前节点为搜索中心,以一定范围(比如说4m)进行广度搜索,得到了所有关联的节点,然后利用节点生成数据链(数据链需要包含约5个ID连续的关键帧)(不包括当前结点,否则放弃该数据链),用当前节点进行匹配,若响应值达到一定阈值,产生一条边,这条边一端是当前节点,还有一端是数据链中质心距离当前节点质心最近的节点。
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
3)回环检测
回环检测的操作和添加邻近边类似,步骤较为繁琐:
(1)依据当前的节点, 从Graph中找到与之相邻的所有节点(一定距离范围内)。
(2)采取广度优先搜索的方式,将相邻(next)与相连(adjacentVertices)添加进nearLinkedScans。
(3)从sensorManager中取从前到后,依据id序号挑选与当前在一定距离范围内,且不在nearLinkedScans中的candidateScans, 当数量达到一定size,返回。
(4)loopScanMatcher进行scanTomap的匹配,当匹配response 和covariance达到一定要求认为闭环检测到。得到调整的correct pose。
(5)Add link to loop : 调整边(全局闭环)。
(6)触发correctPose: spa优化
第一步,首先去除那些和当前节点的时间相邻的节点,针对那些在搜索范围内,且在时间点上并不相邻的节点产生一个数据链,然后进行匹配,若响应值大于阈值,添加回环并进行全局优化。
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
4)优化求解
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
整个优化算法使用了LM优化算法,原文中,作者大篇幅的都是介绍如何得到海森矩阵,以及高效进行优化求解,LM系统如下:
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
海森矩阵的稀疏:
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网

3、karto_slam总结

1)单从论文角度看,论文并没有关于整个框架的介绍,大部分的内容都是关于优化方法、黑森矩阵的构造等。关于扫描匹配、顶点和边、回环检测等所提甚少。
2)源代码包含主函数和库函数两部分,框架清晰,可读性强;除了eigen,不需要额外的依赖库。
3)关于karto_slam的介绍不如hector和gmapping多,可能是 没有采用主流的地图更新机制(概率地图)。只保存了pose节点和激光数据,作为pose-graph。
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网

五、ORB_SLAM

1、ORB_SLAM功能包

基于特征点的实时单目SLAM系统
实时解算摄像机的移动轨迹
构建三维点云地图
不仅适用于手持设备获取的一组连续图像,也可以应用于汽车行驶过程中获取的连续图像Raul Mur-Artal,J.M.M.Montiel和Juan D.Tardos于2015年发表在IEEE Transactions on Robotics上

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

注意:第7步,编译功能包 出现的问题:
机器人SLAM与自主导航(三)——SLAM功能包-编程知识网
解决方法:

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

机器人SLAM与自主导航(三)——SLAM功能包-编程知识网

3、启动单目SLAM示例(基于数据包)

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。

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

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

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