一种基于李群的发射系惯导/卫导松耦合组合导航方法

本发明涉及飞行器导航领域，具体涉及一种基于李群的发射系惯导/卫导松耦合组合导航方法。

背景技术：

1、飞行器在初始对准时，如果出现恶劣情况，无法保证初始对准的精度，将出现大失准角进入组合导航的情况。如制导炮弹存在因舵机卡滞或空中对准精度差导致大失准角进入组合导航过程的情况，这对组合导航算法性能是巨大的考验。因此，研究大失准角误差下的组合导航算法是该领域的技术难点。

2、传统组合导航方法滤波模型是基于小扰动误差假设通过线性化过程建立的，而大失准角误差下线性化的假设可能不再满足，导致组合导航滤波器无法收敛。本发明提出的一种基于李群的发射系惯导/卫导松耦合组合导航方法，利用发射坐标系(发射系)捷联惯导机械编排姿态速度和位置之间是松散耦合的优势，将姿态、速度和位置状态量纳入到一个李群中，构建新的组合导航方法模型，提高惯性卫星组合导航方法的性能。

技术实现思路

1、针对现有技术中的上述不足，本发明提供了一种基于李群的发射系惯导/卫导松耦合组合导航方法。

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

3、一种基于李群的发射系惯导/卫导松耦合组合导航方法，包括如下步骤：

4、s1、基于发射坐标系捷联惯导机械编排将姿态矩阵、辅助速度矢量和位置矢量纳入一个李群中构成李群状态量，按左误差模型建立基于李群的发射坐标系捷联惯导算法编排；

5、s2、根据所建立的发射系捷联惯导算法编排，建立基于李群的发射系捷联惯导误差模型，根据所构建的误差模型得到姿态误差、速度误差和位置误差；

6、s3、将失准角误差、速度误差、位置误差、加速度计零偏构成多维状态量建立状态方程，同时将惯性导航系统和卫星导航系统得到的辅助速度矢量和位置矢量做差得到量测矢量，根据量测矢量建立量测方程；

7、s4、对状态方程和量测方程进行离散化；

8、s5、利用滤波递推算法计算下一时刻的状态，并重复滤波直至收敛状态得到姿态、速度、位置、陀螺仪漂移和加速度计漂移的估算误差值；

9、s6、根据所得到的失准角误差、速度误差和位置误差的估计值对惯性导航结算的结果进行校正。

10、进一步的，所述s1中李群状态量表示为：

11、

12、其中，χ为李群状态量，为飞行器在发射坐标系下的姿态矩阵，为发射坐标系下辅助速度矢量，pg为发射坐标系下飞行器的位置，se2(3)为扩展姿态的双直空间等距群，01×3为1×3阶零矩阵。

13、进一步的，所述s2中基于李群的发射系捷联惯导误差模型表示为：

14、

15、其中，表示惯性导航系统解算的李群状态量的逆，τ为基于李群的发射系捷联惯导误差模型，和pg依次为发射坐标系下的姿态矩阵、辅助速度矢量和位置矢量；和依次为由惯性导航系统解算得到的发射坐标系下含误差的姿态矩阵、辅助速度矢量和位置矢量；τa、τv和τp依次为发射系捷联惯导误差模型中的姿态、速度和位置误差；se2(3)为扩展姿态的双直空间等距群，01×3为1×3阶零矩阵。

16、进一步的，所述s2中根据所构建的误差模型得到姿态误差、速度误差和位置误差分别表示为：

17、

18、

19、

20、其中，φ×为失准角误差的反对称矩阵，δpg依次为发射坐标系下辅助速度误差和位置误差。

21、进一步的，所述s3中状态方程表示为：

22、

23、其中，x为状态矢量且

24、

25、式中，φ为失准角误差，τa、τv和τp依次为发射系捷联惯导误差模型中的姿态、速度和位置误差，εb为陀螺仪零偏，为加速度计零偏；

26、f为系统矩阵且

27、

28、式中，为陀螺仪测量的角速度的反对称矩阵，fb为加速度计测量比力的反对称矩阵，03×3为3×3阶零矩阵，i3×3为3维单位矩阵；

29、g为噪声转移矩阵且

30、

31、w为过程噪声矢量且

32、

33、式中，wg为陀螺仪角速度测量白噪声，wa为加速度计比力测量白噪声。

34、进一步的，所述s3中量测方程表示为：

35、z＝hx+v

36、其中，z为量测矢量且

37、

38、式中，和依次为由惯性导航系统解算得到的发射坐标系下含误差的辅助速度矢量和位置矢量；和pg依次为发射坐标系下的辅助速度矢量和位置矢量；δpg依次为发射坐标系下辅助速度误差和位置误差；

39、h为量测矩阵且

40、

41、式中，为飞行器在发射坐标系下的姿态矩阵，03×3为3×3阶零矩阵；

42、v为量测噪声矢量且

43、

44、式中，vv、vp依次为卫星导航系统速度测量白噪声和位置测量白噪声，为发射坐标系相对于发射惯性坐标系的旋转角速度的反对称矩阵。

45、进一步的，所述s4中状态方程和量测方程的离散化分别表示为：

46、xk＝φk/k-1xk-1+γk/k-1wk-1

47、zk＝hkxk+vk

48、其中，xk是k时刻的状态矢量，xk-1是k-1时刻的状态矢量，wk-1是k-1时刻的过程噪声矢量，zk为k时刻的位置量测矢量，hk为k时刻的观测矩阵，vk为k时刻的量测噪声向量，φk/k-1与γk/k-1为状态方程和噪声驱动矩阵的离散化。

49、

50、

51、其中，f(tn)为tn时刻的状态转移矩阵，f(tk)和g(tk)为tk时刻的状态转移矩阵和噪声驱动矩阵，t为离散化的周期，n为离散化的阶数。

52、进一步的，所述s5中利用滤波递推算法计算下一时刻的状态具体方式为：

53、s51、根据初始状态进行状态一步预测

54、

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

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

57、

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

59、

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

61、

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

63、

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

65、s54、经过重复计算n次得到的收敛值，即姿态误差值速度误差值位置误差值陀螺漂移误差值加速度计漂移误差值

66、进一步的，所述s6中对惯性导航结算的结果进行校正后的结果为：

67、

68、其中，依次为k时刻校正后的姿态矩阵、辅助速度矢量和位置矢量；依次为k时刻惯性导航解算的姿态矩阵、辅助速度矢量和位置矢量；依次为卡尔曼滤波后失准角误差、速度误差和位置误差的估计值。

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

70、利用发射坐标系(发射系)捷联惯导机械编排姿态速度和位置之间是松散耦合的优势，将姿态、速度和位置状态量纳入到一个李群中，构建新的组合导航方法模型，提高惯性卫星组合导航方法的性能。