一种基于虚拟墙的机械臂控制方法、装置及机器人与流程

本发明涉及机器人控制，特别涉及一种基于虚拟墙的机械臂控制方法、装置及机器人。

背景技术：

1、随着工程技术的不断发展，机械臂协作的应用越来越广泛，对于应用过程中的安全防护问题也越发关注，为了提高协同作业时人类的安全，需要添加虚拟墙功能，保证机械臂在一个特定的工作空间内工作，而不会穿过虚拟墙威胁人身安全。

2、在现有技术中，虚拟墙一般只对机械臂末端法兰或者工具进行笛卡尔空间内的约束。通过对笛卡尔坐标系三个方向上进行限制，将机械臂末端法兰或者工具的工作空间约束在一定范围内。但是在机械臂末端法兰或者工具在运动过程中，机械臂的其他关节也可能会与虚拟墙接触，甚至超出虚拟墙边界从而威胁人身安全，因此也需要对机械臂的其他关节进行约束。

3、在现有技术中，虚拟墙常被用于医疗行业中引导手术机器人的末端手术刀等工具实施手术，根据手术特点通常将虚拟墙设置为圆锥形状，但是随着机械臂的广泛应用，圆锥形状的虚拟墙已经满足不了所有的应用需求，因此需要其他更多可供操作人员选择的几何形状虚拟墙。

4、在现有技术中，通常将机械臂的所有关节、连杆、末端法兰或者工具用复杂的几何形状进行包络，因此在交互过程中需要消耗大量的计算资源，这会增加机械臂的使用难度，严重限制机械臂的应用普及。

技术实现思路

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、其中，agenti为对所述机械臂末端及各个关节进行包络的包络体，为所述机械臂末端或关节的笛卡尔空间线速度，为所述机械臂末端或关节与所述虚拟墙的距离值，kvirtual为虚拟刚度系数，bvirtual为虚拟阻尼系数；

33、当所述虚拟墙为长方体时，所述虚拟力的计算公式为：

34、

35、

36、

37、

38、其中，agenti为对第i个所述机械臂末端及各个关节进行包络的包络体，为所述机械臂末端或关节的笛卡尔空间线速度，为所述机械臂末端或关节与所述虚拟墙的距离值，kvirtual为虚拟刚度系数，bvirtual为虚拟阻尼系数。

39、进一步地，当基于所述机械臂的关节电流检测机械臂关节力矩时，所述虚拟操控力fvirtual的计算公式为：

40、

41、j(θ)＝[j1，j2，j3，j4，j5，j6]；

42、其中，为所述机械臂关节1～关节6的虚拟关节力矩，j(θ)为agenti对应的雅可比矩阵，θ为机械臂的关节角度，和分别为所述虚拟力在笛卡尔空间坐标系中的值，agenti为对第i个所述机械臂末端及各个关节进行包络的包络体；

43、当基于所述机械臂末端多维力传感器检测机械臂关节力矩时，所述虚拟操控力fvirtual的计算公式为：

44、

45、其中，agenti为对第i个所述机械臂末端及各个关节进行包络的包络体，和分别为所述虚拟力在笛卡尔空间坐标系中的值。

46、进一步地，所述基于机械臂控制器类型获取所述机械臂末端或相应关节的真实操控力，包括：

47、当基于所述机械臂的关节电流检测机械臂关节力矩时，所述真实操控力的计算公式为：

48、freal＝τ(t)＝kmi(t)；

49、其中，τ(t)为关节输出转矩，i(t)为关节反馈电流，km为机械臂转矩常数；

50、当基于所述机械臂末端多维力传感器检测机械臂关节力矩时，所述真实操控力的计算公式为：

51、

52、其中，表示六维力传感器坐标系相对于笛卡尔坐标系的位姿，fs表示六维力传感器在六维力传感器坐标系下经过重力补偿补偿后的环境接触力。

53、进一步地，所述依据所述真实操控力控制所述机械臂运行，包括：

54、基于所述真实操控力计算所述机械臂的控制量；

55、依据所述机械臂的控制量控制所述机械臂运行；

56、当基于所述机械臂的关节电流检测机械臂关节力矩时，所述机械臂的控制量的计算公式为：

57、

58、其中，为惯性项，m(θ)为机械臂惯性矩阵，为离心力、科氏力项，为离心力、科氏力矩阵，g(θ)为重力项，为机械臂关节摩擦力，τext为机械臂承受外力矩，freal为真实操控力；

59、当基于所述机械臂末端多维力传感器检测机械臂关节力矩时，所述机械臂的控制量的计算公式为：

60、

61、xe=x-x0；

62、xu＝x0+xe(t+1)；

63、其中，md为刚度系数对角矩阵，bd为阻尼系数对角矩阵矩阵，kd为惯性系数对角矩阵，xe为机械臂末端的实际位姿x与机械臂末端的期望位姿x0之差，xe(t+1)为下一控制周期期望的机械臂末端修正的位姿偏差，xu为机械臂末端最终的位姿控制量。

64、进一步地，所述依据所述真实操控力和所述虚拟操控力控制所述机械臂运行，包括：

65、基于所述真实操控力和所述虚拟操控力，计算所述机械臂的控制量；

66、依据所述机械臂的控制量控制所述机械臂运行；

67、当基于所述机械臂的关节电流检测机械臂关节力矩时，所述机械臂的控制量的计算公式为：

68、

69、其中，为惯性项，m(θ)为机械臂惯性矩阵，为离心力、科氏力项，为离心力、科氏力矩阵，g(θ)为重力项，为机械臂关节摩擦力，τext为机械臂承受外力矩，freal为真实操控力，fvirtual为虚拟操控力；

70、当基于所述机械臂末端多维力传感器检测机械臂关节力矩时，所述机械臂的控制量的计算公式为：

71、

72、xe＝x-x0；

73、xu＝x0+xe(t+1)；

74、其中，md为刚度系数对角矩阵，为机械臂末端的实际加速度与机械臂末端的期望加速度之差，bd为阻尼系数对角矩阵矩阵，为机械臂末端的实际速度与机械臂末端的期望速度之差，kd为惯性系数对角矩阵，xe为机械臂末端的实际位姿x与机械臂末端的期望位姿x0之差，xe(t+1)为下一控制周期期望的机械臂末端修正的位姿偏差，xu为机械臂末端最终的位姿控制量。

75、相应地，本发明实施例的第二方面提供了一种基于虚拟墙的机械臂控制装置，包括：

76、位置判断模块，其用于基于虚拟墙类型，判断机械臂末端及各个关节是否位于所述虚拟墙范围之内；

77、第一控制模块，其用于如所述机械臂末端及各个关节均位于所述虚拟墙范围之内，则基于机械臂控制器类型获取所述机械臂末端的真实操控力，依据所述真实操控力控制所述机械臂运行；

78、第二控制模块，其用于如所述机械臂末端及各个关节任一位于所述虚拟墙范围之外，则基于机械臂控制器类型获取所述机械臂末端或相应关节的真实操控力，并获取所述机械臂末端或相应关节的虚拟操控力，依据所述真实操控力和所述虚拟操控力控制所述机械臂运行。

79、相应地，本发明实施例的第三方面提供了一种机器人，基于上述任一所述的基于虚拟墙的机械臂控制方法对其机械臂进行控制。

80、相应地，本发明实施例的第四方面提供了一种电子设备，包括：至少一个处理器；以及与所述至少一个处理器连接的存储器；其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器执行上述任一所述的基于虚拟墙的机械臂控制方法。

81、相应地，本发明实施例的第五方面提供了一种计算机可读存储介质，其上存储有计算机指令，该指令被处理器执行时实现上述任一所述的基于虚拟墙的机械臂控制方法。

82、本发明实施例的上述技术方案具有如下有益的技术效果：

83、通过将机械臂各个关节纳入笛卡尔空间约束中，解决机械臂关节超出虚拟墙边界导致的危害人身安全的问题；还通过多种虚拟墙类型提高了对机械臂应用场景的适应性；基于虚拟墙类型相应的几何参数来判断机械臂参与虚拟墙控制的相应关节，无需机械臂所有关节均参与到计算过程中，有效降低了机械臂的计算消耗资源。