[发明专利]一种基于约束引导和空间优化策略的场景主动式建图方法

权利要求书

1.一种基于约束引导和空间优化策略的场景主动式建图方法，其特征在于，包括全局规划阶段和局部规划阶段；全局规划阶段中计算机器人的下一探索目标点，用于引导机器人探索场景；当下一探索目标点确定后，局部规划阶段根据该下一探索目标点、机器人所在位置以及构建的占用地图，生成具体动作，驱动机器人前往下一探索目标点，同时收集观测数据，用于更新占用地图信息；具体包括以下步骤：

步骤1：根据机器人扫描数据生成状态；

状态包括三部分，分别为占用地图M_t、距离地图D(M_t,ω_t)和边界点熵I(M_t)；ω_t表示机器人所处位置；

s(ω_t)＝(M_t,D(M_t,ω_t),I(M_t))

1.1)占用地图；

根据机器人在位置ω_t处的观测值C(ω_t)，通过带有机器人位姿信息的深度图反投影得到3D场景模型；以3D场景模型从上向下的视角构建一个2D全局地图作为占用地图M_t；在t时刻，该占用地图表示为M_t∈[0,1]^X×Y×2，X、Y分别表示占用地图长度、占用地图宽度；占用地图中包括两个通道分别表示已探索区域和占用区域；对占用地图M_t中的网格进行分类，已探索但是未被占用的网格类别为自由，占用的网格类别为占用，尚未探索的网格类别为未知；边界网格其为与未知网格相邻的自由网格；

1.2)距离地图；

给定当前位置ω_t和当前构建的占用地图M_t，构建距离地图其中D_x,y(M_t,ω_t)表示位置(x,y)距离机器人所处位置ω_t的测地距离：

测地距离是占用地图M_t中两点之间无碰撞的最短距离，使用快速行进法计算测地距离

1.3)边界点熵；

引入边界点熵作为约束，用于占用地图M_t高度不完整时减少搜索空间；边界点f∈F_t表示潜在的下一最佳探索目标点，基于边界点具有小范围聚集、大范围分散的特点，引入边界点熵I用于编码边界点的空间分布信息，编码后的空间分布信息作为全局规划策略Π中演员网络的输入之一，用于约束动作搜索，边界点熵I的定义如下：

其中，I_x,y(M_t)表示占用地图M_t中以边界点所在位置(x,y)为中心的γ×γ邻域范围内边界点的数量；每个边界点所包含的空间分布信息包括该点所在位置的(x,y)坐标和其邻域范围内边界点空间分布的统计信息；

步骤2：根据状态输入计算机器人动作空间的概率分布；

离线策略学习方法近端策略优化PPO作为策略优化器，用于全局规划策略的训练优化和决策执行；策略优化器包括演员网络和评论家网络；

演员网络使用多层感知机MLP作为编码器进行特征提取，并使用图神经网络进行特征融合；根据状态s(ω_t)给定的边界点构建图，对所构建的图进行特征提取和特征融合，获得边界点分数；

评论家网络包括五层卷积层、一个展平操作以及三层线性层，每个卷积层和线性层后面均连接一个ReLu激活函数，展平操作用于将多维数据展平成一维；评论家网络用于预测占用地图的状态值V(s(ω_t))以指示边界点当前状态获得的评论值，评论值作为损失函数的组成，用于训练演员网络；

根据状态输入计算动作空间的概率分布过程具体如下：

基于边界网格F_t和探索路径Ω_t＝{ω₀,...,ω_t}构建一个图G(F_t,Ω_t)用于表示当前场景的上下文信息，图G(F_t,Ω_t)建立机器人同占用地图M_t中提取的边界点之间的对应关系，并将状态s(ω_t)给出的信息赋予G(F_t,Ω_t)的节点和边中；对于每个节点n_i，节点输入特征包括：占用地图M_t中的(x,y)坐标信息，语义标签信息表示n_i∈F_t或n_i∈Ω_t，历史标签信息表示n_i是当前节点ω_t或历史探索节点n_i∈{ω₀,...,ω_t-1}，以及边界点熵I_ni(M_t)；节点边缘特征由多层感知机MLP进行特征提取得到，其中表示节点n_j到节点n_i的测地距离；将节点输入特征和节点边缘特征输入到演员网络进行特征提取和特征融合并输出一组边界点分数；基于该组边界点分数，计算得到机器人每个动作的采样概率Π_mask(f|s(ω_t))；

步骤3：动作掩码引导的空间对齐和下一探索目标点的选取；

机器人根据选择具有最高分数的边界点作为下一探索目标点ω_t+1，其中Π表示全局规划策略，用于计算当前状态下每个边界点的得分；下一探索目标点ω_t+1为从边界网格F_t中选择一个边界点f；

基于边界点分数，引入动作掩码策略用于解决空间度量的错位问题；动作掩码策略包括两个动作掩码：有效距离掩码和脱困掩码，用于过滤全局规划策略Π动作空间中的动作，将动作采样约束在有效动作空间；

3.1)有效距离掩码；

有效距离掩码用于过滤全局规划策略动作空间中无效的目标；根据机器人位置ω_t到下一个最佳探索目标点的测地距离信息过滤动作空间；设置最近阈值β_near和最远阈值β_far；对于超出阈值范围[β_near,β_far]的潜在的下一最佳探索目标点，将其采样概率设置为0，有效距离掩码后的∏_mask(f|s(ω_t))如下：

其中，Π_mask(f|s(ω_t))表示选择边界点f作为下一探索目标点的动作掩码概率，是机器人位置ω_t到边界点f的测地距离，Π(f|s(ω_t))是来自演员网络编码器的原始概率分布；

3.2)脱困掩码

脱困掩码用于过滤掉动作空间中导致机器人持续困住的动作；通过计算过去3个全局规划阶段内机器人的最大移动距离和最大扫描新增面积c_max＝max_{i∈{t-1,t-2}}||c(M_i)-c(M_i+1)||，其中，c(M_t)表示根据占用地图M_t计算t时刻扫描区域的面积大小，当移动距离l_max和扫描新增面积c_max大于设定阈值时，认定该动作合理；否则，将该动作所对应的概率值设置为0，即Π_mask(a|s(ω_t))＝0；

步骤4：规划路径前往下一探索目标点；

机器人根据其自身位置ω_t、计算得到的下一探索目标点ω_t+1和构建的占用地图M_t，使用快速行进算法FMM规划一条移动轨迹驱动机器人前往下一探索目标点，同时收集机器人移动过程中扫描的观测数据，用于更新地图信息；

步骤5：探索终止判断

重复步骤1-步骤4，判断探索是否满足终止条件，当机器人的扫描覆盖率超过设定的阈值，或者机器人的探索步数超过了设置的最大步数，终止探索。