CN111077902A - 基于虚拟结构的多翼伞协同编队控制方法和控制系统 - Google Patents

Abstract

本发明公开了一种基于虚拟结构的多翼伞协同编队控制方法和控制系统。其中控制方法包括：1、确定编队虚拟结构以及参考点；2、计算每个翼伞在虚拟结构中相对于参考点的期望位置；3、规划虚拟结构参考点的归航航迹；获取虚拟结构中参考点的航向角速度、速度、航向角、下滑角和位置；4、计算惯性坐标系下每个时刻每个翼伞的期望位置；5、计算当前时刻t的编队位置误差；6、计算当前时刻t每个翼伞的制导参数；对于未着陆翼伞，循环执行步骤5和6，直到所有翼伞都着陆。该方法将编队队形定义为虚拟结构，该结构沿规划航迹运动，各翼伞只需要跟踪虚拟结构上相应的期望位置即可形成编队，由此实现多翼伞系统的协同控制。

Description

基于虚拟结构的多翼伞协同编队控制方法和控制系统

技术领域

本发明属于多翼伞系统协同控制技术领域，具体为多个翼伞利用虚拟结构实现协同控制的方法和系统。

背景技术

相比传统圆形伞，翼伞的特点是具有滑翔能力、可控性和大载荷比，它能够通过调节翼伞操纵绳，实现转弯、滑翔等动作，从而控制翼伞的飞行方向和速度，实现精确空投；同时，翼伞在着陆阶段可以通过下拉伞衣后缘模仿鸟类的落地动作，实现翼伞的平稳逆风软着陆，因此用翼伞替换不可控的传统圆形伞，实现大量物资的运输，在近年来引起了越来越多研究者的兴趣。

实现大量物资的空投，目前主要有两种思路，一种思路是设计重载翼伞，通过提高翼伞的载荷能力实现大量物资的空投，这种重载翼伞的面积一般较大，目前载荷能力最高已经达到11吨左右，但这基本上已达到上限，想要进一步提升载荷能力已经很困难；同时更大的翼伞需要更大容量的运输机机舱，这对运输机提出了更高的要求；另外，大型翼伞还涉及到分阶段开伞、卷帆等复杂情况，其空投操作比较复杂。

另一种思路是多翼伞空投，通过同时空投多个中型翼伞，用多个成本较低的翼伞同样可以实现大量物资的空投。这种方式具有更好的容错性能，即使一具翼伞损毁或者失败，剩下的翼伞仍然可以继续将载荷空投到目标点，增加了空投任务的成功可能性。这种空投方式的可扩展性较好，如果有更多的物资需要空投，只要增加空投翼伞的数量即可，因此对多翼伞空投的研究，具有越来越重要的战略意义。

多个翼伞在空投过程中需要从分散状态逐渐聚拢到一起，并以编队的形式向目标点飞行，可以降低着陆散布，同时避免在飞行过程中发生互相碰撞事故。目前常用的编队方法主要有领导者-跟随者法、基于行为的方法。在领导者-跟随者法中，领导者发送信息到跟随者，跟随者在一定距离内跟随领航者，由于其简便易行而被广泛采用，基于这种方法已设计出多种协同编队控制器，在仿真实验和飞行验证方面都取得了很多研究成果，该方法缺点是对领导者的鲁棒性较差，容易产生单点故障问题，一旦领导者发生故障，整个编队就群龙无首，导致编队失败。基于行为法一般是通过预先定义一组智能体的基本行为，如避免碰撞、避免障碍物、搜索目标、队形保持、轨迹跟踪等，通过对这些基本行为求加权平均值，可求得智能体的控制行为，行为法的难点在于各种基本行为的设计，以及基本行为权重的确定，此外由于该方法难于用数学描述，导致相应的理论分析及稳定性证明难于实现。

发明内容

发明目的：针对现有技术中的缺点，本发明公开了一种基于虚拟结构的多翼伞协同编队控制方法，该方法将编队队形定义为虚拟刚体结构，虚拟刚体结构沿规划好的航迹运动，翼伞个体只需要跟踪虚拟结构刚体上相应的期望位置即可形成编队，由此实现多翼伞系统的协同航迹控制。

技术方案：本发明一方面公开了一种基于虚拟结构的多翼伞协同编队控制方法，包括：

(1)根据期望的编队队形确定编队虚拟结构，并在虚拟结构中确定参考点；

(2)计算每个翼伞在虚拟结构中相对于参考点的期望位置

(3)规划虚拟结构参考点的归航航迹Fp＝{path(m)}，path(m)为归航航迹上的第m个航迹点，m＝1,…,M，M为航迹点总数；获取虚拟结构中参考点的航向角速度ω_f(t)、速度V_f(t)、航向角

和下滑角γ_f(t)，以及虚拟结构参考点的位置(x_f(t),y_f(t),z_f(t))^T；

(4)计算惯性坐标系下每个时刻每个翼伞的期望位置，其中第i个翼伞在时刻t的期望位置

为：

(5)计算当前时刻t的编队位置误差

其中R_if(t)＝(x_if(t),y_if(t),z_if(t))^T为第i个翼伞在时刻t时在虚拟结构编队坐标系下的位置；

(6)计算当前时刻t每个翼伞的制导参数：

其中V_i(t)、

γ_i(t)分别为t时刻第i个翼伞的速度、航向角和下滑角，k₁、k₂、k₃分别为第一控制增益系数、第二控制增益系数、第三控制增益系数；

对于没有着陆的翼伞，循环执行步骤5和6，直到所有翼伞都着陆。

另一方面，本发明公开了一种实现上述控制方法的多翼伞协同编队控制系统，包括：

虚拟结构生成模块，用于根据期望的编队队形确定编队虚拟结构，并在虚拟结构中确定参考点；

相对期望位置生成模块，用于计算每个翼伞在虚拟结构中相对于参考点的期望位置；

虚拟结构归航航迹规划模块，用于规划虚拟结构参考点的归航航迹，获取虚拟结构中参考点的航向角速度ω_f(t)、速度V_f(t)、航向角

和下滑角γ_f(t)，以及虚拟结构参考点的位置(x_f(t),y_f(t),z_f(t))^T；

多翼伞编队轨迹生成模块，用于计算惯性坐标系下每个时刻每个翼伞的期望位置；

多翼伞编队位置误差计算模块，用于实时计算编队中每个翼伞的位置误差；

多翼伞编队制导模块，用于实时计算编队中每个翼伞的制导参数，所述制导参数为每个翼伞的速度、航向角和下滑角。

有益效果：本发明公开的多翼伞协同编队控制方法和控制系统具有以下优点：1、虚拟结构法不存在实际的领航者，解决了领导者-跟随者编队方式的单点故障问题；2、将整个编队看成一个整体，容易指定编队的整体行为，简化了任务的描述；3、每个个体只需要跟踪虚拟结构上对应的点即可，因此易于个体控制策略的实现。

附图说明

图1为本发明公开的多翼伞协同编队控制方法的流程图；

图2为实施例1中的多翼伞期望编队队形示意图；

图3为实施例1中分段归航规划的水平投影示意图；

图4为虚拟结构的编队队形的示意图；

图5为多翼伞编队的运动轨迹在水平面和三维空间的效果图；

图6为编队过程中的队形误差曲线图；

图7为翼伞之间的间距曲线图；

图8为本发明公开的多翼伞协同编队控制系统的组成示意图。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明。

实施例一：

本实施例以6个翼伞组成三角形编队为例来详述本发明公开的基于虚拟结构的多翼伞协同编队控制方法，流程如图1所示，包括：

步骤1、根据期望的编队队形确定编队虚拟结构，并在虚拟结构中确定参考点；

本实施例中设定的期望虚拟结构编队队形为三角形，虚拟结构参考点坐标位于(0,0,0)，形成的三角形队形如图2所示。

步骤2、计算每个翼伞在虚拟结构中相对于参考点的期望位置

根据设定的虚拟结构和参考点，可以得到6个翼伞的相对位置矢量为：

可以看到翼伞之间的间距有两个值，一个是120米，另一个是

米，即翼伞之间的间距最小为

米，约等于84.9米。

步骤3、规划虚拟结构参考点的归航航迹Fp＝{path(m)}，path(m)为归航航迹上的第m个航迹点，m＝1,…,M，M为航迹点总数；获取虚拟结构中参考点的航向角速度ω_f(t)、速度V_f(t)、航向角

和下滑角γ_f(t)，以及虚拟结构参考点的位置(x_f(t),y_f(t),z_f(t))^T；

本实施例采用分段归航规划虚拟结构参考点的归航航迹，包括步骤：

(3.1)如图3所示，将从空投释放点path(1)(点A)到着陆目标点path(M)(点O)间的归航航迹分为：第一转弯过渡段、向心飞行段、第二转弯过渡段、能量控制段、第三转弯过渡段、逆风对准着陆段；

虚拟结构的参考点在空投释放点A以初始航向角

空投后，开始向目标点转向，经过一段弧度为β₁的过渡转弯后，开始由B点进入向心飞行段，此时虚拟结构以滑翔飞行为主，到C点处虚拟结构退出向心飞行段。从向心飞行BC段到能量控制DE段需要一段过渡转弯，转弯弧度为β₂，之后从进入点(Entry Point)D正式转换到能量控制段，进入点D的参数为(R_EP,θ_EP)，其中，R_EP为能量控制段的转弯半径，θ_EP为进入点D的弧角，能量控制段的主要任务是通过转弯飞行盘旋削高，如果虚拟结构高度合适，在经过弧度为β₃的圆弧转弯后即可从E点退出能量管理段，如果高度过高，此时还需要再多绕几圈才能消耗掉多余高度，或者增大能量管理段的半径消耗多余高度。从E点退出能量管理段后，经过一段弧度为β₄的过渡转弯即进入最终着陆段，然后从F点起虚拟结构开始逆风飞行，假定风场的方向沿x轴正向，则虚拟结构将逆风飞往着陆点，并最终逆风着陆到目标点O。A到B为第一转弯过渡段、B到C为向心飞行段、C到D为第二转弯过渡段、D到E为能量控制段、E到F为第三转弯过渡段、F到O为逆风对准着陆段。

利用上述各分段的几何位置关系，可将分段归航航迹的优化问题转化为进入点D的参数优化问题。确定整个分段航迹的关键是进入点(Entry Point)D的参数(R_EP,θ_EP)，R_EP的值一方面要比最小转弯半径大，以满足翼伞对操纵绳单侧下拉量的控制约束，另一方面R_EP也不能过大，以免影响着陆精度。

(3.2)通过优化进入点参数，即可得到优化的分段航迹，分段航迹的优劣可由优化目标函数反映，本发明采用如下目标函数，其优化目标是使归航轨迹的实际着陆点与目标点的水平偏差最小，且翼伞能逆风着陆。

目标函数为：

其中R_min为规划航迹能量控制段的转弯半径，

为向心飞行段的滑翔长度，

为逆风对准着陆段的滑翔长度，z₀(0)/|tanγ₀(0)|为规划航迹初始投放高度对应的水平飞行距离，γ₀(0)为规划航迹初始下滑角；

通过最小化目标函数J获取能量控制段的转弯半径R_EP和进入点弧角θ_EP；

在进入点参数确定的情况下，利用上述几何关系，即可将分段归航轨迹的设计问题转换为目标函数的参数优化问题，最终通过选择合适的优化算法得到分段归航航迹。模拟退火算法对初值不敏感，且可有效地避免算法陷入局部最优，因此本实施例采用模拟退火算法最小化目标函数，得到能量控制段的转弯半径R_EP和进入点弧角θ_EP。

模拟退火算法的基本实现流程如下：首先，给待求解问题一个初始解，同时将温度值设定为一个较高值；然后给当前解一个随机扰动，产生一个新的解，并比较前后两个解的能量变化来判断是否接受该新解。对于使得能量减少的解，算法直接接受，对于使得能量增加的解，则在高温时以大概率接受，以跳出局部最小区域，后期随着温度的下降，降低接受的可能性，直到不再接受，此时得到优化解，其中能量函数由目标函数确定。

(3.3)计算分段归航每一段的参数：

第一转弯过渡段的圆心O₁的位置为：

其中(x₀(0),y₀(0),z₀(0))为虚拟结构参考点的初始投放位置，

为初始航向角；

第二转弯过渡段的圆心O₂的位置为：

向心飞行段的滑翔长度

的长度为||O₁O₂||；

第一转弯过渡段的转弯角度β₁为：

第二转弯过渡段的转弯角度β₂为：

能量控制段圆弧的圆心角β₃为：

第三转弯过渡段的转弯角度β₄为：

对分段归航航迹的每一段进行采样，所有采样点按序构成归航航迹Fp＝{path(m)}。

当给定了虚拟结构在每一段的速度、航向角和下滑角等限制条件后，可以得到每个航迹点处的航向角速度以及线速度，根据归航航迹上航迹点之间的转弯角度、路径长度，以及“时间＝路径长度/线速度、时间＝转弯角度/角速度”，可获取虚拟结构中参考点的航向角速度、速度、航向角和下滑角，以及虚拟结构参考点的位置与时间的关系，分别记为：虚拟结构中参考点的航向角速度ω_f(t)、速度V_f(t)、航向角

和下滑角γ_f(t)，以及虚拟结构参考点的位置(x_f(t),y_f(t),z_f(t))^T。

步骤4、根据编队坐标系与惯性坐标系之间的转换关系，计算惯性坐标系下每个时刻每个翼伞的期望位置，其中第i个翼伞在时刻t的期望位置

为：

将期望编队在同一时刻的位置连线，即可绘制出虚拟结构的编队队形，可以看到在沿规划轨迹运动的过程中，虚拟结构的队形一直保持为三角形，如图4所示。

步骤5、计算当前时刻t的编队位置误差

其中R_if(t)＝(x_if(t),y_if(t),z_if(t))^T为第i个翼伞在时刻t时在虚拟结构编队坐标系下的位置；

步骤6、编队制导律是为了保证每个翼伞都跟踪上对应的虚拟结构点。本发明利用翼伞当前位置和期望位置的误差设计制导律，将翼伞调整到期望队形位置，达到编队协同控制的目的。

首先建立虚拟结构参考点的运动学模型。

设R_i＝(x_i,y_i,z_i)^T、R_f＝(x_f,y_f,z_f)^T分别为翼伞i和虚拟结构参考点在惯性坐标系O_xyz下的坐标位置矢量，R_if＝(x_if,y_if,z_if)^T为翼伞i在虚拟结构编队坐标系下的相对位置矢量，

为虚拟结构编队坐标系下翼伞的期望相对位置矢量，编队队形可由一组相对位置矢量

确定，N为待编队翼伞的总数，若

则翼伞可形成期望的编队队形。

R_i、R_f和R_if之间满足如下矢量关系：

R_if＝R_i-R_f (9)

对上式关于时间求导，并将位置误差转换至编队坐标系下，可以得到编队坐标系下位置误差的运动学模型：

其中上标d表示期望值，航向角误差

编队位置误差

V_i cosγ_i为翼伞i在水平面的速度，V_f cosγ_f为虚拟参考点在水平面的速度。多翼伞系统的虚拟结构编队制导问题可总结为，在给定一组参考信号

且系统存在初始误差[x_eif(0)y_eif(0)z_eif(0)]^T的情况下，通过求解每个翼伞的制导指令V_i、

和γ_i，使得式(10)的闭环轨迹收敛到0，即

考虑到编队保持的最终目标是设置制导律使得(x_eif y_eif z_eif)→0，因此本发明基于Lyapunov稳定性原理推导翼伞的制导指令，定义的Lyapunov函数如下：

对上式求导，并将式(10)代入，可得

当李雅普诺夫函数的导数小于0时，编队误差将趋于0，因此令

其中k₁、k₂和k₃都大于0，代入(12)式可得：

求解(13)式可得：

即t时刻每个翼伞的制导参数为：

其中V_i(t)、

γ_i(t)分别为t时刻第i个翼伞的速度、航向角和下滑角，k₁、k₂、k₃分别为第一控制增益系数、第二控制增益系数、第三控制增益系数；本实施例中，取值为：k₁＝0.4、k₂＝0.5和k₃＝0.5。

对于没有着陆的翼伞，循环执行步骤5和6，直到所有翼伞都着陆。

图5-(a)和图5-(b)分别为多翼伞编队的运动轨迹在水平面和三维空间的效果图。从图5-(a)中可以直观的看到，初始时刻，各翼伞离各自的参考点有一定距离，此时误差较大，翼伞之间没有形成编队，但随着制导指令的作用，翼伞逐渐跟踪上了参考点，到向心飞行段的末端，基本上形成了三角形编队，在能量管理段，实际编队与虚拟结构几乎重合。因此本发明公开的虚拟结构制导律，可以使得翼伞准确跟踪虚拟结构的航迹，实现多个翼伞的编队飞行。需要指出的是，在从能量管理段到最终着陆段转换的过程中，由于翼伞要实现从转弯飞行向直线滑翔的转换，此时实际编队与虚拟结构的位置误差较大，但完成转换之后，编队恢复正常，并逆风着陆到目标点，仿真结果说明了本文编队方法的有效性。在图5-(b)中，从多个不同位置和不同航向空投的翼伞，在开始阶段的散布较大，但在制导算法作用下，各翼伞开始调整飞行方向，并逐渐向彼此靠拢，然后开始执行向心飞行，并形成三角形编队；通过能量转弯段的盘旋削高后，编队转换到最终着陆段，最终所有翼伞逆风着陆到目标点。从着陆点的散布可以看到，翼伞基本上着陆在目标点(0,0,0)附近，散布比刚开始要小很多，实现了多翼伞的精确空投，降低了着陆散布。

图6为编队过程中的队形误差曲线，可以看到在初始阶段，各翼伞离各自的期望目标较远，此时初始误差较大，但在制导律作用下，队形误差逐渐降低，在150秒左右，队形误差基本上降为0，各翼伞跟踪上了各自的虚拟结构参考点，形成了编队，但从图中可以看到，在330秒左右编队误差突然增加，原因正如前所述，此时翼伞从能量控制段转换到了最终着陆段，此时误差较大，但在最终着陆前，编队误差重新又降低到0米左右。

图7为翼伞之间的间距曲线，可以看到刚开始翼伞之间的间距较远，但在制导律的作用下，间距逐渐减小，翼伞开始彼此靠拢，在150秒左右时，翼伞之间的间距逐渐收敛到稳定值，稳定值共有两个，一个大约在120米左右，另一个大约在85米左右，对比前面图2的编队队形图可以发现，翼伞之间的间距正好是这两个值。从图7还可以看到，在形成编队之后，翼伞的间距并没有进一步降低，翼伞彼此之间没有发生碰撞，保证了彼此的安全，这说明了本发明公开的方法在避碰方面的有效性。

实施例二：

本实施例公开了实现实施例一的多翼伞协同编队控制系统，如图8所示，包括：

虚拟结构生成模块，用于根据期望的编队队形确定编队虚拟结构，并在虚拟结构中确定参考点；

相对期望位置生成模块，用于计算每个翼伞在虚拟结构中相对于参考点的期望位置；

虚拟结构归航航迹规划模块，用于按照实施例一中步骤(3)的方法规划虚拟结构参考点的归航航迹，获取虚拟结构中参考点的航向角速度ω_f(t)、速度V_f(t)、航向角

和下滑角γ_f(t)，以及虚拟结构参考点的位置(x_f(t),y_f(t),z_f(t))^T；

多翼伞编队轨迹生成模块，用于根据实施例一中步骤(4)的方法计算惯性坐标系下每个时刻每个翼伞的期望位置；

多翼伞编队位置误差计算模块，用于根据实施例一中步骤(5)的方法实时计算编队中每个翼伞的位置误差；

多翼伞编队制导模块，用于根据实施例一中步骤(6)的方法实时计算编队中每个翼伞的制导参数，所述制导参数为每个翼伞的速度、航向角和下滑角。

Claims (8)

1.基于虚拟结构的多翼伞协同编队控制方法，其特征在于，包括：

(1)根据期望的编队队形确定编队虚拟结构，并在虚拟结构中确定参考点；

(2)计算每个翼伞在虚拟结构中相对于参考点的期望位置

(3)规划虚拟结构参考点的归航航迹Fp＝{path(m)}，path(m)为归航航迹上的第m个航迹点，m＝1,…,M，M为航迹点总数；获取虚拟结构中参考点的航向角速度ω_f(t)、速度V_f(t)、航向角

和下滑角γ_f(t)，以及虚拟结构参考点的位置(x_f(t),y_f(t),z_f(t))^T；

(4)计算惯性坐标系下每个时刻每个翼伞的期望位置，其中第i个翼伞在时刻t的期望位置

为：

(5)计算当前时刻t的编队位置误差

其中R_if(t)＝(x_if(t),y_if(t),z_if(t))^T为第i个翼伞在时刻t时在虚拟结构编队坐标系下的位置；

(6)计算当前时刻t每个翼伞的制导参数：

其中V_i(t)、

γ_i(t)分别为t时刻第i个翼伞的速度、航向角和下滑角，k₁、k₂、k₃分别为第一控制增益系数、第二控制增益系数、第三控制增益系数；

对于没有着陆的翼伞，循环执行步骤5和6，直到所有翼伞都着陆。

2.根据权利要求1所述的多翼伞协同编队控制方法，其特征在于，步骤(3)采用分段归航规划虚拟结构参考点的归航航迹，包括步骤：

(3.1)将从空投释放点path(1)到着陆目标点path(M)间的归航航迹分为：第一转弯过渡段、向心飞行段、第二转弯过渡段、能量控制段、第三转弯过渡段、逆风对准着陆段；

所述第一转弯过渡段、第二转弯过渡段、第三转弯过渡段的转弯角度分别为β₁、β₂、β₄；所述向心飞行段为直线滑翔，所述能量控制段的转弯半径为R_EP，进入点弧角为θ_EP，圆心角为β₃；

(3.2)建立目标函数：

其中R_min为规划航迹能量控制段的转弯半径，

为向心飞行段的滑翔长度，

为逆风对准着陆段的滑翔长度，z₀(0)/|tanγ₀(0)|为规划航迹初始投放高度对应的水平飞行距离，γ₀(0)为规划航迹初始下滑角；

通过最小化目标函数J获取能量控制段的转弯半径R_EP和进入点弧角θ_EP；

(3.3)计算分段归航每一段的参数：

第一转弯过渡段的圆心O₁的位置为：

其中(x₀(0),y₀(0),z₀(0))为虚拟结构参考点的初始投放位置，

为初始航向角；

第二转弯过渡段的圆心O₂的位置为：

向心飞行段的滑翔长度

的长度为||O₁O₂||；

第一转弯过渡段的转弯角度β₁为：

第二转弯过渡段的转弯角度β₂为：

能量控制段圆弧的圆心角β₃为：

第三转弯过渡段的转弯角度β₄为：

对分段归航航迹的每一段进行采样，所有采样点按序构成归航航迹Fp＝{path(m)}。

3.根据权利要求2所述的多翼伞协同编队控制方法，其特征在于，所述步骤(3.2)采用模拟退火算法最小化目标函数，得到能量控制段的转弯半径R_EP和进入点弧角θ_EP。

4.根据权利要求2所述的多翼伞协同编队控制方法，其特征在于，所述控制增益的取值为：k₁＝0.4、k₂＝0.5和k₃＝0.5。

5.基于虚拟结构的多翼伞协同编队控制系统，其特征在于，包括：

虚拟结构生成模块，用于根据期望的编队队形确定编队虚拟结构，并在虚拟结构中确定参考点；

相对期望位置生成模块，用于计算每个翼伞在虚拟结构中相对于参考点的期望位置；

虚拟结构归航航迹规划模块，用于规划虚拟结构参考点的归航航迹，获取虚拟结构中参考点的航向角速度ω_f(t)、速度V_f(t)、航向角

和下滑角γ_f(t)，以及虚拟结构参考点的位置(x_f(t),y_f(t),z_f(t))^T；

多翼伞编队轨迹生成模块，用于计算惯性坐标系下每个时刻每个翼伞的期望位置；

多翼伞编队位置误差计算模块，用于实时计算编队中每个翼伞的位置误差；

多翼伞编队制导模块，用于实时计算编队中每个翼伞的制导参数，所述制导参数为每个翼伞的速度、航向角和下滑角。

6.根据权利要求5所述的多翼伞协同编队控制系统，其特征在于，所述虚拟结构归航航迹规划模块采用分段归航规划虚拟结构参考点的归航航迹，包括步骤：

(3.1)将从空投释放点path(1)到着陆目标点path(M)间的归航航迹分为：第一转弯过渡段、向心飞行段、第二转弯过渡段、能量控制段、第三转弯过渡段、逆风对准着陆段；

所述第一转弯过渡段、第二转弯过渡段、第三转弯过渡段的转弯角度分别为β₁、β₂、β₄；所述向心飞行段为直线滑翔，所述能量控制段的转弯半径为R_EP，进入点弧角为θ_EP，圆心角为β₃；

(3.2)建立目标函数：

其中R_min为规划航迹能量控制段的转弯半径，

为向心飞行段的滑翔长度，

为逆风对准着陆段的滑翔长度，z₀(0)/|tanγ₀(0)|为规划航迹初始投放高度对应的水平飞行距离，γ₀(0)为规划航迹初始下滑角；

通过最小化目标函数J获取能量控制段的最优转弯半径R_EP和最优进入点弧角θ_EP；

(3.3)计算分段归航每一段的参数：

第一转弯过渡段的圆心O₁的位置为：

其中(x₀(0),y₀(0),z₀(0))为规划航迹的初始投放位置，

为初始航向角；

第二转弯过渡段的圆心O₂的位置为：

向心飞行段的滑翔长度

的长度为||O₁O₂||；

第一转弯过渡段的转弯角度β₁为：

第二转弯过渡段的转弯角度β₂为：

能量控制段圆弧的圆心角β₃为：

第三转弯过渡段的转弯角度β₄为：

对分段归航航迹的每一段进行采样，所有采样点按序构成归航航迹Fp＝{path(m)}。

7.根据权利要求6所述的多翼伞协同编队控制系统，其特征在于，所述虚拟结构归航航迹规划模块采用模拟退火算法最小化目标函数，得到能量控制段的转弯半径R_EP和进入点弧角θ_EP。

8.根据权利要求5所述的多翼伞协同编队控制系统，其特征在于，所述多翼伞编队制导模块根据下式计算每个翼伞的制导参数：

其中V_i(t)、

γ_i(t)分别为t时刻第i个翼伞的速度、航向角和下滑角，k₁、k₂、k₃分别为第一控制增益系数、第二控制增益系数、第三控制增益系数；(x_eif(t),y_eif(t),z_eif(t))为t时刻第i个翼伞的位置误差。

Images