一种复杂城区环境中的高精度点云地图创建系统及方法

摘要

本发明公开了一种复杂城区环境中的高精度点云地图创建系统及方法，该系统主要包括：里程计模块、回环检测模块、后端优化模块和地图生成模块。该方法的主要思路是，里程计模块利用语义分割将点云分为地面点云和障碍物点云，并在点云配准中引入双向对应并构建基于L1范数的鲁棒目标函数；利用全局特征和局部特征双阶段匹配的方法有效检测回环，其中全局特征用于快速剔除错误回环，局部特征用于精确筛选回环；后端优化模块构造基于L1范数的鲁棒目标函数，以克服回环和GPS包含外点的情况；地图生成模块对地面点云和障碍物点云分别处理，提升了地图标注效率和在线定位的鲁棒性。

说明书

技术领域

本发明属于智能交通领域与高精度建图系统领域，特别涉及一种复杂城区环境中的高精度点云地图创建系统及方法。

背景技术

高精度建图是无人驾驶的核心模块。通过提前构建环境的精确三维描述，可以最大限度地减少无人驾驶在线感知模块的不确定性，提升整个无人驾驶系统的可靠性。另一方面，高精度点云地图是高精度建图的基础，通过点云地图可以进一步构造特征级地图如车道线/道路边界/交通灯地图，和拓扑级地图如导航地图等。

给定由激光雷达采集的点云帧序列、GPS和IMU信息，高精度建图方法通常包括位姿计算和地图生成两个步骤。然而，上述两个步骤受城区环境和激光传感器的扫描方式影响。位姿计算通常包括里程计、回环检测和后端优化，里程计使用相邻帧点云推算位姿，其核心点云配准算法易受动态障碍物、遮挡的影响，回环检测常常使用GPS快速剔除错误回环，但城区环境下GPS信号易受高楼、隧道、林荫道影响，后端优化基于高斯噪声假设融合里程计、回环检测和GPS信息，但不可靠的GPS和误检的回环会导致后端优化失效；地图生成上，由于城区环境的复杂性和激光雷达的扫描方式，导致生成的点云地图地面点比例很高，影响地图标注和在线定位算法的可靠性。

综上所述，目前的建图系统无法完成开放式、复杂城区下的建图任务，不具备在技术研发和应用上的推广。

发明内容

本发明的目的在于提供一种复杂城区环境中的高精度点云地图创建系统及方法，以解决上述问题。

为实现上述目的，本发明采用以下技术方案：

一种复杂城区环境中的高精度点云地图创建系统，包括里程计模块M1、回环检测模块M2、后端优化模块M3和地图生成模块M4；里程计模块M1、回环检测模块M2均连接到后端优化模块M3，后端优化模块M3连接地图生成模块M4，里程计模块M1连接回环检测模块M2；

里程计模块M1输入点云帧序列和IMU，输出障碍物点云、地面点云和里程计信息；回环检测模块M2输入障碍物点云帧序列和IMU，输出回环信息；后端优化模块M3输入里程计模块、回环检测模块和GPS，输出最优位姿；地图生成模块M4输入最优位姿、障碍物点云和地面点云，输出高精度点云地图。

进一步的，里程计模块M1包括语义分割模块M11和点云配准模块M12；语义分割模块M11用于将输入点云划分为空间栅格，通过栅格特征将点云划分为地面和障碍物两类；点云配准模块M12给定连续两帧、经语义分割的点云。

进一步的，回环检测模块M2包括全局特征筛选模块M21和局部特征筛选模块M22；全局特征筛选模块M22包括全局特征构建模块和特征匹配模块，全局特征构建模块的目标是对每帧点云构建全局特征；全局特征筛选模块M21计算每帧点云对应的全局特征，通过全局特征之间的匹配筛选得到回环的索引集合。

进一步的，一种复杂城区环境中的高精度点云地图创建方法，包括以下步骤：

步骤1，语义分割模块M11采用栅格图算法将单帧点云

步骤2，点云配准模块M12输入两帧点云

步骤3，对输入的障碍物点云帧序列

步骤4，局部特征筛选模块M22通过局部特征匹配对回环索引

步骤5，后端优化模块M3构造基于里程计、回环检测和稀疏GPS信号的目标函数，对回环检测和GPS施加基于L1范数的鲁棒函数以增强算法对外点的鲁棒性，优化目标函数得到点云序列的最优位姿；

步骤6，地图生成模块M4由最优位姿、地面点云帧序列和障碍物点云帧序列生成地面点云地图M

进一步的，语义分割模块M11将输入点云划分为空间栅格，具体过程如下：

语义分割模块M11将原始的无序点云

其中单个栅格是三维点集

其中

针对单个栅格

根据上述特征，构造地面点云

进一步的，点云配准模块M12给定连续两帧、经语义分割的点云

对应关系求解即给定当前的刚体变换

进一步的，刚性变换求解即根据对应关系求解最优刚性变换：

其中p∈(0，1]是基于l

上式变换为如下的带约束的优化问题：

其中z

进一步的，全局特征筛选模块M22包括全局特征构建模块和特征匹配模块；全局特征构建模块的目标是对每帧点云构建全局特征，该全局特征可以快速计算且对刚性变换具有不变性；

和里程计模块类似，将点云

将每个栅格

由于协方差矩阵提取全局特征，将点云的NDT表征记作

f

f

f

f

f

其中λ

基于全局特征的回环筛选通过全局特征之间的匹配快速去除错误回环；假设有N帧点云帧序列对应的全局特征

通过相邻帧构建训练集；

时域上相邻的索引集合为

其中

阈值学习；由于相邻帧的全局特征比较类似，假设

输出回环索引；测试索引集合

其中t

进一步的，局部特征筛选模块M22通过局部特征匹配，对回环做进一步的筛选，并给出回环之间的精确刚性变换；从回环索引中抽取两帧点云

关键点提取；给定点云

局部特征计算；采用点云中常用的RoPS特征，对关键点

生成局部特征

刚性变换计算；给定两帧点云的局部特征

t＝p

该刚性变换的质量评价采用两帧点云之间的重叠率，参照里程计模块，计算基于双向对应的对应关系

其中

进一步的，后端优化模块M3通过因子图融合里程计、回环和稀疏GPS信息，得到精确的位姿序列，为地图生成模块提供输入；为克服回环和稀疏GPS信息可能存在的外点，在目标函数中引入基于L1范数的鲁棒估计；假设输入点云帧序列为

其中，Ω..是对应测量方程的信息矩阵，可以由点云配准算法或GPS接收机计算或读出，

进一步的，地图生成模块M4利用最优位姿序列、地面点云序列和障碍物点云序列生成适用于在线定位的高精度点云地图；具体过程如下：

语义地图生成；里程计模块将每帧点云分割为地面点云和障碍物点云，根据最优位姿将上述两类点云拼接为路面点云地图M

地图处理；人工删除掉障碍物点云地图M

语义地图合并；将上一步中的两类点云地图合并成点云地图M，并计算M中每个点的法向量、协方差矩阵几何信息。

与现有技术相比，本发明有以下技术效果：

本发明与常用的基于差分GPS的建图系统相比，算法性能不受复杂城区环境GPS信号差的影响，因此可以广泛应用于无人驾驶高精度建图、智能车辅助驾驶等领域。

本发明与其他基于激光点云、GPS和IMU的定位系统相比，里程计模块M1中所用的点云配准模块M12，利用双向对应和L1鲁棒估计，克服了常见点云配准算法易受动态障碍物和遮挡影响的弱点。

本发明与其他基于激光点云、GPS和IMU的定位系统相比，所用回环检测模块M2采用全局和局部特征双阶段筛选的机制，可有效提升回环检测效率；所用回环检测模块M2无需使用GPS信号，可完成复杂城区环境内大范围建图任务。

本发明与其他基于激光点云、GPS和IMU的定位系统相比，地图生成算法利用语义分割生成地面点云地图和障碍物点云地图，其中障碍物点云地图由于没有地面点影响，动态障碍物所造成的拖影可以被人轻易检测到，有效提升了地图标注系统的效率；地面点云地图没有障碍物点，可以灵活地随机降采样以满足在线定位算法的需求。

附图说明

图1为本发明的系统整体框架图；

图2为栅格划分示意图；

图3为语义分割结果示意图；

图4为点云配准流程图；

图5为回环检测流程图；

图6为全局特征结果示意图；

图7为全局特征筛选结果示意图；

图8为局部特征筛选结果示意图；

图9为后端优化结果示意图；

图10为地图生成结果示意图；

图11为本发明所生成的高精度点云地图。

具体实施方式

以下结合附图对本发明进一步说明：

参见图1，为本发明的系统整体框架示意图，包括里程计模块M1、回环检测模块M2、后端优化模块M3和地图生成模块M4；里程计模块输入点云帧序列和IMU，输出障碍物点云、地面点云和里程计信息；回环检测模块输入障碍物点云帧序列和IMU，输出回环信息；后端优化模块输入里程计模块、回环检测模块和GPS，输出最优位姿；地图生成模块输入最优位姿、障碍物点云和地面点云，输出高精度点云地图；

里程计模块M1包括语义分割模块M11和点云配准模块M12；

回环检测模块M2包括全局特征筛选模块M21和局部特征筛选模块M22；

具体包括以下步骤：

步骤1，语义分割模块M11采用栅格图算法将单帧点云

步骤2，点云配准模块M12输入两帧点云

步骤3，对输入的障碍物点云帧序列

步骤4，局部特征筛选模块M22通过局部特征匹配对回环索引

步骤5，后端优化模块M3构造基于里程计、回环检测和稀疏GPS信号的目标函数，对回环检测和GPS施加基于L1范数的鲁棒函数以增强算法对外点的鲁棒性，优化目标函数得到点云序列的最优位姿；

步骤6，地图生成模块M4由最优位姿、地面点云帧序列和障碍物点云帧序列生成地面点云地图M

参见图2，为本发明的栅格划分示意图，其中极坐标栅格图用于语义分割模块M11，笛卡尔栅格图用于全局特征筛选模块M21。

极坐标栅格图计算如下：假设原始的无序点云为

其中单个栅格是三维点集

其中

同极坐标栅格图类似，可以将点云

参见图3，为本发明的语义分割结果示意图。由图可知，语义分割可以准确地将点云划分为地面点云和障碍物点云。具体计算过程如下：针对单个栅格

根据上述特征，可构造地面点云

参见图4，为本发明的点云配准流程图。点云配准模块M12给定连续两帧、经语义分割的点云

对应关系求解即给定当前的刚体变换

进一步的，刚性变换求解即根据对应关系求解最优刚性变换：

其中p∈(0，1]是基于l

上式可以变换为如下的带约束的优化问题：

其中z

其中

前两个是优化问题，最优一个是拉格朗日乘子更新。针对第一个优化问题，可将拉格朗日函数整理为关于Z的函数：

可以看出，为最大化拉格朗日函数，仅需求解优化问题：

针对欧式空间中的任意向量

因此，优化问题

针对第二个优化问题，将拉格朗日函数整理为关于刚性变换的表达式，进一步的化简如下：

上式是标准的基于点到面距离的点云配准目标函数，已有很多成熟的求解器可用。

参见图5，为本发明的回环检测流程图。回环检测模块M2包括全局特征筛选模块M21和局部特征筛选模块M22；全局特征筛选模块M21通过全局特征匹配快速剔除错误回环；局部特征筛选模块M22通过局部特征对上一步中的回环做判断，并给出回环对应的刚性变换。

全局特征筛选模块M21包括全局特征构建模块和特征匹配模块；全局特征构建模块的目标是对每帧点云构建全局特征，该全局特征可以快速计算且对刚性变换具有不变性；

和里程计模块类似，可以将点云

将每个栅格

由于协方差矩阵可以提取全局特征，将点云的NDT表征记作

f

f

f

f

f

其中λ

基于全局特征的回环筛选通过全局特征之间的匹配快速去除错误回环。假设有N帧点云帧序列对应的全局特征

通过相邻帧构建训练集。时域上相邻的索引集合为

其中

阈值学习；由于相邻帧的全局特征比较类似，假设

输出回环索引。测试索引集合

其中t

进一步的，局部特征筛选模块M22通过局部特征匹配，对回环做进一步的筛选，并给出回环之间的精确刚性变换。从回环索引中抽取两帧点云

关键点提取。给定点云

局部特征计算。本发明采用点云中常用的RoPS特征，对关键点

刚性变换计算。给定两帧点云的局部特征

t＝p

该刚性变换的质量评价采用两帧点云之间的重叠率，参照里程计模块，计算基于双向对应的对应关系

其中

由局部特征筛选得到的回环索引如下：

参见图6，为本发明的全局特征结果示意图。第一行是激光雷达扫描得到的点云，第二行是各个点云对应的全局特征。可以看出，点云A和点云B对应于不同视角下的相同场景，其全局特征较为相似。点云C和其他两帧点云在空间上是不同的场景，，其全局特征和A、B特征相差较大，说明本发明所提出的全局特征具有刚性变换不变性，可以有效地筛选回环。

参见图7，为本发明的全局特征筛选结果示意图。可以看出，通过阈值学习得到的阈值可以剔除掉绝大多数错误回环，有效提升了回环检测算法的效率。

参见图8，为本发明的局部特征筛选结果示意图。第一行是点云以及局部特征之间的匹配连线，每张子图中的两帧点云来自于激光雷达不同时刻的采集数据，第二行是利用局部特征对齐后的点云。第一列的场景平移量约为18米，第二列场景旋转角度约为180度，在较大的平移和旋转下，局部特征筛选仍然可以恢复出正确的刚性变换。

参见图9，为本发明的后端优化结果示意图。可以看出，融合里程计、回环和稀疏GPS信号后，位姿的累计误差明显地变小，里程计所认为的两条路被正确合并为同一条路。

参见图10，为本发明的地图生成结果示意图。传统的方法不区分地面和障碍物，所生成的点云动态障碍物和地面交叠，导致地图标注无法保证效率和准确度。本发明中的地图生成模块M4分别生成地面点云和障碍物点云，可以看出，障碍物点云由于没有地面点，动态障碍物如车和人等可以清晰被人分辨出来，有利于地图的标注。

参见图11，为本发明所生成的高精度点云地图。可以看出，在高动态、开放式的复杂校园和园区环境中，本发明所生成的高精度点云地图细节较为精确，满足无人驾驶导航和在线定位需求。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施方式仅限于此，对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单的推演或替换，都应当视为属于本发明由所提交的权利要求书确定专利保护范围。