CN112669358A - 一种适用于多平台协同感知的地图融合方法 - Google Patents

Abstract

本发明公开了一种适用于多平台协同感知的地图融合方法，方法包括：获取里程信息和局部地图信息；根据所述里程信息对所述局部地图信息进行预处理，得到子地图特征点；根据所述子地图特征点进行点云配准，得到子地图间的匹配关系；根据所述里程信息、局部地图信息和子地图间的匹配关系构建全局位姿图，优化得到融合地图。本发明实施例降低了配准难度且扩展了应用场景，可广泛应用于点云处理技术领域。

Description

一种适用于多平台协同感知的地图融合方法

技术领域

本发明涉及点云处理技术领域，尤其是一种适用于多平台协同感知的地图融合方法。

背景技术

目前单机器人定位与建图领域已有不错的技术成果，尤其对于地面机器人，可通过多传感器融合等方案实现良好的自我定位，从而得到较好的单机地图，但在探测大范围的未知环境时，使用单个机器人进行感知有很大的局限性，一方面，在复杂环境中机器人运动受到限制，使用单个机器人可能无法完成整个地图的探测，且探测效率低下，另一方面，只使用地面或空中机器人进行探测，往往会由于视角有限而无法得到完整的环境地图。而使用多机器人进行大范围环境探测，则可以高效地获得环境地图，但在这种情况下，各个机器人在建立局部地图时是以各自的起始位置和朝向建立世界坐标系的，无法直接叠加得到完整地图，需要进行局部地图的融合，即统一各个局部地图的坐标系，才可以得到最终的完整环境地图。

目前地图融合方法主要通过对局部点云地图之间进行粗配准得到良好的相对变换初始值，再利用初始值进行精配准来得到局部地图之间准确的变换关系。

点云粗配准的方法较多，其中基于局部特征匹配的方法有基于FPFH点特征的SAC-IA算法、基于线特征的ICL算法、基于SHOT特征的AO算法等，这类方法通过探测环境本身具有的特征构建点云间的匹配对，然后计算点云间的变换关系；基于概率分布的NDT则通过构造正态分布来优化得到使得概率密度最大的变换关系；基于空间拓扑架构的有4PCS及其改进版本Super4PCS、V4PCS等，4PCS算法通过构建共面四点集合，然后通过仿射不变性约束，在共面四点集合中匹配符合条件的对应点对，使用LCP策略寻找配准后最大重叠度四点对，得到最优匹配结果，Super4PCS通过智能索引策略降低4PCS算法复杂度，V4PCS则通过体积一致法将算法拓展到非共面四点匹配，提高计算效率。

点云精配准的方法有lCP及其改进版Point-to-Plane ICP、Plane-to-Plane ICP、GICP、NICP等，ICP算法通过迭代计算点云变换关系得到最优匹配，Point-to-Plane ICP将计算点与点距离改为计算源点到目标点所在平面的距离，考虑了点云结构，但它的优化是非线性的，计算速度慢，Plane-to-PIane ICP则考虑面到面的距离，GICP则综合点到点、点到面、面到面的三种策略，提高计算精度，NICP则引入法向量、局部曲率，进一步利用了点云结构信息。这些精配准方法在匹配点云间有较大的重叠区域时，可以利用粗配准得到的初始值进行迭代计算，得到较为准确的相对位姿。

在粗配准阶段，基于局部特征和概率密度的方法不适用于多机感知任务，因为它们都要求源点云与目标点云有较大的重叠区域，而在多机器人探测环境时，搜索路线往往只有少量的交叠，得到的局部点云地图也只有很小的重叠区域，在这种情况下，这些方法往往会将点云错误地混叠；而4PCS等基于四点集合的方法可以在重叠区域较小的情况下完成点云配准，但这类方法只能在小型点云正确确定对应点，难以进行大规模地图配准，同时配准效果非常依赖预设的RANSAC内点阈值。

在精配准阶段，ICP等算法需要一个相对准确的相对位姿作为初值，才可以迭代计算出最终的变换关系，当粗配准失败时，精配准往往也无法完成正确的点云配准。另外，对于重叠区域较小的点云配准任务，ICP算法也难以完成正确拼接，其迭代的趋势基本都是尽量将点云混合到有较大的重叠区域，不适用于多机探测地图的融合。

此外，粗配准加精配准的方案无法对单机的局部地图进行优化，因为它们都是以局部点云地图作为输入，忽略了单机建图过程中估计出来的里程信息，在单机自身没有回环点时，单机的轨迹估计很容易发生漂移，得到的局部地图也会随着里程估计的偏差出现漂移，在这种情况下，融合后的地图也必然出现扭曲，特别当漂移发生在重叠区域时，点云配准失败是难以避免的。

发明内容

有鉴于此，本发明实施例提供一种适用于多平台协同感知的地图融合方法，以降低配准难度并且扩展应用场景。

本发明实施例提供了一种适用于多平台协同感知的地图融合方法，包括：

获取里程信息和局部地图信息；

根据所述里程信息对所述局部地图信息进行预处理，得到子地图特征点；

根据所述子地图特征点进行点云配准，得到子地图间的匹配关系；

根据所述里程信息、局部地图信息和子地图间的匹配关系构建全局位姿图，优化得到融合地图。

优选地，所述根据所述里程信息对所述局部地图信息进行预处理，得到子地图特征点，包括：

对所述局部地图进行分割，得到多个子地图；

对所述每个子地图进行点云投影，得到深度图像；

对所述深度图像进行特征提取，得到所述子地图特征点。

优选地，所述对所述深度图像进行特征提取，得到所述子地图特征点，包括：

查找所述深度图像中每个深度点与不跨越边缘的相邻点之间的典型3D距离；

根据所述典型3D距离，计算每个所述深度点属于边缘点的可信度；

根据每个所述深度点属于边缘点的可信度，通过非极大值抑制获取边缘点位置；

根据每个所述深度点的2D邻域点对应的3D位置，计算每个所述深度点的法向量；

计算每个所述深度点的领域表面变化的分数和变化主方向；

根据所述法向量、所述分数和变化主方向，通过非极大值抑制计算得到子地图特征点。

优选地，所述根据所述子地图特征点进行点云配准，得到子地图间的匹配关系，包括：

根据所述子地图特征点，计算源点云中四点集合的两个比例因子；所述比例因子用于保持点云在进行旋转和平移变换时的仿射不变性；

根据所述比例因子，计算四点集合的基线交点位置，并确定所述交点的坐标；

根据所述交点的坐标，确定匹配对；

根据LCP策略对所述匹配对进行计算，得到最大重叠度四点对，进而确定所述子地图间的匹配关系。

优选地，所述根据LCP策略对所述匹配对进行计算，得到最大重叠度四点对，进而确定所述子地图间的匹配关系，包括：

通过GC-RANSAC框架进行迭代计算，通过图割算法来求解内点集；

根据所述内点集中每个单点的判别误差约束和邻近点对的空间一致性约束来构建能量函数；

根据所述能量函数确定所述子地图间的匹配关系。

优选地，所述能量函数为：

其中，E_K(L)代表当前内外点标签设置对于每个点带来的误差；E_S(L)代表基于空间一致性假设构造的能量函数；L代表所有点的内外点标签；θ表示模型参数，模型参数包括点云间的旋转和平移；φ(p，θ)表示点p在参数θ下计算得到的误差值，误差值包括旋转和平移后到目标点的距离；∈为内外点判别阈值；

代表点p的K(δ，∈)值；K_q代表点q的K(δ，∈)值。

优选地，所述根据所述里程信息、局部地图信息和子地图间的匹配关系构建全局位姿图，优化得到融合地图，包括：

以每个子地图的首帧位姿信息作为位姿图顶点，根据各单机里程信息，设置单机上连续的两个子地图首帧之间的相对位姿约束边，得到若干个局部位姿图；

根据所述子地图间的匹配关系，额外加入约束边，将所有所述局部位姿图相连成为一个全局位姿图；

通过对所述全局位姿图进行优化，统一各个局部地图的坐标系，并对各个所述局部地图进行优化，得到融合地图。

本发明的实施例首先获取里程信息和局部地图信息；根据所述里程信息对所述局部地图信息进行预处理，得到子地图特征点；根据所述子地图特征点进行点云配准，得到子地图间的匹配关系；根据所述里程信息、局部地图信息和子地图间的匹配关系构建全局位姿图，优化得到融合地图。本发明实施例降低了配准难度，利用了单机建图过程中估计的里程信息，在单机缺乏回环点而发生漂移时可以对局部地图进行矫正，并且对平台没有同构要求，不同平台可以根据需要选择激光雷达或相机等传感器进行建图，可以选择地面或空中设备进行探测，只要探测的局部地图之间有重叠，则可以利用彼此的信息优化自身地图并得到融合的环境地图，扩展了应用场景。

附图说明

为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的步骤流程图；

图2为本发明实施例提供的系统框架示意图；

图3为本发明实施例提供的共面四点集合示意图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

针对现有技术存在的问题，本发明提出一种适用于多平台协同感知的地图融合方法，该方法利用了单机建图过程中估计的里程信息，通过构建全局位姿图可以进行局部地图优化，更好地完成地图融合，同时在配准过程中将局部地图进行分割，对子地图进行特征提取和4PCS配准，避免了大规模点云配准运算，并且以图割算法进行内外点判别，避免了对内点阈值的强依赖，可以适用不同场景的多机感知任务。

如图1所示，本发明的方法具体包括以下步骤：

获取里程信息和局部地图信息；

根据所述里程信息对所述局部地图信息进行预处理，得到子地图特征点；

根据所述子地图特征点进行点云配准，得到子地图间的匹配关系；

根据所述里程信息、局部地图信息和子地图间的匹配关系构建全局位姿图，优化得到融合地图。

优选地，所述根据所述里程信息对所述局部地图信息进行预处理，得到子地图特征点，包括：

对所述局部地图进行分割，得到多个子地图；

对所述每个子地图进行点云投影，得到深度图像；

对所述深度图像进行特征提取，得到所述子地图特征点。

优选地，所述对所述深度图像进行特征提取，得到所述子地图特征点，包括：

查找所述深度图像中每个深度点与不跨越边缘的相邻点之间的典型3D距离；

根据所述典型3D距离，计算每个所述深度点属于边缘点的可信度；

根据每个所述深度点属于边缘点的可信度，通过非极大值抑制获取边缘点位置；

根据每个所述深度点的2D邻域点对应的3D位置，计算每个所述深度点的法向量；

计算每个所述深度点的领域表面变化的分数和变化主方向；

根据所述法向量、所述分数和变化主方向，通过非极大值抑制计算得到子地图特征点。

优选地，所述根据所述子地图特征点进行点云配准，得到子地图间的匹配关系，包括：

根据所述子地图特征点，计算源点云中四点集合的两个比例因子；所述比例因子用于保持点云在进行旋转和平移变换时的仿射不变性；

根据所述比例因子，计算四点集合的基线交点位置，并确定所述交点的坐标；

根据所述交点的坐标，确定匹配对；

根据LCP策略对所述匹配对进行计算，得到最大重叠度四点对，进而确定所述子地图间的匹配关系。

优选地，所述根据LCP策略对所述匹配对进行计算，得到最大重叠度四点对，进而确定所述子地图间的匹配关系，包括：

通过GC-RANSAC框架进行迭代计算，通过图割算法来求解内点集；

根据所述内点集中每个单点的判别误差约束和邻近点对的空间一致性约束来构建能量函数；

根据所述能量函数确定所述子地图间的匹配关系。

优选地，所述能量函数为：

其中，E_K(L)代表当前内外点标签设置对于每个点带来的误差；E_S(L)代表基于空间一致性假设构造的能量函数；L代表所有点的内外点标签；θ表示模型参数，模型参数包括点云间的旋转和平移；φ(p，θ)表示点p在参数θ下计算得到的误差值，误差值包括旋转和平移后到目标点的距离；∈为内外点判别阈值；

K_p代表点p的K(δ，∈)值；K_q代表点q的K(δ，∈)值。

优选地，所述根据所述里程信息、局部地图信息和子地图间的匹配关系构建全局位姿图，优化得到融合地图，包括：

以每个子地图的首帧位姿信息作为位姿图顶点，根据各单机里程信息，设置单机上连续的两个子地图首帧之间的相对位姿约束边，得到若干个局部位姿图；

根据所述子地图间的匹配关系，额外加入约束边，将所有所述局部位姿图相连成为一个全局位姿图；

通过对所述全局位姿图进行优化，统一各个局部地图的坐标系，并对各个所述局部地图进行优化，得到融合地图。

下面结合说明书附图，详细描述本发明的地图融合方法的具体实现过程：

本发明提出一种可用于多平台大规模协同感知的点云地图融合方案，以各单机建图过程中的里程信息(每帧点云对应的位姿)和建成的局部地图作为输入，完成各个局部地图的优化和融合，从而得到完善的环境地图。如图2所示，该发明包含三个部分：局部地图预处理、子地图配准和全局位姿图优化。局部地图预处理部分完成对局部地图的分割和点云特征提取，子地图配准部分以局部地图预处理部分的输出(只保留特征点的子地图)作为输入，进行点云配准，全局位姿图优化部分则根据单机建图时的位姿估计信息和子地图配准部分输出的变换关系构建位姿图，进行地图优化，融合各局部地图。

(1)、在局部地图预处理部分，以单机里程信息和局部地图作为输入，首先根据单机建图顺序进行局部地图的分割，从第1帧点云开始，以每20帧点云作为一个子地图，然后对每个子地图以子地图中首帧点云对应位姿进行点云投影得到深度图像，对深度图像提取NARF特征。特征提取分为边缘提取和特征点提取两步进行，在边缘提取步骤，对深度图中每个深度点先用启发式的方法来找到不跨越边缘的相邻点的典型3D距离，然后计算该点是边缘点的可信度，再用非极大值抑制获取准确的边缘点位置；在特征点提取步骤，对每个深度点利用其2D邻域点对应的3D位置，通过PCA算法估计其法向量，并计算表示其领域表面变化的分数和变化主方向，再用非极大值抑制确定最终的特征点。

(2)、在子地图配准部分，以局部地图预处理得到的子地图NARF特征点作为输入，为完成重叠区域较小的点云配准任务，选取4PCS作为基础算法进行配准，根据4PCS算法，在源点云S与目标点云T中构建共面四点集合，如图3所示，计算源点云S中四点集合{a，b，c，d}的两个比例因子r₁和r₂：

其中，这两个比例因子在点云做旋转、平移变换时具有仿射不变性。得到r₁、r₂后对q₁，q₂∈Q，可以计算交点位置：

通过计算Q中点对的交点坐标，然后比较交点的坐标确定匹配对，e_i≈e_j则表示寻找到对应一致全等四点集合，其中i，j分别表示Q中第i个和第j个点对，e_i为使用r₁、r₂其中一个比例因子求得的交点坐标，e_j为使用另一个比例因子求得的交点坐标。这样利用仿射不变性约束得到备选的匹配对后，再利用LCP(Largest Common Pointset)策略找到最大重叠度四点对。与原始4PCS算法不同，本发明实施例将迭代计算的RANSAC框架改为GC-RANSAC(Graph-Cut RANSAC)，以图割算法来求解内点集，即不以预设的内点阈值来划分得到每次迭代的内外点，而是只作为当前的一个初始分割，构建图后根据每个单点p的判别误差约束和邻近点对(p，q)的空间一致性约束构建能量函数E_K(L)和E_S(L)：

其中，L指所有点的内外点标签，θ表示模型参数，在本发明实施例即为点云间的旋转、平移，φ(p，θ)表示点p在参数θ下计算得到的误差值，在本发明实施例即为旋转、平移后到目标点的距离，∈为内外点判别阈值。E_K(L)描述了当前内外点标签设置对于每个点带来的误差，E_S(L)则是基于空间一致性假设构造的能量函数，空间一致性假设是指空间中邻近的点很有可能具有相同的标签，这对于点云配准问题是非常符合的，因为两个点云的重叠部分往往是相连的一块区域，在重叠区域为内点，在非重叠区域则为外点，在同一局部区域中，往往都是内点或者都是外点。综合这两种约束，使用图割算法来最小化总能量函数E(L)＝E_K(L)+λE_S(L)，即可得到当前的内外点集，若内点数目增多则更新模型，设置一定的迭代次数限制和误差阈值，当误差小于阈值时认为配准成功，说明进行配准的两个子地图是不同平台分别探测到的同一局部环境的两份点云，此时停止迭代但并不直接进行地图融合，而是记录两份点云间的匹配关系(包括两个点云所属平台、所属平台第几个子地图及点云间变换关系)，若超出设置迭代次数仍未小于误差阈值，则认为两个子地图属于不同环境区域，没有重叠部分。

(3)、在全局位姿图优化部分，以单机里程信息、局部地图和子地图配准部分输出的点云匹配关系作为输入，首先以每个子地图首帧位姿信息作为位姿图顶点，根据各单机里程信息，设置单机上连续的两个子地图首帧之间的相对位姿约束边，得到若干个局部位姿图，然后根据子地图配准得到的子地图间的匹配关系，额外加入约束边，使得所有局部位姿图相连成为一个全局位姿图，通过优化则可以统一各个局部地图的坐标系，同时每个局部地图可以利用其他局部地图对自身的约束信息来优化自身的地图，最终得到一个良好的融合环境地图相较于现有技术，本发明具有以下突出特点：

1、在大规模地图融合问题中，通过引入单机里程信息，对大规模局部地图做分割和子地图关联，将大规模地图融合任务转化为小规模点云配准和优化问题。

2、在点云配准问题中，进行四点法求解时先提取了NARF特征点，再利用GC-RANSAC框架，引入空间一致性约束，使用图割方式进行内外点判别，提高求解精度和鲁棒性。

综上所述，本发明对大规模地图做了分割和特征提取，降低了配准难度，在配准过程中通过图割方式避免了算法对预设内外点阈值的强依赖，使得算法可以灵活适应更多场景，同时算法利用了单机建图过程中估计的里程信息，在单机缺乏回环点而发生漂移时可以对局部地图进行矫正，并且对平台没有同构要求，不同平台可以根据需要选择激光雷达或相机等传感器进行建图，可以选择地面或空中设备进行探测，只要探测的局部地图之间有重叠，则可以利用彼此的信息优化自身地图并得到融合的环境地图。

在一些可选择的实施例中，在方框图中提到的功能/操作可以不按照操作示图提到的顺序发生。例如，取决于所涉及的功能/操作，连续示出的两个方框实际上可以被大体上同时地执行或所述方框有时能以相反顺序被执行。此外，在本发明的流程图中所呈现和描述的实施例以示例的方式被提供，目的在于提供对技术更全面的理解。所公开的方法不限于本文所呈现的操作和逻辑流程。可选择的实施例是可预期的，其中各种操作的顺序被改变以及其中被描述为较大操作的一部分的子操作被独立地执行。

此外，虽然在功能性模块的背景下描述了本发明，但应当理解的是，除非另有相反说明，所述的功能和/或特征中的一个或多个可以被集成在单个物理装置和/或软件模块中，或者一个或多个功能和/或特征可以在单独的物理装置或软件模块中被实现。还可以理解的是，有关每个模块的实际实现的详细讨论对于理解本发明是不必要的。更确切地说，考虑到在本文中公开的装置中各种功能模块的属性、功能和内部关系的情况下，在工程师的常规技术内将会了解该模块的实际实现。因此，本领域技术人员运用普通技术就能够在无需过度试验的情况下实现在权利要求书中所阐明的本发明。还可以理解的是，所公开的特定概念仅仅是说明性的，并不意在限制本发明的范围，本发明的范围由所附权利要求书及其等同方案的全部范围来决定。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

以上是对本发明的较佳实施进行了具体说明，但本发明并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。

Claims (7)

1.一种适用于多平台协同感知的地图融合方法，其特征在于，包括：

获取里程信息和局部地图信息；

根据所述里程信息对所述局部地图信息进行预处理，得到子地图特征点；

根据所述子地图特征点进行点云配准，得到子地图间的匹配关系；

根据所述里程信息、局部地图信息和子地图间的匹配关系构建全局位姿图，优化得到融合地图。

2.根据权利要求1所述的一种适用于多平台协同感知的地图融合方法，其特征在于，所述根据所述里程信息对所述局部地图信息进行预处理，得到子地图特征点，包括：

对所述局部地图进行分割，得到多个子地图；

对所述每个子地图进行点云投影，得到深度图像；

对所述深度图像进行特征提取，得到所述子地图特征点。

3.根据权利要求2所述的一种适用于多平台协同感知的地图融合方法，其特征在于，所述对所述深度图像进行特征提取，得到所述子地图特征点，包括：

查找所述深度图像中每个深度点与不跨越边缘的相邻点之间的典型3D距离；

根据所述典型3D距离，计算每个所述深度点属于边缘点的可信度；

根据每个所述深度点属于边缘点的可信度，通过非极大值抑制获取边缘点位置；

根据每个所述深度点的2D邻域点对应的3D位置，计算每个所述深度点的法向量；

计算每个所述深度点的领域表面变化的分数和变化主方向；

根据所述法向量、所述分数和变化主方向，通过非极大值抑制计算得到子地图特征点。

4.根据权利要求1所述的一种适用于多平台协同感知的地图融合方法，其特征在于，所述根据所述子地图特征点进行点云配准，得到子地图间的匹配关系，包括：

根据所述子地图特征点，计算源点云中四点集合的两个比例因子；所述比例因子用于保持点云在进行旋转和平移变换时的仿射不变性；

根据所述比例因子，计算四点集合的基线交点位置，并确定所述交点的坐标；

根据所述交点的坐标，确定匹配对；

根据LCP策略对所述匹配对进行计算，得到最大重叠度四点对，进而确定所述子地图间的匹配关系。

5.根据权利要求4所述的一种适用于多平台协同感知的地图融合方法，其特征在于，所述根据LCP策略对所述匹配对进行计算，得到最大重叠度四点对，进而确定所述子地图间的匹配关系，包括：

通过GC-RANSAC框架进行迭代计算，通过图割算法来求解内点集；

根据所述内点集中每个单点的判别误差约束和邻近点对的空间一致性约束来构建能量函数；

根据所述能量函数确定所述子地图间的匹配关系。

6.根据权利要求5所述的一种适用于多平台协同感知的地图融合方法，其特征在于，所述能量函数为：

其中，E_K(L)代表当前内外点标签设置对于每个点带来的误差；E_S(L)代表基于空间一致性假设构造的能量函数；L代表所有点的内外点标签；θ表示模型参数，模型参数包括点云间的旋转和平移；φ(p，θ)表示点p在参数θ下计算得到的误差值，误差值包括旋转和平移后到目标点的距离；∈为内外点判别阈值；

K_p代表点p的K(δ，∈)值；K_q代表点q的K(δ，∈)值。

7.根据权利要求1所述的一种适用于多平台协同感知的地图融合方法，其特征在于，所述根据所述里程信息、局部地图信息和子地图间的匹配关系构建全局位姿图，优化得到融合地图，包括：

以每个子地图的首帧位姿信息作为位姿图顶点，根据各单机里程信息，设置单机上连续的两个子地图首帧之间的相对位姿约束边，得到若干个局部位姿图；

根据所述子地图间的匹配关系，额外加入约束边，将所有所述局部位姿图相连成为一个全局位姿图；

通过对所述全局位姿图进行优化，统一各个局部地图的坐标系，并对各个所述局部地图进行优化，得到融合地图。

Images