CN104764467A - 空天飞行器惯性传感器误差在线自适应标定方法 - Google Patents

Abstract

本发明公开了一种空天飞行器惯性传感器误差在线自适应标定方法，该方法包括以下步骤：首先建立惯性传感器在空天飞行器高动态飞行过程中的误差模型，包括安装误差、刻度因子误差和随机常值误差；随后建立包含惯性传感器误差参数在内的高阶在线标定滤波状态方程和量测方程；最后在空天飞行器高动态飞行过程中对惯性传感器误差进行在线自适应标定与实时补偿，获得惯性传感器误差补偿校正后的惯性导航系统导航结果。本方法能够在空天飞行器高动态飞行过程中实现对惯性导航系统惯性传感器安装误差、刻度因子误差和随机常值误差的实时标定及补偿，有效提高空天飞行环境下的惯性导航系统性能，适合于工程应用。

Description

空天飞行器惯性传感器误差在线自适应标定方法

技术领域

本发明公开了空天飞行器惯性传感器误差在线自适应标定方法，属于惯性导航惯性传感器误差标定技术领域。

背景技术

空天飞行器是一种集航空器、航天器和运载器于一体的可重复使用的新型飞行器，兼备了航空与航天的优势，以其极大的民用价值和战略军事应用价值而得到各发达国家的重视。空天飞行器从起飞到降落的整个飞行过程中，要经历起飞，加速入轨，轨道驻留，灵活变轨，高速再入五个阶段，这种复杂的运动特性要求其导航系统具有多任务、多工作模式、大空域机动下的可靠和高精度工作能力，以适应空天飞行器各飞行阶段不同的导航需求。

捷联惯性导航系统是空天飞行器的基本导航系统，惯性传感器(陀螺仪和加速度计)的测量误差是影响捷联惯性导航系统精度的主要因素。在空天飞行器高动态飞行过程中，由于飞行模式切换和高动态飞行过程中的强振动冲击、气流扰动等影响导致的机体变形将引起惯性传感器的轴线不能与机体轴线完全重合，进而导致安装误差和刻度因子误差显著增大；长航时的巡航飞行也将导致惯性传感器的随机常值误差随着时间的增加而偏离初始标定值。这些误差如果在空天飞行器高动态飞行过程中不能进行在线标定与补偿，将在很大程度上影响导航精度。

惯性传感器误差在线标定大多采用卡尔曼滤波进行误差参数的实时估计，但传统的卡尔曼滤波要求系统噪声和量测噪声的统计特性精确已知。由于空天飞行器多任务、多工作模式、大范围高速机动的特点，系统噪声和量测噪声的统计特性会受到飞行模式与环境的影响，不可能完全已知，传统卡尔曼滤波的标定方法将不再适用。因此，研究一种在空天飞行器高动态飞行过程中对惯性传感器误差进行在线自适应标定的方法，能够有效提高空天飞行器高动态飞行过程中的惯性导航系统精度，将具有突出的应用价值。

发明内容

本发明所要解决的技术问题，是针对前述背景技术的缺陷和不足，提出一种空天飞行器惯性传感器误差在线自适应标定方法，实现高精度的导航要求。

本发明为解决上述技术问题采用以下技术方案：

一种空天飞行器惯性传感器误差在线自适应标定方法，该方法包括以下步骤：

步骤一，建立惯性传感器误差模型，包括陀螺仪和加速度计的安装误差、刻度因子误差和随机常值误差矩阵；步骤二，将所述三类误差的误差参数扩展为系统状态变量，构建高阶卡尔曼滤波状态方程及量测方程；步骤三，对系统状态方程和量测方程进行离散化处理，并且对状态量、量测量实时更新，实现对空天飞行器惯性导航系统惯性传感器安装误差、刻度因子误差及随机常值误差的在线自适应标定与补偿。

进一步的，所述步骤一具体指：

建立陀螺仪的安装误差矩阵 $\mathrm{\δG}=\left[\begin{array}{ccc}0& \mathrm{\δ}{G}_z& -\mathrm{\δ}{G}_y\\ -{\mathrm{\δG}}_z& 0& \mathrm{\δ}{G}_x\\ \mathrm{\δ}{G}_y& -\mathrm{\δ}{G}_x& 0\end{array}\right],$ δG_x、δG_y、δG_z分别为X轴、Y轴及Z轴陀螺仪的安装误差角；陀螺仪的刻度因子误差矩阵δK_g＝diag[δK_gx δK_gy δK_gz]，δK_gx、δK_gy、δK_gz分别为X轴、Y轴及Z轴陀螺仪的刻度因子；陀螺仪的随机常值误差矩阵ε_b＝[ε_bx ε_by ε_bz]^T，ε_bx，ε_by，ε_bz分别为X轴、Y轴及Z轴陀螺仪的随机常值误差；所述陀螺仪的安装误差角和刻度因子均取为随机常数，X、Y、Z三轴的陀螺仪误差模型相同：

$\left.\begin{array}{c}\mathrm{\δ}\stackrel{\·}{G}=0\\ \mathrm{\δ}{\stackrel{\·}{K}}_g=0\end{array}\right\}---\left(2\right)$

式(2)中，为陀螺仪的安装误差矩阵δG的一阶导数，为陀螺仪的刻度因子误差矩阵δK_g的一阶导数；

建立加速度计的安装误差矩阵 $\mathrm{\δA}=\left[\begin{array}{ccc}0& \mathrm{\δ}{A}_z& -\mathrm{\δ}{A}_y\\ -{\mathrm{\δA}}_z& 0& \mathrm{\δ}{A}_x\\ \mathrm{\δ}{A}_y& -\mathrm{\δ}{A}_x& 0\end{array}\right],$ δA_x、δA_y、δA_z分别为X轴、Y轴及Z轴加速度计的安装误差角；加速度计的刻度因子误差矩阵δK_a＝diag[δK_ax δK_ay δK_az]，δK_ax、δK_ay、δK_az分别为X轴、Y轴及Z轴加速度计的刻度因子；加速度计的随机常值误差矩阵 ${\▿}_a={\left[\begin{array}{ccc}{\▿}_{\mathrm{ax}}& {\▿}_{\mathrm{ay}}& {\▿}_{\mathrm{az}}\end{array}\right]}^T,$ 分别为X轴、Y轴及Z轴加速度计的随机常值误差；所述加速度计的安装误差角和刻度因子均取为随机常数，X、Y、Z三轴的加速度计误差模型相同：

$\left.\begin{array}{c}\mathrm{\δ}\stackrel{\·}{A}=0\\ \mathrm{\δ}{\stackrel{\·}{K}}_a=0\end{array}\right\}---\left(4\right)$

式(4)中，为加速度计的安装误差矩阵δA的一阶导数，为加速度计的刻度因子误差矩阵δK_a的一阶导数。

进一步的，所述步骤二具体指：

在步骤一对惯性传感器安装误差、刻度因子误差及随机常值误差建模的基础上，将所述三类误差的误差参数扩展为系统状态变量，构建高阶卡尔曼滤波状态方程及量测方程：

$\left.\begin{array}{c}\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}\\ Z=\mathrm{HX}+V\end{array}\right\}---\left(5\right)$

式(5)中，X为系统状态变量；为状态变量X的一阶导数；F为系统矩阵；G为系统噪声系数矩阵；W为系统噪声矩阵；Z为观测量矩阵；H为量测矩阵；V为量测噪声矩阵；

高阶滤波器的系统状态变量X为：

式(6)中，分别为惯性导航系统中的一种坐标系三个方向平台误差角状态量；δv_E，δv_N，δv_U分别为惯性导航系统中对应所述坐标系的三个方向速度误差状态量；δL，δλ，δh分别为惯性导航系统中的纬度、经度及高度误差状态量；ε_bx，ε_by，ε_bz分别为惯性导航系统中X轴、Y轴及Z轴方向陀螺仪的随机常值误差状态量；分别为惯性导航系统中X轴、Y轴及Z轴方向加速度计的随机常值误差状态量；δA_x，δA_y，δA_z分别为惯性导航系统中X轴、Y轴及Z轴方向加速度计的安装误差角状态量；δK_ax，δK_ay，δK_az分别为惯性导航惯性导航系统中X轴、Y轴及Z轴方向加速度计的刻度因子误差状态量；δG_x，δG_y，δG_z分别为惯性导航系统中X轴、Y轴及Z轴方向陀螺仪的安装误差角状态量；δK_gx，δK_gy，δK_gz分别为惯性导航系统中X轴、Y轴及Z轴方向陀螺仪的刻度因子误差状态量。

进一步的，所述步骤三具体指：

步骤3.1，将滤波器状态方程和量测方程离散化处理：

$\left.\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right\}---\left(17\right)$

式(17)中，X_k为t_k时刻系统状态变量；X_k-1为t_k-1时刻系统状态变量；Φ_k，k-1为t_k-1时刻至t_k时刻系统的一步状态转移矩阵；Γ_k-1为t_k-1时刻至t_k时刻系统噪声系数矩阵；W_k-1为t_k-1时刻系统噪声矩阵；Z_k为t_k时刻的位置、速度及姿态观测量矩阵；H_k为t_k时刻量测矩阵；V_k为t_k时刻量测噪声矩阵；

步骤3.2，按照式(18)-(23)对步骤3.1部分系统噪声协方差阵与量测噪声协方差阵进行自适应估计：

${\hat{X}}_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1|k-1}---\left(18\right)$

${\tilde{Z}}_{k|k-1}=Z_k-H_k{\hat{X}}_{k|k-1}---\left(19\right)$

$C_k={\tilde{Z}}_{k|k-1}{\tilde{Z}}_{k|k-1}^T---\left(20\right)$

${\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T=K_{k-1}{C}_k{K}_{k-1}^T---\left(21\right)$

$P_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(22\right)$

$R_k=C_k-H_k{P}_{k|k-1}{H}_k^T---\left(23\right)$

其中，为t_k-1时刻至t_k时刻一步预测状态量；为t_k-1时刻滤波状态估计量；为t_k时刻的新息序列；C_k为t_k时刻的新息协方差阵；Q_k-1为t_k-1时刻的系统噪声估计协方差阵；K_k-1为t_k-1时刻滤波增益矩阵；P_k|k-1为t_k-1时刻至t_k时刻一步预测协方差矩阵；P_k-1为t_k-1时刻滤波状态估计协方差矩阵；R_k为t_k时刻的量测噪声估计协方差阵；

步骤3.3，按照式(27)-(29)实现对系统状态量和协方差信息的量测更新：

$K_k=P_{k|k-1}{H}_k^T{[H_k{P}_{k|k-1}{H}_k^T+R_k]}^{-1}---\left(27\right)$

${\hat{X}}_{k|k}={\hat{X}}_{k|k-1}+K_k[Z_k-H_k{\hat{X}}_{k|k-1}]---\left(28\right)$

P_k＝[I-K_kH_k]P_k|k-1[I-K_kH_k]^T+K_kR_kK_k ^T   (29)其中，K_k为t_k时刻滤波增益矩阵；为t_k时刻滤波状态估计量；P_k为t_k时刻滤波状态估计协方差矩阵；

步骤3.4，在步骤3.3得到对传感器安装误差、刻度因子误差及随机常值误差的标定结果后，暂存标定值，利用标定值对传感器安装误差、刻度因子误差及随机常值误差进行补偿校正，校正在一个导航解算周期内完成，误差补偿校正算法为：

$\left\{\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ib}}^b=[I-\mathrm{\δ}{K}_g-\mathrm{\δG}]({\widetilde{\mathrm{\ω}}}_{\mathrm{ib}}^b-{\mathrm{\ϵ}}_b)\\ f^b=[I-\mathrm{\δ}{K}_a-\mathrm{\δA}]({\tilde{f}}^b-{\▿}_a)\end{array}\right.---\left(30\right)$

式(30)中，分别为陀螺仪和加速度计的测量输出；f^b分别为陀螺仪和加速度计的理论输出值。

作为一种优选，在步骤3.2中实时估计量测噪声协方差阵R_k时，由于传感器噪声和环境的影响，可能造成某些时刻观测量中存在突变误差，新息突增，导致标定精度降低，滤波收敛速度减慢甚至发散，因此需要按照式(24)对该时刻的新息序列进行判断：

${\mathrm{\η}}_{k,i}=\left\{\begin{array}{cc}1& \left|{\tilde{Z}}_{k|k-1,i}\right|\≤t_{k,i}\\ \left|{\tilde{Z}}_{k|k-1,i}\right|/t_{k,i}& \left|{\tilde{Z}}_{k|k-1,i}\right|>t_{k,i}\end{array}\right.---\left(24\right)$

式(24)中，为t_k时刻新息序列的第i个分量，t_k，i为新息序列第i个分量对应的阈值因子，η_k，i(i＝1，2，…，9)为权重因子；

权重矩阵为：

D_k＝diag(η_k，1 η_k，2 η_k，3 η_k，4 η_k，5 η_k，6 η_k，7 η_k，8 η_k，9)   (25)则量测噪声协方差阵R_k的实时估计公式变为：

$R_k=D_k{R}_k{D}_k^T---\left(26\right).$

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

本发明在对惯性传感器误差建模的基础上，能够在空天飞行器高动态飞行过程中实现对惯性传感器误差的在线自适应标定，利用标定得到的结果可以对惯性传感器误差进行实时补偿，采用本发明可以显著减小空天飞行器高动态飞行过程中的惯性导航系统误差，有效提高空天飞行器的导航精度，适合工程应用。

附图说明

图1为本发明的空天飞行器惯性传感器误差在线自适应标定方法结构图；

图2为陀螺仪安装误差标定曲线图；

图3为陀螺仪刻度因子误差标定曲线图；

图4为陀螺仪随机常值误差标定曲线图；

图5为加速度计安装误差标定曲线图；

图6为加速度计刻度因子误差标定曲线图；

图7为加速度计随机常值误差标定曲线图；

图8为惯性传感器误差标定补偿后的捷联惯导姿态误差曲线图；

图9为惯性传感器误差标定补偿后的捷联惯导速度误差曲线图；

图10为惯性传感器误差标定补偿后的捷联惯导位置误差曲线图。

具体实施方式

本发明提供一种空天飞行器惯性传感器误差在线自适应标定方法，为使本发明的目的，技术方案及效果更加清楚、明确，参照附图并举实例对本发明进一步详细说明。应当理解，此处所描述的具体实施仅用以解释本发明，并不用于限定本发明。

下面结合附图对本发明的技术方案做进一步的详细说明：如图1所示，本发明所述的空天飞行器惯性传感器误差在线自适应标定方法的原理是：陀螺仪和加速度计的测量输出分别为及其中均包含了安装误差、刻度因子误差及随机常值误差；及f^b是经过误差补偿修正后的陀螺仪和加速度计的理论输出值，用于捷联惯性导航系统的导航解算；在建立陀螺仪和加速度计安装误差、刻度因子误差及随机常值误差模型的基础上，构建捷联惯性导航系统误差在线标定模型，通过位置、速度及姿态观测量，实现滤波状态量的更新；在滤波过程中，需要利用新息对系统噪声协方差阵和量测噪声协方差阵进行实时估计，实现对惯性传感器误差参数的在线自适应标定；利用标定的结果对惯性传感器误差进行补偿修正，能够有效提高空天飞行器惯性导航系统精度。

本发明的具体实施方式如下：

1.建立空天飞行器惯性传感器误差模型

(1.1)陀螺仪误差模型

在捷联惯性导航系统中，由于空天飞行器高频振动会导致陀螺仪和加速度计都存在安装误差及刻度因子误差，长航时飞行也会导致随机常值误差随时间增加而与初始值存在较大偏差。忽略高阶非线性误差项与随机性误差，陀螺仪的误差模型为：

$\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ib}}^b=[\mathrm{\δG}+\mathrm{\δ}{K}_g]{\mathrm{\ω}}_{\mathrm{ib}}^b+{\mathrm{\ϵ}}_b---\left(1\right)$

式(1)中， $\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ib}}^b={\left[\begin{array}{ccc}\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ibx}}^b& \mathrm{\δ}{\mathrm{\ω}}_{\mathrm{iby}}^b& \mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ibz}}^b\end{array}\right]}^T$ 为机体系陀螺仪误差； $\mathrm{\δG}=\left[\begin{array}{ccc}0& \mathrm{\δ}{G}_z& -\mathrm{\δ}{G}_y\\ -{\mathrm{\δG}}_z& 0& \mathrm{\δ}{G}_x\\ \mathrm{\δ}{G}_y& -\mathrm{\δ}{G}_x& 0\end{array}\right]$ 为三轴陀螺仪的安装误差矩阵；δK_g＝diag[δK_gx δK_gy δK_gz]为三轴陀螺仪的刻度因子误差矩阵；ε_b＝[ε_bx ε_by ε_bz]^T为三轴陀螺仪的随机常值误差矩阵； ${\mathrm{\ω}}_{\mathrm{ib}}^b={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{ibx}}^b& {\mathrm{\ω}}_{\mathrm{iby}}^b& {\mathrm{\ω}}_{\mathrm{ibz}}^b\end{array}\right]}^T$ 为机体系三轴陀螺仪测量输出的机体角速率；

陀螺仪的安装误差角和刻度因子均取为随机常数，假定三个轴陀螺仪的误差模型相同，可以得到：

$\left.\begin{array}{c}\mathrm{\δ}\stackrel{\·}{G}=0\\ \mathrm{\δ}{\stackrel{\·}{K}}_g=0\end{array}\right\}---\left(2\right)$

(1.2)加速度计误差模型

与陀螺仪误差建模方法类似，加速度计的误差模型为：

$\mathrm{\δ}{f}^b=[\mathrm{\δA}+\mathrm{\δ}{K}_a]f^b+{\▿}_a---\left(3\right)$

式(3)中， $\mathrm{\δ}{f}^b={\left[\begin{array}{ccc}\mathrm{\δ}{f}_c^b& \mathrm{\δ}{f}_y^b& \mathrm{\δ}{f}_z^b\end{array}\right]}^T$ 为机体系加速度计误差； $\mathrm{\δA}=\left[\begin{array}{ccc}0& \mathrm{\δ}{A}_z& -\mathrm{\δ}{A}_y\\ -{\mathrm{\δA}}_z& 0& \mathrm{\δ}{A}_x\\ \mathrm{\δ}{A}_y& -\mathrm{\δ}{A}_x& 0\end{array}\right]$ 为三轴加速度计安装误差矩阵；δK_a＝diag[δK_ax δK_ay δK_az]为三轴加速度计刻度因子误差矩阵； ${\▿}_a={\left[\begin{array}{ccc}{\▿}_{\mathrm{ax}}& {\▿}_{\mathrm{ay}}& {\▿}_{\mathrm{az}}\end{array}\right]}^T$ 为三轴加速度计随机常值误差矩阵； $f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T$ 为机体系三轴加速度计测量输出的比力；

加速度计的安装误差角和刻度因子均取为随机常数，假定三个轴加速度计的误差模型相同，可以得到：

$\left.\begin{array}{c}\mathrm{\δ}\stackrel{\·}{A}=0\\ \mathrm{\δ}{\stackrel{\·}{K}}_a=0\end{array}\right\}---\left(4\right)$

2.建立基于惯性传感器误差模型的误差在线标定模型

(2.1)建立在线标定滤波状态方程

本实施例中，选取机体坐标系(b系)为机体的“右、前、上”(XYZ)方向，选取导航坐标系(n系)为东北天(ENU)地理坐标系，本发明提供的空天飞行器惯性传感器误差在线自适应标定方法不仅限于上述坐标系，在其他方向坐标系的飞行环境中仍然适用。

建立在线标定滤波状态方程，如式(5)所示：

$\left.\begin{array}{c}\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}\\ Z=\mathrm{HX}+V\end{array}\right\}---\left(5\right)$

式(5)中，X为系统状态变量；为状态变量X的一阶导数；F为系统矩阵；G为系统噪声系数矩阵；W为系统噪声矩阵；Z为观测量矩阵；H为量测矩阵；V为量测噪声矩阵；

高阶滤波状态变量X为：

式(6)中，分别为惯性导航系统中的东向、北向及天向平台误差角状态量；δv_E，δv_N，δv_U分别为惯性导航系统中的东向、北向及天向速度误差状态量；δL，δλ，δh分别为惯性导航系统中的纬度、经度及高度误差状态量；ε_bx，ε_by，ε_bz分别为惯性导航系统中X轴、Y轴及Z轴方向陀螺仪的随机常值误差状态量；分别为惯性导航系统中X轴、Y轴及Z轴方向加速度计的随机常值误差状态量；δA_x，δA_y，δA_z分别为惯性导航系统中X轴、Y轴及Z轴方向加速度计的安装误差角状态量；δK_ax，δK_ay，δK_az分别为惯性导航系统中X轴、Y轴及Z轴方向加速度计的刻度因子误差状态量；δG_x，δG_y，δG_z分别为惯性导航系统中X轴、Y轴及Z轴方向陀螺仪的安装误差角状态量；δK_gx，δK_gy，δK_gz分别为惯性导航系统中X轴、Y轴及Z轴方向陀螺仪的刻度因子误差状态量；

根据步骤1中所建立的惯性传感器误差模型，与式(5)对应的系统矩阵F为：

$F={\left[\begin{array}{ccc}{F}_N& F_M& F_S\\ & 0_{18\×27}& \end{array}\right]}_{27\×27}---\left(7\right)$

式(7)中，F_N为9个基本导航参数误差(数学平台误差角、速度误差及位置误差)之间的关系矩阵，其非零项元素为：

式(8)中，f_e，f_n，f_u分别为导航系东向、北向及天向的加速度；R_M和R_N分别为地球子午圈曲率半径和卯酉圈曲率半径；

式(7)中， $F_M={\left[\begin{array}{c}\begin{array}{cc}-C_b^n& 0_{3\×3}\end{array}\\ \begin{array}{cc}{0}_{3\×3}& C_b^n\end{array}\\ 0_{3\×6}\end{array}\right]}_{9\×6}$ 为9个基本导航参数误差与惯性传感器随机常值误差之间的关系矩阵；其中，为机体系到导航系的转换矩阵；

F_s为9个基本导航参数误差与惯性传感器安装误差、刻度因子误差之间的关系矩阵：

$F_S={\left[\begin{array}{ccc}{0}_{3\×6}& F_1& F_3\\ F_2& F_4& 0_{3\×6}\\ & 0_{3\×12}& \end{array}\right]}_{9\×12}---\left(9\right)$

式(9)中：

$F_1=\left[\begin{array}{ccc}-C_{12}{\mathrm{\ω}}_{\mathrm{ibz}}^b+C_{13}{\mathrm{\ω}}_{\mathrm{iby}}^b& C_{11}{\mathrm{\ω}}_{\mathrm{ibz}}^b-C_{13}{\mathrm{\ω}}_{i\mathrm{bx}}^b& -C_{11}{\mathrm{\ω}}_{\mathrm{iby}}^b+C_{12}{\mathrm{\ω}}_{\mathrm{ibx}}^b\\ -C_{22}{\mathrm{\ω}}_{\mathrm{ibz}}^b+C_{23}{\mathrm{\ω}}_{\mathrm{iby}}^b& C_{21}{\mathrm{\ω}}_{\mathrm{ibz}}^b-C_{23}{\mathrm{\ω}}_{\mathrm{ibx}}^b& -C_{21}{\mathrm{\ω}}_{\mathrm{iby}}^b+C_{22}{\mathrm{\ω}}_{\mathrm{ibx}}^b\\ -C_{32}{\mathrm{\ω}}_{\mathrm{ibz}}^b+C_{33}{\mathrm{\ω}}_{\mathrm{iby}}^b& C_{31}{\mathrm{\ω}}_{\mathrm{ibz}}^b-C_{33}{\mathrm{\ω}}_{\mathrm{ibx}}^b& -C_{31}{\mathrm{\ω}}_{\mathrm{iby}}^b+C_{32}{\mathrm{\ω}}_{\mathrm{ibx}}^b\end{array}\right]---\left(10\right)$

$F_2=\left[\begin{array}{ccc}{C}_{12}{f}_z^b-C_{13}{f}_y^b& -C_{11}{f}_z^b+C_{13}{f}_x^b& C_{11}{f}_y^b-C_{12}{f}_x^b\\ C_{22}{f}_z^b-C_{23}{f}_y^b& -C_{21}{f}_z^b+C_{23}{f}_x^b& C_{21}{f}_y^b-C_{22}{f}_x^b\\ C_{32}{f}_z^b-C_{33}{f}_y^b& -C_{31}{f}_z^b+C_{33}{f}_x^b& C_{31}{f}_y^b-C_{32}{f}_x^b\end{array}\right]---\left(11\right)$

$F_3=\left[\begin{array}{ccc}-C_{11}{\mathrm{\ω}}_{\mathrm{ibx}}^b& -C_{12}{\mathrm{\ω}}_{\mathrm{iby}}^b& -C_{13}{\mathrm{\ω}}_{\mathrm{ibz}}^b\\ -C_{21}{\mathrm{\ω}}_{\mathrm{ibx}}^b& {-C}_{22}{\mathrm{\ω}}_{\mathrm{iby}}^b& -C_{23}{\mathrm{\ω}}_{\mathrm{ibz}}^b\\ -C_{31}{\mathrm{\ω}}_{\mathrm{ibx}}^b& -C_{32}{\mathrm{\ω}}_{\mathrm{iby}}^b& -C_{33}{\mathrm{\ω}}_{\mathrm{ibz}}^b\end{array}\right]---\left(12\right)$

$F_4=\left[\begin{array}{ccc}{C}_{11}{f}_x^b& C_{12}{f}_y^b& C_{13}{f}_z^b\\ C_{21}{f}_x^b& C_{22}{f}_y^b& C_{23}{f}_z^b\\ C_{31}{f}_x^b& C_{32}{f}_y^b& C_{33}{f}_z^b\end{array}\right]---\left(13\right)$

其中， $C_b^n=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$ 为机体系到导航系的转换矩阵；

系统噪声矩阵为：

W＝[w_gx w_gy w_gz w_ax w_ay w_az]^T   (14)

系统噪声系数矩阵为：

$G={\left[\begin{array}{c}\begin{array}{cc}-C_b^n& 0_{3\×3}\end{array}\\ \begin{array}{cc}{0}_{3\×3}& C_b^n\end{array}\\ 0_{21\×6}\end{array}\right]}_{27\×6}---\left(15\right)$

(2.2)建立在线标定滤波量测方程

采用SINS/GPS松组合方式来提供位置、速度及姿态量测信息，则系统量测方程为：

$Z=\left[\begin{array}{c}{Z}_p\\ Z_v\\ Z_{\mathrm{\φ}}\end{array}\right]=\left[\begin{array}{c}{H}_p\\ H_v\\ H_{\mathrm{\φ}}\end{array}\right]X+\left[\begin{array}{c}{V}_p\\ V_v\\ V_{\mathrm{\φ}}\end{array}\right]=\left[\begin{array}{ccc}{0}_{3\×6}& \mathrm{diag}\left[\begin{array}{ccc}{R}_M& R_N\cos L& 1\end{array}\right]& 0_{3\×18}\\ 0_{3\×3}& \mathrm{diag}\left[\begin{array}{ccc}1& 1& 1\end{array}\right]& 0_{3\×21}\\ & \begin{array}{cc}{H}_{\mathrm{\α}}& 0_{3\×24}\end{array}& \end{array}\right]X+\left[\begin{array}{c}{V}_p\\ V_v\\ V_{\mathrm{\φ}}\end{array}\right]---\left(16\right)$

式(16)中，Z_p，Z_v，Z_φ分别为导航系下位置、速度及姿态的量测信息量；H_p，H_v，H_φ分别为对应的量测矩阵；V_P＝[N_N N_E N_U]^T，V_V＝[M_E M_N M_U]^T和V_θ＝[v_γ v_θ v_φ]^T分别为对应的量测噪声量；H_α为捷联惯导数学平台误差角到飞机姿态误差角的转换矩阵，并且 $H_{\mathrm{\α}}=\frac{-1}{\cos \mathrm{\θ}}\left[\begin{array}{ccc}\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ \cos \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\cos \mathrm{\θ}& 0\\ \sin \mathrm{\ψ}\sin \mathrm{\θ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}& -\cos \mathrm{\θ}\end{array}\right];$ 其中，γ，θ，ψ分别为飞机的横滚角、俯仰角和航向角；

3.空天飞行器惯性导航系统惯性传感器误差的在线自适应标定与补偿

(3.1)将滤波器状态方程和量测方程离散化处理：

$\left.\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right\}---\left(17\right)$

式(17)中，X_k为t_k时刻系统状态变量；X_k-1为t_k-1时刻系统状态变量；Φ_k，k-1为t_k-1时刻至t_k时刻系统的一步状态转移矩阵；Γ_k-1为t_k-1时刻至t_k时刻系统噪声系数矩阵；W_k-1为t_k-1时刻系统噪声矩阵；Z_k为t_k时刻的位置、速度及姿态观测量矩阵；H_k为t_k时刻量测矩阵；V_k为t_k时刻量测噪声矩阵；

(3.2)按照式(18)-(23)对步骤(3.1)部分系统噪声协方差阵与量测噪声协方差阵进行自适应估计：

${\tilde{X}}_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1|k-1}---\left(18\right)$

${\tilde{Z}}_{k|k-1}=Z_k-H_k{\hat{X}}_{k|k-1}---\left(19\right)$

$C_k={\tilde{Z}}_{k|k-1}{\tilde{Z}}_{k|k-1}^T---\left(20\right)$

${\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T=K_{k-1}{C}_k{K}_{k-1}^T---\left(21\right)$

$P_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(22\right)$

$R_k=C_k-H_k{P}_{k|k-1}{H}_k^T---\left(23\right)$

其中，为t_k-1时刻至t_k时刻一步预测状态量；为t_k-1时刻滤波状态估计量；为t_k时刻的新息序列；C_k为t_k时刻的新息协方差阵；Q_k-1为t_k-1时刻的系统噪声估计协方差阵；K_k-1为t_k-1时刻滤波增益矩阵；P_k|k-1为t_k-1时刻至t_k时刻一步预测协方差矩阵；P_k-1为t_k-1时刻滤波状态估计协方差矩阵；R_k为t_k时刻的量测噪声估计协方差阵；

(3.3)在步骤(3.2)中实时估计量测噪声协方差阵R_k时，由于传感器噪声和环境的影响，可能造成某些时刻观测量中存在突变误差，新息突增，导致标定精度降低，滤波收敛速度减慢甚至发散，因此需要按照式(24)对该时刻的新息序列进行判断：

${\mathrm{\η}}_{k,i}=\left\{\begin{array}{cc}1& \left|{\tilde{Z}}_{k|k-1,i}\right|\≤t_{k,i}\\ \left|{\tilde{Z}}_{k|k-1,i}\right|/t_{k,i}& \left|{\tilde{Z}}_{k|k-1,i}\right|>t_{k,i}\end{array}\right.---\left(24\right)$

式(24)中，为t_k时刻新息序列的第i个分量，t_k，i为新息序列第i个分量对应的阈值因子，η_k，i(i＝1，2，…，9)为权重因子；

权重矩阵为：

D_k＝diag(η_k，1 η_k，2 η_k，3 η_k，4 η_k，5 η_k，6 η_k，7 η_k，8 η_k，9)   (25)

则量测噪声协方差阵R_k的实时估计公式变为：

$R_k=D_k{R}_k{D}_k^T---\left(26\right)$

(3.4)按照式(27)-(29)实现对系统状态量和协方差信息的量测更新：

$K_k=P_{k|k-1}{H}_k^T{[H_k{P}_{k|k-1}{H}_k^T+R_k]}^{-1}---\left(27\right)$

${\hat{X}}_{k|k}={\hat{X}}_{k|k-1}+K_k[Z_k-H_k{\hat{X}}_{k|k-1}]---\left(28\right)$

P_k＝[I-K_kH_k]P_k|k-1[I-K_kH_k]^T+K_kR_kK_k ^T   (29)其中，K_k为t_k时刻滤波增益矩阵；为t_k时刻滤波状态估计量；P_k为t_k时刻滤波状态估计协方差矩阵；

(3.5)在步骤(3.4)得到对惯性传感器安装误差、刻度因子误差及随机常值误差的标定结果后，暂存标定值，利用标定值对惯性传感器安装误差、刻度因子误差及随机常值误差进行补偿校正，校正在一个导航解算周期内完成，误差补偿校正算法为：

$\left\{\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ib}}^b=[I-{\mathrm{\δK}}_g-\mathrm{\δG}]({\widetilde{\mathrm{\ω}}}_{\mathrm{ib}}^b-{\mathrm{\ϵ}}_b)\\ f^b=[I-\mathrm{\δ}{K}_a-\mathrm{\δA}]({\tilde{f}}^b-{\▿}_a)\end{array}\right.---\left(30\right)$

式(30)中，分别为陀螺仪和加速度计的测量输出；f^b分别为陀螺仪和加速度计的理论输出值。

(3.6)在步骤(3.5)对惯性传感器安装误差、刻度因子误差及随机常值误差校正补偿完成后，不再使用滤波器，进入纯惯性导航系统工作模式。

为了验证发明所提出的空天飞行器惯性传感器误差在线自适应标定方法的正确性及有效性，采用本发明方法建立在线自适应标定模型，采用Matlab进行在线自适应标定算法和传统在线标定算法的仿真验证对比。对惯性传感器安装误差、刻度因子误差及随机常值误差的标定结果如图2-图7所示。

在空天飞行器高动态飞行过程中利用标定结果对惯性传感器安装误差、刻度因子误差及随机常值误差进行实时补偿修正，采用在线自适应标定算法和传统在线标定算法进行补偿修正后的惯性导航结果对比如图8-图10所示。

图2-图7中实线代表采用本发明的在线自适应标定算法进行标定的结果，点划线代表惯性传感器误差设定值，虚线代表采用传统在线标定算法进行标定的结果。从图中可以看出，采用本发明的在线自适应标定算法进行标定时，陀螺仪安装误差、刻度因子误差与随机常值误差的标定精度较传统在线标定算法有显著提高，而加速度计安装误差、刻度因子误差与随机常值误差的标定精度较传统在线标定算法相差不大。图8-图10中虚线代表采用本发明的在线自适应标定算法进行补偿修正后的纯惯导导航结果，实线代表采用传统在线标定算法进行补偿修正后的纯惯导导航结果。从图中可以看出，采用本发明的在线自适应标定算法对惯性传感器误差进行补偿修正后，惯性导航系统的导航精度较传统在线标定算法有所提高，具有有益的工程应用价值。

可以理解的是，对本领域普通技术人员来说，可以根据本发明的技术方案及其发明构思加以等同替换或改变，而所有这些改变或替换都应属于本发明所附的权利要求的保护范围。

Claims (5)

1.一种空天飞行器惯性传感器误差在线自适应标定方法，其特征在于，该方法包括以下步骤：步骤一，建立惯性传感器误差模型，包括陀螺仪和加速度计的安装误差、刻度因子误差和随机常值误差矩阵；步骤二，将所述三类误差的误差参数扩展为系统状态变量，构建高阶卡尔曼滤波状态方程及量测方程；步骤三，对系统状态方程和量测方程进行离散化处理，并且对状态量、量测量实时更新，实现对空天飞行器惯性导航系统惯性传感器安装误差、刻度因子误差及随机常值误差的在线自适应标定与补偿。

2.根据权利要求1所述的一种空天飞行器惯性传感器误差在线自适应标定方法，其特征在于，所述步骤一具体指：

建立陀螺仪的安装误差矩阵 $\mathrm{\δG}=\left[\begin{array}{ccc}0& \mathrm{\δ}{G}_z& -\mathrm{\δ}{G}_y\\ -\mathrm{\δ}{G}_z& 0& \mathrm{\δ}{G}_x\\ \mathrm{\δ}{G}_y& -\mathrm{\δ}{G}_x& 0\end{array}\right],$ δG_x、δG_y、δG_z分别为X轴、Y轴及Z轴陀螺仪的安装误差角；陀螺仪的刻度因子误差矩阵δK_g＝diag[δK_gx δK_gy δK_gz]，δK_gx、δK_gy、δK_gz分别为X轴、Y轴及Z轴陀螺仪的刻度因子；陀螺仪的随机常值误差矩阵ε_b＝[ε_bx ε_by ε_bz]^T，ε_bx，ε_by，ε_bz分别为X轴、Y轴及Z轴陀螺仪的随机常值误差；所述陀螺仪的安装误差角和刻度因子均取为随机常数，X、Y、Z三轴的陀螺仪误差模型相同：

$\left.\begin{array}{c}\mathrm{\δ}\stackrel{\·}{G}=0\\ \mathrm{\δ}{\stackrel{\·}{K}}_g=0\end{array}\right\}---\left(2\right)$

式(2)中，为陀螺仪的安装误差矩阵δG的一阶导数，为陀螺仪的刻度因子误差矩阵δK_g的一阶导数；

建立加速度计的安装误差矩阵 $\mathrm{\δA}=\left[\begin{array}{ccc}0& \mathrm{\δ}{A}_z& -\mathrm{\δ}{A}_y\\ -\mathrm{\δ}{A}_z& 0& \mathrm{\δ}{A}_x\\ \mathrm{\δ}{A}_y& -\mathrm{\δ}{A}_x& 0\end{array}\right],$ δA_x、δA_y、δA_z分别为X轴、Y轴及Z轴加速度计的安装误差角；加速度计的刻度因子误差矩阵δK_a＝diag[δK_ax δK_ay δK_az]，δK_ax、δK_ay、δK_az分别为X轴、Y轴及Z轴加速度计的刻度因子；加速度计的随机常值误差矩阵 ${\▿}_a={\left[\begin{array}{ccc}{\▿}_{\mathrm{ax}}& {\▿}_{\mathrm{ay}}& {\▿}_{\mathrm{az}}\end{array}\right]}^T,$ 分别为X轴、Y轴及Z轴加速度计的随机常值误差；所述加速度计的安装误差角和刻度因子均取为随机常数，X、Y、Z三轴的加速度计误差模型相同：

$\left.\begin{array}{c}\mathrm{\δ}\stackrel{\·}{A}=0\\ \mathrm{\δ}{\stackrel{\·}{K}}_a=0\end{array}\right\}---\left(4\right)$

式(4)中，为加速度计的安装误差矩阵δA的一阶导数，为加速度计的刻度因子误差矩阵δK_a的一阶导数。

3.根据权利要求1所述的一种空天飞行器惯性传感器误差在线自适应标定方法，其特征在于，所述步骤二具体指：

在步骤一对惯性传感器安装误差、刻度因子误差及随机常值误差建模的基础上，将所述三类误差的误差参数扩展为系统状态变量，构建高阶卡尔曼滤波状态方程及量测方程：

$\left.\begin{array}{c}\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}\\ Z=\mathrm{HX}+V\end{array}\right\}---\left(5\right)$

式(5)中，X为系统状态变量；为状态变量X的一阶导数；F为系统矩阵；G为系统噪声系数矩阵；W为系统噪声矩阵；Z为观测量矩阵；H为量测矩阵；V为量测噪声矩阵；

高阶滤波器的系统状态变量X为：

式(6)中，分别为惯性导航系统中的一种坐标系三个方向平台误差角状态量；δv_E,δv_N,δv_U分别为惯性导航系统中对应所述坐标系的三个方向速度误差状态量；δL,δλ,δh分别为惯性导航系统中的纬度、经度及高度误差状态量；ε_bx,ε_by,ε_bz分别为惯性导航系统中X轴、Y轴及Z轴方向陀螺仪的随机常值误差状态量；分别为惯性导航系统中X轴、Y轴及Z轴方向加速度计的随机常值误差状态量；δA_x,δA_y,δA_z分别为惯性导航系统中X轴、Y轴及Z轴方向加速度计的安装误差角状态量；δK_ax,δK_ay,δK_az分别为惯性导航惯性导航系统中X轴、Y轴及Z轴方向加速度计的刻度因子误差状态量；δG_x,δG_y,δG_z分别为惯性导航系统中X轴、Y轴及Z轴方向陀螺仪的安装误差角状态量；δK_gx,δK_gy,δK_gz分别为惯性导航系统中X轴、Y轴及Z轴方向陀螺仪的刻度因子误差状态量。

4.根据权利要求1所述的一种空天飞行器惯性传感器误差在线自适应标定方法，其特征在于，所述步骤三具体指：

步骤3.1，将滤波器状态方程和量测方程离散化处理：

$\left.\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right\}---\left(17\right)$

式(17)中，X_k为t_k时刻系统状态变量；X_k-1为t_k-1时刻系统状态变量；Φ_k,k-1为t_k-1时刻至t_k时刻系统的一步状态转移矩阵；Γ_k-1为t_k-1时刻至t_k时刻系统噪声系数矩阵；W_k-1为t_k-1时刻系统噪声矩阵；Z_k为t_k时刻的位置、速度及姿态观测量矩阵；H_k为t_k时刻量测矩阵；V_k为t_k时刻量测噪声矩阵；

步骤3.2，按照式(18)-(23)对步骤3.1部分系统噪声协方差阵与量测噪声协方差阵进行自适应估计：

${\hat{X}}_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1|k-1}---\left(18\right)$

${\tilde{Z}}_{k|k-1}=Z_k-H_k{\hat{X}}_{k|k-1}---\left(19\right)$

$C_k={\tilde{Z}}_{k|k-1}{\tilde{Z}}_{k|k-1}^T---\left(20\right)$

${\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T=K_{k-1}{C}_k{K}_{k-1}^T---\left(21\right)$

$P_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(22\right)$

$R_k=C_k-H_k{P}_{k|k-1}{H}_k^T---\left(23\right)$

其中，为t_k-1时刻至t_k时刻一步预测状态量；为t_k-1时刻滤波状态估计量；为t_k时刻的新息序列；C_k为t_k时刻的新息协方差阵；Q_k-1为t_k-1时刻的系统噪声估计协方差阵；K_k-1为t_k-1时刻滤波增益矩阵；P_k|k-1为t_k-1时刻至t_k时刻一步预测协方差矩阵；P_k-1为t_k-1时刻滤波状态估计协方差矩阵；R_k为t_k时刻的量测噪声估计协方差阵；

步骤3.3，按照式(27)-(29)实现对系统状态量和协方差信息的量测更新：

$K_k=P_{k|k-1}{H}_k^T{[H_k{P}_{k|k-1}{H}_k^T+R_k]}^{-1}---\left(27\right)$

${\hat{X}}_{k|k}={\hat{X}}_{k|k-1}+K_k[Z_k-H_k{\hat{X}}_{k|k-1}]---\left(28\right)$

$P_k=[I-K_k{H}_k]P_{k|k-1}{[I-K_k{H}_k]}^T+K_k{R}_k{K}_k^T---\left(29\right)$

其中，K_k为t_k时刻滤波增益矩阵；为t_k时刻滤波状态估计量；P_k为t_k时刻滤波状态估计协方差矩阵；

步骤3.4，在步骤3.3得到对传感器安装误差、刻度因子误差及随机常值误差的标定结果后，暂存标定值，利用标定值对传感器安装误差、刻度因子误差及随机常值误差进行补偿校正，校正在一个导航解算周期内完成，误差补偿校正算法为：

$\left\{\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ib}}^b=[I-\mathrm{\δ}{K}_g-\mathrm{\δG}]({\widetilde{\mathrm{\ω}}}_{\mathrm{ib}}^b-{\mathrm{\ϵ}}_b)\\ f^b=[I-\mathrm{\δ}{K}_a-\mathrm{\δA}]({\tilde{f}}^b-{\▿}_a)\end{array}\right.---\left(30\right)$

式(30)中，分别为陀螺仪和加速度计的测量输出；f^b分别为陀螺仪和加速度计的理论输出值。

5.根据权利要求4所述的一种空天飞行器惯性传感器误差在线自适应标定方法，其特征在于，

在步骤3.2中实时估计量测噪声协方差阵R_k时，由于传感器噪声和环境的影响，可能造成某些时刻观测量中存在突变误差，新息突增，导致标定精度降低，滤波收敛速度减慢甚至发散，因此需要按照式(24)对该时刻的新息序列进行判断：

${\mathrm{\η}}_{k,i}=\left\{\begin{array}{cc}1& \left|{\tilde{Z}}_{k|k-1,i}\right|\≤t_{k,i}\\ \left|{\tilde{Z}}_{k|k-1,i}\right|/t_{k,i}& \left|{\tilde{Z}}_{k|k-1,i}\right|>t_{k,i}\end{array}\right.---\left(24\right)$

式(24)中，为t_k时刻新息序列的第i个分量，t_k,i为新息序列第i个分量对应的阈值因子，η_k,i(i＝1,2,…,9)为权重因子；

权重矩阵为：

D_k＝diag(η_k,1 η_k,2 η_k,3 η_k,4η_k,5 η_k,6 η_k,7 η_k,8 η_k,9)   (25)

则量测噪声协方差阵R_k的实时估计公式变为：

$R_k=D_k{R}_k{D}_k^T---\left(26\right).$