一种基于扩散模型的凿岩台车钻臂逆解控制方法及装置

本发明涉及机械臂控制，特别是指一种基于扩散模型的凿岩台车钻臂逆解控制方法及装置。

背景技术：

1、凿岩台车是地下矿山和隧道钻爆法施工的核心装备，其主要功能是通过钻臂来定位凿岩工具和进行钻孔作业。钻孔作业前，控制系统通过调整钻臂各关节的位置或角度，使其末端执行器（钻头）与巷道或隧道掌子面上预设炮孔位姿对齐，即寻孔控制过程。逆运动学求解是实现钻臂精确快速寻孔控制的核心，其任务旨在根据目标位姿信息计算各关节的角度和位置，为钻臂液压执行器提供控制参考目标。由于作业面钻孔作业的复杂需求，钻臂必须具有多自由度特性（通常具有八个以上自由度），进而导致其逆运动学求解较为繁琐。因此，求解出合适的关节姿态在降低企业生产成本、减少生产安全事故以及提高生产效率等方面有着巨大的潜力。目前，机械臂运动学求逆的主要方法包括解析法、数值法、启发式算法和学习法。其中，解析法适用于关节之间满足特定结构的机械臂，通过构造逆运动学的解析方程进行求解，该方法的计算效率高，但通用性差。数值法采用迭代计算的方式逐步逼近目标位置和姿态，该方法适用范围广，但求解效率相对较低。启发式算法依赖于经验、启发式信息，通过模拟一些生物或物理现象来引导搜索最优解。该方法构造简单、灵活高效，但性能高度依赖参数设置，且较难找到全局最优解。学习法根据样本信息和反馈信息调节神经网络中的权值参数，构建映射关系，在输入冗余机械臂末端执行器位姿后，通过神经网络自动映射出对应关节的数值，实现逆运动学求解。该方法求解精度高、速度快，但依赖于数据集支撑，存在网络输入信息与实际作业信息不匹配的问题。

2、近年来随着扩散模型技术的不断发展，借助其产生的扩散策略在图像生成、最优控制等领域均得到广泛应用，这也为解决冗余机构的逆运动学问题提供了新的研究方向。目前，在进行高自由度机械臂逆运动学求解时，仍然面临着无法高效获得多组解以及合理进行轨迹规划的情况，只是由给定的末端位姿输入对应单一的关节输出，可能忽略部分其他合适的关节位姿，进而影响台车寻孔的效率和精度。

技术实现思路

1、为了解决现有技术存在的如何提升台车钻臂逆运动学求解的准确性和运动过程的平稳性的技术问题，本发明实施例提供了一种基于扩散模型的凿岩台车钻臂逆解控制方法及装置。所述技术方案如下：

2、一方面，提供了一种基于扩散模型的凿岩台车钻臂逆解控制方法，该方法由凿岩台车钻臂逆解控制设备实现，该方法包括：

3、s1、获取待控制的凿岩台车钻臂的目标孔位数据。

4、s2、将目标孔位数据输入至训练好的凿岩台车钻臂逆解控制模型。

5、s3、根据目标孔位数据以及凿岩台车钻臂逆解控制模型，得到钻臂的各个关节变量。

6、s4、根据各个关节变量，得到轨迹规划结果，完成凿岩台车钻臂控制。

7、可选地，s2中的凿岩台车钻臂逆解控制模型的构建过程，包括：

8、s21、获取凿岩台车钻臂的参数数据，根据参数数据以及dh矩阵方法，得到钻臂的末端执行器的姿态和位置。

9、s22、根据钻臂的末端执行器的姿态和位置，构建扩散模型输入。

10、其中，扩散模型输入包括孔位信息输入以及标准高斯分布噪声输入。

11、s23、根据扩散模型输入，搭建扩散模型网络。

12、s24、根据扩散模型网络，构建寻孔误差函数。

13、s25、根据寻孔误差函数，更新扩散模型网络参数，直至扩散模型网络收敛，得到训练好的凿岩台车钻臂逆解控制模型。

14、可选地，s21中的获取凿岩台车钻臂的参数数据，根据参数数据以及dh矩阵方法，得到钻臂的末端执行器的姿态和位置，包括：

15、s211、获取凿岩台车钻臂的参数数据，根据参数数据以及dh矩阵方法，对钻臂的各个连杆建立dh坐标系，进而得到钻臂的dh参数信息。

16、s212、根据dh坐标系以及dh参数信息，得到各个连杆的齐次变换矩阵，如下式（1）所示：

17、（1）

18、式中：表示各个连杆的齐次变换矩阵，表示钻臂的连杆数量，表示各个连杆绕轴得到的旋转矩阵，表示关节扭角，表示各个连杆绕轴得到的旋转矩阵，表示关节角，表示各个连杆沿轴得到的平移矩阵，表示连杆长度，表示各个连杆沿轴得到的平移矩阵，表示连杆偏距。

19、s213、建立钻臂的末端执行器相对于基坐标系的转换方式。

20、s214、根据齐次变换矩阵以及转换方式，得到末端执行器的姿态的变换矩阵，根据变换矩阵得到末端执行器的姿态和位置。

21、其中，末端执行器的姿态的变换矩阵，如下式（2）所示：

22、（2）

23、式中：表示末端执行器的姿态的变换矩阵，表示钻臂的末端执行器相对于基坐标系的旋转矩阵，用于获得钻头相对于基坐标系的方向向量，表示轴，表示轴，表示轴，表示钻臂的末端执行器相对于基坐标系的位置，表示末端执行器在dh坐标系上的轴坐标，表示末端执行器在dh坐标系上的轴坐标，表示末端执行器在dh坐标系上的轴坐标。

24、可选地，s22中的孔位信息输入，如下式（3）所示：

25、（3）

26、式中：表示钻孔的起始点坐标，表示矩阵的转置，表示钻臂的末端执行器相对于基坐标系的位置，表示钻孔的终点坐标，表示钻臂的末端执行器相对于基坐标系的旋转矩阵，表示轴，表示轴，表示轴，表示孔的深度，表示孔位信息输入。

27、可选地，s23中的扩散模型网络，如下式（4）所示：

28、（4）

29、式中：表示标准高斯分布噪声输入，表示时间序列，，表示时间相关信息，表示从1到的连乘，表示待学习的策略，表示孔位信息输入，，表示标准正态分布，表示方差为1。

30、可选地，s24中的寻孔误差函数，如下式（5）所示：

31、（5）

32、式中：表示寻孔误差函数，表示每一批量误差的数学期望，表示孔位信息输入，表示正运动学运算，表示模型输出，表示关节角。

33、可选地，s4中的根据各个关节变量，得到轨迹规划结果，完成凿岩台车钻臂控制，包括：

34、对各个关节变量进行五次多项式插值，根据插值后的各个关节变量以及设定好的各个关节的运动时间，得到轨迹规划结果，完成凿岩台车钻臂控制。

35、另一方面，提供了一种基于扩散模型的凿岩台车钻臂逆解控制装置，该装置应用于基于扩散模型的凿岩台车钻臂逆解控制方法，该装置包括：

36、获取模块，用于获取待控制的凿岩台车钻臂的目标孔位数据。

37、输入模块，用于将目标孔位数据输入至训练好的凿岩台车钻臂逆解控制模型。

38、模型输出模块，用于根据目标孔位数据以及凿岩台车钻臂逆解控制模型，得到钻臂的各个关节变量。

39、输出模块，用于根据各个关节变量，得到轨迹规划结果，完成凿岩台车钻臂控制。

40、可选地，输入模块，进一步用于：

41、s21、获取凿岩台车钻臂的参数数据，根据参数数据以及dh矩阵方法，得到钻臂的末端执行器的姿态和位置。

42、s22、根据钻臂的末端执行器的姿态和位置，构建扩散模型输入。

43、其中，扩散模型输入包括孔位信息输入以及标准高斯分布噪声输入。

44、s23、根据扩散模型输入，搭建扩散模型网络。

45、s24、根据扩散模型网络，构建寻孔误差函数。

46、s25、根据寻孔误差函数，更新扩散模型网络参数，直至扩散模型网络收敛，得到训练好的凿岩台车钻臂逆解控制模型。

47、可选地，输入模块，进一步用于：

48、s211、获取凿岩台车钻臂的参数数据，根据参数数据以及dh矩阵方法，对钻臂的各个连杆建立dh坐标系，进而得到钻臂的dh参数信息。

49、s212、根据dh坐标系以及dh参数信息，得到各个连杆的齐次变换矩阵，如下式（1）所示：

50、（1）

51、式中：表示各个连杆的齐次变换矩阵，表示钻臂的连杆数量，表示各个连杆绕轴得到的旋转矩阵，表示关节扭角，表示各个连杆绕轴得到的旋转矩阵，表示关节角，表示各个连杆沿轴得到的平移矩阵，表示连杆长度，表示各个连杆沿轴得到的平移矩阵，表示连杆偏距。

52、s213、建立钻臂的末端执行器相对于基坐标系的转换方式。

53、s214、根据齐次变换矩阵以及转换方式，得到末端执行器的姿态的变换矩阵，根据变换矩阵得到末端执行器的姿态和位置。

54、其中，末端执行器的姿态的变换矩阵，如下式（2）所示：

55、（2）

56、式中：表示末端执行器的姿态的变换矩阵，表示钻臂的末端执行器相对于基坐标系的旋转矩阵，用于获得钻头相对于基坐标系的方向向量，表示轴，表示轴，表示轴，表示钻臂的末端执行器相对于基坐标系的位置，表示末端执行器在dh坐标系上的轴坐标，表示末端执行器在dh坐标系上的轴坐标，表示末端执行器在dh坐标系上的轴坐标。

57、可选地，孔位信息输入，如下式（3）所示：

58、（3）

59、式中：表示钻孔的起始点坐标，表示矩阵的转置，表示钻臂的末端执行器相对于基坐标系的位置，表示钻孔的终点坐标，表示钻臂的末端执行器相对于基坐标系的旋转矩阵，表示轴，表示轴，表示轴，表示孔的深度，表示孔位信息输入。

60、可选地，扩散模型网络，如下式（4）所示：

61、（4）

62、式中：表示标准高斯分布噪声输入，表示时间序列，，表示时间相关信息，表示从1到的连乘，表示待学习的策略，表示孔位信息输入，，表示标准正态分布，表示方差为1。

63、可选地，寻孔误差函数，如下式（5）所示：

64、（5）

65、式中：表示寻孔误差函数，表示每一批量误差的数学期望，表示孔位信息输入，表示正运动学运算，表示模型输出，表示关节角。

66、可选地，输出模块，进一步用于：

67、对各个关节变量进行五次多项式插值，根据插值后的各个关节变量以及设定好的各个关节的运动时间，得到轨迹规划结果，完成凿岩台车钻臂控制。

68、另一方面，提供一种凿岩台车钻臂逆解控制设备，所述凿岩台车钻臂逆解控制设备包括：处理器；存储器，所述存储器上存储有计算机可读指令，所述计算机可读指令被所述处理器执行时，实现如上述基于扩散模型的凿岩台车钻臂逆解控制方法中的任一项方法。

69、另一方面，提供了一种计算机可读存储介质，所述存储介质中存储有至少一条指令，所述至少一条指令由处理器加载并执行以实现上述基于扩散模型的凿岩台车钻臂逆解控制方法中的任一项方法。

70、本发明实施例提供的技术方案带来的有益效果至少包括：

71、本发明实施例中，建立了与实际巷道一致的坐标信息作为扩散模型的数据集，区别于传统神经网络模型以关节变量正运动学获得的数据，数据更为真实，且构造简单；本发明设计了扩散模型的结构，以已知目标炮孔的信息去鼓励标准正态分布的噪声还原为关节量，实现数据样本的重建；本发明构造了扩散模型的逆运动学求解方案，以目标孔位和逆运动学孔位差值构造优化目标驱动网络更新，大幅提高了逆运动学求解精度；本发明构造了各关节的轨迹规划曲线，在角位移、角速度和角加速度方面均保证连续无突变的情况，实现了运动的平顺性和光滑性。