面向无人机编队保持的航路规划方法

1.本发明属于航空航天领域，尤其是涉及一种面向无人机编队保持的航路规划方法。


背景技术：

2.面对复杂的物理环境以及多样化任务背景，相比于单架无人机执行任务，采用无人机编队的方式逐渐凸显其优势。无人机编队保持是编队的重要环节，在许多场景下对于编队保持的要求也越来越高。然而编队在快速转弯时，无人机受自身的动力学约束难以保持期望队形。
3.目前最为实用的编队保持方法是领航跟随法。领航跟随法规定编队中有主机和从机，主机航路由任务系统给定，从机通过控制器控制自身和主机之间的相对位置关系不变，从而保持期望队形。在领航跟随法中，基于队形不变的前提，从机的期望航路是由主机航路决定。领航跟随法侧重于通过控制器设计来提高编队精度，然而却忽略了从机的期望航路是否符合自身的动力学约束。
4.因此领航跟随法主要集中于提升控制器的控制精度，而忽略了期望航路是否可行。所以在主机快速转弯时，从机存在因不满足自身动力学约束而破坏队形的情况。基于此问题，本发明提出了一种面向无人机编队保持的航路规划方法，确保在编队快速转弯时能生成同时满足动力学约束以及队形保持要求的协同航路。


技术实现要素：

5.针对领航跟随法存在的问题，本发明提出了一种面向无人机编队保持的航路规划方法，首先基于编队方向角建立编队模型，构建编队转弯时的最优控制问题，通过伪谱法求解最优控制问题，从而获得能够同时满足动力学约束和队形保持要求的从机航路。
6.本发明提供了一种面向无人机编队保持的航路规划方法，其包括如下步骤：
7.1)定义编队方向角用以描述编队朝向，基于无人机间的相对位置关系和编队方向角建立编队模型；
8.2)将编队方向角和编队方向角速度作为状态变量，将无人机转弯时候的推力和法向加速度作为约束变量，构建最优控制问题；
9.3)基于伪谱法将最优控制问题离散为非线性规划问题，然后对非线性规划问题进行求解，获得编队转弯过程中编队方向角的变化，从而获得满足动力学约束的从机航路。
10.作为本发明的优选方案，所述步骤1)中编队模型具体建立过程如下：
11.首先对于单架无人机采用如下动力学模型：
[0012][0013]
其中(x,y)为无人机的二维坐标，v为无人机的速度，θ为偏航角，g为重力加速度，a为法向加速度，t、d分别为推力和阻力，m为无人机质量，k1和k2为阻力系数，k为诱导阻力系数，ρ为空气密度，c
d0
为零升阻力系数，s为机翼参考面积，无人机控制输入为推力t和法向加速度a；为区分主机和从机的动力学变量，本发明中用下标l代表主机，用下表fi代表第i架从机；
[0014]
编队中指定某架无人机为主机，主机航路由任务系统给定；整个编队队形视为刚体，编队转弯时保持刚体队形不变；针对编队朝向进行单独分析，在二维平面内定义编队方向角为队形刚体的朝向方向与x轴间的夹角，用以描述编队朝向，则主机和从机之间的位置关系为：
[0015][0016]
式(2)中li为主机与第i架从机之间的距离，为主机和第i架从机间连线与编队朝向之间的夹角；式(2)即为编队模型，要想维持队形不变，则li和应均为固定值，因此通过编队方向角θv可以确定从机位置。
[0017]
作为本发明的优选方案，所述步骤2)中，
[0018]
将无人机编队方向角θv和编队方向角速度作为状态变量z1和z2，可得状态方程：
[0019][0020]
式(3)中u为虚拟控制作用；最优控制问题的路径约束为从机在飞行过程中的动力学约束，表现为最大推力限制和偏航角速度限制：
[0021][0022]
式(4)中a
fmax
和t
fmax
分别代表允许的最大法向加速度和最大推力，且式(4)中具体有：
[0023][0024]
式(5)中ω
l
为主机转弯时的角速度大小，并且对于第i架从机速度大小有：
[0025][0026]
在时域[t0,tf]内，令编队方向角尽量与主机偏航角一致，选取性能指标为：
[0027][0028]
在终端tf时刻，编队方向角达到期望值并且编队方向角速度为0，即终端条件为：
[0029][0030]
以上所描述的状态方程、路径约束、性能指标以及终端条件共同构成本发明的最优控制问题。
[0031]
作为本发明的优选方案，所述步骤3)中，基于伪谱法求解最优控制问题的具体过程如下：在建立最优控制问题后，采用自适应radau伪谱法对问题进行求解；
[0032]
首先对最优控制问题进行离散，获得非线性规划问题；采用如下时间变换：
[0033][0034]
从而将时域从[t0,tf]变换至τ∈[τ0,τf]，其中τ0＝-1，τf＝1；再将变换后的时域τ∈[τ0,τf]分为k个网格区间，在每个子网格区间上再对最优控制问题进行离散，从而将编队转弯协同航路规划的最优控制问题转化为非线性规划问题，通过非线性规划求解器求解；
[0035]
非线性规划求解后，需要对每个网格内配点处的最大误差进行判断；如果最大误差大于容许值，那么采用hp自适应方法更新网格。
[0036]
作为本发明的优选方案，所述采用hp自适应方法更新网格，具体步骤为：
[0037]
(1)计算所有配点处的残差，进而获得最大残差与平均残差之比β；
[0038]
(2)判断比值β是否超过允许的最大值，如果超过则采用改变插值多项式的阶次更新网格，否则采用增加配点数的方法更新网格；
[0039]
(3)更新网格后重新对问题进行求解，直到满足误差要求。
[0040]
与现有技术相比，本发明的有益效果包括：
[0041]
(1)定义了编队方向角的概念，基于编队方向角建立编队模型，为求解编队保持问题提供一种新的思路；
[0042]
(2)采用基于编队方向角的编队模型，同时考虑无人机的动力学约束，构建最优控制问题，克服了传统领航跟随法框架下主机快速转弯而导致从机受自身动力学约束而无法保持队形的问题，能够求解出同时满足动力学约束和队形保持要求的从机航路；
[0043]
(3)采用hp自适应radau伪谱法对最优控制进行求解，当误差不满足要求时会自适应更新网格，增加了求解的稳定性，同时求解精度高。
附图说明
[0044]
图1是本发明的编队模型图。
[0045]
图2是伪谱法求解示意图。
[0046]
图3是基于领航跟随法的编队期望航路。
[0047]
图4是本发明技术方案生成的编队期望航路。
[0048]
图5是编队方向角曲线图。
[0049]
图6是编队方向角速度曲线图。
[0050]
图7是虚拟控制作用曲线图。
[0051]
图8为从机法向加速度曲线图。
[0052]
图9为从机推力曲线图。
具体实施方式
[0053]
下面结合具体实施方式对本发明做进一步阐述和说明。所述实施例仅是本公开内容的示范且不圈定限制范围。本发明中各个实施方式的技术特征在没有相互冲突的前提下，均可进行相应组合。
[0054]
本实施例提供的面向无人机编队保持的航路规划方法，具体步骤为：
[0055]
1.定义编队方向角的概念来描述编队朝向，基于无人机间的相对位置关系和编队方向角建立编队模型。
[0056]
2.将编队方向角和编队方向角速度作为状态变量，将无人机转弯时候的推力和法向加速度作为约束变量，构建最优控制问题。
[0057]
3.基于伪谱法首先将最优控制问题离散为非线性规划问题，然后对非线性规划问题进行求解，获得编队转弯过程中编队方向角的变化，从而获得满足动力学约束的从机航路。
[0058]
在本发明的一个具体实施例中，所述步骤1中编队模型具体建立过程如下：
[0059]
对于单架无人机采用如下动力学模型：
[0060][0061]
其中(x,y,v)为无人机的二维坐标和速度，θ为偏航角。g为重力加速度，t、d分别为推力和阻力。m为无人机质量，k1和k2为阻力系数，k为诱导阻力系数，ρ为空气密度，c
d0
为零升阻力系数，s为机翼参考面积。无人机控制输入为推力t和法向加速度a。编队中指定某架无人机为主机，主机航路由任务系统给定，为区分主机和从机的动力学变量，本发明中用下标l代表主机，用下标fi代表第i架从机。
[0062]
整个编队队形可视为刚体，编队转弯时应保持刚体队形不变。传统的领航跟随法将主机的航向作为整个编队的朝向，然而当主机快速转弯时，会存在从机受自身动力学约束而无法保持期望队形的情况。而本发明针对编队朝向进行单独分析，首先在二维平面内定义编队方向角用以描述编队朝向：如图1所示，队形刚体的朝向与x轴间的夹角作为编队方向角θv，则主机和从机之间的位置关系为：
[0063][0064]
式中下标l代表主机，下标fi代表第i架从机，li为主机与第i架从机之间的距离，为主机和第i架从机间连线与编队朝向之间的夹角。要想维持队形不变，则li和应均为固定值，因此通过编队方向角θv可以确定从机位置。
[0065]
所述步骤2中最优控制问题具体建立过程如下：
[0066]
通过控制编队在转弯时编队方向角的变化，能够使从机同时满足队形保持要求以及自身动力学约束。将无人机编队方向角θv和编队方向角速度作为状态变量z1和z2，则状态方程为：
[0067][0068]
式(3)所描述的状态方程可简化表示为：
[0069][0070]
式(3)中u为编队方向角的二阶导，因为编队方向角是抽象出的概念，因此u不是实际物理输入，故称为虚拟控制作用。
[0071]
从机在飞行过程中的动力学约束主要为最大推力限制和偏航角速度限制：
[0072][0073]
其中a
fmax
和t
fmax
分别代表允许的最大法向加速度和推力，a
fi
和t
fi
分别代表第i架从机的法向加速度和推力。主机转弯时角速度大小为ω
l
，可根据式(1)、(2)、(3)可推出：
[0074][0075]
其中：
[0076][0077]
式(5)所描述的路径约束可简化表示为：
[0078]
c(z(t),u(t),t)≤0 (8)
[0079]
在终端时刻，应该满足的约束为：
[0080][0081]
式中为期望编队方向角。满足式(9)第一个等式时说明达到期望的编队方向角，此时编队到达期望位置。
[0082]
对式(2)进行求导，可得：
[0083][0084]
根据式(10)，当满足式(9)第二个等式时此时从机和主机在转弯完成后速度一致。式(9)所描述的终端时刻的边界条件可简化为：
[0085]
b(z(t0),t0,z(tf),tf)＝0 (11)
[0086]
在编队转弯过程中，可令编队方向角与主机偏航角尽量接近，因此可将性能指标选择为：
[0087][0088]
因此，上述共同构成最优控制问题：
[0089][0090]
所述步骤3中基于伪谱法求解最优控制问题的具体过程如下：
[0091]
在建立最优控制问题后，采用自适应radau伪谱法对问题进行求解。首先对最优控制问题进行离散，获得非线性规划问题。采用如下时间变换：
[0092][0093]
从而将时域从[t0,tf]变换至τ∈[τ0,τf]，其中τ0＝-1，τf＝1。变换后的最优控制问题为：
[0094][0095]
将整个时域τ∈[τ0,τf]分再为k个网格区间，在第k(k＝1,...,k)个区间上离散过程如下：
[0096]
基于legendre多项式生成插值节点，对状态变量进行如下拉格朗日插值近似：
[0097][0098]
其中l
i(k)
(τ)为插值基函数。同理，对控制变量进行拉格朗日插值近似，可得：
[0099][0100]
状态变量的导数可近似为：
[0101][0102]
其中将式(18)代入状态方程，可将状态方程离散化为：
[0103][0104]
对性能指标进行近似可得：
[0105][0106]
其中积分权值为：
[0107][0108]
边界条件为：
[0109][0110]
路径约束为：
[0111][0112]
此外，为保证第k个子区间和第k 1个子区间的边界处连续，应满足：
[0113][0114]
至此已将编队转弯协同航路规划的最优控制问题转化为非线性规划问题，进而可通过非线性规划求解器求解。如图2所示，非线性规划求解后，需要对每个网格内配点处的最大误差进行判断。如果最大误差大于容许值，那么采用hp自适应方法更新网格。具体步骤为：
[0115]
(1)计算所有配点处的残差，进而获得最大残差与平均残差之比ρ。
[0116]
(2)判断比值ρ是否超过允许的最大值，如果超过则采用改变插值多项式的阶次更新网格，否则采用增加配点数的方法更新网格。
[0117]
(3)更新网格后重新对问题进行求解，直到满足误差要求。
[0118]
基于i5-1135g7@2.40ghz处理器、64位操作系统、16.0gb内存的计算机上的matlab r2021a平台，对本发明技术方案进行仿真实验。编队队形为三架无人机组成的正三角形，具体仿真参数如下表：
[0119]
[0120][0121]
主机航路由任务系统给定，在仿真实验中假设主机先进行5s的直线飞行，然后向右转弯飞行，转弯角度为最后向左转弯飞行，转弯角度为仿真结果为图3-图9，仿真计算时间为0.97s。图3为基于领航跟随法框架下的编队航路，图4为基于本发明技术方案的-编队航路。将图3与图4进行对比，领航跟随法框架下从机期望航路存在明显的尖点，而依据本发明所生成的从机航路在满足队形保持要求的同时航路曲线较为平滑。图5与图6为编队方向角和编队方向角速度曲线图，图7为虚拟控制作用曲线图，图8为从机的法向加速度曲线图，图9为推力曲线图。通过图8和图9可知，从机航路满足最大法向加速度限制和最大推力限制。综上，基于本发明生成的从机航路能够同时满足队形保持要求以及无人机动力学限制。以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。