一种四旋翼无人机六自由度快速协同精细编队方法

本发明涉及无人机协同航迹规划领域，尤其涉及一种四旋翼无人机六自由度快速协同精细编队方法。

背景技术：

1、无人机编队飞行在军用侦查、突袭、搜捕等协同指挥作战领域应用成熟，近年来随着低空空域管控政策放开以及无人机技术成熟、成本下降，无人机应用逐步由军用扩展至民用领域，特别是无人机编队飞行在民用飞行表演、航空摄影、应急运输等通用航空领域应用广泛。

2、由于无人机编队飞行研究领域的前瞻性和复杂性以及多学科交叉融合等特性，国内外针对无人机编队飞行方法已有一定的进展与成果。严俊坤等公开了一种基于侦察定位任务的无人机编队轨迹优化方法，建立了一种以二维平面坐标系为基础的编队轨迹优化模型，其不足之处在于未考虑无人机z轴方向的运动轨迹，运动模型过于理想化；皇甫雅帆等公开了一种基于虚拟领航的多无人机航迹规划方法，通过人工势场法实现多无人机的航迹协同规划，其优势在于路径平滑安全、易于计算，但人工势场法普遍存在局部最优问题，在复杂环境下可靠性不高，同时在通信能力不足情况下，控制精度较差。

3、因此，现有无人机编队飞行方法存在航迹规划方法运行较为缓慢，所规划航迹精度差，难以进行经度、纬度、高度、时间四维航迹规划等问题；亟需提出一种高效、灵活、精细的无人机群快速协同编队方法。

技术实现思路

1、本发明的目的是克服背景技术中存在的上述缺陷，提供一种四旋翼无人机六自由度快速协同精细编队方法，实现安全、高效、快速、精准的无人机群协同编队航迹规划。

2、本发明为解决上述技术问题采用以下技术方案：

3、一种四旋翼无人机六自由度快速协同精细编队方法，其特征在于，所述方法具体包括以下步骤：

4、步骤s1：构建四旋翼无人机六自由度动力学模型；

5、步骤s2：构建四旋翼无人机碰撞盒模型；

6、步骤s3：基于kronecker因子信任阈执行者/评论者深度强化学习的智能体训练；

7、步骤s4：基于人工智能体进行协同编队航迹规划。

8、进一步的，所述步骤s1构建四旋翼无人机六自由度动力学模型的具体内容为：

9、将四旋翼无人机看作一个中心对称的刚体，则四旋翼无人机在空间中的运动主要为：平动和转动；仅考虑无人机平动，有牛顿第二定律：

10、

11、其中，f合为无人机所受合力，ft为无人机所受升力，fg为无人机所受重力，ff为无人机所受空气阻力，a为无人机加速度，x为无人机位移，m为无人机质量；

12、考虑到欧拉旋转中万向锁的情况以及无人机运动特征，需要避免无人机运行中产生俯仰角为±90度的情况，定义无人机旋转顺序为z轴→y轴→x轴，对应偏航角ψ，俯仰角θ，以及横滚角φ，因此，四旋翼无人机欧拉旋转矩阵re:

13、

14、对于无人机操控系统来说，控制无人机平动和转动的方法是通过调节四个旋翼的转速大小，可得无人机受力平衡方程为：

15、

16、其中，x”为x轴方向的无人机加速度，y”为y轴方向的无人机加速度，z”为z轴方向的无人机加速度；x'为x轴方向的无人机速度，y'为y轴方向的无人机速度，z'为z轴方向的无人机速度；kx为无人机x轴方向空气阻力系数，ky为y轴方向空气阻力系数，kz为z轴方向空气阻力系数；re为四旋翼无人机欧拉旋转矩阵，b为无人机旋翼升力系数，wi为无人机第i个螺旋桨转速，g为重力加速度，u1为无人机旋翼产生升力合力；

17、因此，四旋翼无人机状态转移方程：

18、

19、

20、

21、其中，x为x轴方向的无人机位移，y为y轴方向的无人机位移，z为z轴方向的无人机位移。

22、进一步的，所述步骤s2构建四旋翼无人机碰撞盒模型的具体内容为：

23、无人机形状关键坐标点在大地坐标系下的三维坐标描述为：

24、

25、其中，ψ为无人机偏航角，θ为无人机俯仰角，φ为无人机横滚角；[x’,y’,z’]t为无人机在无人机旋转中心坐标系下的向量坐标，[x0,y0,z0]t为无人机旋转中心相对于大地坐标系原点的位移；表示矩阵叉乘；此时，x，y，z就是无人机关键点坐标在经过位移[x0,y0,z0]t，此时，[x,y,z]t是无人机绕偏航轴、俯仰轴、滚转轴旋转[ψ,θ,φ]后的大地坐标。

26、进一步的，所述步骤s3基于kronecker因子信任阈执行者/评论者深度强化学习的智能体训练的具体步骤可分为：

27、s31：环境设计；

28、设计无人机接近目标编队位置奖励函数为：

29、

30、其中，si,t为第i架无人机第t时刻状态，dpi为第i架无人机目标编队位置；si,t和dpi均为无人机经度x，无人机纬度y，无人机高度h组成的列向量；

31、设计两架无人机间保持安全距离奖励函数为：

32、其中

33、其中，pit为无人机i在第t秒构成关键无人机位置集合，且

34、pit＝{(xit,yit,zit)1,(xit,yit,zit)2,(xit,yit,zit)3,(xit,yit,zit)4,(xit,yit,zit)5}

35、其中，(xit,yit,zit)是四旋翼无人机的五个关键碰撞检测点，即左前旋翼、右前旋翼，左后旋翼、右后旋翼位置；

36、最终两架四旋翼无人机t时刻获得单步奖励rt为：

37、

38、s32：智能体设计；

39、设计内部神经网络，记无人机内部全连接误差反向传播状态-动作对值q的神经网络q(st,at,w)；

40、设计动作选择策略，智能体基于e-greedy策略选择最大q值对应的动作，即：

41、

42、其中，e为一个较小的0-1区间正数；

43、s33：环境-智能体交互模式；

44、对于每次环境-智能体交互，智能体基于q值神经网络q(st,at,w)和e-greedy策略选择动作at，两架四旋翼无人机执行动作at进行状态转移，两架无人机进入下一状态st+1，同时，环境对智能体给出状态st下执行动作at的反馈奖励值rt，供智能体更新内部q值神经网络隐含参数用；

45、s34：智能体动作价值迭代；

46、初始化智能体内部神经网络为任意权重矩阵w以及最优策略估计神经网络，最优策略估计神经网络权重矩阵θ，记最优策略估计为π(θ)；初始化隐含学习率α(θ)，α(w)；初始化折扣因子γ；

47、智能体根据两架无人机状态st做出动作决策at后，获得环境奖励值rt，记u←r+γq(st+1,at,w)；

48、记映射则基于这两个全连接神经网络的kronecker因子近似曲率表达式可以推导出对应fisher矩阵fθ和fw：

49、

50、

51、然后，更新以减小-q(at|st；w)·π(at|st；θ)值；

52、更新以减小[u-q(at|st；w)]·π(at|st；θ)的值；

53、更新无人机状态，st＝st+1，进入下一轮迭代过程；

54、进一步的，所述步骤s4基于人工智能体进行协同编队航迹规划的具体步骤可分为：

55、s41：初始化空域中航空器集合k＝{uav1,uav2,uav3,...,uavn}；

56、s42：无人机对的优先级选择；

57、求得无人机uavi与无人机uavj之间欧式距离distance(uavi,uavj)：

58、distance(uavi,uavj)＝dij＝||[xi,yi]-[xj,yj]||2

59、选取欧式距离最近的无人机对i，j：

60、

61、s43：无人机对协同航迹规划与无人机群遍历；

62、选取欧式距离最近的无人机对i，j后，若uavi∪uavj∈k，则智能体为uavi和uavj分配动作，并从集合k中删除uavi和uavj；若则智能体基于uavj已分配的动作为uavi分配动作，并从集合k中删除uavi。重复上述步骤直到则该时间戳的一轮无人机协同航迹规划完成，进入下一时间戳无人机协同航迹规划进程；

63、s44：无人机实现航迹意图判定；

64、经过t秒的无人机对协同航迹规划与无人机群遍历后，判定无人机是否到达预期的目标编队位置，判定下式：

65、

66、其中，si,t表示无人机i在t秒时的状态，dpi表示无人机i的目标编队位置，si,t和dpi均为无人机经度x，无人机纬度y，无人机高度h组成的列向量。

67、本发明采用以上技术方案与现有技术相比，具有以下有益效果：

68、1.本发明基于四旋翼无人机六自由度灵活性以及关键碰撞节点空间位置追踪，采用基于kronecker因子信任阈执行者/评论者深度强化学习算法训练人工智能体，并通过设计启发式算法降低无人机协同编队复杂度。

69、2.如说明书附图4、5所示，本发明实现25架航空器协同编队仅需0.753秒，且编队航迹规划结果为带时间戳和经度、纬度、高度的四维航迹，同时航迹粒度达到1s，可以实现安全、高效、快速、精准的无人机群协同编队航迹规划。