激光室内定位的混合式高效点云地图构建方法

史鹏程，李加元，张永军

(1. 武汉大学计算机学院, 武汉430072；2.武汉大学遥感信息工程学院,武汉 430079)

0 引言

随着自动化及无人驾驶的不断发展，导航定位技术已广泛应用于物流配送、军事侦察、环卫清洁等众多领域。高精度定位是无人驾驶和移动机器人系统的核心组成部分。近年来，随着定位技术的不断迭代，新兴的地图定位方法可有效解决车辆或机器人的绑架(Kidnapping)问题。目前，众多科研机构和商业公司正致力于打造适用于自动驾驶的高精度地图(High Definition Map，HD Map)。室内环境下,由于无法接收全球定位系统(Global Positioning System，GPS)信号，机器人的自主定位与地图创建受到一定的限制，常采用高精度三维激光扫描仪采集并拼接点云地图。这导致地图数据十分庞大，不利于数据的存储与管理，因此研究一种高效且丰富的地图创建管理方法对于室内机器人的定位研究至关重要。

目前,众多学者已对点云地图的创建和管理进行了大量研究,主要有四种代表性地图：点云地图、语义地图、栅格地图及拓扑地图。点云地图中包含海量的点云数据，存储开销较大。LOAM(LiDAR Odometry and Mapping)方法利用曲率计算出机械式激光雷达数据中的特征点，通过最小化点到线和点到面之间的距离估计出位姿转换关系，采用低频的方式构建了稠密的点云地图。LOAM-Livox对LOAM进行了改进，在特征提取中加入了反射强度值作为判断依据，利用固态激光雷达完成地图构建任务。但以上定位与建图方案若缺少后端优化，在长时间运行下位姿会出现明显的漂移，导致地图误差较大。使用传统的点云配准方法如迭代最近点 (Iterative Closest Point，ICP)，对地面三维激光扫描仪的多站数据进行配准，也可得到稠密的点云地图。但是涉及的配准方法对数据初始位置要求较高，容易失败。此外，海量的点云也会造成配准效率较低。

语义地图可有效提高机器人对周围环境的理解能力。文献[13]将条件随机场(Conditional Random Field, CRF)-循环神经网络(Recurrent Neural Net-work, RNN)与基于ORB特征识别的同时定位与建图系统(Oriented FAST and Rotated BRIEF, ORB-Simultaneous Localization and Mapping, SLAM)相结合，实现语义信息和三维点云的关联，形成语义地图。SuMa++利用RangeNet++网络对LiDAR点云进行语义分割，构建了高精度的语义地图。通常语义地图的信息提取较为复杂，大范围自动驾驶场景需要繁琐的人工标注步骤，代价较高。栅格地图可在二维空间中描述周围地物的分布状态，由于信息复杂度降低，定位效率显著提高。Cartographer将机器人当前扫描数据与局部地图进行匹配，快速地计算出机器人位姿，并构建了栅格地图。但是该方法在位姿融合中采用无迹卡尔曼滤波(Unscented Kalman Filter,UKF)，容易出现位姿融合不准确，存在延迟等问题。文献[16]利用Kinect相机中的彩色图和点云，结合视觉SLAM技术构建了占据格网地图(Occu-pancy Grid Map，OGM)。拓扑地图关注场景中地物之间的连通关系，这使得其十分有利于机器人的控制规划任务。但是拓扑地图中缺少表达真实地物的特征信息，使得机器人对目标的感知和理解能力不佳。

机器人在实时运行过程中，鲁棒的定位和控制规划算法可以有效保证系统的安全性。复杂的外界环境对机器人的上述两种模块均具有较大挑战性。若存在鲁棒的地图表达，则在相同的硬件条件下，定位和规划算法的鲁棒性将得到有效提高。为此，本文针对目前室内环境下点云地图构建方法中形式单一、存储空间大、特征信息匮乏等不足之处，提出了一种融合特征地图、通行地图和精简地图的点云地图构建新方法。该方法将原始单一的点云地图组织为具有三种地图形式的混合地图，在为机器人定位提供特征信息的同时，还可为其控制规划提供边缘和通行信息，最后在保留场景主体信息的前提下，实现地图数据的精简，降低了点云地图的存储空间，为点云地图的创建与管理提供了一种新方法。

1 算法框架

本文地图构建方法的技术路线如图1所示，主要包括：A特征地图构建，B通行地图构建，C精简地图构建。在构建特征地图时，本文首先考虑将法线和曲率突变点选取为特征点，并利用特征值间相互关系及非极大值抑制选取出局部显著点。在构建通行地图时，本文认为室内布局符合曼哈顿假设，以平面分割为基础，通过平面之间的空间关系分割出地面，进一步利用障碍物高度构建2D范围的可通行区域,并将三维空间中聚类目标的轮廓作为3D辅助信息,以指示机器人通行区域。在构建精简地图时，本文将点云按照空间格网划分，利用特征值关系将格网分为线性、平面和球面三类。针对不同类型的格网点云，采用与其相适应的点云精简方法，实现点云地图的精简。

图1 地图构建技术路线Fig.1 Technical route of map construction

2 特征地图表达

2.1 曲率和法线特征点

曲率可以间接地反映局部凹凸程度，是一种典型的特征点筛选依据。通常大曲率值的位置弯曲程度较大。本文首先采用KD树在点云地图中建立拓扑结构。对于任意三维点，其邻域点集为={,,…,}，构建如下协方差矩阵

(1)

(2)

式中，是邻域集合的重心点坐标。协方差矩阵为半正定对称矩阵，采用特征值分解得到特征值,,(>>)和特征向量,,。描述了某曲面沿法向量的变化程度，和则表示邻域内各点在切平面的分布情况。计算任一点的曲率如下

(3)

选取作为当前点法线的估计值。根据式(4)、式(5)分别挑选出曲率特征点和法线特征点

(4)

(5)

式中，[,]表示计算法线和的夹角；和为两阈值。

2.2 局部显著点

本文通过邻域点之间的特征值关系,挑选出在局部范围内具有代表性的点，称为局部显著点。主要包括以下步骤：

1)采用Alpha Shape算法提取整个点云地图中的边缘点。边缘点的位置远离内部特征区域，不可作为局部显著点。

2)在每一个非边缘点的邻域点集中寻找是否存在边缘点，若存在，则将当前非边缘点定义为边缘临界点，该类点同样不可作为局部显著点。

3)由于2.1节中已计算每一个地图点的特征值、曲率及法线，在同时满足非边缘和非边缘临界点的点中，判断其特征值是否满足如下关系

(6)

式中，和为两个比例阈值。

4)对于任意满足式(6)的地图点，判断其特征值是否为邻域点集中最大的第三特征值。若为局部最大值，则该点为局部显著点。

3 通行地图表达

3.1 场景边缘点

边缘信息是对目标轮廓的直接描述。在点云地图中获取目标的轮廓先验信息，有利于保证机器人系统的安全性。在点云地图中，本文首先采用欧式聚类，将点云地图分成多个尺寸不同的点云聚类，并利用包围盒体积描述每一个聚类的尺寸。部分小聚类目标对于机器人实时运行影响较小，剔除盒子体积较小的点云聚类。在剩余的点云聚类中，使用Alpha Shape找出每个聚类的边缘点，并将边缘点作为场景边缘信息保存。在这里，地图中各聚类的边缘作为静态目标的3D轮廓先验信息，若机器人附近某时刻出现较多边缘点，应及时进行路径规划。

3.2 2D通行地图

本文中的2D通行地图即在平面内给出机器人可通行的位置。室内通常为多平面环境，这种环境布局满足曼哈顿假设。同时，室内坡度起伏较小，地面可以近似为一个平面特征。基于以上特征分析，该方法通常适用于室内环境。本文采用图2中技术路线，利用点云地图构建2D通行地图。假定点云地图的坐标系未进行水平方向校正，即地图坐标可在任意坐标系下。主要包括以下步骤：

图2 2D通行地图构建流程Fig.2 Construction pipeline of 2D passable maps

1)平面分组与合并。采用区域生长方法分割出平面点云，将近似平行的平面分为同组，得到={},其中为第个平面分组，表示该组内第个平面。每组选取点数最多的平面。

2)确定地面所在分组。曼哈顿假设表明室内环境主要有3个平面方向，即1个水平和2个竖直方向，地面隶属于水平方向。选出包含平面数量最多的3个平面分组={,,}。在每组平面中，按式(7)～式(9)计算出变换矩阵,对点云地图进行旋转和平移变换。其中式(8)为罗德里格斯旋转矩阵计算公式。计算转换后的地图在平面的投影面积，本文采用平面内外接矩形的面积近似表达投影面积。室内环境下地面的面积通常会大于墙面的面积。因此，投影面积最大的平面分组则是包含地面的分组。

(7)

=+sin+(1-cos)

(8)

=[]

(9)

式中，=(×)，×为叉积运算，为每组所挑选的代表性平面的单位法向量，=[0 0 1]；为旋转矩阵，使在旋转后与平行；为与之间的夹角；＾表示反对称矩阵运算；(,,)为平面的重心坐标。

3)地平面检测。在平面分组中，地面具有两个显著的特性：(a)地面位于该组平面的两侧，属于最外层的平面；(b)当该组平面中同时存在地面和棚顶平面时，室内地物距离地面的距离明显小于棚顶。据此，提取出中的地平面。

4)通行图构建。采用栅格化手段，在地平面点云中，沿平面方向生成二值图。若栅格内包含地面点，则为目标像素，否则为背景像素。栅格化的同时保留其坐标。采用Canny算子在转为灰度图的上检测其轮廓，填充最大轮廓内的所有孔洞元素,得到填充图。预先设定障碍物高度范围，将地图中位于高度范围内的地物点及3.1节中所提取的边缘点，映射到中作为背景要素。按式(10)计算出可通行栅格图，并反算回三维空间中。

(10)

4 精简地图表达

在机器人的全局定位中，常利用实时数据与地图进行精匹配,进而解算位姿。然而，特征地图的原始信息过少，点云地图的维护与动态加载等需要消耗巨大资源。为此，本文提出了一种点云地图精简方法，旨在降低点云存储空间的同时，具备以下特性：1)为传感器的精匹配定位提供数据支持；2)在保留场景主体信息的前提下，实现地图数据的快速预览和可视化，以便于地图进行检查和更新等操作。

4.1 空间格网划分

首先,在地图点云中划分三维体素网格如下

(11)

(12)

式中，和为比例阈值。参考文献[21]中参数设置，选取==01。对网格内点云精简时，预先设定初始数据精简比值=01，网格内原始点云总数为。

4.2 线性网格精简

当第一特征值远大于第二特征值时，网格内点云在某主方向的分布显著大于其他方向，整体呈线状。为此，提出了基于主方向权重的点云精简方法，权重计算方式如式(13)所示。根据格网内各点的权重，保留网格内权重位于前(1-)的点。

(13)

4.3 面类网格精简

当第一、二特征值相近，且第二特征值显著大于第三特征值时，网格内点云在前两个主方向的分布相近，第三方向分量权重显著小于其他两方向，网格内点云呈面状分布。面状点云通常无明显几何特征，本文采用随机采样的方式精简网格内的点云。固定输出后的点数为(1-)，对网格内点云随机采样。

4.4 球类网格精简

当3个特征值相差不大时，网格内点云不在任一方向上存在显著分布，整体呈球面状。本文采用K均值聚类的方法对网格内的点云进行精简。主要包括以下步骤：

5 实验与分析

5.1 实验数据与环境

为验证本文方法的可行性，采用图3中3组室内稠密的点云地图进行测试，实验场景分别为卧室、办公室和会议室。其中卧室和会议室数据来自文献[22]中的实验数据，办公室数据是利用FARO扫描仪在真实场景下多站扫描拼接得到。其中卧室与会议室数据坐标系统存在任意性，不便于渲染显示；办公室数据坐标系统在采集之前对设备进行严格的整平，因此根据高度对办公室数据进行渲染显示，以提高可读性。3个场景均包含显著的地平面，符合本文方法中所述的地面特点。此外，办公室场景还存在天花板。实验环境为Intel Core i9-10850K，Ubuntu20.04LTS系统，采用C++编程实现。

(a) 卧室

(b)办公室

(c)会议室

5.2 实验结果

1)参数设置。对特征地图中涉及的3个关键参数进行实验，分析在不同取值情况下的特征点数量分布。图4所示为卧室场景下曲率阈值、局部显著点中特征值比例阈值(=)、法线偏差阈值与特征点数量分布的曲线图。为提高显示效果，纵坐标采用对数表示法。纵坐标为0时，表示未检测到特征点。综合各类特征点的数量，本文分别选取=06，==07，=50进行后续实验。点云地图通常数据海量，在计算特征值、曲率和法线时，具有丰富的邻域信息，因此该参数阈值可应用于一般性的点云地图。

(a) εc和ν1

(b) εn图4 参数设置实验Fig.4 Experiments of parameters setup

2)地图构建效果。(a)特征地图：图5所示为各场景中构建的特征地图，3个场景中分别提取了9089、14422和12172个特征点。(b) 通行地图：图6所示为在不同高度范围内的2D通行图，其中红色表示该位置附近存在障碍物不可通行，绿色为可通行区域。图中构建了5个高度范围内的2D通行地图，分别为0～0.4m，0～0.8m，0～1.2m，0～1.6m和0～2.0m。图7所示为3D空间中的边缘图，可直接在三维空间反映出目标的边缘细节。(c)精简地图：图8所示为初始精简比率设置为0.9时的精简地图。从图8中可以看出，地图点云的精简程度较大时，本文方法仍可较好地保持整个场景的主体信息和细节特征。其中高度阈值范围是根据室内环境下的移动平台高度而设定的，本文实验选取0.4m，可用于一般室内环境下的机器人平台，若使用某种大型机器人，则相应地调整高度阈值即可。

3)地图内存分析。表1记录了本文方法构建的三种地图的内存。其中办公室场景由于使用高精度扫描仪采集，原始内存达到了1.6Gb，地图的存储和加载十分浪费资源。本文构建的特征地图内存均小于1Mb。由于会议室的地面面积显著大于其他两场景，导致其通行地图和精简地图内存相对较大，但仍仅需2Mb。同时为了实现以上内存统计的统一，均是以ASCII格式存储的，若使用二进制格式存储，内存将进一步减少。

表1 地图内存

4)运行效率分析。 表2记录了本文方法构建的三种地图的运行时间。卧室场景相比其他两场景，布局和地物较简单，因此其地图构建的速率也相对较快，其中特征地图需要约43s，通行地图大约23s，精简地图大约42s。办公室场景由于点云密度最大，因此其特征地图和精简地图的构建耗时最长，分别需要约74s和79s。会议场景由于显著地面的原因，使得在通行图构建时最为耗时，约36s。由于点云地图数据量较大，构建KD树等结构较为耗时，同时构建地图时需要进行大量的计算，在实时运行效果方面仍有待提高。对于室内激光定位等任务，通常会预先采集点云地图，地图的构建任务也可离线完成，因此本文的地图构建管理方法仍可有效应用于室内激光定位中。

表2 运行时间

(a) 卧室

(b) 办公室

(c)会议室

(a) 卧室

(b) 办公室

(c) 会议室图6 2D可通行图Fig.6 2D passable maps

(a) 卧室

(b) 办公室

(c)会议室

(a) 卧室

(b) 办公室

(c)会议室

5.3 精度评价

1)地面分割精度。本文中的2D通行图是基于地面分割结果所构建的。为此，首先对地面分割的精度进行评估。利用CloudCompare软件的语义标注功能，人工标记出室内场景的地面点云作为真值数据，采用式(14)和式(15)计算出地面分割的准确率Precision和召回率Recall

(14)

(15)

式中，TP为正确分割的地面点数；FP为错误分割的地面点数；FN为漏分割的地面点数。表3详细记录了各场景地面分割的准确率和召回率。表中为真实地面点数，为本文方法分割的地面点数。3个场景的地面分割准确率均达到95%以上，分割的召回率在85%～90%之间。本文假定室内地面为平面，并从区域生长所分割的平面中选择出地平面。区域生长在分割平面时利用邻域点的法线角度实现种子点的扩充。在平面边缘附近处法线的偏差开始逐渐增加，区域生长在到达真实边缘之前，提前停止生长造成欠分割。

表3 地面分割精度

2)精简性能。实验选取随机采样和体素采样作为对比方法，评估点云地图的精简效果，在精简点云中采用滚球法(Ball Pivoting)重建曲面，并采用文献[24]中的模型误差评估精简质量。

(16)

式中，Δ为模型误差均值，为原始点云地图，′为重建后的曲面；(,′)表示上的一点到重建曲面′上的投影距离。图9描述了各方法在不同精简程度下的模型误差均值和标准差。从图9中可以看出，本文方法和体素格网方法在模型误差方面优于随机采样方法。即使当点云地图减少到原数据的5%时，仍能取得0.8mm的最佳模型精度。由于精简方法中综合了主方向加权、随机采样和K均值聚类方法，多种采样方法使得模型误差的标准差稍大于体素采样。

(a) 模型误差均值

(b) 模型误差标准差图9 精简质量评估Fig.9 Simplification performance evaluations

6 结论

1)针对机器人室内激光定位中的地图创建管理问题，提出了一种新的混合形式的地图构建方法。将稠密的点云地图组织为特征地图、通行地图和精简地图三种，克服了传统地图形式单一的局限性，强化了地图在机器人系统的作用。

2)选取部分曲率、法线特征点和局部特征点作为显著特征信息，构建场景特征地图，可为移动平台提供鲁棒的点特征信息。

3)基于曼哈顿假设，以室内地平面检测为基础，构建2D通行地图和3D边缘地图，可以为机器人在室内环境下的控制规划提供良好的先验信息。

4)融合主方向权重、随机采样和K均值聚类等方法，在保留数据主体信息的前提下，显著降低地图的存储开销。既可为传感器精匹配定位提供数据支持，还利于实现地图数据的快速可视化。

文中选取三种典型的室内场景进行实验，结果表明，本文地图构建方法在保留场景特征的同时，可提供鲁棒的通行信息，并可有效降低地图存储空间。后续将研究基于深度学习的目标实例分割方法，为通行图提供更准确的边缘信息。