CN112985384A - 一种抗干扰磁航向角优化系统 - Google Patents

Abstract

一种抗干扰磁航向角优化系统，包括姿态角生成模块、磁场信号生成模块、原始磁航向角计算模块和综合信息滤波处理模块，姿态角生成模块用于生成姿态角数据，磁场信号生成模块用于生成磁场信号和磁场信号的统计特性实时分析数据，原始磁航向角计算模块用于根据磁力计采集的磁场信号计算生成原始磁航向角，根据姿态角生成模块中的MEMS惯性测量单元生成的角速度信号、原始磁航向角数据和统计特性实时分析数据调整综合信息滤波处理模块中的卡尔曼滤波器模型参数，采用参数调整后的卡尔曼滤波器模型对输入的航向角速度积分数据和原始磁航向角之间的差进行处理以获得优化后的磁航向角数据。

Description

一种抗干扰磁航向角优化系统

技术领域

本申请涉及一种惯性导航IMU系统技术，尤其涉及一种基于多传感器的抗干扰磁航向角优化技术。

背景技术

磁力计用于测量地磁场在载体三个方向的分量，结合载体姿态角计算地磁场的水平分量，进而计算出磁航向角，实现定向功能，具有抗冲击性、抗振动性等优点，但地磁场测量信号极易受到干扰。硬铁干扰和软铁干扰对磁航向角的计算影响可以通过磁力计磁场标定的方法基本消除，但是当磁力计附近存在随机干扰源时，干扰磁场的幅度、方向具有不确定性，无法给出估计值进行消除，导致磁航向角误差很大，甚至失效。本发明中采用低成本、低精度的MEMS惯性测量单元，辅助磁航向角的计算。

发明内容

本申请涉及一种抗干扰磁航向角优化系统，包括姿态角生成模块(1)、磁场信号生成模块(2)、原始磁航向角计算模块(3)和综合信息滤波处理模块(4)，其中姿态角生成模块包括MEMS惯性测量单元和互补滤波模块，磁场信号生成模块包括三轴磁力计和统计特性实时分析模块，原始磁航向角计算模块包括地磁场水平分量计算模块和原始磁航向角计算模块，其中磁场信号生成模块优选包括硬铁和软铁补偿模块，三轴磁力计输出的磁场信号经过硬铁和软铁补偿模块的补偿后送入地磁场水平分量计算模块；MEMS惯性测量单元优选包括三个MEMS陀螺仪和三个MEMS加速度计。综合信息滤波处理模块包括卡尔曼滤波器。MEMS惯性测量单元用于采集三路角速度信号和三路加速度信号，将三路角速度信号和三路加速度信号输出到互补滤波模块，互补滤波模块对角速度信号和加速度信号进行互补滤波处理，经过互补滤波处理后生成姿态角数据，姿态角数据被输出到地磁场水平分量计算模块；三轴磁力计用于采集输出磁场信号，输出的磁场信号一路输出到地磁场水平分量计算模块，另一路输出到统计特性实时分析模块，地磁场水平分量计算模块用于根据互补滤波模块输出的姿态角数据和三轴磁力计输出的磁场信号计算地磁场水平分量，原始磁航向角计算模块根据地磁场水平分量计算模块输出的地磁场水平分量计算原始磁航向角，统计特性实时分析模块接收三轴磁力计输出的磁场信号，统计计算其特性并输出统计特性数据，综合信息滤波处理模块接收MEMS惯性测量单元输出的角速度、原始磁航向角计算模块输出的原始磁航向角和统计特性实时分析模块输出的统计特性数据，根据输入的数据对综合信息滤波处理模块中的卡尔曼滤波器的参数进行调整，采用调整后的卡尔曼滤波器模型对输入的航向角速度积分数据和原始磁航向角之间的差进行处理，得到航向角增量△Ψ，修正后的磁航向角Ψ＝Ψ_G-△Ψ作为优化的磁航向角输出信号。

进一步互补滤波模块对角速度信号和加速度信号进行互补滤波处理具体包括：由俯仰角速度ω_x积分得到姿态角中的俯仰角θ₀，由滚动角速度ω_y积分得到姿态角中的滚动角γ₀，根据三路加速度信号a_x、a_y、a_z分解计算得到θ_a和γ_a，根据互补滤波公式计算得到姿态角中的俯仰角和滚动角：

θ＝αθ₀+(1-α)·θ_α

γ＝αγ₀+(1-α)·γ_α，

其中俯仰角速度ω_x、滚动角速度ω_y、航向角速度ω_z是由MEMS惯性测量单元采集输出的三路角速度信号，a_x、a_y、a_z是由MEMS惯性测量单元采集输出的三路加速度信号。

进一步地磁场水平分量计算模块用于根据互补滤波模块输出的姿态角数据和三轴磁力计输出的磁场信号计算地磁场水平分量具体包括:三轴磁力计输出的磁场信号向量为[M_x，M_y，M_z]，磁场强度

存储最近的N个数据点M_i(i＝1，2，…N),N为自然数；磁场水平分量计算模块接收互补滤波模块输出的姿态角信息和三轴磁力计输出的磁场信号，计算得到地磁场的水平分量。

地磁场的水平分量：

进一步原始磁航向角计算模块根据地磁场水平分量计算模块输出的地磁场水平分量计算原始磁航向角，具体包括：原始磁航向角计算模块根据地磁场的水平分量M_cx和M_cy计算得到原始的磁航向角：

进一步统计特性实时分析模块接收三轴磁力计输出的磁场信号，统计计算其特性并输出统计特性数据具体包括：三轴磁力计输出的磁场信号向量为[M_x，M_y，M_z]，磁场强度

存储最近的N个数据点M_i(i＝1，2，...N),N为自然数，计算的统计特性包括磁场信号方差VarM、相对于地磁场强度理论值M_E的偏离值ErrorM和干扰幅度大小DistM，其中：(1)N个数据点的磁场信号方差：

式中

是N个数据点磁场平均值；(2)相对于地磁场强度理论值M_E的偏离值：

(3)磁场干扰幅度大小：

进一步综合信息滤波处理模块接收MEMS惯性测量单元输出的角速度、原始磁航向角计算模块输出的原始磁航向角和统计特性实时分析模块输出的统计特性数据，根据输入的数据对综合信息滤波处理模块中的卡尔曼滤波器的参数进行调整具体包括：(1)实时计算更新航向角速度ω_z的方差VarG，调整系统噪声协方差矩阵Q中的参数Q(3，3)＝VarG；(3)根据磁场信号统计特性中的方差VarM以及记录的方差最大值VarMax，进行以下计算：

式中R₀为卡尔曼滤波器中观测噪声协方差矩阵设置的初始值，ExpVar和R_VM为自定义的中间变量，用来反映磁场干扰带来的影响，调整观测噪声协方差矩阵R：

(4)根据磁场信号统计特性中的偏离值ErrorM、平均值

和干扰幅度大小DistM计算自定义的干扰判断因子：

式中A和B为可调系数，取值范围[10,100]，干扰判断因子限幅在范围[0.5,2.0]，即

调整卡尔曼滤波器中的增益矩阵K：K＝K/β。

进一步采用参数调整后的卡尔曼滤波器模型对输入的航向角速度ω_z的积分Ψ_G和原始磁航向角之间的差进行处理，得到航向角增量△Ψ，修正后的磁航向角Ψ＝Ψ_G-△Ψ作为优化的磁航向角输出信号，具体包括：MEMS惯性测量单元采集输出的航向角速度ω_z积分后得到航向角数据Ψ_G，生成一阶矩阵Z，其中Z＝[Ψ_G-Ψ_M]，一阶矩阵Z作为卡尔曼滤波器的输入，经过卡尔曼滤波器的处理得到当前时刻的航向角增量△Ψ，修正后的磁航向角Ψ＝Ψ_G-△Ψ作为优化的磁航向角输出信号。

附图说明

附图1抗干扰磁航向角优化处理系统

具体实施方式

如图1所示，该抗干扰磁航向角优化系统包括姿态角生成模块(1)、磁场信号生成模块(2)、原始磁航向角计算模块(3)和综合信息滤波处理模块(4)。其中姿态角生成模块包括MEMS惯性测量单元和互补滤波模块，MEMS惯性测量单元优选包括三个MEMS陀螺仪和三个MEMS加速度计,磁场信号生成模块包括三轴磁力计以及统计特性实时分析模块，优选的，还可以包括硬铁和软铁补偿模块，原始磁航向角计算模块包括地磁场水平分量计算模块和原始磁航向角计算模块。其中MEMS惯性测量单元用于采集角速度和加速度信号，优选的，通过三个MEMS陀螺仪采集三路角速度，三路角速度包括俯仰角速度ω_x、滚动角速度ω_y、航向角速度ω_z，通过三个MEMS加速度计采集三路加速度信号a_x、a_y、a_z。MEMS惯性测量单元输出的角速度信号和加速度信号送入互补滤波模块，对信号进行互补滤波处理，得到姿态角数据。具体为：由俯仰角速度ω_x积分得到姿态角中的俯仰角θ₀，由滚动角速度ω_y积分得到姿态角中的滚动角γ₀。根据三路加速度信号a_x、a_y、a₂分解计算得到θ_a和γ_a，根据互补滤波公式计算得到姿态角中的俯仰角和滚动角：

θ＝αθ₀+(1-α)·θ_α

γ＝αγ₀+(1-α)·γ_α，

三轴磁力计输出磁场信号，输出的磁场信号向量为[M_x，M_y，M_z]，优选的，三轴磁力计输出的磁场信号可以进行硬铁和软铁补偿后输出三个方向的磁场信号，三轴磁力计输出的磁场信号另一路输出至统计特性实时分析模块，实时计算其三轴磁力计输出的磁场信号的统计特性。磁场强度

存储最近的N个数据点M_i(i＝1，2，...N),N为自然数，优选N＝100。要计算的统计特性包括磁场信号方差VarM、相对于地磁场强度理论值M_E的偏离值ErrorM和干扰幅度大小DistM。

其中：(1)N个数据点的磁场信号方差：

式中

是N个数据点磁场平均值。

(2)相对于地磁场强度理论值M_E的偏离值：

(3)磁场干扰幅度大小：

磁场水平分量计算模块接收互补滤波模块输出的姿态角信息和三轴磁力计输出的磁场信号，计算得到地磁场的水平分量。

地磁场的水平分量：

原始磁航向角计算模块根据地磁场的水平分量M_cx和M_cy计算得到原始的磁航向角：

综合信息滤波处理模块接收原始磁航向角计算模块输出的原始磁航向角、MEMS惯性测量单元输出的三路角速度信号和统计特性实时分析模块输出的信息，对其进行融合处理后输出最终的磁航向角。

综合信息滤波处理模块主要是采用卡尔曼滤波器，通过卡尔曼滤波器进行状态向量估计和信息融合。本系统的改进还在于通过输入到综合信息滤波处理模块的参数对卡尔曼滤波器中的系统噪声协方差矩阵Q(Q为3*3矩阵)、观测噪声协方差矩阵R(R为1*1矩阵)、增益矩阵K(K为3*1矩阵)进行调整，从而提高了抗干扰性能。

综合信息滤波处理模块中的系统状态方程和观测方程：X(k+1)＝ΦX(k)+W(k)

Z(k)＝HX(k)+V(k)

式中三阶状态向量X＝[ΔΨ，ΔK，b]^T，即MEMS陀螺仪的航向角增量、标度因数误差、零偏；观测向量取一阶Z＝[Ψ_G-Ψ_M]，即陀螺仪航向角速度积分计算得到的航向角与磁航向角之差,W和V分别是系统噪声和观测噪声。观测矩阵H＝[1，0，0]。

根据综合信息滤波处理模块的输入，实时调整卡尔曼滤波器中的系统噪声协方差矩阵Q、观测噪声协方差矩阵R、增益矩阵K的具体过程为：

(1)实时计算更新航向角速度ω_z的方差VarG，调整系统噪声协方差矩阵Q中的参数Q(3，3)＝VarG。

(2)根据磁场信号统计特性中的方差VarM以及记录的方差最大值VarMax，进行以下计算：

式中ExpVar和R_VM为自定义的中间变量，用来反映磁场干扰带来的影响从而去调整观测噪声协方差矩阵R；R₀为卡尔曼滤波器中观测噪声协方差矩阵设置的初始值。

调整卡尔曼滤波器中的观测噪声协方差矩阵R：

(3)根据磁场信号统计特性中的偏离值ErrorM、平均值

和干扰幅度大小DistM计算自定义的干扰判断因子：

式中A和B为可调系数，取值范围[10,100]。干扰判断因子限幅在范围[0.5,2.0]，即

调整卡尔曼滤波器中的增益矩阵K：K＝K/β

采用参数调整后的卡尔曼滤波器模型对输入的航向角速度积分数据和原始磁航向角之间的差进行处理，得到航向角增量△Ψ，修正后的磁航向角Ψ＝Ψ_G-△Ψ作为最终的磁航向角输出信号，具体包括：MEMS惯性测量单元采集输出的航向角速度ω_z积分后得到航向角数据Ψ_G，生成一阶矩阵Z，其中Z＝[Ψ_G-Ψ_M]，一阶矩阵Z作为卡尔曼滤波器的输入，经过卡尔曼滤波器的处理得到当前时刻的航向角增量△Ψ，修正后的磁航向角Ψ＝Ψ_G-△Ψ作为最终的磁航向角输出信号。

本发明利用了三个MEMS陀螺仪和三个MEMS加速度计的信息辅助磁力计，并利用磁场信号的统计特性，使磁航向角的计算具有很强的抗干扰性能。综合信息滤波处理模块中引入了原始磁航向角信息，调整卡尔曼滤波器中的系统噪声协方差矩阵Q、观测噪声协方差矩阵R、增益矩阵K，迭代计算得到当前时刻的状态向量估计值去修正磁航向角，提高了输出的磁航向角的角度精度和抗干扰性能。

本发明实施例中处理器可以是通用处理器，例如但不限于，中央处理器(CentralProcessing Unit，CPU)，也可以是专用处理器，例如但不限于，数字信号处理器(DigitalSignal Processor，DSP)、应用专用集成电路(Application Specific IntegratedCircuit，ASIC)和现场可编程门阵列(Field Programmable Gate Array，FPGA)等。此外，处理器还可以是多个处理器的组合。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的模块及方法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

在本申请所提供的实施例中，应该理解到，所揭露的装置可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。

所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。

Claims (9)

1.一种抗干扰磁航向角优化系统，包括姿态角生成模块(1)、磁场信号生成模块(2)、原始磁航向角计算模块(3)和综合信息滤波处理模块(4)，其中姿态角生成模块包括MEMS惯性测量单元和互补滤波模块，磁场信号生成模块包括三轴磁力计和统计特性实时分析模块，原始磁航向角计算模块包括地磁场水平分量计算模块和原始磁航向角计算模块，MEMS惯性测量单元用于采集三路角速度信号和三路加速度信号，将三路角速度信号和三路加速度信号输出到互补滤波模块，互补滤波模块对角速度信号和加速度信号进行互补滤波处理，经过互补滤波处理后生成姿态角数据，姿态角数据被输出到地磁场水平分量计算模块；三轴磁力计用于采集输出磁场信号，输出的磁场信号一路输出到地磁场水平分量计算模块，另一路输出到统计特性实时分析模块，地磁场水平分量计算模块用于根据互补滤波模块输出的姿态角数据和三轴磁力计输出的磁场信号计算地磁场水平分量，原始磁航向角计算模块根据地磁场水平分量计算模块输出的地磁场水平分量计算原始磁航向角，统计特性实时分析模块接收三轴磁力计输出的磁场信号，统计计算其特性并输出统计特性数据，综合信息滤波处理模块接收MEMS惯性测量单元输出的角速度、原始磁航向角计算模块输出的原始磁航向角和统计特性实时分析模块输出的统计特性数据，根据输入的数据对综合信息滤波处理模块中的卡尔曼滤波器的参数进行调整，采用调整后的卡尔曼滤波器模型对输入的航向角速度积分数据和原始磁航向角之间的差进行处理，得到航向角增量△Ψ，修正后的磁航向角Ψ＝Ψ_G-△Ψ作为优化的磁航向角输出信号。

2.如权利要求1所述的抗干扰磁航向角优化系统，其中所述互补滤波模块对角速度信号和加速度信号进行互补滤波处理具体包括：由俯仰角速度ω_x积分得到姿态角中的俯仰角θ₀，由滚动角速度ω_y积分得到姿态角中的滚动角γ₀，根据三路加速度信号a_x、a_y、a_z分解计算得到θ_a和γ_a，根据互补滤波公式计算得到姿态角中的俯仰角和滚动角：

θ＝αθ₀+(1-α)·θ_α

γ＝αγ₀+(1-α)·γ_α，

其中俯仰角速度ω_x、滚动角速度ω_y、航向角速度ω_z是由MEMS惯性测量单元采集输出的三路角速度信号，a_x、a_y、a_z是由MEMS惯性测量单元采集输出的三路加速度信号。

3.如权利要求2所述的抗干扰磁航向角优化系统，其中所述地磁场水平分量计算模块用于根据互补滤波模块输出的姿态角数据和三轴磁力计输出的磁场信号计算地磁场水平分量具体包括:三轴磁力计输出的磁场信号向量为[M_x，M_y，M_z]，磁场强度

存储最近的N个数据点M_i(i＝1，2，...N),N为自然数；

磁场水平分量计算模块接收互补滤波模块输出的姿态角信息和三轴磁力计输出的磁场信号，计算得到地磁场的水平分量。

地磁场的水平分量：

4.如权利要求3所述的抗干扰磁航向角优化系统，其中所述原始磁航向角计算模块根据地磁场水平分量计算模块输出的地磁场水平分量计算原始磁航向角具体包括：原始磁航向角计算模块根据地磁场的水平分量M_cx和M_cy计算得到原始的磁航向角：

5.如权利要求1所述的抗干扰磁航向角优化系统，其所述统计特性实时分析模块接收三轴磁力计输出的磁场信号，统计计算其特性并输出统计特性数据具体包括：三轴磁力计输出的磁场信号向量为[M_x，M_y，M_z]，磁场强度

存储最近的N个数据点M_i(i＝1，2，...N),N为自然数，计算的统计特性包括磁场信号方差VarM、相对于地磁场强度理论值M_E的偏离值ErrorM和干扰幅度大小DistM，

其中：(1)N个数据点的磁场信号方差：

式中

是N个数据点磁场平均值；

(2)相对于地磁场强度理论值M_E的偏离值：

(3)磁场干扰幅度大小：

6.如权利要求1所述的抗干扰磁航向角优化系统，所述综合信息滤波处理模块接收MEMS惯性测量单元输出的角速度、原始磁航向角计算模块输出的原始磁航向角和统计特性实时分析模块输出的统计特性数据，根据输入的数据对综合信息滤波处理模块中的卡尔曼滤波器的参数进行调整具体包括：(1)实时计算更新航向角速度ω_z的方差VarG，调整系统噪声协方差矩阵Q中的参数Q(3，3)＝VarG；(3)根据磁场信号统计特性中的方差VarM以及记录的方差最大值VarMax，进行以下计算：

式中ExpVar和R_VM为自定义的中间变量，R₀为卡尔曼滤波器中观测噪声协方差矩阵设置的初始值，用来反映磁场干扰带来的影响，调整观测噪声协方差矩阵R：

根据磁场信号统计特性中的偏离值ErrorM、平均值

和干扰幅度大小DistM计算自定义的干扰判断因子：

式中A和B为可调系数，取值范围[10,100]，干扰判断因子限幅在范围[0.5,2.0]，即

调整卡尔曼滤波器中的增益矩阵K：K＝K/β。

7.如权利要求1所述的抗干扰磁航向角优化系统，其中所述采用参数调整后的卡尔曼滤波器模型对输入的航向角速度ω_z的积分Ψ_G和原始磁航向角之间的差进行处理，得到航向角增量△Ψ，修正后的磁航向角Ψ＝Ψ_G-△Ψ作为优化的磁航向角输出信号，具体包括：MEMS惯性测量单元采集输出的航向角速度ω_z积分后得到航向角数据Ψ_G，生成一阶矩阵Z，其中Z＝[Ψ_G -Ψ_M]，一阶矩阵Z作为卡尔曼滤波器的输入，经过卡尔曼滤波器的处理得到当前时刻的航向角增量△Ψ，修正后的磁航向角Ψ＝Ψ_G-△Ψ作为优化的磁航向角输出信号。

8.如权利要求1所述的抗干扰磁航向角优化系统，其中磁场信号生成模块(2)进一步包括硬铁和软铁补偿模块，三轴磁力计输出的磁场信号经过硬铁和软铁补偿模块的补偿后送入地磁场水平分量计算模块。

9.如权利要求1所述的抗干扰磁航向角优化系统，其中MEMS惯性测量单元包括三个MEMS陀螺仪和三个MEMS加速度计。

Images