CN104613984A - 临近空间飞行器传递对准模型不确定性的鲁棒滤波方法 - Google Patents

Abstract

本发明公开了一种临近空间飞行器传递对准模型不确定性的鲁棒滤波方法，包括四个步骤：步骤一，根据临近空间飞行器传递对准系统的工作原理和特点，建立系统的数学平台失准角误差方程、速度误差方程、位置误差方程和观测方程；步骤二，根据系统的误差方程建立模型不确定的状态方程和观测方程；步骤三，给出状态变量初始值(x₀)和预测误差方差矩阵初始值(Σ_0|0)，给出稀疏网格求积分点集(ξ_j，ε_j；j＝1,2，…N_p)，给出鲁棒滤波参数γ和ε；步骤四，利用鲁棒滤波对系统状态进行估计并对子惯导系统进行误差修正，完成传递对准过程。本发明适用于临近空间飞行器动态条件下主子惯导系统具有模型不确定性的传递对准。

Description

临近空间飞行器传递对准模型不确定性的鲁棒滤波方法

技术领域

本发明涉及导航系统非线性鲁棒滤波领域，具体涉及一种临近空间飞行器传递对准模型不确定性的鲁棒滤波方法。

背景技术

传递对准是解决高超声速飞行器在动基座条件下初始对准问题的主要方法，一般采用非线性卡尔曼滤波器对系统的状态进行估计。基于非线性高斯逼近的滤波器其性能取决于系统模型以及干扰特性假设的精确程度。在实际工作中，高超声速飞行器的外挂武器和传感器吊舱一般悬挂在机翼或机腹下，而飞行器在高速机动飞行情况下，受空气气流、载荷变更、发动机噪声等多种因素的影响，机体会发生时变结构变形，复合材料的更多使用和现代战斗机的高机动特性使机身和机翼的弹性特性增强，产生的弹性变形对传递对准的精度影响更加剧烈。临近空间飞行器传递对准时，存在动态杆臂和挠曲变形难以建模等问题，迫切需要利用模型不确定的鲁棒滤波方法来解决。

目前模型不确定的鲁棒滤波方法主要是鲁棒EKF算法，该算法由于EKF将非线性系统线性化，在强非线性时将会引起较大的估计误差，并且在线性化处理时需要用雅克比(Jacobian)矩阵，其繁琐的计算过程导致该方法实现相对困难；基于稀疏网格理论的积分点配置策略以其积分点数目少、计算精度高的特性，形成了一系列基于矩匹配法、Gauss-Hermite准则以及Kronrod-Patterson准则的稀疏网格求积分非线性高斯滤波算法。基于此，研究一种精度高鲁棒性强的滤波方法，成为了行业发展的方向。

发明内容

发明目的：为了解决临近空间飞行器传递对准中存在动态杆臂和挠曲变形难以建模等而导致传递对准性能下降，利用模型不确定的鲁棒滤波方法，将鲁棒算法与一系列稀疏网格求积分非线性滤波技术结合，提供一种精度高鲁棒性强的滤波方法。

技术方案：一种临近空间飞行器传递对准模型不确定性的鲁棒滤波方法，其特征在于，该鲁棒滤波方法具体步骤如下：

步骤1)根据临近空间飞行器传递对准系统的工作原理和特点，建立系统的数学平台失准角误差方程、速度误差方程、位置误差方程和观测方程；

步骤2)根据系统的误差方程建立模型不确定的状态方程和观测方程；

x_k＝f(x_k-1)+Φ(x_k-1)η_k+G_k|k-1w_k-1   (29)

z_k＝h(x_k)+Ψ(x_k)υ_k+v_k   (30)

式中：

x_k是n维状态向量,z_k是m维观测向量，f(·)和h(·)分别对应非线性状态方程和观测方程；G_k|k-1是n×r维系统过程噪声输入矩阵，w_k-1是r维系统过程噪声序列,v_k是m维系统观测噪声序列；Φ(·)∈R^n×n是系统状态方程中模型不确定性部分的有界输入矩阵，Ψ(·)∈R^m×m是系统观测方程中模型不确定性部分的有界输入矩阵；η_k∈Rⁿ是系统状态方程中模型不确定性未知有界变量，υ_k∈R^m是系统观测方程中模型不确定性未知有界变量；

步骤3)给出状态变量初始值(x₀)和预测误差方差矩阵初始值(Σ_0|0)，给出N_p个稀疏网格求积分点集(ξ_j，ε_j；j＝1,2，…N_p)；

状态变量初始值x₀＝[000 000 000 000 000 000]^T；

预测误差方差矩阵初始值：

${\mathrm{\Σ}}_{0|0}=\mathrm{diag}[{\mathrm{\φ}}_{x0}^2,{\mathrm{\φ}}_{y0}^2,{\mathrm{\φ}}_{z0}^2,\mathrm{\δ}{V}_{x0}^2,\mathrm{\δ}{V}_{y0}^2,\mathrm{\δ}{V}_{z0}^2,\mathrm{\δ}{S}_{x0}^2,\mathrm{\δ}{S}_{y0}^2,\mathrm{\δ}{S}_{z0}^2,{\mathrm{\ϵ}}_{\mathrm{gyx}0}^2,{\mathrm{\ϵ}}_{\mathrm{gy}0}^2,{\mathrm{\ϵ}}_{\mathrm{gz}0}^2,{\▿}_{\mathrm{ax}0}^2,{\▿}_{\mathrm{ay}0}^2,{\▿}_{\mathrm{az}0}^2,{\mathrm{\μ}}_{x0}^2{,\mathrm{\μ}}_{y0}^2,{\mathrm{\μ}}_{z0}^2];$

系统过程噪声初始值 $Q_0=\mathrm{diag}[w_{\mathrm{gx}}^2,w_{\mathrm{gy}}^2,w_{\mathrm{gz}}^2,w_{\mathrm{ax}}^2,w_{\mathrm{ay}}^2,w_{\mathrm{az}}^2];$

系统观测噪声初始值 $R_0=\mathrm{diag}[{\mathrm{\σ}}_{\mathrm{ax}}^2,{\mathrm{\σ}}_{\mathrm{ay}}^2,{\mathrm{\σ}}_{\mathrm{az}}^2,{\mathrm{\σ}}_{\mathrm{vx}}^2,{\mathrm{\σ}}_{\mathrm{vy}}^2,{\mathrm{\σ}}_{\mathrm{vz}}^2,{\mathrm{\σ}}_{\mathrm{sx}}^2,{\mathrm{\σ}}_{\mathrm{sy}}^2,{\mathrm{\σ}}_{\mathrm{sz}}^2];$

式中：

φ_x0、φ_y0和φ_z0是初始数学平台失准角；

δV_x0、δV_y0和δV_z0是初始速度误差；δS_x0、δS_y0和δS_z0是初始位置误差；

ε_gx0、ε_gy0和ε_gz0是陀螺仪常值漂移初值；▽_ax0、▽_ay0和▽_az0是加速度计常值偏移初值；μ_x0、μ_y0和μ_z0是主子惯导间安装误差初值；

w_gx、w_gy和w_gz是陀螺仪随机噪声；w_ax、w_ay和w_az是加速度计随机噪声；

σ_ax、σ_ay和σ_az是姿态观测噪声标准差；σ_vx、σ_vy和σ_vz是速度观测噪声标准差；

σ_sx、σ_sy和σ_sz是位置观测噪声标准差；

根据稀疏网格求积分准则给定一组积分点集(ξ_j，ω_j)其中j＝1,2，…N_p，N_p表示积分点集的个数；

${\mathrm{\ξ}}_j=\left\{\begin{array}{cc}{[\mathrm{0,0},...,0]}^T;& j=1\\ {[0,...,-1,...,0]}^T;& j=2,...,n+1\\ {[0,...,1,...,0]}^T;& j=n+2,...,2n+1\\ {[0,...,-\sqrt{3},...,0]}^T;& j=2n+2,...,3n+1\\ {[0,...,\sqrt{3},...,0]}^T;& j=3n+2,...,4n+1\\ {[0,...,-1,...,-1,...,0]}^T;& j=4n+2,...,n(n-1)/2+4n+1\\ {[0,...,-1,...,1,...,0]}^T;& j=n(n-1)/2+4n+2,...,n(n-1)+4n+1\\ {[0,...,1,...,-1,...,0]}^T;& j=n(n-1)+4n+2,...,3n(n-1)/2+4n+1\\ {[0,...,1,...,1,...,0]}^T;& j=3n(n-1)/2+4n+2,...,2n(n-1)+4n+1\end{array}\right.$

${\mathrm{\ϵ}}_j=\left\{\begin{array}{cc}-n^2/2-5n/6+1& j=1\\ -n/2+1/2& j=2,...,2n+1\\ 1/6& j=2n+2,...,4n+1\\ 1/4& j=4n+2,...,2n^2+2n+1\end{array}\right.$

其中Np＝2n²+2n+1,n为状态变量维数；

给定满足γ>1和ε>0的鲁棒滤波参数；具体为：第一组γ₁＝500，ε₁＝0.05：

第二组γ₁＝100，ε₁＝0.01：

步骤4)利用鲁棒稀疏网格求积分滤波经过初始采样、时间更新、重采样、量测更新和鲁棒更新过程对临近空间飞行器传递对准系统状态进行估计，并对子惯导系统进行误差修正，判断k+1是否大于等于步长L，如果是，状态估计结束，完成传递对准过程，否则返回初始采样过程进行下一次估计；

2、临近空间飞行器传递对准模型不确定性的鲁棒滤波方法，其特征在于，所述步骤1)系统数学平台失准角误差方程、速度误差方程、位置误差方程和观测方程具体为：

1.1)数学平台失准角误差方程

${\stackrel{\·}{\mathrm{\φ}}}^i=-C_w^{-1}{C}_b^{\hat{i}}\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ib}}^b---\left(31\right)$

其中：

i是发射点惯性坐标系，此处也是导航坐标系；

是惯导解算的发射点惯性坐标系，即数学平台坐标系；i系依次经过三次变换可得数学平台坐标系系，三次转动角分别为：绕z轴旋转φ_z、绕y轴旋转φ_y和绕x轴旋转φ_x；

发射点惯性坐标系下数学平台失准角φⁱ＝[φ_x φ_y φ_z]^T；

$C_w^{-1}=\frac{1}{\cos {\mathrm{\φ}}_y}\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_y& \sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_y& \cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y\\ 0& \cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y& -\sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\\ 0& \sin {\mathrm{\φ}}_x& \cos {\mathrm{\φ}}_x\end{array}\right];$

b是弹体坐标系，即子惯导坐标系，也可以用bs表示；原点O_b为弹体质心，O_bx_b轴为弹体纵轴对称轴，指向弹体头部；O_by_b轴在弹体纵向对称面内，并垂直于纵轴O_bz_b向上；O_bz_b按照右手规则确定；

为子惯导解算的姿态矩阵，也可以用表示，表示弹体坐标系b到数学平台坐标系的姿态转换矩阵；

为陀螺测量误差，且ε_g＝[ε_gx ε_gy ε_gz]^T为三只陀螺仪测量误差的随机常值部分，w_g＝[w_gx w_gy w_gz]^T为三只陀螺仪测量误差的随机噪声部分，随机噪声均假设为高斯白噪声；

1.2)速度误差方程

惯性坐标系下速度误差微分方程为，

$\mathrm{\δ}{\stackrel{\·}{V}}^i=(I-C_{\hat{i}}^i)C_b^{\hat{i}}{f}_{\mathrm{ib}}^b+C_{\hat{i}}^i{C}_b^{\hat{i}}\mathrm{\δ}{f}_{\mathrm{ib}}^b+\mathrm{\δ}{g}^i---\left(32\right)$

式中：

δVⁱ＝[δV_x δV_y δV_z]^T；

是子惯导IMU的加速度计测量值；

是子惯导IMU的加速度计测量误差，且▽_a＝[▽_ax ▽_ay ▽_az]^T为加速度计测量误差的随机常值部分，w_a＝[w_ax w_ay w_gz]^T为三只加速度计测量误差的随机噪声部分，随机噪声均假设为高斯白噪声；

是系至i系的变换阵；

δgⁱ是引力场模型的引力加速度误差；

1.3)位置误差方程

发射点惯性坐标系下位置误差δSⁱ微分方程为，

$\mathrm{\δ}{\stackrel{\·}{S}}^i=\mathrm{\δ}{V}^i---\left(33\right)$

式中：δSⁱ＝[δS_x δS_y δS_z]^T；

1.4)姿态匹配观测方程

$Z_{\mathrm{dcm}}=[I-({\mathrm{\φ}}_m^{i*}\×)]C_{\mathrm{bm}}^i{C}_{\mathrm{bm}}^{\mathrm{bs}}{C}_i^{\mathrm{bm}}{C}_{\hat{i}}^i---\left(34\right)$

式中：

bm是载体坐标系即载机坐标系；

bs是子惯导坐标系；

是主惯导的姿态阵，是的转置矩阵；

—主惯导的姿态误差角，可将其视为白噪声；

是bm系到bs系的变换矩阵；

姿态匹配观测方程由观测矩阵Z_dcm矩阵求得，具体为：

$Z_{\mathrm{am}}=\left[\begin{array}{c}\arctan \left(\frac{z_{21}}{z_{11}}\right)\\ -\arcsin \left(z_{31}\right)\\ \arctan \left(\frac{z_{32}}{z_{33}}\right)\end{array}\right]---\left(35\right)$

式中：Z_am是姿态匹配的观测量， $Z_{\mathrm{dcm}}=\left[\begin{array}{ccc}{z}_{11}& z_{12}& z_{13}\\ z_{21}& z_{22}& z_{23}\\ z_{31}& z_{32}& z_{33}\end{array}\right];$

1.5)速度匹配观测方程

Z_v＝H_vx_k+v_v   (36)

式中：

$H_v=\left[\begin{array}{cccccccccccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right];$

v_v是速度匹配等效观测噪声；

1.6)位置匹配观测方程

Z_s＝H_sx_k+v_s   (37)

式中：

$H_s=\left[\begin{array}{cccccccccccccccccc}0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

3、临近空间飞行器传递对准模型不确定性的鲁棒滤波方法，其特征在于，所述步骤2)模型不确定的状态方程和观测方程具体为：

2.1)x^k具x_k＝[φ_x φ_y φ_z δV_x δV_y δV_z δS_x δS_y δS_z ε_gx ε_gy ε_gz ▽_ax ▽_ay ▽_az μ_x μ_y μ_z]^T

可简写为 $x_k={\left[\begin{array}{cccccc}{\mathrm{\φ}}^{\mathrm{iT}}& \mathrm{\δ}{V}^{\mathrm{iT}}& \mathrm{\δ}{S}^{\mathrm{iT}}& {\mathrm{\ϵ}}_g^{\mathrm{iT}}& {\▿}_a^{\mathrm{iT}}& {\mathrm{\μ}}^{\mathrm{iT}}\end{array}\right]}^T;$

包括导航系为发射点惯性坐标系下失准角φⁱ＝[φ_x φ_y φ_z]^T、速度误差δVⁱ＝[δV_x δV_y δV_z]^T、位置误差δSⁱ＝[δS_x δS_y δS_z]^T、陀螺仪随机常值误差ε_g ⁱ＝[ε_gx ε_gy ε_gz]^T、加速度计随机常值误差▽_a ⁱ＝[▽_ax ▽_ay ▽_az]^T、安装误差μⁱ＝[μ_x μ_y μ_z]^T；系统噪声向量为：w_k＝[ε_gx ε_gy ε_gz ▽_ax ▽_ay▽_az]^T；

2.2)系统状态方程中模型不确定性部分的有界输入矩阵Φ(·)具体为

$\mathrm{\Φ}\left(x_{k-1}\right)=\left[\begin{array}{cc}\mathrm{\Φ}{\left({\mathrm{\φ}}^i\right)}_{3\×2}& 0_{3\×16}\\ \mathrm{\Φ}{\left({\mathrm{\ϵ}}_g\right)}_{3\×2}& 0_{3\×16}\\ 0_{12\×2}& 0_{12\×16}\end{array}\right],$ $\mathrm{\Φ}{\left({\mathrm{\φ}}^i\right)}_{3\×2}=\left[\begin{array}{cc}{\mathrm{\φ}}_z& -{\mathrm{\φ}}_y\\ -{\mathrm{\φ}}_z& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y& -{\mathrm{\φ}}_x\end{array}\right],$ $\mathrm{\Φ}{\left({\mathrm{\ϵ}}_g\right)}_{3\×2}=\left[\begin{array}{cc}{\mathrm{\ϵ}}_{\mathrm{gz}}& -{\mathrm{\ϵ}}_{\mathrm{gy}}\\ -{\mathrm{\ϵ}}_{\mathrm{gz}}& {\mathrm{\ϵ}}_{\mathrm{gx}}\\ {\mathrm{\ϵ}}_{\mathrm{gy}}& -{\mathrm{\ϵ}}_{\mathrm{gx}}\end{array}\right]---\left(38\right)$

η_k满足如下关系：

$E\left[{\mathrm{\η}}_k{\mathrm{\η}}_k^T\right]{\≤q}_{k}I---\left(39\right)$

式中：η_k＝[η_φ 0_1×12]^T,η_φ∈R⁶, $E\left[{\mathrm{\η}}_{\mathrm{\φ}}{\mathrm{\η}}_{\mathrm{\φ}}^T\right]{\≤q}_{\mathrm{\φ},k}I,$ $q_{\mathrm{\φ},k}=\frac{1}{1+{\mathrm{\ϵ}}^{-1}}{w}_g$

2.3)模型不确定的状态方程具体为：

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}^i\\ \mathrm{\δ}{\stackrel{\·}{V}}^i\\ \mathrm{\δ}{\stackrel{\·}{S}}^i\\ {\stackrel{\·}{\mathrm{\ϵ}}}_g\\ {\stackrel{\·}{\▿}}_a\\ {\stackrel{\·}{\mathrm{\μ}}}^i\end{array}\right]=\left[\begin{array}{c}-C_w^{-1}{C}_b^{\hat{i}}{\mathrm{\ϵ}}_g\\ (I-C_{\hat{i}}^i)C_b^{\hat{i}}{f}_{\mathrm{ib}}^b+C_{\hat{i}}^i{C}_b^{\hat{i}}{\▿}_a+\mathrm{\δ}{g}^i\\ \mathrm{\δ}{V}^i\\ 0\\ 0\\ 0\end{array}\right]+\left[\begin{array}{cc}\mathrm{\Φ}{\left({\mathrm{\φ}}^i\right)}_{3\×2}& 0_{3\×16}\\ \mathrm{\Φ}{\left({\mathrm{\ϵ}}_g\right)}_{3\×2}& 0_{3\×16}\\ 0_{12\×2}& 0_{12\×16}\end{array}\right]\left[\begin{array}{c}{\mathrm{\η}}_{\mathrm{\φ}}^T\\ 0_{12\×1}\end{array}\right]\left[\begin{array}{cc}-C_w^{-1}{C}_b^{\hat{i}}& 0_{3\×3}\\ 0_{3\×3}& C_{\hat{i}}^i{C}_b^{\hat{i}}\\ 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]w_{k-1}---\left(40\right)$

2.4)系统观测方程中模型不确定性部分的有界输入矩阵Ψ(·)具体为：

$\mathrm{\Ψ}\left(x_k\right)=\left[\begin{array}{cc}{\mathrm{\Ψ}}_{\mathrm{\φ}}\×& 0_{3\×6}\\ 0_{6\×3}& 0_{3\×3}\end{array}\right]---\left(41\right)$

Ψ_φ是姿态匹配模型不确定部分，主要由初始安装误差确定，具体为：

${\mathrm{\Ψ}}_{\mathrm{\φ}}\×=\left[\begin{array}{ccc}0& -{\mathrm{\μ}}_{z0}& {\mathrm{\μ}}_{y0}\\ {\mathrm{\μ}}_{z0}& 0& -{\mathrm{\μ}}_{x0}\\ -{\mathrm{\μ}}_{y0}& {\mathrm{\μ}}_{x0}& 0\end{array}\right]---\left(42\right)$

2.5)模型不确定的观测方程具体为:

$\left[\begin{array}{c}{Z}_a\\ Z_v\\ Z_s\end{array}\right]=\left[\begin{array}{c}{Z}_{\mathrm{am}}\\ H_v{x}_k\\ H_s{x}_k\end{array}\right]+\left[\begin{array}{cc}{\mathrm{\Ψ}}_{\mathrm{\φ}}\×& 0_{3\×6}\\ 0_{6\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}{\mathrm{\υ}}_{\mathrm{\φ}}\\ 0_{6\×1}\end{array}\right]+v_k---\left(43\right)$

υ_φ∈R³是与姿态相关的观测模型不确定性未知有界变量，具体为

${\mathrm{\υ}}_k={\left[\begin{array}{cc}{\mathrm{\υ}}_{\mathrm{\φ}}& 0_{1\×6}\end{array}\right]}^T,E\left[{\mathrm{\υ}}_{\mathrm{\φ}}{\mathrm{\υ}}_{\mathrm{\φ}}^T\right]\≤r_{k}I,r_k=\frac{1}{1+{\mathrm{\ϵ}}^{-1}}{w}_g$

观测向量为:z_k＝[Z_a Z_v Z_s]^T；其中姿态观测量Z_a＝[Z_ax Z_ay Z_az]^T，速度观测量Z_v＝[Z_vx Z_vy Z_vz]^T，位置观测量Z_s＝[Z_sx Z_sy Z_sz]^T；

4、临近空间飞行器传递对准模型不确定性的鲁棒滤波方法，其特征在于，所述步骤4)利用鲁棒稀疏网格求积分滤波经过初始采样、时间更新、重采样、量测更新和鲁棒更新过程对临近空间飞行器传递对准系统状态进行估计，并对子惯导系统进行误差修正，判断k+1是否大于等于步长L，如果是，状态估计结束，完成传递对准过程，否则返回初始采样过程进行下一次估计，具体为：

4.1)初始采样

${\mathrm{\γ}}_{j,k-1}=A{\mathrm{\ξ}}_j+{\hat{x}}_{k-1};{\mathrm{AA}}^T={\mathrm{\Σ}}_{k-1|k-1},j=\mathrm{1,2},...,N_p---\left(44\right)$

4.2)时间更新

χ_j,k|k-1＝f(γ_j,k-1)   (45)

${\hat{x}}_{k|k-1}=\underset{j=1}{\overset{N_p}{\mathrm{\Σ}}}{\mathrm{\ω}}_j{\mathrm{\χ}}_{j,k|k-1}---\left(46\right)$

${\mathrm{\Σ}}_{k|k-1}=\underset{j=1}{\overset{N_p}{\mathrm{\Σ}}}{\mathrm{\ω}}_j({\mathrm{\χ}}_{j,k|k-1}-{\hat{x}}_{k|k-1}){({\mathrm{\χ}}_{j,k|k-1}-{\hat{x}}_{k|k-1})}^T+{\hat{Q}}_{k-1}^{*}---\left(47\right)$

其中

${\hat{Q}}_{k-1}^{*}=[(1+2\mathrm{\ϵ}){(1-{\mathrm{\γ}}^{-2})}^{-1}-1]\underset{j=1}{\overset{N_p}{\mathrm{\Σ}}}{\mathrm{\ω}}_j({\mathrm{\χ}}_{j,k|k-1}-{\hat{x}}_{k|k-1}){({\mathrm{\χ}}_{j,k|k-1}-{\hat{x}}_{k|k-1})}^T+(2+{\mathrm{\ϵ}}^{-1})q_k{\stackrel{\‾}{\mathrm{\Φ}}}_k{\stackrel{\text{-}}{\mathrm{\Φ}}}_k^T+(2+{\mathrm{\ϵ}}^{-1})G_{k|k-1}{Q}_{k-1}{G}_{k|k-1}^T---\left(48\right)$

4.3)重采样

${\widetilde{\mathrm{\γ}}}_j=A{\mathrm{\ξ}}_j+{\hat{x}}_{k|k-1};{\mathrm{AA}}^T={\text{\Σ}}_{k|k-1},j=\mathrm{1,2},...,N_p---\left(49\right)$

4.4)量测更新

${\hat{z}}_{k|k-1}=\underset{j=1}{\overset{N_p}{\mathrm{\Σ}}}{\mathrm{\ω}}_{j}h\left({\widetilde{\mathrm{\γ}}}_j\right)---\left(50\right)$

${\mathrm{\Σ}}_{\mathrm{zz},k|k-1}=\underset{j=1}{\overset{N_p}{\mathrm{\Σ}}}{\mathrm{\ω}}_j(h\left({\widetilde{\mathrm{\γ}}}_j\right)-{\hat{z}}_{k|k-1}){(h\left({\widetilde{\mathrm{\γ}}}_j\right)-{\hat{z}}_{k|k-1})}^T---\left(51\right)$

${\hat{R}}_k^{*}=(2+{\mathrm{\ϵ}}^{-1})r_k{\stackrel{\‾}{\mathrm{\Ψ}}}_k{\stackrel{\‾}{\mathrm{\Ψ}}}_k^T+(2+{\mathrm{\ϵ}}^{-1})R_k---\left(52\right)$

${\mathrm{\Σ}}_{\mathrm{xz},k|k-1}=\underset{j=1}{\overset{N_p}{\mathrm{\Σ}}}{\mathrm{\ω}}_j({\widetilde{\mathrm{\γ}}}_j-{\hat{x}}_{k|k-1}){(h\left({\widetilde{\mathrm{\γ}}}_j\right)-{\hat{z}}_{k|k-1})}^T---\left(53\right)$

$K_k=(1+2\mathrm{\ϵ}){(1-{\mathrm{\γ}}^{-2})}^{-1}{\mathrm{\Σ}}_{\mathrm{xz},k|k-1}{[(1+2\mathrm{\ϵ}){(1-{\mathrm{\γ}}^{-2})}^{-1}{\mathrm{\Σ}}_{\mathrm{zz},k|k-1}+{\hat{R}}_k^{*}]}^{-1}---\left(54\right)$

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})---\left(55\right)$

4.5)鲁棒更新

${\mathrm{\Σ}}_{k|k}=(1+2\mathrm{\ϵ}){(1-{\mathrm{\γ}}^{-2})}^{-1}[{\mathrm{\Σ}}_{k|k-1}-K_k{\mathrm{\Σ}}_{\mathrm{zz},k|k-1}{K}_k^T]+K_k{\hat{R}}_k^{*}{K}_k^T---\left(56\right)$

式中：

Σ_k-1|k-1为预测误差方差矩阵；为状态变量；Σ_k|k-1为一步预测误差方差矩阵；A为Σ_k-1|k-1或Σ_k|k-1通过Cholupdate或SVD分解得到的矩阵；ξ_j为稀疏网格求积分点集采样点；ω_j为稀疏网格求积分点集采样点对应的权重；γ_j，k-1为初始采样点；N_p为积分点集个数；χ_j，k|k-1为时间更新的Sigma点；为时间更新的状态变量一步预测值；R_k为系统观测噪声矩阵；为计算系统观测噪声矩阵；Q_k-1为系统过程噪声矩阵；G_k|k-1为系统过程噪声输入矩阵；为计算系统过程噪声矩阵；为重采样的Sigma点；为观测一步预测值；Σ_zz,k|k-1为观测一步预测误差方差矩阵；Σ_xz,k|k-1为协误差方差矩阵；K_k为滤波增益矩阵；为状态估计值；Σ_k|k为估计误差方差矩阵。

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

本发明的有点在于

(1)本发明采用鲁棒算法与稀疏网格求积分滤波相结合，形成了一套高精度高鲁棒性的滤波方法。

(2)本发明在滤波过程中不需要增加额外的计算量，只需要选择两个实数鲁棒因子，即可保证系统的鲁棒性，所以具有容易实现的优点。

(3)本发明不需要对系统的杆臂挠曲建模，减小系统维数，减小系统计算量。

(4)本发明的鲁棒滤波方法对实际物理系统中由于物理参数的测量误差、运行环境的变化或系统辨识不精确而引起的模型不确定性均具有较好的适应性。

本发明提出的方案通过如下仿真实验加以验证：

(1)传感器数据采样时间为1ms，滤波周期Tf为20ms，仿真时间6分钟；

(2)初始位置为北纬31.98°，东经118.8°，高度50Km，初始速度为3马赫，安装误差角为5′5′5′，陀螺仪常值漂移为0.01°/h,随机漂移为0.001°/h，加速度计常偏为0.1mg,随机为0.05mg；

(3)静态杆臂为0.15m，0.15m，0.30m，未建模动态杆臂为8-12mm，8-14mm，25-30mm；

(4)横滚角从0°到34°做摇翼机动，俯仰角和航向角从0°到10°做匀速变化；仿真过程中注入挠曲变形干扰，采用“姿态+速度+位置”匹配方式进行传递对准，系统的状态方程和观测方程不对主子惯导间的杆臂和挠曲变形进行建模；

(5)子惯导初始失准角分别为40°/30°/20°，滤波器的初始条件设为

x₀＝[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]^T；

Σ_0|0＝diag{(30°)²,(20°)²,(40°)²,(0.5m/s)²,(0.5m/s)²,(0.5m/s)²,(10.0m)²,(10.0m)²,(10.0m)²,(0.01°/h)²,(0.01°/h)²,(0.01°/h)²,(0.1mg)²,(0.1mg)²,(0.1mg)²,(5′)²,(5′)²,(5′)²}^T；

Q₀＝diag{(0.01°/h)²,(0.01°/h)²,(0.01°/h)²,(0.05mg)²,(0.05mg)²,(0.05mg)²}；

R₀＝diag{(0.01°/h)²,(0.01°/h)²,(0.01°/h)²,(0.5m/s)²,(0.5m/s)²,(0.5m/s)²,(10.0m)²,(10.0m)²,(10.0m)²}；

(6)分别仿真两组不同鲁棒滤波参数γ₁＝500和ε₁＝0.05，鲁棒滤波参数γ₂＝100和ε₂＝0.01。

附图说明

图1为基于临近空间飞行器传递对准模型不确定性的鲁棒滤波流程框图；

图2为鲁棒滤波参数γ₁＝500和ε₁＝0.05仿真传递对准姿态误差曲线图；

图3为鲁棒滤波参数γ₂＝100和ε₂＝0.01仿真传递对准姿态误差曲线图。

具体实施方式

下面结合附图对本发明做更进一步的解释。

根据下述实施例，可以更好的理解本发明。如图1所示，本发明的一种基于临近空间飞行器传递对准模型不确定的鲁棒滤波方法，具体步骤如下：

步骤1)根据临近空间飞行器传递对准系统的工作原理和特点，建立系统的数学平台失准角误差方程、速度误差方程、位置误差方程和观测方程；

步骤2)根据系统的误差方程建立模型不确定的状态方程和观测方程；

x_k＝f(x_k-1)+Φ(x_k-1)η_k+G_k|k-1w_k-1   (57)

z_k＝h(x_k)+Ψ(x_k)υ_k+v_k   (58)

式中：

x_k是n维状态向量,z_k是m维观测向量，f(·)和h(·)分别对应非线性状态方程和观测方程；G_k|k-1是n×r维系统过程噪声输入矩阵，w_k-1是r维系统过程噪声序列,v_k是m维系统观测噪声序列；Φ(·)∈R^n×n是系统状态方程中模型不确定性部分的有界输入矩阵，Ψ(·)∈R^m×m是系统观测方程中模型不确定性部分的有界输入矩阵；η_k∈Rⁿ是系统状态方程中模型不确定性未知有界变量，υ_k∈R^m是系统观测方程中模型不确定性未知有界变量；

步骤3)给出状态变量初始值(x₀)和预测误差方差矩阵初始值(Σ_0|0)，给出N_p个稀疏网格求积分点集(ξ_j，ε_j；j＝1,2，…N_p)；

状态变量初始值x₀＝[000 000 000 000 000 000]^T；

预测误差方差矩阵初始值：

${\mathrm{\Σ}}_{0|0}=\mathrm{diag}[{\mathrm{\φ}}_{x0}^2,{\mathrm{\φ}}_{y0}^2,{\mathrm{\φ}}_{z0}^2,\mathrm{\δ}{V}_{x0}^2,\mathrm{\δ}{V}_{y0}^2,\mathrm{\δ}{V}_{z0}^2,\mathrm{\δ}{S}_{x0}^2,\mathrm{\δ}{S}_{y0}^2,\mathrm{\δ}{S}_{z0}^2,{\mathrm{\ϵ}}_{\mathrm{gyx}0}^2,{\mathrm{\ϵ}}_{\mathrm{gy}0}^2,{\mathrm{\ϵ}}_{\mathrm{gz}0}^2,{\▿}_{\mathrm{ax}0}^2,{\▿}_{\mathrm{ay}0}^2,{\▿}_{\mathrm{az}0}^2,{\mathrm{\μ}}_{x0}^2{,\mathrm{\μ}}_{y0}^2,{\mathrm{\μ}}_{z0}^2];$

系统过程噪声初始值 $Q_0=\mathrm{diag}[w_{\mathrm{gx}}^2,w_{\mathrm{gy}}^2,w_{\mathrm{gz}}^2,w_{\mathrm{ax}}^2,w_{\mathrm{ay}}^2,w_{\mathrm{az}}^2];$

系统观测噪声初始值 $R_0=\mathrm{diag}[{\mathrm{\σ}}_{\mathrm{ax}}^2,{\mathrm{\σ}}_{\mathrm{ay}}^2,{\mathrm{\σ}}_{\mathrm{az}}^2,{\mathrm{\σ}}_{\mathrm{vx}}^2,{\mathrm{\σ}}_{\mathrm{vy}}^2,{\mathrm{\σ}}_{\mathrm{vz}}^2,{\mathrm{\σ}}_{\mathrm{sx}}^2,{\mathrm{\σ}}_{\mathrm{sy}}^2,{\mathrm{\σ}}_{\mathrm{sz}}^2];$

式中：

φ_x0、φ_y0和φ_z0是初始数学平台失准角；

δV_x0、δV_y0和δV_z0是初始速度误差；δS_x0、δS_y0和δS_z0是初始位置误差；

ε_gx0、ε_gy0和ε_gz0是陀螺仪常值漂移初值；▽_ax0、▽_ay0和▽_az0是加速度计常值偏移初值；μ_x0、μ_y0和μ_z0是主子惯导间安装误差初值；

w_gx、w_gy和w_gz是陀螺仪随机噪声；w_ax、w_ay和w_az是加速度计随机噪声；

σ_ax、σ_ay和σ_az是姿态观测噪声标准差；σ_vx、σ_vy和σ_vz是速度观测噪声标准差；

σ_sx、σ_sy和σ_sz是位置观测噪声标准差；

根据稀疏网格求积分准则给定一组积分点集(ξ_j，ω_j)其中j＝1,2，…N_p，N_p表示积分点集的个数；

${\mathrm{\ξ}}_j=\left\{\begin{array}{cc}{[\mathrm{0,0},...,0]}^T;& j=1\\ {[0,...,-1,...,0]}^T;& j=2,...,n+1\\ {[0,...,1,...,0]}^T;& j=n+2,...,2n+1\\ {[0,...,-\sqrt{3},...,0]}^T;& j=2n+2,...,3n+1\\ {[0,...,\sqrt{3},...,0]}^T;& j=3n+2,...,4n+1\\ {[0,...,-1,...,-1,...,0]}^T;& j=4n+2,...,n(n-1)/2+4n+1\\ {[0,...,-1,...,1,...,0]}^T;& j=n(n-1)/2+4n+2,...,n(n-1)+4n+1\\ {[0,...,1,...,-1,...,0]}^T;& j=n(n-1)+4n+2,...,3n(n-1)/2+4n+1\\ {[0,...,1,...,1,...,0]}^T;& j=3n(n-1)/2+4n+2,...,2n(n-1)+4n+1\end{array}\right.$

${\mathrm{\ϵ}}_j=\left\{\begin{array}{cc}-n^2/2-5n/6+1& j=1\\ -n/2+1/2& j=2,...,2n+1\\ 1/6& j=2n+2,...,4n+1\\ 1/4& j=4n+2,...,2n^2+2n+1\end{array}\right.$

其中Np＝2n²+2n+1,n为状态变量维数；

给定满足γ>1和ε>0的鲁棒滤波参数；具体为：第一组γ₁＝500，ε₁＝0.05：

第二组γ₁＝100，ε₁＝0.01：

步骤4)利用鲁棒稀疏网格求积分滤波经过初始采样、时间更新、重采样、量测更新和鲁棒更新过程对临近空间飞行器传递对准系统状态进行估计，并对子惯导系统进行误差修正，判断k+1是否大于等于步长L，如果是，状态估计结束，完成传递对准过程，否则返回初始采样过程进行下一次估计；

2、临近空间飞行器传递对准模型不确定性的鲁棒滤波方法，其特征在于，所述步骤1)系统数学平台失准角误差方程、速度误差方程、位置误差方程和观测方程，具体为：

1.1)数学平台失准角误差方程

${\stackrel{\·}{\mathrm{\φ}}}^i=-C_w^{-1}{C}_b^{\hat{i}}\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ib}}^b---\left(59\right)$

其中：

i是发射点惯性坐标系，此处也是导航坐标系；

是惯导解算的发射点惯性坐标系，即数学平台坐标系；i系依次经过三次变换可得数学平台坐标系系，三次转动角分别为：绕z轴旋转φ_z、绕y轴旋转φ_y和绕x轴旋转φ_x；

发射点惯性坐标系下数学平台失准角φⁱ＝[φ_x φ_y φ_z]^T；

$C_w^{-1}=\frac{1}{\cos {\mathrm{\φ}}_y}\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_y& \sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_y& \cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y\\ 0& \cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y& -\sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\\ 0& \sin {\mathrm{\φ}}_x& \cos {\mathrm{\φ}}_x\end{array}\right];$

b是弹体坐标系，即子惯导坐标系，也可以用bs表示；原点O_b为弹体质心，O_bx_b轴为弹体纵轴对称轴，指向弹体头部；O_by_b轴在弹体纵向对称面内，并垂直于纵轴O_bz_b向上；O_bz_b按照右手规则确定；

为子惯导解算的姿态矩阵，也可以用表示，表示弹体坐标系b到数学平台坐标系的姿态转换矩阵；

为陀螺测量误差，且ε_g＝[ε_gx ε_gy ε_gz]^T为三只陀螺仪测量误差的随机常值部分，w_g＝[w_gx w_gy w_gz]^T为三只陀螺仪测量误差的随机噪声部分，随机噪声均假设为高斯白噪声；

1.2)速度误差方程

惯性坐标系下速度误差微分方程为，

$\mathrm{\δ}{\stackrel{\·}{V}}^i=(I-C_{\hat{i}}^i)C_b^{\hat{i}}{f}_{\mathrm{ib}}^b+C_{\hat{i}}^i{C}_b^{\hat{i}}\mathrm{\δ}{f}_{\mathrm{ib}}^b+\mathrm{\δ}{g}^i---\left(60\right)$

式中：

δVⁱ＝[δV_x δV_y δV_z]^T；

是子惯导IMU的加速度计测量值；

是子惯导IMU的加速度计测量误差，且▽_a＝[▽_ax ▽_ay ▽_az]^T为加速度计测量误差的随机常值部分，w_a＝[w_ax w_ay w_gz]^T为三只加速度计测量误差的随机噪声部分，随机噪声均假设为高斯白噪声；

是系至i系的变换阵；

δgⁱ是引力场模型的引力加速度误差；

1.3)位置误差方程

发射点惯性坐标系下位置误差δSⁱ微分方程为，

$\mathrm{\δ}{\stackrel{\·}{S}}^i=\mathrm{\δ}{V}^i---\left(61\right)$

式中：δSⁱ＝[δS_x δS_y δS_z]^T；

1.4)姿态匹配观测方程

$Z_{\mathrm{dcm}}=[I-({\mathrm{\φ}}_m^{i*}\×)]C_{\mathrm{bm}}^i{C}_{\mathrm{bm}}^{\mathrm{bs}}{C}_i^{\mathrm{bm}}{C}_{\hat{i}}^i---\left(62\right)$

式中：

bm是载体坐标系即载机坐标系；

bs是子惯导坐标系；

是主惯导的姿态阵，是的转置矩阵；

—主惯导的姿态误差角，可将其视为白噪声；

是bm系到bs系的变换矩阵；

姿态匹配观测方程由观测矩阵Z_dcm矩阵求得，具体为：

$Z_{\mathrm{am}}=\left[\begin{array}{c}\arctan \left(\frac{z_{21}}{z_{11}}\right)\\ -\arcsin \left(z_{31}\right)\\ \arctan \left(\frac{z_{32}}{z_{33}}\right)\end{array}\right]---\left(63\right)$

式中：Z_am是姿态匹配的观测量， $Z_{\mathrm{dcm}}=\left[\begin{array}{ccc}{z}_{11}& z_{12}& z_{13}\\ z_{21}& z_{22}& z_{23}\\ z_{31}& z_{32}& z_{33}\end{array}\right];$

1.5)速度匹配观测方程

Z_v＝H_vx_k+v_v(64)

式中：

$H_v=\left[\begin{array}{cccccccccccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right];$

v_v是速度匹配等效观测噪声；

1.6)位置匹配观测方程

Z_s＝H_sx_k+v_s   (65)

式中：

$H_s=\left[\begin{array}{cccccccccccccccccc}0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

3、临近空间飞行器传递对准模型不确定性的鲁棒滤波方法，其特征在于，所述步骤2)模型不确定的状态方程和观测方程具体为：

2.1)x^k具x_k＝[φ_x φ_y φ_z δV_x δV_y δV_z δS_x δS_y δS_z ε_gx ε_gy ε_gz ▽_ax ▽_ay ▽_az μ_x μ_y μ_z]^T

可简写为 $x_k={\left[\begin{array}{cccccc}{\mathrm{\φ}}^{\mathrm{iT}}& \mathrm{\δ}{V}^{\mathrm{iT}}& \mathrm{\δ}{S}^{\mathrm{iT}}& {\mathrm{\ϵ}}_g^{\mathrm{iT}}& {\▿}_a^{\mathrm{iT}}& {\mathrm{\μ}}^{\mathrm{iT}}\end{array}\right]}^T;$

包括导航系为发射点惯性坐标系下失准角φⁱ＝[φ_x φ_y φ_z]^T、速度误差δVⁱ＝[δV_x δV_y δV_z]^T、位置误差δSⁱ＝[δS_x δS_y δS_z]^T、陀螺仪随机常值误差ε_g ⁱ＝[ε_gx ε_gy ε_gz]^T、加速度计随机常值误差▽_a ⁱ＝[▽_ax ▽_ay ▽_az]^T、安装误差μⁱ＝[μ_x μ_y μ_z]^T；系统噪声向量为：w_k＝[ε_gx ε_gy ε_gz ▽_ax ▽_ay▽_az]^T；

2.2)系统状态方程中模型不确定性部分的有界输入矩阵Φ(·)具体为

$\mathrm{\Φ}\left(x_{k-1}\right)=\left[\begin{array}{cc}\mathrm{\Φ}{\left({\mathrm{\φ}}^i\right)}_{3\×2}& 0_{3\×16}\\ \mathrm{\Φ}{\left({\mathrm{\ϵ}}_g\right)}_{3\×2}& 0_{3\×16}\\ 0_{12\×2}& 0_{12\×16}\end{array}\right],$ $\mathrm{\Φ}{\left({\mathrm{\φ}}^i\right)}_{3\×2}=\left[\begin{array}{cc}{\mathrm{\φ}}_z& -{\mathrm{\φ}}_y\\ -{\mathrm{\φ}}_z& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y& -{\mathrm{\φ}}_x\end{array}\right],$ $\mathrm{\Φ}{\left({\mathrm{\ϵ}}_g\right)}_{3\×2}=\left[\begin{array}{cc}{\mathrm{\ϵ}}_{\mathrm{gz}}& -{\mathrm{\ϵ}}_{\mathrm{gy}}\\ -{\mathrm{\ϵ}}_{\mathrm{gz}}& {\mathrm{\ϵ}}_{\mathrm{gx}}\\ {\mathrm{\ϵ}}_{\mathrm{gy}}& -{\mathrm{\ϵ}}_{\mathrm{gx}}\end{array}\right]---\left(66\right)$

η_k满足如下关系：

$E\left[{\mathrm{\η}}_k{\mathrm{\η}}_k^T\right]{\≤q}_{k}I---\left(67\right)$

式中：η_k＝[η_φ 0_1×12]^T,η_φ∈R⁶, $E\left[{\mathrm{\η}}_{\mathrm{\φ}}{\mathrm{\η}}_{\mathrm{\φ}}^T\right]{\≤q}_{\mathrm{\φ},k}I,$ $q_{\mathrm{\φ},k}=\frac{1}{1+{\mathrm{\ϵ}}^{-1}}{w}_g$

2.3)模型不确定的状态方程具体为：

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}^i\\ \mathrm{\δ}{\stackrel{\·}{V}}^i\\ \mathrm{\δ}{\stackrel{\·}{S}}^i\\ {\stackrel{\·}{\mathrm{\ϵ}}}_g\\ {\stackrel{\·}{\▿}}_a\\ {\stackrel{\·}{\mathrm{\μ}}}^i\end{array}\right]=\left[\begin{array}{c}-C_w^{-1}{C}_b^{\hat{i}}{\mathrm{\ϵ}}_g\\ (I-C_{\hat{i}}^i)C_b^{\hat{i}}{f}_{\mathrm{ib}}^b+C_{\hat{i}}^i{C}_b^{\hat{i}}{\▿}_a+\mathrm{\δ}{g}^i\\ \mathrm{\δ}{V}^i\\ 0\\ 0\\ 0\end{array}\right]+\left[\begin{array}{cc}\mathrm{\Φ}{\left({\mathrm{\φ}}^i\right)}_{3\×2}& 0_{3\×16}\\ \mathrm{\Φ}{\left({\mathrm{\ϵ}}_g\right)}_{3\×2}& 0_{3\×16}\\ 0_{12\×2}& 0_{12\×16}\end{array}\right]\left[\begin{array}{c}{\mathrm{\η}}_{\mathrm{\φ}}^T\\ 0_{12\×1}\end{array}\right]\left[\begin{array}{cc}-C_w^{-1}{C}_b^{\hat{i}}& 0_{3\×3}\\ 0_{3\×3}& C_{\hat{i}}^i{C}_b^{\hat{i}}\\ 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]w_{k-1}---\left(68\right)$

2.4)系统观测方程中模型不确定性部分的有界输入矩阵Ψ(·)具体为：

$\mathrm{\Ψ}\left(x_k\right)=\left[\begin{array}{cc}{\mathrm{\Ψ}}_{\mathrm{\φ}}\×& 0_{3\×6}\\ 0_{6\×3}& 0_{3\×3}\end{array}\right]---\left(69\right)$

Ψ_φ是姿态匹配模型不确定部分，主要由初始安装误差确定，具体为：

${\mathrm{\Ψ}}_{\mathrm{\φ}}\×=\left[\begin{array}{ccc}0& -{\mathrm{\μ}}_{z0}& {\mathrm{\μ}}_{y0}\\ {\mathrm{\μ}}_{z0}& 0& -{\mathrm{\μ}}_{x0}\\ -{\mathrm{\μ}}_{y0}& {\mathrm{\μ}}_{x0}& 0\end{array}\right]---\left(70\right)$

2.5)模型不确定的观测方程具体为:

$\left[\begin{array}{c}{Z}_a\\ Z_v\\ Z_s\end{array}\right]=\left[\begin{array}{c}{Z}_{\mathrm{am}}\\ H_v{x}_k\\ H_s{x}_k\end{array}\right]+\left[\begin{array}{cc}{\mathrm{\Ψ}}_{\mathrm{\φ}}\×& 0_{3\×6}\\ 0_{6\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}{\mathrm{\υ}}_{\mathrm{\φ}}\\ 0_{6\×1}\end{array}\right]+v_k---\left(71\right)$

υ_φ∈R³是与姿态相关的观测模型不确定性未知有界变量，具体为

${\mathrm{\υ}}_k={\left[\begin{array}{cc}{\mathrm{\υ}}_{\mathrm{\φ}}& 0_{1\×6}\end{array}\right]}^T,E\left[{\mathrm{\υ}}_{\mathrm{\φ}}{\mathrm{\υ}}_{\mathrm{\φ}}^T\right]\≤r_{k}I,r_k=\frac{1}{1+{\mathrm{\ϵ}}^{-1}}{w}_g$

观测向量为:z_k＝[Z_a Z_v Z_s]^T；其中姿态观测量Z_a＝[Z_ax Z_ay Z_az]^T，速度观测量Z_v＝[Z_vx Z_vy Z_vz]^T，位置观测量Z_s＝[Z_sx Z_sy Z_sz]^T；

4、临近空间飞行器传递对准模型不确定性的鲁棒滤波方法，其特征在于，所述步骤4)利用鲁棒稀疏网格求积分滤波经过初始采样、时间更新、重采样、量测更新和鲁棒更新过程对临近空间飞行器传递对准系统状态进行估计，并对子惯导系统进行误差修正，判断是否大于等于步长L，如果是，状态估计结束，完成传递对准过程，否则返回初始采样过程进行下一次估计，具体为：

4.1)初始采样

${\mathrm{\γ}}_{j,k-1}=A{\mathrm{\ξ}}_j+{\hat{x}}_{k-1};{\mathrm{AA}}^T={\mathrm{\Σ}}_{k-1|k-1},j=\mathrm{1,2},...,N_p---\left(72\right)$

4.2)时间更新

χ_j,k|k-1＝f(γ_j,k-1)   (73)

${\hat{x}}_{k|k-1}=\underset{j=1}{\overset{N_p}{\mathrm{\Σ}}}{\mathrm{\ω}}_j{\mathrm{\χ}}_{j,k|k-1}---\left(74\right)$

${\mathrm{\Σ}}_{k|k-1}=\underset{j=1}{\overset{N_p}{\mathrm{\Σ}}}{\mathrm{\ω}}_j({\mathrm{\χ}}_{j,k|k-1}-{\hat{x}}_{k|k-1}){({\mathrm{\χ}}_{j,k|k-1}-{\hat{x}}_{k|k-1})}^T+{\hat{Q}}_{k-1}^{*}---\left(75\right)$

其中

${\hat{Q}}_{k-1}^{*}=[(1+2\mathrm{\ϵ}){(1-{\mathrm{\γ}}^{-2})}^{-1}-1]\underset{j=1}{\overset{N_p}{\mathrm{\Σ}}}{\mathrm{\ω}}_j({\mathrm{\χ}}_{j,k|k-1}-{\hat{x}}_{k|k-1}){({\mathrm{\χ}}_{j,k|k-1}-{\hat{x}}_{k|k-1})}^T+(2+{\mathrm{\ϵ}}^{-1})q_k{\stackrel{\‾}{\mathrm{\Φ}}}_k{\stackrel{\text{-}}{\mathrm{\Φ}}}_k^T+(2+{\mathrm{\ϵ}}^{-1})G_{k|k-1}{Q}_{k-1}{G}_{k|k-1}^T---\left(76\right)$

4.3)重采样

${\widetilde{\mathrm{\γ}}}_j=A{\mathrm{\ξ}}_j+{\hat{x}}_{k|k-1};{\mathrm{AA}}^T={\text{\Σ}}_{k|k-1},j=\mathrm{1,2},...,N_p---\left(77\right)$

4.4)量测更新

${\hat{z}}_{k|k-1}=\underset{j=1}{\overset{N_p}{\mathrm{\Σ}}}{\mathrm{\ω}}_{j}h\left({\widetilde{\mathrm{\γ}}}_j\right)---\left(78\right)$

${\mathrm{\Σ}}_{\mathrm{zz},k|k-1}=\underset{j=1}{\overset{N_p}{\mathrm{\Σ}}}{\mathrm{\ω}}_j(h\left({\widetilde{\mathrm{\γ}}}_j\right)-{\hat{z}}_{k|k-1}){(h\left({\widetilde{\mathrm{\γ}}}_j\right)-{\hat{z}}_{k|k-1})}^T---\left(79\right)$

${\hat{R}}_k^{*}=(2+{\mathrm{\ϵ}}^{-1})r_k{\stackrel{\‾}{\mathrm{\Ψ}}}_k{\stackrel{\‾}{\mathrm{\Ψ}}}_k^T+(2+{\mathrm{\ϵ}}^{-1})R_k---\left(80\right)$

${\mathrm{\Σ}}_{\mathrm{xz},k|k-1}=\underset{j=1}{\overset{N_p}{\mathrm{\Σ}}}{\mathrm{\ω}}_j({\widetilde{\mathrm{\γ}}}_j-{\hat{x}}_{k|k-1}){(h\left({\widetilde{\mathrm{\γ}}}_j\right)-{\hat{z}}_{k|k-1})}^T---\left(81\right)$

$K_k=(1+2\mathrm{\ϵ}){(1-{\mathrm{\γ}}^{-2})}^{-1}{\mathrm{\Σ}}_{\mathrm{xz},k|k-1}{[(1+2\mathrm{\ϵ}){(1-{\mathrm{\γ}}^{-2})}^{-1}{\mathrm{\Σ}}_{\mathrm{zz},k|k-1}+{\hat{R}}_k^{*}]}^{-1}---\left(82\right)$

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})---\left(83\right)$

4.5)鲁棒更新

${\mathrm{\Σ}}_{k|k}=(1+2\mathrm{\ϵ}){(1-{\mathrm{\γ}}^{-2})}^{-1}[{\mathrm{\Σ}}_{k|k-1}-K_k{\mathrm{\Σ}}_{\mathrm{zz},k|k-1}{K}_k^T]+K_k{\hat{R}}_k^{*}{K}_k^T---\left(84\right)$

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (4)

1.一种临近空间飞行器传递对准模型不确定性的鲁棒滤波方法，其特征在于，该鲁棒滤波方法具体步骤如下：

步骤1)建立临近空间飞行器传递对准系统的数学平台失准角误差方程、速度误差方程、位置误差方程和观测方程；

步骤2)根据系统的误差方程建立模型不确定的状态方程和观测方程；

x_k＝f(x_k-1)+Φ(x_k-1)η_k+G_k|k-1w_k-1               (1) 

z_k＝h(x_k)+Ψ(x_k)υ_k+v_k               (2) 

式中：

x_k是n维状态向量,z_k是m维观测向量，f(·)和h(·)分别对应非线性状态方程和观测方程；G_k|k-1是n×r维系统过程噪声输入矩阵，w_k-1是r维系统过程噪声序列,v_k是m维系统观测噪声序列；Φ(·)∈R^n×n是系统状态方程中模型不确定性部分的有界输入矩阵，Ψ(·)∈R^m×m是系统观测方程中模型不确定性部分的有界输入矩阵；η_k∈Rⁿ是系统状态方程中模型不确定性未知有界变量，υ_k∈R^m是系统观测方程中模型不确定性未知有界变量；

步骤3)给出状态变量初始值(x₀)和预测误差方差矩阵初始值(Σ_0|0)，给出N_p个稀疏网格求积分点集(ξ_j，ε_j；j＝1,2，…N_p)；

状态变量初始值x₀＝[000 000 000 000 000 000]^T；

预测误差方差矩阵初始值：

系统过程噪声初始值

系统观测噪声初始值

式中：

φ_x0、φ_y0和φ_z0是初始数学平台失准角；

δV_x0、δV_y0和δV_z0是初始速度误差；δS_x0、δS_y0和δS_z0是初始位置误差；

ε_gx0、ε_gy0和ε_gz0是陀螺仪常值漂移初值；▽_ax0、▽_ay0和▽_az0是加速度计常值偏移初值；μ_x0、μ_y0和μ_z0是主子惯导间安装误差初值；

w_gx、w_gy和w_gz是陀螺仪随机噪声；w_ax、w_ay和w_az是加速度计随机噪声；

σ_ax、σ_ay和σ_az是姿态观测噪声标准差；σ_vx、σ_vy和σ_vz是速度观测噪声标准差；

σ_sx、σ_sy和σ_sz是位置观测噪声标准差；

根据稀疏网格求积分准则给定一组积分点集(ξ_j，ω_j)其中j＝1,2，…N_p，N_p表示积分 点集的个数；

其中Np＝2n²+2n+1,n为状态变量维数；

给定满足γ>1和ε>0的鲁棒滤波参数；具体为：第一组γ₁＝500，ε₁＝0.05：

第二组γ₁＝100，ε₁＝0.01：

步骤4)利用鲁棒稀疏网格求积分滤波经过初始采样、时间更新、重采样、量测更新和鲁棒更新过程对临近空间飞行器传递对准系统状态进行估计，并对子惯导系统进行误差修正，判断k+1是否大于等于步长L，如果是，状态估计结束，完成传递对准过程，否则返回初始采样过程进行下一次估计。

2.根据权利要求1所述临近空间飞行器传递对准模型不确定性的鲁棒滤波方法，其特征在于，所述步骤1)系统数学平台失准角误差方程、速度误差方程、位置误差方程和观测方程，具体为：

1.1)数学平台失准角误差方程

其中：

i是发射点惯性坐标系，此处也是导航坐标系；

是惯导解算的发射点惯性坐标系，即数学平台坐标系；i系依次经过三次变换可得数学平台坐标系系，三次转动角分别为：绕z轴旋转φ_z、绕y轴旋转φ_y和绕x轴旋转φ_x；

发射点惯性坐标系下数学平台失准角φⁱ＝[φ_x φ_y φ_z]^T；

b是弹体坐标系，即子惯导坐标系，也可以用bs表示；原点O_b为弹体质心，O_bx_b轴为弹体纵轴对称轴，指向弹体头部；O_by_b轴在弹体纵向对称面内，并垂直于纵轴O_bz_b向上；O_bz_b按照右手规则确定；

为子惯导解算的姿态矩阵，也可以用表示，表示弹体坐标系b到数学平台坐标系的姿态转换矩阵；

为陀螺测量误差，且ε_g＝[ε_gx ε_gy ε_gz]^T为三只陀螺仪测量误差的随机常值部分，w_g＝[w_gx w_gy w_gz]^T为三只陀螺仪测量误差的随机噪声部分，随机噪声均假设为高斯白噪声；

1.2)速度误差方程

惯性坐标系下速度误差微分方程为，

式中：

δVⁱ＝[δV_x δV_y δV_z]^T；

是子惯导IMU的加速度计测量值；

是子惯导IMU的加速度计测量误差，且▽_a＝[▽_ax ▽_ay ▽_az]^T为加速度计测量误差的随机常值部分，w_a＝[w_ax w_ay w_gz]^T为三只加速度计测量误差的随机噪声部分，随机噪声均假设为高斯白噪声；

是系至i系的变换阵；

δgⁱ是引力场模型的引力加速度误差；

1.3)位置误差方程

发射点惯性坐标系下位置误差δSⁱ微分方程为，

式中：δSⁱ＝[δS_x δS_y δS_z]^T；

1.4)姿态匹配观测方程

式中：

bm是载体坐标系即载机坐标系；

bs是子惯导坐标系；

是主惯导的姿态阵，是的转置矩阵；

—主惯导的姿态误差角，可将其视为白噪声；

是bm系到bs系的变换矩阵；

姿态匹配观测方程由观测矩阵Z_dcm矩阵求得，具体为：

式中：Z_am是姿态匹配的观测量，

1.5)速度匹配观测方程

Z_v＝H_vx_k+v_v               (8) 

式中：

v_v是速度匹配等效观测噪声；

1.6)位置匹配观测方程

Z_s＝H_sx_k+v_s               (9) 

式中：

。

3.根据权利要求1所述临近空间飞行器传递对准模型不确定性的鲁棒滤波方法，其特征在于，所述步骤2)模型不确定的状态方程和观测方程具体为：

2.1)x_k具x_k＝[φ_x φ_y φ_z δV_x δV_y δV_z δS_x δS_y δS_z ε_gx ε_gy ε_gz ▽_ax ▽_ay ▽_az μ_x μ_y μ_z]^T

可简写为

包括导航系为发射点惯性坐标系下失准角φⁱ＝[φ_x φ_y φ_z]^T、速度误差δVⁱ＝[δV_x δV_y δV_z]^T、位置误差δSⁱ＝[δS_x δS_y δS_z]^T、陀螺仪随机常值误差ε_g ⁱ＝[ε_gx ε_gy ε_gz]^T、加速度计随机常值误差▽_a ⁱ＝[▽_ax ▽_ay ▽_az]^T、安装误差μⁱ＝[μ_x μ_y μ_z]^T；系统噪声向量为：w_k＝[ε_gx ε_gy ε_gz ▽_ax ▽_ay ▽_az]^T；

2.2)系统状态方程中模型不确定性部分的有界输入矩阵Φ(·)具体为

η_k满足如下关系：

式中：η_k＝[η_φ 0_1×12]^T,η_φ∈R⁶,

2.3)模型不确定的状态方程具体为：

2.4)系统观测方程中模型不确定性部分的有界输入矩阵Ψ(·)具体为：

Ψ_φ是姿态匹配模型不确定部分，主要由初始安装误差确定，具体为：

2.5)模型不确定的观测方程具体为:

υ_φ∈R³是与姿态相关的观测模型不确定性未知有界变量，具体为

υ_k＝[υ_φ 0_1×6]^T,

观测向量为:z_k＝[Z_a Z_v Z_s]^T；其中姿态观测量Z_a＝[Z_ax Z_ay Z_az]^T，速度观测量Z_v＝[Z_vx Z_vy Z_vz]^T，位置观测量Z_s＝[Z_sx Z_sy Z_sz]^T。

4.根据权利要求1所述临近空间飞行器传递对准模型不确定性的鲁棒滤波方法，其特征在于，所述步骤4)利用鲁棒稀疏网格求积分滤波经过初始采样、时间更新、重采样、量测更新和鲁棒更新过程对临近空间飞行器传递对准系统状态进行估计，并对子惯导系统进行误差修正，判断k+1是否大于等于步长L，如果是，状态估计结束，完成传递对准过程,具体为：

4.1)初始采样

4.2)时间更新

χ_j,k|k-1＝f(γ_j,k-1)               (17) 

其中

4.3)重采样

4.4)量测更新

4.5)鲁棒更新

判断k+1是否大于等于步长L，如果是，状态估计完成，结束计算，否则返回步骤4.1)进行下一次估计；

式中：

Σ_k-1|k-1为预测误差方差矩阵；为状态变量；Σ_k|k-1为一步预测误差方差矩阵；A为Σ_k-1|k-1或Σ_k|k-1通过Cholupdate或SVD分解得到的矩阵；ξ_j为稀疏网格求积分点集采样点；ω_j为稀疏网格求积分点集采样点对应的权重；γ_j，k-1为初始采样点；N_p为积分点集个数；χ_j,k|k-1为时间更新的Sigma点；为时间更新的状态变量一步预测值；R_k为系统观测噪声矩阵；为计算系统观测噪声矩阵；Q_k-1为系统过程噪声矩阵；G_k|k-1为系统过程噪声输入矩阵；为计算系统过程噪声矩阵；为重采样的Sigma点；为观测一步预测值；Σ_zz,k|k-1为观测一步预测误差方差矩阵；Σ_xz,k|k-1为协误差方差矩阵；K_k为滤波增益矩阵；为状态估计值；Σ_k|k为估计误差方差矩阵。