轨迹跟踪控制方法及装置与流程

本发明涉及飞行控制，尤其涉及一种轨迹跟踪控制方法及装置。

背景技术：

1、在多旋翼飞行器领域，系统响应延迟对控制性能的影响较大，目前常用的轨迹跟踪控制算法为比例-积分-微分控制(proportion integration differentiation，pid)和模型预测控制(model predictive control，mpc)，pid控制利用系统误差的比例、积分和微分计算出控制量，但是这种控制将当前时刻的轨迹点作为目标信息，忽略了系统的响应时延，从而调节滞后，使得轨迹跟踪误差较大，mpc通过求解优化问题得到最优的控制序列，但是求解基于模型的优化问题需要较多的计算资源，不适用于控制频率和实时性要求较高的系统。

2、上述内容仅用于辅助理解本发明的技术方案，并不代表承认上述内容是现有技术。

技术实现思路

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、在本发明中，通过获取无碰撞轨迹点集，根据当前飞行速度，在无碰撞轨迹点集中确定目标轨迹点，将当前飞行位置映射到无碰撞轨迹点集中，得到投影轨迹点，根据投影轨迹点与目标轨迹点，确定轨迹弯曲度，在轨迹弯曲度大于预设弯曲度阈值时，对目标轨迹点的速度进行动态调节，得到目标速度，基于投影轨迹点与平滑后的目标速度，生成平滑速度指令，基于平滑速度指令控制飞行器进行轨迹跟踪。相较于使用比例-积分-微分控制方式，忽略响应时延使得轨迹跟踪误差较大，本发明能够在规划输出的轨迹点集中选择合适的目标点，并对选点的速度进行动态调节和平滑处理，考虑系统时延和飞行平稳性，保证飞行器精确地跟踪轨迹，提高跟踪精度，相较于使用模型预测控制方式难以达到实时性要求，本发明能够在计算资源有限且高速飞行的多旋翼飞行器上实时运行，对控制指令进行平滑处理，确保飞行的过程中姿态平稳。