基于图神经网络增强的凸集图机器人轨迹规划方法及系统

摘要: 本发明涉及机器人运动规划与智能控制,具体涉及一种基于图神经网络增强的凸集图机器人轨迹规划方法及系统。背景技术:1、机器人轨迹规划旨在为机器人生成从起始构形到目标...
基于图神经网络增强的凸集图机器人轨迹规划方法及系统

本发明涉及机器人运动规划与智能控制,具体涉及一种基于图神经网络增强的凸集图机器人轨迹规划方法及系统。

背景技术:

1、机器人轨迹规划旨在为机器人生成从起始构形到目标构形的无碰撞可执行轨迹,广泛应用于移动机器人、机械臂、移动操作臂及多机器人协作等场景。在复杂障碍环境和高维构形空间中,规划方法不仅需要保证轨迹安全性,还需要兼顾计算效率、轨迹连续性和约束满足能力。

2、现有轨迹规划方法主要包括基于采样和优化的方法。基于采样的方法能够获得可行路径,但在高维空间、狭窄通道或障碍物密集环境中计算效率容易下降,且输出路径通常需要进一步平滑处理。基于优化的方法能够生成连续轨迹并处理多类约束,但在非凸环境下对初始解较为敏感,容易陷入局部最优。

3、凸集图方法通过将自由构形空间表示为多个安全凸集,并根据安全凸集之间的连通关系构建凸集图,将机器人轨迹规划转化为离散路径选择与连续轨迹优化相结合的问题。该方法能够较好地兼顾全局路径搜索和连续轨迹优化,但通常需要求解包含离散变量和连续变量的混合整数凸优化问题。随着凸集图规模增大,整图松弛求解和随机取整过程的计算开销显著增加,限制了其在复杂场景和实时规划中的应用。

4、现有学习型规划方法虽然能够利用神经网络提高路径搜索效率,但学习型方法难以保证输出路径满足凸集图中的流守恒关系和拓扑一致性;若直接由神经网络生成连续轨迹,也难以保证轨迹安全性、连续性和约束满足情况。因此,亟需一种能够利用图神经网络降低凸集图松弛求解和取整计算开销,同时又能满足轨迹约束的机器人轨迹规划方法。

技术实现思路

1、为解决现有凸集图机器人轨迹规划方法中整图松弛求解与随机取整计算开销较高、复杂环境下规划效率受限的问题,本发明提出一种基于图神经网络增强的凸集图机器人轨迹规划方法及系统,本发明包含如下步骤:

2、步骤s1:获取机器人规划环境信息,并基于所述机器人规划环境信息构建凸集图。

3、具体地,获取机器人规划环境信息,所述机器人规划环境信息包括环境障碍物信息以及机器人构形空间约束。

4、设机器人构形为:

5、

6、其中,表示机器人的构形向量,表示机器人构形空间。根据环境障碍物信息以及机器人自身运动学、动力学或几何约束,可以确定机器人自由构形空间:

7、

8、其中,表示由环境障碍物在构形空间中诱导得到的障碍构形空间。

9、进一步地,对所述自由构形空间进行凸分解,得到多个安全凸集:

10、

11、其中,表示第个安全凸集,n表示安全凸集的数量。每个安全凸集满足:

12、

13、所述安全凸集可以采用半空间不等式、凸多面体、椭球体、二阶锥可表示凸集或其组合形式表示。例如,当安全凸集采用凸多面体表示时,可写为:

14、

15、其中,和分别表示第个安全凸集对应的半空间约束矩阵和约束向量。

16、根据多个安全凸集之间的连通关系构建凸集图:

17、

18、其中,表示顶点集合,表示边集合。顶点集合中的顶点与安全凸集一一对应,边集合中的边表示安全凸集与安全凸集之间存在可通行连接关系。

19、两个安全凸集之间建立边的条件可以表示为:

20、

21、通过上述方式,将自由构形空间划分为多个安全凸集并构建凸集图,将连续非凸空间中的轨迹规划问题转化为凸集图上的拓扑路径选择问题,为后续图神经网络推理及确定拓扑下的凸优化求解提供结构化基础。

22、步骤s2:获取机器人规划任务的起始构形和目标构形,并将其作为单一构形安全凸集添加至所述凸集图中。

23、具体地,获取本次机器人规划任务的起始构形和目标构形,分别记为:

24、

25、其中,表示机器人起始构形,表示机器人目标构形。

26、将所述起始构形和目标构形分别作为单一构形安全凸集:

27、

28、其中,表示起始单一构形安全凸集,表示目标单一构形安全凸集。将所述和分别以起始顶点和目标顶点的形式添加至所述凸集图中,得到添加任务顶点后的顶点集合:

29、

30、进一步地,根据起始构形、目标构形与所述凸集图中各安全凸集之间的可达关系,分别建立起始顶点、目标顶点与对应安全凸集顶点之间的连接边。

31、对于起始顶点,若存在安全凸集使得起始构形与安全凸集之间满足连通条件,则建立连接边:

32、

33、其中,表示与第个安全凸集对应的凸集图顶点。所述连通条件可以表示为:

34、

35、目标顶点的边构建与起始顶点类似。由此,得到添加起始顶点和目标顶点后的凸集图:

36、

37、其中:

38、

39、其中,表示扩展后的边集合,表示起始顶点与安全凸集顶点之间的连接边集合,表示安全凸集顶点与目标顶点之间的连接边集合。

40、通过上述方式,起始顶点和目标顶点作为单一构形安全凸集统一并入凸集图中,使后续图神经网络、边流预测、流守恒投影以及候选路径生成过程能够在统一的图结构上进行处理。

41、步骤s3:构建凸集图的图结构特征。

42、具体地,基于添加所述起始顶点和目标顶点后的凸集图,构建用于输入消息传递图神经网络的图结构特征。所述图结构特征包括顶点特征、边特征以及边索引信息。

43、对于任一顶点,根据该顶点对应安全凸集的几何属性、任务相关属性以及顶点类型属性构建顶点特征向量:

44、

45、其中,表示顶点对应安全凸集的代表位置,所述代表位置包括几何中心、采样中心或其他能够表征安全凸集位置的特征;表示安全凸集的尺度参数;表示顶点对应安全凸集到起始构形的距离;表示顶点对应安全凸集到目标构形的距离;表示顶点是否与起始构形相关联;表示顶点是否与目标构形相关联;表示顶点类型标识,用于区分普通安全凸集顶点、起始顶点和目标顶点。

46、其中,顶点类型标识可以表示为:

47、

48、对于任一边,根据该边连接的两个顶点之间的相对几何关系和连通属性构建边特征向量:

49、

50、其中,表示由顶点指向顶点的相对位置向量;表示两个顶点代表位置之间的距离;表示相邻安全凸集之间的交叠尺度或局部连接尺度;表示边代价;

51、进一步地,根据边集合构建边索引信息:

52、

53、由此,添加起始顶点和目标顶点后的凸集图可以表示为:

54、

55、其中,表示顶点特征集合,表示边特征集合,表示边索引信息。所述图结构特征作为后续消息传递图神经网络的输入,用于计算顶点嵌入特征和边嵌入特征,并进一步预测凸集图中各边的初始松弛边流。

56、通过上述方式,将凸集图的几何、任务及拓扑信息统一编码为图结构特征,为消息传递图神经网络提取顶点和边的嵌入特征、预测初始松弛边流提供结构化输入,从而提高候选路径推理的准确性和效率。

57、步骤s4:将所述图结构特征输入消息传递图神经网络,预测凸集图中各边的初始松弛边流。

58、具体地,将步骤s3构建得到的图结构特征 输入消息传递图神经网络。所述消息传递图神经网络根据顶点特征集合、边特征集合以及边索引信息,计算凸集图中顶点和边的嵌入特征。

59、首先,对顶点特征和边特征进行编码,得到初始顶点嵌入特征和初始边嵌入特征:

60、

61、其中,表示顶点的初始顶点嵌入特征,表示边的初始边嵌入特征,和分别表示顶点特征编码函数和边特征编码函数。

62、然后,在凸集图中进行消息传递。对于第轮消息传递,顶点接收其邻接顶点和关联边的信息,并更新顶点嵌入特征:

63、

64、其中,表示顶点在第轮消息传递中接收到的聚合消息,表示消息生成函数,表示顶点更新函数。

65、在完成预设轮数的消息传递后,根据边两端顶点的嵌入特征以及边自身的嵌入特征,计算边嵌入特征:

66、

67、其中, 表示消息传递的轮数,表示边的最终边嵌入特征。

68、进一步地,利用边流预测函数对每条边的松弛边流进行预测:

69、

70、其中,表示边的初始松弛边流,表示边流预测函数,表示将预测值映射至预设范围的激活函数。

71、通过上述方式,利用消息传递图神经网络融合凸集图的局部几何信息、任务信息和全局拓扑信息,快速预测各边的初始松弛边流,为后续流守恒修正和候选路径生成提供可靠先验,从而降低在线路径搜索的计算开销。

72、步骤s5:对所述初始松弛边流进行可微流守恒投影,得到满足起点到终点流约束的投影边流。

73、具体地,由于步骤s4中消息传递图神经网络预测得到的初始松弛边流是逐边预测结果,其可能不满足从起始顶点到目标顶点的整体流守恒约束。因此,本步骤对所述初始松弛边流进行可微流守恒投影,以获得满足凸集图流约束的投影边流。

74、对于任一顶点,构建流守恒约束:

75、

76、其中,表示边上的投影边流,表示顶点的流供需值,并定义为:

77、

78、也就是说,起始顶点输出单位流,目标顶点接收单位流,其余普通安全凸集顶点满足流入量与流出量相等。

79、为了使投影后的边流尽可能接近消息传递图神经网络预测得到的初始松弛边流,可以构建如下投影优化问题:

80、

81、满足:

82、

83、

84、其中,表示投影后的边流集合,表示边的投影权重。所述投影权重可以为预设常数,也可以根据边代价、边长度或图神经网络预测置信度确定。

85、在一种实现方式中,可以将上述约束写成矩阵形式。表示凸集图的顶点-边关联矩阵,表示由所有初始松弛边流组成的向量,表示由所有投影边流组成的向量,表示由所有顶点流供需值组成的向量,则流守恒约束可表示为:

86、

87、对应的可微投影问题可以表示为:

88、

89、满足:

90、

91、其中,是经流守恒投影修正后得到的全部边流组成的向量,表示加权二范数。

92、通过上述可微流守恒投影,能够将逐边独立预测得到的初始松弛边流修正为满足起点到终点连通约束的投影边流,从而提高后续候选路径生成的稳定性和可行性。

93、步骤s6:基于所述投影边流自回归生成候选凸集路径。

94、具体地,基于步骤s5得到的投影边流集合,从起始顶点开始,自回归地选择下一顶点或下一条边,直至到达目标顶点,从而生成一条或多条候选凸集路径。

95、设第t步已经生成的部分路径为:

96、

97、其中,表示当前顶点。根据当前顶点的出边集合,确定候选下一顶点集合:

98、

99、对于任一候选边,根据该边的投影边流、边嵌入特征以及边代价计算路径扩展得分:

100、

101、其中,表示第步从当前顶点扩展至候选顶点的得分,表示该候选边的边嵌入特征,表示该候选边的投影边流,表示该候选边的边代价,(⋅)表示路径扩展评分函数。

102、进一步地,可以将各候选边的扩展得分归一化为选择概率:

103、

104、其中,表示在当前部分路径下选择候选顶点作为下一顶点的概率。

105、根据所述选择概率,可以采用采样、贪心选择或束搜索方式生成候选凸集路径。当生成路径到达目标顶点时,得到一条完整候选凸集路径:

106、

107、为了获得多样化候选结果,可以重复执行上述自回归路径生成过程,或在每一步保留多个得分较高的部分路径,得到候选凸集路径集合:

108、

109、其中,k表示生成的候选凸集路径数量。

110、通过上述方式,结合投影边流、边嵌入特征和边代价,自回归生成一条或多条候选凸集路径,在保证路径由起始顶点向目标顶点连续扩展的同时,提高候选路径生成的效率和多样性。

111、步骤s7:基于候选离散凸集路径构建确定的拓扑凸优化问题,并输出目标机器人轨迹。

112、具体地,对于步骤s6生成得到的任一候选凸集路径,根据该候选凸集路径中依次经过的安全凸集,确定凸集图中的路径拓扑,并在所述确定的路径拓扑下构建凸优化问题,以求解对应的轨迹变量和路径代价。

113、具体地,设候选凸集路径对应的安全凸集序列为:

114、

115、其中,表示起始单一构形安全凸集,表示目标单一构形安全凸集,表示候选凸集路径中第i个中间安全凸集。

116、在所述安全凸集序列上构建轨迹优化变量,并约束控制点位于对应的安全凸集内,并根据机器人运动连续性约束、边连接约束以及轨迹代价函数,构建确定的路径拓扑凸优化问题:

117、

118、其中表示第调候选凸集路径中,与第个安全凸集对应的机器人构型向量,满足,满足相邻安全凸集之间的连接约束。其中,表示候选凸集路径对应的优化路径代价,表示相邻轨迹点或轨迹段之间的代价函数,所述代价函数包括路径长度、轨迹平滑性、控制能耗、时间代价中的至少一种。

119、对候选凸集路径集合,分别构建并求解对应的确定路径拓扑凸优化问题,得到各候选凸集路径的路径代价,然后,根据轨迹代价进行排序,并从中选取路径代价最小的候选凸集路径作为目标机器人轨迹。

120、在一种可选实施方式中,在执行在线轨迹规划之前,预先对所述消息传递图神经网络和边选择网络进行训练,所述训练过程包括如下步骤。

121、训练步骤t1:生成专家样本。

122、具体地,针对多个训练规划场景,分别构建对应的凸集图,并利用凸集图轨迹规划求解器生成专家样本。所述凸集图轨迹规划求解器包括传统凸集图求解器、混合整数凸优化求解器、随机取整求解器或其组合。

123、对于第个训练样本,专家样本可以表示为:

124、

125、其中,表示第个训练场景对应的凸集图,表示专家松弛边流,表示专家离散凸集路径,表示专家连续轨迹参数,表示专家轨迹代价。

126、所述专家样本可以包括专家松弛边流、专家离散凸集路径、专家连续轨迹参数和专家轨迹代价中的至少一种。

127、训练步骤t2:基于专家松弛边流进行边流预测监督训练。

128、具体地,将训练样本中的图结构特征输入消息传递图神经网络,得到预测松弛边流:

129、

130、其中,表示预测松弛边流,表示参数为的消息传递图神经网络。

131、基于专家松弛边流和预测松弛边流,构建边流预测损失:

132、

133、其中,e表示凸集图的边集合,表示边的预测松弛边流,表示边的专家松弛边流。

134、训练步骤t3:构建流守恒损失和边界约束损失。

135、为了提高预测松弛边流与全局流守恒约束的一致性,构建流守恒损失:

136、

137、其中,a表示顶点-边关联矩阵,b表示预设散度向量。

138、进一步地,为使预测松弛边流位于预设流值范围内,构建边界约束损失:

139、

140、其中,用于惩罚小于0或大于1的预测松弛边流。

141、训练步骤t4:基于专家离散凸集路径训练边选择网络。

142、具体地,在自回归生成候选离散凸集路径的过程中,将专家离散凸集路径中的边作为正样本,对边选择网络进行监督训练。

143、设第t个路径生成步骤中,边选择网络对候选出边输出的选择概率为:

144、

145、其中,表示第t步的路径生成状态,表示边选择网络参数。

146、若专家路径在第t步选择的边为,则候选路径选择损失可以表示为:

147、

148、其中,t表示专家离散凸集的数量。

149、当存在多条代价接近的专家候选路径时,可以将多条专家候选路径同时作为正样本,构建多正例路径选择损失:

150、

151、其中,表示第t步中属于专家正样本路径的候选边集合。

152、进一步地,可以根据候选路径对应的轨迹代价构建候选路径代价排序损失,使代价较低的候选路径具有更高排序优先级:

153、

154、其中,和表示两条候选离散凸集路径,和表示对应轨迹代价,和表示边选择网络给出的路径评分,表示排序间隔参数。

155、因此,监督训练总损失可以表示为:

156、

157、其中,为损失权重系数。

158、训练步骤 t5:采用强化学习对边选择网络进行微调。

159、在完成监督训练之后,将候选离散凸集路径的生成过程建模为马尔可夫决策过程,并采用强化学习算法对边选择网络进行微调。

160、具体地,状态包括当前顶点、已生成的部分路径、候选出边集合、顶点嵌入特征、边嵌入特征以及投影松弛边流;动作为从当前顶点的候选出边中选择一条边作为下一扩展边。

161、策略网络表示为:

162、

163、其中,表示参数为的边选择策略网络。

164、奖励函数可以表示为:

165、

166、其中,表示到达目标顶点奖励,表示候选路径对应的轨迹代价,表示无效路径惩罚,表示由确定的拓扑凸优化问题求解结果确定的终端奖励,为奖励权重系数。

167、所述终端奖励可以根据候选路径是否可行、约束是否满足以及轨迹代价确定。例如:

168、

169、其中,和为预设正数。

170、在一种实现方式中,可以采用策略梯度算法、近端策略优化算法、演员-评论家算法、深度q网络算法或其组合,对边选择网络进行微调。

171、通过上述训练方式,消息传递图神经网络能够学习凸集图中的松弛边流分布规律,边选择网络能够学习从起始顶点到目标顶点的候选路径生成策略,从而在在线规划阶段减少整图松弛求解和随机取整计算开销,并提高候选路径生成的成功率和轨迹质量。

172、基于图神经网络增强的凸集图机器人轨迹规划系统包括环境感知与建图单元、计算服务器、机器人控制单元以及通信单元。所述环境感知与建图单元用于获取机器人规划环境信息,包括环境障碍物信息以及机器人构形空间约束,并将所述机器人规划环境信息传输至计算服务器。所述计算服务器用于执行凸集图构建、图结构特征编码、松弛边流预测、可微流守恒投影、候选离散凸集路径生成以及确定的拓扑凸优化问题求解;其中,所述计算服务器内设置有凸集图构建模块、任务获取模块、图特征编码模块、边流预测模块、可微流守恒投影模块、候选路径生成模块以及确定的拓扑凸优化模块。

173、具体地,计算服务器根据机器人规划环境信息对机器人自由构形空间进行凸分解,得到多个安全凸集,并根据安全凸集之间的连通关系构建凸集图(步骤 s1)。在获取机器人规划任务的起始构形和目标构形后,计算服务器将起始构形和目标构形分别作为起始顶点和目标顶点加入凸集图,并建立对应连接边(步骤 s2)。随后,计算服务器提取凸集图的顶点特征、边特征和边索引信息,构建图结构特征(步骤 s3),并将图结构特征输入消息传递图神经网络,获得顶点嵌入特征和边嵌入特征,预测各边的初始松弛边流(步骤 s4)。计算服务器对初始松弛边流进行可微流守恒投影,得到满足流守恒约束的投影松弛边流(步骤 s5),并基于投影松弛边流、顶点嵌入特征和边嵌入特征生成至少一条候选离散凸集路径(步骤 s6)。对于生成的候选离散凸集路径,计算服务器建立确定的拓扑凸优化问题,求解连续轨迹参数,并根据轨迹代价和约束满足情况选取目标机器人轨迹输出至机器人控制单元(步骤 s7)。

174、在一种实现方式中,计算服务器还包括训练模块。所述训练模块用于利用凸集图轨迹规划求解器生成专家样本,并基于专家样本对消息传递图神经网络和边选择网络进行监督训练;在监督训练之后,所述训练模块还用于将候选离散凸集路径的生成过程建模为马尔可夫决策过程,并采用强化学习算法对边选择网络进行微调。所述机器人控制单元用于接收所述目标机器人轨迹,并根据所述目标机器人轨迹控制机器人执行运动。所述通信单元用于实现环境感知与建图单元、计算服务器和机器人控制单元之间的数据传输。

175、有益效果:本发明提出一种基于图神经网络增强的凸集图机器人轨迹规划方法及系统。相对于现有技术,本发明至少具有以下有益效果。

176、第一,本发明利用图神经网络预测各边的初始松弛边流,为拓扑路径选择提供数据驱动的全局先验,避免传统方法在每次在线规划时反复求解大规模整图松弛问题,减少无目的图搜索及低价值边的扩展,从而降低在线计算开销。

177、第二,本发明设置可微流守恒投影层,在尽可能保留初始预测边流分布的同时,对不满足流守恒关系的预测结果进行全局修正,减少中间顶点流入与流出不一致、路径中断及无效分支等情况,提高边流分布的拓扑一致性。所述投影层具有可微性,能够将流守恒约束引入图神经网络的端到端训练过程,使网络逐步学习符合凸集图连通规律的边流表示。

178、第三,本发明形成“学习模型进行拓扑快速推理、数学优化进行连续轨迹求解”的分层规划架构,发挥图神经网络推理速度快和凸优化约束处理能力强的优势,在轨迹规划效率、轨迹质量及约束可验证性之间取得平衡。同时,预测边流、候选凸集路径和连续轨迹均具有明确的物理或拓扑含义,便于对规划结果进行分析、筛选和故障定位,提高轨迹规划过程的可解释性。

179、综上,本发明通过凸集图结构化建模、图神经网络边流预测、流守恒投影修正、自回归候选路径生成以及确定拓扑下的连续轨迹凸优化,实现离散拓扑推理与连续轨迹求解的有效结合,减少传统整图松弛求解和随机取整所产生的在线计算开销,并提高候选路径的连通性、最终轨迹的安全性、规划结果的稳定性及轨迹规划系统的实时处理能力。