一种机械臂全身动态避障方法和相关设备

本申请涉及机械臂，尤其涉及一种机械臂全身动态避障方法和相关设备。

背景技术：

1、机械臂在执行末端轨迹跟踪、抓取或装配任务的同时，不仅末端执行器需要避障，肩部、上臂、前臂、腕部等全部连杆也可能与移动人员、工具或其他机器人发生碰撞。

2、示例性技术中，机械臂在运动过程中，通过机械臂与障碍物之间的距离，确定机械臂的控制量，从而通过控制量控制机械臂运动使得机械臂躲避障碍物。

3、但此种方式，动态障碍物与静态障碍物是被机械臂同等对待，容易产生误差，导致机械臂可能会碰撞到障碍物，机械臂的避障效果较差。

技术实现思路

1、基于上述技术现状，本申请提供一种机械臂全身动态避障方法和相关设备，用于解决机械臂的避障效果较差的问题。

2、为了达到上述技术目的，本申请具体提出如下技术方案：

3、第一方面，本申请提供了一种机械臂全身动态避障方法，包括：

4、根据机械臂中各个关节部位的当前运动状态以及预设的约束条件，构建所述机械臂的最优控制问题函数，并确定所述机械臂中的胶囊最近的目标障碍物；

5、根据所述目标障碍物的速度参数以及距离参数，确定各个所述关节部位躲避所述目标障碍物所需的代价参数，并根据各个所述代价参数对所述最优控制问题函数进行求解，得到目标控制量；

6、根据所述目标控制量，控制各个所述关节部位进行运动。

7、在一些实施方式中，所述根据所述目标障碍物的速度参数以及距离参数，确定各个所述关节部位躲避所述目标障碍物所需的代价参数，包括：

8、根据所述目标障碍物的速度参数以及距离参数，确定所述目标障碍物对应的关节部位的软屏障代价参数以及方向性代价参数；

9、根据所述软屏障代价参数以及所述方向性代价参数，构建所述关节部位躲避对应的目标障碍物所需的代价参数。

10、在一些实施方式中，所述根据所述目标障碍物的速度参数以及距离参数，确定所述目标障碍物对应的关节部位的软屏障代价参数以及方向性代价参数，包括：

11、根据所述目标障碍物的速度参数以及距离参数，确定所述关节部位的控制屏障函数的离散余量参数以及有效安全距离，所述有效安全距离是所述关节部位与对应的所述目标障碍物之间的有效的安全距离；

12、根据所述有效安全距离确定目标距离度量，并根据所述目标有效安全距离以及目标距离度量，确定所述关节部位对应的胶囊的动态权重；

13、根据所述动态权重、所述目标距离度量、所述关节部位的切向速度比例以及有效安全距离，确定所述关节部位的软屏障代价参数。

14、在一些实施方式中，所述根据所述目标障碍物的速度参数以及距离参数，确定所述关节部位的方向性代价参数，包括：

15、根据所述目标障碍物的速度参数以及距离参数，确定所述关节部位的控制屏障函数的离散余量参数以及有效安全距离，所述有效安全距离是所述关节部位与所述目标障碍物之间的有效的安全距离；

16、根据所述有效安全距离确定目标距离度量；

17、根据所述目标障碍物的速度参数以及距离参数，确定所述关节部位的径向净距变化率、所述目标障碍物与所述关节部位对应的胶囊之间的单位径向、所述关节部位与所述目标障碍物之间的相对速度、基准增益参数以及线速度贡献参数；

18、根据所述径向净距变化率、所述单位径向、所述相对速度、所述基准增益参数以及所述线速度贡献参数，确定所述关节不稳的方向性代价参数。

19、在一些实施方式中，所述根据各个所述代价参数对所述最优控制问题函数进行求解，包括：

20、将各个所述代价参数代入至所述最优控制问题函数对应的第一映射关系，得到第二映射关系；

21、在所述第二映射关系上，对部分的度量参数进行求解，得到第三映射关系；

22、对所述第三映射关系进行求解，得到所述目标控制量。

23、在一些实施方式中，所述根据所述目标控制量，控制各个所述关节部位进行运动之后，还包括：

24、在各个所述关节部位的运动过程中，接收障碍物的量测参数、量测时间戳以及心跳数据；

25、在根据所述量测参数、所述量测时间戳以及所述心跳数据，确定所述机械臂未存在运动故障的情况下，返回执行所述根据机械臂中各个关节部位的当前运动状态以及预设的约束条件，构建所述机械臂的最优控制问题函数的步骤。

26、在一些实施方式中，所述接收障碍物的量测参数、量测时间戳以及心跳数据之后，还包括：

27、在根据所述量测参数、所述量测时间戳以及所述心跳数据，确定所述机械臂存在运动故障的情况下，控制所述关节部位停止运动。

28、第二方面，本申请提供了一种机械臂全身动态避障装置，包括存储器和处理器，其中，

29、所述存储器与所述处理器连接，用于存储程序；

30、所述处理器用于通过运行所述存储器中的程序，实现如第一方面或者第一方面任一实现方式所述的机械臂全身动态避障方法。

31、第三方面，本申请提供了一种计算机程序产品，包括计算机指令，所述计算机指令在被处理器执行时，实现如第一方面或者第一方面任一实现方式所述的机械臂全身动态避障方法。

32、第四方面，本申请提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器运行时，实现如第一方面或者第一方面任一实现方式所述的机械臂全身动态避障方法。

33、本申请实施例提供的一种机械臂全身动态避障方法和相关设备，通过机械臂中各个关节部位的当前运动状态以及预设的约束条件，构建机械臂的最优控制问题函数，并确定机械臂中的胶囊最近的目标障碍物，基于目标障碍物的速度参数以及距离参数确定各个关节部位躲避目标障碍物所需的代价参数，并基于各个代价参数对最优控制问题函数进行求解得到目标控制量，从而基于各个目标控制量控制各个关节部位进行运动。本申请中，通过与关节部位最近的目标障碍物的速度参数以及距离参数对机械臂进行控制，也即充分考虑的动态障碍物的速度因素，提高了机械臂的避障效果。

1.一种机械臂全身动态避障方法，其特征在于，包括：

2.根据权利要求1所述的机械臂全身动态避障方法，其特征在于，所述根据所述目标障碍物的速度参数以及距离参数，确定各个所述关节部位躲避所述目标障碍物所需的代价参数，包括：

3.根据权利要求2所述的机械臂全身动态避障方法，其特征在于，所述根据所述目标障碍物的速度参数以及距离参数，确定所述目标障碍物对应的关节部位的软屏障代价参数以及方向性代价参数，包括：

4.根据权利要求2所述的机械臂全身动态避障方法，其特征在于，所述根据所述目标障碍物的速度参数以及距离参数，确定所述关节部位的方向性代价参数，包括：

5.根据权利要求1所述的机械臂全身动态避障方法，其特征在于，所述根据各个所述代价参数对所述最优控制问题函数进行求解，包括：

6.根据权利要求1所述机械臂全身动态避障方法，其特征在于，所述根据所述目标控制量，控制各个所述关节部位进行运动之后，还包括：

7.根据权利要求6所述的机械臂全身动态避障方法，其特征在于，所述接收障碍物的量测参数、量测时间戳以及心跳数据之后，还包括：

8.一种机械臂全身动态避障装置，其特征在于，包括存储器和处理器，其中，

9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器运行时，实现如权利要求1-7中任一项所述的机械臂全身动态避障方法。

10.一种计算机程序产品，其特征在于，包括计算机指令，所述计算机指令在被处理器执行时，实现如权利要求1-7中任一项所述的机械臂全身动态避障方法。