惯性基组合导航系统误差状态模型修正与优化方法和系统

本发明属于组合导航控制领域，更具体地，涉及惯性基组合导航系统误差状态模型修正与优化方法和系统。

背景技术：

1、为解决传统组合导航系统误差状态模型的线性化误差问题，近年来st-ekf受到了导航领域学者的关注。st-ekf考虑了姿态误差(即失准角)导致的计算导航坐标系和真实导航坐标系之间的偏差，重新定义了非线性的速度误差状态，在大失准角的情况下，相比传统的滤波算法具有更好的估计性能。

2、然而，导航坐标系的建立是以测者位置坐标为原点，计算导航坐标系就是以sins(strapdown inertial navigation system，捷联惯性导航系统)的输出位置为原点，因此，位置误差角同样会导致计算导航坐标系偏离真实导航坐标系。尤其在位置误差较大的情况下，会对组合导航系统状态估计性能产生不利影响。

技术实现思路

1、针对现有技术的缺陷，本发明的目的在于提供惯性基组合导航系统误差状态模型修正与优化方法和系统，旨在解决现有技术面对适用于位置误差较大的情形，组合导航系统状态估计性能受到不利影响的问题。

2、为实现上述目的，第一方面，本发明提供了一种惯性基组合导航系统误差状态模型修正与优化方法，包括：

3、s1.获取基于kf的惯性基组合导航系统的误差状态和误差状态模型，所述误差状态包括失准角、速度误差和位置误差，所述误差状态模型包括姿态误差方程、速度误差方程和位置误差方程；

4、s2.将位置误差转换为位置误差角，以失准角和位置误差角对速度误差进行修正，获得修正后的非线性速度误差状态；

5、s3.以修正后的非线性速度误差状态为基础，对组合导航系统的误差状态模型进行修正和优化。

6、优选地，优化后非线性速度误差状态δvγ表示如下：

7、

8、其中，

9、

10、θ＝n4δp

11、

12、δvn为基于kf的速度误差状态，vn为导航系中载体的速度，上标～表示对应变量的估计量，φ为失准角，θ为位置误差角，(·×)为反对称矩阵运算符，n4为中间矩阵，无实际物理含义，δp为位置误差状态，l为纬度。

13、优选地，优化后姿态误差方程表示如下：

14、

15、其中，

16、

17、

18、

19、

20、上标·表示一阶导数，n1、n2、n3、n4分别为第一至第四中间矩阵，无实际物理含义，vn为导航系中载体的速度，上标～表示对应变量的估计量，(·×)为反对称矩阵运算符，为导航系相对惯性系的角速度，φ为失准角，δvγ为修正后的非线性速度误差状态，δp为位置误差状态，为陀螺测量误差，l为纬度，h为高度，rm为子午圈主曲率半径，rn为卯酉圈主曲率半径，ωie为地球自转角速度，ve为东向速度，vn为北向速度。

21、优选地，优化的速度误差方程表示如下：

22、

23、其中，

24、

25、

26、

27、

28、上标·表示一阶导数，n1、n2、n3、n4为中间矩阵，无实际物理含义，vn为导航系中载体的速度，上标～表示对应变量的估计量，(·×)为反对称矩阵运算符，为地球自转角速度在导航系中的投影，为重力在导航系的投影，φ为失准角，为导航系相对于地球系的角速度在导航系的投影，δvγ为修正后的非线性速度误差状态，为导航系到载体系的转换矩阵，δp为位置误差状态，为比力，为陀螺测量误差，为加速度计测量的比力误差，l为纬度，h为高度，rm为子午圈主曲率半径，rn为卯酉圈主曲率半径，ωie为地球自转角速度，ve为东向速度，vn为北向速度。

29、优选地，修正后的位置误差方程表示如下：

30、

31、其中，

32、

33、

34、

35、上标·表示一阶导数，npv、n4、npp为中间矩阵，无实际物理含义，vn为导航系中载体的速度，上标～表示对应变量的估计量，(·×)为反对称矩阵运算符，δvγ为修正后的非线性速度误差状态，δp为位置误差状态，l为纬度，h为高度，rm为子午圈主曲率半径，rn为卯酉圈主曲率半径，ωie为地球自转角速度，ve为东向速度，vn为北向速度。

36、为实现上述目的，第二方面，本发明提供了一种惯性基组合导航系统误差状态模型修正与优化系统，包括：处理器和存储器；所述存储器，用于存储计算机执行指令；所述处理器，用于执行所述计算机执行指令，使得第一方面所述的方法被执行。

37、为实现上述目的，第三方面，本发明提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，当所述计算机程序在处理器上运行时，使得所述处理器执行第一方面所述的方法。

38、总体而言，通过本发明所构思的以上技术方案与现有技术相比，具有以下有益效果：

39、本发明提供惯性基组合导航系统误差状态模型修正与优化方法和系统，在当地组合导航坐标系中，综合考虑失准角和位置误差角对计算导航坐标系和真实导航坐标系失配问题的影响，重新定义了经过失准角和位置误差角修正的非线性速度误差状态，在此基础上，推导建立了新的惯性基组合导航系统误差状态模型。本发明相比st-ekf，不仅考虑了失准角造成的线性化误差的影响，而且增加了位置误差角的修正项，进一步完善了惯性基组合导航系统误差状态模型优化理论，有效提升惯性基组合导航系统状态估计的性能和环境适用性，尤其在初始状态误差较大的情况下，优越性更为显著。

技术特征：

1.一种惯性基组合导航系统误差状态模型修正与优化方法，其特征在于，包括：

2.如权利要求1所述的方法，其特征在于，优化后非线性速度误差状态δvγ表示如下：

3.如权利要求1所述的方法，其特征在于，优化后姿态误差方程表示如下：

4.如权利要求1所述的方法，其特征在于，优化的速度误差方程表示如下：

5.如权利要求1所述的方法，其特征在于，修正后的位置误差方程表示如下：

6.一种惯性基组合导航系统误差状态模型修正与优化系统，其特征在于，包括：处理器和存储器；

7.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，当所述计算机程序在处理器上运行时，使得所述处理器执行权利要求1至5任一项所述的方法。

技术总结
本发明公开了惯性基组合导航系统误差状态模型修正与优化方法和系统，属于组合导航控制领域。在当地组合导航坐标系中，综合考虑失准角和位置误差角对计算导航坐标系和真实导航坐标系失配问题的影响，重新定义了经过失准角和位置误差角修正的非线性速度误差状态，在此基础上，推导建立了新的惯性基组合导航系统误差状态模型。本发明相比ST‑EKF，不仅考虑了失准角造成的线性化误差的影响，而且增加了位置误差角的修正项，进一步完善了惯性基组合导航系统误差状态模型优化理论，有效提升惯性基组合导航系统状态估计的性能和环境适用性，尤其在初始状态误差较大的情况下，优越性更为显著。

技术研发人员：唐君,卞鸿巍,王荣颖,马恒,文者,胡耀金,丁贤
受保护的技术使用者：中国人民解放军海军工程大学
技术研发日：
技术公布日：2024/1/13