一种基于归一性的惯性导航5参数坐标变换矩阵解算方法与流程

本发明涉及一种基于归一性的惯性导航5参数坐标变换矩阵解算方法，属于惯性导航。

背景技术：

1、惯性导航广泛应用于航天器、飞机、舰船等领域，主要作用是实时确定载体相对导航系的位置、速度和姿态信息。捷联式惯性系统与载体直接固连，通过陀螺仪测量角速度并经数学解算后给出载体坐标系相对导航系的坐标变换矩阵。

2、目前，确定姿态信息的方法有方向余弦运动学方程、欧拉-克雷洛夫角运动学方程以及四元数运动学方程。

3、在工程中应用较多的四元数参数及其微分方程只有4个，但缺点是该4个参数只是中间变量，还需9个方程以求解出载体坐标系相对导航系的坐标变换矩阵。另外，由四元数描述的坐标变换矩阵与4个参数之间并不是唯一对应的关系，这是因为一个坐标变换矩阵可以对应两组不同的四元数。

4、相对而言，欧拉-克雷洛夫角运动学方程只有3个，但在本技术领域《惯性器件(上)》(中国宇航出版社)第46页中，认为用欧拉-克雷洛夫角描述的运动学方程存在奇点，方程会退化。中国专利cn202010333184.9中提出了一种基于克雷洛夫角的姿态解算方法，可以实现载体的全姿态运动描述。但面临的问题是，当俯仰角＝90°时，进行离散化处理时姿态解算的误差较大，从而导致解算的速度和位置误差较大。

5、用方向余弦阵表示两坐标系之间的转动运动关系和姿态矩阵的方法使用方便、直观、易于理解，但缺点是变换矩阵的参数及其微分方程为9个，积分计算量较大。在本技术领域《惯性器件(下)》(中国宇航出版社)第18章中给出了基于方向余弦运动学方程和四元数运动学方程的坐标变换矩阵解算方法。

6、在全姿态的前提下，为实现坐标变换矩阵与参数一一对应，同时解决方向余弦阵9个参数过多引起计算量大的问题，中国专利cn202210617894.3中提出了一种基于正交性的惯性导航5参数坐标变换矩阵解算方法，但需要在6个条件中选择其一作为更新方程。

技术实现思路

1、本发明要解决的技术问题是：克服现有技术的不足，提供了一种基于归一性的惯性导航5参数坐标变换矩阵解算方法，简化了判断条件和减小计算量，提高了惯性导航和飞行控制的快速性。

2、本发明目的通过以下技术方案予以实现：

3、一种基于归一性的惯性导航5参数坐标变换矩阵解算方法，以正交安装于捷联式惯性系统本体上的陀螺仪输出的角速率作为坐标变换矩阵5个参数运动学方程的输入信息，通过积分解算，实现惯性导航坐标变换矩阵的实时更新，在解算过程中不出现奇异值，保证了本体坐标系相对导航坐标系的全姿态、计算量小的要求。

4、具体包括：

5、计算得到捷联式导航系统本体坐标系相对导航坐标系在tk时刻的坐标变换矩阵；

6、计算在tk时刻本体坐标系相对于导航坐标系的角速度；

7、从所述坐标变换矩阵中选取5个参数；

8、根据所述坐标变换矩阵和角速度，对所述5个参数进行更新解算，得到更新后的坐标变换矩阵；

9、根据所述更新后的坐标变换矩阵，进行速度更新和位置更新。

10、进一步地，在上述解算方法中，捷联式导航系统本体坐标系相对导航坐标系的坐标变换矩阵为

11、

12、式中，a11、a12、a13、a21、a22、a23、a31、a32和a33为坐标变换矩阵的9个参数。

13、在tk时刻捷联式导航系统本体坐标系相对导航坐标系的坐标变换矩阵为

14、

15、式中，a11,k、a12,k、a13,k、a21,k、a22,k、a23,k、a31,k、a32,k和a33,k为在tk时刻坐标变换矩阵的9个参数。

16、进一步地，在上述解算方法中，计算在tk时刻本体坐标系相对于导航坐标系的角速度，具体为：

17、

18、其中，分别为本体x、y、z轴上的角速度。

19、进一步地，在上述解算方法中，所述从坐标变换矩阵中选取9个参数中的5个参数，具体为：

20、选取坐标变换矩阵中a11,k、a21,k和a31,k，以及在3个元素a13,k、a23,k和a33,k中绝对值最大值被排除后的其余2个元素作为5个参数。

21、进一步地，在上述解算方法中，根据所述坐标变换矩阵和角速度，对所述5个参数进行更新解算，并解算出更新后的坐标变换矩阵，具体为：

22、当|a13,k|最大时，选取5个参数a11,k、a21,k、a23,k、a31,k和a33,k；在下一时刻tk+1＝tk+δt经积分后的值a11,k+1、a21,k+1、a23,k+1、a31,k+1和a33,k+1；

23、当|a23,k|最大时，选取5个参数a11,k、a13,k、a21,k、a31,k和a33,k；在下一时刻tk+1＝tk+δt经积分后的值a11,k+1、a13,k+1、a21,k+1、a31,k+1和a33,k+1；

24、当|a33,k|最大时，选取5个参数a11,k、a13,k、a21,k、a23,k和a31,k；在下一时刻tk+1＝tk+δt经积分后的值a11,k+1、a13,k+1、a21,k+1、a23,k+1和a31,k+1；

25、进一步地，在上述解算方法中，当|a13,k|最大时，选取5个参数a11,k、a21,k、a23,k、a31,k和a33,k；在下一时刻tk+1＝tk+δt经积分后的值a11,k+1、a21,k+1、a23,k+1、a31,k+1和a33,k+1，具体计算公式为：

26、

27、把tk+1时刻的a11,k+1、a21,k+1、a23,k+1、a31,k+1和a33,k+1值代入下式，求解出从导航坐标系至本体坐标系的坐标变换矩阵为

28、

29、进一步地，在上述解算方法中，所述当|a23,k|最大时，选取5个参数a11,k、a13,k、a21,k、a31,k和a33,k；在下一时刻tk+1＝tk+δt经积分后的值a11,k+1、a13,k+1、a21,k+1、a31,k+1和a33,k+1，计算公式为：

30、

31、把tk+1时刻的a11,k+1、a12,k+1、a13,k+1、a31,k+1和a32,k+1值代入下式，求解出从导航坐标系至本体坐标系的坐标变换矩阵为：

32、

33、进一步地，在上述解算方法中，所述当|a33,k|最大时，选取5个参数a11,k、a13,k、a21,k、a23,k和a31,k；在下一时刻tk+1＝tk+δt经积分后的值a11,k+1、a13,k+1、a21,k+1、a23,k+1和a31,k+1，计算公式为：

34、

35、把tk+1时刻的a11,k+1、a12,k+1、a21,k+1、a22,k+1和a23,k+1值代入下式，求解出从导航坐标系至本体坐标系的坐标变换矩阵为

36、

37、进一步地，在上述解算方法中，所述根据所述更新后的坐标变换矩阵，进行速度更新和位置更新，具体为：

38、当p为惯性系时，导航方程为：

39、

40、其中，为更新后的坐标变换矩阵，为视加速度，为重力加速度，v为更新后的速度，t为矩阵的转置，r为更新后的位置。

41、本发明相比于现有技术具有如下有益效果：

42、(1)本发明公开了基于归一性的惯性导航5参数坐标变换矩阵解算方法，相对于基于正交性的惯性导航5参数坐标变换矩阵解算方法，判断条件由6个降为3个，减小了计算量；

43、(2)本发明公开了惯性导航5参数坐标变换矩阵解算方法，相对于9参数的方向余弦法少了4个微分方程，相对于四元数表征的坐标变换矩阵，具有5个参数与坐标变换矩阵之间的关系具有唯一性；

44、(3)本发明公开了惯性导航5参数坐标变换矩阵解算方法，相对于3个克雷洛夫角表征的坐标变换矩阵，可实现全姿态无奇异高精度解算；

45、(4)本发明公开了惯性导航5参数坐标变换矩阵解算方法，结构简单，易于工程实现。