机械臂通用圆弧指令位置与姿态过渡同步规划方法与系统与流程

本发明涉及机器人控制，更具体的，涉及一种机械臂通用圆弧指令位置与姿态过渡同步规划方法与系统。

背景技术：

1、在机器人和机械臂的运动控制领域，圆弧指令规划是实现复杂轨迹和姿态同步控制的重要部分，圆弧运动通常用于工业机器人进行焊接、涂胶、切割等应用，要求机械臂末端沿着圆弧轨迹移动，且姿态变化要与位置同步。

2、目前，现有技术中的使用方案如下：

3、一、利用几何关系直接计算圆弧路径，通常通过已知起点、终点和参考点确定圆弧轨迹，再利用旋转矩阵或四元数描述姿态，优点：计算简单可直接利用圆弧的数学性质，计算效率高，且易于实现，适用于固定圆心和半径的规则圆弧路径；同时也存在明显缺点，即姿态规划不灵活，对姿态的插值常需额外设计，可能导致姿态过渡不平滑，以及缺乏动态优化，无法很好地处理速度和加速度约束

4、二、通过样条插值或参数化方法，生成连续平滑的路径和姿态，其中，姿态插值常用欧拉角插值或四元数插值，优点：平滑性好，位置和姿态在轨迹上具有良好的连续性和光滑性，以及灵活性高，支持复杂轨迹和多样姿态的规划；同样存在明显缺点，即计算复杂度高，插值计算需要较多资源，尤其在实时性要求高的场景中，以及可能失真，关键点数量不足或分布不合理时，可能导致路径失真或姿态误差。

技术实现思路

1、本发明的目的是提供一种机械臂通用圆弧指令位置与姿态过渡同步规划方法与系统，基于几何特性插值，可动态调整，无需姿态的插值额外设计，也可以做到姿态过渡的高阶平滑，通过同步映射，将姿态的变化与圆弧路径的弧长参数关联，使姿态过渡更加自然，不会出现突兀的旋转。

2、本发明第一方面提供了一种机械臂通用圆弧指令位置与姿态过渡同步规划方法，包括以下步骤：

3、基于用户数据确定圆心和半径，其中，所述用户数据包括起点、终点、空间参考点；

4、基于空间参考点结合所述圆心得到目标圆弧，对所述目标圆弧进行比例划分得到不同弧长；

5、确定同步条件，具体包括获取设置的过渡段参数与不同弧长相乘得到过渡起始点和过渡结束点；

6、根据圆弧总弧长进行曲线速度规划，并根据插补点确定位置与姿态值，基于所述位置与姿态值生成机器臂的关节控制命令。

7、本方案中，所述基于用户数据确定圆心和半径，其中，所述用户数据包括起点、终点、空间参考点，具体包括：

8、获取空间参考点p1、p2和p3，其中，p1：(x1,y1,z1,a1,b1,c1)、p2：(x2,y2,z2,a2,b2,c2)、p3：(x3,y3,z3,a3,b3,c3)；

9、基于空间参考点p1、p2和p3确定平面以及平面法向量；

10、在确定平面后通过等距离约束、平面约束联立求得所述圆心，并同步计算出所述半径。

11、本方案中，所述基于空间参考点结合所述圆心得到目标圆弧，对所述目标圆弧进行比例划分得到不同弧长，具体包括：

12、计算空间参考点p1、p2和p3的姿态差得到同步规划值；

13、并将空间参考点p1、p2和p3与所述圆心得到的目标圆弧基于不同位置分成不同弧长，其中，空间参考点p1与p2段为arcl1，空间参考点p2与p3段为arcl2。

14、本方案中，所述确定同步条件，具体包括获取设置的过渡段参数与不同弧长相乘得到过渡起始点和过渡结束点，具体包括：

15、获取设置的过渡段参数blend-ratio，具体为[0,1]区间的值，且最大长度不超过两段弧长中最短弧长的一半；

16、将不同弧长与所述过渡段参数blend-ratio相乘，得到过渡起始点与过渡结束点对应的弧长位置。

17、本方案中，根据圆弧总弧长进行曲线速度规划，并根据插补点确定位置与姿态值，基于所述位置与姿态值生成机器臂的关节控制命令，具体包括：

18、根据所述圆弧总弧长进行s曲线速度规划以确定位移函数和速度函数；

19、基于所述位移函数与所述速度函数确定圆弧总弧长对应的插补点；

20、基于所述插补点进行位置和姿态同步比例映射运动得到所述位置与姿态值进行插补运动，其中，具体基于所述位置与姿态值分解为当前机器臂对应的不同关节控制命令以控制机器臂运动。

21、本方案中，所述方法还包括在基于插补点运动过程中，通过五次多项式衔接两段弧长的过渡段，其中，五次多项式如下：

22、x(t)= a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5；

23、其中，x(t) 是某时刻 t 的位移；a_0,a_1,a_2,a_3,a_4,a_5是多项式的系数，t是时间。

24、本发明第二方面还提供一种机械臂通用圆弧指令位置与姿态过渡同步规划系统，包括存储器和处理器，所述存储器中包括机械臂通用圆弧指令位置与姿态过渡同步规划方法程序，所述机械臂通用圆弧指令位置与姿态过渡同步规划方法程序被所述处理器执行时实现如下步骤：

25、基于用户数据确定圆心和半径，其中，所述用户数据包括起点、终点、空间参考点；

26、基于空间参考点结合所述圆心得到目标圆弧，对所述目标圆弧进行比例划分得到不同弧长；

27、确定同步条件，具体包括获取设置的过渡段参数与不同弧长相乘得到过渡起始点和过渡结束点；

28、根据圆弧总弧长进行曲线速度规划，并根据插补点确定位置与姿态值，基于所述位置与姿态值生成机器臂的关节控制命令。

29、本方案中，所述基于用户数据确定圆心和半径，其中，所述用户数据包括起点、终点、空间参考点，具体包括：

30、获取空间参考点p1、p2和p3，其中，p1：(x1,y1,z1,a1,b1,c1)、p2：(x2,y2,z2,a2,b2,c2)、p3：(x3,y3,z3,a3,b3,c3)；

31、基于空间参考点p1、p2和p3确定平面以及平面法向量；

32、在确定平面后通过等距离约束、平面约束联立求得所述圆心，并同步计算出所述半径。

33、本方案中，所述基于空间参考点结合所述圆心得到目标圆弧，对所述目标圆弧进行比例划分得到不同弧长，具体包括：

34、计算空间参考点p1、p2和p3的姿态差得到同步规划值；

35、并将空间参考点p1、p2和p3与所述圆心得到的目标圆弧基于不同位置分成不同弧长，其中，空间参考点p1与p2段为arcl1，空间参考点p2与p3段为arcl2。

36、本方案中，所述确定同步条件，具体包括获取设置的过渡段参数与不同弧长相乘得到过渡起始点和过渡结束点，具体包括：

37、获取设置的过渡段参数blend-ratio，具体为[0,1]区间的值，且最大长度不超过两段弧长中最短弧长的一半；

38、将不同弧长与所述过渡段参数blend-ratio相乘，得到过渡起始点与过渡结束点对应的弧长位置。

39、本方案中，根据圆弧总弧长进行曲线速度规划，并根据插补点确定位置与姿态值，基于所述位置与姿态值生成机器臂的关节控制命令，具体包括：

40、根据所述圆弧总弧长进行s曲线速度规划以确定位移函数和速度函数；

41、基于所述位移函数与所述速度函数确定圆弧总弧长对应的插补点；

42、基于所述插补点进行位置和姿态同步比例映射运动得到所述位置与姿态值进行插补运动，其中，具体基于所述位置与姿态值分解为当前机器臂对应的不同关节控制命令以控制机器臂运动。

43、本方案中，所述方法还包括在基于插补点运动过程中，通过五次多项式衔接两段弧长的过渡段，其中，五次多项式如下：

44、x(t)= a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5；

45、其中，x(t) 是某时刻 t 的位移；a_0,a_1,a_2,a_3,a_4,a_5是多项式的系数，t是时间。

46、本发明第三方面提供了一种计算机可读存储介质，所述计算机可读存储介质中包括机器的一种机械臂通用圆弧指令位置与姿态过渡同步规划方法程序，所述机械臂通用圆弧指令位置与姿态过渡同步规划方法程序被处理器执行时，实现如上述任一项所述的一种机械臂通用圆弧指令位置与姿态过渡同步规划方法的步骤。

47、本发明公开的一种机械臂通用圆弧指令位置与姿态过渡同步规划方法与系统，基于几何特性插值，可动态调整，无需姿态的插值额外设计，也可以做到姿态过渡的高阶平滑，计算简单，不需要太多额外算力。

48、其中，通过同步映射，将姿态的变化与圆弧路径的弧长参数关联，使姿态过渡更加自然，不会出现突兀的旋转，同步映射可以保证位置和姿态在路径上的一阶、二阶甚至更高阶导数的连续性，提高轨迹整体的平滑性。

49、此外，运动协调性高，通过映射，姿态和位置的变化直接相关，避免出现路径完成后姿态未跟随的问题，允许按比例调整姿态随路径变化的速度，适配不同任务需求，例如，在关键点处增加姿态变化的速度，能够确保整个圆弧轨迹上，姿态与位置的关系保持一致，不易出现局部不协调的现象。