融合泊松重建的激光语义SLAM系统

曾圣尧，张 雷，徐 方，杜振军，刘明敏

(1.中国科学院沈阳自动化研究所机器人学国家重点实验室，辽宁 沈阳 110016； 2.中国科学院机器人与智能制造创新研究院，辽宁 沈阳 110169；3.中国科学院大学，北京 100049；4.沈阳新松机器人自动化股份有限公司，辽宁 沈阳 110168)

0 引 言

当前，依赖激光雷达的激光SLAM和依赖图像的视觉SLAM是当下SLAM领域的两大主要分支[1,2]，多传感器融合的SLAM技术也日趋成熟[3]。在激光SLAM领域，以SuMa[4]和LiTAMIN[5]为代表的激光SLAM系统使用各种改进的ICP作为帧间匹配的方法；以LOAM、LeGO-LOAM[6]为代表的激光SLAM系统则在匹配前首先对点云中的点进行基于几何信息的特征提取，筛选出一系列特征点进行处理。在地图表示方面，大部分激光SLAM系统都使用点云地图[5,6]，Cartographer[7]中使用占据栅格地图，SuMa使用基于surfel的面元地图。Vizzo等[8]提出在地图建立中使用泊松重建建立面元地图同时配合点面ICP的激光SLAM方法，但是由于泊松三维重建的时间耗散过大，系统无法实时运行，仅在理论上论证了泊松重建的面元地图在恢复场景的三维细节，节省存储空间等多方面的优越性。近些年来，随着深度学习的崛起，计算机对数据的理解能力的加深使得语义SLAM成为近来研究的热点问题[9]。语义SLAM加深了机器人对于环境的理解能力。在激光SLAM领域，SuMa++[10]利用Rangnet++网络建立基于surfel面元的语义地图，同时利用语义信息加强了在动态环境中的定位精度。但是surfel面元仍是较为稀疏的面元表示方式。本文所提出的框架使用全局点云语义地图和局部基于泊松重建的面元地图，可以在保证实时性的基础上，一方面保证建图精度，另一方面增加面元地图的表示能力。

1 总体架构设计

系统的总体框架设计如图1所示。系统接受6自由度的3D雷达数据，输出6自由度的位姿估计与语义地图。系统主体可以分为定位与语义地图的建立两大部分。在定位部分中，系统采用了基于LeGO-LOAM的设计思路——点云几何信息的特征提取方案。在建图模块中，雷达点云数据中的每一个点基于神经网络进行语义标注，结合定位模块中给出的位姿信息，进行局部语义点云地图的构建，通过泊松重建最终获得含有语义信息的面元地图。在此架构中，主要依赖于CPU解算的定位模块与主要依赖于GPU解算的语义模块并发执行，保证系统算力的有效使用。

图1 系统总体架构

2 具体实现

2.1 定位模块的实现

定位模块由基于几何特征提取的前端与回环检测以及图优化3部分组成。

在几何特征提取中，所有点按照平滑度大小排列，选取平滑度较大的点集合为平面点，平滑度较小的点集合为角点。其中，对于点云P中的第i个点pi平滑度Cpi定义为

(1)

其中，n表示选取近邻点的个数，S表示由近邻点构成的集合。

对于平面点而言，进一步被区分为地面点P和其它平面点E。其判定依据为该点与相邻线束的激光点的连线在其雷达坐标系中的俯仰角θ。点pi的俯仰角θ计算为

(2)

其中， (xi,yi,zi) 为pi在雷达坐标系下的位置， (xj,yj,zj) 为点pi的相邻线束的激光点pj在雷达坐标系下的位置。当θ足够小，即 |θ|<θthreshold时，判定为地面点。

(3)

当前时刻的总位姿变换为

(4)

2.2 语义地图的建立

对于激光SLAM而言，通过深度学习对雷达点云中各点所属物体进行分类，通过SLAM算法为点云提供其在世界坐标系下的三维位姿，建立的地图被归为语义地图。

语义分割是建立语义地图的第一步。在语义分割中，首先将3D点云数据进行球面投影，处理为2维的深度图像，在深度图像上进行基于全卷积网络的语义分割，最后对深度图像上带有语义标签的像素点转换回3D点云数据中。在语义地图的建立过程中，系统维护一个以点云方式存储的全局语义地图以及以面元形式存储的局部面元语义地图。语义地图的整体处理流程如图2所示。其中图2(a)表示雷达获取的原始点云数据，图2(b)表示系统的全卷积神经网络架构，图2(c)表示具有语义信息的点云，图2(d)表示表示泊松重建后具有语义信息的局部面元地图。

图2 语义地图的处理流程

2.2.1 语义点云的获取

语义分割神经网络选用深度图像作为输入。对点云数据进行球面投影，获得对应的深度图像是语义分割的第一步。在球面投影的过程中，对于点云中的点pi=(xj,yj,zj)， 先计算其在雷达坐标系中的俯仰角与偏转角θpitch、θyaw

(5)

当雷达传感器垂直分辨率(线数)为h，水平分辨率(一圈扫描点数)为w，上下测量角度范围分别为θup、θdown时，对于创建的分辨率为[h×w]的图像，利用式(6)计算其在深度图像中的坐标(u,v)

(6)

(7)

语义分割完成后的深度图像反向解算将语义映射回原始的三维点云中去，完成对于点云的语义分割。具有语义信息的点云一方面将融合进关键帧，参与全局语义点云的构建；另外一方面将融合进局部子图，参与局部面元地图的构建。

2.2.2 泊松重建的处理

在局部面元地图的处理过程中，关键步骤是将点云转化为面元。泊松重建是一种将三维离散点转换为连续表面的先进三维重建方法，重建后生成具有水密性质的三角面元[13]。与传统的三维重建方法相比，泊松重建将点云中的所有点作为一个整体考虑，不进行局部分割与拼接，最大程度保留了原始点云的局部细节与空间信息。使用泊松重建可以构建一个高质量的局部面元地图，面片光滑细腻，提供更多环境的细节信息。这样的局部地图可以为人机交互提供一个更清晰、明了的局部环境感知。

泊松表面重建算法的本质是根据点云建立的指示函数，提取出由三角形面片拼接成的光滑曲面，属于一种隐式曲面重建算法。其具体的计算流程如图3所示。

图3 泊松重建基本流程

(8)

(9)

其中，*n表示进行n次相同的卷积操作。在对点云进行八叉树划分后，每个八叉树的节点o中的点q使用如下平滑函数进行处理

(10)

其中，o.w表示节点o的宽度，o.c表示节点o的中心。同时对于梯度空间采用如下近似

(11)

(12)

泊松重建生成具有水密性的网格地图，网格地图在数据构成上由各个顶点的三维坐标集合v以及每一个三角形面元所包含的3个顶点索引 (n1,n2,n3) 所组成。泊松重建的水密性质决定了生成的网格地图具有封闭性，与室外环境地图自然带有的非封闭特性结合，会在重建过程中生成大量多余面元，从而影响对与环境信息的直接观测。所以在对局部子图的泊松重建中，去除多余的面元是十分重要的。针对道路环境的结构特点，选取面元平均距离作为移除多余面元的判定依据。在距离原始点云过远处生成的面片是十分不可靠的面片，因为这表示原始的雷达没有扫描到此处的空间信息。同时，当扫描到一处的点数量过少时，此处的信息仍是不够准确的，所以在评价指标中选取面片到原始点云中多个点的平均距离进行测量，增加面片信息的准确性。顶点pi平均距离tpi的定义为

(13)

其中，n为选取近邻点的个数，S为点Pi在重建前的原始点云中的近邻点的集合。面元m的平均距离定义为其3个顶点p1,p2,p3平均距离的最小值，即tm=min(tp1,tp2,tp3)。 图4展示了重建后面元的平均距离的分布情况，图中面元颜色反应了面元的平均距离变化。为了便于显示，图4中对于平均距离采用对数拉伸处理，颜色深度T与平均距离tpi的对应关系为T=70log(tm+1)。 从图中可以看到，需要移除的多余面片向穹庐一样环绕包裹，多余面片颜色较深，主要集中在远离点云的上部区域，单块三角形面片面积较大。而下部靠近道路部分的面片呈现浅色，这部分面片反应的是雷达采集到数据的真实环境，面片十分密集，单块面片面积细小，能够反应局部细节。通过选取合适的门限值，移除多余面片，获得重建后的局部子图。

图4 泊松重建后的全部面元及面元平均距离分布

在移除多余面片时，如果对移除的面片的顶点进行删除，则会影响其余所有面片的顶点排序，需要对其余所有面片的顶点索引进行更新，这要无疑在计算复杂度上是不可接受的。为了减小计算量，在移除多余面片时，只移除面片顶点的索引，而保留顶点坐标，这样在渲染面元地图时便不会显示需要被去除的面片。具体计算流程如算法1所示。其中近邻点查找使用KDTree[14]算法对进行加速。算法1中，移除面片顶点索引表达为将面片索引置为非法索引值-1。在本系统中，选取近邻点个数K为10，面元平均距离门限值tmin为0.5。

泊松重建的过程中不会对语义信息进行处理，重建后的面元地图中不包含语义信息。所以在获得重建完成后的面元地图后还需要对顶点进行语义信息的关联。在语义渲染中，面元地图的每个顶点在数据构成上包含6个维度：空间坐标 (X,Y,Z) 与颜色 (R,G,B)。 在关联语义信息时对于顶点坐标v在重建前的语义点云P中寻找最近邻点，以其语义所对应的颜色作为该顶点的颜色数据。基于现实环境中同一垂直空间的物体归属同一语义标签的假设，在关联语义信息时可以将所有待渲染顶点和语义点云投影到XOY平面上，在二维平面上进行近邻关系的寻找。在具体实现上，可以先对于包含语义信息的原始点云在的XOY平面上构建KDTree，然后对于待渲染的面片顶点进行基于KDTree的查找,找到距离其最近的语义点进行关联，加快渲染速度。

算法1:多余面片移除

输入：原始点云:RawPoint[n1][3];

面元顶点:Vertex[n2][3];

面元顶点索引:Mesh[n3][3];

近邻点个数:K;

面元平均距离门限值:tmin;

输出：移除多余面片以后的面元顶点索引Mesh[n3][3]

(1)KdTree=MakeKdTree(RawPoint) //利用原始点云建立KdTree

(2)fori=1,2,…,n2do

(3)Nearest=KdTreeSearch(KdTree,Vretex,K)

(4)t[i]=0

(5)forj=1,2,…,Kdo

(6)t[i]=t[i]+||RawPoint[Nearest[i] ]-Vertx[i]||

(7)endfor

(8)t[i]=t[i]/K

(9)endfor

(10)fori=1,2,…,n3do

(11)t1=t[Mesh[i][1]]

(12)t2=t[Mesh[i][2]]

(13)t3=t[Mesh[i][3]]

(14)tm=min(t1,t2,t3)

(15)iftm>tminthen

(16)Mesh[k]={-1,-1,-1}

(17)endif

(18)endfor

3 实 验

为了检测系统的运行效果，实验针对KITTI数据集[15]和MaiCity数据集[16]进行验证。其中KITTI数据集为真实世界场景下录制的数据集，MaiCity数据集为基于仿真世界建立的静态场景数据集。实验的硬件条件为i7-1800H，显卡为RTX3060，操作系统选用Ubuntu16，搭载ROS kinetic。点云处理部分使用PCL库，语义提取网络基于CUDA10.0与TensorRT构建，ICP优化部分使用ceres库，后端图优化使用gtsam构建。系统程序框架基于ROS构建，利用ROS自带的rviz可视化机器人开发工具包可以方便显示系统当前的各种状态信息。全局点云与当前机器人的位姿信息通过rviz进行显示。由于rviz中不能方便的显示面元地图，局部面元地图通过调用PCL点云库中的可视化模块进行实时显示更新。

3.1 基于真实世界的测试实验

在基于真实世界的测试实验中，选取KITTI数据集。KITTI数据集是目前自动驾驶领域的重要测试数据集。数据集主要针对室外大场景录制了包含激光雷达，灰度双目相机、IMU、GPS等多传感器数据。本实验中选取其中的激光雷达数据集odometry进行测试。其中，激光雷达型号为Velodyne HDL-64E，安装在距离地面1.65 m高的车顶。该64线激光雷达的探测距离为100 m，垂直角分辨率为0.4°，水平角分辨率为0.09°。雷达坐标系的方位为以向上为z轴，向前为x轴建立的右手坐标系。

全局点云语义地图的建图效果如图5所示。在建图过程中对全局点云使用了0.4m×0.4m的栅格体素滤波。可以看到，在全局点云地图中归属道路，树木、车辆等物体的点标记有对应的标签，其中小窗内图像是对方框内局部区域的放大效果展示。本系统能良好的实时建立全局点云语义地图，为后续机器人在导航等各个方面提供更多环境信息。

图5 KITTI 05序列运行生成的全局语义点云地图

图6展示了本系统的局部面元地图与SuMa++中的surfel面元地图的效果对比，图(a)、图(b)来自KITTI数据集05序列，图(c)、图(d)来自KITTI数据集00序列。其中图(a)、图(c)为本系统采用方法建立的面元地图，图(b)、图(d)为SuMa++方法所建立的surfel面元地图。图中语义标签与图5中相同。可以明显看出，本文通过泊松重建建立的三角面元地图比surfel面元地图在地图的连续性上效果更优，包含更多的环境细节信息。观察图中的面元地图的道路部分可以发现，surfel面元地图中的道路充满各种孔洞，而泊松重建后的面元地图中，则没有出现细小孔洞，道路保持了连续性。从图中可以看出，多余面片裁剪算法有效裁去了泊松重建中所残留的导致面片闭合的多余部分，边界清晰。同时，实验发现，当场景中存在动态物体时，泊松重建的面元地图会出现较差效果，放大点云地图中动态物体的拖尾现象。

从图5的全局点云地图的局部放大图与图6(a)、图6(b)泊松重建后的面元地图对比可以看出，泊松重建后渲染出的基于三角形面片的语义地图对比地图的点云表示更具有直观的展示效果，在人机交互中更有利于展示机器人当前所处的环境状态，提升了地图对于环境的表达能力。

在系统的实时性方面，一些关键步骤的平均运行时间见表1。定位模块平均解算时间为0.05 s，可以保证定位模块以10 Hz速度运行不丢帧，保证系统的实时性。全卷积神经网络的点云语义提取模块的运行时间也小于激光雷达的数据发射时间间隔0.1 s。全局点云地图以1 Hz的频率更新。局部面元地图由于面元重建与语义渲染需要较多的时间，所以需要8 s左右的时长，对于非自动驾驶领域的低速移动机器人而言，仍然是可以接受的。此外，由于本系统在实现中完成的是基于CPU的泊松重建，而目前基于GPU并行解算的泊松重建算法也已经趋于成熟。GPU并行的泊松重建相较CPU版本能对算法加速7倍以上[13]，可以进一步提高系统的运行效率。

3.2 基于仿真世界的测试实验

为了验证面元地图对于环境的还原精度，我们选取MaiCity数据集01段作为验证。MaiCity数据集是由恩波大学建立的CAD城市模型仿真生成的激光雷达数据集。城市模型中包含建筑、树木、汽车等城市常见元素。仿真城市模型01段中整体由两个十字路口与连接十字路口的一条马路所组成。数据集提供16线、64线及128线雷达仿真运行数据，通过比对地图数据与城市模型，可以测试SLAM算法的建图精度。实验选取64线激光雷达进行验证，对本文系统和SuMa++中的面元地图进行比较。由于选用的数据集只包含100 m的街道数据，所以此实验中选取维护40m×40m的局部面元地图。

首先定义如下评价指标，平均误差距离Et，有效面积S。令P为局部子图中的所有点的点云集合，共包含N1个点，点p为点云P中的点。Mmodel为原始城市模型中的面元集合。Mbuild为重建模型中的面元集合，共包含N2个面元。

点云的平均误差距离定义为

(14)

其中， d(p,m1) 为点p到平面m1的距离。面元的平均误差距离定义为

(15)

其中， d(m1,m2) 为平面m1到平面m2的距离。对于SuMa++中的surfel面元地图而言，每个面元包含参数半径r，定义其有效面积为：S=∑m2∈Mπr2。 对于本文方法中的三角形面元地图而言，每个面元m包含的3个顶点为pm1,pm2,pm3，定义有效面积为

(16)

实验对数据集中两十字路口之间路段进行测量，得到图7所示数据。其中横坐标表示时间，纵坐标表示面元的有效面积。虚线表示本文系统的局部面元地图的有效面积，实线表示surfel面元地图的有效面积。为了对齐两种方法的数据，表中的横坐标时间轴进行了拉伸处理。图中两种方法在起始时和终止时均有翘起的局部峰值出现，对应于机器人处于两个十字路口时的数据，中部较为平缓区的数据对应于仿真城市模型中的一字型马路部分。从表中可以看出，本文基于泊松重建后建立的三角形面元地图在有效面积上全程都要远优于surfel面元地图。

图7 面元地图有效面积对比

为了更有效对比两者的平均距离误差与有效面积。选取数据集中两十字路口中间的一字型马路部分进行多次测量，得到表2所示的平均测量参数。从表中的数据可以看出，本文方法在平均误差距离的定义标准下，在此数据集的静态场景中不论是从全局点云还是从局部面元的两方面分析，其建图效果都要优于SuMa++方法所建立的surfel面元地图，平均误差距离Et更小。同时从面元有效面积上分析，基于泊松重建的三角面元的表示方法对原始模型的还原能力约为surfel面元的1.57倍。考虑到实际surfel面元还存在大量面元之间相互遮叠的情况，两者之间的差距应该更大。

表2 平均距离误差与有效面积

4 结束语

本文针对户外大场景下移动机器人的定位与建图，提出一种局部面元与全局点云结合的激光SLAM系统，能够在保证系统实时性的基础上，提升地图对于环境的表示能力。基于特征提取的帧间匹配保证建图精度，通过泊松重建构建的三角形面元对比surfel面元能将地图有效面积提升1.5倍以上。利用全卷积神经网络基于球面投影的方法能够有效对点云进行语义分割。在本方法中暂未对动态物体进行处理，结合语义信息加强动态环境下的定位精度与动态环境下面元地图的处理优化是下一步的改进点。基于GPU加速的泊松重建提升局部地图的刷新速度也是今后的改进方向。