[发明专利]一种基于鲁棒SCKF滤波的紧组合导航方法

权利要求书

1.一种基于鲁棒SCKF滤波的紧组合导航方法，其特征在于，包括以下步骤：

步骤1、通过轨迹发生器、卫星信号模拟器依次模拟生成导弹的IMU数据和GPS中频信号；

步骤2、将模拟生成的IMU数据进行惯导解算，将GPS中频信号注入接收机进行导航解算；

步骤3、建立发射惯性坐标系下GPS/SINS紧组合导航系统模型；

步骤4、在标准平方根容积卡尔曼滤波即SCKF的基础上，引入稳健M，对系统观测噪声阵进行自适应调节，构成鲁棒平方根容积卡尔曼滤波即RSCKF算法，对系统状态进行滤波校正。

2.根据权利要求1中所述的基于鲁棒SCKF滤波的紧组合导航方法，其特征在于，步骤1中所述通过轨迹发生器、卫星信号模拟器依次模拟生成导弹的IMU数据和GPS中频信号，具体为：

根据弹道导弹飞行的物理模型，设置飞行各阶段的参数，生成导弹的飞行弹道轨迹和相应的IMU数据，将飞行弹道轨迹导入卫星模拟器处理，得到相应的GPS中频信号。

3.根据权利要求1中所述的基于鲁棒SCKF滤波的紧组合导航方法，其特征在于，步骤2中所述将模拟生成的IMU数据进行惯导解算，将GPS中频信号注入接收机进行导航解算，具体为：

惯导解算更新弹体的位置、速度、姿态信息；接收机导航解算得到弹体位置、卫星位置、伪距信息。

4.根据权利要求1中所述的基于鲁棒SCKF滤波的紧组合导航方法，其特征在于，步骤3中所述建立发射惯性坐标系下GPS/SINS紧组合导航系统模型，具体步骤如下：

(3.1)状态方程：

$\left[\begin{array}{c}{\stackrel{\·}{X}}_s\left(t\right)\\ {\stackrel{\·}{X}}_g\left(t\right)\end{array}\right]=\left[\begin{array}{cc}{F}_s\left(t\right)& 0\\ 0& F_g\left(t\right)\end{array}\right]\left[\begin{array}{c}{X}_s\left(t\right)\\ X_g\left(t\right)\end{array}\right]+\left[\begin{array}{cc}{G}_s\left(t\right)& 0\\ 0& G_g\left(t\right)\end{array}\right]\left[\begin{array}{c}{w}_s\left(t\right)\\ w_g\left(t\right)\end{array}\right]$

其中，X_s包含SINS的15个状态误差量；X_g包含GPS的2个状态误差量，具体如下：

其中，为系统的姿态失准角；δV_x、δV_y、δV_z为发射惯性坐标系下三轴方向的速度误差；δX、δY、δZ为发射惯性坐标系下三轴方向的位置误差；ε_x、ε_y、ε_z和▽_x、▽_y、▽_z分别为陀螺常值漂移、加速度计常值偏置在三轴方向的分量，Δl_u为等效钟差的距离误差，Δl_ru为与钟漂等效的距离率误差；

w_g(t)＝[w_u w_ru]^T

$F_s\left(t\right)={\left[\begin{array}{ccccc}{0}_{3\×3}& 0_{3\×3}& 0_{3\×3}& C_b^n& 0_{3\×3}\\ F_1& 0_{3\×3}& G_e& 0_{3\×3}& C_b^n\\ 0_{3\×3}& I& & 0_{3\×9}& \\ & & 0_{6\×15}& & \end{array}\right]}_{15\×15}$

其中，T_ru为GNSS时钟频率漂移的相关时间，w_u为GNSS时钟误差白噪声；w_ru为GNSS时钟频率误差白噪声；F₁为X、Y、Z三轴比力的反对称矩阵；G_e为观测点到地下矢量的三轴偏导矩阵；为弹体坐标系转换到导航坐标系的转换矩阵；I是单位矩阵；系统的噪声驱动矩阵G_s(t)为：

$G_s\left(t\right)={\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{9\×3}& 0_{9\×3}\end{array}\right]}_{15\×6}$

系统的噪声向量w_s(t)为：

w_s(t)＝[ω_gx ω_gy ω_gz ω_ax ω_ay ω_az]^T

其中，w_gx、w_gy、w_gz分别为陀螺仪在X、Y、Z三轴下的高斯白噪声；w_ax、w_ay、w_az分别为加速度计在X、Y、Z三轴下的高斯白噪声；

(3.2)系统观测方程

发射惯性坐标系下，弹道导弹GPS/SINS深组合导航系统的观测方程分为伪距差和伪距率差两个部分，具体为：

$Z={\left[\begin{array}{c}\mathrm{\δ}\mathrm{\ρ}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\ρ}}\end{array}\right]}_{2m\×1}={\left[\begin{array}{c}{\mathrm{\ρ}}_I-{\mathrm{\ρ}}_G\\ {\stackrel{\·}{\mathrm{\ρ}}}_I-{\stackrel{\·}{\mathrm{\ρ}}}_G\end{array}\right]}_{2m\×1}=H\left(X\right(t\left)\right)+v\left(t\right)$

其中：m表示GPS接收机接收到的卫星数目；ρ_I为惯导解算的位置信息计算所得伪距，ρ_G为GPS接收机测量所得伪距；H(·)为非线性量测函数；v(t)为各元素为零均值的高斯白噪声。

5.根据权利要求1中所述的基于鲁棒SCKF滤波的紧组合导航方法，其特征在于，步骤4中所述在标准平方根容积卡尔曼滤波即SCKF的基础上，引入稳健M估计，对系统观测噪声阵进行自适应调节，构成鲁棒平方根容积卡尔曼滤波即RSCKF算法，对系统状态进行滤波校正，具体步骤如下：

首先将步骤3中建立的系统模型进行离散化，得到下式：

$\left\{\begin{array}{c}{X}_k=F_k{X}_{k-1}+w_k\\ Z_k=h\left(X_k\right)+v_k\end{array}\right.$

式中：X_k∈Rⁿ,Z_k∈R^m分别为k时刻系统的状态向量和量测向量；F_k为线性状态转移矩阵，h(·)为系统非线性量测函数；w_k、v_k为互不相关的零均值高斯白噪声序列，统计特性满足如下条件：

$\left\{\begin{array}{cc}e\[w_k\]=0& \cos (w_k,w_j)=Q_k{\mathrm{\δ}}_{kj}\\ e\[v_k\]=0& \cos (v_k,v_j)=R_k{\mathrm{\δ}}_{kj}\\ \cos (w_k,v_k)=0& \end{array}\right.$

式中：Q_k为非负定矩阵，R_k为正定矩阵；δ_kj为Kronecker-δ函数；

要实现SCKF算法，首先按照三阶容积准则，选取一组2n个等权值分布的容积点{ω_i，ξ_i}实现非线性逼近，其中：

${\mathrm{\ω}}_i=\frac{1}{m},i=1,2,...,m;m=2n$

${\mathrm{\ξ}}_i=\sqrt{\frac{m}{2}{\[l\]}_i}$

其中：ξ_i为容积点向量；ω_i为对应权重；n为系统状态变量的维数；[l]∈Rⁿ，为生成算子，当n＝2时，表示为如下所示点集：

$\left\{\left(\begin{array}{c}1\\ 0\end{array}\right),\left(\begin{array}{c}0\\ 1\end{array}\right),\left(\begin{array}{c}-1\\ 0\end{array}\right),\left(\begin{array}{c}0\\ -1\end{array}\right)\right\}$

[l]_i是[l]∈R²中第i列元素；SCKF算法具体步骤如下：

1)滤波初始化

${\hat{x}}_0={\mathrm{Ex}}_0$

$P_0=E\[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\]$

S₀＝chol(P₀)

其中，x₀为系统状态初值；为x₀的均值；P₀为x₀的状态误差协方差阵；上标T为对该矩阵或向量转置，以下同；E(·)为求数学期望；chol(·)表示乔里斯基分解；S₀为P₀的乔里斯基因子；

2)时间更新：

鲁棒平方根容积卡尔曼滤波的时间更新标准平方根卡尔曼滤波一致；

${\widetilde{\mathrm{\χ}}}_{k-1}^{\left(i\right)}={\hat{x}}_{k-1}+S_{k-1}{\mathrm{\ξ}}_i$

${\widetilde{\mathrm{\χ}}}_{k|k-1}^{*\left(i\right)}=f({\widetilde{\mathrm{\χ}}}_{k-1}^{\left(i\right)},u_{k-1})$

${\hat{x}}_{k|k-1}=\underset{1}{\overset{m}{\Σ}}\frac{1}{m}{\widetilde{\mathrm{\χ}}}_{k|k-1}^{*\left(i\right)}$

$S_{k|k-1}=Tria(\[{\mathrm{\χ}}_{k|k-1}^{*},S_{Q,k-1}\])$

其中，S_k-1为k-1时刻的平方根因子；为k-1时刻的状态向量更新值；为第i个状态容积采样点；u_k-1为系统输入量；f(·)为非线性状态转移函数；为第i个一步预测状态采样值；为加权平均得到的系统状态一步预测值；Tria(·)表示QR分解；S_k|k-1为估计的k时刻平方根因子；矩阵表示为：

${\mathrm{\χ}}_{k|k-1}^{*}=\frac{1}{\sqrt{m}}\left[\begin{array}{cccc}{\widetilde{\mathrm{\χ}}}_{k|k-1}^{*\left(1\right)}-{\hat{x}}_{k|k-1}& {\widetilde{\mathrm{\χ}}}_{k|k-1}^{*\left(2\right)}-{\hat{x}}_{k|k-1}& \mathrm{...}& {\widetilde{\mathrm{\χ}}}_{k|k-1}^{*\left(m\right)}-{\hat{x}}_{k|k-1}\end{array}\right]$

3)量测更新：

与标准SCKF量测更新过程一致，仅对噪声矩阵R做出改动：

${\widetilde{\mathrm{\χ}}}_{k|k-1}^{\left(i\right)}={\hat{x}}_{k|k-1}+S_{k|k-1}{\mathrm{\ξ}}_i$

$Z_{k|k-1}^{\left(i\right)}=h\left({\widetilde{\mathrm{\χ}}}_{k|k-1}^{\left(i\right)}\right)$

${\hat{z}}_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}\frac{1}{m}{Z}_{k|k-1}^{\left(i\right)}$

$S_{zz,k|k-1}=Tria\left(\left[\begin{array}{cc}{\mathrm{\ζ}}_{k|k-1}& {\stackrel{\‾}{S}}_{R,k}\end{array}\right]\right)$

其中，为量测更新的第i个容积采样点；h(·)为非线性量测函数；为第i个量测值采样点；为量测值采样点加权平均所得的量测估计值；为根据稳健M估计算法对噪声R进行自适应调节后的观测噪声矩阵；矩阵ζ_k|k-1表示为：

${\mathrm{\ζ}}_{k|k-1}=\frac{1}{\sqrt{m}}\left[\begin{array}{cccc}{Z}_{k|k-1}^{\left(1\right)}-{\hat{z}}_{k|k-1}& Z_{k|k-1}^{\left(2\right)}-{\hat{z}}_{k|k-1}& \mathrm{...}& Z_{k|k-1}^{\left(m\right)}-{\hat{z}}_{k|k-1}\end{array}\right]$

估计互协方差矩阵为：

$P_{xz,k|k-1}={\mathrm{\χ}}_{k|k-1}\·{\mathrm{\ζ}}_{k|k-1}^T$

其中，矩阵χ_k|k-1表示为：

${\mathrm{\χ}}_{k|k-1}=\frac{1}{\sqrt{m}}\left[\begin{array}{cccc}{\widetilde{\mathrm{\χ}}}_{k|k-1}^{\left(1\right)}-{\hat{x}}_{k|k-1}& {\widetilde{\mathrm{\χ}}}_{k|k-1}^{\left(2\right)}-{\hat{x}}_{k|k-1}& \mathrm{...}& {\widetilde{\mathrm{\χ}}}_{k|k-1}^{\left(m\right)}-{\hat{x}}_{k|k-1}\end{array}\right]$

$K_k=(P_{xz,|k-1}/S_{zz,k|k-1}^T)/S_{zz,k|k-1}$

${\hat{x}}_k={\hat{x}}_{k|k-1}+K_k\[z_k-{\hat{z}}_{k|k-1}\]$

$S_k=Tria(\[{\mathrm{\χ}}_{k|k-1}-K_k\·{\mathrm{\ζ}}_{k|k-1},K_k\·S_{R,k}\])$

其中，K_k为所求得k时刻的卡尔曼滤波增益；S_k为为经过QR分解的平方根因子；

4)抗差修正R，具体步骤如下：

基于标准SCKF方程，建立抗差SCKF的滤波模型；由于量测信息只影响模型中的量测更新过程，所以鲁棒SCKF算法仅对量测更新方程进行了调整修正，抗差噪声阵是与R_k等价的量测噪声方差阵，由抗差M估计方法中的等价权矩阵求逆获得，即

${\stackrel{\‾}{R}}_k={\stackrel{\‾}{P}}^{-1}$

此处采用Huber法求取等价权矩阵；设的矩阵元素为i,j＝1,2,…,n，则有如下方法确定等价权矩阵：

$\begin{array}{cc}{\stackrel{\‾}{p}}_{t\left(ii\right)}=\left\{\begin{array}{cc}\frac{1}{{\mathrm{\σ}}_{ii}},& \left|\frac{v_i}{{\mathrm{\σ}}_{v_i}}\right|=\left|{\stackrel{\‾}{v}}_i\right|\≤k\\ \frac{k}{{\mathrm{\σ}}_{ii}\left|{\stackrel{\‾}{v}}_i\right|},& \left|{\stackrel{\‾}{v}}_i\right|\>k\end{array}\right.& {\stackrel{\‾}{p}}_{t\left(ij\right)}=\left\{\begin{array}{cc}\frac{1}{{\mathrm{\σ}}_{ij}},& \left|{\stackrel{\‾}{v}}_i\right|\≤k,and\left|{\stackrel{\‾}{v}}_j\right|\≤k\\ \frac{k}{{\mathrm{\σ}}_{ij}\·\max \{\left|{\stackrel{\‾}{v}}_i\right|,\left|{\stackrel{\‾}{v}}_j\right|\}},& \left|{\stackrel{\‾}{v}}_i\right|\>k,or\left|{\stackrel{\‾}{v}}_j\right|\>k\end{array}\right.\end{array}$

其中，分别为等价权矩阵的对角元素与非对角元素；σ_ii,σ_ij为原R_k阵的对角元素和非对角元素；k为常数，取1.2～1.5；v_i为观测量z_i的残差分量，为标准残差分量，由得出，其中：

${\mathrm{\σ}}_{v_i}={\left(P_{yy,k|k-1}\right)}_{ii}$

$v_i={(z_k-{\hat{z}}_{k|k-1})}_i$

其中，(P_yy,k|k-1)_ii为取P_yy,k|k-1矩阵的i行i列元素；z_k为真实量测值；为求得的量测估计值；v_i为观测残差向量v的第i个元素；

将抗差修正得到的代入步骤3)中量测更新，即得到鲁棒平方根容积卡尔曼滤波；按照以上步骤，进行滤波后，得到系统中位置、速度、姿态状态量的误差量，再对组合导航的SINS状态进行校正，输出最终的导航结果。