激光slam_移动测量中的激光SLAM

王程，厦门大学信息学院教授，IET Fellow，福建省智慧城市感知与计算重点实验室的执行主任，国际摄影测量与遥感学会的多传感器集成与融合工作组的主席。研究兴趣包括三维视觉，激光雷达点云处理，空间大数据分析，遥感图像分析，移动测量。王老师发表论文200余篇，包括CVPR，AAAI，VR，ISPRS JPRS，TRGS等高水平会议和期刊，是IEEE GRSL的编委，作为大会主席和组委会主席组织过CVPS2012，MMT-SS2015， ChinaLiDAR2018等多个国际学术会议和学术交流活动。

本次整理自王程老师在第二届SLAM技术论坛中《移动测量中的激光SLAM》的报告。

---

01 移动激光扫描简介

激光扫描经历了从一维到二维再到三维的发展过程，大家都知道激光测距的原理，在街上经常能看到测绘单点无反射的全站仪，其中激光测距的装置就是一维的激光雷达(LiDAR)。

实际上一个单点也叫做LiDAR；如果把这条激光扫起来，就可以得到一个二维的剖面，例如一些用于工业界避障用的激光雷达；

如果不断在一个方面进行360度进行环扫，同时把环扫的剖面进行旋转，就能得到环境中一个三维的表面模型，这就形成了三维的激光扫描。

如果希望获得更大范围的三维建模，那就需要让激光雷达动起来。 三维激光雷达可以放在不同的移动平台上，例如手持平台、车载平台、机载平台、船载的平台和背包，除此之外，现在卫星平台上面也已经开始尝试用激光雷达，但是目前卫星上还没有办法进行表面大规模的成像。 上述统称叫做移动测量或者Mobile Mapping，因为它们使用了激光扫描，所以又称mobile laser scanning(简称MLS)。

激光扫描获得的一些典型场景

用于测图的移动激光扫描系统，其传感器一般分成两大类：

• 一类是位置和方向估算，例如IMU(惯性测量单元)、GNSS(全球导航卫星系统)、DMI。

  IMU一般是必不可少的，只是在不同场景下使用不同等级的IMU，如果控制成本就用比较便宜的MEMS IMU，高精度场景中可能要用几十万一台的战术级甚至战略级IMU，比如用于导弹或者洲际导弹上；

  GNSS包括GPS和北斗，在开放的空间下GNSS显得非常重要，因为它能够通过差分得到近乎实时全球定位2厘米左右的定位精度；

  DMI是指从车的轮毂中心处接出一条线做码盘上的测量，接到车载的码表上，当然也可以外接，接入到整个组合导航系统。

• 另一类是环境地图数据的收集和融合，主要是相机和激光扫描仪。

  目前，为了能够覆盖周边环境比较大的范围，一般都使用多个相机；激光雷达一般要配置两个单线的激光扫描，形成所谓butterfly的模式，该模式下扫一栋四方的楼能够扫到三面，如果只有一条单线只能扫到两面。所以一般在测绘级的应用上激光雷达会采用两个。

移动激光扫描系统获得的点云为什么能够得到全球的定位坐标呢？ 原理如下图所示：

车上有一个组合导航系统，这个组合导航系统从卫星定位能够获得比较高频率、高精度的定位信息，同时IMU提供了比较精确的方向，IMU跟相机或激光雷达做了系统标定。

由激光雷达打一束光到P点，得到P点和雷达之间的相对坐标，同时因为激光雷达与IMU有系统标定，所以可以计算P点在全球定位坐标里的位置。

但该原理并不适用于所有环境，比如不能使用GNSS的环境(城市峡谷、隧道里、室内、水下等)。

除此之外，林业测量领域面临同样的问题，在山上使用GPS的精度和树林的茂密程度有关，在树林下使用GPS的误差能够达到50米，最多能够达到300米；300米意味着在福建就在另外一个山头上，其实树林下是非常典型的GNSS Denial环境。

在GNSS Denial环境下，一般的传感器不再可靠，可选的替代方案有基于相机或者LiDAR SLAM，本文将重点介绍LiDAR SLAM，在此之前，先给大家简单介绍一下使用SLAM (Simultaneous Localization And Mapping)的原因。

02 多线激光雷达SLAM

SLAM是机器人构建环境地图并同时使用该地图计算自身位置的过程。

• Localization：根据地图推断位置

  在不正确的GPS下；

  GPS拒绝的环境。

• Mapping：推断给定位置的地图

  先验地图不可用且需要构建的场景；

  地图可以为人或机器提供路径规划信息或直观的可视化效果。

像扫地机器人这一类2D的LiDAR SLAM一般是单线激光扫描，得到一个二维的grid map，而三维激光扫描主要用多线激光扫描，得到三维点云的模型。

下图是常见的多光束激光扫描仪( Multi beam LiDAR scanner )，在早期Google的无人车以及诸多国产无人驾驶车上都可看到。其中，左图为64线的激光扫描仪，中间黑窟窿是车或者是传感器架所在的位置，当前这一帧的数据是一圈一圈地展开。实际上，在传感器端几何模型为：中间一个轴，64个beam围绕该轴在旋转，每一个beam朝着不同方向，右图只展示了16线的模型。

目前常用激光雷达为旋转式的LiDAR scanner，随着无人驾驶不断发展，已经有多家厂商在生产激光雷达传感器，例如镭神、禾赛等。其中一个很明显的趋势是前视的激光雷达。

上图中的传感器模型是旋转式360度的，作为人类驾驶者在开车的时候眼睛是不会一圈一圈向后看的，可能偶尔看一下后视镜，但主要是向前看。

在无人驾驶这个领域的应用，向后扫描的点有些浪费，因此前视的激光雷达是其发展的方向之一，最后的形态可能会像匣子一样，前面的窗是激光雷达多线向前扫描，类似于RGBD，但是它的扫描距离一般都要超过150米才能达到无人驾驶的标准，而且在这样的距离要求下达到10厘米到几十米的分辨率。

市场预期多线LiDAR scanner发展会非常迅速，预计今年多线激光扫描仪的市场总量会呈一个指数级的快速增长。有人预测在每一辆车大灯里都应该安装前视的激光雷达，从这个角度考虑，全球每年可能有100万台的出货量。

以下参数是当前多线激光扫描传感器的发展方 向：

• 一台激光雷达在1000美金以下；
• 扫描距离达到150-200米；

• 在扫描距离上能够达到10公分到20公分的空间分辨率。

2.1  常用数据集介绍

目前国际上的benchmark，第一个是KITTI，含有比较丰富的原始数据以及标定结果，包括相机、64线以及高等级GPS/IMU的组合，涉及范围和物体主要包括城区环境，还有一些居民地、高速公路和校园的环境。

第二个常用的数据为Ford Campus Vision 和LiDAR database，既有相机也有激光雷达，获取的数据特点与KITTI类似，但其周边情况更加复杂一些。

第三个数据集是RobotCar，有大概1000公里的数据，其特点是在一环道上面反复获取，每周获取两次，一年的期间采集了较多数据，对于测试可重复的情况较为有用。

第四个数据集是DBNet(drivers behavior net)，由我们课题组于2018年推出，该数据集包括camera采集的前视图像以及激光雷达的数据，同时还记录了操作者踩刹车和转方向盘的角度。

因为在驾驶车辆时，驾驶员与车辆互动的行为主要包含踩刹车、加油门、转方向盘这三件事，将这些数据记录下来，可以建立从drivers behavior到外界数据之间对应的数据集。

目前DBNet数据集大概有1000公里的数据，包括前视视频、激光雷达的点云、GPS轨迹和drivers behavior，以及从总线里面取出来的速度和转速的情况。

第五个数据集为MI3DMAP，数据采用Backpacked system采集得到，数据包含激光雷达采集的原始点云数据以及团队做出来的SLAM数据，还包括相机采集的数据、MEMS IMU的结果等。

此外，Backpacked system上有多个手机，便于获得WIFI 与蓝牙的数据，这些数据可以为后续做基于电磁指纹的室内定位提供ground truth。

03 LiDAR SLAM介绍

LiDAR SLAM是工程性较强的问题，研究者们提出了诸多方法，本文重点介绍LiDAR Odometry的相关算法。

首先介绍一下SLAM的时间线，最早可以把主要论文分成四类：

第一类是point-based即用点云来做SLAM；第二类是feature-based，先提取出一些feature或者类似于feature的东西，用这些feature做匹配；第三类是learning-based；第四类是framework。

point-based LiDAR Odometry方法中，ICP是非常传统的方法，但到目前为止很多主流算法还是采用ICP的基本方案。在传统ICP基础上，还有从点到面(Point –to –Plane)的ICP方法。

在基于点的LiDAR Odometry的基础上，feature-based也是非常有效的一类方法，该类方法中比较经典的为LOAM方法，其后又发展出了V-LOAM，即在LOAM基础上加了相机，相机跟LiDAR去结合。

除了一些简单Feature，我们在开放场景下可以将独立物体切出来，我们称之为Segment as Feature，较为经典的方法为SegMap框架。

04 基于学习的SLAM

4.1  PointNet

深度学习在二维图像中的应用取得了较大突破，但三维点云深度学习框架实际上从2017年下半年逐渐发展起来，其中比较典型的算法是PointNet。目前，该方法已经成为点云处理的基本步骤。

看起来跟CNN很像，区别在于图中红框中的这样一个结构，这个结构进来是N×3个点，这个N是什么呢？

比如说你有1024个点，或者是2048个点，N就是1024或者2048，为什么是3呢？因为你有XYZ，这样输进来就是一堆点，这堆点它的顺序没关系，其实 PointNet的本质就是要把点云顺序这个维度上面的随机去掉。

接下来有一个MLP，多层感知的一个结构。对于每一个点它都做了64和64双层感知，而且对每个点采用的网络参数都是互相share，这样XYZ的一个点经过这样双层感知变成了一个 64维长度的矢量，N×3变成了N×64，中间跨过了share MLP的这么一个框架。

它有多层，这样整个框架的输入是一个N×3的点，比如说1024个点或者2048个点，输出是什么呢？ 

是1024点或者2048个点得到的一个1024维的global feature description，而且这个description跟你输入的点的顺序无关，这也是PointNet标榜的一个主要性能。

4.2 PointNet++

在此基础上PointNet++ 把点云用pointNet做了一个多层结构，这样能够扩大感知欲。

4.3 PointCNN

pointCNN跟pointNet有很多相似之处，示意图如下所示：

这上面是图像，图像convolution的一个示意图，底下是点云，一个很小的点云，10个点的点云，就是所谓的X-convolution示意图。

其实建立点云深度学习最关键的问题就是建立底层这样一个类似于convolution的计算，这里面将它称为X-convolution。

这个作法就是把每个点的信息向着临近的逐点去投射，这个投射的过程是用了一个类似于convolution的卷积计算的方法，而且X-convolution可以做多层，比如说一开始是10个点，第一层X-convolution以后变成了五个主点，接下来再投可以变成两个点，这样多层做下去以后，大规模的点云变成了一个点数很少，但是在每个点上有着多维特征的这样一个简化描述，跟我们图像上面的convolution非常相像。

4.4  End-to-End

下面给大家介绍End-to-End，Video进来以后直接用深度学习网络，里面把每一张图像通过一个CNN加上RNN，这样前后两帧关联起来，输出就是一个pose。

4.5 Point Cloud Matching

在三维点云里面也有类似End-to-End的工作，比如下面这个是2019年CVPR上的一个工作，叫做Point Cloud Matching。

这个工作它的特点首先在这里面找到一些特征点 ，这个特征点是用别的一些特征点提取方法来实现，对于每个特征点上的特征描述，先找到一个主轴投影方向，然后变成了一个类似于图像的投影，接下来就是跟图像很像的一个多层的类似CNN的处理方式，然后做两边的matching。

4.6 3Dfeat-Net

下面介绍3Dfeat-Net，这个是2018年的工作，它是弱监督的工作，这里边采用正样本、负样本和中性样本，上面的是正样本，下面的是负样本，中间的是中性样本。 有三个分支的孪生网络，先进行特征点的提取，但特征点提取的时候有一个非常有意思的输出，在这里面不仅有所谓的feature descriptor，还有一个当地local feature的指向，这个指向应该说是个标准化的指向，为后续的匹配提供了一个很好的输入，这个feature在后续的descriptor网络里面作为一个输入进来，这样对descriptor一致性有一个非常好的提高。

4.7 LO-Net

前面介绍了从point based、feature based到Learning-based一些基本框架。接下来我介绍我们今年CVPR上的工作LO-Net，我们做这件事动机有三个：

1. 我们刚才说了，其实点云的三维深度学习整个框架还不是很有效，不管从处理效率还是准确度上，还是跟基于图像的描述方式有差距。

2. 激光雷达它有一个特点，不同的传感器，实际上每一支传感器都有自己的一些激变，而且这些激变你要把它精确进行标定实际上是非常困难。

   因为SLAM它是在运动平台做，在运动过程中激光雷达扫描还是每秒10-30赫兹，它转一圈是有时间的，在它转一圈的时间里面你在向前走， 如果原来是一个圆的话，你走得越快这个圆变得越扁，大家能想象到这个圆变得越扁。

   这应该是一个直线的墙，但是由于这样的激变导致它是一个弧线，因此这样的一些激变在不同品牌，甚至不同只的传感器之间挺复杂的。

   我们希望把这一部分用深度学习跨过去，不要每一只传感器都要做特别的标定。

3. 像KITTI这样的数据它更多的是在一些欧洲的城市里面来做，它周边的环境还是比较静态，我这里弄的比较夸张，采集的古城的数据。

   而我们真正使用SLAM的时候经常是这样一个动态的环境，包括我们背着背包在屋子里面走的时候，周边也经常有人开门关门等等的运动。运动环境下LiDAR SLAM是非常重要的一个motivation。

我们做的第一步工作，就像下图展示的那样，虽然是一个激光雷达，扫出来是点云，但是我们根据码盘的读数把它再投影回类似于一个图像的东西，是一个很长条的图像，如果使用的是64线的激光雷达，得到的是一个64线高，大概2048宽的这么一个类似于图像的条带。

得到XYZ，同时还有回波强度以及距离，XYZ实际上可以反演出距离，但是因为这五个数据从激光雷达可以直接读出来，所以我们把它变成了五张图像。这里面不可避免有些点没有反馈，所以有些点是需要你做标记。

我们网络情况下大概是下面这样，采用双分支，旁边画了两个Mask，这个Mask就是为了防止把边上的这些运动物体，让它在训练的时候对运动物体有所感知，训练的时候一路这样下来以后往Mask走，Mask要输出的是一个运动物体的提取。

但是训练完了以后Mask这一步就不用了，唯一使用是encoder这个通道，把它share到用来做feature description，用feature来提取，这两个feature提取完了以后使用一个全连接，输直接输出Odometry。

这个方法它的特点是什么呢？我们在这里面考虑到了在特征描述的时候对于运动物体的这样一个描述，因此在真正做Odometry的时候能够对周边的运动物体有所感知，这样通过多任务的方式能够提高整个网络在SLAM时候的性能。

下面是LO-Net 和其他方法的一个对比。我们看到在KITTI这样的数据，我们有不少能够比LOAM要好。更明显是在Ford数据上， Ford这样的数据它周边的运动物体会更多一些，我们的优势也会更明显一些。

下面是Trajectory的结果，中间红色这一条是结果。

下面这个做的一个大范围SLAM结果，这个范围大概在几百米，500-1000米这个距离上。这个是仅仅使用Mapping Results得到的结果，中间没有用其他的，比如说GNSS，或者是camera其他一些传感器，直接就是使用LiDAR的数据然后做的SLAM。

05 总结

1. 基于SLAM的移动激光扫描系统已经能够为GNSS失效的环境服务。
2. 端到端的激光SLAM现在渐渐成为主流。
3. 多任务能够提升LiDAR SLAM能够它的可靠性。
4. 激光SLAM为城市建模提供一个有效支撑。
5. BIM和AVP目前是该领域较为有效的应用方向。