学习笔记之——激光雷达SLAM（LOAM系列的复现与学习）

Posted 2021-09-06 gwpscut

tags:

篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了学习笔记之——激光雷达SLAM（LOAM系列的复现与学习）相关的知识，希望对你有一定的参考价值。

概述

SLAM问题被认为是真正意义上实现机器人自主移动的关键。其问题可以理解为如下的生活问题。

当你来到一个陌生的环境时，为了迅速熟悉环境并完成自己的任务（比如找饭馆，找旅馆），你应当做以下事情：

a.用眼睛观察周围地标如建筑、大树、花坛等，并记住他们的特征（特征提取）
b.在自己的脑海中，根据双目获得的信息，把特征地标在三维地图中重建出来（三维重建）
c.当自己在行走时，不断获取新的特征地标，并且校正自己头脑中的地图模型（bundle adjustment or EKF）
d.根据自己前一段时间行走获得的特征地标，确定自己的位置（trajectory）
e.当无意中走了很长一段路的时候，和脑海中的以往地标进行匹配，看一看是否走回了原路（loop-closure detection）。实际这一步可有可无。

以上五步是同时进行的，因此是simultaneous localization and mapping。

目前用在SLAM上的Sensor主要分两大类，激光雷达和摄像头。

由于Sensor和需求的不同，SLAM的呈现形式略有差异。大致可以分为激光SLAM（也分2D和3D）和视觉SLAM（也分Sparse、semiDense、Dense）两类，但其主要思路大同小异。

SLAM算法在实现的时候主要要考虑以下4个方面吧：

地图表示问题，比如dense和sparse都是它的不同表达方式，这个需要根据实际场景需求去抉择
信息感知问题，需要考虑如何全面的感知这个环境，RGBD摄像头FOV通常比较小，但激光雷达比较大
数据关联问题，不同的sensor的数据类型、时间戳、坐标系表达方式各有不同，需要统一处理
定位与构图问题，就是指怎么实现位姿估计和建模，这里面涉及到很多数学问题，物理模型建立，状态估计和优化

其他的还有回环检测问题，探索问题（exploration），以及绑架问题（kidnapping）。

本博文主要学习激光雷达SLAM，激光雷达传感器利用光原理进行工作，激光雷达代表光探测和测距。它们可以探测到300米以内的障碍物，并准确估计它们的位置。在自动驾驶汽车中，这是用于位置估计的最精确的传感器。

激光雷达传感器由两部分组成：激光发射（顶部）和激光接收（底部）。发射系统的工作原理是利用多层激光束，层数越多，激光雷达就越精确。层数越多，传感器就越大。激光被发射到障碍物并反射，当这些激光击中障碍物时，它们会产生一组点云，传感器与飞行时间（TOF）进行工作，从本质上说，它测量的是每束激光反射回来所需的时间。如下图：

当激光雷达的质量和价格非常高时，激光雷达是可以创建丰富的三维环境，并且每秒最多可以发射200万个点。点云表示三维世界激光雷达传感器获得每个撞击点的精确（X，Y，Z）位置。

激光雷达传感器可以是固态的，也可以是旋转的，固态激光雷达将把检测的重点放在一个位置上，并提供一个覆盖范围（比如FOV为90° ）。在后一种情况下，它将围绕自身旋转，并提供360度旋转。在这种情况下，一般把它放在设备顶上，以提高能见度。

激光雷达很少用作独立传感器。它们通常与相机或雷达结合在一起，这一过程称为传感器融合。融合过程可分为早期融合和后期融合。早期融合是指点云与图像像素融合，后期融合是指单个检测物的融合。

激光雷达进行障碍物的步骤通常分为4个步骤：

点云处理
点云分割
障碍聚类
边界框拟合

为了处理点云，我们可以使用最流行的库PCL（point cloud library）。它在Python中可用，但是在C++中使用它更为合理，因为语言更适合机器人学。它也符合ROS（机器人操作系统）。PCL库可以完成探测障碍物所需的大部分计算，从加载点到执行算法。这个库相当于OpenCV的计算机视觉。因为激光雷达的输出很容易达到每秒100000个点，所以我们需要使用一种称为体素网格的方法来对点云进行下采样。体素网格是一个三维立方体，通过每个立方体只留下一个点来过滤点云。立方体越大，点云的最终分辨率越低。最终，我们可以将点云的采样从几万点减少到几千点。

滤波完成后我们可以进行的第二个操作是ROI（感兴趣区域）的提取，我们只需删除不属于特定区域的每一些点云数据，例如左右距离10米以上的点云，前后超过100米的点云都通过滤波器滤除。现在我们有了降采样并滤波后的点云了，此时可以继续进行点云的分割、聚类和边界框实现。

点云分割任务是将场景与其中的障碍物分离开来，其实就是地面的分割。一种非常流行的分割方法称为RANSAC（RANdom Sample consenses）。该算法的目标是识别一组点中的异常值。点云的输出通常表示一些形状。有些形状表示障碍物，有些只是表示地面上的反射。RANSAC的目标是识别这些点，并通过拟合平面或直线将它们与其他点分开。

为了拟合直线，我们可以考虑线性回归。但是有这么多的异常值，线性回归会试图平均结果，而得出错误的拟合结果，与线性回归相反，这里的ransac算法将识别这些异常值，且不会拟合它们。

如上图所示我们可以将这条线视为场景的目标路径（即道路），而孤立点则是障碍物。它是如何工作的？

过程如下：

随机选取2个点
将线性模型拟合到这些点计算每隔一点到拟合线的距离。如果距离在定义的阈值距离公差范围内，则将该点添加到内联线列表中。

因此需要算法一个参数：距离阈值。

最后选择内点最多的迭代作为模型；其余的都是离群值。这样，我们就可以把每一个内点视为道路的一部分，把每一个外点视为障碍的一部分。RANSAC应用在3D点云中。在这种情况下，3个点之间的构成的平面是算法的基础。然后计算点到平面的距离。

下面点云上RANSAC算法的结果。紫色区域代表车辆。

RANSAC是一个非常强大和简单的点云分割算法。它试图找到属于同一形状的点云和不属于同一形状的点云，然后将其分开。

LOAM

LOAM是Ji Zhang早期开源的多线LiDAR SLAM算法。该代码可读性很差，作者后来将其闭源。其效果如下：

A-LOAM

A-LOAM是港科大秦通博士（VINS-Mono一作）在LOAM原有代码基础上，使用Ceres-solver和Eigen库对其进行重构和优化，在保持原有算法原理的基础上，使其可读性大大增加，作为入门多线激光slam最好选择。

先基于作者的github工程（https://github.com/HKUST-Aerial-Robotics/A-LOAM）来构建环境进行数据集测试。

LeGO-LOAM

LeGO-LOAM 是Tixiao Shan在原有LOAM基础上，做了一些改进包括:1、对前端里程计的前量化改造，提取地面点更适配水平安装的LiDAR; 2、使用SLAM中的Keyframe概念以及回环检测位姿图优化的方式对后端进行重构。

LIO-SAM

LIO-SAM 是Tixiao Shan在LeGO-LOAM的扩展，添加了IMU预积分因子和GPS因子：前端使用紧耦合的IMU融合方式，替代原有的帧间里程计，使得前端更轻量；后端沿用LeGO-LOAM，在此基础上融入了GPS观测。同时前端后端相互耦合，提高系统精度。

CCM- SLAM

除了激光雷达slam外，本博文也跑了一下CMM-SLAM，基于单目camera，多个机器人的collaboration SLAM。

CCM（centralized，collaborative，monocular cmaera）

首先编译两个库

进入ccm_slam/cslam/thirdparty/DBoW2/然后运行

mkdir build
cd build
cmake ..
make -j8

进入g2o然后运行

cd ~/ccmslam_ws/src/ccm_slam/cslam/thirdparty/g2o
mkdir build
cd build
cmake --cmake-args -DG2O_U14=0 ..
make -j8

Unzip Vocabulary:

cd ~/ccmslam_ws/src/ccm_slam/cslam/conf
unzip ORBvoc.txt.zip

直接在当前工作空间做好像不行。。。新建一个工作空间试试～

按照github里面的教程步骤走即可（注意若为ubuntu18.04需要将里面的ros版本改为melodic）

编译的时候如果出现报错，可以修改其中的cpp文件

/home/kwanwaipang/ccmslam_ws/src/ccm_slam/cslam/src/KeyFrame.cpp

// size_t KeyFrame::nNextId=0;
long unsigned int  KeyFrame::nNextId=0;

然后再编译一下就成功了～

注意每次运行都应该先source一下工作空间

source ~/ccmslam_ws/devel/setup.bash

通过下面命令开启服务

roslaunch ccmslam Server.launch

写一个sh文件如下

#! /bin/bash

###############################################################
gnome-terminal -t "server" -x bash -c "roslaunch ccmslam Server.launch;exec bash;"
sleep 15

gnome-terminal -t "agent1" -x bash -c "roslaunch ccmslam Client0_euroc.launch;exec bash;"
gnome-terminal -t "agent2" -x bash -c "roslaunch ccmslam Client1_euroc.launch;exec bash;"
gnome-terminal -t "agent3" -x bash -c "roslaunch ccmslam Client2_euroc.launch;exec bash;"
gnome-terminal -t "agent4" -x bash -c "roslaunch ccmslam Client3_euroc.launch;exec bash;"


sleep 15
gnome-terminal -t "rviz" -x bash -c "cd /home/kwanwaipang/ccmslam_ws/src/ccm_slam/cslam; rviz -d conf/rviz/ccmslam.rviz;exec bash;" 
#如果roscd报错的话就直接用cd

sleep 15
gnome-terminal -t "agentbag1" -x bash -c "cd /home/kwanwaipang/下载;rosbag play MH_01_easy.bag --start 45;exec bash;"  
#cd 数据集包所在的位置
gnome-terminal -t "agentbag2" -x bash -c "cd /home/kwanwaipang/下载;rosbag play MH_02_easy.bag --start 35 /cam0/image_raw:=/cam0/image_raw1;exec bash;" 
gnome-terminal -t "agentbag2" -x bash -c "cd /home/kwanwaipang/下载;rosbag play MH_03_medium.bag --start 15 /cam0/image_raw:=/cam0/image_raw2;exec bash;" 
gnome-terminal -t "agentbag2" -x bash -c "cd /home/kwanwaipang/下载;rosbag play MH_04_difficult.bag --start 15 /cam0/image_raw:=/cam0/image_raw3;exec bash;"

EuRoC dataset数据集下载

https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets

实验视频结果如下：

(ccmslam)基于三个agents的单目SLAM地图融合