惯性导航5参数坐标变换矩阵解算方法与流程

本发明属于航空、航天，尤其涉及惯性导航5参数坐标变换矩阵解算方法。

背景技术：

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

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

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

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

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

6、在全姿态的前提下，为实现坐标变换矩阵与参数一一对应，同时解决方向余弦阵9个参数过多引起计算量大的问题，需要研究一种新的载体坐标系相对导航系的解算方法，以提高惯性导航和飞行控制的快速性和精度。

技术实现思路

1、本发明的技术解决问题：克服现有技术的不足，提供惯性导航5参数坐标变换矩阵解算方法，以正交安装于捷联式惯性系统本体上的陀螺仪输出的角速率作为坐标变换矩阵5个参数运动学方程的输入信息，通过积分解算、归一化和正交化处理，实现惯性导航坐标变换矩阵的实时更新，在解算过程中不出现奇异值，保证了本体坐标系相对导航坐标系的全姿态、计算量小的要求。

2、本发明的技术方案为：

3、本发明公开了惯性导航5参数坐标变换矩阵解算方法，包括：

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

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

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

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

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

9、进一步地，在上述解算方法中，所述计算得到捷联式导航系统本体坐标系相对导航坐标系在tk时刻的坐标变换矩阵，具体为：

10、

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

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

13、

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

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

16、选取坐标变换矩阵中a13,k、a23,k和a33,k的绝对值中最大值在中对应的行中3个元素，以及最小值在中对应的行中其它2个元素作为5个参数。

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

18、当|a13,k|最大、|a23,k|最小时，选取5个参数a11,k、a12,k、a13,k、a21,k和a22,k；在下一时刻tk+1＝tk+δt经积分后的值b11,k+1、b12,k+1、b13,k+1、b21,k+1和b22,k+1；

19、当|a13,k|最大、|a33,k|最小时，选取5个参数a11,k、a12,k、a13,k、a31,k和a32,k；在下一时刻tk+1＝tk+δt经积分后的值b11,k+1、b12,k+1、b13,k+1、b31,k+1和b32,k+1；

20、当|a23,k|最大、|a13,k|最小时，选取5个参数a11,k、a12,k、a21,k、a22,k和a23,k；在下一时刻tk+1＝tk+δt经积分后的值b11,k+1、b12,k+1、b21,k+1、b22,k+1和b23,k+1；

21、当|a23,k|最大、|a33,k|最小时，选取5个参数a21,k、a22,k、a23,k、a31,k和a32,k；在下一时刻tk+1＝tk+δt经积分后的值b21,k+1、b22,k+1、b23,k+1、b31,k+1和b32,k+1；

22、当|a33,k|最大、|a13,k|最小时，选取5个参数a11,k、a12,k、a31,k、a32,k和a33,k；在下一时刻tk+1＝tk+δt经积分后的值b11,k+1、b12,k+1、b31,k+1、b32,k+1和b33,k+1；

23、当|a33,k|最大、|a23,k|最小时，选取5个参数a21,k、a22,k、a31,k、a32,k和a33,k；在下一时刻tk+1＝tk+δt经积分后的值b21,k+1、b22,k+1、b31,k+1、b32,k+1和b33,k+1。

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

25、

26、把b11,k+1、b12,k+1、b13,k+1、b21,k+1和b22,k+1代入下式，求解出tk+1时刻的a11,k+1、a12,k+1、a13,k+1、a21,k+1和a22,k+1值，以及从导航坐标系至本体坐标系的坐标变换矩阵为

27、

28、式中，

29、

30、

31、

32、

33、

34、其中，δt为采样时间。

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

36、

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

38、

39、式中，

40、

41、

42、

43、

44、

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

46、

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

48、

49、式中，

50、

51、

52、

53、

54、

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

56、

57、把b21,k+1、b22,k+1、b23,k+1、b31,k+1和b32,k+1代入下式，求解出tk+1时刻的a21,k+1、a22,k+1、a23,k+1、a31,k+1和a32,k+1值，以及从导航坐标系至本体坐标系的坐标变换矩阵为：

58、

59、式中，

60、

61、

62、

63、

64、

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

66、

67、把b11,k+1、b12,k+1、b31,k+1、b32,k+1和b33,k+1代入下式，求解出tk+1时刻的a11,k+1、a12,k+1、a31,k+1、a32,k+1和a33,k+1值，以及从导航坐标系至本体坐标系的坐标变换矩阵为

68、

69、式中，

70、

71、

72、

73、

74、

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

76、

77、把b11,k+1、b12,k+1、b31,k+1、b32,k+1和b33,k+1代入下式，求解出tk+1时刻的a11,k+1、a12,k+1、a31,k+1、a32,k+1和a33,k+1值，以及从导航坐标系至本体坐标系的坐标变换矩阵为：

78、

79、式中，

80、

81、

82、

83、

84、

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

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

87、

88、

89、

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

91、本发明具有以下优点：

92、(1)本发明公开了惯性导航5参数坐标变换矩阵解算方法，相对于9参数的方向余弦法少了4个微分方程，减小了计算量；

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

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

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