[发明专利]一种抗干扰磁航向角优化系统

权利要求书

1.一种抗干扰磁航向角优化系统，包括姿态角生成模块(1)、磁场信号生成模块(2)、原始磁航向角计算模块(3)和综合信息滤波处理模块(4)，其中姿态角生成模块包括MEMS惯性测量单元和互补滤波模块，磁场信号生成模块包括三轴磁力计和统计特性实时分析模块，原始磁航向角计算模块包括地磁场水平分量计算模块和原始磁航向角计算子模块，MEMS惯性测量单元用于采集三路角速度信号和三路加速度信号，将三路角速度信号和三路加速度信号输出到互补滤波模块，互补滤波模块对角速度信号和加速度信号进行互补滤波处理，经过互补滤波处理后生成姿态角数据，姿态角数据被输出到地磁场水平分量计算模块；三轴磁力计用于采集输出磁场信号，输出的磁场信号一路输出到地磁场水平分量计算模块，另一路输出到统计特性实时分析模块，地磁场水平分量计算模块用于根据互补滤波模块输出的姿态角数据和三轴磁力计输出的磁场信号计算地磁场水平分量，原始磁航向角计算子模块根据地磁场水平分量计算模块输出的地磁场水平分量计算原始磁航向角，统计特性实时分析模块接收三轴磁力计输出的磁场信号，统计计算其特性并输出统计特性数据，综合信息滤波处理模块接收MEMS惯性测量单元输出的角速度、原始磁航向角计算子模块输出的原始磁航向角和统计特性实时分析模块输出的统计特性数据，根据输入的数据对综合信息滤波处理模块中的卡尔曼滤波器的参数进行调整，采用调整后的卡尔曼滤波器模型对输入的航向角速度积分数据Ψ_G和原始磁航向角之间的差进行处理，得到航向角增量△Ψ，修正后的磁航向角Ψ＝Ψ_G-△Ψ作为优化的磁航向角输出信号；所述综合信息滤波处理模块接收MEMS惯性测量单元输出的角速度、原始磁航向角计算子模块输出的原始磁航向角和统计特性实时分析模块输出的统计特性数据，根据输入的数据对综合信息滤波处理模块中的卡尔曼滤波器的参数进行调整具体包括：(1)实时计算更新航向角速度ω_z的方差VarG，调整系统噪声协方差矩阵Q中的参数Q(3,3)＝VarG；(3)根据磁场信号统计特性中的方差VarM以及记录的方差最大值VarMax，进行以下计算：

，式中ExpVar和R_VM为自定义的中间变量，R₀为卡尔曼滤波器中观测噪声协方差矩阵设置的初始值，用来反映磁场干扰带来的影响；调整观测噪声协方差矩阵R：(4)根据磁场信号统计特性中的偏离值ErrorM、平均值和干扰幅度大小DistM计算自定义的干扰判断因子：

式中A和B为可调系数，取值范围[10,100]，干扰判断因子限幅在范围[0.5,2.0]，即调整卡尔曼滤波器中的增益矩阵K：K＝K/β；其中，平均值为最近的N个数据点M_i的磁场强度平均值，i＝1，2，...N，偏离值ErrorM是平均值相对于地磁场强度理论值M_E的偏离值，干扰幅度大小

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

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

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

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