基于自主探索的室内地图建立方法及装置

摘要

本发明公开了一种基于自主探索的室内地图建立方法及装置，该方法包括获取雷达设备采集的环境信息，并基于环境信息构建局部室内地图；判断局部室内地图是否具有前沿点；若是，驱动建图机器人移动至目标探索点，并返回执行获取雷达设备采集的环境信息，并基于环境信息构建局部室内地图步骤；否则，结束探索过程并保存地图。本发明通过改进的建图方法，并考虑RRT路径长、信息增益和方向差，驱动建图机器人移动至探索收益最高的目标探索点，在此之后，重复获取环境信息并移动建图机器人，直至获得完整室内地图；同时根据建图机器人停留时间及前沿点数量，对建图机器人进行回收和脱困处理，提高了建图质量、建图效率以及建图机器人的回收率。

说明书

技术领域

本发明属于室内探索与建图技术领域，尤其涉及基于自主探索的室内地图建立方法及装置。

背景技术

当前，室内地图的建立大多依靠操作人员通过驾驶、遥控或手持等方法移动建图设备。此类方法所建地图质量较高，但缺乏自主性，在灾害救援、敌情探查等操作人员难以进入的场景下无法使用。

而现有的无需人工操作，利用机器人进行自主建图方法较少，且普遍存在建图质量较差、探索效率低下、机器人易被困于狭窄区域且探索完成难以回收等问题。因此，如何提高建图质量、建图效率以及建图机器人回收率，是一个亟需解决的技术问题。

发明内容

本发明的主要目的在于提供一种基于自主探索的室内地图建立方法及装置，旨在解决目前基于自主探索的室内地图建立过程中，建图质量、建图效率以及建图机器人回收率不高的技术问题。

为实现上述目的，本发明提供一种基于自主探索的室内地图建立方法，用于建图机器人的控制设备，所述建图机器人还设置有雷达设备，所述方法包括以下步骤：

获取雷达设备采集的环境信息，并基于所述环境信息构建局部室内地图；

判断所述局部室内地图是否具有前沿点；其中，所述前沿点为所述局部室内地图中已知区域和未知区域的边界点；

若是，驱动建图机器人移动至目标探索点，并返回执行获取雷达设备采集的环境信息，并基于所述环境信息构建局部室内地图步骤；否则，结束探索过程并保存地图；其中，所述目标探索点为所述建图机器人的RRT路径长、信息增益和方向差的探索收益最高的前沿点。

进一步的，所述获取雷达设备采集的环境信息，并基于所述环境信息构建局部室内地图步骤，具体包括：

获取雷达设备采集的环境信息，并基于所述环境信息构建代价地图；

根据所述雷达的扫描频率，调整所述建图机器人运动时所述代价地图中的运动畸变，以构建局部室内地图。

进一步的，获取雷达设备采集的环境信息，并基于所述环境信息构建代价地图步骤，具体包括：

获取雷达设备采集的环境信息；其中，所述环境信息为环境点集；

根据环境点集构建代价地图；其中，所述代价地图包括环境点集中的环境点；

获取静态层生成阈值，并根据所述静态层生成阈值更新代价地图。

进一步的，判断所述局部室内地图是否具有前沿点步骤之前，还包括利用RRT获取所述局部室内地图中的前沿点；

驱动建图机器人移动至目标探索点步骤，具体包括：

获取当前位置点到每一个所述前沿点的RRT路径长、信息增益和方向差；

利用所述RRT路径长、所述信息增益和所述方向差，计算当前位置到每一所述前沿点的探索收益；

将探索收益最高的前沿点作为目标探索点，并驱动建图机器人移动至目标探索点。

进一步的，所述方法还包括：

获取所述建图机器人的停留时间；

判断所述停留时间是否超过预设阈值，若是，根据所述局部室内地图，驱动所述建图机器人移动至脱困目标点或返程目标点。

进一步的，所述根据所述局部室内地图，驱动所述建图机器人移动至脱困目标点或返程目标点步骤，具体包括：

若所述局部室内地图具有前沿点，驱动所述建图机器人移动至脱困目标点；其中，所述脱困目标点为所述建图机器人当前位置之前的前沿点。

若所述局部室内地图不具有前沿点，驱动所述建图机器人移动至返程目标点；其中，所述返程目标点为所述建图机器人的初始位置点。

另一方面，本发明还提供了一种基于自主探索的室内地图建立装置，所述基于自主探索的室内地图建立装置配置于建图机器人，所述建图机器人还设置有雷达设备，所述装置包括：

构建模块，用于获取雷达设备采集的环境信息，并基于所述环境信息构建局部室内地图；

判断模块，用于判断所述局部室内地图是否具有前沿点；其中，所述前沿点为所述局部室内地图中已知区域和未知区域的边界点；

第一驱动模块，用于局部室内地图具有前沿点时，驱动建图机器人移动至目标探索点，并返回执行获取雷达设备采集的环境信息，并基于所述环境信息构建局部室内地图步骤；否则，结束探索过程并保存地图；其中，所述目标探索点为探索收益最高的所述前沿点。

可选的，所述构建模块还用于获取雷达设备采集的环境信息，并基于所述环境信息构建代价地图；根据所述雷达的扫描频率，调整所述建图机器人运动时所述代价地图中的运动畸变，以构建局部室内地图。

可选的，所述构建模块还用于获取雷达设备采集的环境信息；其中，所述环境信息为环境点集；利用Cartographer程序处理所述环境信息，以构建代价地图；其中，所述代价地图包括环境点集中的环境点获取静态层生成阈值，并根据所述静态层生成阈值更新代价地图；

可选的，所述基于自主探索的室内地图建立装置还包括获取模块，所述获取模块用于利用RRT获取所述局部室内地图中的前沿点；

所述第一驱动模块还用于获取当前位置点到每一个所述前沿点的RRT路径长、信息增益和方向差；利用所述RRT路径长、所述信息增益和所述方向差，计算当前位置到每一所述前沿点的探索收益；将探索收益最高的前沿点作为目标探索点，并驱动建图机器人移动至目标探索点。

可选的，所述基于自主探索的室内地图建立装置还包括第二驱动模块，所述第二驱动模块用于获取所述建图机器人的停留时间；判断所述停留时间是否超过预设阈值，若是，根据所述局部室内地图，驱动所述建图机器人移动至脱困目标点或返程目标点。

可选的，所述第二驱动模块还用于若所述局部室内地图具有前沿点，驱动所述建图机器人移动至脱困目标点；其中，所述脱困目标点为所述建图机器人当前位置之前的前沿点。若所述局部室内地图不具有前沿点，驱动所述建图机器人移动至返程目标点；其中，所述返程目标点为所述建图机器人的初始位置点。

本发明的有益效果在于：

该方法包括获取雷达设备采集的环境信息，并基于环境信息构建局部室内地图；判断局部室内地图是否具有前沿点；若是，驱动建图机器人移动至目标探索点，并返回执行获取雷达设备采集的环境信息，并基于环境信息构建局部室内地图步骤；否则，结束探索过程并保存地图。本发明通过获取当前位置的环境信息构建局部室内地图，并根据局部室内地图驱动建图机器人移动至通过多策略确定的探索收益最高的目标探索点，在此之后，重复获取环境信息并移动建图机器人，直至获得完整室内地图；同时根据建图机器人停留时间及前沿点数量，对建图机器人进行回收和脱困的策略设定，提高了建图质量、建图效率以及建图机器人的回收率。

附图说明

图1为本发明基于自主探索的室内地图建立方法的第一实施例的流程示意图；

图2为本发明基于自主探索的室内地图建立方法的第一实施例中计算方向差的示意图；

图3为本发明基于自主探索的室内地图建立方法的第二实施例的流程示意图；

图4为本发明实施例中一种基于自主探索的室内地图建立装置的结构框图。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例1

当前，室内地图的建立大多依靠操作人员通过驾驶、遥控或手持等方法移动建图设备。此类方法所建地图质量较高，但缺乏自主性，在灾害救援、敌情探查等操作人员难以进入的场景下无法使用。而现有的无需人工操作，利用机器人进行自主建图方法较少，且普遍存在建图质量较差、探索效率低下、机器人易被困于狭窄区域且探索完成难以回收等问题。因此，如何提高建图质量、建图效率以及建图机器人回收率，是一个亟需解决的技术问题。

为了解决这一问题，提出本发明的基于自主探索的室内地图建立方法的各个实施例。本发明提供的基于自主探索的室内地图建立方法通过改进的建图方法，并考虑RRT路径长、信息增益和方向差，驱动建图机器人移动至探索收益最高的目标探索点，在此之后，重复获取环境信息并移动建图机器人，直至获得完整室内地图；同时根据建图机器人停留时间及前沿点数量，对建图机器人进行回收和脱困处理，提高了建图质量、建图效率以及建图机器人的回收率。

本发明实施例提供了一种基于自主探索的室内地图建立方法，参照图1，图1为本发明基于自主探索的室内地图建立方法的第一实施例的流程示意图。

本实施例中，所述基于自主探索的室内地图建立方法用于建图机器人的控制设备，建图机器人还设置有雷达设备，该方法包括以下步骤：

步骤S100，获取雷达设备采集的环境信息，并基于所述环境信息构建局部室内地图。

在本实施例中，建图机器人使用搭载思岚A2激光雷达与英伟达Jetson NX计算单元的Turtlebot3 Waffle机器人平台，平台搭载Ubuntu18.04系统，并借助ROS开发套件实现基本的运动建模与控制。利用机器人搭载的路由进行局域网组网，可以实时回传环境数据与所建地图，帮助操作人员进行决策。

具体而言，雷达设备采集的环境信息为环境点集，进而基于环境信息构建局部室内地图，通过获取雷达设备采集的环境信息，并基于环境信息构建代价地图，然后，根据雷达的扫描频率，调整建图机器人运动时代价地图中的运动畸变，用以构建局部室内地图。

其中，获取雷达设备采集的环境信息，并基于环境信息构建代价地图通过获取雷达设备采集的环境信息点集，然后利用Cartographer程序处理环境信息，以构建具有环境点集中的环境点的代价地图，最后，获取静态层生成阈值，并根据静态层生成阈值更新代价地图。

需要说明的是，在本实施例中，在建图机器人执行建图动作后，获取激光雷达扫描得到的环境点集：

H＝{h

其中，H为环境点集，h

进一步地，当空间中对应的障碍物被雷达扫描到时，在获取雷达设备采集的环境信息后，利用Cartographer程序处理所述环境信息，生成环境点的代价地图，并根据获取的静态层生成阈值，更新代价地图，赋予障碍物对应的环境点概率为p

在本实施例中，由于自主探索任务中，建图机器人在同一位置停留时间较短，建图时会出现地图边缘不清晰的现象，因此，需要加快边缘成形速度。对应的，可根据场景实验结果调整p

其中，p为最终概率。

需要说明的是，在本实施例中，由于环境信息是雷达设备在建图机器人不断运动的情况下获得的，如果假定每帧扫描中的所有点集都发自同一基准，则会导致地图的运动畸变，因此，需要根据雷达的扫描频率，调整建图机器人运动时代价地图中的运动畸变。

进一步地，消除畸变的方法为根据给定的扫描频率，对每帧扫描中的所有点束赋予时间戳，假设雷达设备处于匀速运动中，根据帧间位移和时间间隔计算出速度，根据时间戳与估计速度，计算基准偏移量，来矫正畸变。然而，在实际运动中，由于雷达设备的扫描频率会在标定值附近波动，因此，点束的时间戳存在一定偏差。因此，使用连续两帧之间实际时间差来代替根据频率计算出的固定值，避免频率波动带来的时间估计误差。

在实际运行中，可使用Ceres优化库，构建最小二乘问题，对指定时间窗口内的扫描帧进行匹配，求解扫描位姿ξ，通过将静态层的障碍物判定阈值设定为55，使得正常生成代价地图。进而，在时间窗口内帧的关联完成后，会与所有扫描帧一起参与回环检测。回环检测的任务为寻找搜索窗口W中的一个扫描帧的位姿ξ

其中，窗口W中的一个扫描帧的位姿的表达式为：

其中，T

容易理解的，在得到地图的回环预设后，进一步优化地图，以此调整建图机器人运动时代价地图中的运动畸变。

步骤S200，判断所述局部室内地图是否具有前沿点；其中，所述前沿点为所述局部室内地图中已知区域和未知区域的边界点。

具体而言，在获得建图机器人在当前位置构建的局部室内地图后，需要判断该局部室内地图是否具有前沿点；其中，前沿点为局部室内地图中已知区域和位置区域的边界点。通过判断当前局部室内地图是否具有前沿点，以此得知当前获取的局部室内地图是否为完整的室内地图。

容易理解的，若当前局部室内地图已无前沿点，则表明建图机器人已获得目标空间的所有环境信息，该局部室内地图为目标空间的完整的室内地图；若当前局部室内地图还有前沿点，则表明建图机器人仅获得目标空间的部分环境信息，从而根据部分环境信息构建的室内地图也即为局部室内地图。

进一步地，为了获得完整的室内地图，在判断当前局部室内地图还有前沿点时，需要生成新的探索点进行环境信息的采集，以此来完善室内地图的完整性。

步骤S300，若是，驱动建图机器人移动至目标探索点，并返回执行获取雷达设备采集的环境信息，并基于所述环境信息构建局部室内地图步骤；否则，结束探索过程并保存地图；其中，所述目标探索点为探索收益最高的所述前沿点。

具体而言，在判断局部室内地图中具有已知区域和未知区域的边界点，即前沿点时，驱动建图机器人移动至探索收益最高的前沿点，并再次构建局部室内地图，直至局部室内地图中无前沿点，则当前局部室内地图变为建立好的完整室内地图。

在本实施例中，探索收益最高的前沿点需要满足路径短、获取信息多和方向差最小的前沿点。

在计算前沿点的路径时，使用RRT树中起点与前沿点x

进一步地，每当系统找到一个前沿点，都会计算一次当前位置到该点的RRT路径长，与其坐标一起返回给评估单元。系统计算RRT路径长的方法为：在随机树中，从该前沿点开始逐级回溯，直至找到树的起点，即建图机器人的近似当前位置。在每一级中，寻找与上一级相连的、最早进入RRT列表的点，将其坐标计入一个新的近似路径列表。计算近似路径列表中所有相邻元素的欧氏距离并相加，即为所求RRT路径长。

在计算获取信息时，通过信息增益I代表到达一个前沿点所能获得的信息量。具体而言，可以该前沿点为圆心作圆，其半径为信息增益半径，该圆范围内未知区域的大小即信息增益。

进一步地，计算以前沿点为圆心，半径为信息增益半径的圆内未知区域的大小，作为该前沿点的信息增益值。应用时，需综合雷达设备扫描范围与算力选取合适的信息增益半径。本实施例使用的思岚A2激光雷达在12m内具有较高的精度，同时希望减少运算资源占用，经测试选取8m作为信息增益半径。

在计算方向差时，如图2所示，系统首先计算前沿点到机器人当前位置的连线与坐标系x轴正向的夹角β，再求其与机器人航向角α的差值，即可得到方向差δ。将方向差值统一到区间[0,π]，并通过ELU函数变体进行处理，得到最终参与计算的方向差值。

容易理解的，在获取上述三项信息后，对导航成本(即路径长)、信息增益进行Z-Score归一化处理，使处理后的数据符合均值为0的正态分布。随后，综合三项信息进行前沿点探索收益评估。评估式中，信息增益、导航成本和方向差的调节权重λ

进一步地，导航成本、信息增益两个指标分属于长度与面积，在计算时存在规模差异。先前方案使用固定权重来平衡上述差异，难以合理调节不断变化的规模差距。因此，本发明在利用上述指标计算前沿点的综合收益前，首先使用归一化方法来平衡导航成本、信息增益的规模，而仅使用权重来表明不同指标的重要性。

本实施例采用z-score归一化方法，处理后数据符合标准正态分布。分别使用Avg()与StdDev()表示求均值与标准差的运算，对于数据d∈D，其归一化值d

其中，D为数据d的集合，d为前述中提到的数据，包括信息增益、路径长等。

因为需避免在到达前沿点时，当前位置前沿点对应的导航成本过小，导致信息增益调节弱化或失效,所以λ

其中，x

当前沿点x

选择前沿点的目标为：经过尽可能短的路径，获取尽可能多的信息，同时避免过大的转向。综合上述信息，对于给定的前沿点x

R(x

式中，机器人当前位置为x

在本实施例中，通过获取当前位置的环境信息构建局部室内地图，并根据局部室内地图驱动建图机器人移动至探索收益最高的目标探索点，在此之后，重复获取环境信息并移动建图机器人，直至获得完整室内地图，提高了建图质量和建图效率。

为了便于理解，参阅图3，图3为本发明基于自主探索的室内地图建立方法的第二实施例的流程示意图。基于如图1所示的基于自主探索的室内地图建立方法的第一实施例，本实施例提出基于自主探索的室内地图建立方法的第二实施例，具体如下：

步骤S400，获取所述建图机器人的停留时间。

建图机器人在移动过程中，可能会被障碍物所阻挡导致中止建图过程，也可能在建图完成后无法正确返程，虽然路径规划在理论上可帮助建图机器人躲避障碍物，但在实际应用中，受参数调节失准、系统响应延迟等因素的影响，机器人可能出现被墙壁、障碍物等阻挡的情况，即陷入被困状态。传统建图机器人中已有部分预设的基本脱困策略，包括定期清除代价地图、进行原地旋转等。这些策略较为简单，未能指引机器人例开受困区域，仅能在部分场景下生效。

具体而言，若要帮助被阻挡的建图机器人调整位置以便重新执行建图任务，或者帮助建图完成的建图机器人返程，需要先判断当前建图机器人所处状态，即处于建图状态、阻挡状态还是完成状态。本实施例中，通过获取建图机器人的停留时间，若停留时间未超过预设值，则认为当前建图机器人处于建图状态，否则，认为当前建图机器人处于非建图状态。

步骤S500，判断所述停留时间是否超过预设阈值，若是，根据所述局部室内地图，驱动所述建图机器人移动至脱困目标点或返程目标点。

具体而言，在判断当前建图机器人处于非建图状态时，进一步判断此时建图机器人构建的局部室内地图是否具有前沿点；若所述局部室内地图具有前沿点，驱动所述建图机器人移动至脱困目标点；其中，所述脱困目标点为所述建图机器人当前位置之前的前沿点。若所述局部室内地图不具有前沿点，驱动所述建图机器人移动至返程目标点；其中，所述返程目标点为所述建图机器人的初始位置点。

在本实施例中，提供了一种根据建图机器人停留时间及前沿点数量，对建图机器人进行回收和脱困处理的方案，在建图机器人停留时间超过预设阈值后，根据建图机器人当前建立的局部室内地图，判断建图机器人的状态，并根据状态执行对应的回收或脱困处理，提高了建图机器人在室内建图过程中的回收率。

参照图4，图4为本发明基于自主探索的室内地图建立装置第一实施例的结构框图。

如图4所示，本发明实施例提出的基于自主探索的室内地图建立装置配置于建图机器人，所述建图机器人还设置有雷达设备，所述装置包括：

构建模块10，用于获取雷达设备采集的环境信息，并基于所述环境信息构建局部室内地图；

判断模块20，用于判断所述局部室内地图是否具有前沿点；其中，所述前沿点为所述局部室内地图中已知区域和未知区域的边界点；

第一驱动模块30，用于局部室内地图具有前沿点时，驱动建图机器人移动至目标探索点，并返回执行获取雷达设备采集的环境信息，并基于所述环境信息构建局部室内地图步骤；否则，结束探索过程并保存地图；其中，所述目标探索点为探索收益最高的所述前沿点。

作为一种实施方式，构建模块10还用于获取雷达设备采集的环境信息，并基于所述环境信息构建代价地图；根据所述雷达的扫描频率，调整所述建图机器人运动时所述代价地图中的运动畸变，以构建局部室内地图。

作为一种实施方式，构建模块10还用于获取雷达设备采集的环境信息；其中，所述环境信息为环境点集；利用Cartographer程序处理所述环境信息，以构建代价地图；其中，所述代价地图包括环境点集中的环境点获取静态层生成阈值，并根据所述静态层生成阈值更新代价地图。

作为一种实施方式，基于自主探索的室内地图建立装置还包括获取模块40，所述获取模块40用于利用RRT获取所述局部室内地图中的前沿点；所述第一驱动模块30还用于获取当前位置点到每一个所述前沿点的RRT路径长、信息增益和方向差；利用所述RRT路径长、所述信息增益和所述方向差，计算当前位置到每一所述前沿点的探索收益；将探索收益最高的前沿点作为目标探索点，并驱动建图机器人移动至目标探索点。

本实施例提供的基于自主探索的室内地图建立装置，通过获取当前位置的环境信息构建局部室内地图，并根据局部室内地图驱动建图机器人移动至探索收益最高的目标探索点，在此之后，重复获取环境信息并移动建图机器人，直至获得完整室内地图，提高了室内的建图质量与建图效率。

基于本发明上述基于自主探索的室内地图建立装置第一实施例，提出本发明基于自主探索的室内地图建立装置的第二实施例。在本实施例中，基于自主探索的室内地图建立装置还包括第二驱动模块50，所述第二驱动模块50用于获取所述建图机器人的停留时间；判断所述停留时间是否超过预设阈值，若是，根据所述局部室内地图，驱动所述建图机器人移动至脱困目标点或返程目标点。

作为一种实施方式，第二驱动模块50还用于若所述局部室内地图具有前沿点，驱动所述建图机器人移动至脱困目标点；其中，所述脱困目标点为所述建图机器人当前位置之前的前沿点。若所述局部室内地图不具有前沿点，驱动所述建图机器人移动至返程目标点；其中，所述返程目标点为所述建图机器人的初始位置点。

本实施例提供了一种根据建图机器人停留时间及前沿点数量，对建图机器人进行回收和脱困处理的技术方案，在建图机器人停留时间超过预设阈值后，根据建图机器人当前建立的局部室内地图，判断建图机器人的状态，并根据状态执行对应的回收或脱困处理，提高了建图机器人在室内建图过程中的回收率。

本实施例提供的基于自主探索的室内地图建立装置的其他实施例或具体实现方式可参照上述各方法实施例，此处不再赘述。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。