机械臂过腕部奇异点的轨迹规划方法、系统、设备及介质与流程

本发明涉及机械臂控制，特别是涉及一种机械臂过腕部奇异点的轨迹规划方法、系统、设备及介质。

背景技术：

1、机械臂在笛卡尔空间中走直线时，路径中很容易出现五轴为零度的情况，此时机械臂进入腕部奇异状态，会导致四轴和六轴的速度无限大，造成超速停机。

2、目前大多数机械臂在遇到奇异问题时，会报告路径中存在错误重新选择位置或改变姿态进行规划，这种方式由于改变了路径或初末姿态，容易造成路径的丢失。

3、或者是，划分奇异区域在奇异区域内进行关节空间的规划；在路径规划时，首先在笛卡尔空间把路径分割，获得离散的笛卡尔空间内的点和运动总时间，然后再根据笛卡尔空间的点反解得到各轴的角度，如果路径中存在五轴过零的点，就会引起四轴和六轴的速度无穷大，导致无法通过，而该中方式就是在四轴和六轴超速开始位置和终止位置的这一段在关节空间内进行重新规划，会牺牲一部分位置精度，导致奇异区域内路径精度误差过大。

技术实现思路

1、为了解决上述问题，本发明提出了一种机械臂过腕部奇异点的轨迹规划方法、系统、设备及介质，在保证末端位置精度的同时，使机械臂末端经过奇异位置时不会出现四六轴超速和末端精度误差大的问题。

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

3、第一方面，本发明提供一种机械臂过腕部奇异点的轨迹规划方法，包括：

4、根据对机械臂所规划的初始路径得到在笛卡尔空间内每个时刻对应的位置，根据初始路径总时间、起点位置的轴角度和终点位置的轴角度，得到在四轴、五轴和六轴匀速状态下每个时刻对应的四轴、五轴和六轴的轴角度；

5、根据每个时刻对应的四轴、五轴和六轴的轴角度和每个时刻对应的位置得到一轴、二轴和三轴的轴角度，从而根据一轴至六轴的轴角度得到使机械臂通过腕部奇异点的运动轨迹。

6、作为可选择的实施方式，四轴、五轴和六轴匀速的轴速度分别为：

7、

8、其中，为四轴的轴速度，为五轴的轴速度，为六轴的轴速度，、、分别为起点位置的四轴、五轴和六轴的轴角度，、、分别为终点位置的四轴、五轴和六轴的轴角度，t为初始路径总时间。

9、作为可选择的实施方式，每个时刻对应的四轴、五轴和六轴的轴角度分别为：

10、

11、其中，为t时刻四轴的轴角度，为t时刻五轴的轴角度，为t时刻六轴的轴角度，为插值时间。

12、作为可选择的实施方式，一轴的轴角度为：

13、；

14、其中，为四轴的轴角度，为五轴的轴角度，为一轴的轴角度，（x,y）为每个时刻对应的位置坐标， d6是第6个关节的关节距离。

15、作为可选择的实施方式，三轴的轴角度为：

16、；

17、其中，为三轴的轴角度，c3、s3、k3均为中间系数。

18、作为可选择的实施方式，二轴的轴角度为：

19、；

20、其中，为二轴的轴角度，为三轴的轴角度，z为每个时刻对应的位置坐标中的z坐标值， d1为第1个关节的关节距离，为从3轴坐标系变换到6轴坐标系后的位置坐标。

21、作为可选择的实施方式，得到一轴、二轴和三轴的轴角度后，选择使所有轴的轴角度变化最小的解为一轴、二轴和三轴的最优轴角度，根据一轴至六轴的最优轴角度得到使机械臂通过腕部奇异点的运动轨迹。

22、第二方面，本发明提供一种机械臂过腕部奇异点的轨迹规划系统，包括：

23、四轴至六轴轴角度确定模块，被配置为根据对机械臂所规划的初始路径得到在笛卡尔空间内每个时刻对应的位置，根据初始路径总时间、起点位置的轴角度和终点位置的轴角度，得到在四轴、五轴和六轴匀速状态下每个时刻对应的四轴、五轴和六轴的轴角度；

24、一轴至三轴轴角度确定模块，被配置为根据每个时刻对应的四轴、五轴和六轴的轴角度和每个时刻对应的位置得到一轴、二轴和三轴的轴角度，从而根据一轴至六轴的轴角度得到使机械臂通过腕部奇异点的运动轨迹。

25、第三方面，本发明提供一种电子设备，包括存储器和处理器以及存储在存储器上并在处理器上运行的计算机指令，所述计算机指令被处理器运行时，完成第一方面所述的方法。

26、第四方面，本发明提供一种计算机可读存储介质，用于存储计算机指令，所述计算机指令被处理器执行时，完成第一方面所述的方法。

27、与现有技术相比，本发明的有益效果为：

28、本发明提出一种机械臂过腕部奇异点的轨迹规划方法、系统、设备及介质，应用于机械臂笛卡尔空间轨迹规划，根据起点位置和终点位置的轴角度，固定四轴、五轴和六轴的轴速度，从而得到在四轴、五轴和六轴匀速状态下每个时刻对应的四轴、五轴和六轴的轴角度，继而采用四轴、五轴和六轴的轴角度反解一轴、二轴和三轴的轴角度，在保证末端位置精度的同时，使机械臂末端经过奇异位置时不会出现四六轴超速和末端精度误差大的问题。

29、本发明无需改变路径和初末姿态，无需绕过奇异点规划，实现原路径通过奇异点，其关键过程是固定四轴、五轴和六轴的轴速度，再结合位置反解一轴、二轴和三轴的轴角度；因为五轴为0度时，四轴和六轴旋转轴重合，在求反解时会引起四轴和六轴的轴速度出现超速，那么先固定四轴、五轴和六轴的轴速度就不会出现该问题，从而保证位置精度。

30、本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

技术特征：

1.机械臂过腕部奇异点的轨迹规划方法，其特征在于，包括：

2.如权利要求1所述的机械臂过腕部奇异点的轨迹规划方法，其特征在于，四轴、五轴和六轴匀速的轴速度分别为：

3.如权利要求2所述的机械臂过腕部奇异点的轨迹规划方法，其特征在于，每个时刻对应的四轴、五轴和六轴的轴角度分别为：

4.如权利要求1所述的机械臂过腕部奇异点的轨迹规划方法，其特征在于，一轴的轴角度为：

5.如权利要求1所述的机械臂过腕部奇异点的轨迹规划方法，其特征在于，三轴的轴角度为：

6.如权利要求1所述的机械臂过腕部奇异点的轨迹规划方法，其特征在于，二轴的轴角度为：

7.如权利要求1所述的机械臂过腕部奇异点的轨迹规划方法，其特征在于，得到一轴、二轴和三轴的轴角度后，选择使所有轴的轴角度变化最小的解为一轴、二轴和三轴的最优轴角度，根据一轴至六轴的最优轴角度得到使机械臂通过腕部奇异点的运动轨迹。

8.机械臂过腕部奇异点的轨迹规划系统，其特征在于，包括：

9.一种电子设备，其特征在于，包括存储器和处理器以及存储在存储器上并在处理器上运行的计算机指令，所述计算机指令被处理器运行时，完成权利要求1-7任一项所述的方法。

10.一种计算机可读存储介质，其特征在于，用于存储计算机指令，所述计算机指令被处理器执行时，完成权利要求1-7任一项所述的方法。

技术总结
本发明公开一种机械臂过腕部奇异点的轨迹规划方法、系统、设备及介质，涉及机械臂控制技术领域，包括：根据对机械臂所规划的初始路径得到在笛卡尔空间内每个时刻对应的位置，根据初始路径总时间、起点位置的轴角度和终点位置的轴角度，得到在四轴、五轴和六轴匀速状态下每个时刻对应的四轴、五轴和六轴的轴角度；根据每个时刻对应的四轴、五轴和六轴的轴角度和每个时刻对应的位置得到一轴、二轴和三轴的轴角度，从而根据一轴至六轴的轴角度得到使机械臂通过腕部奇异点的运动轨迹。在保证末端位置精度的同时，使机械臂末端经过奇异位置时不会出现四六轴超速和末端精度误差大的问题。

技术研发人员：王迎智,靳津,张书猛
受保护的技术使用者：极限人工智能（北京）有限公司
技术研发日：
技术公布日：2024/1/13