一种物体质心测量方法及相关设备与流程

本技术涉及机器人控制，具体而言，涉及一种物体质心测量方法及相关设备。

背景技术：

1、随着科学技术的不断发展，机器人技术因具有极大的研究价值及应用价值受到了各行各业的广泛重视，目前通常需要利用机器人的机械臂末端携带着物体执行各式各样的作业任务(例如，物体搬运任务、物体投掷任务、物体抛体运动等)。而对机器人来说，物体质心位置的准确性是影响机器人任务执行轨迹的轨迹有效性、设计合理性和轨迹准确度的重要因素。因此，如何准确测量被操作物体的质心位置，便是当今机器人任务执行过程中的一项亟需解决的重要问题。

技术实现思路

1、有鉴于此，本技术的目的在于提供一种物体质心测量方法及装置、机器人控制设备和可读存储介质，能够根据多个机械臂针对待测物体协同执行物体位姿变化运动后的机械臂运动参数进行物体质心位置估计，并持续提高物体质心位置估计精度直到最大精度上限，使最终估计出的物体质心位置与待测物体的真实质心位置尽可能地保持一致，以提高物体质心位置的辨识精度和辨识准确性，同时保证本技术提供的物体质心测量方案具备较强的方案通用性，可适用于不同体积和/或质量的待测物体，来针对不同体积和/或质量的待测物体实现质心位置准确测量效果。

2、为了实现上述目的，本技术实施例采用的技术方案如下：

3、第一方面，本技术提供一种物体质心测量方法，所述方法包括：

4、控制多臂机器人通过多个机械臂携带着待测物体进行物体位姿变化运动；

5、获取所述多个机械臂的机械臂末端各自在本次物体位姿变化运动后的实际末端位姿数据和实际末端力参数；

6、根据所述多个机械臂各自对应的实际末端位姿数据和实际末端力参数，基于所述多个机械臂的机械臂末端在所述待测物体上的力矩平衡关系进行物体质心位置计算，得到所述待测物体的当前质心疑似位置；

7、检测所述当前质心疑似位置与本次物体位姿变化运动前最近计算出的历史质心疑似位置之间的质心位置距离是否小于预设距离阈值；

8、若检测到所述质心位置距离小于所述预设距离阈值，则将所述当前质心疑似位置作为所述待测物体的实际质心位置，否则跳转到所述控制多臂机器人通过多个机械臂携带着待测物体进行物体位姿变化运动的步骤继续执行。

9、在可选的实施方式中，每个机械臂所对应的实际末端位姿数据包括对应机械臂的机械臂末端坐标系在世界坐标系下的实际位置和实际姿态，每个机械臂所对应的实际末端力参数包括对应机械臂在机械臂末端坐标系下测量出的实际力向量和实际力矩向量，则所述根据所述多个机械臂各自对应的实际末端位姿数据和实际末端力参数，基于所述多个机械臂的机械臂末端在所述待测物体上的力矩平衡关系进行物体质心位置计算，得到所述待测物体的当前质心疑似位置的步骤，包括：

10、将所述多个机械臂各自的实际姿态映射到参考机械臂的机械臂末端坐标系下，得到所述多个机械臂各自在所述参考机械臂的机械臂末端坐标系下的第一相对姿态，其中所述参考机械臂为所述多个机械臂中的任意一个机械臂；

11、将所述多个机械臂中的每个机械臂依次作为目标机械臂，将除所述目标机械臂以外的所有机械臂各自的实际位置和实际姿态映射到所述目标机械臂的机械臂末端坐标系下，得到除所述目标机械臂以外的所有机械臂各自在所述目标机械臂的机械臂末端坐标系下的相对位置和第二相对姿态；

12、根据所述多个机械臂各自对应的第一相对姿态、实际力向量和实际力矩向量，以及除所述目标机械臂以外的所有机械臂各自对应的相对位置和第二相对姿态，针对所述待测物体在所述参考机械臂的机械臂末端坐标系下的质心位置构建与所述目标机械臂对应且满足所述力矩平衡关系的质心位置求解方程；

13、对所述多个机械臂各自对应的质心位置求解方程进行联立求解，得到所述待测物体在所述参考机械臂的机械臂末端坐标系下的当前质心疑似位置。

14、在可选的实施方式中，当所述多个机械臂中的第j个机械臂作为目标机械臂时，与第j个机械臂对应的质心位置求解方程采用如下式子进行表示：

15、

16、其中，m用于表示所述多个机械臂的机械臂总数目，n用于表示所述多个机械臂中的参考机械臂，mj用于表示第j个机械臂在对应机械臂末端坐标系下的实际力矩向量，rji用于表示第i个机械臂在第j个机械臂的机械臂末端坐标系下的第二相对姿态，mi用于表示第i个机械臂在对应机械臂末端坐标系下的实际力矩向量，pji用于表示第i个机械臂在第j个机械臂的机械臂末端坐标系下的相对位置，fi用于表示第i个机械臂在对应机械臂末端坐标系下的实际力向量，fgj用于表示所述待测物体在第j个机械臂的机械臂末端坐标系下的物体重力向量，rni用于表示第i个机械臂在所述参考机械臂的机械臂末端坐标系下的第一相对姿态，png用于表示所述待测物体在所述参考机械臂的机械臂末端坐标系下的质心位置。

17、在可选的实施方式中，在所述根据所述多个机械臂各自对应的实际末端位姿数据和实际末端力参数，基于所述多个机械臂的机械臂末端在所述待测物体上的力矩平衡关系进行物体质心位置计算，得到所述待测物体的当前质心疑似位置的步骤之前，所述方法还包括：

18、检测本次物体位姿变化运动的运行执行次序是否小于预设次序阈值；

19、若检测到所述运行执行次序小于所述预设次序阈值，则继续执行所述根据所述多个机械臂各自对应的实际末端位姿数据和实际末端力参数，基于所述多个机械臂的机械臂末端在所述待测物体上的力矩平衡关系进行物体质心位置计算，得到所述待测物体的当前质心疑似位置的步骤，否则根据所述历史质心疑似位置及所述多个机械臂各自的实际末端位姿数据，检测所述多个机械臂各自的实际末端力参数是否存在异常；

20、在检测到至少一个机械臂的实际末端力参数存在异常的情况下，对所述多个机械臂各自的实际末端位姿数据和实际末端力参数进行删除，并跳转到所述控制多臂机器人通过多个机械臂携带着待测物体进行物体位姿变化运动的步骤继续执行；

21、在检测到所述多个机械臂各自的实际末端力参数均不存在异常的情况下，继续执行所述根据所述多个机械臂各自对应的实际末端位姿数据和实际末端力参数，基于所述多个机械臂的机械臂末端在所述待测物体上的力矩平衡关系进行物体质心位置计算，得到所述待测物体的当前质心疑似位置的步骤。

22、在可选的实施方式中，所述根据所述历史质心疑似位置及所述多个机械臂各自的实际末端位姿数据，检测所述多个机械臂各自的实际末端力参数是否存在异常的步骤，包括：

23、根据所述历史质心疑似位置、所述多个机械臂各自的实际末端位姿数据，以及所述多个机械臂各自的实际末端力参数所包括的实际力向量，基于所述力矩平衡关系计算所述多个机械臂各自的机械臂末端的理论力矩向量；

24、针对每个机械臂，计算该机械臂的实际末端力参数所包括的实际力矩向量与对应理论力矩向量之间的力矩差异度；

25、根据该机械臂所对应的力矩差异度，检测该机械臂的实际末端力参数是否存在异常。

26、在可选的实施方式中，所述根据该机械臂所对应的力矩差异度，检测该机械臂的实际末端力参数是否存在异常的步骤，包括：

27、将该机械臂所对应的力矩差异度与该机械臂所对应的预设差异度阈值进行比较；

28、若所述力矩差异度小于所述预设差异度阈值，则判定该机械臂的实际末端力参数不存在异常，否则判定该机械臂的实际末端力参数存在异常。

29、第二方面，本技术提供一种物体质心测量装置，所述装置包括：

30、物体运动控制模块，用于控制多臂机器人通过多个机械臂携带着待测物体进行物体位姿变化运动；

31、末端数据获取模块，用于获取所述多个机械臂的机械臂末端各自在本次物体位姿变化运动后的实际末端位姿数据和实际末端力参数；

32、物体质心计算模块，用于根据所述多个机械臂各自对应的实际末端位姿数据和实际末端力参数，基于所述多个机械臂的机械臂末端在所述待测物体上的力矩平衡关系进行物体质心位置计算，得到所述待测物体的当前质心疑似位置；

33、质心位置验证模块，用于检测所述当前质心疑似位置与本次物体位姿变化运动前最近计算出的历史质心疑似位置之间的质心位置距离是否小于预设距离阈值；

34、质心位置输出模块，用于若检测到所述质心位置距离小于所述预设距离阈值，则将所述当前质心疑似位置作为所述待测物体的实际质心位置，否则驱动所述物体运动控制模块继续控制多臂机器人通过多个机械臂携带着待测物体进行物体位姿变化运动。

35、在可选的实施方式中，所述装置还包括：

36、运动次序检测模块，用于检测本次物体位姿变化运动的运行执行次序是否小于预设次序阈值；

37、所述运动次序检测模块，还用于在检测到所述运行执行次序小于所述预设次序阈值时，驱动所述物体质心计算模块根据所述多个机械臂各自对应的实际末端位姿数据和实际末端力参数，基于所述多个机械臂的机械臂末端在所述待测物体上的力矩平衡关系进行物体质心位置计算，得到所述待测物体的当前质心疑似位置；

38、数据异常检测模块，用于在所述运动次序检测模块检测到所述运行执行次序大于或等于所述预设次序阈值时，根据所述历史质心疑似位置及所述多个机械臂各自的实际末端位姿数据，检测所述多个机械臂各自的实际末端力参数是否存在异常；

39、异常数据删除模块，用于在所述数据异常检测模块检测到至少一个机械臂的实际末端力参数存在异常时，对所述多个机械臂各自的实际末端位姿数据和实际末端力参数进行删除，并驱动所述物体运动控制模块控制多臂机器人通过多个机械臂携带着待测物体进行物体位姿变化运动；

40、所述数据异常检测模块，还用于在检测到所述多个机械臂各自的实际末端力参数均不存在异常的情况下，驱动所述物体质心计算模块根据所述多个机械臂各自对应的实际末端位姿数据和实际末端力参数，基于所述多个机械臂的机械臂末端在所述待测物体上的力矩平衡关系进行物体质心位置计算，得到所述待测物体的当前质心疑似位置。

41、第三方面，本技术提供一种机器人控制设备，包括处理器和存储器，所述存储器存储有可被所述处理器执行的计算机程序，所述处理器可执行所述计算机程序，以实现前述实施方式中任意一项所述的物体质心测量方法。

42、第四方面，本技术提供一种可读存储介质，其上存储有计算机程序，所述计算机程序被运行时，实现前述实施方式中任意一项所述的物体质心测量方法。

43、在此情况下，本技术实施例的有益效果可以包括以下内容：

44、本技术通过控制多臂机器人利用多个机械臂携带着待测物体进行物体位姿变化运动，并获取多个机械臂的机械臂末端各自在本次物体位姿变化运动后的实际末端位姿数据和实际末端力参数，基于多个机械臂的机械臂末端在待测物体上的力矩平衡关系进行物体质心位置计算，得到待测物体的当前质心疑似位置，而后在当前质心疑似位置与本次物体位姿变化运动前最近计算出的历史质心疑似位置之间的质心位置距离不小于预设距离阈值的情况下，跳转到控制多臂机器人通过多个机械臂携带着待测物体进行物体位姿变化运动的步骤继续执行，直至对应质心位置距离小于预设距离阈值为止，从而在多个机械臂协同地针对待测物体进行移动的过程中有效估计物体质心位置，并持续提高物体质心位置估计精度直到最大精度上限，使最终估计出的物体质心位置与待测物体的真实质心位置尽可能地保持一致，以提高物体质心位置的辨识精度和辨识准确性，同时保证本技术提供的物体质心测量方案具备较强的方案通用性，可适用于不同体积和/或质量的待测物体，来针对不同体积和/或质量的待测物体实现质心位置准确测量效果。

45、为使本技术的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。