一种双臂协同作业规划方法、装置、设备及介质

本发明涉及机械臂运动规划，具体而言，涉及一种双臂协同作业规划方法、装置、设备及介质。

背景技术：

1、水下机器人双机械臂系统 (underwater vehicle dual manipulator system,uvdms) 是一种专为水下环境设计的机器人系统，它配备有两个机械臂，能够执行复杂的水下任务。uvdms广泛应用于海洋科学、水下工程、资源勘探、水下结构检测与维护等领域。

2、为了对uvdms的两个机械臂进行运动规划，现有技术主要采用约束条件和规划算法进行路径规划，但是现有技术规划的路径并未考虑两个机械臂之间的相对运动可能造成的碰撞问题，得到的规划路径并不合理，碰撞风险高。

技术实现思路

1、本发明解决的问题是如何降低uvdms两个机械臂规划路径的碰撞风险。

2、为解决上述问题，第一方面，本发明提供了一种双臂协同作业规划方法，包括：

3、对uvdms三维模型进行水动力分析，生成水动力约束条件；

4、根据d-h参数法，获取第一机械臂的起始位置和目标位置以及第二机械臂的起始位置和目标位置；

5、基于所述水动力约束条件，根据所述第一机械臂的起始位置和目标位置以及所述第二机械臂的起始位置和所述目标位置，分别对所述第一机械臂和所述第二机械臂进行路径规划，生成第一规划路径和第二规划路径；

6、离散所述第一规划路径和所述第二规划路径，生成至少一个离散路径点区间；

7、采用包围盒简化所述uvdms三维模型，生成双臂简化模型；

8、基于所述双臂简化模型和所述离散路径点区间，提取所述第一机械臂和所述第二机械臂之间的最小距离；

9、根据所述最小距离和预设最小阈值，确定碰撞情况，并根据所述碰撞情况、所述第一规划路径和所述第二规划路径，生成目标规划路径。

10、可选地，所述基于所述双臂简化模型和所述离散路径点区间，提取所述第一机械臂和所述第二机械臂之间的最小距离，包括：

11、根据所述双臂简化模型，确定所述第一机械臂和所述第二机械臂在每个所述离散路径点区间的空间位姿数据；

12、根据所述空间位姿数据，确定所述第一机械臂和所述第二机械臂的公垂线；

13、根据所述公垂线的端点位置，确定所述最小距离。

14、可选地，所述根据所述公垂线的端点位置，确定所述最小距离，包括：

15、当所述公垂线的端点均在所述第一机械臂和所述第二机械臂上时，根据第一距离公式组，确定所述最小距离，所述第一距离公式组包括：

16、；

17、；

18、其中， dmin为所述最小距离， pk1为所述公垂线在所述第一机械臂上的端点， qk2为所述公垂线在所述第二机械臂上的端点，表示为 pk1和 qk2的距离， ps为所述第一机械臂的第一端点， qs为所述第二机械臂的第一端点， k1为第一比例系数， k2为第二比例系数，且 p= pe- ps， q= qe- qs， qe为所述第二机械臂的第二端点， pe为所述第一机械臂的第二端点；

19、当所述公垂线的端点在所述第一机械臂或所述第二机械臂上时，根据第二距离公式组，确定所述最小距离，所述第二距离公式组包括：

20、；

21、；

22、其中， d1为第一距离， d2为第二距离， d3为第三距离， d4为第四距离， d5为第五距离， d6为第六距离；

23、当所述公垂线的端点均不在所述第一机械臂和所述第二机械臂上时，根据第三距离公式组，确定所述最小距离，所述第三距离公式组包括：

24、；

25、；

26、其中， dminq为第七距离， dminp为第八距离。

27、可选地，所述基于所述水动力约束条件，根据所述第一机械臂的起始位置和目标位置以及所述第二机械臂的起始位置和所述目标位置，分别对所述第一机械臂和所述第二机械臂进行路径规划，生成第一规划路径和第二规划路径，包括：

28、获取障碍物数据；

29、基于所述障碍物数据，根据所述第一机械臂的起始位置和目标位置，采用第一规划算法，对所述第一机械臂进行路径规划，生成所述第一规划路径；

30、基于所述障碍物数据和所述第一规划路径，根据所述第二机械臂的起始位置和目标位置，采用第二规划算法，对所述第二机械臂进行路径规划，生成所述第二规划路径；

31、当所述第一规划路径或所述第二规划路径不满足所述水动力约束条件时，重新进行路径规划，生成所述第一规划路径或所述第二规划路径。

32、可选地，所述采用第二规划算法，对所述第二机械臂进行路径规划，生成所述第二规划路径，包括：

33、根据所述第一规划路径和所述障碍物数据，确定障碍物点位；

34、根据所述障碍物点位，采用人工势场法，确定节点生长方向；

35、基于所述第二机械臂的起始位置和目标位置，根据所述节点生长方向，采用rrt算法和rrt*算法，生成所述第二规划路径。

36、可选地，所述基于所述第二机械臂的起始位置和目标位置，根据所述节点生长方向，采用rrt算法和rrt*算法，生成所述第二规划路径，包括：

37、基于所述第二机械臂的起始位置和目标位置，根据所述rrt算法，初始化树结构，并进行随机采样，确定候选父节点；

38、根据所述rrt*算法和所述候选父节点，确定所述树结构的父节点；

39、根据所述父节点和所述节点生长方向，确定新节点，并根据所述父节点和所述新节点，生成初步规划路径；

40、根据所述rrt*算法，校验所述初步规划路径，根据校验结果，生成所述第二规划路径。

41、可选地，所述对uvdms三维模型进行水动力分析，生成水动力约束条件，包括：

42、根据所述uvdms三维模型，采用水动力仿真软件，生成水动力受力数据；

43、根据所述水动力受力数据，确定无因次水动力系数；

44、根据所述无因次水动力系数，对所述uvdms三维模型进行cfd分析，生成所述水动力约束条件。

45、第二方面，本发明提供了一种双臂协同作业规划装置，包括：

46、分析模块，用于对uvdms三维模型进行水动力分析，生成水动力约束条件；

47、位置模块，用于根据d-h参数法，获取第一机械臂的起始位置和目标位置以及第二机械臂的起始位置和目标位置；

48、规划模块，用于基于所述水动力约束条件，根据所述第一机械臂的起始位置和目标位置以及所述第二机械臂的起始位置和所述目标位置，分别对所述第一机械臂和所述第二机械臂进行路径规划，生成第一规划路径和第二规划路径；

49、离散模块，用于离散所述第一规划路径和所述第二规划路径，生成至少一个离散路径点区间；

50、简化模块，用于采用包围盒简化所述uvdms三维模型，生成双臂简化模型；

51、距离模块，用于基于所述双臂简化模型和所述离散路径点区间，提取所述第一机械臂和所述第二机械臂之间的最小距离；

52、碰撞模块，用于根据所述最小距离和预设最小阈值，确定碰撞情况，并根据所述碰撞情况、所述第一规划路径和所述第二规划路径，生成目标规划路径。

53、第三方面，本发明提供了一种电子设备，包括存储器和处理器；

54、所述存储器，用于存储计算机程序；

55、所述处理器，用于当执行所述计算机程序时，实现如第一方面所述的双臂协同作业规划方法。

56、第四方面，本发明提供了一种计算机可读存储介质，所述存储介质上存储有计算机程序，当所述计算机程序被处理器执行时，实现如第一方面所述的双臂协同作业规划方法。

57、本发明的双臂协同作业规划方法、装置、设备及介质的有益效果是：

58、通过对uvdms的三维模型进行水动力分析，可以确定该模型在水中的受力情况，得到水动力约束条件，有助于后续路径规划时考虑了实际操作环境的影响，提高了规划路径的可行性和精确性；然后根据d-h参数法，获取第一机械臂的起始位置和目标位置以及第二机械臂的起始位置和目标位置，保证了位置数据的准确性和一致性，便于后续进行路径规划。再基于水动力约束条件，根据起始位置和目标位置，分别对第一机械臂和第二机械臂进行路径规划，生成第一规划路径和第二规划路径，得到第一机械臂和第二机械臂初步运动路径，然后将第一规划路径和所述第二规划路径进行离散为至少一个离散路径点区间，以便于在每个离散路径点区间内进行碰撞检测，并只对存在碰撞风险的离散路径点区间重新进行规划路径，从而降低碰撞风险，同时提高碰撞检测的精度，减少重新规划路径的成本；之后采用包围盒简化uvdms三维模型，即简化机械臂的几何形状，可以快速高效地进行碰撞检测，避免复杂计算的同时保证了检测的准确性。而后根据一机械臂和第二机械臂之间的最小距离和预设最小阈值，确定碰撞情况，并根据碰撞情况、第一规划路径和第二规划路径，生成最终的目标规划路径，解决了现有技术未考虑两个机械臂之间的相对运动可能造成的碰撞问题，减少了路径规划的不合理性，降低了两个机械臂的碰撞风险，保证了uvdms在复杂的水动力环境下安全、高效地完成预定任务。