一种基于状态机的导航任务的调度方法、系统和存储介质与流程

本发明涉及导航任务调度领域，更具体地，涉及一种基于状态机的导航任务的调度方法、系统和存储介质。

背景技术：

1、随着科学技术的飞速进步，导航技术是到自动驾驶汽车、物流机器人、无人机等领域的核心技术之一。传统的导航系统在任务切换的过程中，由于对不同的事件处理会产生不同的结果，产生处理冲突的问题。

技术实现思路

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、若是，则获取并记录所述第一坐标信息，结束当前状态机的状态，并将全局规划状态放入状态队列。

39、本发明第二方面提供了一种基于状态机的导航任务的调度系统，包括基于状态机的导航任务的调度方法程序，所述基于状态机的导航任务的调度方法程序被所述处理器执行时实现如下步骤：

40、进入全局规划状态，获取第一坐标信息和第二坐标信息；

41、根据所述第一坐标信息、第二坐标信息和预设的地图信息，得到第一路径信息；

42、根据所述第一路径信息，确定第一旋转角度信息；

43、根据所述第一旋转角度信息调整运动朝向；

44、获取第三坐标信息，根据所述第三坐标信息和所述第一路径信息，发送第一角速度控制指令；

45、判断所述第三坐标信息是否与所述第二坐标信息相同；

46、若是，则根据第一路径信息设置终点朝向，并发送第二角速度控制指令。

47、本方案中，还包括：

48、当有限状态机被唤醒后，进入全局规划状态时；

49、根据坐标信息和预设的地图信息，启动路径规划；

50、判断是否得到所述第一路径信息；

51、若是，则将旋转到起点状态放入状态队列；

52、若否，则获取第一时间信息；

53、判断所述第一时间信息是否超过预设的时间阈值；

54、若是，则将恢复行为状态放入状态队列。

55、本方案中，还包括：

56、当有限状态机被唤醒后，进入旋转到起点状态时；

57、获取第一角度信息，并根据所述第一路径信息确定第三坐标信息；

58、根据所述第一坐标信息和所述第三坐标信息，得到第二角度信息；

59、根据所述第一角度信息和所述第二角度信息，确定所述第一旋转角度信息；

60、根据所述第一旋转角度信息发送第三角速度控制指令，实现原地转向，用以调整运动朝向。

61、本发明第三方面提供了一种计算机可读存储介质，所述计算机可读存储介质中包括一种基于状态机的导航任务的调度方法程序，所述基于状态机的导航任务的调度方法程序被处理器执行时，实现如上述任一项所述的基于状态机的导航任务的调度方法的步骤。

62、本发明提供了一种基于状态机的导航任务的调度方法、系统和存储介质，首先，进入全局规划状态，根据第一坐标信息和第二坐标信息对路径进行规划；其次，进入旋转到起始点状态，根据所规划的路径实施原地旋转至路径起始点方向；然后，进入局部控制状态，根据第三坐标信息和所规划的路径对行进速度进行调整；最后，进入旋转到目标点状态，抵达目标位置后通过原地旋转至目标点方向；此外，还设置有减速缓停状态和恢复行为状态，通过减速运行至停止保持运动姿态稳定以及通过恢复行为达到脱困的目的，提高导航运行的稳定性。

1.一种基于状态机的导航任务的调度方法，其特征在于，所述方法包括：

2.根据权利要求1所述的一种基于状态机的导航任务的调度方法，其特征在于，还包括：

3.根据权利要求1所述的一种基于状态机的导航任务的调度方法，其特征在于，还包括：

4.根据权利要求2所述的一种基于状态机的导航任务的调度方法，其特征在于，所述恢复行为状态，具体为：

5.根据权利要求1所述的一种基于状态机的导航任务的调度方法，其特征在于，还包括：

6.根据权利要求1所述的一种基于状态机的导航任务的调度方法，其特征在于，还包括：

7.一种基于状态机的导航任务的调度系统，其特征在于，所述系统包括存储器和处理器，所述存储器中包括基于状态机的导航任务的调度方法程序，所述基于状态机的导航任务的调度方法程序被所述处理器执行时实现如下步骤：

8.根据权利要求7所述的一种基于状态机的导航任务的调度系统，其特征在于，还包括：

9.根据权利要求7所述的一种基于状态机的导航任务的调度系统，其特征在于，还包括：

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机可读存储介质中包括一种基于状态机的导航任务的调度方法程序，所述基于状态机的导航任务的调度方法程序被处理器执行时，实现如权利要求1至6中任一项所述的基于状态机的导航任务的调度方法的步骤。