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

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

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

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

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

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

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

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

其中，F

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

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

其中，x”为x轴方向的无人机加速度，y”为y轴方向的无人机加速度，z”为z轴方向的无人机加速度；x'为x轴方向的无人机速度，y'为y轴方向的无人机速度，z'为z轴方向的无人机速度；k

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

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

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

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

其中，ψ为无人机偏航角，θ为无人机俯仰角，φ为无人机横滚角；[x’,y’,z’]

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

S31：环境设计；

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

其中，S

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

其中，P

P

其中，(x

最终两架四旋翼无人机t时刻获得单步奖励R

S32：智能体设计；

设计内部神经网络，记无人机内部全连接误差反向传播状态-动作对值q的神经网络q(S

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

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

S33：环境-智能体交互模式；

对于每次环境-智能体交互，智能体基于q值神经网络q(S

S34：智能体动作价值迭代；

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

智能体根据

记映射

然后，更新

更新

更新无人机状态，S

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

S41：初始化空域中航空器集合K＝{uav

S42：无人机对的优先级选择；

求得无人机uav

Distance(uav

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

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

选取欧式距离最近的无人机对i，j后，若uav

S44：无人机实现航迹意图判定；

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

其中，S

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

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

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

附图说明

图1为一种四旋翼无人机六自由度快速协同精细编队方法的整体步骤图；

图2为一种四旋翼无人机六自由度快速协同精细编队方法的详细技术路线图；

图3为旋翼无人机空间刚体坐标旋转示意图；

图4为无人机六自由度姿态飞行示意图；

图5为无人机协同编队场景示范；

图6为本发明中无人机协同编队第5s时的效果图；

图7为本发明中无人机协同编队第10s时的效果图；

图8为本发明中无人机协同编队第25s时的效果图；

图9为本发明中无人机协同编队第30s时的效果图；

图10为本发明中无人机协同编队第50s时的效果图。

具体实施方式

下面将结合说明书附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明的保护范围。

如图1所示，一种四旋翼无人机六自由度快速协同精细编队方法，包括以下步骤：

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

构建四旋翼无人机六自由度动力学模型，将四旋翼无人机看作一个中心对称的刚体，则四旋翼无人机在空间中的运动主要为：平动和转动；四旋翼无人机的运动可以看作是无人机中心在地理坐标系下的三维空间运动与无人机本身六自由度旋转运动的组合；

仅考虑无人机平动，有牛顿第二定律：

其中，F

对于既有平动又有转动的无人机的空间运动，可以采用地理坐标系无人机几何中心的运动与无人机飞行状态偏航角、俯仰角、横滚角的欧拉角组合描述，如图2所示；

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

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

其中，x”为x轴方向的无人机加速度，y”为y轴方向的无人机加速度，z”为z轴方向的无人机加速度；x'为x轴方向的无人机速度，y'为y轴方向的无人机速度，z'为z轴方向的无人机速度；k

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

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

步骤S2：构建四旋翼无人机碰撞盒模型

如图3所示，中心对称旋翼无人机几何中心为欧拉旋转中心且旋翼无人机为刚体结构，则在欧拉旋转过程中无人机形状特征始终不变，即无人机各个关键点坐标相对于无人机几何中心的相对位置始终不变；因此，无人机形状关键坐标点在大地坐标系(旧参考系)下的三维坐标描述为：

其中，ψ为无人机偏航角，θ为无人机俯仰角，φ为无人机横滚角；[x’,y’,z’]

步骤S3：基于Kronecker因子信任阈执行者/评论者深度强化学习的智能体训练；具体包括以下步骤：

S31：环境设计

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

其中，S

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

其中，P

P

其中，(x

最终两架四旋翼无人机t时刻获得单步奖励R

S32：智能体设计

设计内部神经网络，记无人机内部全连接误差反向传播q值(状态-动作对价值)神经网络q(S

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

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

S33：环境-智能体交互模式

对于每次环境-智能体交互，智能体基于q值神经网络q(S

S34：智能体动作价值迭代

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

智能体根据

现在，有两个全连接神经网络的权重矩阵W和θ，记映射

然后，更新

更新

更新无人机状态，S

步骤S4：协同编队航迹规划，具体包括：

S41：初始化空域中航空器集合K＝{uav

S42：无人机对的优先级选择

求得无人机uav

Distance(uav

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

S43：无人机对协同航迹规划与无人机群遍历

选取欧式距离最近的无人机对i，j后，若uav

S44：无人机实现航迹意图判定

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

其中，S

如图4所示为无人机六自由度姿态飞行示意图，如图5所示为无人机协同编队场景示范。

如图6-图10所示为无人机协同编队的效果图，可以看出且编队航迹规划结果为带时间戳和经度、纬度、高度的四维航迹，同时航迹粒度达到1s，可以实现安全、高效、快速、精准的无人机群协同编队航迹规划。

以上仅为本发明的实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均包含在申请待批的本发明的权利要求范围之内。