一种舰载火箭弹垂直转弯控制方法与流程

本发明涉及一种火箭弹姿态控制技术，特别涉及一种舰载火箭弹垂直转弯控制技术，属于飞行控制系统设计技术领域。

背景技术：

从20世纪70年代开始，舰空导弹系统率先引用垂直发射技术。由于垂直发射的导弹具有很多特点，例如：空中快速转弯,反应能力好；装弹量大，火力强，全方位发射，无发射盲区，发射结构简单，工作可靠，生存力强；有利于模块化，通用化设计；成本低等。因此，垂直发射已成为舰载导弹系统发展的必然趋势。

火箭弹垂直发射技术具有以下优点:(1)可以向任何方向发射弹药,不存在射击盲区；(2)反应时间短,发射一枚弹药只需一秒钟,而倾斜发射则需要10～30s；(3)通用性好,储弹量大；(4)与雷达联合使用,可大大提高抗饱和攻击的能力等。低成本、高射速的中近程制导火箭弹能够适应舰载作战全天候、强机动、快速性的作战要求，能够进一步提高我国火力驱逐舰的作战能力。

对于火箭弹这类低成本、高射速的中近程制导武器而言，为了适应舰载垂直发射系统的使用要求，对制导控制系统提出了全方位快速转弯的要求：导弹的姿态俯仰角和弹道倾角应在初制导段结束时达到预定指向，方位角应控制在拦截目标方位上。具体到舰载武器而言，导弹在垂直发射离筒后的短时间内，须完成导弹的快速大角度转弯，实现全方位作战飞行，而这时导弹速度较低，舵面和翼面产生的控制力及力矩较小，这就产生了导弹初始段弹道大角度拐弯的难题。

经对现有技术文献的检索发现，目前对于垂直转弯控制技术存在三类技术方案：第一类技术方案是采用摆动喷管进行姿态控制，如运载火箭、导弹武器等，该方案研制成本较高，不适用于低成本火箭武器，其控制方法可以参考李君，董朝阳，程兴，陈宇等在《导弹与航天运载技术》(2017年6期)上发表的文章“基于姿控喷管开关控制的全量耦合动力学建模与控制优化技术研究”等相关文献；第二类技术方案是采用燃气舵+空气舵进行姿态控制，如防空导弹等，该方案中的燃气舵机构复杂，需要做大量的风洞试验确定燃气舵的特性，研制成本同样不低，其控制方法可以参考汤善同、陈素贞在《北京航空航天大学学报》(1991年第4期)发表的文章“最优控制理论在垂直发射箭空导弹转弯控制中的应用”以及朱成、陈谋、姜长生在《电光与控制》(2014年05期)发表的“基于四元数的垂直发射拦截导弹姿态自抗扰控制”等相关文献；第三类技术方案是采用固体脉冲发动机阵列，如旋转火箭弹，该技术方案成本较低、无需大量额外试验，比较适合低成本火箭武器，其控制方法可以参考张璐华、廖欣、姜中昊、葛朓琳、杨永强在《上海航天》(第34卷2017年增刊)上发表的“垂直发射旋转导弹空中转弯控制技术研究”。

技术实现要素：

本发明解决的技术问题是：克服现有技术的不足，提出一种转弯系统成本较低，简单有效，适合于工程实现的一种舰载火箭弹垂直转弯控制方法。

本发明解决技术的方案是：一种舰载火箭弹垂直转弯控制方法，所述的舰载火箭弹采用冷弹垂直发射，通过下述方式实现：

首先在火箭弹弹射出筒后，使用固体脉冲姿控发动机阵列完成垂直转弯动作，使弹体的纵轴即弹体系的x轴指向目标所在方位；所述的固体脉冲姿控发动机阵列至少四个发动机且周向均布；

然后使用空气舵完成滚转角控制，使弹体系的y轴垂直于水平面向上；

最后通过全弹制导稳定控制，导引火箭弹飞向目标点。

优选的，依次通过启动转弯的点火逻辑、停止转弯的点火逻辑以及主发动机的点火逻辑三部分配合完成垂直转弯的控制；其中启动转弯的点火逻辑是在抑制俯仰和偏航通道角速率的同时使火箭弹纵轴指向目标所在方位；停止转弯的点火逻辑是抑制俯仰和偏航通道的角速率；主发动机的点火逻辑是满足俯仰偏航通道姿态及角速度的约束条件并防止主发动机的推力偏斜对转弯产生影响，完成转弯后才进行主发动机点火。

优选的，姿控发动机的编号顺序为投影在垂直弹体纵轴的横截面即弹体系yoz平面上，从0#开始逆时针顺序编号，其中0#发动机位于弹体正z轴。

优选的，所述的启动转弯的点火逻辑通过计算姿控发动机产生的角速度增量使得与俯仰和偏航通道角速度的合成矢量指向转弯角速度矢量所在方位。

优选的，所述的启动转弯的点火逻辑具体操作如下：

a.计算发射方向与弹体纵轴的夹角θ；

b.如果满足条件：θ>δ，则按照①～⑦计算开始转弯的姿控发动机开机指令；否则，跳转至c；所述的δ预设的角度偏差上限；

①计算期望的姿态变化方向在弹体yoz平面的相角θexp；

②计算干扰角速度产生的姿态变化方向在弹体yoz平面的相角θdis；

③计算姿控发动机需要产生的角速度增量在弹体yoz平面的相角θcmd；

④计算推力在弹体yoz平面的相角θpush，并将取值调整到[0°,360°)范围内；

⑤根据步骤④调整后的相角θpush以及姿控发动机的安装角度间隔σ，计算姿控发动机开机阵列的中心编号以及开机相位的余角；

⑥计算姿控发动机的开机阵列最小序号non_min和最大序号non_max，并将该序号的范围调整至[0,n-1]范围内的整数；n为姿控发动机数量；

⑦设置姿控发动机阵列的开机指令：如果non_min≤non_max，则设置序号为non_min,…,non_max的姿控发动机点火；否则，设置序号为non_max,…,n-1,0,…,non_min的姿控发动机点火。

c.计算结束。

优选的，所述的停止转弯的点火逻辑通过计算姿控发动机产生的角速度增量使得与俯仰和偏航通道角速度的合成矢量指向相反。

优选的，所述的停止转弯的点火逻辑具体操作如下：

①计算发射方向与弹体纵轴的夹角θ以及弹体纵轴与发射系y轴正向的夹角θxy；

②如果条件1与条件2同时满足，则执行③；否则，跳转至⑨；

条件1：已经执行过启动转弯；

条件2：π/2-θxy≤θ，其中，θxy为弹体纵轴与天向的夹角，θ为装订的发射高低角；

③如果则执行④～⑧；否则，跳转至⑨；所述的δ为预设的角速度偏差上限；

④根据俯仰和偏航合成角速度计算推力在弹体yoz平面的相角，并将取值调整到[0°,360°)范围内；

⑤计算姿控发动机开机阵列的中心编号以及开机的扇区角，调整姿控发动机开机阵列的中心编号，计算姿控发动机的开机阵列最小序号和最大序号，并将该序号的范围调整至[0,n-1]范围内的整数；

⑥初步确定点火的姿控发动机序号；如果non_min≤non_max，则设置序号为non_min,…,non_max的姿控发动机点火；否则，设置序号为non_max,…,n-1,0,…,non_min的姿控发动机点火；

⑦统计本次点火的发动机序号中在开始转弯的姿控过程中已经用过的发动机数量，调整需要点火的姿控发动机序号，排除已用过的发动机序号，然后将调整后序号的范围调整至[0,n-1]范围内的整数；

⑧设置姿控发动机阵列的开机指令：如果non_min≤non_max，则设置序号为non_min,…,non_max的姿控发动机点火；否则，设置序号为non_max,…,n-1,0,…,non_min的姿控发动机点火。

⑨结束计算。

优选的，所述的主发动机点火逻辑为：

在转弯结束后，判断约束条件且|ψ|＜σ是否满足：如果满足，则主发动机点火；否则，放弃主发动机点火；

所述的转弯结束的条件为俯仰角小于发射高低角；

θ为装订的发射高低角，为俯仰角，ψ为偏航角，σ为姿控发动机的安装间隔。

优选的，所述的转弯结束条件具体操作取姿控发动机反喷完成后延时，延时时间不小于姿控发动机作用时间。

优选的，所述的滚转角控制通过规划滚转角指令γc作为滚转角控制回路的输入，计算舵机控制指令，根据该舵机控制指令控制空气舵实现滚转角控制；

其中，sign_γc为滚转角指令的正负号，为设定的滚转角速率，δ1，δ2为阈值。

优选的，所述的滚转角控制回路为：

δc＝kp·(γc-γ)+ki·∫(γc-γ)dτ-wgyro·kd·ωx

其中，γ为滚转角，ωx为滚转角速度，kp，ki，kd为控制参数，wgyro为角速度滤波器，δc为舵机控制指令。

本发明与现有技术相比的有益效果是：

本发明针对垂直发射火箭弹的空中转弯控制需求，采用沿弹体周向均匀布局的固体脉冲发动机阵列作为动力源，设计了一种能够对全方位目标进行有效打击的转弯控制方法。相对于摆动喷管、燃气舵等用于垂直转弯控制的执行机构，固体脉冲发动机具有低成本、数量可配置、作用时间短的特点，使中近程制导火箭弹具备一定的价格竞争优势。本发明的转弯控制方法适合于冷弹发射的火箭弹，在火箭弹垂直弹射出筒后、主发动机点火前，采用正喷和反喷两次点火完成转弯、反喷结束后主发动机点火，不仅能够使弹体头部快速平稳的指向目标所在方位，而且能够让主发动机的能量全部用于增加射程。转弯结束后，使用空气舵进行大角度滚转控制，让弹体的纵向平面平滑的调整到垂直于当地水平面的方向，顺利将转弯控制过渡到全弹的制导稳定控制。

附图说明

图1为本发明制导控制方案原理框图；

图2为本发明转弯控制的时序逻辑流程图；

图3为本发明启动转弯的点火逻辑原理框图；

图4停止转弯的点火逻辑原理框图；

图5为本发明滚转角控制回路原理框图；

图6为本发明发射系下导航姿态角仿真曲线；

图7为本发明陀螺输出的角速度仿真曲线；

图8为本发明等效舵偏角指令仿真曲线；

图9为本发明姿控发动机开关控制序号；

图10为本发明固体脉冲姿控发动机阵列(30个)的开关控制指令仿真曲线；

图11为本发明姿控发动机阵列的布局。

具体实施方式

下面结合实施例对本发明作进一步阐述。

火箭弹弹射出筒后的飞行速度较低，舵系统提供的力矩不足以改变弹体姿态，需要使用固体脉冲姿控发动机阵列提供的直接力来产生转弯动作。但由于姿控发动机的推力过弹体纵轴，所以该直接力不能改变滚转通道的姿态，这表明滚转通道的姿态需要火箭弹速度建立起来后通过舵系统来进行调整。当舵系统将滚转姿态调整完毕，使弹体y轴正向指向天空后，火箭弹开始传统的制导控制。根据以上对于舰载火箭弹垂直转弯过程的分析，本发明的技术方案主要分为转弯控制和滚转控制两部分，从时序上划分为自由飞行、转弯控制和滚转控制三个阶段。

(1)自由飞行阶段。保证火箭弹弹射出筒，俯仰、偏航和滚转通道的舵偏角指令置零，时间1s；

(2)转弯控制阶段。控制脉冲姿控发动机阵列点火，实现快速转弯，指向目标，并在转弯完成后进行主发动机点火；

(3)滚转控制阶段。滚转通道利用空气舵实现滚转角控制，使得弹体y轴正向指向天空，俯仰和偏航通道的舵偏角指令置零。

下面结合图1主要介绍转弯控制方案和滚转控制方案。

(一)转弯控制方案

垂直转弯的控制分为启动转弯的点火逻辑、停止转弯的点火逻辑以及主发动机的点火逻辑三部分，其中启动转弯的点火逻辑设计原则是在抑制俯仰和偏航通道角速率的同时使火箭弹纵轴指向目标所在方位，停止转弯的点火逻辑设计原则是抑制俯仰和偏航通道的角速率，主发动机的点火逻辑设计原则是满足俯仰偏航通道姿态及角速度的合理约束条件并防止主发动机的推力偏斜对转弯产生影响，完成转弯后才进行主发动机点火。转弯控制的时序逻辑如下：

图2中，θ为装订的发射高低角，为俯仰角，ψ为偏航角，ωz为俯仰角速度，ωy为偏航角速度。转弯结束的条件为俯仰角小于发射高低角，姿控发动机反喷完成后延时300ms判断约束条件且|ψ|＜12°是否满足：如果满足，则主发动机点火；否则，放弃主发动机点火。

(1)启动转弯的点火逻辑设计

根据姿控发动机的性能参数，确定使用12或13个姿控发动机进行转弯控制的效率最高。如图3所示，取火箭弹弹体系的yoz平面(弹体横截面)，从弹体尾部向弹体头部看，将30个脉冲发动机投影到该平面，同时将转弯方向、转弯角速度矢量姿控发动机产生的角速度增量以及俯仰和偏航通道角速度合矢量绘制于该平面上。

由于滚转角速度与姿控发动机的方向垂直，所以姿控发动机无法抑制滚转角速度干扰，而且弹体纵轴的指向不受滚转角速度干扰的约束。根据这个特点，启动转弯的点火逻辑的设计思路为：姿控发动机产生的角速度增量在抵消俯仰和偏航通道的干扰角速度后，将剩余的能力用来转弯。在数学上，计算姿控发动机产生的角速度增量使得与的合成矢量指向转弯角速度矢量所在方位。具体的操作如下：

a.计算发射方向与弹体纵轴的夹角θ；

b.计算开始转弯的姿控发动机开机指令，分解为步骤①～⑦；

如果满足条件：θ＞2°，则执行如下操作；否则，跳转至c。

①计算期望的姿态变化方向在弹体yoz平面的相角θexp；

②计算干扰角速度产生的姿态变化方向在弹体yoz平面的相角θdis；

③计算姿控发动机产生的角速度增量在弹体yoz平面的相角θcmd；

④计算推力在弹体yoz平面的相角θpush，并将取值调整到[0°，360°)范围内；

⑤计算姿控发动机开机阵列的中心编号以及开机相位的余角；

⑥计算姿控发动机的开机阵列最小序号non_min和最大序号non_max，并将该序号的范围调整至[0，29]范围内的整数；

⑦设置姿控发动机阵列的开机指令：如果non_min≤non_max，则设置序号为non_min，…，non_max的姿控发动机点火；否则，设置序号为non_max，…，29，0，…，non_min的姿控发动机点火。

c.计算结束。

(2)停止转弯的点火逻辑设计

如图4所示，取火箭弹弹体系的yoz平面(弹体横截面)，从弹体尾部向弹体头部看，将30个脉冲发动机投影到该平面，同时将姿控发动机产生的角速度增量和俯仰和偏航通道角速度合矢量绘制于该平面上。

停止转弯的点火逻辑的设计思路为：姿控发动机产生的角速度增量用于抵消俯仰和偏航通道的干扰角速度。在数学上，计算姿控发动机产生的角速度增量使得与的合成矢量指向相反。具体的操作如下：

①计算发射方向与弹体纵轴的夹角θ以及弹体纵轴与发射系y轴正向(天向)的夹角θxy；

②如果条件1与条件2同时满足，则执行③～⑧；否则，跳转至⑨。

条件1：已经执行过启动转弯；

条件2：π/2-θxy≤θ，其中θ为装订的发射高低角。

③如果则执行④～⑧；否则，跳转至⑨。

④根据俯仰和偏航合成角速度计算推力在弹体yoz平面的相角，并将取值调整到[0°，360°)范围内；

⑤计算姿控发动机开机阵列的中心编号以及开机的扇区角，调整姿控发动机开机阵列的中心编号，计算姿控发动机的开机阵列最小序号和最大序号，并将该序号的范围调整至[0，29]范围内的整数；

⑥初步确定点火的姿控发动机序号。如果non_min≤non_max，则设置序号为non_min，…，non_max的姿控发动机点火；否则，设置序号为non_max，…，29，0，…，non_min的姿控发动机点火。

⑦统计本次点火的发动机序号中(在开始转弯的姿控过程中)已经用过的发动机数量，调整需要点火的姿控发动机序号，排除已用过的发动机序号，然后将调整后序号的范围调整至[0，29]范围内的整数。

⑧设置姿控发动机阵列的开机指令：如果non_min≤non_max，则设置序号为non_min，…，non_max的姿控发动机点火；否则，设置序号为non_max，…，29，0，…，non_min的姿控发动机点火。

⑨结束计算。

(3)主发动机的点火逻辑设计

主发动机的推力偏斜会产生干扰力矩，所以主发动机的点火逻辑考虑如下两个方面：

①在转弯完成后(即停止转弯的点火指令发出后延时300ms)进行主发动机点火；

②满足条件：且|ψ|＜12°。

上述条件①与②中都满足，则进行主发动机点火。

(二)滚转控制方案

滚转控制阶段处于低动压区，此时主发动机刚开始工作，飞行速度正在逐渐增大，滚转角由于转弯控制而处于任意状态，俯仰和偏航通道对各类干扰比较敏感(如，主发动机推力偏斜、剩余角速度、较大的攻角和侧滑角等)，不适合进行制导控制。所以，只利用空气舵实现滚转角控制，将弹体y轴正向指向天空，不对俯仰和偏航通道进行主动控制，让攻角和侧滑角在空气动力作用下收敛。

滚转通道的控制方案如图5所示，采用角度控制回路。

δc＝kp·(γc-γ)+ki·∫(γc-γ)dτ-wgyro·kd·ωx

其中，δc为舵机控制指令，γc为滚转角指令，sign_γc为滚转角指令的正负号，为设定的滚转角速率，δ1，δ2为阈值，一般δ1≤5°，δ2≤20°，γ为滚转角，ωx为滚转角速度，kp，ki，kd为控制参数，wgyro为角速度滤波器。

实施例

本发明中揭示了一种舰载火箭弹垂直转弯控制技术，目前该控制系统及控制策略，在释放后对敌方雷达干扰，提高己方弹道导弹生存能力方面发挥重要作用，现说明如下。

一、舰载火箭弹转弯系统主要组成

(1)固体脉冲姿控发动机阵列的结构布局

姿控发动机阵列位于弹体头部，共30个，见图11。脉冲固体发动机沿着弹体轴向共2层，每层环向均匀分布15个，相邻两层交错分布。单个微型脉冲固体发动机推力垂直于弹体纵轴，当点火控制器接收到点火指令后，启动相应位置的脉冲发动机组工作，产生所需要的推力。

对30个脉冲发动机的编号定义如下：从弹体尾部向头部看，将两排脉冲发动机投影到弹体横截面上，定义iv象限(弹体正z轴)的发动机编号为0#，30个发动机依次逆时针排列，每个间隔12°。靠近弹尖的第一排脉冲发动机依次编号为偶数：0#，2#，4#，6#，......，28#，相对于iv象限的安装角度依次为0°，24°，48°，......，336o；第二排脉冲发动机依次编号为奇数：1#，3#，5#，......，29#，相对于iv象限的安装角度依次为12°，36°，60°，......，348°。

(2)单个固体脉冲姿控发动机

向火箭弹提供垂直转弯控制动力，依据控制指令实现火箭弹姿态的调整与稳定，其主要指标如下：

总冲量：35ns；

工作时间：26ms。

二、舰载火箭弹转弯控制策略。

将垂直发射火箭弹的转弯控制从时序上划分为自由飞行阶段、转弯控制阶段以及滚转控制阶段，共3个阶段。令控制阶段的标识phase_ctrl，其取值范围为0～2。phase_ctrl＝0表示自由飞行阶段；phase_ctrl＝1表示转弯控制阶段；phase_ctrl＝2表示滚转控制阶段。

转弯控制程序按照下面的公式进行控制指令的计算，其中等效舵偏角、舵机控制指令以及姿控发动机点火指令每5ms进行一次计算。下面规定了每个计算周期中，各阶段的控制指令以及跳转条件的计算方法，具体如下：

1自由飞行阶段

当phase_ctrl＝0时，飞行控制位于自由飞行阶段。该阶段是起飞后的默认阶段，从弹上零秒开始。本阶段的控制目标为锁舵、关闭姿控发动机和主发动机，使火箭弹处于弹射后的自由飞行状态。每个控制周期的具体操作如下：

(1)执行机构的指令置零

a.等效舵偏角指令置零

其中，δz为俯仰通道等效舵偏角，δy为偏航通道等效舵偏角，δx为滚转通道等效舵偏角。

b.舵机控制指令置零

δduo_i＝0，i＝1，2，3，4

其中，δduo_i为1号至4号舵机的偏转角指令。

c.姿控发动机点火指令置零

δzk_i＝0，i＝0，…，29

其中，δzk_i为0号至29号固体姿控脉冲发动机的开关指令，1代表点火，0代表关闭。

(2)判断是否跳转到下一阶段

如果满足条件：飞行时间tfly＞ls，则执行操作①～④；否则，跳转至(3)。

①将当前时间记录为tstate；

②计算转弯指令

其中，为发射方向单位矢量在发射系下的投影，为根据滚转角γc偏航角ψc和俯仰角计算的2-3-1转序(先偏航再俯仰后滚转)的转换矩阵，γc＝0，ψc＝0，θ为装订的发射高低角。

③设置垂直转弯阶段的调姿状态标志：state_turn＝0，表示转弯初始状态；

④设置控制阶段标识phase_ctrl＝1。

(3)本阶段计算结束。

2转弯控制阶段

当phase_ctrl＝1时，飞行控制位于转控制弯阶段。本阶段的控制目标为火箭弹的垂直转弯，即通过点火逻辑控制固体脉冲姿控发动机阵列的直接力输出，实现火箭弹的姿态改变。

(1)计算脉冲姿控发动机的开机逻辑

a.计算发射方向与弹体纵轴的夹角θ

其中，θ为发射方向与弹体纵轴的夹角，为发射方向单位矢量在发射系下的投影，为发射方向单位矢量在弹体系下的投影，∠(·，·)为计算向量夹角的函数。

b.计算开始转弯的姿控发动机开机指令

如果满足条件：θ＞2°且state_tum＝0，则执行如下操作；否则，设置state_turn＝1(表示不需要正喷转弯)，然后跳转至c。

①计算期望的姿态变化方向在弹体yoz平面的相角θexp

其中，为向量的y向和z向分量。

②计算干扰角速度的姿态变化方向在弹体yoz平面的相角θcmd

(i)ωy，ωz为偏航和俯仰角速度；

(ii)如果ωyz·dod＞1°/s且ωyz·dod＜15°/s，则

θdis＝arctan2(ωy，ωz)+π/2

否则，θdis＝0，ωyz＝0。

(iii)解一元二次方程ax²+bx+c＝0，取两个解中正值，记为rcmd，其中a＝1，b＝-2ωyzcos(θexp-θdis)，ωrcs＝26.5/dod为根据弹体控制能力设置的低头角速度。

(iv)计算θcmd

θcmd＝arctan2(ycmd，xcmd)

③计算推力在弹体yoz平面的相角θpush，将θpush由弧度转换成度，并将取值调整到[0°，360°)范围内

θpush＝(θcmd+π)·dod

④计算姿控发动机开机阵列的中心编号以及开机相位的余角

non_mid＝「θpush/δzk]

ε＝θpush-non_mid·δzk

其中，non_mid为姿控发动机开机阵列的中心编号；δzk＝12°为固体脉冲姿控发动机安装布局的角度间隔；表示向上取整运算，表格中插值结果按表格中的数据取整，如表1中插值出角速度15°，则开机数量取值7；ε为开机相位的余角，取值范围[0，δzk)。

⑤计算姿控发动机的开机阵列最小序号和最大序号，并将该序号的范围调整至[0，29]范围内的整数

其中，non_min为姿控发动机开机阵列的最小编号；non_max为姿控发动机开机阵列的最大编号；non_half＝6为姿控发动机预计开机数量的一半。

⑥设置姿控发动机阵列的开机指令及相关状态标识

(i)如果non_min≤non_max，则设置序号为non_min，…，non_max的姿控发动机点火；否则，设置序号为non_min，…，29，0，…，non_max的姿控发动机点火。

(ii)设置state_turn＝1，表示开始转弯，即完成转弯角速度的建立。

c.计算结束转弯的姿控发动机开机指令

①计算弹体纵轴与发射系y轴正向的夹角θxy

其中，θxy为弹体纵轴与发射系y轴正向的夹角，为发射系y轴单位矢量在弹体系下的投影，∠(·，·)为计算向量夹角的函数。

②如果条件1与条件2同时满足，则执行后续操作；否则，跳转至(3)。

条件1：state_turn＝1；

条件2：

其中，θ为发射方向与弹体纵轴的夹角，为装订的发射高低角。

③如果ωyz·dod＞2°/s，则执行后续操作；否则，跳转至(3)。

其中，ωy，ωz为偏航和俯仰角速度，ψ为导航解算的2-3-1转序的俯仰角和偏航角。

④计算推力在弹体yoz平面的相角θpush，将θpush由弧度转换成度，并将取值调整到[0°，360°)范围内

θpush＝[arctan2(ωy，ωz)+π/2]·dod

⑤计算姿控发动机开机阵列的中心编号以及开机相位的余角

ε＝θpush-non_mid·δzk

其中，non_mid为姿控发动机开机阵列的中心编号；δzk＝12°为固体脉冲姿控发动机安装布局的角度间隔；表示向上取整运算，表格中插值结果按表格中的数据取整，如表2中插值出角速度22°，则开机数量取值10；ε为开机相位的余角，取值范围[0，δzk)。

⑥调整姿控发动机开机阵列的中心编号，计算姿控发动机的开机阵列最小序号和最大序号，并将该序号的范围调整至[o，29]范围内的整数

当0°≤ε＜3°时，根据表1插值角速度ωyz·dod获得姿控发动机点火数量，并向上取整为non_odd，然后计算non_half＝(non_odd-1)/2+1，最后计算姿控发动机开机阵列为non_min＝non_mid-non_half，non_max＝non_mid+non_half；

当3°≤ε＜9°时，根据表2插值角速度ωyz·dod获得姿控发动机点火数量，并向上取整为non_even，然后计算non_ha1f＝non_even/2，最后计算姿控发动机开机阵列为non_min＝non_mid-non_half+1，non_max＝non_mid+non_half；

当9°≤ε＜12°时，根据表1插值角速度ωyz·dod获得姿控发动机点火数量，并向上取整为non_odd，然后计算non_half＝(non_odd-1)/2+1，最后计算姿控发动机开机阵列为non_min＝non_mid-non_half-1，non_max＝non_mid+non_half+1。

其中，non_min为姿控发动机开机阵列的最小编号；non_max为姿控发动机开机阵列的最大编号；non_ha1f为姿控发动机预计开机数量的一半。

表1奇数个姿控发动机的开机关系

表2偶数个姿控发动机的开机关系

⑦初步确定点火的姿控发动机序号。如果non_min≤non_max，则设置序号为non_min，…，non_max的姿控发动机点火；否则，设置序号为non_max，…，29，0，…，non_min的姿控发动机点火。

⑧统计本次点火的发动机序号中(在开始转弯的姿控过程中)已经用过的发动机数量，记为non_over。

⑨调整需要点火的姿控发动机序号，然后将调整后序号的范围调整至[0，29]范围内的整数。

如果non_min≤non_max，则调整后的序号为non_min＝non_min+non_over，non_max＝non_max-non_over的姿控发动机点火；否则，调整后的序号为non_min＝non_min-non_over，non_max＝non_max+non_over的姿控发动机点火。

⑩设置姿控发动机阵列的开机指令及相关状态标识

(i)如果non_min≤non_max，则设置序号为non_min，…，non_max的姿控发动机点火；否则，设置序号为non_min，…，29，0，…，non_max的姿控发动机点火。

(ii)设置state_turn＝2，表示结束转弯，即完成转弯角速度的消除。

(3)判断是否跳转到下一阶段

如果满足条件：state_turn＝2，则延时300ms，然后执行操作①～⑤；否则，跳转至(4)。

①将当前时间记录为tstate；

②设置控制阶段标识phase_ctrl＝2，等待主发点火条件判断完毕才进入下一阶段；

③设置舵偏角指令系数kduo＝0；

④判断条件且|ψ|＜12°是否满足：如果满足，则主发动机点火，并将该时刻记录为tzf_on，进入下一阶段；否则，主发动机放弃点火，进入下一阶段；

⑤设置默认的滚转角指令及其初始标志和正负号。

γc＝γ

flag_γc＝0

其中，γc为滚转角指令，γ为弹体滚转角，sign_γc为滚转角指令的正负号，flag_γc为滚转角指令的初始标志。

(4)本阶段计算结束。

3滚转控制阶段

当phase_ctrl＝2时，飞行控制位于滚转控制阶段。本阶段的控制目标为火箭弹的滚转通道控制，即通过空气舵的偏转将滚转角收敛到零。具体操作如下：

(1)滚转角控制

a.根据滚转角指令的初始标志计算滚转角指令γc：如果flag_γc为0，表示γc未初始化，从操作①开始执行；否则，从操作②开始执行；

①初始化滚转角指令γc

γc＝γ

其中，ωx为滚转角速度。

②计算更新滚转角指令γc

其中，dod＝57.3。

b.调用“滚转角控制模块”计算滚转通道的等效舵偏角δgz_ang；

δx＝kduo·δgz_ang

c.设置俯仰和偏航通道的等效舵偏角：δz＝0，δy＝0。

(2)本阶段计算结束。

滚转角控制模块的输入为：滚转角指令γc，滚转角γ，滚转角速度ωx；输出为：滚转通道的等效舵偏角δgz。

滚转角偏差：γe＝γc-γ

滚转角速度限幅：

pid控制量：

积分项限幅：

滚转通道等效舵偏角：δgz＝up_gz+ui_gz-ud_gz

等效舵偏角限幅：

其中，γe为滚转角偏差；kp_ph，ki_ph，kd_ph为滚转角控制参数，其值根据具体的气动参数进行设计获得；∫(·)表示数值积分，积分项初值为零，dod＝57.3。

根据任务的相关参数及方案设计，设定火箭弹弹射速度40m/s，初始发射高低角87°，期望的发射高低角60°，对垂直转弯控制进行仿真，主要仿真结果如下：

如图6-10仿真曲线中给出了火箭弹的角速度、姿态角、等效舵偏角指令、开机逻辑以及姿控发动机开关的变化规律。在垂直转弯过程中弹体的角速度较大，俯仰和偏航的合成角速度不小于20°/s，弹体的偏航角姿态收敛到零，俯仰角调整到设定的发射高低角60°。在设定的标称初始条件下，转弯完成时滚转角较大，需要在气动舵的控制下完成滚转角的调整。

本发明未详细说明部分属于本领域技术人员公知常识。