CN112799096B - 基于低成本车载二维激光雷达的地图构建方法 - Google Patents

Abstract

本发明公开了基于低成本车载二维激光雷达的地图构建方法，设置全局点云地图的基准坐标系；获得当前帧扫描点云为第一帧扫描点云时，从第一帧扫描点云的扫描点中获得特征点，将第一帧扫描点云的扫描点和特征点导入到全局点云地图的坐标系中形成初始化全局点云地图；第一帧扫描点云的特征点作为初始化全局点云地图的全局特征点，第一帧扫描点云的扫描点作为初始化全局点云地图的融合点；获得后续当前帧扫描点云时，采用前端融合全局特征点匹配法，将后续当前帧扫描点云的特征点变换到基准坐标系后与上一更新全局点云地图的全局特征点进行融合，将后续当前帧扫描点云的扫描点变换到基准坐标系后与上一更新全局点云地图的融合点进行融合。

Description

基于低成本车载二维激光雷达的地图构建方法

技术领域

本发明涉及地图构建领域，主要涉及基于低成本车载二维激光雷达的地图构建方法。

背景技术

移动互联网及智能传感器等技术的实时发展，推动了基于位置服务相关应用的普及。在地下停车场、购物中心、医院、机场等大规模室内建筑空间的定位研究成为室内位置服务的研究热点。

近年来基于地图辅助的室内定位技术受到人们的广泛关注，该技术通过增加平面地图约束来实现更准确、稳定的室内定位。室内环境的感知以及室内平面地图的构建是在室内进行导航定位中不可或缺的一部分。

对于室内地图构建问题来说，传统的方法是利用全站仪在室内重复架站，测量获取控制点坐标，然后再事后处理成图；目前新兴的测量方法主要有基于激光雷达和惯性器件的组合测量法、视觉里程计法。

背景技术的缺陷：

研究表明，对于室内地图构建问题来说，目前主流的方法有全站仪测量法、激光雷达和惯性器件组合法、视觉里程计法。

全站仪测量法是通过全站仪在室内重复架站，测量获取控制点坐标，然后再事后处理成图。虽然该方法测量精度高，但测量作业过程不安全，执行效率低，不便于大范围的推广使用。

三维激光雷达和惯性器件的组合测量法具有较高的测量精度，但三维激光雷达价格昂贵，数据量大，数据处理过程复杂，难以满足实时性，且传感器标定、时间同步较为复杂，其更适合需求为三维地图的构建。

视觉里程计法是通过匹配相邻时间序列图像来确定相机的位置和姿态，从而进行地图的构建，但该方法传感器受环境光照、纹理的影响较大，不具有较好的可靠性，且图像数据量大，数据处理过程复杂，难以满足实时性。

发明内容

本发明目的提供基于低成本车载二维激光雷达的地图构建方法，该方法同时具有低成本、高效率、高精度、实时运算处理得到室内平面地图的实际应用价值。

本发明通过下述技术方案实现：

基于低成本车载二维激光雷达的地图构建方法，包括以下步骤：

步骤S1、解析数据：

获取2D激光雷达扫描输出的当前帧扫描点云；从当前帧扫描点云中解析出多个扫描点和当前帧扫描点云的坐标系；

步骤S2、设置全局点云地图的基准坐标系；

步骤S3、初始化全局点云地图：

获得当前帧扫描点云为第一帧扫描点云时，从第一帧扫描点云的扫描点中获得特征点，将第一帧扫描点云的扫描点和特征点导入到全局点云地图的坐标系中形成初始化全局点云地图；第一帧扫描点云的特征点作为初始化全局点云地图的全局特征点，第一帧扫描点云的扫描点作为初始化全局点云地图的融合点；

步骤S4、更新全局点云地图：

获得后续当前帧扫描点云时，采用前端融合全局特征点匹配法，将后续当前帧扫描点云的特征点变换到基准坐标系后与上一更新全局点云地图的全局特征点进行融合，将后续当前帧扫描点云的扫描点变换到基准坐标系后与上一更新全局点云地图的融合点进行融合，从而构建当前更新的全局点云地图。

本发明的技术构思如下：

首先，获取2D激光雷达扫描输出的当前帧扫描点云的设备，称为采集系统，采集系统例如由2D激光雷达设置在移动平台（移动的人、移动的载具）上构成，然后使得移动平台在目标区域（一个停车场、商场等室内区域）。

由于本发明的目标区域是特殊的场景区域，其具有反射物，因此，2D激光雷对其探测后，会感应到扫描点，2D激光雷的输出可以理解为一个雷达扫描图（1帧画面），雷达扫描图在本发明中称为扫描点云，在该扫描图内具有扫描点，扫描点是标定反射对象的位置。其改进2D激光雷后使得其输出扫描点在2D激光雷的坐标系下的横纵坐标。由于2D激光雷的扫描范围有限，需要连续的移动2D激光雷达，这样会连续的得到多帧的雷达扫描图，可以设置与基于低成本车载二维激光雷达的地图构建方法所对应的计算系统来实现对上述雷达扫描图的处理；从而将多个雷达扫描图连续重构获得完整的全局点云地图。该过程有点类似图像拼接过程。

由于，2D激光雷输出的扫描点云，一般存在大量的扫描点，信息数据量大。为了解决其数据处理的难度，本发明的核心思想是：从1帧的扫描点云中寻找到特征点，然后利用当前帧的扫描点云的特征点与前一更新的全局点云地图的特征点现场特征点对，基于该特征点对，计算获得当前扫描点云变换到基准坐标系所需的变换参数，基于该变换参数将扫描点变换到前一更新的全局点云地图中得到当前全局点云地图；即可实现将后续当前帧扫描点云的扫描点变换到基准坐标系后与上一更新全局点云地图的融合点进行融合，从而构建当前更新的全局点云地图；其本质是，利用特征点最为变换计算的基准（参考系统），从而实现全局的变换。因此，本发明采用的是一种提取特征点的匹配算法，通过提取特征点能大幅度减小点云数据量，为实时构图提供保障。例如，本发明先以第一帧扫描点云作为全局地图基准（第1个全局点云地图，也称初始化全局点云地图），然后将后续扫描的点云变换到第一帧扫描点云（初始化全局点云地图）基准上，第二帧扫描点云和初始化全局点云地图得到第二全局点云地图，第三帧扫描点云和第二全局点云地图得到第三全局点云地图，以此类推，在每一个新帧扫描点云进入后，更新得到1个全局点云地图，直到最后1个新帧扫描点云进入后得到最后1个全局点云地图，从而持续构建地图。

进一步的技术方案有：

由于，需要得到1个初始化全局点云地图，而第一帧扫描点云并不能直接作为初始化全局点云地图，因此，需要对第一帧扫描点云进行处理，从而得到可以应用的初始化全局点云地图。

因此：

步骤S3的具体过程为：

步骤S31、对第一帧扫描点云进行点云区域分割；

步骤S32、对完成区域分割的第一帧扫描点云进行特征直线提取、并基于特征直线进行特征点提取；

步骤S33、对完成特征点提取的第一帧扫描点云进行特征点筛选；

步骤S34、将步骤S33所获得特征点导入到初始化全局点云地图中，作为初始化全局点云地图的全局特征点；将第一帧扫描点云的扫描点的坐标导入到初始化全局点云地图中，作为初始化全局点云地图的融合点。

步骤S3的具体过程的目的是将第一帧扫描点云的信息提取出特征点，然后基于基准坐标系和特征点第一帧扫描点云的特征点构建初始化全局点云地图。然后便于后续帧扫描点云以初始化全局点云地图为初始基准进行叠加。

所述步骤S4具体过程为：

步骤S41、对当前帧扫描点云进行点云区域分割；

步骤S42、对完成区域分割的当前帧扫描点云进行特征直线提取、并基于特征直线进行特征点提取；

步骤S43、对完成特征点提取的当前帧扫描点云进行特征点筛选；

步骤S44、对完成特征点筛选的当前帧扫描点云进行动态初始化粗匹配到在上一更新的全局点云地图上；

步骤S45、以上一更新的全局点云地图的全局特征点为特征点对的寻找基准、搜索对应的当前帧扫描点云的特征点后形成特征点对；

步骤S46、利用最小二乘法对选取的多组特征点对进行迭代平差，从而求得当前帧扫描点云相对于基准坐标系的旋转角变换参数和平移量变换参数、相对位姿变换参数；

步骤S47、最后根据旋转角变换参数和平移量变换参数、相对位姿变换参数，进行扫描点融合或/和进行特征点融合得到当前更新的全局点云地图；

扫描点融合：将当前帧扫描点云的扫描点变换到基准坐标系内，并与上一更新的全局点云地图中的融合点进行叠加，得到当前更新的全局点云地图的融合点；

特征点融合：将当前帧扫描点云的特征点变换到基准坐标系内，并与上一更新的全局点云地图中的全局特征点进行叠加，获得当前更新的全局点云地图的全局特征点，当前更新的全局点云地图的全局特征点作为下一待匹配帧扫描点云的特征点对的寻找基准；

步骤S48、重复S41-S47步骤，持续获得当前更新的全局点云地图。

上述步骤S4具体过程中的当前帧扫描点云应至少是第二帧扫描点云。其中，全局特征点的作用是：由于现有点云匹配方法，大多数是通过匹配相邻两帧点云数据进行变换参数求解，并依次传递相邻变换参数来递增的构建地图，该方法随着点云帧数的增加，其累加误差会明显增大。对此，为了减小累积误差，本发明通过融合每次匹配完后的点云特征点，将融合后的全局特征点作为下一次匹配的基准，让更多的全局信息参与后一帧的匹配，来完成每帧的全局匹配，从而减小累加误差，为后续回环检测提供一个良好的位置基准。

上述过程可以理解为以下具体示意：

假设第一帧扫描点云具有80个扫描点，80个扫描点中有20个扫描点为筛选后的特征点，因此，可以构建含有该20个特征点和80个扫描点在基准坐标系内的初始化全局点云地图；在第二帧扫描点云获得后，从第二帧扫描点云的60个扫描点中，找到15个扫描点为筛选后的特征点；之后，将15个特征点与初始化全局点云地图的20个特征点进行配对形成特征点对，再基于特征点对计算变换参数，最终利用变换参数将第二帧扫描点云的60个扫描点和15个特征点变换到初始化全局点云地图中，这样在初始化全局点云地图中具有80+60个扫描点的叠加，其特征点有20个特征点+15个特征点的叠加，最终得到含有上述叠加状态的当前更新的全局点云地图，再基于该当前更新的全局点云地图作为前一更新的全局点云地图作为基础，对第二帧扫描点云进行处理。以此类推。

具体的，所述步骤S41中，根据扫描点间的欧氏距离对当前帧扫描点云进行点云区域分割，其中扫描点间的欧氏距离为Q，动态分割阈值为δ；当Q大于δ时进行点云区域分割，Q小于或等于δ时不进行点云区域分割；

其中式1：

；

式1中，δ表示动态分割阈值，r表示扫描点到激光雷达中心的距离，α表示相邻扫描点的角度间隔，d表示激光雷达中心到相邻扫描点所在直线的距离；

所述步骤S42中，采用IEPF分割算法来对各个分割后的点云区域进行特征直线提取；由于当激光雷达到环境建筑物的距离不同时，其观测到的扫描点的个数以及相邻距离都不相同。对此，可以以待提取区域的两端点连线距离为标准，设定其分割阈值为该端点距离的15%-18%来进行直线特征的提取。

所述步骤S42中，将特征直线的端点、角点视为特征点进行特征点提取；本发明采用特征直线的端点、角点作为特征点，有利于针对本发明的应用领域，本发明是应用于建筑物，在建筑物中存在边墙，用2个端点表征，角点表示墙角的变化，对于两个不同的帧数据，这些特征点具有很明显的共性，有利于准确提取特征点和形成后续的特征点对。本发明采用特征点来进行不同扫描时刻的点云匹配能大幅度压缩点云的数据量，为室内平面地图的实时创建提供了重要保障，因此特征点的准确提取在点云匹配中起到了至关重要的作用。

所述步骤S43中，特征点筛选的标准为：若特征点为边缘点，则进行剔除筛选，若特征点为到激光雷达中心的距离在设定的极限阈值范围内则进行剔除筛选。本发明中，由于其应用利用为针对建筑物，本发明的激光雷达是按照一定的角度，由近向远发射脉冲激光束，当激光束遇到障碍物时反射回来，从而进行扫描采集。因此，一方面会存在当后面的建筑物被前面的建筑物挡住时，后面的建筑物会出现间断点的情况，而这样的间断点会被之前的操作误当做直线的端点提取出来，因此，本发明要进行边缘点的剔除，避免错误的特征点存在，由于激光雷达是有一定的测距范围，因此当扫描点到激光雷达中心的距离在极限阈值范围时，我们也应将其剔除，避免错误的特征点存在。

所述步骤S44中，粗匹配过程为：

将上一帧扫描点云求取的最佳变换参数R_t，作为当前帧扫描点云进行变换所需变换矩阵的初始值

，对其进行初始化，完成粗匹配；

其中，上一帧扫描点云求取的最佳变换参数R_t包括：上一帧扫描点云相对于基准坐标系的旋转角变换参数和平移量变换参数、相对位姿变换参数。

本发明中，由于利用2D激光雷达采集到的连续点云数据的位姿变换参数具有强相关的特性。采用将上一帧点云求取的最佳变换参数R_t作为下一帧待匹配点云变换矩阵的初始值

，对其进行初始化，完成粗匹配，使其能与全局地图较好的匹配在一起，为后续对应点对搜索提供更好的初值基准，以及减少后续迭代匹配次数，提高匹配效率。

所述步骤S45中，特征点对的形成过程为：

将选择的1个当前帧扫描点云的特征点视为待匹配特征点，求这1个待匹配特征点与上一更新的全局点云地图中每个全局特征点的距离，将距离最小的全局特征点与该待匹配特征点组成特征点对。

具体的，多个当前帧扫描点云由2D激光雷达在目标区域内不同位置处扫描获得。本发明针对室内平面地图构建所采用的低成本、高效率的车载二维激光雷达实时采集系统。

具体地，移动平台可选择各类汽车等移动装置，通过移动平台在室内环境进行不断移动来高效率、实时地构建平面地图。

具体地，激光点云采集传感器（激光雷达）使用的是百元到千元级的低成本2D激光雷达，通过编写程序对其二次开发后，能实时输出采集点（扫描点）的（x、y）坐标的输出，并将该采集点（扫描点）的（x、y）坐标的输出作为本方法的唯一数据输入源。

具体地，例如，本发明的执行主体可以是：一种实时计算平台，比如是在一台笔记本电脑上实现，对采集的扫描点云进行实时处理，生成平面地图。

其中，所述2D激光雷达搭载在移动平台上于目标区域内连续移动位置，2D激光雷达根据扫描时刻的不同，形成多个一一对应于不同移动位置的当前帧扫描点云。

全局点云地图的基准坐标系一般可以是系统自己设置形成，但本发明考虑到人工设置的参数与实际的存在较大差距，因此不利于后续变换，导致变换误差较大。因此，本发明优选以将第一帧扫描点云的坐标系设置为全局点云地图的基准坐标系，后续每个新的全局点云地图的基准坐标系是不变的。

进一步的，由于本发明采用的连续行进，获得不同位置处的扫描点云，然后迭代的方式更新全局点云地图；因此，其最终的全局点云地图会存在精度不够的问题，特别的，随着扫描的帧数的越多，精度会越差。为了解决该问题，本发明提出了在发送回环时，进行优化的处理，该优化仅针对最终的全局点云地图进行。具体地，本发明提出的后端整体间接平差优化时机是：通过回环检测判断当前帧数据是否与之前扫描过的帧数据发生回环（回环即是否达到先前扫描过的位置），如果发生回环，则利用本发明提出的整体间接平差模型进行平差优化，从而提高以建地图的精度，得到最后的优化地图。

因此，进一步的技术方案还包括步骤S5、步骤S6，步骤S5、步骤S6于步骤S4之后执行；

步骤S5：对每次当前更新的全局点云地图进行回环检测，若检测状态为回环，则执行步骤S6；

步骤S6：采用间接平差模型对整体的回环数据进行优化，获得优化后的全局点云地图；

整体的回环数据是指：发生回环时，形成回环的所有全局点云地图的集合。

本发明与现有技术相比，具有如下的优点和有益效果：本发明提出了一种同时实现低成本、高效率、高精度的室内地图实时构建方法。

1、低成本：仅仅利用单个车载二维激光雷达建图，不依赖其他诸如IMU、里程计等传感器，省去多传感器标定、时间同步等过程，设备架设成本低，处理过程简单，人工成本低，运行简单，运营成本低。

2、高效率：通过本发明执行提出的车载二维激光雷达实时构图平台，车载二维激光雷达具有快速扫描的能力，直接开车在场地内正常交通规则速度行进也可以实现，无需超低速采集，耗时约为180秒，就能获得面积约为5000㎡的室内平面地图。

3、高精度：通过融合全局特征点匹配算法，能减少累积误差，为回环检测提供一个较好的地图基准。并通过整体间接平差模型进行回环优化，从而提高构图精度。

4、可实时建图，将本发明构建到移动终端中，并车载一个激光雷达，可以让驾驶员的移动终端快速获得场地的地图，并可以结合自动化标定场地的信息（比如出入口），并用得到的定位和地图信息来辅助汽车进行导航、定位、建图。

附图说明

此处所说明的附图用来提供对本发明实施例的进一步理解，构成本申请的一部分，并不构成对本发明实施例的限定。

在附图中。

图1为本发明总的流程处理图。

图2为前端融合全局特征点匹配处理的流程图。

图3为本发明对应的执行设备的组成图。

图4是本发明实施例中的输出的点云地图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施例和附图，对本发明作进一步的详细说明，本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本发明的限定。

实施例1

参见附图1：基于低成本车载二维激光雷达的地图构建方法，包括以下步骤：

步骤S1、解析数据：

获取2D激光雷达扫描输出的当前帧扫描点云；从当前帧扫描点云中解析出多个扫描点和当前帧扫描点云的坐标系；

步骤S2、设置全局点云地图的基准坐标系；

步骤S3、初始化全局点云地图：

获得当前帧扫描点云为第一帧扫描点云时，从第一帧扫描点云的扫描点中获得特征点，将第一帧扫描点云的扫描点和特征点导入到全局点云地图的坐标系中形成初始化全局点云地图；第一帧扫描点云的特征点作为初始化全局点云地图的全局特征点，第一帧扫描点云的扫描点作为初始化全局点云地图的融合点；

步骤S4、更新全局点云地图：

获得后续当前帧扫描点云时，采用前端融合全局特征点匹配法，将后续当前帧扫描点云的特征点变换到基准坐标系后与上一更新全局点云地图的全局特征点进行融合，将后续当前帧扫描点云的扫描点变换到基准坐标系后与上一更新全局点云地图的融合点进行融合，从而构建当前更新的全局点云地图。

本发明的技术构思如下：

首先，获取2D激光雷达扫描输出的当前帧扫描点云的设备，称为采集系统，采集系统例如由2D激光雷达设置在移动平台（移动的人、移动的载具）上构成，然后使得移动平台在目标区域（一个停车场、商场等室内区域）。

由于本发明的目标区域是特殊的场景区域，其具有反射物，因此，2D激光雷对其探测后，会感应到扫描点，2D激光雷的输出可以理解为一个雷达扫描图（1帧画面），雷达扫描图在本发明中称为扫描点云，在该扫描图内具有扫描点，扫描点是标定反射对象的位置。其改进2D激光雷后使得其输出扫描点在2D激光雷的坐标系下的横纵坐标。由于2D激光雷的扫描范围有限，需要连续的移动2D激光雷达，这样会连续的得到多帧的雷达扫描图，可以设置与基于低成本车载二维激光雷达的地图构建方法所对应的计算系统来实现对上述雷达扫描图的处理；从而将多个雷达扫描图连续重构获得完整的全局点云地图。该过程有点类似图像拼接过程。

由于，2D激光雷输出的扫描点云，一般存在大量的扫描点，信息数据量大。为了解决其数据处理的难度，本发明的核心思想是：从1帧的扫描点云中寻找到特征点，然后利用当前帧的扫描点云的特征点与前一更新的全局点云地图的特征点现场特征点对，基于该特征点对，计算获得当前扫描点云变换到基准坐标系所需的变换参数，基于该变换参数将扫描点变换到前一更新的全局点云地图中得到当前全局点云地图；即可实现将后续当前帧扫描点云的扫描点变换到基准坐标系后与上一更新全局点云地图的融合点进行融合，从而构建当前更新的全局点云地图；其本质是，利用特征点最为变换计算的基准（参考系统），从而实现全局的变换。因此，本发明采用的是一种提取特征点的匹配算法，通过提取特征点能大幅度减小点云数据量，为实时构图提供保障。例如，本发明先以第一帧扫描点云作为全局地图基准（第1个全局点云地图，也称初始化全局点云地图），然后将后续扫描的点云变换到第一帧扫描点云（初始化全局点云地图）基准上，第二帧扫描点云和初始化全局点云地图得到第二全局点云地图，第三帧扫描点云和第二全局点云地图得到第三全局点云地图，以此类推，在每一个新帧扫描点云进入后，更新得到1个全局点云地图，直到最后1个新帧扫描点云进入后得到最后1个全局点云地图，从而持续构建地图。

进一步的技术方案有：

由于，需要得到1个初始化全局点云地图，而第一帧扫描点云并不能直接作为初始化全局点云地图，因此，需要对第一帧扫描点云进行处理，从而得到可以应用的初始化全局点云地图。

因此：

步骤S3的具体过程为：

步骤S31、对第一帧扫描点云进行点云区域分割；

步骤S32、对完成区域分割的第一帧扫描点云进行特征直线提取、并基于特征直线进行特征点提取；

步骤S33、对完成特征点提取的第一帧扫描点云进行特征点筛选；

步骤S34、将步骤S33所获得特征点导入到初始化全局点云地图中，作为初始化全局点云地图的全局特征点；将第一帧扫描点云的扫描点的坐标导入到初始化全局点云地图中，作为初始化全局点云地图的融合点。

步骤S3的具体过程的目的是将第一帧扫描点云的信息提取出特征点，然后基于基准坐标系和特征点第一帧扫描点云的特征点构建初始化全局点云地图。然后便于后续帧扫描点云以初始化全局点云地图为初始基准进行叠加。

参见附图2：所述步骤S4具体过程为：

步骤S41、对当前帧扫描点云进行点云区域分割；

步骤S42、对完成区域分割的当前帧扫描点云进行特征直线提取、并基于特征直线进行特征点提取；

步骤S43、对完成特征点提取的当前帧扫描点云进行特征点筛选；

步骤S44、对完成特征点筛选的当前帧扫描点云进行动态初始化粗匹配到在上一更新的全局点云地图上；

步骤S45、以上一更新的全局点云地图的全局特征点为特征点对的寻找基准、搜索对应的当前帧扫描点云的特征点后形成特征点对；

步骤S46、利用最小二乘法对选取的多组特征点对进行迭代平差，从而求得当前帧扫描点云相对于基准坐标系的旋转角变换参数和平移量变换参数、相对位姿变换参数；

步骤S47、最后根据旋转角变换参数和平移量变换参数、相对位姿变换参数，进行扫描点融合或/和进行特征点融合得到当前更新的全局点云地图；

扫描点融合：将当前帧扫描点云的扫描点变换到基准坐标系内，并与上一更新的全局点云地图中的融合点进行叠加，得到当前更新的全局点云地图的融合点；

特征点融合：将当前帧扫描点云的特征点变换到基准坐标系内，并与上一更新的全局点云地图中的全局特征点进行叠加，获得当前更新的全局点云地图的全局特征点，当前更新的全局点云地图的全局特征点作为下一待匹配帧扫描点云的特征点对的寻找基准；

步骤S48、重复S41-S47步骤，持续获得当前更新的全局点云地图。

上述步骤S4具体过程中的当前帧扫描点云应至少是第二帧扫描点云。其中，全局特征点的作用是：由于现有点云匹配方法，大多数是通过匹配相邻两帧点云数据进行变换参数求解，并依次传递相邻变换参数来递增的构建地图，该方法随着点云帧数的增加，其累加误差会明显增大。对此，为了减小累积误差，本发明通过融合每次匹配完后的点云特征点，将融合后的全局特征点作为下一次匹配的基准，让更多的全局信息参与后一帧的匹配，来完成每帧的全局匹配，从而减小累加误差，为后续回环检测提供一个良好的位置基准。

上述过程可以理解为以下具体示意：

假设第一帧扫描点云具有80个扫描点，80个扫描点中有20个扫描点为筛选后的特征点，因此，可以构建含有该20个特征点和80个扫描点在基准坐标系内的初始化全局点云地图；在第二帧扫描点云获得后，从第二帧扫描点云的60个扫描点中，找到15个扫描点为筛选后的特征点；之后，将15个特征点与初始化全局点云地图的20个特征点进行配对形成特征点对，再基于特征点对计算变换参数，最终利用变换参数将第二帧扫描点云的60个扫描点和15个特征点变换到初始化全局点云地图中，这样在初始化全局点云地图中具有80+60个扫描点的叠加，其特征点有20个特征点+15个特征点的叠加，最终得到含有上述叠加状态的当前更新的全局点云地图，再基于该当前更新的全局点云地图作为前一更新的全局点云地图作为基础，对第二帧扫描点云进行处理。以此类推。

参见附图2：

具体的，所述步骤S41中，根据扫描点间的欧氏距离对当前帧扫描点云进行点云区域分割，其中扫描点间的欧氏距离为Q，动态分割阈值为δ；当Q大于δ时进行点云区域分割，Q小于或等于δ时不进行点云区域分割；

其中式1：

；

式1中，δ表示动态分割阈值，r表示扫描点到激光雷达中心的距离，α表示相邻扫描点的角度间隔，d表示激光雷达中心到相邻扫描点所在直线的距离；

所述步骤S42中，采用IEPF分割算法来对各个分割后的点云区域进行特征直线提取；由于当激光雷达到环境建筑物的距离不同时，其观测到的扫描点的个数以及相邻距离都不相同。对此，可以以待提取区域的两端点连线距离为标准，设定其分割阈值为该端点距离的15%-18%来进行直线特征的提取。

所述步骤S42中，将特征直线的端点、角点视为特征点进行特征点提取；本发明采用特征直线的端点、角点作为特征点，有利于针对本发明的应用领域，本发明是应用于建筑物，在建筑物中存在边墙，用2个端点表征，角点表示墙角的变化，对于两个不同的帧数据，这些特征点具有很明显的共性，有利于准确提取特征点和形成后续的特征点对。本发明采用特征点来进行不同扫描时刻的点云匹配能大幅度压缩点云的数据量，为室内平面地图的实时创建提供了重要保障，因此特征点的准确提取在点云匹配中起到了至关重要的作用。

所述步骤S43中，特征点筛选的标准为：若特征点为边缘点，则进行剔除筛选，若特征点为到激光雷达中心的距离在设定的极限阈值范围内则进行剔除筛选。本发明中，由于其应用利用为针对建筑物，本发明的激光雷达是按照一定的角度，由近向远发射脉冲激光束，当激光束遇到障碍物时反射回来，从而进行扫描采集。因此，一方面会存在当后面的建筑物被前面的建筑物挡住时，后面的建筑物会出现间断点的情况，而这样的间断点会被之前的操作误当做直线的端点提取出来，因此，本发明要进行边缘点的剔除，避免错误的特征点存在，由于激光雷达是有一定的测距范围，因此当扫描点到激光雷达中心的距离在极限阈值范围时，我们也应将其剔除，避免错误的特征点存在。

所述步骤S44中，粗匹配过程为：

将上一帧扫描点云求取的最佳变换参数R_t，作为当前帧扫描点云进行变换所需变换矩阵的初始值

，对其进行初始化，完成粗匹配；

其中，上一帧扫描点云求取的最佳变换参数R_t包括：上一帧扫描点云相对于基准坐标系的旋转角变换参数和平移量变换参数、相对位姿变换参数。

本发明中，由于利用2D激光雷达采集到的连续点云数据的位姿变换参数具有强相关的特性。采用将上一帧点云求取的最佳变换参数R_t作为下一帧待匹配点云变换矩阵的初始值

，对其进行初始化，完成粗匹配，使其能与全局地图较好的匹配在一起，为后续对应点对搜索提供更好的初值基准，以及减少后续迭代匹配次数，提高匹配效率。

所述步骤S45中，特征点对的形成过程为：

将选择的1个当前帧扫描点云的特征点视为待匹配特征点，求这1个待匹配特征点与上一更新的全局点云地图中每个全局特征点的距离，将距离最小的全局特征点与该待匹配特征点组成特征点对。

参见附图3，具体的，多个当前帧扫描点云由2D激光雷达在目标区域内不同位置处扫描获得。本发明针对室内平面地图构建所采用的低成本、高效率的车载二维激光雷达实时采集系统。

具体地，移动平台可选择各类汽车等移动装置，通过移动平台在室内环境进行不断移动来高效率、实时地构建平面地图。

具体地，激光点云采集传感器（激光雷达）使用的是百元到千元级的低成本2D激光雷达，通过编写程序对其二次开发后，能实时输出采集点（扫描点）的（x、y）坐标的输出，并将该采集点（扫描点）的（x、y）坐标的输出作为本方法的唯一数据输入源。

具体地，例如，本发明的执行主体可以是：一种实时计算平台，比如是在一台笔记本电脑上实现，对采集的扫描点云进行实时处理，生成平面地图。

其中，所述2D激光雷达搭载在移动平台上于目标区域内连续移动位置，2D激光雷达根据扫描时刻的不同，形成多个一一对应于不同移动位置的当前帧扫描点云。

全局点云地图的基准坐标系一般可以是系统自己设置形成，但本发明考虑到人工设置的参数与实际的存在较大差距，因此不利于后续变换，导致变换误差较大。因此，本发明优选以将第一帧扫描点云的坐标系设置为全局点云地图的基准坐标系，后续每个新的全局点云地图的基准坐标系是不变的。

进一步的，由于本发明采用的连续行进，获得不同位置处的扫描点云，然后迭代的方式更新全局点云地图；因此，其最终的全局点云地图会存在精度不够的问题，特别的，随着扫描的帧数的越多，精度会越差。为了解决该问题，本发明提出了在发送回环时，进行优化的处理，该优化仅针对最终的全局点云地图进行。具体地，本发明提出的后端整体间接平差优化时机是：通过回环检测判断当前帧数据是否与之前扫描过的帧数据发生回环（回环即是否达到先前扫描过的位置），如果发生回环，则利用本发明提出的整体间接平差模型进行平差优化，从而提高以建地图的精度，得到最后的优化地图。

参见附图1：因此，进一步的技术方案还包括步骤S5、步骤S6，步骤S5、步骤S6于步骤S4之后执行；

步骤S5：对每次当前更新的全局点云地图进行回环检测，若检测状态为回环，则执行步骤S6；

步骤S6：采用间接平差模型对整体的回环数据进行优化，获得优化后的全局点云地图；

整体的回环数据是指：发生回环时，形成回环的所有全局点云地图的集合。

本发明提出了后端整体间接平差优化处理方法，其首先通过回环检测，判断当前点云帧数据是否与之前扫描过的点云帧数据发生回环（回环即是否重复达到先前扫描过的位置），如果发生回环，则通过增加闭环约束条件，建立整体的间接平差模型对回环帧数据进行平差优化，从而提高以建地图的精度，得到最后的优化地图。

参见附图4，基于上述方法，为了更好的展示本发明的效果，本实施例具体的，以选取某停车场进行实验，该停车场面积大约5000㎡。

从构图成本、效率、精度、实时性四个方面分析本发明的实际应用效果。

1、构图成本：本发明使用的激光点云采集传感器是百元到千元级的低成本2D激光雷达，并无增加其他传感器。

2、构图效率：通过车载移动平台在室内停车场进行移动测量，耗时约为180秒，就能构建面积约为5000㎡的室内平面地图，是一种高效率的构图技术。

3、构图精度，参见附图4：在附图4中，图4为某停车场采用本发明技术实时构建的平面地图。由该图可以发现，其中的黑点即扫描点，这些扫描点都是有多个沿其行进路线连续扫描后融合到基准坐标系后形成的叠加效果。其中，在边界位置形成的线实际是由多个扫描点组成的。其精度在附图中按照厘米级标定。本发明构图技术能较好的减小累积误差，当形成回环时，能较好的回到出发点，其匹配效果较好。

为了进一步分析本发明提出的融合全局特征点匹配算法效果和整体间接平差优化模型精度，我们选取某一回环起始帧点云和回环结束帧点云的所有对应点对的欧氏距离和的平均值作为精度指标

对各算法进行评定，其精度指标

计算公式如下：

，

；

式中

为精度指标，单位为cm，

、

分别为回环起始点云帧和结束点云帧的对应特征点坐标，

为对应点对的个数。计算得本发明构图精度

。

为了更好地分析本发明构建的全局地图精度，我们用全站仪实际测量停车场的几条长边和短边，将其得到的边长距离作为标准，与本发明构图所得的对应边长度进行对比，其对比分析结果见下表1：

表1：全站仪与本发明构图精度对比表：

由此可得本发明提出的车载二维激光雷达构图相对精度达到百分之1左右，构图精度在20cm以内，满足室内地图需求，在实际应用中具有较好的匹配效果。

4、构图实时性：为了验证本发明技术的构图实时性，我们将采集的原始点云帧数据按照每秒（12帧）为一个单位进行处理，统计处理每秒（12帧）数据所需要的实际时间，对本次采集的180秒数据进行统计，其统计结果见下表2。可见每秒数据实际处理时间都小于0.5秒，具有很好的实时性。

表2每秒数据实际处理所需时间表：

由以上分析可得，本发明提出的利用车载二维激光雷达进行协同导航与实时地图构建方法，是一种低成本、高效率、高精度的实时室内平面地图构建方法，能广泛的为室内定位提供地图服务，在实际应用中具有较好的匹配效果，具有重要的实际应用价值。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.基于低成本车载二维激光雷达的地图构建方法，其特征在于，包括以下步骤：

步骤S1、解析数据：

获取2D激光雷达扫描输出的当前帧扫描点云；从当前帧扫描点云中解析出多个扫描点和当前帧扫描点云的坐标系；

步骤S2、设置全局点云地图的基准坐标系；

步骤S3、初始化全局点云地图：

获得当前帧扫描点云为第一帧扫描点云时，从第一帧扫描点云的扫描点中获得特征点，将第一帧扫描点云的扫描点和特征点导入到全局点云地图的坐标系中形成初始化全局点云地图；第一帧扫描点云的特征点作为初始化全局点云地图的全局特征点，第一帧扫描点云的扫描点作为初始化全局点云地图的融合点；

步骤S4、更新全局点云地图：

获得后续当前帧扫描点云时，采用前端融合全局特征点匹配法，将后续当前帧扫描点云的特征点变换到基准坐标系后与上一更新全局点云地图的全局特征点进行融合，将后续当前帧扫描点云的扫描点变换到基准坐标系后与上一更新全局点云地图的融合点进行融合，从而构建当前更新的全局点云地图；

所述步骤S4具体过程为：

步骤S41、对当前帧扫描点云进行点云区域分割；

步骤S42、对完成区域分割的当前帧扫描点云进行特征直线提取、并基于特征直线进行特征点提取；

步骤S43、对完成特征点提取的当前帧扫描点云进行特征点筛选；

步骤S44、对完成特征点筛选的当前帧扫描点云进行动态初始化粗匹配到在上一更新的全局点云地图上；

步骤S45、以上一更新的全局点云地图的全局特征点为特征点对的寻找基准、搜索对应的当前帧扫描点云的特征点后形成特征点对；

步骤S46、利用最小二乘法对选取的多组特征点对进行迭代平差，从而求得当前帧扫描点云相对于基准坐标系的旋转角变换参数和平移量变换参数、相对位姿变换参数；

步骤S47、最后根据旋转角变换参数和平移量变换参数、相对位姿变换参数，进行扫描点融合或/和进行特征点融合得到当前更新的全局点云地图；

扫描点融合：将当前帧扫描点云的扫描点变换到基准坐标系内，并与上一更新的全局点云地图中的融合点进行叠加，得到当前更新的全局点云地图的融合点；

特征点融合：将当前帧扫描点云的特征点变换到基准坐标系内，并与上一更新的全局点云地图中的全局特征点进行叠加，获得当前更新的全局点云地图的全局特征点，当前更新的全局点云地图的全局特征点作为下一待匹配帧扫描点云的特征点对的寻找基准；

步骤S48、重复S41-S47步骤，持续获得当前更新的全局点云地图。

2.根据权利要求1所述的基于低成本车载二维激光雷达的地图构建方法，其特征在于，

步骤S3的具体过程为：

步骤S31、对第一帧扫描点云进行点云区域分割；

步骤S32、对完成区域分割的第一帧扫描点云进行特征直线提取、并基于特征直线进行特征点提取；

步骤S33、对完成特征点提取的第一帧扫描点云进行特征点筛选；

步骤S34、将步骤S33所获得特征点导入到初始化全局点云地图中，作为初始化全局点云地图的全局特征点；将第一帧扫描点云的扫描点的坐标导入到初始化全局点云地图中，作为初始化全局点云地图的融合点。

3.根据权利要求1所述的基于低成本车载二维激光雷达的地图构建方法，其特征在于，

所述步骤S41中，根据扫描点间的欧氏距离对当前帧扫描点云进行点云区域分割，其中扫描点间的欧氏距离为Q，动态分割阈值为δ；当Q大于δ时进行点云区域分割，Q小于或等于δ时不进行点云区域分割；其中式1：

式1中，δ表示动态分割阈值，r表示扫描点到激光雷达中心的距离，α表示相邻扫描点的角度间隔，d表示激光雷达中心到相邻扫描点所在直线的距离；

所述步骤S42中，采用IEPF分割算法来对各个分割后的点云区域进行特征直线提取；

所述步骤S42中，将特征直线的端点、角点视为特征点进行特征点提取；

所述步骤S43中，特征点筛选的标准为：若特征点为边缘点，则进行剔除筛选，若特征点为到激光雷达中心的距离在设定的极限阈值范围内则进行剔除筛选。

4.根据权利要求1所述的基于低成本车载二维激光雷达的地图构建方法，其特征在于，

所述步骤S44中，粗匹配过程为：

将上一帧扫描点云求取的最佳变换参数R_t，作为当前帧扫描点云进行变换所需变换矩阵的初始值R′_t+1，对其进行初始化，完成粗匹配；

其中，上一帧扫描点云求取的最佳变换参数R_t包括：上一帧扫描点云相对于基准坐标系的旋转角变换参数和平移量变换参数、相对位姿变换参数。

5.根据权利要求1所述的基于低成本车载二维激光雷达的地图构建方法，其特征在于，

所述步骤S45中，特征点对的形成过程为：

将选择的1个当前帧扫描点云的特征点视为待匹配特征点，求这1个待匹配特征点与上一更新的全局点云地图中每个全局特征点的距离，将距离最小的全局特征点与该待匹配特征点组成特征点对。

6.根据权利要求1所述的基于低成本车载二维激光雷达的地图构建方法，其特征在于，

多个当前帧扫描点云由2D激光雷达在目标区域内不同位置处扫描获得。

7.根据权利要求1所述的基于低成本车载二维激光雷达的地图构建方法，其特征在于，

将第一帧扫描点云的坐标系设置为全局点云地图的基准坐标系。

8.根据权利要求1-7中任意一项所述的基于低成本车载二维激光雷达的地图构建方法，其特征在于，

还包括步骤S5、步骤S6，步骤S5、步骤S6于步骤S4之后执行；

步骤S5：对每次当前更新的全局点云地图进行回环检测，若检测状态为回环，则执行步骤S6；

步骤S6：采用间接平差模型对整体的回环数据进行优化，获得优化后的全局点云地图；

整体的回环数据是指：发生回环时，形成回环的所有全局点云地图的集合。

Images