一种机械臂恒力控制过程中的奇异点规避方法及装置与流程

本发明涉及机械臂奇异点规避，具体涉及一种机械臂恒力控制过程中的奇异点规避方法及装置，

背景技术：

1、目前恒力控制主要应用在打磨、装配等领域。这些场景下作业环境是已知的，操作人员可通过离线编程或现场示教等方式获得规避了奇异点的作业路径，这样在恒力控制作业过程中就无需考虑规避奇异点的问题。但是这种方法无法应用在作业环境未提前知晓或作业路径需要在线生成的场景。

2、目前针对机械臂在笛卡尔空间中的奇异点规避问题，已知的方案主要包括势场法和阻尼倒数法及其变种。例如：申请号为：202110968731.5，名称为：一种机械臂奇异点的通用规避方法与系统的中国发明专利公开了以奇异点构成排斥势场，并以此计算关节空间排斥速度，然后利用雅各比矩阵计算笛卡尔空间排斥速度，再结合驱动速度计算得到最终的关节速度，完成奇异点的规避。但是该方法应用过程中机器人末端工具在笛卡尔空间中的姿态可能发生变化，不适合作业环境未提前知晓的在线恒力控制过程。申请号为：202011439303.5，名称为：一种非球形手腕机械臂奇异点规避方法和系统的中国发明专利公开了利用雅各比矩阵变化方法实现非球形手腕机械臂的奇异因子推导的方法。但是该方法对雅各比矩阵进行了修改，在笛卡尔空间速度转换为关节空间速度时引入了变化量，使得机器人的末端工具位姿相比期望值出现不可控的偏移。

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、第三处理模块，用于将所述笛卡尔空间虚拟力输入所述目标机械臂的恒力控制器得到笛卡尔空间位姿控制量；

34、第四处理模块，用于利用所述笛卡尔空间位姿控制量控制所述目标机械臂运动，并返回所述基于所述目标机械臂的关节角度并利用雅各比矩阵和操作指数的计算方式，计算笛卡尔空间虚拟力的步骤，直至所述目标机械臂对应的操作指数大于虚拟力使能阈值。

35、根据第三方面，本发明实施例提供了一种计算机可读存储介质，所述计算机可读存储介质存储计算机指令，所述计算机指令被处理器执行时实现本发明第一方面及其任意一种可选方式所述的方法。

36、根据第四方面，本发明实施例提供了一种电子设备，包括：存储器和处理器，所述存储器和所述处理器之间互相通信连接，所述存储器中存储有计算机指令，所述处理器通过执行所述计算机指令，从而执行本发明第一方面及其任意一种可选方式所述的方法。

37、本发明技术方案，具有如下优点：

38、本发明实施例提供了一种机械臂恒力控制过程中的奇异点规避方法及装置，通过建立目标机械臂的运动学模型，并基于运动学模型确定雅各比矩阵和操作指数的计算方式；基于目标机械臂的关节角度并利用雅各比矩阵和操作指数的计算方式，计算笛卡尔空间虚拟力；将笛卡尔空间虚拟力输入目标机械臂的恒力控制器得到笛卡尔空间位姿控制量；利用笛卡尔空间位姿控制量控制目标机械臂运动，并返回基于目标机械臂的关节角度并利用雅各比矩阵和操作指数的计算方式，计算笛卡尔空间虚拟力的步骤，直至目标机械臂对应的操作指数大于虚拟力使能阈值。从而通过计算笛卡尔空间虚拟力，该虚拟力的大小与机械臂当前操作指数成负相关的关系，即机械臂当前操作指数越大则虚拟力越小，同时虚拟力的方向与使得机械臂远离奇异点的方向相同。然后通过将该虚拟力引入到恒力控制器输入力信号的计算中，就可以使得机械臂恒力控制过程中受到来自于奇异区域的排斥力，从而实现在线奇异点规避，并且通过笛卡尔空间位姿控制量进行关节位置调节，使得在规避奇异点的过程中可以保持机械臂末端姿态不变，满足恒力控制对末端姿态保持的要求。