基于参数动态调整的双激光扫描头SLAM方法

万乐民，张云生， 余 天

(中南大学 地球科学与信息物理学院，长沙 410083)

随着社会经济的发展，传统的二维地图已经不能满足社会生产发展的需要。三维点云地图能够更加立体真实的还原建筑物的结构信息，在生产建设中能够发挥重要的数字底图作用。因此，三维空间点云地图的制图技术，成为目前测绘领域的重要研究方向[1]。激光扫描(lidar detection and ranging, LiDAR)技术[2]，能够快速获取物体表面的高精度三维点云信息，被广泛应用于三维测绘领域。基于LiDAR的同步定位与地图构建技术(simultaneous localization and mapping，SLAM)[3]，融合了LiDAR的高精度和快速扫描的优势以及SLAM技术高效而低成本的特点，能够在移动测量平台运动的过程中近乎实时地获取场景的三维点云地图[4]，提升了三维空间点云数据的获取效率。

SLAM技术最初起源于机器人领域，随着硬件系统的更新以及学科的交叉发展，SLAM技术的应用领域也逐渐扩大，测绘领域也应用该技术来实现三维环境地图的构建[5-6]。近年来，学者们提出了一些三维激光SLAM方法，比如Google公司推出的Cartographer[7]算法，该框架可以实现二维和三维条件下的建图，主要针对的是计算资源相对有限条件下的室内二维建图。不同于Cartographer算法，卡内基梅隆大学Zhang等人提出的LOAM[8]算法，可以直接面向三维LiDAR点云数据，仅通过激光里程计实现场景的三维点云地图构建。Shan等人在LOAM算法框架中加入回环检测以及图优化模块，提出了一种改进的三维SLAM方法 LeGO-LOAM[9]，该方法通过预先剔除地面点，提高前端激光里程计的效率，然后通过回环检测算法，探测存在的回环路径，最后通过因子图的形式构建图优化模型进行全局位姿图优化，获取全局一致的优化点云地图和轨迹位姿数据。

虽然以上方法可以较好实现场景重建，但都是以处理单个水平放置的激光扫描头为主，强调实时性，并且对点云数据做了一定抽稀。面向城市高建筑物场景时，单个水平激光扫描头由于其视场角的限制，常常只能扫描到建筑物靠近地面部分的立面信息，难以获取建筑物上部的立面信息。且因为多线扫描头的线束具有稀疏特性，单个扫描头获取的点云帧信息也不够稠密，导致建筑物的细节信息丢失过多，重建结果不全。而面向测绘领域高精度建模时，对点云的密度、覆盖度等提出了更高的需求。因此，搭载多个激光雷达传感器进行融合建图成为一种可行的方案。香港大学提出的Decentralized_LOAM[10]算法框架和香港科技大学提出的M-LOAM[11]算法框架均能够实现多个激光雷达的标定以及同步建图工作，但是这些方案主要是面向自动驾驶进行设计的，且系统设计复杂、设备研制成本较高。在双激光头的外参标定方法上，文献[12]依赖环境中存在3个线性独立平面的场景来实现标定。而文献[13]提出的一种通过提取几何特征点、线、面特征进行多激光雷达进行自动标定的方法，该标定方法需要扫描数据中具备足够多的点(交通锥)、线(交通杆)等几何特征。

针对以上问题，文中提出一种融合双激光扫描头的SLAM方法，该方法通过固化两个具有一定角度的扫描头，进行不同角度的扫描数据获取，并借助地面激光扫描获取的高精度稠密点云数据作为媒介，实现双激光扫描头的标定。然后利用两者之间的标定参数，实现初步融合，在此基础上，提出点云帧-子图的ICP(iterative closest point)[14]配准的动态优化方法，实现双激光扫描头点云数据的高精度融合，获得高覆盖度与高精度的三维点云。

1 方 法

方法流程如图1所示，输入的水平和竖直方向点云帧经过激光扫描仪的信号控制实现严格的帧同步。本方法主要包括以下3部分：

图1 双扫描头的数据融合方法

1) 双激光雷达外参标定：利用密集点云为媒介，进行水平方向和竖直方向双激光雷达的外参标定。

2) 水平激光雷达SLAM建图：利用LeGO-LOAM算法进行水平LiDAR点云的SLAM，获得水平点云帧建立的点云地图和全局轨迹位姿数据KeyPoses6D。

3) 双扫描头点云数据融合：通过KeyPoses6D数据和标定的外参数T，将竖直LiDAR点云帧初步变换到水平点云地图的坐标系下，然后通过点云帧-子图ICP算法进一步优化转换参数，实现竖直点云帧数据到水平点云地图的精确配准。

1.1 双激光雷达扫描头的外参标定

为实现激光传感器之间数据的融合，首先必须确定双扫描头之间的外部变换参数T，其表示如式1所示。对于角度不同的两个扫描头获取的扫描数据，点云之间重叠率较小，难以通过直接配准单帧点云数据获取二者之间的转换参数。因此使用地面激光扫描仪获取标定场景中稠密点云数据作为参考点云，将水平和竖直方向上激光雷达同时扫描的单帧点云数据与参考点云匹配，分别得到水平与竖直两个激光雷达相对于参考点云的坐标转换参数T1、T2。

(1)

式中：R表示旋转矩阵；t表示平移关系。

标定时取出待标定水平点云和竖直点云各1帧。以水平点云处理过程为例，用单帧水平点云先与参考点云进行粗配准，再使用ICP算法进行动态优化精配准，获得水平点云与参考点云之间的变换矩阵T1。采用类似方法，获得竖直激光雷达相对于参考点云变换矩阵为T2。设参考点云所在坐标系为Tw，水平点云的原始坐标系为Tl，待标定竖直点云的坐标系为Tv，则3个坐标系之间存在如下变换关系：

(2)

由式(2)可以获得水平和竖直点云之间的转换关系为：

(3)

因此双扫描头间外参数T如式(4)所示，由式(4)可见，分别获得T1、T2后，即可获得两个扫描头点云间的转换关系，为后续的点云融合提供基础。

(4)

1.2 水平激光雷达SLAM建图

LeGO-LOAM算法是基于LOAM算法的一种三维激光点云SLAM建图算法，不依赖惯性导航定位装置，就能够进行同步定位和建图，且在后端处理中融入了回环检测以及图优化模块，有效保证了激光SLAM建图结果的精度。因此文中采用LeGO-LOAM算法处理水平激光扫描头的数据。

利用LeGO-LOAM算法对水平安置的激光扫描头点云数据进行处理后，可以获取水平点云地图和对应的轨迹位姿数据KeyPoses6D。KeyPoses6D包含了每帧水平点云建图时对应的三维坐标以及姿态等信息，该数据为竖直点云数据的融合提供了基础。

1.3 双扫描头点云数据融合

在导出水平点云地图和轨迹位姿数据基础上，使用标定获取的外参数T，根据两个扫描头的同步时间戳，对同时刻下的竖直激光雷达扫描数据进行位姿变换，将单帧竖直点云初步转换到水平点云地图坐标系下。但采集过程中，可能因为传感器松动等原因，使得初步转换的竖直和水平方向点云之间存在一定的误差，因此需要进一步优化两者之间的变换参数。由于扫描线稀疏，且竖直方向和水平方向角度差异较大，文中提出一种参数动态调整的单帧竖直点云与水平点云子图匹配的方式优化单帧竖直方向点云的转换参数。

首先计算出待配准单帧竖直点云的外包围盒，然后用该包围盒裁剪出水平点云地图的对应区域获得水平点云子图，该子图经过水平连续多帧点云拼接而成，相对于原始单帧数据密集度有大幅提高，因此可以采用ICP进行优化配准。以裁剪的水平点云子图为参考点云，将单帧竖直点云精确配准至水平点云中。经过动态调整优化后，点云地图内部不一致性得到了很大程度上的消除，保证了点云三维重建的精度。

1.4 精度评价指标

本方法面向SLAM建图，采用场景中存在的平面来进行精度评定。选择一定区域的平面点，利用最小二乘法拟合平面参数，然后计算所有点到该平面距离的均值间接评价结果的精度，算式如式(5)所示。

(5)

式中：n是拟合平面的点数；di是点到拟合平面的距离。

2 实验与分析

2.1 实验系统搭建

2.1.1 硬件原型系统

为验证本文方法，搭建了如图2所示的原型系统，水平和竖直方向搭载双激光雷达扫描头夹角约为90°，激光扫描头采用Velodyne_16线激光雷达。

2.1.2 双激光扫描头标定结果

根据设计的双激光扫描头外标定方法，选择了笔者所在单位的一个大厅(如图3(a)所示)进行原型系统标定。使用FARO X330地面三维激光扫描仪对该场景进行扫描作为标定的参考点云，标定实验场地实景照片及扫描的点云如图3所示。

图2 实验原型系统

利用1.1节方法进行处理，获得两个扫描头之间的参数T，然后将竖直方向的点云转换到水平方向点云所在坐标系，标定前后结果如图4所示。由图4可见，本标定方法标定的外参数，可以实现竖直方向至水平方向点云的坐标位姿的转换，为两个方向的点云融合提供初始位姿。

2.2 实验数据

为验证本方法，笔者对两栋建筑物进行数据获取，分布如图5所示，分别记为楼A、楼B，获取的原始点云数据统计如表1所示。

表1 实验数据概况

图3 标定场地

图4 融合前后对比

图5 研究区域俯视图

2.3 实验结果与分析

对于数据楼A，基于单个激光雷达(水平安置的激光雷达)进行LeGO-LOAM的建图结果如图6(a)所示，使用文中提出的基于双激光扫描头数据融合建图结果如图6(c)所示。从图中可以看出，基于单个水平激光雷达LeGO-LOAM的建图结果受到其视场角的限制，导致目标建筑的立面信息缺失，无法对建筑外表面进行完整的三维重建。而使用融合双激光扫描头建图方法，能够弥补单个水平激光扫描头的视场角受限的缺点，获取更丰富的建筑上部立面信息，单个激光扫描头和双激光扫描头数据融合前后点云地图局部区域对比如图6(b)(d)所示。参数动态调整优化后，局部区域的重影得到消除，优化前后的局部区域放大对比如图7所示。

图6 数据楼A水平单激光扫描头和双激光扫描头建图结果对比

图7 参数动态调整优化前后的融合点云对比结果

对于数据楼B，基于单个激光雷达(水平安置的激光雷达)进行LeGO-LOAM的建图结果如图8(a)所示，水平地图局部放大结果如图8(b)所示。使用双激光扫描头融合建图方法结果如图8(c)所示，融合地图局部放大结果如图8(d)所示。经过ICP算法优化后，局部区域的重影得到消除，参数动态调整优化前后的点云对比结果如图9所示。

图8 楼B水平单激光扫描头和双激光扫描头建图结果对比

2.4 定量精度评定

为了进一步定量评价本方法三维重建点云精度，在实验数据楼A、楼B上分别选取了如图10所示的5个区域进行平面拟合度计算，结果如表2所示。从结果可以看出，经过动态调整优化处理后，RMS值与优化前相比都有所减小，而且楼A和楼B的平均RMS精度提高33%和24%以上，说明了本方法不仅可以提高覆盖度，也可以保证重建点云的精度。

表2 ICP优化前后平面拟合RMS值 m

3 结束语

针对单个扫描头进行激光SLAM点云覆盖度较低的问题，提出了一种基于双扫描头的SLAM方法。针对标定参数的微小误差，提出基于点云-子图匹配的方式优化配准方法，将水平和竖直方向两个方向的扫描数据进行高精度融合。通过室外重建实验结果分析表明，提出的基于参数动态调整的双激光扫描头SLAM方法，与传统的基于单个激光扫描头的SLAM算法相比，能够获取更加丰富的立面信息。