CN112182957B - 一种基于天牛群算法的一子级伞控回收航迹规划方法 - Google Patents

Abstract

本发明提供了一种基于天牛群算法的一子级伞控回收航迹规划方法，建立系统航迹规划简化质点模型，建立对应航迹的几何关系，在分段归航的基础上，建立一子级归航航迹多目标函数约束条件，满足一子级落点精度之后对归航过程系统能量损耗进行约束，并在多目标寻优的过程中增加了盘旋阶段盘旋圈数的寻优，将基于天牛须搜索的粒子群优化算法(BSO)引入一子级回收航迹规划当中。本发明建立简化的系统航迹规划质点模型，计算简单，在工程上易于实现，保证一子级落点精度之后对归航过程系统的能量损耗进行约束，加快了一子级回收航迹规划中参数寻优的收敛速度和收敛稳定性，提高最优参数的求解精度，使一子级精准回收的同时，能量损耗达到最少。

Description

一种基于天牛群算法的一子级伞控回收航迹规划方法

技术领域

本发明涉及航天器回收航迹规划技术领域，尤其是一种回收航迹规划方法。

背景技术

各国正在相互科技竞争的当下，我国航天事业的发展已经取得了非常显著的成绩，航天发射活动、发射频次位居世界前列。但是通常分离体残骸以非自主式的方式进行落区控制存在较大的安全隐患，会带来较大的人力物力成本浪费，航天发射活动的频次和灵活性也会受到诸多限制，无法满足未来火箭低成本灵活发射的要求。这些对运载火箭分离体落点控制技术提出了新的要求。考虑到回收系统需要具备一定的机动可控能力，人们开始把注意力转移到跳伞运动中常用的翼伞(也称可控翼伞)上来。通过翼伞系统来回收火箭一子级，相当于给一子级配置了“柔性机翼”，利用翼伞系统的可控性和高滑翔特性使一子级精准、安全降落在指定区域内。2020年3月9号，航天科技集团也进行了翼伞落区控制的初步验证工作。我国众多学者采用分段归航策略对翼伞系统轨迹进行规划，利用各段轨迹的几何关系，将轨迹优化问题转化为参数寻优问题求解，实现了空投物投放过程中的航迹设计。

以上的设计仅考虑了盘旋阶段为不完整圆的可能性，对于高空冗余很多的情况下都是基于降低高度设计出来的，对于降低高度后的一子级航迹保证了一子级的落点精度，但翼伞系统能量损耗过多的问题并没有很好的解决，需要进一步的完善，以使一子级精准回收的同时减少系统能量损耗。

发明内容

为了克服现有技术的不足，本发明提供一种基于天牛群算法的一子级伞控回收航迹规划方法，改进了现有航迹规划方法中对于高度冗余很多的情况下的规划设计问题，实现了对于落地精度以及能量消耗的双方面最优。本发明在一子级-翼伞系统六自由度模型的基础上，建立系统航迹规划简化质点模型，将一子级归航航迹划分为径向飞行段、盘旋削高段、逆风接近段、雀降段四个阶段，建立对应航迹的几何关系；然后，在分段归航的基础上，建立一子级归航航迹多目标函数约束条件，满足一子级落点精度之后对归航过程系统能量损耗进行约束，并在多目标寻优的过程中增加了盘旋阶段盘旋圈数的寻优，将基于天牛须搜索的粒子群优化算法(BSO)引入一子级回收航迹规划当中，加快航迹规划过程参数寻优的收敛速度和稳定性，提高最优参数的求解精度，使一子级精准回收的同时能量损耗达到最少。

本发明解决其技术问题所采用的技术方案的步骤如下：

步骤1、建立一子级-翼伞系统归航过程六自由度数学模型，系统的十二个微分方程如下：

一子级-翼伞系统动力学方程：

一子级-翼伞系统角运动方程如下：

一子级-翼伞系统的姿态角与角速度之间关系如下：

一子级-翼伞系统质心在地面坐标系中的位置关系如下：

其中，一子级-翼伞系统总质量m＝m_w+m_c+m_f，m_w,m_c,m_f分别为火箭一子级质量、翼伞质量和系统附加质量；F_n,F_a,F_z分别为一子级翼伞系统沿三轴的力；x,y,z分别是一子级-翼伞系统三轴的位移；

分别表示一子级翼伞系统在地面坐标系下的沿三轴的速度；u,v,w分别表示一子级-翼伞速度在体坐标系内沿x轴、y轴、z轴的速度分量；

分别表示一子级-翼伞速度在体坐标系内沿x轴、y轴、z轴的加速度分量；p、q、r分别是一子级-翼伞绕x轴、y轴和z轴的角速度分量；

分别表示一子级翼伞在机体系下沿三轴的角速率；

θ、ψ分别是一子级-翼伞的滚转角、俯仰角、偏航角；

分别为一子级翼伞系统的滚转角变化率、俯仰角变化率、偏航角变化率；(I_xx、I_yy、I_zz),(L、M、N),(M_x、M_y、M_z)分别为系统在x轴、y轴和z轴上的转动惯量、气动力矩和重力力矩，g表示重力加速度；

在翼伞系统控制下，火箭一子级做稳定滑翔运动，在受到重力和气动力的平衡作用后，系统水平飞行速度v_l和垂直下降速度v_z保持不变，此时，将一子级-翼伞系统六自由度模型简化为质点模型，取坐标系为大地坐标系，坐标原点为航迹规划的目标点，得到的系统质点模型为，

其中，ψ，

分别为一子级-翼伞系统飞行过程中的转弯角度和转弯角速度，u表示系统控制量；

步骤2、结合一子级-翼伞系统模型仿真获取的系统飞行特性，对一子级回收航迹进行划分，将归航航迹划分为径向飞行段、盘旋削高段、逆风着陆段、雀降段四个过程，其中径向飞行段指一子级翼伞系统无控制量作稳定滑翔飞行阶段，盘旋削高段指一子级翼伞系统单侧下偏使系统围绕一个圆心做圆盘旋运动，逆风着陆段指调整一子级翼伞系统的单侧下偏量使得系统能转向到逆风的方向实现逆风进入，雀降段指系统做滑翔状态到达指定目标点的阶段；

步骤3、建立一子级回收过程各段航迹的位置和长度的几何关系；

从航迹初始点到径向飞行段BC的起点之间的过渡圆弧圆心坐标O₁为：

其中，x_o1表示圆心O₁的横坐标，y_o1表示圆心O₁的纵坐标；x₀表示航迹起始点的横坐标，y₀表示航迹起始点的纵坐标；α₁表示为初始速度与风向的夹角，风向与地面坐标系x轴重合，为初始速度与x轴的夹角，x轴向上为正，x轴向下为负；B点表示翼伞系统从初始方向转到径向飞行段的过渡圆弧段终点，C点表示系统进入盘旋阶段之前过渡圆弧段的起点，径向飞行段的位移即BC之间的距离为：

式(7)中O₂表示径向飞行段结束后的过渡圆弧段对应的圆心，径向飞行阶段BC与地面坐标系X轴正向夹角为：

其中

表示BC段的矢量线段，即分别用点C的横纵坐标减去点B的横纵坐标得出的矢量，

表示

的横坐标，

表示

的纵坐标；

圆心为O₁的圆弧对应的圆心角β₁为：

在盘旋削高阶段前，径向飞行段结束后的过渡圆弧段对应的圆心O₂坐标为：

径向飞行段结束后的过渡圆弧段的两个端点分别为C和D，其中C为径向飞行段的终点，D为顺时针过渡圆弧的结束点，过渡圆弧CD对应的圆心角β₂为：

其中，s取为1或-1，1代表过渡圆弧为逆时针，-1代表过渡圆弧为顺时针，航迹规划中的过渡圆弧为顺时针旋转，取s为-1；

圆弧CD对应的圆心角β₃为：

β₃＝-α₃-s×θ_ep，若β₃＜0，β₃＝β₃+2π (13)

在逆风接近阶段，过渡圆弧EF的夹角β₄为：

其中，R_min表示一子级回收航迹中AB段、CD段、EF段的转弯半径；β₁，β₂，β₄分别表示一子级返回航迹中转弯段对应的圆弧段对应的圆心角的弧度；R_ep表示盘旋阶段DE的盘旋半径；β₃表示盘旋阶段对应的圆弧段弧度；θ_ep表示盘旋削高阶段的方位角；

α₀表示初始位置对应的方位角；

步骤4、一子级回收航迹规划的关键是盘旋削高阶段的进入点D的确定；

根据一子级-翼伞系统的质点模型以及评价航迹的标准，即实现着陆点与目标点距离最近，建立目标函数为：

其中，

表示一子级返回中滑翔段在水平面内投影的距离，2kπ·R_ep表示为盘旋阶段过程中盘旋的整圆圈数的总距离，k为正整数，f为一子级-翼伞系统稳定飞行时的滑翔比，z₀为翼伞系统开始工作时一子级的高度；

F₁表示一子级着陆时的偏差，即设计的一子级航迹的水平飞行距离与定滑翔比条件下初始高度对应的水平飞行距离(f·z₀)的差值的绝对值；F₂表示一子级回收过程消耗的能量，u表示翼伞系统的控制量，大小与系统的转弯半径有关；

F＝k₁|F₁|+k₂|F₂| (17)

F表示F₁、F₂的加权求和，k₁、k₂为比例系数，将F₁、F₂优化至相同量纲；

设定航迹规划约束条件为：盘旋阶段的盘旋半径R_ep∈[R₁,R₂]，R₁,R₂分别为一子级-翼伞模型的转弯半径的上限和下限；盘旋削高阶段进入点D长度||OD||∈[R₁,R₂]，方位角θ_ep∈[-π,π]，盘旋的完整圈数k∈[0,10]；

步骤5、初始化BSO算法参数，设置粒子群的种群大小N，学习因子为c₁,c₂，惯性权重w以及每个天牛两个天线间的距离d₀和迭代次数；每个甲虫的位置pop(i)即寻优的变量；天牛粒子当前位置的适应度表示为Y＝f(X)，其中，Y为目标函数值，设置速度边界范围[v_min,v_max]，位置边界[pop_min,pop_max]；初始步长step；

步骤6、引入Rand()随机函数产生1行3列的在0～1之间的随机数组，即1行3列的数组中的每个元素都是0～1之间的随机数，初始化种群，生成一个天牛群，公式(18)表示设定的初始种群的每个天牛粒子的位置为：

pop_i＝pop_min+Rand(1,3)×(pop_max-pop_min) (18)

上式中，pop_i表示每个甲虫的初始位置，即分别为三个未知数x坐标，y坐标和盘旋量k，pop_max表示三个未知数，x坐标，y坐标和盘旋量k取值的最大值，本发明pop_max取[500,-π/2,10]，pop_min表示三个未知数，x坐标，y坐标和盘旋量k取值的最小值，本发明pop_min取[200,-π,0]；

将pop_i代入公式(17)的目标函数，得出初始值的适应度值；

步骤7、个体最优及全局最优初始化，计算出当前状态下的适应度值，通过公式(17)计算每个天牛个体的适应度函数f(x)，将当前的天牛个体当做个体的最优值，令初始的全局最优值设为一个无限大的数，将每个个体的适应度值逐个与全局最优值进行比较，当个体的适应度值比全局最优值小时，将全局最优值的值更新为这个个体的适应度值，直到每个个体都比较一遍，得出全局最优此个体的信息并放入到个体最佳中；

步骤8、执行单次迭代寻优，并利用如下公式对其中的学习因子及权重系数进行更新；

c₁＝a+2×i/maxgen，c₂＝b-2×i/maxgen (19)

w＝w_max-(w_max-w_min)×i/maxgen (20)

其中a,b均为常数，w_min,w_max分别表示最小惯性权重和最大惯性权重，i表示当前迭代次数，maxgen表示算法最大迭代总次数；

步骤9、根据天牛的位置计算每个天牛的左侧距离x_left和适应度f_left及右侧距离x_right和适应度f_right：

其中v_i＝rands(1,3)，通过引入Matlab中的rands()函数进行计算，f_left和f_right通过公式(17)计算得到，比较f_left和f_right，如果f_right小，得出下一步天牛个体向着右须的方向前进，如果f_left小，得出下一步天牛个体向着左须的方向前进，则分别得到在前进方向的位移增量：

Yb_i＝step·rand(1,3)·sign(f_right-f_left) (22)

其中，Yb_i表示由天牛两个触角探测到的信息强度决定的部分位移增量

步骤10、将每个个体的位置代入公式(17)计算得出当前个体的最小值，并且将所有个体的最优解中的最小值作为全局的最优解gbest，并且基于此对天牛个体进行速度及位置更新：

并对天牛的步长以及两个触角的质心之间的距离进行更新：

step^k+1＝eta_δ·step^k (25)

其中，eta_δ为步长的衰减系数，取0.95，d₀为天牛两须之间的距离，e为常系数，本发明取2；

步骤11、重复步骤7-步骤10，当重复次数达到所设定的最大迭代次数，得到最终的全局最优解及最优解相对应的各项参数，参数为位置信息x坐标，y坐标，盘旋圈数k，即一子级回收航迹设计中盘旋阶段的最优进入点坐标；

步骤12、将步骤11得到的各项参数带入一子级回收规划的航迹中，计算径向飞行阶段、盘高阶段、逆风着陆阶段以及雀降阶段的航迹设计参数；

步骤13、将一子级各阶段航迹设计参数带入一子级-翼伞六自由度模型中，进行仿真并显示一子级回收航迹规划结果。

本发明的有益效果提出一种基于天牛群算法的一子级伞控回收航迹规划方法，改进了现有航迹规划方法中对于高度冗余很多的情况下的规划设计问题，实现了对于落地精度以及能量消耗的双方面最优。与现有技术相比，本发明体现在：

1)在一子级-翼伞系统六自由模型的基础上，根据一子级回收过程运动特性，建立简化的系统航迹规划质点模型，计算简单，在工程上易于实现；

2)在分段归航方法设计航迹的基础上，建立多目标函数约束条件，保证一子级落点精度之后对归航过程系统的能量损耗进行约束；

3)将新型的天牛须搜索算法与粒子群优化算法相结合，引入一子级回收航迹规划当中，加快了一子级回收航迹规划中参数寻优的收敛速度和收敛稳定性，提高最优参数的求解精度，使一子级精准回收的同时，能量损耗达到最少。

附图说明

图1为本发明方法进行运载火箭一子级伞控回收航迹规划流程图；

图2为图1实施分段设计一子级归航航迹的示意图；

图3为图1实施BSO算法寻找一子级最优归航航迹的算法流程图；

图4为图1实施例中一子级回收航迹优化目标函数收敛迭代过程图；

图5为图1实施例中一子级回收过程的能量控制输出图；

图6为图1实施例中一子级回收航迹规划三维轨迹图；

图7为图1实施例中一子级回收航迹规划平面轨迹。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

下面结合附图1-7及实施例对本发明进行进一步详细描述。

本实例实验的硬件环境为：处理器：Intel(R)Core(TM)i5-8400 CPU@2.80GHz2.81GHz,操作系统：Windows 10家庭中文版，64位，内存：16G，软件采用MATLABR2018a进行仿真实验。

运载火箭一子级伞控回收航迹规划方法，具体实施步骤如下：

步骤1、为实时得到火箭一子级返回过程的速度和位置信息，在MATLAB/Simulink软件中搭建一子级-翼伞系统的六自由度数学模型，来描述系统质心三个方向的线运动以及角运动，建立的一子级-翼伞系统12个微分方程如下：

一子级-翼伞系统动力学方程：

一子级-翼伞系统角运动方程如下：

一子级-翼伞系统的姿态角与角速度之间关系如下：

一子级-翼伞系统质心在地面坐标系中的位置关系如下：

在翼伞系统控制下，一子级做稳定滑翔运动，此时一子级-翼伞系统为一个质点，在受到重力和气动力的平衡作用后，系统水平飞行速度v_l和垂直下降速度v_z保持不变。基于以上假设，将一子级-翼伞系统六自由度模型简化为质点模型，便于航迹规划。取坐标系为大地坐标系，坐标原点为航迹规划的目标点，得到的系统质点模型为，

设定某型火箭一子级回收航迹规划初始条件：翼伞完全展开时，系统的初始位置为(0，0，3500)，着陆点为(-4000,-1500,0)，系统稳定飞行时水平速度30m/s，垂直速度为10m/s，并假设一子级做转弯运动时，速度不受操纵量的影响。

步骤2、翼伞系统主要包括滑翔、盘旋下降以及雀降三种基本的运动方式，对于有明确着陆目标和落地速度要求的一子级来说，雀降实施的高度是一定的。因此在系统航迹规划过程中只考虑滑翔、盘旋两项运动。翼伞的滑翔运动在水平面的投影是一条直线，盘旋运动在水平面的投影为圆或圆弧，因此一子级回收的航迹在水平面的投影是直线或圆弧的组合。结合步骤1系统模型仿真得到的坐标信息，对一子级回收航迹进行划分，一子级-翼伞系统自A点投放，于AB段调整系统飞行航向；BC段为径向飞行段，系统以稳定速度飞行预定目标上空；到达C点后，选取最小的转弯半径进行盘旋阶段的过渡，到达盘旋阶段的切入点D；通过DE段进行盘旋削高，于E点进入逆风接近段；当接近预定着陆点时，进入FG段直至一子级最终着陆。

步骤3、建立一子级回收过程各段航迹的位置和长度的几何关系，具体包括以下步骤：

步骤3.1、一子级回收航迹中径向飞行阶段的过渡圆弧圆心坐标O₁为

其中

α₀为初始飞行方向(水平速度与x轴正向的夹角，向上为正)，s＝-1表示顺时针，s＝1表示逆时针。径向飞行段的位移即为BC之间的距离

径向飞行阶段BC与x轴正向夹角为

圆弧AB对应的圆心角β₁为

步骤3.2、盘旋削高段的过渡圆弧对应的圆心O₂坐标为

过渡圆弧CD对应的圆心角β₂为

盘旋削高阶段对应的圆弧DE对应的圆心角β₃为

β₃＝-α₃-s×θ_ET，若β₃＜0，β₃＝β₃+2π (13)

步骤3.3、逆风接近阶段的过渡圆弧EF的夹角β₄为

其中，R_min表示一子级回收航迹中AB段、CD段、EF段的转弯半径；β₁，β₂，β₄分别表示一子级返回航迹中转弯段对应的圆弧段弧度；R_ep表示盘旋阶段DE的盘旋半径；β₃表示盘旋阶段对应的圆弧段弧度；θ_ep表示盘旋削高阶段的方位角。

步骤4、建立一子级回收航迹优化的目标函数，设定航迹规划约束条件。

步骤4.1、根据步骤3中一子级回收过程各段路径的几何关系，将航迹优化问题转化为最优解求解问题。一子级回收航迹规划的关键是盘旋削高阶段的进入点D的确定。对于一子级回收而言，航迹规划的目标是控制能量小，落点精度高，根据一子级-翼伞系统的质点模型，以及评价航迹的标准，建立目标函数为：

u表示翼伞系统的控制量，大小与系统的转弯半径有关，在简化的翼伞质点模型中，其大小等于系统的偏航角速度。

F＝k₁|Y₁|+k₂|Y₂| (17)

F值是F₁、F₂的加权求和，由于F₁表示距离而F₂表示能量，因此加入比例系数k₁、k₂,将F₁、F₂优化至相同量纲。F值越接近于0，表示规划的轨迹越满足要求。

步骤4.2、为实现一子级的精准、稳定回收着陆，航迹规划时，从三个方面进行约束规划：1)根据步骤1中的一子级-翼伞模型仿真获取的系统飞行特性，一子级回收过程中盘旋半径有一定的范围；2)为了提高一子级-翼伞系统的稳定性以及降低能耗，盘旋阶段的盘旋半径取较大值；3)盘旋阶段的盘旋半径也不应过大，便于控制一子级的着陆精度。

因此，设定航迹规划约束条件：盘旋阶段的盘旋半径R_ep∈[R₁,R₂]，R₁＝100m,R₂＝500m为一子级-翼伞模型转弯半径的上、下限，最小转弯半径R_min；盘旋削高阶段进入点D方位角θ_ep∈[-π,π]。

步骤5、初始化BSO算法参数，设置粒子群的种群大小N,学习因子c₁,c₂，惯性权重w,以及每个天牛两个天线间的距离d₀和迭代次数。每个甲虫的位置pop(i)即寻优的变量；天牛粒子当前位置的适应度表示为Y＝f(X)，其中，Y为目标函数值。设置速度边界范围[v_min,v_max]，位置边界[pop_min,pop_max]；初始步长step。在仿真图中，为了显示清楚，设置了横轴迭代次数的显示为0～100，实际仿真过程中的迭代次数为300。

步骤6、引入Rand()随机函数产生1行3列的在0～1之间的随机数组，初始化种群，生成一个天牛群，公式18表示设定的初始种群的每个天牛粒子的位置；

pop_i＝pop_min+Rand(1,3)×(pop_max-pop_min) (18)

并且代入公式17目标函数，得出初始值的适应度值。

步骤7、个体最优及全局最优初始化，计算出当前状态下的适应度值，通过公式17计算每个天牛个体的适应度函数f(x)，将当前的天牛个体当做个体的最优值，令初始的全局最优值设为一个无限大的数，将每个个体的适应度值逐个与全局最优值进行比较，当个体的适应度值比全局最优值小时，将全局最优值的值更新为这个个体的适应度值，直到每个个体都比较完，得出全局最优此个体的信息放入到个体最佳中。

步骤8、执行单次迭代寻优，并对其中的学习因子及权重系数利用如下公式进行更新。

c₁＝a+2×i/maxgen，c₂＝b-2×i/maxgen (19)

w＝w_max-(w_max-w_min)×i/maxgen (20)

其中a,b均为常数，本发明仿真过程中取a＝b＝1.5，w_min,w_max分别表示最小惯性权重和最大惯性权重，分别取0.4和0.9，i表示当前迭代次数，maxgen表示算法最大迭代总次数，取300。

步骤9、根据天牛的位置计算每个天牛的左侧距离x_left和适应度f_left，右侧距离x_right和适应度f_right，

其中v_i＝rands(1,3)，通过引入Matlab中的rands()函数进行计算，f_left和f_right是通过公式17计算得来，通过比较两者f_left和f_right，如果f_right小，可以得出下一步天牛个体向着右须的方向前进，则可以得到在此方向的位移增量，表示由天牛两个触角探测到的信息强度决定的部分位移增量

通过比较两者，得到天牛群本次迭代中最优的适应度函数值，当最优的适应度函数值比群体极值更小时，将所述群体极值更新为本次迭代的最优值，每个天牛的左右适应度生成的速度更新规则：

Yb_i＝step·rand(1,3)·sign(f_right-f_left) (22)

其中初始step取1。

步骤10、将每个个体的位置代入公式(17)计算得出当前个体的最优解pbest，并且将所有个体的最优解中的最小值作为全局的最优解gbest，并且基于此对天牛个体进行速度及位置更新：：

并对天牛的步长以及两须之间的距离进行更新：

step^k+1＝eta_δ·step^k (25)

其中，eta_δ为步长的衰减系数，通常取0.95，d₀为天牛两须之间的距离，e为常系数，取2。

步骤11、重复步骤7-10，当重复次数达到所设定的最大迭代次数以后，得到最终的全局最优解及最优解相对应的各项参数也就是位置信息，即一子级回收航迹设计中盘旋阶段的最优进入点坐标。

步骤12、将上一步骤得到的各项参数带入一子级回收规划的航迹中，计算径向飞行阶段、盘高阶段、逆风着陆阶段以及雀降阶段的航迹设计参数；

步骤13、将一子级各阶段航迹设计参数带入一子级-翼伞六自由度模型的质点模型中，进行仿真并显示一子级回收航迹规划结果。

结果说明：

图1给出了本发明方法进行运载火箭一子级伞控回收航迹规划流程图；图2为图1中实施分段设计一子级归航航迹的示意图；图3为图1实施天牛群群算法寻找一子级最优归航航迹的算法流程图；图4为图1实施例中一子级回收航迹优化目标函数收敛迭代过程图；图5为图1实施例中一子级回收过程的能量控制输出图；图6～图7为图1实施例中一子级回收航迹规划三维轨迹和平面轨迹仿真图。

由图4所示，本发明中一子级回收航迹优化目标函数值随迭代次数的增加不断减小，最终稳定在0.4附近，经过23次的迭代完成一子级回收航迹参数的优化，最终得到的寻优参数为：一子级回收航迹盘旋削高阶段的盘旋半径为R_ep＝250.42m，盘旋阶段转弯圆弧的度数为θ_ep＝-2.0496rad，盘旋阶段的盘旋完整圆圈数k＝3。

由图6和图7所示，采用本发明中的航迹规划方法设计一子级航迹，一子级的着陆偏差为16.15m；由图5所示，本发明方法寻找的盘旋阶段最优半径为R_ep＝250.42m，而且为顺时针盘旋，对应的翼伞系统控制量为u＝-0.1198，持续188s。综上可见，本发明方法在保证一子级航迹落点精度的条件下，通过建立一子级归航航迹多目标函数约束条件，对归航过程系统能量损耗进行了有效约束；引入基于天牛须的粒子群优化算法，对于归航过程中的切入点位置以及盘旋圈数进行寻优求解，加快了一子级回收航迹规划中参数寻优的收敛速度，提高了最优参数的求解精度，使一子级归航过程系统能量消耗同时达到最少。

Claims (3)

1.一种基于天牛群算法的一子级伞控回收航迹规划方法，其特征在于包括下述步骤：

步骤1、建立一子级-翼伞系统归航过程六自由度数学模型，系统的十二个微分方程如下：

一子级-翼伞系统动力学方程：

一子级-翼伞系统角运动方程如下：

一子级-翼伞系统的姿态角与角速度之间关系如下：

一子级-翼伞系统质心在地面坐标系中的位置关系如下：

其中，一子级-翼伞系统总质量m＝m_w+m_c+m_f，m_w,m_c,m_f分别为火箭一子级质量、翼伞质量和系统附加质量；F_n,F_a,F_z分别为一子级翼伞系统沿三轴的力；x,y,z分别是一子级-翼伞系统三轴的位移；

分别表示一子级翼伞系统在地面坐标系下的沿三轴的速度；u,v,w分别表示一子级-翼伞速度在体坐标系内沿x轴、y轴、z轴的速度分量；

分别表示一子级-翼伞速度在体坐标系内沿x轴、y轴、z轴的加速度分量；p、q、r分别是一子级-翼伞绕x轴、y轴和z轴的角速度分量；

分别表示一子级翼伞在机体系下沿三轴的角速率；

θ、ψ分别是一子级-翼伞的滚转角、俯仰角、偏航角；

分别为一子级翼伞系统的滚转角变化率、俯仰角变化率、偏航角变化率；(I_xx、I_yy、I_zz),(L、M、N),(M_x、M_y、M_z)分别为系统在x轴、y轴和z轴上的转动惯量、气动力矩和重力力矩，g表示重力加速度；

在翼伞系统控制下，火箭一子级做稳定滑翔运动，在受到重力和气动力的平衡作用后，系统水平飞行速度v_l和垂直下降速度v_z保持不变，此时，将一子级-翼伞系统六自由度模型简化为质点模型，取坐标系为大地坐标系，坐标原点为航迹规划的目标点，得到的系统质点模型为，

其中，ψ，

分别为一子级-翼伞系统飞行过程中的转弯角度和转弯角速度，u表示系统控制量；

步骤2、结合一子级-翼伞系统模型仿真获取的系统飞行特性，对一子级回收航迹进行划分，将归航航迹划分为径向飞行段、盘旋削高段、逆风着陆段、雀降段四个过程，其中径向飞行段指一子级翼伞系统无控制量作稳定滑翔飞行阶段，盘旋削高段指一子级翼伞系统单侧下偏使系统围绕一个圆心做圆盘旋运动，逆风着陆段指调整一子级翼伞系统的单侧下偏量使得系统能转向到逆风的方向实现逆风进入，雀降段指系统做滑翔状态到达指定目标点的阶段；

步骤3、建立一子级回收过程各段航迹的位置和长度的几何关系；

从航迹初始点到径向飞行段BC的起点之间的过渡圆弧圆心坐标O₁为：

其中，x_o1表示圆心O₁的横坐标，y_o1表示圆心O₁的纵坐标；x₀表示航迹起始点的横坐标，y₀表示航迹起始点的纵坐标；α₁表示为初始速度与风向的夹角，风向与地面坐标系x轴重合，为初始速度与x轴的夹角，x轴向上为正，x轴向下为负；B点表示翼伞系统从初始方向转到径向飞行段的过渡圆弧段终点，C点表示系统进入盘旋阶段之前过渡圆弧段的起点，径向飞行段的位移即BC之间的距离为：

式(7)中O₂表示径向飞行段结束后的过渡圆弧段对应的圆心，径向飞行阶段BC与地面坐标系X轴正向夹角为：

其中

表示BC段的矢量线段，即分别用点C的横纵坐标减去点B的横纵坐标得出的矢量，

表示

的横坐标，

表示

的纵坐标；

圆心为O₁的圆弧对应的圆心角β₁为：

在盘旋削高阶段前，径向飞行段结束后的过渡圆弧段对应的圆心O₂坐标为：

径向飞行段结束后的过渡圆弧段的两个端点分别为C和D，其中C为径向飞行段的终点，D为顺时针过渡圆弧的结束点，过渡圆弧CD对应的圆心角β₂为：

其中，s取为1或-1，1代表过渡圆弧为逆时针，-1代表过渡圆弧为顺时针，航迹规划中的过渡圆弧为顺时针旋转，取s为-1；

圆弧CD对应的圆心角β₃为：

β₃＝-α₃-s×θ_ep，若β₃＜0，β₃＝β₃+2π (13)

在逆风接近阶段，过渡圆弧EF的夹角β₄为：

其中，R_min表示一子级回收航迹中AB段、CD段、EF段的转弯半径；β₁，β₂，β₄分别表示一子级返回航迹中转弯段对应的圆弧段对应的圆心角的弧度；R_ep表示盘旋阶段DE的盘旋半径；β₃表示盘旋阶段对应的圆弧段弧度；θ_ep表示盘旋削高阶段的方位角；

α₀表示初始位置对应的方位角；

步骤4、一子级回收航迹规划的关键是盘旋削高阶段的进入点D的确定；

根据一子级-翼伞系统的质点模型以及评价航迹的标准，即实现着陆点与目标点距离最近，建立目标函数为：

其中，

表示一子级返回中滑翔段在水平面内投影的距离，2kπ·R_ep表示为盘旋阶段过程中盘旋的整圆圈数的总距离，k为正整数，f为一子级-翼伞系统稳定飞行时的滑翔比，z₀为翼伞系统开始工作时一子级的高度；

F₁表示一子级着陆时的偏差，即设计的一子级航迹的水平飞行距离与定滑翔比条件下初始高度对应的水平飞行距离(f·z₀)的差值的绝对值；F₂表示一子级回收过程消耗的能量，η表示翼伞系统的控制量，大小与系统的转弯半径有关；

F＝k₁|F₁|+k₂|F₂| (17)

F表示F₁、F₂的加权求和，k₁、k₂为比例系数，将F₁、F₂优化至相同量纲；

设定航迹规划约束条件为：盘旋阶段的盘旋半径R_ep∈[R₁,R₂]，R₁,R₂分别为一子级-翼伞模型的转弯半径的上限和下限；盘旋削高阶段进入点D长度||OD||∈[R₁,R₂]，方位角θ_ep∈[-π,π]，盘旋的完整圈数k∈[0,10]；

步骤5、初始化BSO算法参数，设置粒子群的种群大小N，学习因子为c₁,c₂，惯性权重γ以及每个天牛两个天线间的距离d₀和迭代次数；每个甲虫的位置pop(i)即寻优的变量；天牛粒子当前位置的适应度表示为Y＝f(X)，其中，Y为目标函数值，设置速度边界范围[v_min,v_max]，位置边界[pop_min,pop_max]；初始步长step；

步骤6、引入Rand()随机函数产生1行3列的在0～1之间的随机数组，即1行3列的数组中的每个元素都是0～1之间的随机数，初始化种群，生成一个天牛群，公式(18)表示设定的初始种群的每个天牛粒子的位置为：

pop_i＝pop_min+Rand(1,3)×(pop_max-pop_min) (18)

上式中，pop_i表示每个甲虫的初始位置，即分别为三个未知数x坐标，y坐标和盘旋量k，pop_max表示三个未知数，x坐标，y坐标和盘旋量k取值的最大值，pop_min表示三个未知数，x坐标，y坐标和盘旋量k取值的最小值；

将pop_i代入公式(17)的目标函数，得出初始值的适应度值；

步骤7、个体最优及全局最优初始化，计算出当前状态下的适应度值，通过公式(17)计算每个天牛个体的适应度函数f(x)，将当前的天牛个体当做个体的最优值，令初始的全局最优值设为一个无限大的数，将每个个体的适应度值逐个与全局最优值进行比较，当个体的适应度值比全局最优值小时，将全局最优值的值更新为这个个体的适应度值，直到每个个体都比较一遍，得出全局最优此个体的信息并放入到个体最佳中；

步骤8、执行单次迭代寻优，并利用如下公式对其中的学习因子及权重系数进行更新；

c₁＝a+2×i/maxgen，c₂＝b-2×i/maxgen (19)

w＝w_max-(w_max-w_min)×i/maxgen (20)

其中a,b均为常数，w_min,w_max分别表示最小惯性权重和最大惯性权重，i表示当前迭代次数，maxgen表示算法最大迭代总次数；

步骤9、根据天牛的位置计算每个天牛的左侧距离x_left和适应度f_left及右侧距离x_right和适应度f_right：

其中v_i＝rands(1,3)，通过引入Matlab中的rands()函数进行计算，f_left和f_right通过公式(17)计算得到，比较f_left和f_right，如果f_right小，得出下一步天牛个体向着右须的方向前进，如果f_left小，得出下一步天牛个体向着左须的方向前进，则分别得到在前进方向的位移增量：

Yb_i＝step·rand(1,3)·sign(f_right-f_left) (22)

其中，Yb_i表示由天牛两个触角探测到的信息强度决定的部分位移增量；

步骤10、将每个个体的位置代入公式(17)计算得出当前个体的最小值，并且将所有个体的最优解中的最小值作为全局的最优解gbest，并且基于此对天牛个体进行速度及位置更新：

并对天牛的步长以及两个触角的质心之间的距离进行更新：

step^k+1＝eta_δ·step^k (25)

其中，eta_δ为步长的衰减系数，d₀为天牛两须之间的距离，e为常系数；

步骤11、重复步骤7-步骤10，当重复次数达到所设定的最大迭代次数，得到最终的全局最优解及最优解相对应的各项参数，参数为位置信息x坐标，y坐标，盘旋圈数k，即一子级回收航迹设计中盘旋阶段的最优进入点坐标；

步骤12、将步骤11得到的各项参数带入一子级回收规划的航迹中，计算径向飞行阶段、盘高阶段、逆风着陆阶段以及雀降阶段的航迹设计参数；

步骤13、将一子级各阶段航迹设计参数带入一子级-翼伞六自由度模型中，进行仿真并显示一子级回收航迹规划结果。

2.根据权利要求1所述的一种基于天牛群算法的一子级伞控回收航迹规划方法，其特征在于：

所述pop_max取[500,-π/2,10]，pop_min取[200,-π,0]。

3.根据权利要求1所述的一种基于天牛群算法的一子级伞控回收航迹规划方法，其特征在于：

所述eta_δ取0.95，常系数e2。

Images