基于同频异步双线程的机器人运动控制方法及相关装置

本技术涉及机器人运动控制，尤其涉及基于同频异步双线程的机器人运动控制方法及相关装置。

背景技术：

1、随着机器人技术的快速发展，机器人能够在台阶、碎石、陡坡等非结构化环境中稳定运动，因此，对机器人的运动控制系统的性能要求日益提高。

2、目前，现有技术中该方法将强化学习策略网络部署于机器人控制器中，将推理网络部署于机器人控制器中，按照固定控制周期依次执行传感器数据读取、模型推理计算和电机指令发送等，最后将动作指令发送至各关节电机驱动器以驱动电机运转。

3、然而，现有技术按照串行架构执行对机器人的运动控制，当传感器数据量增大或模型推理计算量增加时，多个数据处理任务在单一执行流程中需要排队等待，并且推理任务耗时存在不确定性，易引发数据拥堵，无法保证控制频率，使得机器人在实际应用中运动控制的稳定性下降。

技术实现思路

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、此外，通过线程间共享数据区域使得机器人动作预测数据由推理线程传递至控制线程，并使机器人执行机构反馈的运动控制反馈数据能够由控制线程更新至线程间共享数据区域，以供推理线程进行后续运动状态推理，从而形成机器人运行状态数据、运动控制模型推理结果以及运动控制反馈数据之间的连续交互，提高运动控制模型对机器人运行状态变化的响应能力。