CN115952384A - 一种运载火箭掉头过程坐标系转换方法及其控制仿真应用 - Google Patents

Abstract

本发明涉及一种运载火箭掉头过程坐标系转换方法及其控制仿真应用，选定箭体飞行过程中二级箭体坐标系向末级箭体坐标系的转换时刻为二级分离指令发出时刻，在二级箭体与末级箭体分离之前，采用二级箭体坐标系进行导航、制导和姿态控制计算以及控制系统仿真计算；二级分离后从箭体坐标系切换时刻开始，采用末级箭体坐标系进行导航、制导和姿态控制计算以及控制系统仿真计算；降低了飞控计算机以及仿真计算机的计算运行载荷，并且降低发射前控制软件及控制系统仿真模型的计算复杂度，提高火箭飞行控制系统运行效率。

Description

一种运载火箭掉头过程坐标系转换方法及其控制仿真应用

技术领域

本发明涉及飞行器导航、制导、控制与仿真技术领域，尤其是涉及一种运载火箭掉头过程坐标系转换方法及其控制仿真应用。

背景技术

航天运载火箭采用多级串联的构型形式，在各子级动力系统工作完成后按照时序逐级分离，将承载于最后一级的有效载荷送入预定轨道完成发射任务。通常情况下，全箭状态各子级均采用发动机喷管方向向后布置的正常布局方案，前一子级分离后箭体完成小范围姿态调整和稳定后可直接点火/开机进入下一级飞行阶段。为了使箭体具有优异的气动性能，整流罩通常设计成冯·卡门曲线等形式的旋成体外形。对于小型运载火箭，受箭体长度和直径限制，整流罩内结构的包络空间十分有限，而小卫星单星/多星载荷的外部包络多为长方体形式，若采用正常布局形式将会出现有效载荷顶部到整流罩顶部内壁之间存在较多剩余空间的状况，即整流罩内有效空间没有得到充分利用，出现“运载能力有余量而载荷安装空间不足”的矛盾。通常的解决方式是增大整流罩的包络尺寸从而增大整流罩内部可用空间，这种解决方式的不利方面是会使得全箭箭体压心前移从而导致箭体的气动静不稳定特性变得更恶劣。在不改变整流罩外形尺寸的前提下，一种总体技术方案是末级箭体采用整体倒置的方案，使末级液体推进发动机位于整流罩头部曲线包络空间内，喷管等突出物处于顶部小截面半径的收缩空间中，卫星载荷位于靠近末级前一级箭体的圆柱形包络载荷舱内，这样末级箭体和载荷舱内空间得到更充分地利用，小型运载火箭对卫星载荷的可适配性进一步得到提高。

以三级串联小型运载火箭作为研究对象，采用末级箭体整体倒置安装布局方案，二级滑行段箭体在偏航方向完成掉头调姿过程，二级分离后完成整流罩分离、姿态调整和稳定，之后末级推进发动机开机进入入轨飞行阶段。在运载火箭控制系统进行导航、制导和姿态控制计算过程中，二级分离前二级飞行段采用二级箭体坐标系，箭体坐标系OX轴正向由全箭箭体尾部沿中心轴指向头部整流罩顶点方向；末级飞行段采用末级箭体坐标系，箭体坐标系OX轴正向由末级底部推进发动机沿中心轴指向头部卫星载荷方向；二级箭体坐标系与末级箭体坐标系的OX向共轴且方向相反。取二级箭体坐标系与末级箭体坐标系的OY轴均位于箭体纵向对称面内且方向相同，取与OX轴垂直且箭体平置状态由下方指向上方为正，则按照右手定则，二级箭体坐标系OZ轴正向与末级箭体坐标系OZ轴正向方向相反。

上述两种箭体坐标系定义形式，分别对应各自的描述箭体坐标系相对发射惯性坐标系旋转方位关系的欧拉角

ψ和γ。二级飞行段使用掉头前二级箭体坐标系定义的欧拉角描述箭体在惯性空间的姿态方位关系，末级飞行段使用掉头后末级箭体坐标系定义的欧拉角描述箭体在惯性空间的相对姿态方位关系，这样对应的欧拉角作为导航量计算结果更能直观地描述不同飞行时段箭体在惯性空间的姿态信息，给制导系统和姿态控制系统设计计算带来直接的便利性，同时更有利于作为控制系统的接口参数与相关分系统进行技术指标要求协调和飞行数据判读。

如图1所示为箭体掉头飞行过程控制仿真系统图，图1中，力和力矩计算模型的输入量为飞行状态量(包括飞行速度、位置、姿态角等状态量)和侧喷流发动机控制信号，输出量为箭体受到的合力矢量和合力矩矢量；姿态动力学和运动学计算的输入量为力矢量和力矩矢量，输出量为箭体飞行速度、位置、姿态角速率、欧拉角和四元数等飞行状态量；导航计算的输入量为惯组敏感到的箭体视加速度和姿态角速率，输出量为飞行状态参数计算结果；制导和姿态控制计算的输入量为导航计算得到的飞行状态参数计算结果，输出量为侧喷流发动机控制信号。

运载火箭飞行全程使用两套箭体坐标系定义形式，一种传统的实现方法是从导航计算和控制系统仿真模型计算初始时刻开始，在两套坐标系下分别进行独立的导航计算以及仿真模型动力学和运动学方程计算，在箭体坐标系切换时刻进行整套计算参数数值的切换。这种方法理解比较直观，但是存在不足之处，不足之处在于两套坐标系下同步独立计算对应部分引入了近乎双倍的计算量，大幅度增加了飞控计算机以及仿真计算机的计算运行载荷，并且两套计算需要提供各自的计算初始值，增加了发射前控制软件及控制系统仿真模型的计算流程和计算量。

针对现有技术的以上缺陷或改进需求，本发明提供了一种运载火箭掉头飞行过程坐标系转换设计及控制仿真实现方法。二级箭体与末级箭体分离之前，采用二级箭体坐标系进行导航、制导和姿态控制计算以及控制系统仿真计算。二级分离后从箭体坐标系切换时刻开始，采用末级箭体坐标系进行导航、制导和姿态控制计算以及控制系统仿真计算。

发明内容

本发明所要解决的技术问题是提供一种能够降低飞控计算机以及仿真计算机的计算运行载荷，并且降低发射前控制软件及控制系统仿真模型的计算复杂度，从而提高火箭飞行控制系统运行效率的运载火箭掉头过程坐标系转换方法及其控制仿真应用。

第一方面，本发明所采用的技术方案是，一种运载火箭掉头过程坐标系转换方法，选定箭体飞行过程中二级箭体坐标系向末级箭体坐标系的转换时刻为二级分离指令发出时刻，记二级分离前二级箭体坐标系为O_C0X₁₀Y₁₀Z₁₀，原点O_C0为二级箭体质心理论位置；二级分离后末级箭体坐标系为O_CX₁Y₁Z₁，原点O_C为末级箭体质心理论位置；该方法包括下列步骤：

S1、二级分离指令发出时刻之前，将二级箭体坐标系O_C0X₁₀Y₁₀Z₁₀与末级箭体坐标系O_CX₁Y₁Z₁平移至坐标原点重合位置O，

轴与

轴方向相反，

轴与

轴方向相同，

轴与

轴方向相反；设定向量

在二级箭体坐标系OX₁₀Y₁₀Z₁₀中的坐标为[x₁₀ y₁₀z₁₀]^T，在末级箭体坐标系OX₁Y₁Z₁中的坐标为[x₁ y₁ z₁]^T，则二者之间转换关系式为：

S2、使用三个欧拉角：俯仰角

偏航角ψ₀和滚动角γ₀来描述二级箭体坐标系OX₁₀Y₁₀Z₁₀相对发射惯性坐标系OXYZ的方位姿态关系，发射惯性坐标系OXYZ经过“321”旋转顺序依次绕相应的坐标轴转动

ψ₀、γ₀得到箭体坐标系OX₁₀Y₁₀Z₁₀，则发射惯性坐标系下坐标[x y x]^T与二级箭体坐标系下坐标[x₁₀ y₁₀ z₁₀]^T的转换关系式为：

其中，

，A0为单位正交矩阵，满足A0^-1＝A0^T；

S3、设欧拉角

ψ₀、γ₀对应的标准四元数为Q0＝[q0₀ q0₁ q0₂ q0₃]^T,根据四元数Q0的元素计算出二级箭体坐标系OX₁₀Y₁₀Z₁₀到发射惯性坐标系OXYZ的转换矩阵A0的表达式为：

S4、二级分离指令发出时刻之时，根据步骤S1中所述的转换关系以及步骤S3中所述的转换矩阵A0，得到末级箭体坐标系OX₁Y₁Z₁到发射惯性坐标系OXYZ的转换矩阵A的表达式为：

S5、将末级箭体坐标系OX₁Y₁Z₁相对发射惯性系OXYZ的方位姿态欧拉角记为俯仰角

偏航角ψ和滚动角γ，则使用欧拉角计算得到末级箭体坐标系OX₁Y₁Z₁到发射惯性坐标系OXYZ的转换矩阵A的表达式为：

S6、根据步骤S4得到的转换矩阵A以及步骤S5使用欧拉角计算得到的转换矩阵A，在ψ∈(-π/2，π/2)的条件下由转换矩阵A的元素数值得到对应的欧拉角的表达式为：

S7、根据步骤S6得到的末级箭体坐标系OX₁Y₁Z₁相对发射惯性坐标系OXYZ方位姿态关系的欧拉角

ψ和γ，计算得出对应的四元数Q＝[q₀ q₁ q₂ q₃]^T的表达式为：

本发明的有益效果是：采用上述一种运载火箭掉头过程坐标系转换设计方法，通过该方法能够简便的得到二级箭体坐标系OX₁₀Y₁₀Z₁₀、末级箭体坐标系OX₁Y₁Z₁和发射惯性坐标系OXYZ之间的转换矩阵以及相应的欧拉角和四元数，该种设计方法能够降低飞控计算机以及仿真计算机的计算运行载荷，并且降低发射前控制软件及控制系统仿真模型的计算复杂度，提高火箭飞行控制系统运行效率。

第二方面，本发明所采用的技术方案是，一种用于实施运载火箭掉头过程坐标系转换方法的控制仿真应用，所述控制仿真应用在箭体掉头飞行过程控制仿真系统中实现，所述控制仿真应用包括姿态动力学和运动学计算过程的坐标系转换控制仿真实现过程以及导航计算过程的坐标系转换控制仿真实现过程，选定箭体飞行过程中二级箭体坐标系向末级箭体坐标系的转换时刻为二级分离指令发出时刻，所述姿态动力学和运动学计算过程的坐标系转换控制仿真实现过程为：

在坐标系转换时刻之前，在二级箭体坐标系中根据动量矩定理由力矩计算箭体姿态角加速率，根据箭体姿态角加速率进行积分计算得到箭体姿态角速率，根据箭体姿态角速率进行积分计算得到四元数和欧拉角；

在坐标系转换时刻，根据步骤S1中所述的转换关系式将二级箭体坐标系下的箭体姿态角速率

转换为末级箭体坐标系下箭体姿态角速率

并基于末级箭体坐标系中力矩矢量和转动惯量矩阵，以坐标系转换时刻得到的

为初始值进行积分运算，得到末级箭体坐标系下箭体姿态角速率；在坐标系转换时刻，根据步骤S4转换矩阵A的表达式来计算末级箭体坐标系OX₁Y₁Z₁到发射惯性坐标系OXYZ的转换矩阵A，进而根据步骤S6所述的欧拉角表达式和步骤S7所述的四元数表达式来计算在坐标系转换时刻的末级箭体坐标系OX₁Y₁Z₁相对发射惯性坐标系OXYZ的欧拉角

ψ、γ以及对应的四元数Q，以坐标系转换时刻得到的欧拉角

ψ、γ为初始值进行积分运算，得到末级飞行段末级箭体坐标系相对发射惯性坐标系的欧拉角，以坐标系转换时刻得到的四元数Q为初始值进行积分运算，得到末级飞行段末级箭体坐标系相对发射惯性坐标系的四元数；

所述导航计算过程的坐标系转换控制仿真实现过程为：

在坐标系转换时刻之前，使用二级箭体坐标系下的视速度增量数值和角增量数值进行当前采样时刻点数值减前一采样时刻点数值计算，所述视速度增量数值和角增量数值为俯仰通道、偏航通道和滚动通道分别沿箭体坐标系坐标轴方向的视速度增量数值和角增量数值；在坐标系转换时刻之前的各采样时刻点，在二级箭体坐标系下根据前一采样时刻点的四元数数值计算对应的当前采样时刻点的四元数数值；

在坐标系转换时刻，将前一采样时刻点的视速度增量数值和角增量数值根据步骤S1的转换关系式由二级箭体坐标系转换到末级箭体坐标系，当前采样时刻点的视速度增量数值和角增量数值直接使用末级箭体坐标系下的参数值；从坐标系转换时刻的下一采样时刻点开始，使用末级箭体坐标系下的视速度增量数值和角增量数值进行当前采样时刻点数值减前一采样时刻点数值计算；在坐标系转换时刻，根据步骤S3的转换矩阵A0的表达式，采用二级箭体坐标系四元数Q0数值的前一采样时刻点数值Q0(z^-1)来计算二级箭体坐标系OX₁₀Y₁₀Z₁₀到发射惯性坐标系OXYZ的转换矩阵A0，再根据步骤S4的转换矩阵A的表达式计算末级箭体坐标系OX₁Y₁Z₁到发射惯性坐标系OXYZ的转换矩阵A，进而根据步骤S6的欧拉角表达式和步骤S7的四元数表达式来计算得到对应的末级箭体坐标系下的四元数Q数值的前一采样时刻点数值Q(z^-1)，从而计算得到末级箭体坐标系四元数Q的当前采样时刻点数值；从坐标系转换时刻下一采样时刻点开始，各采样时刻点在末级箭体坐标系下根据前一采样时刻点的四元数数值计算对应的当前采样时刻点的四元数数值。

本发明的有益效果是：根据上述一种用于实施运载火箭掉头过程坐标系转换设计方法的控制仿真应用，计算得到坐标系转换前后各采样计算时刻点四元数、欧拉角等导航计算参数数值。二级箭体与末级箭体分离之前，采用二级箭体坐标系进行导航、制导和姿态控制计算以及控制系统仿真计算；二级分离后从箭体坐标系切换时刻开始，采用末级箭体坐标系进行导航、制导和姿态控制计算以及控制系统仿真计算。箭体坐标系转换前后分别基于二级箭体坐标系和末级箭体坐标系完成算法设计和软件程序实现，从而使输出姿控发动机开关控制指令控制箭体姿态保持在程序姿态角附近稳定飞行，该种控制仿真实现方法降低了飞控计算机以及仿真计算机的计算运行载荷，并且降低发射前控制软件及控制系统仿真模型的计算复杂度，提高火箭飞行控制系统运行效率。

附图说明

图1为本发明中箭体掉头飞行过程控制仿真系统的示意图；

图2为本发明中二级箭体坐标系和末级箭体坐标系的示意图；

图3为本发明箭体坐标系转换前后飞行过程姿态角曲线图；其中，图3(a)为箭体坐标系转换前后飞行过程姿态角曲线图，图3(b)为图3(a)的局部放大图；

图4为本发明箭体坐标系转换前后飞行过程姿态角偏差曲线图；其中，图4(a)为箭体坐标系转换前后飞行过程姿态角偏差曲线图，图4(b)为图4(a)的局部放大图；

图5为本发明箭体坐标系转换前后飞行过程姿态角速率曲线图；图5(a)为箭体坐标系转换前后飞行过程姿态角速率曲线图；图5(b)为图5(a)的局部放大图。

具体实施方式

以下参照附图并结合具体实施方式来进一步描述发明，以令本领域技术人员参照说明书文字能够据以实施，本发明保护范围并不受限于该具体实施方式。

本领域技术人员应理解的是，在本发明的公开中，术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系是基于附图所示的方位或位置关系，其仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此上述术语不能理解为对本发明的限制。

此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

在本申请实施例的描述中，还需要说明的是，除非另有明确的规定和限定，术语“设置”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本申请中的具体含义。

第一方面，本发明涉及一种运载火箭掉头过程坐标系转换方法，选定箭体飞行过程中二级箭体坐标系向末级箭体坐标系的转换时刻为二级分离指令发出时刻，记二级分离前二级箭体坐标系为O_C0X₁₀Y₁₀Z₁₀，原点O_C0为二级箭体质心理论位置；二级分离后末级箭体坐标系为O_CX₁Y₁Z₁，原点O_C为末级箭体质心理论位置；如图2所示，其中O_C0Z₁₀轴和O_CZ₁轴分别在各自坐标系中由右手定则确定；该方法包括下列步骤：

S1、二级分离指令发出时刻之前，将二级箭体坐标系O_C0X₁₀Y₁₀Z₁₀与末级箭体坐标系O_CX₁Y₁Z₁平移至坐标原点重合位置O，

轴与

轴方向相反，

轴与

轴方向相同，

轴与

轴方向相反；设定向量

在二级箭体坐标系OX₁₀Y₁₀Z₁₀中的坐标为[x₁₀ y₁₀z₁₀]^T，在末级箭体坐标系OX₁Y₁Z₁中的坐标为[x₁ y₁ z₁]^T，则二者之间转换关系式为：

S2、使用三个欧拉角：俯仰角

偏航角ψ₀和滚动角γ₀来描述二级箭体坐标系OX₁₀Y₁₀Z₁₀相对发射惯性坐标系OXYZ(通过平移使坐标原点重合)的方位姿态关系，发射惯性坐标系OXYZ经过“321”旋转顺序依次绕相应的坐标轴转动

ψ₀、γ₀得到箭体坐标系OX₁₀Y₁₀Z₁₀，则发射惯性坐标系下坐标[x y z]^T与二级箭体坐标系下坐标[x₁₀ y₁₀ z₁₀]^T的转换关系式为：

其中，

，A0为单位正交矩阵，满足A0^-1＝A0^T；

S3、设欧拉角

ψ₀、γ₀对应的标准四元数为Q0＝[q0₀ q0₁ q0₂ q0₃]^T,根据四元数Q0的元素计算出二级箭体坐标系OX₁₀Y₁₀Z₁₀到发射惯性坐标系OXYZ的转换矩阵A0的表达式为：

与欧拉角相比直接使用四元数进行计算的优点在于可以避免三角函数计算以及不同转换顺序下各角度的定义范围不同的问题；

S4、二级分离指令发出时刻之时，根据步骤S1中所述的转换关系以及步骤S3中所述的转换矩阵A0，得到末级箭体坐标系OX₁Y₁Z₁到发射惯性坐标系OXYZ的转换矩阵A的表达式为：

S5、将末级箭体坐标系OX₁Y₁Z₁相对发射惯性系OXYZ的方位姿态欧拉角记为俯仰角

偏航角ψ和滚动角γ，则使用欧拉角计算得到末级箭体坐标系OX₁Y₁Z₁到发射惯性坐标系OXYZ的转换矩阵A的表达式为：

S6、根据步骤S4得到的转换矩阵A以及步骤S5使用欧拉角计算得到的转换矩阵A，在ψ∈(-π/2，π/2)的条件下由转换矩阵A的元素数值得到对应的欧拉角的表达式为：

S7、根据步骤S6得到的末级箭体坐标系OX₁Y₁Z₁相对发射惯性坐标系OXYZ方位姿态关系的欧拉角

ψ和γ，计算得出对应的四元数Q＝[q₀ q₁ q₂ q₃]^T的表达式为：

基于上述推导的二级箭体坐标系OX₁₀Y₁₀Z₁₀、末级箭体坐标系OX₁Y₁Z₁和发射惯性坐标系OXYZ之间的转换矩阵及相应的欧拉角、四元数计算方法，进行导航、制导和姿态控制算法中箭体坐标系转换设计，以及控制系统仿真模型中箭体坐标系转换设计。考虑到坐标系转换前后，描述箭体姿态的欧拉角/四元数数值需要进行同步切换，同时箭体坐标系下箭体受到的力和力矩也需要进行同步切换，因此选定箭体飞行过程二级箭体坐标系向末级箭体坐标系转换时刻为二级分离指令发出时刻，此时刻处于姿控发动机停控阶段，这种选取方法可以将坐标系转换与飞行控制算法切换、仿真模型参数计算级间切换同步进行，极大程度地为飞行控制软件计算和仿真模型程序计算提供了便利性。

第二方面，本发明涉及一种用于实施运载火箭掉头过程坐标系转换方法的控制仿真应用，所述控制仿真应用在箭体掉头飞行过程控制仿真系统中实现，所述控制仿真应用包括姿态动力学和运动学计算过程的坐标系转换控制仿真实现过程以及导航计算过程的坐标系转换控制仿真实现过程，控制系统仿真模型中姿态动力学和运动学计算过程是于箭体坐标系与发射惯性坐标系进行相关物理量的描述和计算的，所述姿态动力学和运动学计算过程的坐标系转换控制仿真实现过程为：

在坐标系转换时刻之前，在二级箭体坐标系中根据动量矩定理由力矩计算箭体姿态角加速率，根据箭体姿态角加速率进行积分计算得到箭体姿态角速率，根据箭体姿态角速率进行积分计算得到四元数和欧拉角；

在坐标系转换时刻，根据步骤S1中所述的转换关系式将二级箭体坐标系下的箭体姿态角速率

转换为末级箭体坐标系下箭体姿态角速率

并基于末级箭体坐标系中力矩矢量和转动惯量矩阵，以坐标系转换时刻得到的

为初始值进行积分运算，得到末级箭体坐标系下箭体姿态角速率；在坐标系转换时刻，根据步骤S4转换矩阵A的表达式来计算末级箭体坐标系OX₁Y₁Z₁到发射惯性坐标系OXYZ的转换矩阵A，进而根据步骤S6所述的欧拉角表达式和步骤S7所述的四元数表达式来计算在坐标系转换时刻的末级箭体坐标系OX₁Y₁Z₁相对发射惯性坐标系OXYZ的欧拉角

ψ、γ以及对应的四元数Q，以坐标系转换时刻得到的欧拉角

ψ、γ为初始值进行积分运算，得到末级飞行段末级箭体坐标系相对发射惯性坐标系的欧拉角，以坐标系转换时刻得到的四元数Q为初始值进行积分运算，得到末级飞行段末级箭体坐标系相对发射惯性坐标系的四元数；

所述导航计算过程的坐标系转换控制仿真实现过程为：

在坐标系转换时刻之前，使用二级箭体坐标系下的视速度增量数值和角增量数值进行当前采样时刻点数值减前一采样时刻点数值计算，所述视速度增量数值和角增量数值为俯仰通道、偏航通道和滚动通道分别沿箭体坐标系坐标轴方向的视速度增量数值和角增量数值(总共六个参数值)；在坐标系转换时刻之前的各采样时刻点，在二级箭体坐标系下根据前一采样时刻点的四元数数值计算对应的当前采样时刻点的四元数数值；

在坐标系转换时刻，将前一采样时刻点的视速度增量数值和角增量数值根据步骤S1的转换关系式由二级箭体坐标系转换到末级箭体坐标系，当前采样时刻点的视速度增量数值和角增量数值直接使用末级箭体坐标系下的参数值；从坐标系转换时刻的下一采样时刻点开始，使用末级箭体坐标系下的视速度增量数值和角增量数值进行当前采样时刻点数值减前一采样时刻点数值计算；在坐标系转换时刻，根据步骤S3的转换矩阵A0的表达式，采用二级箭体坐标系四元数Q0数值的前一采样时刻点数值Q0(z^-1)来计算二级箭体坐标系OX₁₀Y₁₀Z₁₀到发射惯性坐标系OXYZ的转换矩阵A0，再根据步骤S4的转换矩阵A的表达式计算末级箭体坐标系OX₁Y₁Z₁到发射惯性坐标系OXYZ的转换矩阵A，进而根据步骤S6的欧拉角表达式和步骤S7的四元数表达式来计算得到对应的末级箭体坐标系下的四元数Q数值的前一采样时刻点数值Q(z^-1)，从而计算得到末级箭体坐标系四元数Q的当前采样时刻点数值；从坐标系转换时刻下一采样时刻点开始，各采样时刻点在末级箭体坐标系下根据前一采样时刻点的四元数数值计算对应的当前采样时刻点的四元数数值。

根据上述坐标系转换计算方法，计算得到坐标系转换前后各采样计算时刻点四元数、欧拉角等导航计算参数数值。箭体坐标系转换前后分别基于二级箭体坐标系和末级箭体坐标系完成算法设计和软件程序实现，使得输出姿控发动机开关控制指令控制箭体姿态保持在程序姿态角附近稳定飞行。

箭体飞行过程受到的力和力矩本质上是连续变化的物理量，只与当前飞行状态参数数值有关。二级箭体坐标系向末级箭体坐标系转换时刻取为二级分离指令发出时刻，因此，二级分离之前，控制系统仿真模型中力和力矩计算过程在二级箭体坐标系中建立和求解计算，坐标系转换时刻开始在末级箭体坐标系中建立和求解计算，并且在坐标系转换时刻完成力和力矩计算模型的整体切换。

实施例一：

某三级构型运载火箭采用末级整体倒置结构布局方案，二级滑行段在偏航方向调姿150°完成掉头动作，之后发出二级分离指令。二级分离后末级箭体在偏航方向调至0°偏航角，之后发出整流罩分离指令，然后末级推进发动机开机进行入轨飞行。

引入本发明的箭体坐标系转换设计，选定二级分离指令发出时刻为坐标系转换时刻，由二级箭体坐标系转换到末级箭体坐标系。对于二级分离指令发出时刻之前控制系统各采样计算时刻点以及仿真模型各仿真计算时刻点，导航、制导和姿态控制算法基于表征二级箭体坐标系与发射惯性坐标系方位姿态关系的欧拉角/四元数/转换矩阵进行算法计算，算法中使用到的参数采样计算前点值使用该参数对应的之前采样时刻点数值；仿真模型中使用到的积分算法基于二级箭体坐标系设置计算初始值后进行连续积分运算。

二级分离指令发出时刻提取表征二级箭体坐标系与发射惯性坐标系方位姿态关系的参数量，其中欧拉角为俯仰角

偏航角ψ₀和滚动角γ₀，标准四元数为Q0＝[q0₀ q0₁q0₂ q0₃]^T，二级箭体坐标系到发射惯性坐标系转换矩阵为A0。设任一矢量

在发射惯性坐标系下投影为[x y z]^T，在二级箭体坐标系下坐标为[x₁₀ y₁₀ z₁₀]^T，则有：

二级分离指令发出时刻开始转换到末级箭体坐标系，设该时刻点表征末级箭体坐标系与发射惯性坐标系关系的欧拉角为俯仰角

偏航角ψ和滚动角γ，标准四元数为Q＝[q₀ q₁ q₂ q₃]^T，末级箭体坐标系到发射惯性坐标系转换矩阵为A，

在末级箭体坐标系下坐标为[x₁ y₁ z₁]^T，则有：

ψ＝-sin^-1(a₃₁)，

γ＝atan2(a₃₂,a₃₃)，

导航计算算法中进行以下坐标系转换设计：

俯仰、偏航和滚动通道沿箭体坐标系坐标轴方向的视速度增量/角增量进行当前采样时刻点数值减前一采样时刻点数值计算时，坐标系转换时刻视速度增量/角增量前点值由二级箭体坐标系转换到末级箭体坐标系，当前值直接使用末级箭体坐标系下数值；在坐标系转换时刻之前各采样时刻点，使用二级箭体坐标系下的视速度增量/角增量进行当前值减前一采样时刻点数值计算；坐标系转换时刻下一采样时刻点开始各采样时刻点，使用末级箭体坐标系下的视速度增量/角增量进行当前值减前一采样时刻点数值计算；

根据角增量计算四元数时，需要根据前一采样时刻点四元数数值计算当前采样时刻点四元数数值。坐标系转换时刻，根据二级箭体坐标系四元数Q0数值的前点值Q0(z^-1)计算二级箭体坐标系到发射惯性系的转换矩阵A0，再计算末级箭体坐标系到发射惯性坐标系的转换矩阵A，进而计算末级箭体坐标系四元数Q数值的前点值Q(z^-1)，从而进一步计算末级箭体坐标系四元数Q的当前采样时刻点数值。在坐标系转换时刻之前各采样时刻点，使用二级箭体坐标系下的四元数前点值计算对应的四元数当前采样时刻点数值；在坐标系转换时刻下一采样时刻点开始各时刻点，使用末级箭体坐标系下的四元数前点值计算对应的四元数当前采样时刻点数值。

根据上述坐标系转换计算方法，计算得到坐标系转换前后各采样时刻点四元数、欧拉角等导航计算参数数值，制导系统和姿态控制系统箭体坐标系转换前后分别基于二级箭体坐标系和末级箭体坐标系完成算法设计和软件程序实现。

控制系统仿真计算模型中姿态动力学和运动学计算对应的箭体坐标系转换设计实现方法如下所述：

根据动量矩定理由力矩计算箭体姿态角加速率进一步计算姿态角速率时使用到积分运算。坐标系转换时刻之前，在二级箭体坐标系中进行计算；坐标系转换时刻将二级箭体坐标系箭体姿态角速率

转换为末级箭体坐标系箭体姿态角速率

坐标系转换时刻开始基于末级箭体坐标系下力矩矢量和转动惯量矩阵，以坐标系转换时刻

为初始值进行计算和积分运算，计算得到末级箭体坐标系下的箭体姿态角速率；

根据姿态角速率计算四元数时使用到积分运算。坐标系转换时刻之前，在二级箭体坐标系中进行计算；坐标系转换时刻根据二级箭体坐标系相对发射惯性坐标系标准四元数Q0，逐步计算得到末级箭体坐标系到发射惯性坐标系的转换矩阵A，进而计算末级箭体坐标系相对发射惯性坐标系方位姿态欧拉角

ψ、γ和对应的四元数Q，以坐标系转换时刻四元数Q数值为初始值进行积分运算，计算得到末级飞行段末级箭体坐标系相对发射惯性坐标系的四元数；

根据姿态角速率计算欧拉角时使用到积分运算。坐标系转换时刻之前，在二级箭体坐标系进行计算；坐标系转换时刻根据二级箭体坐标系相对发射惯性坐标系欧拉角

ψ₀和γ₀，逐步计算得到末级箭体坐标系到发射惯性坐标系的转换矩阵A，进而计算末级箭体坐标系相对发射惯性坐标系方位姿态欧拉角

ψ和γ，以坐标系转换时刻

ψ和γ数值为初始值进行积分运算，计算得到末级飞行段末级箭体坐标系相对发射惯性坐标系欧拉角。

控制系统仿真模型中力和力矩计算模块二级分离时刻之前在二级箭体坐标系中建立和求解计算，坐标系转换时刻开始在末级箭体坐标系中建立和求解计算，在坐标系转换时刻完成力和力矩计算模块的整体切换。

完成上述导航、制导和姿态控制算法的坐标系转换设计以及控制系统仿真模型的坐标系转换控制仿真过程之后，开展飞行全程数学仿真试验验证设计及其实现的正确性和合理性。记录箭体坐标系转换前后二级滑行段和末级滑行段Ⅰ飞行过程箭体俯仰角

偏航角ψ和滚动角γ，以及姿态角偏差

Δψ、Δγ和姿态角速率ω_x1、ω_y1、ω_z1，仿真结果曲线见图3～图5。箭体坐标系转换时刻即二级分离指令发出时刻为303.12s，控制系统采样计算周期为10ms。

从飞行全程控制系统仿真结果可以看出，二级分离指令发出时刻箭体坐标系由二级箭体坐标系转换到末级箭体坐标系，导航、制导和姿态控制算法以及控制系统仿真模型中对应的物理参数数值以及计算算法程序均同步实现转换。

上述运载火箭掉头飞行工况箭体坐标系转换、导航制导和姿态控制算法设计以及控制仿真实现方法同样适用于飞行器/运载火箭在其它存在坐标系转换情形下的导航、制导和姿态控制算法设计以及控制系统仿真模型程序实现等理论推演与工程应用中。

Claims (2)

1.一种运载火箭掉头过程坐标系转换方法，其特征在于：选定箭体飞行过程中二级箭体坐标系向末级箭体坐标系的转换时刻为二级分离指令发出时刻，记二级分离前二级箭体坐标系为O_C0X₁₀Y₁₀Z₁₀，原点O_C0为二级箭体质心理论位置；二级分离后末级箭体坐标系为O_CX₁Y₁Z₁，原点O_C为末级箭体质心理论位置；该方法包括下列步骤：

S1、二级分离指令发出时刻之前，将二级箭体坐标系O_C0X₁₀Y₁₀Z₁₀与末级箭体坐标系O_CX₁Y₁Z₁平移至坐标原点重合位置O，

轴与

轴方向相反，

轴与

轴方向相同，

轴与

轴方向相反；设定向量

在二级箭体坐标系OX₁0Y₁0Z₁0中的坐标为[x₁₀ y₁₀z₁₀]^T，在末级箭体坐标系OX₁Y₁Z₁中的坐标为[x₁ y₁ z₁]^T，则二者之间转换关系式为：

S2、使用三个欧拉角：俯仰角

偏航角ψ₀和滚动角γ₀来描述二级箭体坐标系OX₁₀Y₁₀Z₁₀相对发射惯性坐标系OXYZ的方位姿态关系，发射惯性坐标系OXYZ经过“321”旋转顺序依次绕相应的坐标轴转动

ψ₀、γ₀得到箭体坐标系OX₁₀Y₁₀Z₁₀，则发射惯性坐标系下坐标[x yz]^T与二级箭体坐标系下坐标[x₁₀ y₁₀ z₁₀]^T的转换关系式为：

其中，

，

A0为单位正交矩阵，满足A0^-1＝A0^T；

S3、设欧拉角

ψ₀、γ₀对应的标准四元数为Q0＝[q0₀ q0₁ q0₂ q0₃]^T，根据四元数Q0的元素计算出二级箭体坐标系OX₁₀Y₁₀Z₁₀到发射惯性坐标系OXYZ的转换矩阵A0的表达式为：

S4、二级分离指令发出时刻之时，根据步骤S1中所述的转换关系以及步骤S3中所述的转换矩阵A0，得到末级箭体坐标系OX₁Y₁Z₁到发射惯性坐标系OXYZ的转换矩阵A的表达式为：

S5、将末级箭体坐标系OX₁Y₁Z₁相对发射惯性系OXYZ的方位姿态欧拉角记为俯仰角

偏航角ψ和滚动角γ，则使用欧拉角计算得到末级箭体坐标系OX₁Y₁Z₁到发射惯性坐标系OXYZ的转换矩阵A的表达式为：

S6、根据步骤S4得到的转换矩阵A以及步骤S5使用欧拉角计算得到的转换矩阵A，在ψ∈(-π/2，π/2)的条件下由转换矩阵A的元素数值得到对应的欧拉角的表达式为：

S7、根据步骤S6得到的末级箭体坐标系OX₁Y₁Z₁相对发射惯性坐标系OXYZ方位姿态关系的欧拉角

ψ和γ，计算得出对应的四元数Q＝[q₀ q₁ q₂ q₃]^T的表达式为：

2.一种用于实施权利要求1所述的运载火箭掉头过程坐标系转换方法的控制仿真应用，所述控制仿真应用在箭体掉头飞行过程控制仿真系统中实现，其特征在于：所述控制仿真应用包括姿态动力学和运动学计算过程的坐标系转换控制仿真实现过程以及导航计算过程的坐标系转换控制仿真实现过程，所述姿态动力学和运动学计算过程的坐标系转换控制仿真实现过程为：

在坐标系转换时刻之前，在二级箭体坐标系中根据动量矩定理由力矩计算箭体姿态角加速率，根据箭体姿态角加速率进行积分计算得到箭体姿态角速率，根据箭体姿态角速率进行积分计算得到四元数和欧拉角；

在坐标系转换时刻，根据步骤S1中所述的转换关系式将二级箭体坐标系下的箭体姿态角速率

转换为末级箭体坐标系下箭体姿态角速率

并基于末级箭体坐标系中力矩矢量和转动惯量矩阵，以坐标系转换时刻得到的

为初始值进行积分运算，得到末级箭体坐标系下箭体姿态角速率；在坐标系转换时刻，根据步骤S4转换矩阵A的表达式来计算末级箭体坐标系OX₁Y₁Z₁到发射惯性坐标系OXYZ的转换矩阵A，进而根据步骤S6所述的欧拉角表达式和步骤S7所述的四元数表达式来计算在坐标系转换时刻的末级箭体坐标系OX₁Y₁Z₁相对发射惯性坐标系OXYZ的欧拉角

ψ、γ以及对应的四元数Q，以坐标系转换时刻得到的欧拉角

ψ、γ为初始值进行积分运算，得到末级飞行段末级箭体坐标系相对发射惯性坐标系的欧拉角，以坐标系转换时刻得到的四元数Q为初始值进行积分运算，得到末级飞行段末级箭体坐标系相对发射惯性坐标系的四元数；

所述导航计算过程的坐标系转换控制仿真实现过程为：

在坐标系转换时刻之前，使用二级箭体坐标系下的视速度增量数值和角增量数值进行当前样时刻点数值减前一采样时刻点数值计算，所述视速度增量数值和角增量数值为俯仰通道、偏航通道和滚动通道分别沿箭体坐标系坐标轴方向的视速度增量数值和角增量数值；在坐标系转换时刻之前的各采样时刻点，在二级箭体坐标系下根据前一采样时刻点的四元数数值计算对应的当前采样时刻点的四元数数值；

在坐标系转换时刻，将前一采样时刻点的视速度增量数值和角增量数值根据步骤S1的转换关系式由二级箭体坐标系转换到末级箭体坐标系，当前采样时刻点的视速度增量数值和角增量数值直接使用末级箭体坐标系下的参数值；从坐标系转换时刻的下一采样时刻点开始，使用末级箭体坐标系下的视速度增量数值和角增量数值进行当前采样时刻点数值减前一采样时刻点数值计算；在坐标系转换时刻，根据步骤S3的转换矩阵A0的表达式，采用二级箭体坐标系四元数Q0数值的前一采样时刻点数值Q0(z^-1)来计算二级箭体坐标系OX₁₀Y₁₀Z₁₀到发射惯性坐标系OXYZ的转换矩阵A0，再根据步骤S4的转换矩阵A的表达式计算末级箭体坐标系OX₁Y₁Z₁到发射惯性坐标系OXYZ的转换矩阵A，进而根据步骤S6的欧拉角表达式和步骤S7的四元数表达式来计算得到对应的末级箭体坐标系下的四元数Q数值的前一采样时刻点数值Q(z^-1)，从而计算得到末级箭体坐标系四元数Q的当前采样时刻点数值；从坐标系转换时刻下一采样时刻点开始，各采样时刻点在末级箭体坐标系下根据前一采样时刻点的四元数数值计算对应的当前采样时刻点的四元数数值。

Images