一种面向配电柜受限空间插件插拔作业的机械臂自主运动规划方法与系统

摘要: 本发明涉及机械臂运动规划,特别涉及一种面向配电柜受限空间插件插拔作业的机械臂自主运动规划方法与系统。背景技术:1、配电柜内部通常设置有断路器、继电器、端子排、线...
一种面向配电柜受限空间插件插拔作业的机械臂自主运动规划方法与系统

本发明涉及机械臂运动规划,特别涉及一种面向配电柜受限空间插件插拔作业的机械臂自主运动规划方法与系统。

背景技术:

1、配电柜内部通常设置有断路器、继电器、端子排、线束以及各类可插拔功能插件。插件检修、更换和测试过程中,机械臂需要依次完成目标插件识别、受限空间接近、夹爪对准、插件插入或拔出以及安全撤离。由于柜内空间狭窄、器件密集、遮挡明显,机械臂运动过程中容易与柜体、相邻电气器件或者线束发生碰撞;同时,插件插拔具有明确的末端位置、姿态和插拔方向约束,微小偏差可能造成插件卡滞、插接失败或者器件损伤。

2、目前,面向配电柜插件插拔作业的机械臂控制方法主要包括人工示教轨迹复现、基于精确几何模型的轨迹规划以及基于视觉伺服或模仿学习的控制方法。人工示教轨迹复现方法对插件位置和柜内布局变化适应性较弱;基于几何模型的轨迹规划方法依赖精确的柜体、插件、插槽和障碍物模型,在遮挡、安装误差和线束扰动条件下建模成本较高;普通视觉伺服方法需要针对不同插件逐项设计控制规则。

3、基础动作分块模仿学习方法能够根据当前观测一次生成未来连续动作,但直接用于配电柜插件插拔任务时仍存在三方面问题:第一,普通卷积视觉特征对插件边缘、插槽轮廓、末端执行器和邻近障碍物之间的局部空间关系表达不足;第二,动作块内所有时间步等权训练,不能突出即将执行的对准、接触和插拔动作;第三,在线执行时旧动作块预测持续参与融合,在插件位置、末端状态或遮挡关系发生变化时可能产生响应滞后。

技术实现思路

1、本发明提供了一种面向配电柜受限空间插件插拔作业的机械臂自主运动规划方法与系统,以解决背景技术所提到的技术问题。

2、为达到上述目的,本发明采用如下技术方案:

3、本发明提供了一种面向配电柜受限空间插件插拔作业的机械臂自主运动规划方法,包括如下步骤:

4、s1、采集配电柜插件插拔作业环境的多视角图像、机器人本体状态,依据机器人本体状态构建机器人本体状态向量,获取插件插入和插件拔出的专家示教动作;

5、s2、对多视角图像、机器人本体状态向量以及专家示教动作进行预处理,然后构建专家动作块;

6、s3、基于预处理后的多视角图像分别生成对应相机视角的视觉特征,然后将多个相机视角的视觉特征输入动作策略网络,得到融合观测特征;

7、s4、动作策略网络基于潜变量,并以融合观测特征为条件预测未来动作块,然后基于未来动作块计算总体训练损失;依据总体训练损失对动作策略网络进行动态调整,直至完成动作策略网络的训练;

8、s5、使用训练后的动作策略网络进行机械臂自主运动规划,生成多个候选动作;对所有候选动作进行加权融合,得到当前执行动作;

9、s6、将当前执行动作发送至机械臂控制器并控制机械臂执行,执行完成后,返回s5,进入下一个控制周期,直至完成机械臂自主运动规划任务。

10、进一步地,所述s1具体包括如下步骤:

11、s11、在柜体设定位置安装若干环境相机,获取配电柜整体布局图像以及机械臂相对柜体位置关系的图像;

12、s12、在机械臂末端安装若干腕部相机,获取插件把手、插槽边缘、夹爪和邻近器件的局部图像;

13、s13、同步采集机器人本体状态,包括机械臂关节位置、关节速度、末端位姿和夹爪状态,依据机器人本体状态构建机器人本体状态向量;然后利用s11和s12获取的图像以及机器人本体状态向量构建

t时刻的完整观测集;

14、;

15、其中,表示

t时刻第

m个相机视角的图像;

m代表环境相机和腕部相机的总和;

16、s14、获取插件插入和插件拔出的专家示教动作。

17、进一步地,所述s2具体包括如下步骤:

18、s21、按照统一时间戳对多视角图像、机器人本体状态向量和专家示教动作进行对齐;

19、s22、分别求解对齐后的机器人本体状态向量和专家示教动作的均值、标准差,利用对齐后的机器人本体状态向量的均值、标准差对对齐后的机器人本体状态向量进行标准化;利用专家示教动作的均值、标准差对专家示教动作进行标准化;标准化所使用的公式为:

20、;

21、其中,表示标准化后的机器人本体状态向量或者专家示教动作内的第个变量;表示防止分母为零的微小正数;表示均值;表示标准差;

22、至此,完成多视角图像、机器人本体状态向量以及专家示教动作的预处理;

23、s23、以当前时刻

t为起点,从标准化后的专家示教动作中截取长度为

t的未来连续动作,构成专家动作块。

24、进一步地,所述s3具体包括如下步骤:

25、s31、将预处理后的第

m个相机视角的图像重采样至能够被视觉编码器dinov2的图像块尺寸整除的分辨率,并输入视觉编码器dinov2中,输出图像块特征序列;

26、s32、将图像块特征序列按照图像块原始位置还原为二维特征网格;

27、s33、利用1×1卷积将二维特征网格的特征维度c映射为策略网络隐层维度d

,得到第m个相机视角的视觉特征;

28、s34、循环s31至s33,直至得到

m个相机视角的视觉特征;

29、s35、将

m个相机视角的视觉特征按照视角顺序拼接,并与预处理后的机器人本体状态向量的编码特征共同输入动作策略网络,形成融合观测特征。

30、进一步地,所述二维特征网格的表达式为:

31、;

32、其中,表示第

m个相机视角图像经视觉编码器dinov2输出的图像块特征序列;表示由图像块特征序列按照图像块在原图中的空间排列关系还原得到的二维特征网格;reshape表示按照图像块原始空间顺序进行二维网格还原;表示实数集;

b表示批量大小;

c表示每个图像块特征的特征维度;

n表示图像块数量,且;表示二维特征网格的高度,即二维特征网格的行数;表示二维特征网格的宽度,即图像块网格的列数;

m表示相机视角编号,

m=1,2,…,m;

33、所述视觉特征的表达式为:

34、;

35、其中,表示1×1卷积;

36、所述融合观测特征的表达式为:

37、;

38、其中,transformer表示动作策略网络中基于transformer模型的特征融合模块,用于对拼接后的多相机视角视觉特征和机器人本体状态编码特征进行编码融合;表示机器人本体状态编码器,表示特征序列拼接;表示第

m个相机视角的视觉特征。

39、进一步地,所述s4具体包括如下步骤:

40、s41、潜变量编码器根据专家动作块及其对应的融合观测特征估计潜变量的后验分布,在先验分布的约束下,利用后验分布获取潜变量;将潜变量与融合观测特征共同输入动作块解码器,生成未来动作块;

41、s42、依据后验分布、先验分布计算散度损失;

42、s43、针对未来动作块内的每个未来动作,分别计算归一化时间距离;

43、s44、根据每个未来动作的归一化时间距离分别计算每个未来动作的未归一化指数衰减权重,并进一步得到归一化权重;

44、s45、根据每个未来动作的归一化权重计算指数衰减动作重构损失;

45、s46、将散度损失与指数衰减动作重构损失结合,得到总体训练损失,判断总体训练损失是否在设定损失范围内,如果是,则输出得到训练后的动作策略网络,并进入s5中;否则依据总体训练损失对动作策略网络进行动态调整;

46、s47、返回s3中,继续对动作策略网络进行训练。

47、进一步地,所述未来动作块的表达式为:

48、;

49、其中,表示可训练参数为的动作策略网络中的动作块解码器,表示所述动作块解码器的可训练参数,表示动作策略网络根据时刻

t的观测预测的第

t-1个未来动作;

50、所述散度损失的表达式为:

51、;

52、其中,表示相对熵散度运算,用于度量潜变量的后验分布与先验分布之间的差异;表示潜变量编码器的可训练参数;

53、s43中第

i个未来动作的归一化时间距离的表达式为:

54、;

55、其中,表示第

i个未来动作的归一化时间距离;

56、s44中第

i个未来动作的未归一化指数衰减权重的表达式为:

57、;

58、其中,表示第

i个未来动作的未归一化指数衰减权重,表示自然指数函数;为动作块训练权重衰减系数;

59、s44中第

i个未来动作的归一化权重的表达式为:

60、;

61、其中,表示第

i个未来动作的归一化权重,表示求和下标,= 0,1,2,…,

t-1;表示第个未来动作的未归一化指数衰减权重;归一化权重在动作块训练权重衰减系数λ的不同取值下所有未来动作的归一化权重之和为1,表达式为:

62、;

63、指数衰减动作重构损失的表达式为:

64、;

65、其中,表示动作策略网络根据时刻

t的观测预测的第

i个未来动作;表示专家动作块中第

t+i个未来连续动作;表示预测的未来动作与专家动作块中对应未来连续动作在各控制维度上的绝对误差。

66、进一步地,所述s5具体包括如下步骤:

67、s51、使用训练后的动作策略网络进行机械臂自主运动规划,生成多个候选动作;

68、s52、然后针对每个候选动作,分别构造未归一化时间邻近权重,其中第个候选动作的未归一化时间邻近权重的表达式如下:

69、;

70、其中,表示第个候选动作的未归一化时间邻近权重,表示时间邻近集成系数;表示第个候选动作;

j=0,1,…,

k-1;

k表示候选动作的总数;

71、s53、依据每个候选动作的未归一化时间邻近权重,构造对应的归一化时间邻近权重,其中第个候选动作的归一化时间邻近权重的表达式如下:

72、;

73、其中,表示第个候选动作的归一化时间邻近权重;

n表示求和下标,

n= 0,1,2,…,

k-1;表示第

n个候选动作的未归一化指数衰减权重;所有候选动作的归一化时间邻近权重之和为1,表达式为:

74、;

75、s54、对所有候选动作进行加权融合,得到发送至机械臂控制器的当前执行动作,表达式如下:

76、;

77、其中,表示第个候选动作。

78、进一步地,所述s6具体包括如下步骤:

79、s61、将当前执行动作发送至机械臂控制器,控制机械臂执行对应的关节运动、末端位姿调整和夹爪控制;

80、s62、执行完成后,进入下一个控制周期,重新采集当前完整观测集,并返回s5中,形成持续感知、动作块预测、候选动作融合和动作执行的闭环运动规划过程,直至完成机械臂自主运动规划任务。

81、本发明另一方面还提供了一种面向配电柜受限空间插件插拔作业的机械臂自主运动规划系统,被配置为执行一种面向配电柜受限空间插件插拔作业的机械臂自主运动规划方法。

82、本发明的有益效果:

83、本发明公开了一种面向配电柜受限空间插件插拔作业的机械臂自主运动规划方法,在保留候选动作生成能力的基础上,提高了配电柜内局部视觉表征能力、插拔关键阶段的短时动作精度以及根据最新观测进行闭环纠偏的能力以及闭环稳定性,同时也进一步提高了动作策略网络的预测精度。