基于地磁图适配性评估的自适应惯性/地磁组合导航方法

本发明涉及地磁导航领域，具体涉及一种基于地磁图适配性评估的自适应惯性/地磁组合导航方法。

背景技术：

1、地磁导航系统是一种完全自主的导航手段，具有误差不随时间积累的优点，可以增强惯性导航系统，惯性/地磁组合导航系统可以提高导航系统整体性能。组合导航系统采用两种或两种以上导航系统对同一导航信息进行量测并且解算以形成量测量，从这些量测量中计算出导航系统的误差并进行校正。传统的惯性/地磁组合导航系统的工作流程为：首先根据规划好航迹区域的地磁信息选择合适的地磁分量制作地磁基准图，将绘制好的地磁基准图存储在载体飞行器的计算机中，由安装在飞行器上的地磁传感器实时获取该区域的匹配特征量的值。同时根据载体上的惯性导航系统(ins)输出的参考航迹，选取合适大小的匹配区域，进行地磁匹配，将结果与ins的输出结果进行滤波融合得到惯性/地磁组合导航系统的输出。由于地磁匹配算法对匹配区域内的地磁场的变化程度较为敏感，例如在某些地磁图自相似程度较高或者地图数值变化比较平缓的地区，地磁匹配算法的精度可能会下降，甚至出现无法匹配的情况，严重影响组合导航系统的精度。

技术实现思路

1、针对现有技术中的上述不足，本发明提供了一种基于地磁图适配性评估的自适应惯性/地磁组合导航方法。

2、为了达到上述发明目的，本发明采用的技术方案为：

3、一种基于地磁图适配性评估的自适应惯性/地磁组合导航方法，包括如下步骤：

4、s1、给定卡尔曼滤波器初始状态方程x0、初始协方差矩阵p0、初始系统噪声序列的方差矩阵q0和初始量测噪声序列的方差矩阵r0；

5、s2、当卡尔曼滤波器与惯性导航解算周期同频率时进行时间更新，得到k时刻的状态估计值xk和协方差矩阵pk；

6、s3、当量测更新周期到来时获取地磁传感器量测值，并根据当前时刻的惯性导航参考位置提取匹配区域内的地磁图，根据所获取的地磁图计算粗糙方差比；

7、s4、根据计算所得到的粗糙方差比进行卡尔曼滤波器参数自适应更新，若粗糙方差比小于设定阈值则该周期内不进行地磁匹配，若粗糙方差比大于等于设定阈值则通过地磁图适应性修正量测噪声序列的方差阵；

8、s5、根据地磁匹配输出的定位信息与惯性导航输出的参考位置获取k时刻的观测量并进行卡尔曼滤波器量测方程更新，根据量测更新结果校正惯性导航误差。

9、进一步的，所述s1中卡尔曼滤波器的状态方程表示为：

10、

11、其中，x(t)为t时刻以地理坐标系为导航坐标系建立的系统误差状态变量且

12、

13、式中，φe,φn,φu为捷联惯导数学平台姿态误差角分量，δve,δvn,δvu为东北天向速度误差分量，依次为经度、纬度、高度误差，εbx,εby,εbz一次为陀螺常值漂移误差三轴分量，为加速度计零偏；

14、f(t)为系统状态转移矩阵：

15、

16、式中，fn为对应的9维基本导航参数系统阵，fs为惯性器件误差与姿态速度位置误差之间的状态转移矩阵，fm为惯性器件误差自身的状态转移矩阵且

17、

18、fm＝[06×6]

19、为弹体坐标系b到导航坐标系n的坐标转换矩阵；

20、g(t)为系统噪声转移矩阵且

21、

22、w(t)为系统白噪声误差矩阵且

23、

24、式中，为陀螺仪随机误差的三轴分量，为加速度计的随机误差的三轴分量。

25、进一步的，所述s3中粗糙方差比的具体计算方式为：

26、ξ＝r/σm

27、其中，ξ为粗糙方差比，r为地磁变化粗糙度且

28、r＝(rx+ry)/2

29、

30、

31、式中，rx、ry为地磁变化粗糙度对应方向上的分量，m和n为被评估区域的地磁图大小为m×n，m(i，j)为地磁图中编号为(i，j)的网格的地磁强度值；

32、σm为地磁强度值标准差且

33、

34、式中，em为区域内地磁强度值的平均值且

35、

36、进一步的，所述s4中卡尔曼滤波器参数自适应更新的具体方式为：

37、

38、其中，rk为k时刻的量测噪声序列的方差阵，ξ为粗糙方差比。

39、进一步的，所述s5中根据地磁匹配输出的定位信息与惯性导航输出的参考位置获取k时刻的观测量表示为：

40、

41、其中，z为量测量，依次为经度、纬度和高度的误差，依次为捷联惯导输出的经度、纬度和高度信息，λm、为地磁匹配输出的经度和纬度信息，hm是气压高度计输出的临近空间高超声速飞行器的高度。

42、进一步的，所述s5中卡尔曼滤波器量测方程更新的具体方式为：

43、s51、对状态方程离散化，根据初始状态进行状态一步预测

44、

45、其中，为k-1时刻状态矢量的收敛值；

46、s52、利用一步预测值进行状态估计：

47、

48、其中，kk为滤波增益矩阵；

49、

50、其中，rk为k时刻量测噪声的协方差阵，pk/k-1为一步预测均方误差阵，其对角线元素是各个状态估计的方差；

51、

52、其中，qk-1为k-1时刻系统噪声的协方差阵，pk-1为k-1时刻估计均方误差矩阵，k时刻估计均方误差矩阵pk为

53、

54、其中，i为单位阵；

55、s53、给定初值x0和p0，根据k时刻的位置量测矢量zk，递推求得k时刻的状态估计

56、s54、经过重复计算n次得到的收敛值。

57、本发明具有以下有益效果：

58、根据地磁基准图的适配性评估算法，对该匹配区域内的地磁匹配适配性进行评价，根据评价结果自适应改变组合导航滤波器的参数或周期，以提高组合导航系统的精度。

技术特征：

1.一种基于地磁图适配性评估的自适应惯性/地磁组合导航方法，其特征在于，包括如下步骤：

2.根据权利要求1所述的基于地磁图适配性评估的自适应惯性/地磁组合导航方法，其特征在于，所述s1中卡尔曼滤波器的状态方程表示为：

3.根据权利要求1所述的基于地磁图适配性评估的自适应惯性/地磁组合导航方法，其特征在于，所述s3中粗糙方差比的具体计算方式为：

4.根据权利要求1所述的基于地磁图适配性评估的自适应惯性/地磁组合导航方法，其特征在于，所述s4中卡尔曼滤波器参数自适应更新的具体方式为：

5.根据权利要求1所述的基于地磁图适配性评估的自适应惯性/地磁组合导航方法，其特征在于，所述s5中根据地磁匹配输出的定位信息与惯性导航输出的参考位置获取k时刻的观测量表示为：

6.根据权利要求1所述的基于地磁图适配性评估的自适应惯性/地磁组合导航方法，其特征在于，所述s5中卡尔曼滤波器量测方程更新的具体方式为：

技术总结
本发明公开了一种基于地磁图适配性评估的自适应惯性/地磁组合导航方法，根据地磁基准图的适配性评估算法，对该匹配区域内的地磁匹配适配性进行评价，根据评价结果自适应改变组合导航滤波器的参数或周期，以提高组合导航系统的精度。解决了地磁匹配算法对匹配区域内的地磁场的变化程度较为敏感，在适配性较低的区域地磁匹配算法的精度可能会下降，甚至出现无法匹配的情况，严重影响组合导航系统的精度的问题。

技术研发人员：陈凯,梁文超,王志颖,曾诚之
受保护的技术使用者：西北工业大学
技术研发日：
技术公布日：2024/1/15