CN107037739B

CN107037739B - 一种运载火箭半实物仿真试验惯组模拟方法

Info

Publication number: CN107037739B
Application number: CN201611100052.1A
Authority: CN
Inventors: 王迪; 于亚男; 陈海朋; 余薛浩; 范崇盛; 周嘉炜; 刘思
Original assignee: Shanghai Aerospace Control Technology Institute
Current assignee: Shanghai Aerospace Control Technology Institute
Priority date: 2016-12-02
Filing date: 2016-12-02
Publication date: 2020-02-14
Anticipated expiration: 2036-12-02
Also published as: CN107037739A

Abstract

本发明涉及一种运载火箭控制系统半实物仿真试验惯组模拟的方法，在进行半实物仿真调试、测试及正式试验阶段，可用于替代真实惯组及转台进行仿真试验，另外还具备测试真实转台和惯组动态特性的功能，该方法接收动力学模型的姿态、速度、位置信号，解算得到惯组陀螺及加表测量信号，模拟出惯组及转台的主要功能，实现正常模式及故障模式下的开环、闭环半实物仿真试验，测出转台+惯组组合体的动态特性，并且该方法可对现有转台与惯组的动态特性进行测试和模拟，在测试试验时惯组模拟器可模拟转台和惯组的主要功能，由姿态、速度、位置生成加表和陀螺测量信息，在试验过程中可代替真实设备进行试验，显著降低成本，并降低试验复杂度，简化流程。

Description

一种运载火箭半实物仿真试验惯组模拟方法

技术领域

本发明涉及运载火箭控制系统半实物仿真领域，具体涉及一种运载火箭半实物仿真试验惯组模拟方法。

背景技术

为了保证运载火箭控制系统半实物仿真试验的可靠与价值，惯组被普遍用于飞行及试验的高精度测量，通常还配置双惯组冗余。同时，闭环试验时还需配置大型高精度转台驱动惯组，这些配置大大增加了试验成本。

惯组、转台的动态特性对实时性要求极高的半实物仿真试验有很大影响。目前动态特性测试完全依赖专业的动态分析仪器，大大增加了试验成本。

发明内容

本发明的目的在于克服现有技术的上述不足，提供一种运载火箭半实物仿真试验惯组模拟方法，该模拟方法可以测出转台+惯组组合体的动态特性，并且可以代替真实设备进行试验，大大降低成本，具有更广阔的应用范围。

本发明的上述目的主要是通过如下技术方案予以实现的：

一种运载火箭半实物仿真试验惯组模拟方法，包括如下步骤：

步骤(1)、每个时间周期采集外部发送的姿态、速度和位置信息，并将所述姿态、速度和位置信息输入给惯组传递函数，模拟惯组的动态特性，获得动态姿态、速度和位置信息；

步骤(2)、对所述动态姿态、速度和位置信息进行脉冲反算，得到陀螺脉冲增量和加表脉冲增量；

步骤(3)、对所述陀螺脉冲增量和加表脉冲增量进行故障注入，并将故障注入后的陀螺脉冲增量和加表脉冲增量周期性实时向外输出。

在上述运载火箭半实物仿真试验惯组模拟方法中，还包括进行惯组模拟器参数配置，所述惯组模拟器参数配置为根据需要模拟或者被测惯组的实际参数进行惯组模拟器参数配置，包括安装矩阵E_g、E_a，标度因数K_g、K_a，零偏D_g、D_a。

在上述运载火箭半实物仿真试验惯组模拟方法中，所述步骤(1)中惯组传递函数通过如下方法得到：

步骤(1.1)、每个时间周期将正弦激励信号通过转换矩阵转化为转台框角；

步骤(1.2)、采用所述转台框角控制转台运转，并实时采集惯组陀螺脉冲信号；

步骤(1.3)、将所述惯组陀螺脉冲信号进行导航计算得到姿态角信号，包括俯仰姿态角

偏航姿态角ψ和滚动姿态角γ；

步骤(1.4)、根据所述姿态角信号与正弦激励信号得到惯组及转台组合体的惯组传递函数。

在上述运载火箭半实物仿真试验惯组模拟方法中，所述步骤(1.1)、(1.4)中的正弦激励信号包括俯仰、偏航和滚动姿态角的三个正弦信号，所述三个正弦信号分别根据如下正弦函数生成：

f(t)＝A×sin(w×t)

其中A为幅值，w为圆频率，t为模拟器本地时钟。

在上述运载火箭半实物仿真试验惯组模拟方法中，所述转换矩阵为姿态角到转台框角的转化矩阵Czb，所述转台框角包括转台外框α、中框β和内框λ，具体转换公式如下：

其中：ψ_trig、γ_trig分别为俯仰、偏航和滚动姿态角。

在上述运载火箭半实物仿真试验惯组模拟方法中，所述步骤(1.3)中将所述惯组陀螺脉冲信号进行导航计算得到姿态角信号的具体方法如下：

(1.3.1)、将采集到的相邻两拍脉冲求差，得到周期时间内的陀螺脉冲增量ΔN_g，所述ΔN_g包括惯组陀螺三轴及斜轴的脉冲增量ΔN_gx、ΔN_gy、ΔN_gz、ΔN_gs；

(1.3.2)、根据如下公式得到惯组安装位置的角增量，包括惯组陀螺三轴及斜轴的角增量Δθ_x、Δθ_y、Δθ_z、Δθ_s：

其中：K_gx、K_gy、K_gz分别为惯组陀螺三个轴的标度因数，K_gs为惯组陀螺斜轴的标度因数，D_gx、D_gy、D_gz分别为惯组陀螺三个轴的零偏，D_gs为惯组陀螺斜轴的零偏，ΔT为解算周期，即相邻两拍脉冲的时间差；E_g为安装矩阵。

(1.3.3)、根据惯组安装位置的角增量计算当前拍四元数q_0(n)、q_1(n)、q_2(n)、q_3(n)：

其中：q_0(n-1)、q_1(n-1)、q_2(n-1)、q_3(n-1)为上一拍的四元数；n为正整数，且n≥1；

(1.3.4)、根据四元数q_0(n)、q_1(n)、q_2(n)、q_3(n)计算四元数到姿态角的转换矩阵C，再由四元数到姿态角的转换矩阵C计算姿态角信息,包括俯仰姿态角偏航姿态角ψ_nav、滚动姿态角γ_nav，具体公式如下：

当|C₃₁|<0.9999999时

否则：

在上述运载火箭半实物仿真试验惯组模拟方法中，所述步骤(1.4)中根据所述姿态角信号与正弦激励信号采用最小二乘法得到惯组及转台组合体的惯组传递函数

在上述运载火箭半实物仿真试验惯组模拟方法中，步骤(2)中对所述动态姿态、速度和位置信息进行脉冲反算，得到陀螺脉冲增量和加表脉冲增量的具体方法如下：

(3.1)、根据上一拍的动态姿态

ψ_n-1、γ_n-1生成上一拍的姿态角到四元数的转换矩阵C_n-1，根据当前拍的动态姿态

ψ_n、γ_n生成当前拍的姿态角到四元数的转换矩阵C_n；

(3.2)、根据上一拍的姿态角到四元数的转换矩阵C_n-1和当前拍的姿态角到四元数的转换矩阵C_n计算角增量Δθ_x、Δθ_y、Δθ_z：

(3.3)、根据角增量Δθ_x、Δθ_y、Δθ_z计算惯组陀螺三轴及斜轴的陀螺脉冲增量ΔN_gx、ΔN_gy、ΔN_gz、ΔN_gs：

(3.4)、根据上一拍的位置P_x(n-1)、P_y(n-1)、P_z(n-1)计算上一拍的引力加速度g_x(n-1)、g_y(n-1)、g_z(n-1)，根据当前拍的位置P_xn、P_yn、P_zn计算当前拍的引力加速度g_xn、g_yn、g_zn：

(3.5)、根据上一拍的速度V_x(n-1)、V_y(n-1)、V_z(n-1)和当前拍的速度V_xn、V_yn、V_zn计算视速度增量ΔW_x、ΔW_y、ΔW_z：

(3.6)根据视速度增量ΔW_x、ΔW_y、ΔW_z计算惯组加表三轴及斜轴的加表脉冲增量ΔN_ax、ΔN_ay、ΔN_az、ΔN_as。

在上述运载火箭半实物仿真试验惯组模拟方法中，所述步骤(2)中对所述动态姿态、速度和位置信息进行脉冲反算，得到陀螺脉冲增量和加表脉冲增量的具体方法如下，其中所述动态姿态、速度和位置信息包括上一拍的动态姿态ψ_n-1、γ_n-1，速度V_x(n-1)、V_y(n-1)、V_z(n-1)，位置P_x(n-1)、P_y(n-1)、P_z(n-1)，当前拍的动态姿态

ψ_n、γ_n，速度V_xn、V_yn、V_zn，位置P_xn、P_yn、P_zn：

(2.1)、根据上一拍的动态姿态

ψ_n-1、γ_n-1生成上一拍的姿态角到四元数的转换矩阵C_n-1：

根据当前拍的动态姿态

ψ_n、γ_n生成当前拍的姿态角到四元数的转换矩阵C_n：

(2.2)、计算角增量Δθ_x、Δθ_y、Δθ_z：

(2.2.1)根据上一拍的姿态角到四元数的转换矩阵C_n-1得到上一拍的四元数：

根据当前拍的姿态角到四元数的转换矩阵C_n得到当前拍的四元数：

(2.2.2)、根据上一拍和当前拍的四元数计算角增量Δθ_x、Δθ_y、Δθ_z：

Q_n-1＝[q_(n-1)0 -q_(n-1)1 -q_(n-1)2 -q_(n-1)3]

Q_n＝[q_n0 q_n1 q_n2 q_n3]

若Δq≥10^-5，则

否则：

(2.3)、根据角增量Δθ_x、Δθ_y、Δθ_z计算惯组陀螺三轴及斜轴的陀螺脉冲增量ΔN_gx、ΔN_gy、ΔN_gz、ΔN_gs：

(2.4)、根据上一拍的位置P_x(n-1)、P_y(n-1)、P_z(n-1)计算上一拍的引力加速度g_x(n-1)、g_y(n-1)、g_z(n-1)，根据当前拍的位置P_xn、P_yn、P_zn计算当前拍的引力加速度g_xn、g_yn、g_zn：

(r_n-1)²＝(P_x(n-1))²+(P_y(n-1))²+(P_z(n-1))²

(r_n)²＝(P_xn)²+(P_yn)²+(P_zn)²

其中：GM、R_e、J₂为地球相关常数；

(2.5)、根据上一拍的速度V_x(n-1)、V_y(n-1)、V_z(n-1)和当前拍的速度V_xn、V_yn、V_zn计算视速度增量ΔW_x、ΔW_y、ΔW_z：

(2.6)根据视速度增量ΔW_x、ΔW_y、ΔW_z计算计算惯组加表三轴及斜轴的加表脉冲增量ΔN_ax、ΔN_ay、ΔN_az、ΔN_as：

D_ax、D_ay、D_az分别为惯组加表三个轴的零偏，E_a为安装矩阵；D_as为惯组加表斜轴的零偏。

在上述运载火箭半实物仿真试验惯组模拟方法中，所述步骤(3)中对所述陀螺脉冲增量和加表脉冲增量在t₀时刻注入故障，包括常值故障、常零故障、线性故障和交变故障。

在上述运载火箭半实物仿真试验惯组模拟方法中，所述常值故障、常零故障、线性故障、交变故障注入方式如下：

常值故障：

常零故障：

线性故障：

交变故障：

其中：t为时间，k为线性故障斜率，a为设定常值，t₀为故障注入时刻，A'为交变故障幅值，ω'为交变故障圆频率。

本发明与现有技术相比具有如下有益效果：

(1)、本发明提出的运载火箭控制系统半实物仿真试验惯组模拟方法，可代替真实的转台和惯组，接收动力学模型的姿态、速度、位置信号，解算得到惯组陀螺及加表测量信号，模拟出惯组及转台的主要功能，实现正常模式及故障模式下的开环、闭环半实物仿真试验，测出转台+惯组组合体的动态特性，并且本发明方法可对现有转台与惯组的动态特性进行测试和模拟。

(2)、本发明在进行转台与惯组动态特性测试时，惯组模拟器生成正弦激励信号，转化为框角并发送给转台，模拟器采集惯组测量信号并进行导航姿态信息解算得到姿态角信息，将姿态角信息与激励信号对比分析，可得转台+惯组的动态特性。

(3)、本发明在测试试验时，惯组模拟器可模拟转台和惯组的主要功能，由姿态、速度、位置生成加表和陀螺测量信息，在试验过程中可以代替真实设备进行试验，显著降低成本，并降低试验复杂度，简化流程。

(4)、本发明可实现多惯组的实时模拟，并能对生成的测量信息注入特定故障，完成真实转台和惯组无法实现的试验项目，并且在试验中可以修改配置参数，模拟多套惯组，具有较广的应用范围，实用性强。

附图说明

图1为本发明惯组模拟器测量动态特性原理图。

图2为本发明惯组模拟器功能原理图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步详细的描述：

如图1所示为本发明惯组模拟器测量动态特性原理图，图2为本发明惯组模拟器主要功能原理图，本发明运载火箭半实物仿真试验惯组模拟方法，具体包括如下步骤：

步骤(一)、根据需要模拟或者被测的惯组实际参数进行惯组模拟器参数配置，参数配置包括安装矩阵E_g、E_a，标度因数K_g、K_a，零偏D_g、D_a。

步骤(二)、每个时间周期采集外部发送的姿态、速度和位置信息，并将所述姿态、速度和位置信息输入给惯组传递函数，模拟惯组的动态特性，获得动态姿态、速度和位置信息。

其中惯组传递函数通过如下方法得到：

步骤(2.1)、使能惯组模拟器的内部时钟，时间t每毫秒累加，由正弦激励模块产生正弦信号f(t)＝A[0]*sin(ω[0]*t)，当正弦信号激励次数大于N[0]时，即运行时间t>N[0]*2*π/ω[0]时，正弦激励更新为f(t)＝A[1]*sin(ω[1]*t)，当正弦信号激励次数大于N[1]时，即运行时间t>(N[0]*2*π/ω[0]+N[1]*2*π/ω[1])时，正弦激励更新为f(t)＝A[2]*sin(ω[2]*t)，依此更新正弦激励信号为f(t)＝A[i]*sin(ω[i]*t)，更新时间为

步骤(2.2)、每个时间周期将正弦激励信号通过转换矩阵转化为转台框角。正弦激励信号包括俯仰、偏航和滚动姿态角的三个正弦信号，所述三个正弦信号分别根据如下正弦函数生成：

f(t)＝A×sin(w×t)

其中：A为惯组模拟器设置幅值，w为圆频率，t为模拟器本地时钟，周期性累加，惯组模拟器可同时生成三个正弦信号，作为俯仰、偏航、滚动姿态角的模拟。

转换矩阵为姿态角到转台框角的转化矩阵Czb，转台框角包括转台外框α、中框β和内框λ，具体转换公式如下：

其中：

ψ_trig、γ_trig分别为俯仰、偏航和滚动姿态角。

步骤(2.3)、采用所述转台框角控制转台运转，并实时采集惯组陀螺脉冲信号；

步骤(2.4)、将惯组陀螺脉冲信号进行导航计算得到姿态角信号，包括俯仰姿态角

偏航姿态角ψ和滚动姿态角γ；具体方法如下：

(2.4.1)、将采集到的相邻两拍脉冲求差，得到周期时间内的陀螺脉冲增量ΔN_g，所述ΔN_g包括惯组陀螺三轴及斜轴的脉冲增量ΔN_gx、ΔN_gy、ΔN_gz、ΔN_gs；

(2.4.2)、根据如下公式得到惯组安装位置的角增量，包括惯组陀螺三轴及斜轴的角增量Δθ_x、Δθ_y、Δθ_z、Δθ_s：

(2.4.3)、根据惯组安装位置的角增量计算当前拍四元数q_0(n)、q_1(n)、q_2(n)、q_3(n)：

其中：q_0(n-1)、q_1(n-1)、q_2(n-1)、q_3(n-1)为上一拍的四元数；n为正整数，且n≥1。其中初始值即q₀₀、q₀₁、q₀₂、q₀₃为根据试验需求给出，为已知量。且四元数q_0(n)、q_1(n)、q_2(n)、q_3(n)为脉冲计算姿态角的中间变量。

(2.4.4)、根据四元数q_0(n)、q_1(n)、q_2(n)、q_3(n)计算四元数到姿态角的转换矩阵C，再由四元数到姿态角的转换矩阵C计算姿态角信息,包括俯仰姿态角

偏航姿态角ψ_nav、滚动姿态角γ_nav，具体公式如下：

当|C₃₁|<0.9999999时

否则：

步骤(2.4)、根据所述姿态角信号与正弦激励信号采用最小二乘法得到惯组及转台组合体的惯组传递函数。

步骤(三)、对动态姿态、速度和位置信息进行脉冲反算，得到陀螺脉冲增量和加表脉冲增量。其中动态姿态、速度和位置信息包括上一拍的动态姿态

ψ_n-1、γ_n-1，速度V_x(n-1)、V_y(n-1)、V_z(n-1)，位置P_x(n-1)、P_y(n-1)、P_z(n-1)，当前拍的动态姿态

ψ_n、γ_n，速度V_xn、V_yn、V_zn，位置P_xn、P_yn、P_zn。

具体方法如下：

(3.1)、根据上一拍的动态姿态

ψ_n-1、γ_n-1生成上一拍的姿态角到四元数的转换矩阵C_n-1：

根据当前拍的动态姿态ψ_n、γ_n生成当前拍的姿态角到四元数的转换矩阵C_n：

(3.2)、计算角增量Δθ_x、Δθ_y、Δθ_z：

(3.2.1)首先根据上一拍的姿态角到四元数的转换矩阵C_n-1得到上一拍的四元数：

上述四元数q_(n-1)0～q_(n-1)3，q_n0～q_n3为姿态角计算脉冲的中间变量。

(3.2.2)、根据上一拍和当前拍的四元数计算角增量Δθ_x、Δθ_y、Δθ_z：

Q_n-1＝[q_(n-1)0 -q_(n-1)1 -q_(n-1)2 -q_(n-1)3]

Q_n＝[q_n0 q_n1 q_n2 q_n3]

若Δq≥10^-5，则

否则：

其中Δq为中间变量。

其中ΔT为解算周期，即相邻两拍脉冲的时间差。

为安装矩阵。

(r_n-1)²＝(P_x(n-1))²+(P_y(n-1))²+(P_z(n-1))²

(r_n)²＝(P_xn)²+(P_yn)²+(P_zn)²

其中：GM、R_e、J₂为地球相关常数；

其中：C_n为转换矩阵，步骤(3.1)得到。

(3.6)根据视速度增量ΔW_x、ΔW_y、ΔW_z计算计算惯组加表三轴及斜轴的加表脉冲增量ΔN_ax、ΔN_ay、ΔN_az、ΔN_as：

其中：D_ax、D_ay、D_az分别为惯组加表三个轴的零偏，D_as为惯组加表斜轴的零偏。

E_a为安装矩阵；且

步骤(四)、对陀螺脉冲增量ΔN_gx、ΔN_gy、ΔN_gz、ΔN_gs和加表脉冲增量ΔN_ax、ΔN_ay、ΔN_az、ΔN_as进行故障注入。并将故障注入后的陀螺脉冲增量和加表脉冲增量周期性实时向外输出给控制计算机。

即对惯组模拟器陀螺的X、Y、Z、S轴及加表的X、Y、Z、S轴脉冲增量在t₀时刻注入故障，包括常值故障、常零故障、线性故障、交变故障。注入方式如下：

常值故障：

常零故障：

线性故障：

交变故障：

本发明提出的运载火箭控制系统半实物仿真试验惯组模拟方法是在半实物仿真时利用惯组模拟器替换转台和惯组组合体，采集模型解算出的姿态、速度、位置信息，将该信息反算成测量信息再发送给控制计算机，另外，该发明可实现对真实转台和惯组组合体的动态特性测试与模拟。

为了保证运载火箭控制系统半实物仿真试验的可靠与价值，惯组被普遍用于飞行及试验的高精度测量，通常还配置双惯组冗余。同时，闭环试验时还需配置大型高精度转台驱动惯组，这些配置大大增加了试验成本。在未配全这些设备、产品时，采用惯组模拟器可显著节约成本。

惯组、转台的动态特性对实时性要求极高的半实物仿真试验有很大影响。目前动态特性测试完全依赖专业的动态分析仪器，增加了试验成本，利用惯组模拟器即能完成该项测试并能进一步模拟该特性。

不同系列的运载火箭通常配有不同惯组，同一型号的惯组参数也可能有较大差异，采用惯组模拟器可通过配置参数的方式模拟各种不同类型的惯组，可便捷的进行算法分析、参数分析、闭环试验。

本发明在进行半实物仿真调试、测试及正式试验阶段，可用于替代真实惯组及转台进行仿真试验，另外还具备测试真实转台和惯组动态特性的功能。真实惯组一般包含陀螺及加表测量装置，地面试验时需将惯组放置在转台上，随着转台的运动给出测量信号。惯组模拟器主要包含以下功能：测量与模拟转台与惯组组合体的动态特性；通过配置参数模拟不同惯组的主要特性；根据姿态、速度、位置信息模拟惯组的陀螺及加表输出；根据需求模拟惯组的多种故障模式。在不具备转台或者惯组时，可用惯组模拟器进行试验；在系统调试阶段，可用惯组模拟器进行测试、调试与算法验证。

本发明提出的运载火箭控制系统半实物仿真试验惯组模拟方法，该方法的应用可替换转台和惯组，接收动力学模型的姿态、速度、位置信号，解算得到惯组陀螺及加表测量信号，模拟出惯组及转台的主要功能，实现正常模式及故障模式下的开环、闭环半实物仿真试验。另外，应用此方法可对现有转台与惯组的动态特性进行测试和模拟。

以上所述，仅为本发明最佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。