CN117419745A - 一种基于循环ekf的惯性辅助地磁在线标定方法及*** - Google Patents

Abstract

本发明提供一种基于循环EKF的惯性辅助地磁在线标定方法及***，首先根据陀螺仪解算的姿态变化矩阵和地磁传感器输出的关系，构建复杂环境下状态方程，然后根据地磁传感器误差标定模型，构建非线性观测方程，基于状态方程和非线性观测方程确定在线标定滤波模型；通过EKF滤波更新过程，再通过循环滤波量测更新过程，进行在线标定滤波模型状态估计。本发明提出一种循环EKF求解惯性辅助地磁在线标定滤波模型，通过循环滤波量测更新过程，提高状态估计精度，从而实现高精度地磁在线标定。

Description

一种基于循环EKF的惯性辅助地磁在线标定方法及***

技术领域

本发明属于复杂扰动环境下的地磁标定领域，涉及一种基于循环EKF的惯性辅助地磁在线标定方法及***。

背景技术

地磁传感器由于其体积小、成本低、功耗少、接口数字化、抗高过载、便携性好等优点被广泛应用于兵器、工业、车载、航天、航空、航海等导航***中。复杂环境下干扰磁场通常存在固定磁场、感应磁场、涡流磁场及随机磁场等四种磁场。这些扰动磁场严重降低地磁传感器信号的有效性，导致姿态测量、导航定位含有较大误差，地磁传感器标定技术已经成为制约地磁导航技术发展的关键因素之一，因此，复杂环境下地磁传感器标定已经成为亟需解决的难题。

目前，地磁传感器标定方法主要分为三类，一类为自标定，仅仅利用地磁传感器自身采集的数据进行分析，得到各项误差系数；第二类为利用陀螺仪/加速度计等其他传感器辅助标定；第三类为利用无磁转台进行基准比较。通常第一类和第三类都无法实现地磁传感器在线标定，需要在地磁传感器正式工作之前进行离线标定，这在实际应用中不仅时间成本高，而且随着环境的改变，其标定参数的适用性会下降，直接影响导航精度。因此，亟需寻求高精度的地磁传感器在线标定方法。

目前，地磁传感器在线标定方法主要是采用惯性辅助地磁传感器实现在线标定，已经成为当前地磁传感器标定领域的热点难题。Yuanxin Wu等人构建磁力计相对于惯性传感器失准角模型，设计扩展卡尔曼滤波，有效估计了磁力计固有和交叉耦合系数以及陀螺仪偏置，所提方法不受加速度干扰，并可能适用于任何动态条件。尽管这些方法均取得了较好的效果，但对于复杂环境下，如制导炮弹，在飞行过程中具有高过载(≥10000g)、高转速(≥20r/s)等高动态特点，地磁传感器输出中存在复杂噪声，对其标定提出了新的要求，为此亟需寻求新的滤波方法，以适用于复杂环境下惯性辅助地磁在线标定。

针对惯性辅助地磁在线标定非线性滤波模型，通常采用EKF解决滤波非线性问题，但无法解决其不确定扰动等问题。目前，在鲁棒Kalman滤波方面已有大量研究，但将极少应用到惯性辅助地磁在线标定领域，为此本发明设计新型鲁棒滤波方法以适应复杂环境下惯性辅助地磁在线标定。

发明内容

本发明的目的在于提供一种基于循环EKF的惯性辅助地磁在线标定方法及***，以适应复杂环境下惯性辅助地磁在线标定，提高状态估计精度，实现高精度地磁在线标定。

实现本发明目的的技术解决方案为：

一种基于循环EKF的惯性辅助地磁在线标定方法，包括步骤：

根据陀螺仪解算的姿态变化矩阵和地磁传感器输出的关系，构建复杂环境下状态方程；

根据地磁传感器误差标定模型，构建非线性观测方程；

基于状态方程和非线性观测方程确定在线标定滤波模型；

通过EKF滤波更新过程，再通过循环滤波量测更新过程，进行在线标定滤波模型状态估计。

进一步地，所述状态方程为：

X_k＝F_k,k-1X_k-1

其中，状态变量状态转移矩阵H_b(k)为k时刻地磁场矢量在载体坐标系下的投影，K_parameter,k为软磁干扰矩阵，O_k为硬磁干扰矩阵,/>为姿态转换矩阵，为单位矩阵。

进一步地，所述H_b(k)、K_parameter,k、O_k具体为：

进一步地，所述地磁传感器误差标定模型为：

H_s＝KH_b+O+n_m

其中，软磁干扰矩阵K＝C_NC_SF(I_3×3+C_SI),硬磁干扰矩阵O＝KH_HI+h_O，C_N为非正交误差矩阵，C_SF为刻度因子误差矩阵，C_SI为软磁系数矩阵，H_HI为硬磁误差矢量，h_O为传感器零偏误差矢量，H_s为地磁传感器的测量输出矢量，H_b为地磁场矢量在载体坐标系下的投影，n_m为随机噪声。

进一步地，所述软磁干扰矩阵K、硬磁干扰矩阵O具体为：

式中，κ₁₁,κ₂₂,κ₃₃为刻度因子；κ₁₁,κ₂₂,κ₃₃为交叉耦合系数；o₁,o₂,o₃为零偏。

进一步地，所述观测方程为：

其中，X_k,k-1为根据状态方程所得的一步预测结果，X_k,k-1(i),i＝1,2...,12表示一步预测结果中第i个元素，参数矩阵

进一步地，所述线标定滤波模型为：

式中，Z_k＝H_s(k)。

进一步地，所述通过EKF滤波更新过程具体包括包括时间更新和量测更新，所述时间更新为：

所述量测更新为：

P_k,k＝(I_12×12-G_kH_k)P_k,k-1

式中，为一步预测结果，代入观测方程更新观测矩阵，P_k,k-1为一步预测误差协方差矩阵，/>和P_k,k分别表示状态估计和相应的状态估计误差协方差矩阵，G_k表示Kalman滤波增益，Q_k-1为***噪声协方差矩阵，其更新形式为：

式中，α为可控参数，σ_g是陀螺仪零偏，Δt为采样时间。

进一步地，通过循环滤波量测更新过程具体包括：

将状态估计误差协方差矩阵作为循环滤波过程的一步预测估计误差协方差矩阵，对于循环滤波第1步，令P_k,k,1＝P_k,k，对于循环滤波第n步，循环滤波阶段不需要进行时间更新，只需量测更新，即：

P_k,k,n+1＝(I_12×12-G_k,nH_k,n)P_k,k,n

式中， 为地磁传感器输出噪声方差。

一种基于循环EKF的惯性辅助地磁在线标定***，包括复杂环境下状态方程构建单元、非线性观测方程构建单元、线标定滤波模型构建单元、循环滤波状态估计单元；其中：

所述复杂环境下状态方程构建单元用于根据陀螺仪解算的姿态变化矩阵和地磁传感器输出的关系，构建复杂环境下状态方程；

所述非线性观测方程构建单元用于根据地磁传感器误差标定模型，构建非线性观测方程；

所述线标定滤波模型构建单元用于基于状态方程和非线性观测方程确定在线标定滤波模型；

所述循环滤波状态估计单元通过EKF滤波更新过程，再通过循环滤波量测更新过程，进行在线标定滤波模型状态估计。

与现有技术相比，本发明的有益效果为：本发明根据陀螺仪解算的姿态变化矩阵和地磁传感器输出的关系，构建复杂环境下状态方程；根据地磁传感器误差标定模型，构建非线性观测方程，通过循环滤波量测更新过程，提高状态估计精度，从而实现高精度地磁在线标定。

附图说明

图1为复杂环境下惯性辅助地磁在线标定框架图。

图2为REKF核心思想图。

图3为本发明在线标定方法流程图。

具体实施方式

接下来结合附图，对本发明的实施进行详细说明。

为了克服现有技术的不足，本发明提出一种基于循环EKF的惯性辅助地磁在线标定方法，结合图1～图3，首先，根据地磁传感器输出与陀螺仪输出之间的耦合关系，以地磁传感器输出信息和待标定参数作为***状态变量，将未标定的地磁传感器输出信息作为量测值，构建惯性辅助地磁在线标定滤波模型。然后，针对该模型的求解，提出一种循环EKF(Recurrent EKF，REKF)，在传统EKF基础上，将EKF所得的状态估计结果和状态估计协方差矩阵作为量测更新过程的一步预测结果和一步预测协方差矩阵，循环更新量测过程，提高状态估计精度。此外，根据可控性矩阵和可观测性矩阵，证明所提REKF滤波器是一致渐近稳定的。具体包括：

构建地磁传感器误差模型为：

H_s＝KH_b+O+n_m (1)

式中，K＝C_NC_SF(I_3×3+C_SI),O＝KH_HI+h_O。C_N为非正交误差矩阵。C_SF为刻度因子误差矩阵。C_SI为软磁系数矩阵。H_HI为硬磁误差矢量。h_O为传感器零偏误差矢量。H_s为地磁传感器的测量输出矢量。H_b为地磁场矢量在载体坐标系下的投影。n_m为随机噪声。

由式(1)可知，在数学意义上地磁传感器的误差可以统一建模，参数矩阵K和O可描述为：

式中，κ₁₁,κ₂₂,κ₃₃为刻度因子；κ₁₁,κ₂₂,κ₃₃为交叉耦合系数；o₁,o₂,o₃为零偏，K为对称矩阵。K被称为软磁干扰矩阵，O被称为硬磁干扰矩阵，通常都为固定常数。

在炮弹飞行中，捷联于弹体上的地磁传感器可实时测量地磁矢量在弹体坐标系上的三轴分量H_b＝[H_b,x H_b,y H_b,z]^T。地磁矢量在地理坐标系的三轴分量为H_n＝[H_n,x H_n,y H_n,z]^T，由于炮弹的射程范围相对于地球半径而言非常小，因而假设在地理坐标系中，地磁矢量大小与方向始终保持不变，可由发射地经纬度和时间根据IGRF12模型求得。根据坐标系转换关系：

式中，为姿态转换矩阵。

根据式(3)可知，对于k时刻和k-1时刻，该式恒成立，即：

式中，H_n(k)＝H_n(k-1)。

对于k时刻

式中，I_3×3为3×3的单位矩阵。

根据链式法则对式(4)进一步展开可得：

式中，采用优化旋转矢量法求解。

联合式(2)和式(6)，记K_parameter,k＝[κ₁₁ κ₂₂ κ₃₃ κ₁₂ κ₁₃ κ₂₃]，可得

根据式(7)构建惯性辅助地磁在线标定状态方程，选择状态变量可得

X_k＝F_k,k-1X_k-1 (8)

式中，

对式(1)进行离散化列写，对于k时刻有

H_s(k)＝K_kH_b(k)+O_k+n_m,k＝h(X_k)+n_m,k (9)

根据式(9)可知，等式左边为k时刻地磁传感器测量地磁信息，等式右边皆为状态变量，为非线性方程，对式(9)求解雅克比矩阵，可得观测矩阵

式中，X_k,k-1为根据式(8)所得的一步预测结果。为一步预测结果代入式(2)中的软磁干扰矩阵，X_k,k-1(i),i＝1,2...,12表示一步预测结果中第i个元素。/>

结合式(8)和式(9)，可得惯性辅助地磁在线标定滤波模型

式中，Z_k＝H_s(k)。

对于滤波模型式(11)，通常采用EKF进行状态估计，根据观测方程，理论上Z_k＝h(X_k)+n_m,k是恒成立的，可通过循环迭代方式缩小||Z_k-h(X_k)||误差，为此本文提出一种循环EKF(REKF)。REKF主要由EKF滤波更新过程和循环迭代过程组成，首先进行EKF滤波更新过程，包括时间更新和量测更新

时间更新：

量测更新：

P_k,k＝(I_12×12-G_kH_k)P_k,k-1 (12e)

式中，为一步预测结果，可将其代入式(10)更新观测矩阵。P_k,k-1为一步预测误差协方差矩阵。/>和P_k,k分别表示状态估计和相应的估计误差协方差矩阵。G_k表示Kalman滤波增益。Q_k-1为***噪声协方差矩阵，其更新形式为：

式中，α为可控参数，其取值范围一般为10～500之间，根据实际需求进行确定。σ_g是陀螺仪零偏。Δt为采样时间。

在复杂环境下，陀螺仪具有自主性，不受外界干扰，而地磁传感器容易受到外界扰动，导致其输出存在异常噪声，为此循环量测更新过程，实现状态变量高精度估计。以k时刻为例，REKF核心思想见附图2。

将式(12)状态估计结果作为循环滤波阶段的一步预测估计结果，对于循环滤波第1步，令对于循环滤波第n步，更新观测矩阵

将式(12)的状态估计误差协方差矩阵作为循环滤波过程的一步预测估计误差协方差矩阵，对于循环滤波第1步，令P_k,k,1＝P_k,k，对于循环滤波第n步，循环滤波阶段不需要进行时间更新，只需要量测更新，即

P_k,k,n+1＝(I_12×12-G_k,nH_k,n)P_k,k,n (15c)

式(15)中，由于量测信息没有改变，因此其量测噪声协方差矩阵和式(12)中一致，其公式为

式中，为地磁传感器输出噪声方差，可根据具体传感器型号及环境特性进行调节。

提出的REKF的单步运行伪代码如下所示：

接下来对本发明滤波的稳定性进行证明：对于式(12)的EKF滤波具有稳定性，重点分析式(15)的滤波稳定性，其核心思想，类似于重构了一步预测方程，即

量测方程没有改变。因此，式(15)的状态转移矩阵为F_k,k-1,n＝I_12×12，***扰动矩阵也可以视为单位阵，即Γ_k,k-1,n＝I_12×12。

对于本文所提的***可控矩阵为：

因此，式(15)***滤波过程是完全可控的。

可观矩阵为：

式中，重点关注对角线元素，其他部分用*代替。

式中，重点关注对角线元素，其他部分用*代替。

联立式(19)～(21)，可得观测矩阵

因此，式(15)***滤波过程是完全可观的。根据式(18)和式(22)可知，式(15)滤波过程是一致渐近稳定的。

综上所述，基于REKF的惯性辅助地磁在线标定方法的伪代码为：

结合图2本发明将EKF状态估计结果作为循环滤波阶段的一步预测估计结果，将EKF的状态估计误差协方差矩阵作为循环滤波过程的一步预测估计误差协方差矩阵，循环更新量测过程。

Claims (10)

1.一种基于循环EKF的惯性辅助地磁在线标定方法，其特征在于，包括步骤：

根据陀螺仪解算的姿态变化矩阵和地磁传感器输出的关系，构建复杂环境下状态方程；

根据地磁传感器误差标定模型，构建非线性观测方程；

基于状态方程和非线性观测方程确定在线标定滤波模型；

通过EKF滤波更新过程，再通过循环滤波量测更新过程，进行在线标定滤波模型状态估计。

2.根据权利要求1所述的基于循环EKF的惯性辅助地磁在线标定方法，其特征在于，所述状态方程为：

X_k＝F_k,k-1X_k-1

其中，状态变量状态转移矩阵/>H_b(k)为k时刻地磁场矢量在载体坐标系下的投影，K_parameter,k为软磁干扰矩阵，O_k为硬磁干扰矩阵,/>为姿态转换矩阵，为单位矩阵。

3.根据权利要求1所述的基于循环EKF的惯性辅助地磁在线标定方法，其特征在于，所述H_b(k)、K_parameter,k、O_k具体为：

4.根据权利要求1所述的基于循环EKF的惯性辅助地磁在线标定方法，其特征在于，所述地磁传感器误差标定模型为：

H_s＝KH_b+O+n_m

其中，软磁干扰矩阵K＝C_NC_SF(I_3×3+C_SI),硬磁干扰矩阵O＝KH_HI+h_O，C_N为非正交误差矩阵，C_SF为刻度因子误差矩阵，C_SI为软磁系数矩阵，H_HI为硬磁误差矢量，h_O为传感器零偏误差矢量，H_s为地磁传感器的测量输出矢量，H_b为地磁场矢量在载体坐标系下的投影，n_m为随机噪声。

5.根据权利要求4所述的基于循环EKF的惯性辅助地磁在线标定方法，其特征在于，所述软磁干扰矩阵K、硬磁干扰矩阵O具体为：

式中，κ₁₁,κ₂₂,κ₃₃为刻度因子；κ₁₁,κ₂₂,κ₃₃为交叉耦合系数；o₁,o₂,o₃为零偏。

6.根据权利要求1所述的基于循环EKF的惯性辅助地磁在线标定方法，其特征在于，所述观测方程为：

其中，X_k,k-1为根据状态方程所得的一步预测结果，X_k,k-1(i),i＝1,2...,12表示一步预测结果中第i个元素，参数矩阵

7.根据权利要求1所述的基于循环EKF的惯性辅助地磁在线标定方法，其特征在于，所述线标定滤波模型为：

式中，Z_k＝H_s(k)。

8.根据权利要求1所述的基于循环EKF的惯性辅助地磁在线标定方法，其特征在于，所述通过EKF滤波更新过程具体包括包括时间更新和量测更新，所述时间更新为：

所述量测更新为：

P_k,k＝(I_12×12-G_kH_k)P_k,k-1

式中，为一步预测结果，代入观测方程更新观测矩阵，P_k,k-1为一步预测误差协方差矩阵，/>和P_k,k分别表示状态估计和相应的状态估计误差协方差矩阵，G_k表示Kalman滤波增益，Q_k-1为***噪声协方差矩阵，其更新形式为：

式中，α为可控参数，σ_g是陀螺仪零偏，Δt为采样时间。

9.根据权利要求8所述的基于循环EKF的惯性辅助地磁在线标定方法，其特征在于，通过循环滤波量测更新过程具体包括：

将状态估计误差协方差矩阵作为循环滤波过程的一步预测估计误差协方差矩阵，对于循环滤波第1步，令P_k,k,1＝P_k,k，对于循环滤波第n步，循环滤波阶段不需要进行时间更新，只需量测更新，即：

P_k,k,n+1＝(I_12×12-G_k,nH_k,n)P_k,k,n

式中， 为地磁传感器输出噪声方差。

10.一种基于循环EKF的惯性辅助地磁在线标定***，其特征在于，包括复杂环境下状态方程构建单元、非线性观测方程构建单元、线标定滤波模型构建单元、循环滤波状态估计单元；其中：

所述复杂环境下状态方程构建单元用于根据陀螺仪解算的姿态变化矩阵和地磁传感器输出的关系，构建复杂环境下状态方程；

所述非线性观测方程构建单元用于根据地磁传感器误差标定模型，构建非线性观测方程；

所述线标定滤波模型构建单元用于基于状态方程和非线性观测方程确定在线标定滤波模型；

所述循环滤波状态估计单元通过EKF滤波更新过程，再通过循环滤波量测更新过程，进行在线标定滤波模型状态估计。