基于四元数的机械臂节点姿态速度前瞻控制方法及装置与流程

本发明涉及机械臂控制，特别涉及一种基于四元数的机械臂节点姿态速度前瞻控制方法及装置。

背景技术：

1、机械臂在进行节点移动时，拐弯点是指机械臂路径上的转折点或转弯点。在规划机械臂的路径时，通常需要考虑拐弯点的选择，以便使机械臂运动平滑、快速且准确。

2、机械臂的欧拉角表示方法可能会遇到万向节锁死问题，这是由于欧拉角的定义和约束导致的。欧拉角是一种将机械臂的姿态表示为三个旋转角度(通常是绕固定坐标轴的旋转)，如滚转角、俯仰角和偏航角。在某些情况下，当机械臂的姿态转动过程中，特别是当俯仰角接近90度或-90度时，欧拉角存在万向节锁死的问题。这意味着，在特定的欧拉角组合下，无法唯一确定机械臂的姿态。

技术实现思路

1、本发明实施例的目的是提供一种基于四元数的机械臂节点姿态速度前瞻控制方法及装置，通过对机械臂节点预处理一段插补路径，自动分析轨迹路径，发现并处理路径危险点，对速度轨迹提前规划，实现速度的平滑过渡避免加减速频繁变化。

2、为解决上述技术问题，本发明实施例的第一方面提供了一种基于四元数的机械臂节点姿态速度前瞻控制方法，包括如下步骤：

3、在机械臂节点的移动路径上对预设轨迹进行插补计算，得到所述机械臂节点在每一所述预设轨迹的起点后的路径规划；

4、依据所述路径规划获取路径危险点，通过四元数相应调整所述机械臂节点姿态运动的进给速度，以实现所述机械臂节点姿态的平滑过渡。

5、进一步地，所述依据所述路径规划获取路径危险点相应调整所述机械臂节点的速度，包括：

6、基于所述路径危险点，通过四元数获取与其相应的所述机械臂节点在所述路径危险点的安全进给速度；

7、基于所述机械臂节点在所述预设轨迹的起点的实时速度，获取所述机械臂节点在所述预设轨迹的起点和所述路径危险点之间的速度调整起始点；

8、当所述机械臂节点移动至所述速度调整起始点时，控制所述机械臂节点调整进给速度，并在所述路径危险点时调整至所述安全进给速度。

9、进一步地，所述基于所述路径危险点，通过四元数获取与其相应的所述机械臂节点在所述路径危险点的安全进给速度，包括：

10、当所述机械臂节点的移动路径为光滑轨迹时，通过四元数获取所述机械臂节点在所述路径规划中的曲率半径变化趋势，并计算所述安全进给速度，依据所述安全进给速度调整所述机械臂节点的进给速度。

11、进一步地，所述基于所述路径危险点，通过四元数获取与其相应的所述机械臂节点在所述路径危险点的安全进给速度，包括：

12、当所述机械臂节点的移动路径为非光滑轨迹时，基于四元数获取所述机械臂节点在拐弯前后的轨迹切线夹角；

13、依据所述轨迹切线夹角计算所述机械臂节点在所述拐弯处的安全进给速度，并结合所述机械臂节点在所述预设轨迹的起点的实时速度相应调整其速度。

14、进一步地，所述机械臂节点在拐弯前后的轨迹切线夹角的计算公式为：

15、

16、vi＝min(v_front,v_behind,v_included_angle,v_nax)       (2)

17、其中，quaternion_v_front为所述机械臂节点在拐弯前的前一轨迹在末端位置的四元数速度，quaternion_v_behind为所述机械臂节点在拐弯后的后一轨迹在末端位置的四元数速度，v_front轨迹点前段轨迹长度和初速度决定的最大速度，v_behind为轨迹点后段轨迹长度决定的最大速度，v_included_angle为前一轨迹和后一轨迹的夹角决定的最大速度，v_max为最大线速度，vi为所述安全进给速度，i为轨迹端点数。

18、进一步地，所述基于四元数获取所述机械臂节点在拐弯前后的轨迹切线夹角，包括：

19、计算每一姿态点的前一轨迹在所述姿态点的四元数速度和后一轨迹在所述姿态点的四元数速度；

20、计算两个所述四元数速度的夹角；

21、依据两个所述四元数速度的夹角，结合所述前一轨迹长度、所述后一轨迹长度及最大姿态角速度计算所述每一姿态点的姿态角速度。

22、进一步地，所述计算所述每一姿态点的姿态角速度之后，还包括：

23、判断当前所述姿态点是否为最后一个姿态点；

24、如否则计算后一个所述姿态点的姿态角速度；

25、如是判断当前所述姿态点是否受到后一个所述姿态点的速度限制，当由最后一个所述姿态点起，重新计算每一所述姿态点的姿态角速度，至第一个所述姿态点，以得到每一所述姿态点的最优姿态角速度。

26、进一步地，所述控制所述机械臂节点调整进给速度，并在所述路径危险点时调整至所述安全进给速度，包括：

27、当所述机械臂节点在所述预设轨迹的起点的实时进给速度低于所述安全进给速度时，在所述机械臂节点到达所述速度调整起始点后按照第一预设加速度提高所述机械臂节点的进给速度；或，

28、当所述机械臂节点在所述预设轨迹的起点的实时进给速度高于所述安全进给速度时，在所述机械臂节点到达所述速度调整起始点后按照第二预设加速度降低所述机械臂节点的进给速度；或，

29、当所述机械臂节点在所述预设轨迹的起点的实时进给速度等于所述安全进给速度时，在所述机械臂节点到达所述速度调整起始点后保持所述机械臂节点的进给速度。

30、相应地，本发明实施例的第二方面提供了一种基于四元数的机械臂节点姿态速度前瞻控制装置，包括：

31、插补计算模块，其用于在机械臂节点的移动路径上对预设轨迹进行插补计算，得到所述机械臂节点在每一所述预设轨迹的起点后的路径规划；

32、速度控制模块，其用于依据所述路径规划获取路径危险点，通过四元数相应调整所述机械臂节点姿态运动的进给速度，以实现所述机械臂节点姿态的平滑过渡。

33、进一步地，所述速度控制模块包括：

34、安全进给速度获取子模块，其用于基于所述路径危险点，通过四元数获取与其相应的所述机械臂节点在所述路径危险点的安全进给速度；

35、速度调整起始点获取控制子模块，其用于基于所述机械臂节点在所述预设轨迹的起点的实时速度，获取所述机械臂节点在所述预设轨迹的起点和所述路径危险点之间的速度调整起始点；

36、机械臂控制子模块，其用于当所述机械臂节点移动至所述速度调整起始点时，控制所述机械臂节点调整进给速度，并在所述路径危险点时调整至所述安全进给速度。

37、进一步地，所述安全进给速度获取子模块包括：

38、第一安全进给速度获取单元，其用于当所述机械臂节点的移动路径为光滑轨迹时，通过四元数获取所述机械臂节点在所述路径规划中的曲率半径变化趋势，计算所述安全进给速度，依据所述安全进给速度调整所述机械臂节点的进给速度。

39、进一步地，所述安全进给速度获取子模块包括：

40、夹角计算单元，其用于当所述机械臂节点的移动路径为非光滑轨迹时，基于四元数获取所述机械臂节点在拐弯前后的轨迹切线夹角；

41、速度调整单元，其用于依据所述轨迹切线夹角计算所述机械臂节点在所述拐弯处的安全进给速度，并结合所述机械臂节点在所述预设轨迹的起点的实时速度相应调整其速度。

42、进一步地，所述机械臂节点在拐弯前后的轨迹切线夹角的计算公式为：

43、

44、vi＝min(v_front，v_behind,v_included_angle，v_max)        (2)

45、其中，quaternion_v_front为所述机械臂节点在拐弯前的前一轨迹在末端位置的四元数速度，quaternion_v_behind为所述机械臂节点在拐弯后的后一轨迹在末端位置的四元数速度，v_front轨迹点前段轨迹长度和初速度决定的最大速度，v_behind为轨迹点后段轨迹长度决定的最大速度，v_included_angle为前一轨迹和后一轨迹的夹角决定的最大速度，v_max为最大线速度，vi为所述安全进给速度，i为轨迹端点数。

46、进一步地，所述夹角计算单元包括：

47、四元数速度计算子单元，其用于计算每一姿态点的前一轨迹在所述姿态点的四元数速度和后一轨迹在所述姿态点的四元数速度；

48、夹角计算子单元，其用于计算两个所述四元数速度的夹角；

49、姿态角速度计算子单元，其用于依据两个所述四元数速度的夹角，结合所述前一轨迹长度、所述后一轨迹长度及最大姿态角速度计算所述每一姿态点的姿态角速度。

50、进一步地，所述夹角计算单元还包括：

51、姿态点判断子单元，其用于判断当前所述姿态点是否为最后一个姿态点；

52、姿态点控制子单元，其用于在当前所述姿态点不为最后一个姿态点时，计算后一个所述姿态点的姿态角速度；

53、所述姿态点控制子单元还用于在当前所述姿态点为最后一个姿态点时，判断当前所述姿态点是否受到后一个所述姿态点的速度限制，当由最后一个所述姿态点起，重新计算每一所述姿态点的姿态角速度，至第一个所述姿态点，以得到每一所述姿态点的最优姿态角速度。

54、进一步地，所述机械臂控制子模块包括：

55、第一机械臂控制单元，其用于当所述机械臂节点在所述预设轨迹的起点的实时进给速度低于所述安全进给速度时，在所述机械臂节点到达所述速度调整起始点后按照第一预设加速度提高所述机械臂节点的进给速度；或，

56、第二机械臂控制单元，其用于当所述机械臂节点在所述预设轨迹的起点的实时进给速度高于所述安全进给速度时，在所述机械臂节点到达所述速度调整起始点后按照第二预设加速度降低所述机械臂节点的进给速度；或，

57、第三机械臂控制单元，其用于当所述机械臂节点在所述预设轨迹的起点的实时进给速度等于所述安全进给速度时，在所述机械臂节点到达所述速度调整起始点后保持所述机械臂节点的进给速度。

58、相应地，本发明实施例的第三方面提供了一种电子设备，包括：至少一个处理器；以及与所述至少一个处理器连接的存储器；其中，所述存储器存储有可被所述一个处理器执行的指令，所述指令被所述一个处理器执行，以使所述至少一个处理器执行上述基于四元数的机械臂节点姿态速度前瞻控制方法。

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

60、通过对机械臂节点预处理一段插补路径，自动分析轨迹路径，发现并处理路径危险点，对速度轨迹提前规划，实现速度的平滑过渡避免加减速频繁变化。