机械臂的控制方法、装置及电子设备与流程

本技术实施方式涉及机械臂控制，特别是涉及一种机械臂的控制方法、装置及电子设备。

背景技术：

1、机械臂遥控功能作为机械臂控制技术领域的一个基础功能一直备受重视。借助遥控功能，操作人员可以控制机械臂以预期的运动方式到达期望的位置。机械臂遥控技术一般有两种实现方式：在关节空间实现的机械臂遥控技术、在笛卡尔空间实现的机械臂遥控技术。

2、在关节空间实现的机械臂遥控技术的原理较为简单，只要控制关节转动到指定的角度即可，但这种遥控方式不符合人的直觉，而且无法满足笛卡尔空间的作业要求。在笛卡尔空间实现的机械臂遥控技术可以控制机械臂的末端在笛卡尔空间运动到指定的位置，更符合人的直觉，也可以满足笛卡尔空间的作业要求。但这种遥控方式缺乏对异常情况的适应能力，因为在笛卡尔空间控制机械臂需要求机械臂逆运动学的解，而出现下述状况时机械臂逆运动学是无解的。

3、(1)机械臂处于奇异点；

4、(2)机械臂中若干个关节出现故障，无法移动。

技术实现思路

1、本技术实施例提供一种机械臂的控制方法、装置及电子设备，通过雅可比矩阵做最小二乘法计算，计算出最优的机械臂各关节的期望速度，进而使机械臂具有穿越奇异点的能力，在机械臂出现故障时也能够对机械臂进行遥控。

2、本技术实施例提供以下技术方案：

3、第一方面，本技术实施例提供一种机械臂的控制方法，该机械臂包括末端和多个关节，该机械臂的控制方法，包括：

4、获取机械臂的各个关节的当前期望速度和关节角度；

5、基于机械臂微分运动学原理，得到第一雅可比矩阵；

6、修正第一雅可比矩阵，得到第二雅可比矩阵；

7、基于最小二乘法，根据第二雅克比矩阵，计算机械臂的关节期望速度；

8、根据最大约束速度阈值，对关节期望速度进行修正，得到第一修正结果；

9、根据末端的运动方向与当前期望速度方向，对第一修正结果进行修正，得到第二修正结果；

10、根据第二修正结果，得到修正后的关节期望速度，以控制机械臂的运动。

11、在一些实施例中，基于机械臂微分运动学原理，得到第一雅可比矩阵，包括：

12、基于机械臂微分运动学原理，根据机械臂的当前期望速度和关节角度，计算机械臂的当前雅可比矩阵；

13、将当前雅可比矩阵确定为第一雅可比矩阵。

14、在一些实施例中，修正第一雅可比矩阵，得到第二雅可比矩阵，包括：

15、将当前期望速度为零的关节确定为故障关节；

16、若存在故障关节，则将故障关节对应的列从第一雅可比矩阵中删除，得到第二雅可比矩阵，其中，每一关节一一对应第一雅克比矩阵中的一个列；

17、若不存在故障关节，则将第一雅可比矩阵确定为第二雅可比矩阵。

18、在一些实施例中，基于最小二乘法，根据第二雅克比矩阵，计算机械臂的关节期望速度，包括：

19、根据第二雅可比矩阵和第二雅可比矩阵的伪逆，确定第二雅可比矩阵的投影矩阵，具体包括：

20、

21、其中，为第二雅可比矩阵的投影矩阵，ja为第二雅可比矩阵，为第二雅可比矩阵的伪逆矩阵，u为第二雅可比矩阵进行奇异值分解得到的第一正交矩阵，v为第二雅可比矩阵进行奇异值分解得到的第二正交矩阵，σ为第二雅可比矩阵进行奇异值分解得到的对角矩阵，ut为第一正交矩阵的转置矩阵，vt为第二正交矩阵的转置矩阵，σ+为对角矩阵的伪逆矩阵；

22、根据第二雅可比矩阵的投影矩阵和当前期望速度，计算当前期望速度在第二雅可比矩阵列空间中的投影向量，具体包括：

23、

24、其中，为当前期望速度在第二雅可比矩阵列空间中的投影向量，为第二雅可比矩阵的投影矩阵，为当前期望速度；

25、基于最小二乘法，结合投影向量、第二雅可比矩阵和期望速度，计算关节期望速度，具体包括：

26、

27、其中，当时，则取得最小值，此时，得到关节期望速度等于

28、在一些实施例中，根据最大约束速度阈值，对关节期望速度进行修正，得到第一修正结果，包括：

29、设置最大约束速度阈值；

30、若存在若干个关节的关节期望速度大于最大约束速度阈值，则计算若干个关节的关节期望速度与最大约束速度阈值的比值，得到若干个比值；

31、将若干个比值中的最大比值确定为第一比值，并确定第一关节期望速度；

32、若所有的关节期望速度均小于或等于最大约束速度阈值，则将关节期望速度确定为第一关节期望速度；

33、将第一关节期望速度确定为第一修正结果；

34、将若干个比值中的最大比值确定为第一比值，并确定第一关节期望速度，具体包括：

35、根据第一比值，确定第一关节期望速度，具体包括：

36、

37、其中，为第一关节期望速度，为关节期望速度，r为第一比值。

38、在一些实施例中，机械臂包括末端，根据末端的运动方向与当前期望速度方向，对第一修正结果进行修正，得到第二修正结果，包括：

39、计算末端的运动方向与当前期望速度方向的余弦值，计算公式为：

40、

41、

42、其中，cosα为末端的运动方向与当前期望速度方向的余弦值，为末端的速度，为当前期望速度，ja为第二雅可比矩阵，为第一修正结果；

43、根据末端的运动方向与当前期望速度方向的夹角的余弦值，计算夹角的角度；

44、设置夹角的最大阈值；

45、若角度大于或等于夹角的最大阈值，则将机械臂的关节期望速度置为零，以停止机械臂运动；

46、若角度小于夹角的最大阈值，则将第一修正结果确定为第二修正结果。

47、在一些实施例中，在得到第二修正结果之后，方法还包括：

48、根据第二修正结果，计算末端修正后的运动速度，计算公式为：

49、

50、其中，为末端修正后的运动速度，ja为第二雅可比矩阵，为第二修正结果。

51、在一些实施例中，根据第二修正结果，得到修正后的关节期望速度，包括：

52、将第二修正结果确定为修正后的关节期望速度；

53、方法还包括：

54、获取刷新频率；

55、根据刷新频率，计算控制周期，计算公式为：

56、

57、其中，δt为控制周期，fc为刷新频率；

58、根据控制周期和第二修正结果，计算修正后的关节位置，计算公式为：

59、

60、其中，qc为修正后的关节位置，q为机械臂的当前关节位置，为第二修正结果，δt为控制周期。

61、第二方面，本技术实施例提供一种机械臂的控制装置，该机械臂包括末端和多个关节，该机械臂的控制装置，包括：

62、获取单元，用于获取机械臂的各个关节的当前期望速度和关节角度；

63、处理单元，用于基于机械臂微分运动学原理，得到第一雅可比矩阵；修正第一雅可比矩阵，得到第二雅可比矩阵；基于最小二乘法，根据第二雅克比矩阵，计算机械臂的关节期望速度；根据最大约束速度阈值，对关节期望速度进行修正，得到第一修正结果；根据末端的运动方向与当前期望速度方向，对第一修正结果进行修正，得到第二修正结果；根据第二修正结果，得到修正后的关节期望速度，以控制机械臂的运动。

64、第三方面，本技术实施例提供一种电子设备，包括：

65、至少一个处理器；和

66、与至少一个处理器通信连接的存储器；其中，

67、存储器存储有可被至少一个处理器执行的指令，指令被至少一个处理器执行，以使至少一个处理器能够执行如第一方面的机械臂的控制方法。

68、第四方面，本技术实施例提供一种非易失性计算机可读存储介质，非易失性计算机可读存储介质存储有计算机可执行指令，计算机可执行指令用于使电子设备执行如第一方面的机械臂的控制方法。

69、本技术实施方式的有益效果是：区别于现有技术的情况，本技术实施方式提供一种机械臂的控制方法，该机械臂包括末端和多个关节，该机械臂的控制方法，包括：获取机械臂的各个关节的当前期望速度和关节角度；基于机械臂微分运动学原理，得到第一雅可比矩阵；修正第一雅可比矩阵，得到第二雅可比矩阵；基于最小二乘法，根据第二雅克比矩阵，计算机械臂的关节期望速度；根据最大约束速度阈值，对关节期望速度进行修正，得到第一修正结果；根据末端的运动方向与当前期望速度方向，对第一修正结果进行修正，得到第二修正结果；根据第二修正结果，得到修正后的关节期望速度，以控制机械臂的运动，本技术能够通过雅可比矩阵做最小二乘法计算，计算出最优的机械臂各关节的期望速度，进而使机械臂具有穿越奇异点的能力，在机械臂出现故障时也能够对机械臂进行遥控。