[发明专利]一种强机动条件下的弹载深组合ARCKF滤波方法

权利要求书

1.一种强机动条件下的弹载深组合ARCKF滤波方法，其特征在于，包括以下步骤：

步骤1、通过轨迹发生器生成模拟强机动导弹的弹道轨迹和相应IMU数据；

步骤2、将生成的弹道轨迹注入卫星信号模拟器，生成GNSS中频数据；

步骤3、将生成的GNSS中频数据注入给接收机进行导航解算、IMU数据进行惯导解算；

步骤4、建立发射惯性坐标系下GNSS/SINS深组合导航系统的状态方程和观测方程；

步骤5、将抗差估计理论中的抗差M估计算法和自适应因子结合到容积卡尔曼滤波即CKF算法中，得出一种自适应抗差容积卡尔曼滤波即ARCKF算法，对系统状态进行滤波校正。

2.根据权利要求1所述的强机动条件下的弹载深组合ARCKF滤波方法，其特征在于，步骤1中所述通过轨迹发生器生成模拟强机动导弹的弹道轨迹和相应IMU数据，具体如下：

根据需要，建立动力学模型和发射惯性坐标系下的轨迹参数，包括弹道导弹的初始位置、姿态角、加速度、姿态的变化规律，然后生成弹道轨迹和相应的IMU数据。

3.根据权利要求1所述的强机动条件下的弹载深组合ARCKF滤波方法，其特征在于，步骤4中所述建立发射惯性坐标系下GNSS/SINS深组合导航系统的状态方程和观测方程，具体如下：

(4.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子系统的状态误差量；X_g为选取的GNSS的状态误差量，具体如下：

其中，为系统的姿态失准角；δ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时钟频率误差白噪声；为弹体坐标系转换到导航坐标系的转换矩阵；I是单位矩阵；G_e如下式：

$G_e=\left[\begin{array}{ccc}{G}_r^{\′}+(X+R_{0x})\frac{\∂G_r^{\′}}{\∂X}& (X+R_{0x})\frac{\∂G_r^{\′}}{\∂Y}& (X+R_{0x})\frac{\∂G_r^{\′}}{\∂Z}\\ (Y+R_{0y})\frac{\∂G_r^{\′}}{\∂X}& G_r^{\′}+(Y+R_{0y})\frac{\∂G_r^{\′}}{\∂Y}& (Y+R_{0y})\frac{\∂G_r^{\′}}{\∂Z}\\ (Z+R_{0z})\frac{\∂G_r^{\′}}{\∂X}& (Z+R_{0z})\frac{\∂G_r^{\′}}{\∂Y}& G_r^{\′}+(Z+R_{0z})\frac{\∂G_r^{\′}}{\∂Z}\end{array}\right]$

其中，G_r′观测点到地心的矢量；X、Y、Z依次为载体在发射惯性系下的三轴坐标；R_0x、R_0y、R_0z为发射点地心矢量在发射惯性坐标系内的投影分量；分别为G_r′对X、Y、Z三个方向求偏导；

矩阵F₁如下式：

$F_1=\left[\begin{array}{ccc}0& f_z^n& -f_y^n\\ -f_z^n& 0& f_x^n\\ f_y^n& -f_x^n& 0\end{array}\right]$

其中，依次为加速度计在发射惯性坐标系下的X、Y、Z三轴的比力值；

系统的噪声驱动矩阵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三轴下的高斯白噪声；

(4.2)系统观测方程

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

$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为GNSS接收机接收到的卫星数目；ρ_I为惯导解算的位置信息计算所得伪距，ρ_G为GNSS接收机测量所得伪距；H(·)为非线性量测函数；v(t)为各元素为零均值的高斯白噪声。

4.根据权利要求1所述的强机动条件下的弹载深组合ARCKF滤波方法，其特征在于，步骤5中所述将抗差估计理论中的抗差M估计算法和自适应因子结合到容积卡尔曼滤波即CKF算法中，得出一种自适应抗差容积卡尔曼滤波即ARCKF算法，对系统状态进行滤波校正，具体如下：

根据步骤4所建立的模型，进行离散化，得到下式：

$\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为k时刻的系统状态向量；X_k-1为k-1时刻的系统状态向量；F_k为k时刻的状态转移矩阵；Z_k为观测向量，H(·)为非线性量测传递函数；w_k为系统噪声序列，且满足下列关系：其中，Q_k为系统噪声序列的协方差阵，是对称的非负定矩阵；v_k量测噪声序列；

ARCKF算法流程如下：

(5.1)滤波初始化

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

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

其中，x₀为系统状态初值；为x₀的均值；P₀为x₀的状态误差协方差阵；上标T为对该矩阵或向量转置；E(·)为求数学期望；

(5.2)时间更新，包括以下内容：

计算k-1时刻Cubature点：

$P_{k-1|k-1}=S_{k-1}{S}_{k-1}^T$

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

其中，P_k-1|k-1为k-1时刻的系统状态误差协方差；S_k-1通过P_k-1|k-1的Cholesky的分解得到；为k-1时刻的系统状态向量；是第i个状态向量采样点；i＝1,2,…,2n；

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

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

$P_{k|k-1}=\frac{1}{2n}\underset{1}{\overset{2n}{\Σ}}{\widetilde{\mathrm{\χ}}}_{k|k-1}^{*\left(i\right)}{\left({\widetilde{\mathrm{\χ}}}_{k|k-1}^{*\left(i\right)}\right)}^T-{\hat{x}}_{k|k-1}{x}_{k|k-1}^T+Q_{k-1}$

其中，f(·)为系统状态转移非线性函数；为第i个状态采样点经过状态转移所得容积点值；为一步预测状态向量值；Q_k-1为系统噪声协方差阵；P_k|k-1为一步预测状态的误差协方差阵；

(5.3)量测更新，包括以下内容：

计算用于量测更新的Cubature点：

$P_{k|k-1}=S_{k|k-1}{S}_{k|k-1}^T$

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

其中，S_k|k-1为通过(5.2)中所得P_k|k-1的Cholesky分解得到；作为量测更新使用的容积即Cubature点；

通过量测方程传播Cubature点：

$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{2n}{\Σ}}\frac{1}{2n}{z}_{k|k-1}^{\left(i\right)}$

$P_{yy}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}\[z_{k|k-1}^{\left(i\right)}-{\hat{z}}_{k|k-1}\]{\[z_{k|k-1}^{\left(i\right)}-{\hat{z}}_{k|k-1}\]}^T+R_k$

其中，h(·)非线性量测转移函数；为第i个量测预测采样点；为2n个量测采样点加权平均所得量测估计值；R_k为k时刻的量测噪声协方差阵；P_yy为量测值的误差协方差阵；

(5.3.1)抗差修正R，具体内容如下：

${\stackrel{\‾}{P}}_{yy}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}\[z_{k|k-1}^{\left(i\right)}-{\hat{z}}_{k|k-1}\]{\[z_{k|k-1}^{\left(i\right)}-{\hat{z}}_{k|k-1}\]}^T+{\stackrel{\‾}{R}}_k$

其中，为抗差修正后的量测值误差协方差阵；为与R_k等价的量测噪声协方差阵，由抗差M估计方法中的等价权矩阵求逆获得，即

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

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

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

其中，分别为等价权矩阵的对角元素与非对角元素；σ_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矩阵的i行i列元素；z_k为真实量测值；为上文求得的量测估计值；v_i为观测残差向量v的第i个元素；

(5.3.2)自适应因子调节，具体内容如下：

动力学模型误差会整体性地破坏参数估计的效果，因此采用一个自适应因子对系统模型进行整体修正，具体算法为：用自适应因子α_k修正P_k|k-1，等价于修正Cubature点采样过程，结果为：

${\mathrm{\χ}}_{k|k-1}^{\left(i\right)}={\hat{x}}_{k|k-1}+\sqrt{{\mathrm{\α}}_k^{-1}}{S}_{k|k-1}{\mathrm{\ξ}}_i$

其中，I为n×n单位矩阵；S_k|k-1通过P_k|k-1的Cholesky的分解得到的乔里斯基因子；为一步预测状态向量；为第i个状态向量采样点；为自适应因子倒数的平方根；

自适应因子α_k的确定方法如下：

${\mathrm{\α}}_k=\left\{\begin{array}{cc}1,& tr\left({\hat{P}}_{yy,k|k-1}\right)\≤tr\left({\stackrel{\‾}{P}}_{yy,k|k-1}\right)\\ \frac{tr({\stackrel{\‾}{P}}_{yy,k|k-1}-{\stackrel{\‾}{R}}_k)}{tr({\hat{P}}_{yy,k|k-1}-{\stackrel{\‾}{R}}_k)},& tr\left({\hat{P}}_{yy,k|k-1}\right)\>tr\left({\stackrel{\‾}{P}}_{yy,k|k-1}\right)\end{array}\right.$

其中，为抗差修正后的量测噪声协方差阵；为(5.3.1)抗差估计修正后的量测误差协方差阵，tr(·)为求矩阵的迹；由下式得出：

${\hat{P}}_{yy,k|k-1}=v\·v^T$

其中，v为量测残差向量；

(5.3.3)抗差自适应后的量测更新，具体内容如下：

通过量测方程传播Cubature点：

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

其中，为第i个状态向量采样点；h(·)为非线性量测传播函数；为经过量测函数传播得到的第i个量测值；

计算增益矩阵：

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

${\stackrel{\‾}{P}}_{yy}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}\[z_{k|k-1}^{\left(i\right)}-{\hat{z}}_{k|k-1}\]{\[z_{k|k-1}^{\left(i\right)}-{\hat{z}}_{k|k-1}\]}^T+{\stackrel{\‾}{R}}_k$

$P_{xy}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}\[{\mathrm{\χ}}_{k|k-1}^{\left(i\right)}-{\hat{x}}_{k|k-1}\]{\[z_{k|k-1}^{\left(i\right)}-{\hat{z}}_{k|k-1}\]}^T$

$K_k=P_{xy}{\stackrel{\‾}{P}}_{yy}^{-1}$

其中，为量测估计值；为抗差修正后的量测噪声协方差阵；为量测值误差协方差阵；为矩阵的求逆；P_xy为状态误差与量测误差的互协方差阵；K_k为卡尔曼滤波增益；

状态估计值：

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

$P_k=P_{k|k-1}-K_k{\stackrel{\‾}{P}}_{yy}{K}_k^T$

其中，为k时刻滤波状态更新值；P_k为k时刻状态误差协方差阵更新值；为K_k矩阵的转置矩阵；

由ARCKF滤波所估计的状态误差值对SINS系统的状态量进行反馈矫正，完成惯性/卫星系统组合导航。