请选择 进入手机版 | 继续访问电脑版

SLAM demo using 2D LiDAR

2019-1-11 09:02
1410
0 引言

      刚刚入门学了近一个月的SLAM,但对理论推导一知半解,因此在matlab上捣鼓了个简单的2D LiDAR SLAM的demo来体会体会SLAM的完整流程。
(1)数据来源:德意志博物馆Deutsches Museum)的2D激光SLAM数据,链接如下:
Public Data - Cartographer ROS documentation​google-cartographer-ros.readthedocs.io(2)SLAM过程展示(戳下面的小视频)
               SLAM demo using 2D LiDAR-1.jpg                               
2D LiDAR SLAM               
https://www.wengbi.com/video/1050063244621930496
                                      (3)demo得到的SLAM效果:
SLAM demo using 2D LiDAR-2.jpg
SLAM效果(包含路径、当前位姿、栅格地图)
1 数据准备与参数设置

1.1 2DLiDAR数据集准备
       将提供的2DLiDAR数据集'b0-2014-07-11-10-58-16.bag',转为matlab的.mat数据文件,这其中包括有5522批次扫描数据,每次扫描得到1079个强度点。如下:
SLAM demo using 2D LiDAR-3.jpg
1.2 LiDAR的传感器参数设置
       包括:扫描角度的范围、扫描角度间隔、扫描点数、扫描时间、一次扫描的线束数等。(详见源代码)
1.3 位姿与地图参数设置
       包括:栅格地图单元尺寸对于的实际长度、机器人移动多少更新一次地图和位姿、初始位姿等。(详见源代码)
2 程序流程与思路

2.1 数据准备与参数设置。
2.2 遍历每一批次扫描数据(共5522批次),且对于某一批次scanIdx进行如下流程:
(1)求本批次扫描数据的局部(以移动机器人为原点)笛卡尔坐标(ReadAScan.m)。
(2)判断该批次是否为第一批?若是,则初始化(Initialize.m);若否,直接进入下一步。
(3)由本批次扫描数据(实际上这是一个含有1079个点的集合)的局部坐标,和当前位姿,求得当前扫描数据点的全局坐标(Transform.m与ExtractLocalMap.m)。
--------插入一段解释---------
局部坐标:以某次扫描时,机器人所在的空间(二维)位置为原点,得到的该次扫描数据点在空间中的二维坐标。
全局坐标:以机器人初始位置为原点,来描述任意批次的扫描数据的二维坐标。
-------------------------------------
(4)以这批数据的全局坐标,构建该次扫描得到的栅格地图(OccuGrid.m)。例如:
SLAM demo using 2D LiDAR-4.jpg
(5)预测下一位姿(位姿为x坐标、y坐标、旋转角度theta这个三维向量)。
*预测方法为:若当前位姿为初始位姿,预测的下一位姿=当前位姿;否则,预测的下一位姿 = 当前位姿 + ( 当前位姿 - 前一位姿 )。
(6)根据当前位姿的栅格地图来优化预测的下一位姿(FastMatch.m)。
*思路为:在预测的下一位姿上做一些细小的调整(对x、y、theta做细小调整);对于某一次调整后的预测下一位姿,利用下一位姿的扫描数据,构建下一位姿的栅格地图;以下一位姿的栅格地图与当前位姿的栅格地图的重合度作为目标函数,求该目标函数的最大值;此时得到的下一位姿即为优化后的下一位姿。(得到的结果并一定是全局最优解,因为仅在原始预测下一位姿基础上做了有限次的细微调整)
例如:预测位姿为[-16.5250; -0.7344; -3.9177];优化后为[-16.5250; -0.7156; -3.9057]。
(7)判断下一位姿与当前位姿间的差距是否达到设定的阈值?若是,进行更新(AddAKeyScan.m);否则,不进行更新。
更新步骤为:判断预测的下一位姿和当前位姿在x或y或theta上是否存在较大的差别?若是,则判断为预测有误,此时,令当前位姿=上一位姿(因此要求将上一位姿保存起来),从上一位姿开始重新开始遍历;否则,认为预测下一位姿正确,将下一位姿的数据集的全局坐标加入到全局地图中。
(8)把下一位姿并入路径中。
因此,路径为位姿[x;y;theta]的集合,如下:
SLAM demo using 2D LiDAR-5.jpg
(9)绘图(全局地图、路径、当前位姿)(PlotMap.m)
最终绘制的结果如下:
SLAM demo using 2D LiDAR-6.jpg
此外,得到的栅格地图如下:
SLAM demo using 2D LiDAR-7.jpg
3 MATLAB源代码(附有详细注释)

把下面这些函数放在路径下,直接运行main.m即可。
**补充一下:horizental_lidar.mat 这个数据 可戳下面的网盘链接下载,然后放在路径下:
https://pan.baidu.com/s/1KDb9zO1dDlvJq-ut9Xj0gg
----------------------
引用自:
https://github.com/meyiao/LaserSLAM?files=1
----------------------
函数列表:
(1)main.m
(2)SetLidarParameters.m
(3)ReadAScan.m
(4)Initializ.m
(5)Transform.m
(6)ExtractLocalMap.m
(7)OccuGrid.m
(8)FastMatch.m
(9)AddAKeyScan.m
(10)PlotMap.m
(11)DiffPose.m
----------------------
(1)main.m
  1. [/code](2)SetLidarParameters.m
  2. [code]
复制代码
(3)ReadAScan.m
  1. [/code](4)Initializ.m
  2. [code]
复制代码
(5)Transform.m
  1. [/code](6)ExtractLocalMap.m
  2. [code]
复制代码
(7)OccuGrid.m
  1. [/code](8)FastMatch.m
  2. [code]
复制代码
(9)AddAKeyScan.m
  1. [/code](10)PlotMap.m
  2. [code]
复制代码
(11)DiffPose.m
[code][/code]
分享到 :
0 人收藏
您需要登录后才可以回帖 登录 | 邀请

本版积分规则

Archiver|手机版|小黑屋|翁笔

Powered by Discuz! X3.3 © 2001-2018 Comsenz Inc.

返回顶部