[发明专利]一种抗机体磁干扰的无人机航向解算方法和系统

权利要求书

1.一种抗机体磁干扰的无人机航向解算方法，其特征在于，包括以下步骤：

步骤1：在无人机起飞前，完成磁强计校准参数计算以及INS/GNSS组合导航系统导航解算，并记录起飞前磁强计输出的原始值为所述磁强计第一次原始值

步骤11：利用磁强计椭圆校正方法或磁强计椭球校正方法完成所述磁强计校准参数的计算；

步骤12：利用最优滤波方法实现所述INS/GNSS组合导航系统导航参数解算，并实时计算所述无人机的位置、速度、姿态和航向信息；

步骤13：记录起飞前所述无人机静止状态下所述磁强计的原始输出值为所述磁强计第一次原始值其中上角标b表示载体坐标系；

步骤2：设定判断阈值，所述无人机解锁起飞后，当无人机的垂向速度大于所述判断阈值时，记录所述磁强计的输出值为所述磁强计第二次原始值

步骤3：用所述磁强计的所述第二次原始值和所述第一次原始值做差形成磁强计输出值偏差量，利用所述磁强计输出值偏差量补偿磁强计的实时输出值，获得磁偏差补偿后的磁强计实时输出值；

步骤31：将在所述载体坐标系b下获得所述磁强计的所述第二次原始值和所述第一次原始值由载体坐标系b投影到导航坐标系n下，得到和

其中m_x为X轴输出值，m_y为Y轴输出值，m_z为Z轴输出值；

步骤32：由和做差形成磁强计输出值偏差量

步骤33：利用捷联姿态矩阵将所述磁强计输出值偏差量ΔMⁿ变换到所述载体坐标系b下，获得磁强计输出值在所述载体坐标系b下的偏差量ΔM^b；

步骤34：对在载体坐标系b下磁强计的实时输出值M^b进行偏差量补偿，获得经磁偏差补偿后的磁强计实时输出值为

步骤4：用所述磁强计校准参数对所述磁偏差补偿后的磁强计实时输出值进行磁校准补偿，并完成磁航向解算；

步骤41：利用所述磁强计校准参数对所述经磁偏差补偿后的磁强计实时输出值进行补偿计算，获得磁强计的实时输出估计值

步骤42：以所述磁强计的实时输出估计值为输入值，利用姿态和航向参考系统AHRS实时计算所述无人机的航向角。

2.根据权利要求1所述的一种抗机体磁干扰的无人机航向解算方法，其特征在于，所述捷联姿态矩阵由所述无人机的姿态和所述航向角计算得到，其中所述姿态角和所述航向角采用所述INS/GNSS组合导航系统计算得出。

3.根据权利要求1所述的一种抗机体磁干扰的无人机航向解算方法，其特征在于，所述步骤12中的最优滤波方法选用卡尔曼滤波算法。

4.一种抗机体磁干扰的无人机航向解算系统，其特征在于，应用如权利要求1-3中任一项所述的一种抗机体磁干扰的无人机航向解算方法；所述系统包括导航参数计算模块和磁强计补偿模块；所述导航参数计算模块与所述磁强计补偿模块电连接，并进行相互通信；所述导航参数计算模块与GNSS和IMU进行通信；所述磁强计补偿模块与磁传感器电连接通信，所述磁传感器发送磁场强度至所述磁强计补偿模块；

所述导航参数计算模块包括INS/GNSS组合导航模块以及姿态和航向参考系统AHRS模块；所述GNSS发送位置和速度至INS/GNSS组合导航系统模块；所述IMU发送角速度和比力至所述INS/GNSS组合导航系统模块以及所述姿态和航向参考系统AHRS模块；所述INS/GNSS组合导航模块根据所述GNSS发送的所述位置和所述速度，计算获得所述位置、所述速度、所述姿态角和所述航向角；所述导航参数计算模块根据所述位置、所述速度、所述姿态角和所述航向角，计算获得垂向速度和并发送至所述磁强计补偿模块；

所述磁强计补偿模块包括磁强计校准模块和磁强计偏差量计算模块；所述磁强计校准模块接收所述磁场强度并计算得到所述磁强计校准参数并传输至所述磁强计补偿计算模块；所述磁强计输出值偏差量模块实时接收所述垂向速度、所述和所述磁传感器发送的所述磁场强度，计算得出磁强计实时输出值并传输至所述磁强计补偿计算模块；所述磁强计补偿计算模块接收所述磁强计校准参数和所述磁强计实时输出值对所述磁强计实时输出值进行磁强计补偿，获到磁强计实时输出估计值并发送至所述导航参数计算模块；

所述导航参数计算模块接收所述补偿后的磁强计输出值，所述姿态和航向参考系统AHRS模块根据所述IMU发送的所述角速度和所述比力以及磁强计补偿计算模块发送的经磁偏差补偿后的所述磁强计实时输出估计值计算所述姿态角和所述航向角；同时，所述INS/GNSS组合导航模块完成位置、速度、姿态和航向角导航参数解算。