一种运载火箭质量特性及动力学关键参数在线辨识方法

摘要

本发明涉及一种运载火箭质量特性及动力学关键参数在线辨识方法，该方法基于六自由度飞行动力学方程和未知参数动态方程，通过构造增广状态方程实现对状态和参数的同时估计，从而可同时获得需要辨识的参数向量及状态向量，这种方法最大的优势在于可以根据输出量对未知参数进行估计，在系统发生变化时，可以准确获得质量特性及动力学关键参数的估计结果，可为运载火箭控制参数在线优化调整奠定关键的技术基础；同时该辨识方法可以实现在线状态和参数同时估计，对降低火箭系统偏差设计的保守性和提高火箭对故障状态的适应性具有重大意义，可为未来火箭设计提供有益的参考。

说明书

技术领域

本发明涉及一种运载火箭质量特性及动力学关键参数在线辨识方法，属于控制系统设计技术领域。

背景技术

目前我国运载火箭控制系统设计存在着对系统偏差适应性差、设计水平低、设计余量偏保守的问题，尤其是控制系统设计均是针对正常工况进行的，对故障状态的适应性较差，无法准确估计飞行过程中的关键动力学参数，并据此实现控制参数的在线调整以应对各种可能的飞行工况。因此，急需开展运载火箭关键特性参数在线辨识技术研究，一方面，可供现役火箭使用，有利于摸清系统偏差，降低系统参数设计的保守性，进一步挖掘型号潜力，另一方面，也可为未来火箭实现控制参数在线优化调整奠定坚实的技术基础，可以大幅降低飞行偏差及非灾难性故障对飞行任务的影响。

当前运载火箭控制系统均是基于标称状态的参数开展设计的，这些参数与实物产品、飞行工况存在着一定的差异，往往通过较宽的偏差带开展包络设计，而姿控系统设计就是设计一套能够包络各种工况的参数，这必然带来的一定的保守性。在型号研制过程中，经常发生由于偏差带过宽，姿控系统设计困难等问题。虽然某些参数可以进行精确测量，或者通过大量地面试验来加以精确获取，但是，这种方法存在两大问题，一是由于天地差异性，某些参数无法准确获取，二是获取准确参数的代价过大，而且每发产品的散布较大，很难做到一劳永逸。此外，一旦飞行过程中发生故障，基于标称状态设计的控制参数将无法实现控制系统稳定，需要根据实际飞行特性对控制参数进行适应性调整，而这一切都离不开获取准确的运载火箭质量特性及动力学关键参数。

传统的运载火箭关键特性参数辨识技术均为离线辨识技术，往往是在飞行结果数据获取以后，根据飞行结果数据通过优化算法进行参数辨识，这种方法一方面仿真计算量较大，另一方面无法实现参数在线辨识。

因此，急需开展在线参数辨识技术研究，尤其是质量特性及动力学特性参数的在线辨识技术研究，这些参数是决定控制系统设计的关键特性参数。由于目前火箭尚未有办法直接测量这些关键参数(包括质量、转动惯量、质心、压心)，需要研究高性能参数辨识算法对以上参数进行在线辨识。

发明内容

本发明的目的在于克服现有技术的上述不足，提供一种运载火箭质量特性及动力学关键参数在线辨识方法，该辨识方法可以实现在线状态和参数同时估计，对降低火箭系统偏差设计的保守性和提高火箭对故障状态的适应性具有重大意义，可为未来火箭设计提供有益的参考。

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

一种运载火箭质量特性及动力学关键参数在线辨识方法，包括如下步骤：

(1)、根据运载火箭六自由度飞行动力学方程，得到状态方程和测量方程；

(2)、将所述状态方程和测量方程转换为状态向量方程；

(3)、根据所述状态向量方程和需要辨识的参数向量方程组合后得到增广状态方程；

(4)、将增广状态方程进行离散化，转化为离散迭代方程；

(5)、根据所述离散迭代方程，采用滤波算法得到需要辨识的参数向量，同时得到状态向量。

在上述运载火箭质量特性及动力学关键参数在线辨识方法中，步骤(1)中的状态方程如下：

$>\left(\begin{array}{c}{\stackrel{·}{X}}_a\\ {\stackrel{·}{Y}}_a\\ {\stackrel{·}{Z}}_a\end{array}\right)=\left(\begin{array}{c}{V}_{ax}\\ V_{ay}\\ V_{az}\end{array}\right)---\left(8\right)$>

测量方程如下：

$>\left(\begin{array}{c}{X}_a^{\prime }\\ Y_a^{\prime }\\ Z_a^{\prime }\end{array}\right)=\left(\begin{array}{c}{X}_a\\ Y_a\\ Z_a\end{array}\right)+\left(\begin{array}{c}{v}_{xx}\\ v_{xy}\\ v_{xz}\end{array}\right)---\left(12\right)$>

$>\left(\begin{array}{c}{V}_{ax}^{\prime }\\ V_{ay}^{\prime }\\ V_{az}^{\prime }\end{array}\right)=\left(\begin{array}{c}{V}_{ax}\\ V_{ay}\\ V_{az}\end{array}\right)+\left(\begin{array}{c}{v}_{vx}\\ v_{vy}\\ v_{vz}\end{array}\right)---\left(13\right)$>

$>\left(\begin{array}{c}{\omega }_{x_1}^{\prime }\\ {\omega }_{y_1}^{\prime }\\ {\omega }_{z_1}^{\prime }\end{array}\right)=\left(\begin{array}{c}{\omega }_{x_1}\\ {\omega }_{y_1}\\ {\omega }_{z_1}\end{array}\right)+\left(\begin{array}{c}{v}_{\omega x}\\ v_{\omega y}\\ v_{\omega z}\end{array}\right)---\left(15\right)$>

其中：w_i为状态噪声；v_i为测量噪声，X_a,Y_a,Z_a为发射惯性坐标系下的坐标，V_ax,V_ay,V_az为发射惯性坐标系下x、y、z方向的速度，ψ,γ为发射惯性坐标系下的姿态角，为箭体坐标系下绕x、y、z轴的姿态角速度；δ_ψ,δ_γ为发动机摆角；g_ax,g_ay,g_az为发射惯性坐标系下x、y、z方向的引力加速度；X_r为发动机推力作用点与火箭理论尖点的距离，Z_r为发动机推力作用点与火箭轴线的距离；M为火箭质量，J_x1,J_y1,J_z1为箭体坐标系下绕x、y、z轴的火箭转动惯量，X_z为火箭质心与火箭理论尖点的距离，P为单台发动机推力。

在上述运载火箭质量特性及动力学关键参数在线辨识方法中，步骤(2)中状态向量方程如下：

$>\stackrel{·}{x}\left(t\right)=f(x,\lambda ,t)+w_x\left(t\right)---\left(16\right)$>

z(t)＝h(x,t)+v(t) (17)

其中：为系统的状态向量，λ(t)＝(λ₁λ₂)^T为系统的未知参数向量，λ₁＝M，λ₂＝P，为系统的输出向量，h(x,t)＝I_12×12x(t)，I_12×12为与未知参数向量不相关的12维*12维的单位矩阵，f(x,λ,t)为与未知参数向量相关的非线性函数向量，表达式如下：

其中：f_x(λ₁),f_y(λ₁),f_z(λ₁),f_Xz(λ₁)均为与λ₁相关的已知函数，w_x(t)为系统状态噪声向量；v(t)为系统测量噪声向量。

在上述运载火箭质量特性及动力学关键参数在线辨识方法中，系统状态噪声向量w_x(t)均值为零，方差矩阵为的高斯白噪声；所述系统测量噪声向量v(t)均值为零，方差矩阵为R＝E[v(t)v^T(t)]的高斯白噪声。

在上述运载火箭质量特性及动力学关键参数在线辨识方法中，步骤(3)中根据状态向量方程和需要辨识的参数向量方程得到增广状态方程具体方法如下：

需要辨识的参数向量方程如下：

$>\stackrel{·}{\lambda }\left(t\right)=g\left(t\right)+w_{\lambda }\left(t\right)---\left(18\right)$>

其中：g(t)＝(f_M(t)f_P(t))^T为各种工况下的已知函数，w_λ(t)为参数噪声向量，f_M(t)为质量随时间变化函数，f_P(t)为推力随时间变化函数；

结合(16)和(18)式，将系统中的未知参数向量扩展为增广系统的状态向量，得到增广系统的状态向量为：

$>X\left(t\right)=\left(\begin{array}{c}x\left(t\right)\\ \lambda \left(t\right)\end{array}\right)---\left(19\right)$>

由此得到增广状态方程为：

$>\stackrel{·}{X}\left(t\right)=\left(\begin{array}{c}\stackrel{·}{x}\left(t\right)\\ \stackrel{·}{\lambda }\left(t\right)\end{array}\right)=\left(\begin{array}{c}f(x,\lambda ,t)\\ g\left(t\right)\end{array}\right)+\left(\begin{array}{c}{w}_x\left(t\right)\\ w_{\lambda }\left(t\right)\end{array}\right).---\left(20\right)$>

在上述运载火箭质量特性及动力学关键参数在线辨识方法中，参数噪声向量w_λ(t)均值为零，方差矩阵为的高斯白噪声。

在上述运载火箭质量特性及动力学关键参数在线辨识方法中，步骤(4)中将增广状态方程离散化的具体方法如下：

将增广状态方程离散化可得：

$>\begin{array}{c}X(k+1)=\left(\begin{array}{c}x(k+1)\\ \lambda (k+1)\end{array}\right)=\left(\begin{array}{c}x\left(k\right)+f\left(x\right(k),\lambda (k\left)\right)T\\ \lambda \left(k\right)+g\left(k\right)T\end{array}\right)+\left(\begin{array}{c}{w}_x\left(k\right)\\ w_{\lambda }\left(k\right)\end{array}\right)\\ =\left(\begin{array}{c}x\left(k\right)\\ \lambda \left(k\right)\end{array}\right)+\left(\begin{array}{c}f\left(x\right(k),\lambda (k\left)\right)\\ g\left(k\right)\end{array}\right)T+\left(\begin{array}{c}{w}_x\left(k\right)\\ w_{\lambda }\left(k\right)\end{array}\right)=X\left(k\right)+F\left(X\right(k\left)\right)T+w\left(k\right)\end{array}---\left(21\right)$>

其中：T为采样周期，k＝1,2,...，

$>F\left(X\right(k\left)\right)=\left(\begin{array}{c}f\left(x\right(k),\lambda (k\left)\right)\\ g\left(k\right)\end{array}\right),w\left(k\right)=\left(\begin{array}{c}{w}_x\left(k\right)\\ w_{\lambda }\left(k\right)\end{array}\right),$>

z(k)＝h(x(k))+v(k) (22)

其中：f(x(k),λ(k))为f(x,λ,t)离散化后将x(k)和λ(k)代入得到；h(x(k))为h(x,t)离散化后将x(k)代入得到；g(k)为g(t)离散化得到。

在上述运载火箭质量特性及动力学关键参数在线辨识方法中，步骤(5)中采用滤波算法得到需要辨识的参数向量及状态向量的具体方法如下：

(1)、根据第k-1步计算结果初始化P_λ(k|k)，P_x(k|k)，V(k)，其中k＝1,2,...；

(2)、计算Φ(k)，Ψ(k)；

$>\Phi \left(k\right)=I_{12\times 12}+A\left(\hat{x}\right(k|k),\hat{\lambda }(k|k\left)\right)T$>

$>\Psi \left(k\right)=B\left(\hat{x}\right(k|k),\hat{\lambda }(k|k\left)\right)T$>

其中：

$>A\left(\hat{x}\right(k|k),\hat{\lambda }(k|k\left)\right)=\frac{\partial f(x,\lambda ,t)}{\partial x}{|}_{x=\hat{x}\left(k\right|k),\lambda =\hat{\lambda }\left(k\right|k)}$>

$>B\left(\hat{x}\right(k|k),\hat{\lambda }(k\left|k\right))=\frac{\partial f(x,\lambda ,t)}{\partial \lambda }{|}_{x=\hat{x}\left(k\right|k),\lambda =\hat{\lambda }\left(k\right|k)};$>

(3)、计算第k+1步参数最优预测：

$>\hat{\lambda }(k+1|k)=\hat{\lambda }\left(k\right|k)+g\left(k\right)T---\left(23\right)$>

同时计算第k+1步状态最优预测：

$>\hat{x}(k+1|k)=\hat{x}\left(k\right|k)+f\left(\hat{x}\right(k|k),\hat{\lambda }(k|k\left)\right)T---\left(24\right)$>

其中：为f(x,λ,t)离散化将和代入得到；

(4)、计算第k+1步参数预测误差方差阵：

P_λ(k+1|k)＝P_λ(k|k)+Q_λ>

同时计算第k+1步状态预测误差方差阵：

P_x(k+1|k)＝Φ(k)P_x(k|k)Φ^T(k)+Ψ(k)P_λ(k|k)Ψ^T(k)+Q_x>

(5)、计算H(k+1)，Φ(k+1)，Ψ(k+1)：

$>\Phi (k+1)=I+A\left(\hat{x}\right(k+1|k),\hat{\lambda }(k+1|k\left)\right)T---\left(27\right)$>

$>\Psi (k+1)=B\left(\hat{x}\right(k+1|k),\hat{\lambda }(k+1|k\left)\right)T---\left(28\right)$>

$>H(k+1)=\frac{\partial h(x,t)}{\partial x}{|}_{x=\hat{x}(k+1|k)}---\left(29\right)$>

其中：$>A\left(\hat{x}\right(k+1|k),\hat{\lambda }(k+1|k\left)\right)=\frac{\partial f(x,\lambda ,t)}{\partial x}{|}_{x=\hat{x}(k+1|k),\lambda =\hat{\lambda }(k+1|k)},$>

$>B\left(\hat{x}\right(k+1|k),\hat{\lambda }(k+1|k\left)\right)=\frac{\partial f(x,\lambda ,t)}{\partial \lambda }{|}_{x=\hat{x}(k+1|k),\lambda =\hat{\lambda }(k+1|k)};$>

(6)、计算状态最优增益矩阵：

K_x(k+1)＝P_x(k+1|k)H^T(k+1)[H(k+1)P_x(k+1|k)H^T(k+1)+R]^-1>

(7)、计算加权矩阵：

U(k+1)＝Φ(k+1)V(k)+Ψ(k+1) (31)

S(k+1)＝H(k+1)U(k+1) (32)

V(k+1)＝U(k+1)-K_x(k+1)S(k+1)>

K(k+1)＝K_x(k+1)+V(k+1)K_λ(k+1)>

(8)、计算参数最优增益矩阵：

$>\begin{array}{c}{K}_{\lambda }(k+1)=P_{\lambda }(k+1|k)S^T(k+1)[S(k+1)P_{\lambda }(k+1|k)S^T(k+1)\\ +H(k+1)P_x(k+1|k)H^T(k+1)+R{]}^{-1}\end{array}---\left(35\right)$>

(9)、计算参数最优估计值：

$>\hat{\lambda }(k+1|k+1)=\hat{\lambda }(k+1|k)+K_{\lambda }(k+1)[z\left(k\right)-h\left(\hat{x}\right(k+1|k\left)\right)]---\left(36\right)$>

其中：z(k)表示第k步的测量值；

同时计算状态最优估计值：

$>\hat{x}(k+1|k+1)=\hat{x}(k+1|k)+K(k+1)[z(k+1)-h\left(\hat{x}\right(k+1|k\left)\right)]---\left(37\right)$>

其中：为h(x,t)离散化将代入得到；

(10)、计算参数滤波误差方差阵：

P_λ(k+1|k+1)＝[I-K_λ(k+1)S(k+1)]P_λ(k+1|k)>

同时计算状态滤波误差方差阵：

P_x(k+1|k+1)＝[I-K_x(k+1)H(k+1)]P_x(k+1|k)>

(11)、将上述第k步计算的第k+1步的预估值P_λ(k+1|k+1)、P_x(k+1|k+1)、V(k+1)初始化P_λ(k|k)，P_x(k|k)，V(k)，其中k＝1,2,...，返回步骤(2)。

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

(1)、本发明提出的基于状态与参数解耦估计的运载火箭质量特性及动力学关键参数在线辨识方法，该方法可以实现在线状态和参数同时估计，对降低火箭系统偏差设计的保守性和提高火箭对故障状态的适应性具有重大意义，可为未来火箭设计提供有益的参考；

(2)、本发明提出的基于状态与参数解耦估计的运载火箭质量特性及动力学关键参数在线辨识方法，该方法基于六自由度飞行动力学方程和未知参数动态方程，通过构造增广状态方程实现对状态和参数的同时估计，从而可同时获得需要辨识的参数向量及状态向量，这种方法最大的优势在于可以根据输出量对未知参数进行估计，在系统发生变化时，可以准确获得质量特性及动力学关键参数的估计结果，可为运载火箭控制参数在线优化调整奠定关键的技术基础；

(3)、本发明研究成果可应用于我国各型运载火箭控制系统研制中，也可应用于各类飞行器的控制系统设计中，此外，本发明方法具有一般性，也可推广至一般性的控制系统设计中，实用性强。

附图说明

图1为本发明运载火箭质量特性及动力学关键参数在线辨识方法流程图；

图2为本发明实施例中X方向位置估计结果；

图3为本发明实施例中Y方向位置估计结果；

图4为本发明实施例中Z方向位置估计结果；

图5为本发明实施例中俯仰姿态角估计结果；

图6为本发明实施例中偏航姿态角估计结果；

图7为本发明实施例中滚动姿态角估计结果；

图8为本发明实施例中质量估计结果；

图9为本发明实施例中推力估计结果。

具体实施方式

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

本发明在进行运载火箭质量特性和动力学特性参数在线辨识之前，首先要建立各个特征量之间的动力学关系，这是实现参数辨识的关键。然后采用基于状态与参数解耦估计的扩展卡尔曼滤波算法，对未知质量特性和动力学特性参数进行辨识。为了简化分析问题的难度，以大气层外飞行段为例开展相关的技术研究。

如图1所示为本发明运载火箭质量特性及动力学关键参数在线辨识方法流程图；首先建立六自由度飞行动力学方程和未知参数动态方程，然后将未知参数辨识问题转化为状态与参数解耦估计的问题，最后通过状态和参数联合估计的卡尔曼滤波算法得到未知质量特性及动力学关键参数的估计结果，具体实现方法如下：

第一步，建立飞行动力学方程。

根据基本的飞行动力学原理，不加推导的直接给出发惯系下的火箭飞行动力学方程：

$>\left(\begin{array}{c}{\stackrel{·}{X}}_a\\ {\stackrel{·}{Y}}_a\\ {\stackrel{·}{Z}}_a\end{array}\right)=\left(\begin{array}{c}{V}_{ax}\\ V_{ay}\\ V_{za}\end{array}\right)---\left(1\right)$>

$>\left(\begin{array}{c}{\stackrel{·}{V}}_{ax}\\ {\stackrel{·}{V}}_{ay}\\ {\stackrel{·}{V}}_{az}\end{array}\right)=\left(\begin{array}{c}{A}_{ax}\\ A_{ay}\\ A_{az}\end{array}\right)+\left(\begin{array}{c}{g}_{ax}\\ g_{ay}\\ g_{az}\end{array}\right)---\left(2\right)$>

其中，

箭体系下的火箭绕质心转动动力学方程：

质量特性关系式：

$>\begin{array}{c}{X}_z=f_{Xz}\left(M\right)\\ J_{x1}=f_x\left(M\right)\\ J_{y1}=f_y\left(M\right)\\ J_{z1}=f_z\left(M\right)\end{array}---\left(5\right)$>

在飞行过程中火箭质量存在如下动态方程：

$>\stackrel{·}{M}=f_M\left(t\right)---\left(6\right)$>

在飞行过程中火箭推力存在如下动态方程：

$>\stackrel{·}{P}=f_P\left(t\right)---\left(7\right)$>

其中：X_a,Y_a,Z_a为发射惯性坐标系下的坐标，V_ax,V_ay,V_az为发射惯性坐标系下x、y、z方向的速度，A_ax,A_ay,A_az为发射惯性坐标系下x、y、z方向的视加速度，A_x1,A_y1,A_z1为箭体坐标系下x、y、z方向的视加速度，ψ,γ为发射惯性坐标系下x、y、z方向的姿态角，为箭体坐标系下x、y、z方向的角速度，均可以通过导航计算获取；δ_ψ,δ_γ为发动机摆角，可以通过控制指令获取；g_ax,g_ay,g_az为发射惯性坐标系下x、y、z方向的引力加速度；X_r为发动机推力作用点与火箭理论尖点的距离，Z_r为发动机推力作用点与火箭轴线的距离；M为火箭质量，J_x1,J_y1,J_z1为箭体坐标系下x、y、z方向的火箭转动惯量，X_z为火箭质心与火箭理论尖点的距离，P为单台发动机推力，均为需要辨识的量；f_M(t)为质量随时间变化函数，f_P(t)为推力随时间变化函数，f_Xz(M)，f_x(M)，f_y(M)，f_z(M)为随质量变化的函数，上述函数均为已知函数。

考虑到状态和测量参数存在一定的噪声和摄动，可将上述方程写为如下形式：

状态方程：

$>\left(\begin{array}{c}{\stackrel{·}{X}}_a\\ {\stackrel{·}{Y}}_a\\ {\stackrel{·}{Z}}_a\end{array}\right)=\left(\begin{array}{c}{V}_{ax}\\ V_{ay}\\ V_{az}\end{array}\right)---\left(8\right)$>

测量方程：

$>\left(\begin{array}{c}{X}_a^{\prime }\\ Y_a^{\prime }\\ Z_a^{\prime }\end{array}\right)=\left(\begin{array}{c}{X}_a\\ Y_a\\ Z_a\end{array}\right)+\left(\begin{array}{c}{v}_{xx}\\ v_{xy}\\ v_{xz}\end{array}\right)---\left(12\right)$>

$>\left(\begin{array}{c}{V}_{ax}^{\prime }\\ V_{ay}^{\prime }\\ V_{az}^{\prime }\end{array}\right)=\left(\begin{array}{c}{V}_{ax}\\ V_{ay}\\ V_{az}\end{array}\right)+\left(\begin{array}{c}{v}_{vx}\\ v_{vy}\\ v_{vz}\end{array}\right)---\left(13\right)$>

$>\left(\begin{array}{c}{\omega }_{x_1}^{\prime }\\ {\omega }_{y_1}^{\prime }\\ {\omega }_{z_1}^{\prime }\end{array}\right)=\left(\begin{array}{c}{\omega }_{x_1}\\ {\omega }_{y_1}\\ {\omega }_{z_1}\end{array}\right)+\left(\begin{array}{c}{v}_{\omega x}\\ v_{\omega y}\\ v_{\omega z}\end{array}\right)---\left(15\right)$>

其中，w_i为状态噪声，在正常飞行工况下为零均值高斯白噪声，在故障工况下为非零均值有色噪声，可通过故障模式进行建模；v_i为测量噪声，为零均值高斯白噪声。

第二步，转换为状态空间方程。

为了便于采用滤波算法实现运载火箭质量特性及动力学关键参数在线辨识，将上述动力学方程改写为如下状态空间方程的形式(由公式(8)、(9)、(10)、(11)直接得到公式(16)，由公式(12)、(13)、(14)、(15)直接得到公式(17)：

$>\stackrel{·}{x}\left(t\right)=f(x,\lambda ,t)+w_x\left(t\right)---\left(16\right)$>

z(t)＝h(x,t)+v(t) (17)

其中：为系统的状态向量，λ(t)＝(λ₁λ₂)^T为系统的未知参数向量，λ₁＝M，λ₂＝P，为系统的输出向量，h(x,t)＝I_12×12x(t)，I_12×12为与未知参数向量不相关的12维*12维的单位矩阵，f(x,λ,t)为与未知参数向量相关的非线性函数向量，表达式如下：

其中：f_x(λ₁),f_y(λ₁),f_z(λ₁),f_Xz(λ₁)均为与λ₁相关的已知函数，例如本实施例中为：

f_x(λ₁)＝5.7978×10^-3λ₁+3.5436×10⁴

f_y(λ₁)＝2.523×10^-9λ₁³-5.764×10^-4λ₁²+55.3322λ₁+9.28155×10⁵

f_z(λ₁)＝2.523×10^-9λ₁³-5.764×10^-4λ₁²+55.3322λ₁+9.28155×10⁵

f_Xz(λ₁)＝1.000×10^-14λ₁³-2.7956×10^-9λ₁²+2.5586×10^-4λ₁+11.5529

w_x(t)为系统状态噪声向量，为均值为零，方差矩阵为的高斯白噪声，v(t)为系统测量噪声向量，为均值为零，方差矩阵为R＝E[v(t)v^T(t)]的高斯白噪声。

第三步，建立增广系统方程。

在上述状态空间方程中，λ是需要进行辨识的参数。要准确获取未知参数λ，首先需要得到λ的动态规律，对原系统进行增广，然后采用滤波算法对其进行辨识。

λ(t)可以由下面的微分方程描述其变化过程：

$>\stackrel{·}{\lambda }\left(t\right)=g\left(t\right)+w_{\lambda }\left(t\right)---\left(18\right)$>

其中：g(t)＝(f_M(t)f_P(t))^T为各种工况下的已知函数，w_λ(t)为参数噪声向量，为均值为零，方差矩阵为的高斯白噪声。

f_M(t)为质量随时间变化函数，f_P(t)为推力随时间变化函数；

结合(16)和(18)式，可将系统中的未知参数扩展为增广系统的状态，得到增广状态为

$>X\left(t\right)=\left(\begin{array}{c}x\left(t\right)\\ \lambda \left(t\right)\end{array}\right)---\left(19\right)$>

由此得到增广状态方程为

$>\stackrel{·}{X}\left(t\right)=\left(\begin{array}{c}\stackrel{·}{x}\left(t\right)\\ \stackrel{·}{\lambda }\left(t\right)\end{array}\right)=\left(\begin{array}{c}f(x,\lambda ,t)\\ g\left(t\right)\end{array}\right)+\left(\begin{array}{c}{w}_x\left(t\right)\\ w_{\lambda }\left(t\right)\end{array}\right)---\left(20\right)$>

因此，系统未知参数估计的实质就是把未知参数扩展为系统的状态，然后使用各种扩展卡尔曼滤波方法进行增广状态的估计，从而得到未知参数的估计值。

第四步，将增广系统模型离散化。

将增广系统离散化可得：

$>\begin{array}{c}X(k+1)=\left(\begin{array}{c}x(k+1)\\ \lambda (k+1)\end{array}\right)=\left(\begin{array}{c}x\left(k\right)+f\left(x\right(k),\lambda (k\left)\right)T\\ \lambda \left(k\right)+g\left(k\right)T\end{array}\right)+\left(\begin{array}{c}{w}_x\left(k\right)\\ w_{\lambda }\left(k\right)\end{array}\right)\\ =\left(\begin{array}{c}x\left(k\right)\\ \lambda \left(k\right)\end{array}\right)+\left(\begin{array}{c}f\left(x\right(k),\lambda (k\left)\right)\\ g\left(k\right)\end{array}\right)T+\left(\begin{array}{c}{w}_x\left(k\right)\\ w_{\lambda }\left(k\right)\end{array}\right)=X\left(k\right)+F\left(X\right(k\left)\right)T+w\left(k\right)\end{array}---\left(21\right)$>

其中：T为采样周期，k＝1,2,...，

$>F\left(X\right(k\left)\right)=\left(\begin{array}{c}f\left(x\right(k),\lambda (k\left)\right)\\ g\left(k\right)\end{array}\right),w\left(k\right)=\left(\begin{array}{c}{w}_x\left(k\right)\\ w_{\lambda }\left(k\right)\end{array}\right).$>

z(k)＝h(x(k))+v(k) (22)

其中：f(x(k),λ(k))为f(x,λ,t)离散化后将x(k)和λ(k)代入得到；h(x(k))为h(x,t)离散化将x(k)代入得到；g(k)为g(t)离散化得到。

至此，只要将待估计参数扩展为增广系统状态，再采用扩展卡尔曼算法就能得到未知参数的估计值。

第五步，采用状态与参数解耦估计的扩展卡尔曼滤波算法对未知参数进行估计。

上述问题最大的困难在于由于将待估计参数扩展为增广系统状态，从而导致系统的计算量增大，这对于运载火箭这样的高阶系统是极为不利的，因此必须采用状态与参数解耦估计的滤波算法加以解决。它不仅可以实时估计系统中的未知参数，并相应的调整估计状态，而且还可以通过两个并行滤波器对状态和参数进行解耦估计，以达到减少系统计算量的目的。

下面给出基于状态与参数解耦估计的滤波算法，开始计算时，P_λ(k|k)，P_x(k|k)，V(k)的初值预先给定，之后计算时，以上一步的计算结果作为下一步计算的初值，下面以第k步计算为例，

(1)根据第k-1步计算结果初始化P_λ(k|k)，P_x(k|k)，V(k)；k＝1,2,...

(2)计算Φ(k)，Ψ(k)：

$>\Phi \left(k\right)=I+A\left(\hat{x}\right(k|k),\hat{\lambda }(k|k\left)\right)T$>

$>\Psi \left(k\right)=B\left(\hat{x}\right(k|k),\hat{\lambda }(k|k\left)\right)T$>

其中：

$>A\left(\hat{x}\right(k|k),\hat{\lambda }(k|k\left)\right)=\frac{\partial f(x,\lambda ,t)}{\partial x}{|}_{x=\hat{x}\left(k\right|k),\lambda =\hat{\lambda }\left(k\right|k)}$>

$>B\left(\hat{x}\right(k|k),\hat{\lambda }(k|k\left)\right)=\frac{\partial f(x,\lambda ,t)}{\partial \lambda }{|}_{x=\hat{x}\left(k\right|k),\lambda =\hat{\lambda }\left(k\right|k)}$>

(3)计算第k+1步参数最优预测：

$>\hat{\lambda }(k+1|k)=\hat{\lambda }\left(k\right|k)+g\left(k\right)T---\left(23\right)$>

同时计算第k+1步状态最优预测：

$>\hat{x}(k+1|k)=\hat{x}\left(k\right|k)+f\left(\hat{x}\right(k|k),\hat{\lambda }(k|k\left)\right)T---\left(24\right)$>其中：为f(x,λ,t)离散化将和代入得到；

(4)计算第k+1步参数预测误差方差阵：

P_λ(k+1|k)＝P_λ(k|k)+Q_λ>

同时计算第k+1步状态预测误差方差阵：

P_x(k+1|k)＝Φ(k)P_x(k|k)Φ^T(k)+Ψ(k)P_λ(k|k)Ψ^T(k)+Q_x>

(5)计算H(k+1)，Φ(k+1)，Ψ(k+1)：

$>\Phi (k+1)=I+A\left(\hat{x}\right(k+1|k),\hat{\lambda }(k+1|k\left)\right)T---\left(27\right)$>

$>\Psi (k+1)=B\left(\hat{x}\right(k+1|k),\hat{\lambda }(k+1|k\left)\right)T---\left(28\right)$>

$>H(k+1)=\frac{\partial h(x,t)}{\partial x}{|}_{x=\hat{x}(k+1|k)}---\left(29\right)$>

其中：$>A\left(\hat{x}\right(k+1|k),\hat{\lambda }(k+1|k\left)\right)=\frac{\partial f(x,\lambda ,t)}{\partial x}{|}_{x=\hat{x}(k+1|k),\lambda =\hat{\lambda }(k+1|k)}$>

$>B\left(\hat{x}\right(k+1|k),\hat{\lambda }(k+1|k\left)\right)=\frac{\partial f(x,\lambda ,t)}{\partial \lambda }{|}_{x=\hat{x}(k+1|k),\lambda =\hat{\lambda }(k+1|k)}.$>

(6)计算状态最优增益矩阵：

K_x(k+1)＝P_x(k+1|k)H^T(k+1)[H(k+1)P_x(k+1|k)H^T(k+1)+R]^-1>

(7)计算加权矩阵：

U(k+1)＝Φ(k+1)V(k)+Ψ(k+1) (31)

S(k+1)＝H(k+1)U(k+1) (32)

V(k+1)＝U(k+1)-K_x(k+1)S(k+1)>

K(k+1)＝K_x(k+1)+V(k+1)K_λ(k+1)>

(8)计算参数最优增益矩阵：

$>\begin{array}{c}{K}_{\lambda }(k+1)=P_{\lambda }(k+1|k)S^T(k+1)[S(k+1)P_{\lambda }(k+1|k)S^T(k+1)\\ +H(k+1)P_x(k+1|k)H^T(k+1)+R{]}^{-1}\end{array}---\left(35\right)$>

(9)计算参数最优估计值：

$>\hat{\lambda }(k+1|k+1)=\hat{\lambda }(k+1|k)+K_{\lambda }(k+1)[z\left(k\right)-h\left(\hat{x}\right(k+1|k\left)\right)]---\left(36\right)$>

其中：z(k)表示第k步的测量值，

包括角速度、角度、位置、速度的测量值；

同时计算状态最优估计值：

$>\hat{x}(k+1|k+1)=\hat{x}(k+1|k)+K(k+1)[z(k+1)-h\left(\hat{x}\right(k+1|k\left)\right)]---\left(37\right)$>

其中：为h(x,t)离散化将代入得到；

(10)计算参数滤波误差方差阵：

P_λ(k+1|k+1)＝[I-K_λ(k+1)S(k+1)]P_λ(k+1|k)>

同时计算状态滤波误差方差阵：

P_x(k+1|k+1)＝[I-K_x(k+1)H(k+1)]P_x(k+1|k)>

(11)、完成第k步计算，将上述第k步计算的第k+1步的预估值P_λ(k+1|k+1)、P_x(k+1|k+1)、V(k+1)初始化P_λ(k|k)，P_x(k|k)，V(k)，其中k＝1,2,...，返回步骤(2)，开始第k+1步的计算。

$>\hat{\lambda }(k+1|k+1)\to \hat{\lambda }\left(k\right|k),\hat{x}(k+1|k+1)\to \hat{x}\left(k\right|k),$>

P_λ(k+1|k+1)→P_λ(k|k)，P_x(k+1|k+1)→P_x(k|k)，V(k+1)→V(k)。

最终得到状态估计和参数估计结果，即需要辨识的参数向量及状态向量作为开展控制参数在线优化调整的设计依据。

上述参数辨识方法在理论上其实是一种状态和参数联合估计的双估计滤波方法，即对未知动态系统的状态和参数交替进行估计，这种方法使用模型估计信号，并利用估计信号去修正模型。

实施例1

根据式(23)-(39)采用如下初值，即可实现运载火箭质量特性及动力学关键参数在线辨识，具体辨识结果见图2-图9。

P_λ(0|0)＝100I_2×2，P_x(0|0)＝100I_12×12，V(0)＝I_12×12。其中，x₀为飞行状态初值，λ₀为估计参数初值。

如图2为本发明实施例中X方向位置估计结果，图3为本发明实施例中Y方向位置估计结果；图4为本发明实施例中Z方向位置估计结果；图5为本发明实施例中俯仰姿态角估计结果；图6为本发明实施例中偏航姿态角估计结果；图7为本发明实施例中滚动姿态角估计结果；图8为本发明实施例中质量估计结果；图9为本发明实施例中推力估计结果。从辨识结果可知，所提出的基于状态与参数解耦估计的运载火箭质量特性及动力学关键参数在线辨识算法可以实现飞行状态和未知参数的准确估计，其估计精度可以满足设计任务需求。

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

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