顶升机构的控制方法及控制系统与流程

本发明涉及机器人，尤其是一种顶升机构的控制方法及控制系统。

背景技术：

1、自主移动机器人（autonomous mobile robot，amr）是一种能够自主导航和执行任务的机器人系统，他们通常配备了传感系统、控制系统和导航系统，以实现自主感知环境、规划路径和避障等功能。

2、自主移动机器人通常配置有顶升机构，可以用于举起货物、调整高度或改变工作平台。现有的自主移动机器人在使用顶升机构时，主要采用传感器（如接近开关、光电传感器等）来防止控制顶升机构减速和防止运行过冲，采用急停按钮在事故发生后紧急停止顶升动作，预防更严重的安全事故，使用固定的加减速度进行速度规划。

3、现有的自主移动机器人太过依赖于传感器的稳定性，安全性不高，一旦传感器损坏会导致顶升机构脱离控制，造成严重的安全事故；并且，单使用急停按钮在事故发生后紧急停止顶升动作，容错率不高；固定的加减速度也会在运动过程中产生冲击和震动（急升急停），影响顶升动作的稳定性和精度。

技术实现思路

1、本发明的目的在于提供一种顶升机构的控制方法及控制系统，以解决现有的机器人安全性低、容错率低等问题。

2、为了达到上述目的，本发明提供了一种顶升机构的控制方法，包括：

3、提供所述顶升机构在每个周期的目标速度；

4、在每个周期内，根据下列判断操作中的一种或几种组合，控制所述顶升机构以相应的所述目标速度运动：

5、判断操作1：判断所述顶升机构在当前周期的实际速度与目标速度的跟随情况是否在预期范围内；

6、判断操作2：判断所述顶升机构在相邻两个周期的目标速度变化率是否在预期范围内，以及判断所述顶升机构在相邻两个周期的实际速度变化率是否在预期范围内；

7、判断操作3：判断所述顶升机构在相邻两个周期的位置变化率是否在预期范围内；

8、判断操作4：判断所述顶升机构在相邻两个周期的位置差值与所述顶升机构在上一周期内的位移估值的跟随情况是否在预期范围内。

9、可选的，所述判断操作1、判断操作2、判断操作3和判断操作4依次进行。

10、可选的，所述判断操作2、判断操作3和判断操作4依次进行。

11、可选的，根据所述顶升机构的初始位置、初始实际速度、目标位置、最大目标速度及加减速参数，规划出每个周期的所述目标速度。

12、可选的，利用s形加减曲线算法规划出每个周期的所述目标速度。

13、可选的，所述判断操作1的判断方法包括：当时，判定所述顶升机构在当前周期的实际速度与目标速度的跟随情况在预期范围内；当时，判定所述顶升机构在当前周期的实际速度与目标速度的跟随情况不在预期范围内，进行报错；

14、其中，为当前周期对应的所述目标速度，为所述顶升机构在当前周期的实际速度，为第一波动阈值。

15、可选的，所述判断操作2的判断方法包括：当时，判定所述顶升机构在相邻两个周期的目标速度变化率在预期范围内；当时，判定所述顶升机构在相邻两个周期的目标速度变化率不在预期范围内，进行报错；以及，

16、当时，判定所述顶升机构在相邻两个周期的实际速度变化率在预期范围内；当时，判定所述顶升机构在相邻两个周期的实际速度变化率不在预期范围内，进行报错；

17、其中，为当前周期对应的所述目标速度，为上一周期对应的所述目标速度，为所述顶升机构在当前周期的实际速度，为所述顶升机构在上一周期的实际速度，为第二波动阈值，为第三波动阈值。

18、可选的，所述判断操作3的判断方法包括：当时，判定所述顶升机构在相邻两个周期的位置变化率在预期范围内；当时，判定所述顶升机构在相邻两个周期的位置变化率不在预期范围内，进行报错；

19、其中，为所述顶升机构在当前周期的位置，为所述顶升机构在上一周期的位置，为第四波动阈值。

20、可选的，所述判断操作4的判断方法包括：当时，判定所述顶升机构在相邻两个周期的实际位置差值与所述顶升机构在上一周期内的位移估值的跟随情况在预期范围内；

21、当时，判定所述顶升机构在相邻两个周期的实际位置差值与所述顶升机构在上一周期内的位移估值的跟随情况不在预期范围内，进行报错；

22、其中，为所述顶升机构在当前周期的位置，为所述顶升机构在上一周期的位置，为所述顶升机构在上一周期内的位移估值，为第五波动阈值。

23、可选的，根据如下方法获取所述位移估值：

24、；

25、其中，为所述顶升机构在当前周期的实际速度，为所述顶升机构在上一周期的实际速度，为一个周期对应的时间。

26、可选的，每个周期还进行下列判断操作中的一种或几种组合：判断急停标志是否置位操作、判断上限位标志是否置位操作及判断下限位标志是否置位操作。

27、可选的，所述判断急停标志是否置位操作、所述判断上限位标志是否置位操作及所述判断下限位标志是否置位操作发生在每个周期驱动所述顶升机构工作之前的任意时刻。

28、可选的，当判定所述急停标志置位、所述上限位标志置位且所述目标速度的方向向上、所述下限位标志置位且所述目标速度的方向向下中的任一种情况时，进行报错。

29、可选的，所述周期为所述顶升机构的控制程序的运行周期。

30、可选的，进行报错之后，执行下列控制操作中的至少一种：

31、控制操作1：控制所述顶升机构的速度降为零；

32、控制操作2：发送所述顶升机构的电机停止运行的命令。

33、本发明还提供给了一种顶升机构的控制系统，包括：

34、速度规划模块，用于提供所述顶升机构在每个周期的目标速度；

35、安全处理模块，根据下列判断操作中的一种或几种组合，控制所述顶升机构以相应的所述目标速度运动：

36、判断操作1：判断所述顶升机构在当前周期的实际速度与目标速度的跟随情况是否在预期范围内；

37、判断操作2：判断所述顶升机构在相邻两个周期的目标速度变化率是否在预期范围内，以及判断所述顶升机构在相邻两个周期的实际速度变化率是否在预期范围内；

38、判断操作3：判断所述顶升机构在相邻两个周期的位置变化率是否在预期范围内；

39、判断操作4：判断所述顶升机构在相邻两个周期的位置差值与所述顶升机构在上一周期内的位移估值的跟随情况是否在预期范围内。

40、可选的，所述安全处理模块依次进行所述判断操作1、判断操作2、判断操作3和判断操作4。

41、可选的，所述安全处理模块依次进行所述判断操作2、判断操作3和判断操作4。

42、可选的，所述速度规划模块接收所述顶升机构的初始位置、初始实际速度、目标位置、最大目标速度及加减速参数，并规划出每个周期的所述目标速度。

43、可选的，利用s形加减曲线算法规划出每个周期的所述目标速度。

44、可选的，所述安全处理模块按照如下方法进行所述判断操作1：

45、当时，判定所述顶升机构在当前周期的实际速度与目标速度的跟随情况在预期范围内；当时，判定所述顶升机构在当前周期的实际速度与目标速度的跟随情况不在预期范围内，所述安全处理模块进行报错；

46、其中，为当前周期对应的所述目标速度，为所述顶升机构在当前周期的实际速度，为第一波动阈值。

47、可选的，所述安全处理模块按照如下方法进行所述判断操作2：

48、当时，判定所述顶升机构在相邻两个周期的目标速度变化率在预期范围内，当时，判定所述顶升机构在相邻两个周期的目标速度变化率不在预期范围内，所述安全处理模块进行报错；以及，

49、当时，判定所述顶升机构在相邻两个周期的实际速度变化率在预期范围内；当时，判定所述顶升机构在相邻两个周期的实际速度变化率不在预期范围内，所述安全处理模块进行报错；

50、其中，为当前周期对应的所述目标速度，为上一周期对应的所述目标速度，为所述顶升机构在当前周期的实际速度，为所述顶升机构在上一周期的实际速度，为第二波动阈值，为第三波动阈值。

51、可选的，所述安全处理模块按照如下方法进行所述判断操作3：

52、当时，判定所述顶升机构在相邻两个周期的位置变化率在预期范围内；当时，判定所述顶升机构在相邻两个周期的位置变化率不在预期范围内，所述安全处理模块进行报错；

53、其中，为所述顶升机构在当前周期的位置，为所述顶升机构在上一周期的位置，为第四波动阈值。

54、可选的，所述安全处理模块按照如下方法进行所述判断操作4：

55、当时，判定所述顶升机构在相邻两个周期的实际位置差值与所述顶升机构在上一周期内的位移估值的跟随情况在预期范围内；

56、当时，判定所述顶升机构在相邻两个周期的实际位置差值与所述顶升机构在上一周期内的位移估值的跟随情况不在预期范围内，所述安全处理模块进行报错；

57、其中，为所述顶升机构在当前周期的位置，为所述顶升机构在上一周期的位置，为所述顶升机构在上一周期内的位移估值，为第五波动阈值。

58、可选的，根据如下方法获取所述位移估值：

59、；

60、其中，为所述顶升机构在当前周期的实际速度，为所述顶升机构在上一周期的实际速度，为一个周期对应的时间。

61、可选的，所述安全处理模块在每个周期还进行下列判断操作中的一种或几种组合：判断急停标志是否置位操作、判断上限位标志是否置位操作及判断下限位标志是否置位操作。

62、可选的，所述判断急停标志是否置位操作、所述判断上限位标志是否置位操作及所述判断下限位标志是否置位操作发生在每个周期驱动所述顶升机构工作之前的任意时刻。

63、可选的，当判定所述急停标志置位、所述上限位标志置位且所述目标速度的方向向上、所述下限位标志置位且所述目标速度的方向向下中的任一种情况时，所述安全处理模块进行报错。

64、可选的，进行报错之后，执行下列控制操作中的至少一种：

65、控制操作1：控制所述顶升机构的速度降为零；

66、控制操作2：发送所述顶升机构的电机停止运行的命令。

67、在本发明提供的顶升机构的控制方法中，首先提供所述顶升机构在每个周期的目标速度，然后在每个周期内，对所述顶升机构的实际速度与目标速度的跟随情况、相邻两个周期的目标速度变化率、实际速度变化率、位置变化率、位置差值与上一周期内的位移估值的跟随情况进行监测，并判断监测结果是否在预期范围内，根据一种或多种判断操作控制所述顶升机构以相应的所述目标速度运动，从而实现对所述顶升机构的精准控制，确保其运动轨迹和速度符合控制要求，减少因速度或位移不准确带来的时间浪费，提高生产效率；同时，对所述顶升机构的速度和位置进行跟随监测可以及时发现异常情况并采取相应措施，而非只依赖于所述顶升机构的传感系统的稳定性，增加了容错率，提高了设备运行的安全性和稳定性；并且，可以根据实际需要动态调整预期范围，对所述顶升机构的调节更加精细化和灵活，满足不同工作场景的需求。相应的，本发明还提供了一种顶升机构的控制系统。