一种强机动条件下的弹载深组合ARCKF滤波方法与流程

本发明涉及组合导航技术领域，特别是一种强机动条件下的弹载深组合ARCKF滤波方法。

二、

背景技术：

强机动、高动态条件下对载体进行导航定位是一个热点和难点问题，在军事应用中的地位尤其重要，引起国内外众多学者的广泛关注。强机动、高动态条件下的导航定位难点主要有以下几点：惯性导航系统解算误差变大，对定位技术也提出了很高的要求；系统观测传递函数表现为强非线性，线性化误差导致系统发散，因此对数据融合技术的鲁棒性与可靠性提出了更高的要求。

传统组合导航中的非线性滤波一般采取扩展卡尔曼滤波EKF、无迹卡尔曼滤波UKF。EKF需要求导计算雅克比(Jacobian)阵，计算量大，线性化后具有较大的截断误差；UKF缺乏严格的数学推导过程，且当系统维数过高时，UKF的Sigma点权值容易出现负值情况，导致其矩阵积分中引入很大的截断误差，滤波精度下降。近年来提出的容积卡尔曼滤波CKF，有更严格的理论推导，在高维度系统的滤波性能较UKF更优。但在强机动、高动态等复杂环境中，有可能给载体带来观测异常以及由动力学模型不准确导致的误差，组合导航滤波性能还需进一步提高。

三、

技术实现要素：

本发明的目的在于提供一种精确、稳定的强机动条件下的弹载深组合ARCKF滤波方法。

实现本发明目的的技术解决方案为：一种强机动条件下的弹载深组合ARCKF滤波方法，包括以下步骤：

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

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

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

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

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

本发明与现有技术相比，其显著优点是：(1)采用非线性滤波CKF，更能符合组合导航模型的非线性特性；(2)与EKF、UKF滤波相比，具有估计精度更高，不易发散且计算量少的优点；(3)采用的自适应抗差容积卡尔曼滤波(ARCKF)，更能抵御在观测异常扰动与动力学模型不准确所导致的误差。

四、附图说明

图1是本发明强机动条件下的弹载深组合ARCKF滤波方法的流程图。

图2是实施例1中模拟生成的发射惯性坐标系下的真实弹道轨迹。

图3是实施例1中ARCKF与KF滤波在X方向的速度误差对比结果。

图4是实施例1中ARCKF与KF滤波在Y方向的速度误差对比结果。

图5是实施例1中ARCKF与KF滤波在Z方向的速度误差对比结果。

五、具体实施方式

下面结合附图及具体实施例对本发明作进一步详细说明。

结合图1，本发明强机动条件下的弹载深组合ARCKF滤波方法，步骤如下：

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

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

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

步骤3、将步骤2中得到的GNSS中频数据注入给软件接收机进行捕获跟踪和导航解算、同时IMU数据进行惯导解算。

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

(4.1)状态方程为：

其中，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为与钟漂等效的距离率误差；

其中，T_ru为GNSS时钟频率漂移的相关时间，w_u为GNSS时钟误差白噪声；w_ru为GNSS时钟频率误差白噪声；为弹体坐标系转换到导航坐标系的转换矩阵；I是单位矩阵；G_e如下式：

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

矩阵F₁如下式：

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

系统的噪声驱动矩阵G_s(t)为：

其中，为坐标转换矩阵。

系统的噪声矩阵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深组合导航系统的观测方程分为伪距差和伪距率差两个部分，具体为：

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

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

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

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

ARCKF算法流程如下：

(5.1)滤波初始化

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

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

计算k-1时刻Cubature点：

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

i＝1,2,…,2n。

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

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

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

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

通过量测方程传播Cubature点：

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

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

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

此处采用Huber法求取等价权矩阵；设的矩阵元素为则可有如下方法确定等价权矩阵：

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

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

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

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

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

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

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

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

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

通过量测方程传播Cubature点：

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

计算增益矩阵：

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

状态估计值：

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

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

实施例1

为了对本发明方法进行说明，充分展现出该方法的可靠性与鲁棒性，体现该滤波方法在强机动、高动态情况下的滤波性能与效果，进行如下仿真试验。将提出的自适应抗差容积卡尔曼滤波(ARCKF)方法应用于强机动、高动态情况下弹道导弹的GNSS/SINS深组合导航进行仿真校验，并且与普通卡尔曼滤波(KF)算法进行比较，仿真条件如下：

本方法发射惯性坐标系下GNSS/SINS组合导航系统中，取状态方程为伪距、伪距率组合的线性系统方程，取观测方程为伪距、伪距率组合的非线性观测方程。

(1)仿真条件设置如下：

弹体初始俯仰角为90°，初始滚转角和偏航角均为0°；初始姿态误差均设为0°；初始位置为：纬度31.98°，经度118.8°，高度为0m；发射惯性坐标系下初始速度前向为394.8917m/s(地球自转速度)，天向和侧向均为0m/s；发射方位角为90°；60s～68s间最大加速度达到60g，完全符合高动态特性。

发射惯性坐标系下超紧组合导航仿真系统的仿真参数设置如下表1所示。

表1 仿真参数设置

(2)仿真结果与分析：

仿真结果如下：图2为模拟生成的发射惯性坐标系下的真实弹道轨迹；由于速度更能反映出系统强机动、高动态特性，图3～5分别给出ARCKF、KF在三个方向的速度误差对比图。

由上述仿真结果可以看出，在非高动态时段，即在0～60s仿真时段，自适应抗差容积卡尔曼滤波(ARCKF)与卡尔曼滤波(KF)取得较相似的仿真结果；由于高动态时段60～68s，弹体加速度最高达到60g，机动性强，经卡尔曼滤波(KF)组合的X方向的速度误差最高达到-1.5m/s，Y方向速度误差达到-0.25m/s，Z方向速度误差达到-1.1m/s。经过自适应抗差容积卡尔曼滤波(ARCKF)后X方向速度误差小于0.8/s；Y方向速度误差小于0.15m/s，Z方向速度误差小于0.2m/s。ARCKF较KF滤波结果较有一定改善。由此可证明，本发明所提出的自适应抗差容积卡尔曼滤波(ARCKF)算法有效的提高了导航系统在高动态、强机动的条件下的跟踪精度与性能。