一种无人系统地图构建方法、装置及介质

技术领域

本发明涉及计算机视觉技术领域，尤其涉及一种无人系统地图构建方法、装置及介质。

背景技术

未知环境主动探索技术是指无人平台在缺乏先验信息的情况下自主地对未知环境进行搜索探测，收集环境信息，为后续处置提供有利支撑，可广泛应用于城市排爆排险、核生化环境勘测、矿洞救援、小行星开发等危险场景中。即时定位与建图(SLAM)算法是指利用环境感知传感器去确定自身位置的增量，从而确定本体在环境地图中的位置，同时根据位置信息和环境感知数据建立环境点云地图。因此如何通过主动路径规划方法去提升SLAM方法的精度是目前学者们研究的热点。

现有应用在SLAM方法中的机器人在未知环境建图的过程中，往往会通过闭环行走和回环检测技术来减小误差，在常见的回环检测中，数据往往采用线性存储的方式，然后逐个进行匹配对比计算，直到遍历所有数据，这种方式往往需要耗费大量的时间。在很多的室内室外场景中，由于季节变化、布局变化、移动物体等等，无人系统在后续工作中由于地图误差大无法匹配导致定位失败。

发明内容

本发明提供了一种无人系统地图构建方法、装置及介质，所述方法通过增量自下而上的方式构建子空间，基于空间和路径进行回环检测，并通过只更新关键数据帧进行地图更新，优化配准各关键帧的估计位姿，从而能无需预知待测环境的边界，减少回环检测搜索对比的范围，从而提高构建地图的效率和准确率。

为实现上述目的，本发明实施例提供了一种无人系统地图构建方法，包括：

当无人系统处于未知环境时，设定配置地图空间分割参数，以构建八叉树空间索引结构；

控制所述无人系统实时扫描环境数据并对实时位姿进行估计，以将所述无人系统所处当前位置与所述八叉树空间索引结构中的第一个子节点进行绑定，保存扫描到的关键帧数据到该子节点中；其中，一个子节点的关键帧数据表达一个子空间的结构；

控制所述无人系统开始移动，实时扫描环境数据并对实时位姿进行估计；

根据无人系统的移动后的当前位置，通过增量式空间分割扩充所述八叉树空间索引结构，并保存扫描到的关键帧数据到当前节点中；

在所述无人系统移动过程中，对当前位置附近子空间进行搜索，进行回环检测及优化；

将所有关键帧数据根据全局位姿进行堆叠形成全局点云地图。

其中，所述配置地图空间分割参数包含子空间正立方体边长和八叉树最大层数。

进一步的，所述根据无人系统的当前位置，通过增量式空间分割扩充所述八叉树空间索引结构，并保存扫描到的关键帧数据到当前节点中，具体包括：

通过实时位姿确定无人系统的当前位置，若当前位置超出所述八叉树空间索引结构所能描述的空间结构，通过增量式空间分割增加八叉树空间索引结构的节点，在新增加的节点中的第一个子节点保存新的子空间扫描到的关键帧数据；

若当前位置未超出所述八叉树空间索引结构所能描述的空间结构，但当前位置跨越子空间时，保存当前扫描的关键帧数据到该对应的八叉树空间索引结构的子节点中。

可选的，所述对当前位置附近子空间进行搜索，进行回环检测及优化，具体包括：

通过实时位姿确定无人系统的当前位置，根据当前位置基于空间搜索近邻子空间；

当子空间不为空时，计算两组扫描的关键帧数据中的对应点线特征的欧式距离和的第一最小值，当所述第一最小值小于设定阈值时，判定回环检测成功，进行回环误差优化，并更新各关键帧优化后的位姿估计。

可选的，所述对当前位置附近子空间进行搜索，进行回环检测及优化，具体包括：

通过实时位姿确定无人系统的当前位置，根据当前位置基于空间搜索近邻子空间；

当子空间不为空时，搜索无人系统路径上的前后向帧，获取路径上的子空间的关键帧配准成子地图，计算当前帧及子地图中关键帧的对应点线特征的欧式距离和的第二最小值，当所述第二最小值小于设定阈值时，判定回环检测成功，进行回环误差优化，并更新各关键帧优化后的位姿估计。

进一步的，所述将所有关键帧数据根据全局位姿进行堆叠形成全局点云地图后，还包括：

无人系统加载当前的全局点云地图以及配置地图空间分割参数；

控制无人系统开始移动，实时扫描环境数据并对实时位姿进行估计，确定当前位置所在的子空间；

若当前子空间的关键帧数据量未超过阈值，直接保存新扫描的关键帧数据；若当前子空间的关键帧数据量已超过阈值，则删除旧关键帧数据，保存新扫描的关键帧数据；

将每个子空间的关键帧数据进行加权求和生成合成关键帧数据；

将每个子空间的所述合成关键帧数据根据全局位姿进行堆叠更新所述全局点云地图。

其中，所述将每个子空间的关键帧数据进行加权求和生成合成关键帧数据前，还包括：

对关键帧数据赋予不同的权重，按照保存的时间顺序设定关键帧数据的权重；则，所述将每个子空间的关键帧数据进行加权求和生成合成关键帧数据，具体包括：

根据所述关键帧数据的权重，将每个子空间的关键帧数据进行加权求和生成合成关键帧数据。

进一步的，所述将每个子空间的关键帧数据进行加权求和生成合成关键帧数据，其生成合成关键帧数据的计算公式为：

其中，k小于等于八叉树空间索引结构中预设的数据个数，f(·)为激活函数；x

相应地，本发明实施例还提供了一种无人系统地图构建装置，包括：

配置数据模块，用于当无人系统处于未知环境时，设定配置地图空间分割参数，以构建八叉树空间索引结构；

确定位置模块，用于控制所述无人系统实时扫描环境数据并对实时位姿进行估计，以将所述无人系统所处当前位置与所述八叉树空间索引结构中的第一个子节点进行绑定，保存扫描到的关键帧数据到该子节点中；其中，一个子节点的关键帧数据表达一个子空间的结构；

获取实时位置模块，用于控制所述无人系统开始移动，实时扫描环境数据并对实时位姿进行估计；

探索空间模块，用于根据无人系统的移动后的当前位置，通过增量式空间分割扩充所述八叉树空间索引结构，并保存扫描到的关键帧数据到当前节点中；

回环检测模块，用于在所述无人系统移动过程中，对当前位置附近子空间进行搜索，进行回环检测及优化；

生成地图模块，用于将所有关键帧数据根据全局位姿进行堆叠形成全局点云地图。

相应地，本发明实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行上述的无人系统地图构建方法。

相比于现有技术，本发明具有如下有益效果：

本发明通过用增量自下而上的方式构建子空间，先确定最小空间尺寸，在无人系统探索空间的过程中，逐步增加新的节点到八叉树空间索引结构中，这种方式可无需预知待测场景的边界；本发明提供了两种模式进行回环检测，其一是本发明采用空间近邻搜索的方式，采用帧到帧的方式进行相似度匹配；其二是基于空间和路径的方式构建子地图，通过当前关键帧数据和子地图进行相似度匹配，该方式只需对比附近子空间的旧关键帧数据，而无需遍历所有数据，大幅减少搜索对比的范围，从而减少运算量大，提高效率和成功率。在对地图更新时，本发明通过只更新关键数据帧和优化配准各关键帧的估计位姿，进而重新构建地图，从而避免简单粗暴的拼接带来的各种问题。另外，本发明基于八叉树空间索引结构进行存放数据，确定了总的数据量，不会由于长时运行数据越来越多而带来的各种问题。

附图说明

图1是本发明实施例提供的一种无人系统地图构建方法的流程示意图；

图2是本发明实施例中的初始八叉树空间索引结构；

图3是本发明实施例中的初始八叉树空间索引结构所能描述的空间；

图4是本发明实施例中的空间增量示意图；

图5是本发明实施例中八叉树增量的空间索引结构；

图6是本发明实施例中关键帧数据合成的结构示意图；

图7是本发明实施例提供的一种无人系统地图构建装置的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

参见图1，是本发明实施例提供的一种无人系统地图构建方法的流程示意图。

本发明实施例提供的无人系统地图构建方法包括步骤S1至S4，具体如下：

S1，当无人系统处于未知环境时，设定配置地图空间分割参数，以构建八叉树空间索引结构；

S2，控制所述无人系统实时扫描环境数据并对实时位姿进行估计，以将所述无人系统所处当前位置与所述八叉树空间索引结构中的第一个子节点进行绑定，保存扫描到的关键帧数据到该子节点中；其中，一个子节点的关键帧数据表达一个子空间的结构；

S3，控制所述无人系统开始移动，实时扫描环境数据并对实时位姿进行估计；

S4，根据无人系统的移动后的当前位置，通过增量式空间分割扩充所述八叉树空间索引结构，并保存扫描到的关键帧数据到当前节点中；

S5，在所述无人系统移动过程中，对当前位置附近子空间进行搜索，进行回环检测及优化；

S6，将所有关键帧数据根据全局位姿进行堆叠形成全局点云地图。

具体的，在步骤S1中，无人系统初始化，通过扫描环境获得初始位姿，确定无人系统处于未知环境时，设定配置地图空间分割参数。

其中，所述配置地图空间分割参数包含子空间正立方体边长和八叉树的最大层数。

在具体实施例中，子空间正立方体边长和八叉树的最大层数约束了后续增量分割的过程和所能描述的最大空间尺寸；其中，所述描述的最大空间尺寸的边长为：

L＝d*2

其中，d为子空间正立方体边长，n为八叉树的最大层数；在本实施例的具体应用中d设置为0.5米，n设置为20。

参见图2，是本发明实施例中的初始八叉树空间索引结构。

参见图3，是本发明实施例中的初始八叉树空间索引结构所能描述的空间。

根据构建的八叉树空间索引结构和当前位置与第一个子节点绑定，使用图2的八叉树空间索引结构表达图3的空间结构，即图2中的八叉树空间索引结构中的子节点S1-S8对应图3的空间中的子空间S1-S8。

在初始时刻t0，无人系统的位置处于在S1子空间中，保存无人系统扫描的关键帧数据。

进一步的，控制无人系统开始移动，实时扫描环境数据并对实时位姿进行估计；

在步骤S4中，进一步的，所述根据无人系统的当前位置，通过增量式空间分割扩充所述八叉树空间索引结构，并保存扫描到的关键帧数据到当前节点中，具体包括：

通过实时位姿确定无人系统的当前位置，若当前位置超出所述八叉树空间索引结构所能描述的空间结构，通过增量式空间分割增加八叉树空间索引结构的节点，在新增加的节点中的第一个子节点保存新的子空间扫描到的关键帧数据；

若当前位置未超出所述八叉树空间索引结构所能描述的空间结构，但当前位置跨越子空间时，保存当前扫描的关键帧数据到该对应的八叉树空间索引结构的子节点中。

在具体实施例中，根据无人系统的移动后的当前位置，判断当前位置是否超出了当前八叉树空间索引结构所能描述的空间范围或是否跨越了当前子空间。

参见图3，其中，在t0至t1时刻跨越了子空间但未超出当前八叉树空间索引结构所能描述的空间范围，则在S2节点中保存无人系统在该子空间内扫描的关键帧数据。

参见图4，是本发明实施例中的空间增量示意图。在t1至t2时刻，无人系统移动到了当前八叉树空间索引结构无法描述的空间范围，则进行增量式扩充。

参见图5，是本发明实施例中八叉树增量的空间索引结构。

扩充完毕后，在SB下的第一个节点保存无人系统t2时刻在新的子空间扫描的关键帧数据。

在步骤S5中，可选的，所述对当前位置附近子空间进行搜索，进行回环检测及优化，具体包括：

通过实时位姿确定无人系统的当前位置，根据当前位置基于空间搜索近邻子空间；

当子空间不为空时，计算两组扫描的关键帧数据中的对应点线特征的欧式距离和的第一最小值，当所述第一最小值小于设定阈值时，判定回环检测成功，进行回环误差优化，并更新各关键帧优化后的位姿估计。

可选的，所述对当前位置附近子空间进行搜索，进行回环检测及优化，具体包括：

通过实时位姿确定无人系统的当前位置，根据当前位置基于空间搜索近邻子空间；

当子空间不为空时，搜索无人系统路径上的前后向帧，获取路径上的子空间的关键帧配准成子地图，计算当前帧及子地图中关键帧的对应点线特征的欧式距离和的第二最小值，当所述第二最小值小于设定阈值时，判定回环检测成功，进行回环误差优化，并更新各关键帧优化后的位姿估计。

在具体实施例中，在所述无人系统移动过程中进行回环检测，有两种模式对附近子空间进行搜索对比。

其一是基于空间近邻搜索子空间，当发现子空间不为空时，计算两组扫描的关键帧数据中的对应点线特征的欧式距离和的第一最小值，即计算当前帧和原来的关键帧中对应点线特征的欧式距离和的最小值；当最小值值小于设定阈值时，表示相似度两组扫描数据的相似度高，判定为回环检测成功，则进行回环误差优化，并更新各关键帧优化后的位姿估计。

需要说明的是，最小值的求取过程是一个迭代优化的过程，当该最小值仍大于设定阈值时，则认为回环检测失败。其中的回环误差优化是指，当回环检测成功时，会得到位姿误差，将该误差平均分配到前面若干个关键帧数据中。

其二是基于空间和路径搜索子空间，即首先基于空间近邻搜索子空间，当发现子空间的关键帧数据后，搜索无人系统路径上的前后向帧，获取路径上的子空间的关键帧配准成子地图，计算当前帧及子地图中关键帧的对应点线特征的欧式距离和的第二最小值，当所述第二最小值小于设定阈值时，判定回环检测成功，进行回环误差优化，并更新各关键帧优化后的位姿估计。

需要说明的是，在无人系统移动的过程中不断将子空间的关键帧数据根据全局位姿进行堆叠形成全局点云地图。

可选的，在步骤S6之后，本发明实施例提供的方法还包括步骤S7至S11：

S7，无人系统加载当前的全局点云地图以及配置地图空间分割参数；

S8，控制无人系统开始移动，实时扫描环境数据并对实时位姿进行估计，确定当前位置所在的子空间；

S9，若当前子空间的关键帧数据量未超过阈值，直接保存新扫描的关键帧数据；若当前子空间的关键帧数据量已超过阈值，则删除旧关键帧数据，保存新扫描的关键帧数据；

S10，将每个子空间的关键帧数据进行加权求和生成合成关键帧数据；

S11，将每个子空间的所述合成关键帧数据根据全局位姿进行堆叠更新所述全局点云地图。

具体的，当无人系统还需在该场景中继续活动，需要对该场景中的地图持续进行更新。

在具体实施例中，无人系统初始化，通过扫描环境获得初始位姿，确定无人系统处于已知环境时，加载当前的全局点云地图以及配置地图空间分割参数；

其中，所述将每个子空间的关键帧数据进行加权求和生成合成关键帧数据前，还包括：

对关键帧数据赋予不同的权重，按照保存的时间顺序设定关键帧数据的权重；则，所述将每个子空间的关键帧数据进行加权求和生成合成关键帧数据，具体包括：

根据所述关键帧数据的权重，将每个子空间的关键帧数据进行加权求和生成合成关键帧数据。

参见图6，是本发明实施例中关键帧数据合成的结构示意图。

所述将每个子空间的关键帧数据进行加权求和生成合成关键帧数据，其生成合成关键帧数据的计算公式为：

其中，k小于等于八叉树空间索引结构中预设的数据个数，f(·)为输出的激活函数；x

具体的，在八叉树空间索引结构中，一个子节点对应一个子空间，可保存n个关键帧数据，无人系统后续移动到该子空间时，将新扫描的关键帧数据进行保存，当数据量大于n时则舍弃最早保存的数据；并对n帧数据赋予不同的权重，新数据权重大，老数据权重小，将n帧数据进行加权合并后作为该子空间的合成关键帧数据。

需要说明的是，在无人系统移动的过程中将所有子空间的合成关键帧数据根据全局位姿进行堆叠更新当前场景的全局点云地图。

综上，本发明实施例提供的无人系统地图构建方法，通过用增量自下而上的方式构建子空间，先确定最小空间尺寸，在无人系统探索空间的过程中，逐步增加新的节点到八叉树空间索引结构中，这种方式可无需预知待测场景的边界；本发明提供了两种模式进行回环检测，其一是本发明采用空间近邻搜索的方式，采用帧到帧的方式进行相似度匹配；其二是基于空间和路径的方式构建子地图，通过当前关键帧数据和子地图进行相似度匹配，该方式只需对比附近子空间的旧关键帧数据，而无需遍历所有数据，大幅减少搜索对比的范围，从而减少运算量大，提高效率和成功率。在对地图更新时，本发明通过只更新关键数据帧和优化配准各关键帧的估计位姿，进而重新构建更加准确的地图。

相应的，本发明实施例提供了一种无人系统地图构建装置，能够实现上述任一实施例所提供的无人系统地图构建方法的所有流程，装置中的各个模块、单元的作用以及实现的技术效果分别与上述实施例所提供的无人系统地图构建方法的作用以及实现的技术效果对应相同，这里不再赘述。

参见图7，是本发明实施例提供的一种无人系统地图构建装置的结构示意图。

所述无人系统地图构建装置，包括：

配置数据模块11，用于当无人系统处于未知环境时，设定配置地图空间分割参数，以构建八叉树空间索引结构；

确定位置模块12，用于控制所述无人系统实时扫描环境数据并对实时位姿进行估计，以将所述无人系统所处当前位置与所述八叉树空间索引结构中的第一个子节点进行绑定，保存扫描到的关键帧数据到该子节点中；其中，一个子节点的关键帧数据表达一个子空间的结构；

获取实时位置模块13，用于控制所述无人系统开始移动，实时扫描环境数据并对实时位姿进行估计；

探索空间模块14，用于根据无人系统的移动后的当前位置，通过增量式空间分割扩充所述八叉树空间索引结构，并保存扫描到的关键帧数据到当前节点中；

回环检测模块15，用于在所述无人系统移动过程中，对当前位置附近子空间进行搜索，进行回环检测及优化；

生成地图模块16，用于将所有关键帧数据根据全局位姿进行堆叠形成全局点云地图。

本发明实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序；其中，所述计算机程序在运行时控制所述计算机可读存储介质所在的设备执行上述任一实施例所述的无人系统地图构建方法。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到本发明可借助软件加必需的硬件平台的方式来实现，当然也可以全部通过硬件来实施。基于这样的理解，本发明的技术方案对背景技术做出贡献的全部或者部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例或者实施例的某些部分所述的方法。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。