本发明涉及一种惯性导航4参数坐标变换矩阵解算方法，特别是一种惯性导航结合归一性和正交性的4参数坐标变换矩阵解算方法，属于惯性导航。
背景技术：
1、惯性导航广泛应用于航天器、飞机、船舶等领域，主要作用是实时确定载体相对导航系的位置、速度和姿态信息。捷联式惯性系统与载体直接固连，通过陀螺仪测量角速度并经数学解算后给出载体坐标系相对导航系的坐标变换矩阵。
2、目前，确定姿态信息的方法有方向余弦运动学方程、欧拉-克雷洛夫角运动学方程以及四元数运动学方程。
3、在工程中应用较多的四元数参数及其微分方程只有4个，但缺点是该4个参数只是中间变量，还需9个方程以求解出载体坐标系相对导航系的坐标变换矩阵。另外，由四元数描述的坐标变换矩阵与4个参数之间并不是唯一对应的关系，这是因为一个坐标变换矩阵可以对应两组不同的四元数。
4、相对而言，欧拉-克雷洛夫角运动学方程只有3个，但在本技术领域《惯性器件(上)》(中国宇航出版社)第46页中，认为用欧拉-克雷洛夫角描述的运动学方程存在奇点，方程会退化。中国专利cn202010333184.9中提出了一种基于克雷洛夫角的姿态解算方法，可以实现载体的全姿态运动描述，但面临的问题是，当俯仰角＝90°时，进行离散化处理时姿态解算的误差较大，从而导致解算的速度和位置误差较大。
5、用方向余弦阵表示两坐标系之间的转动运动关系和姿态矩阵的方法使用方便、直观、易于理解，但缺点是变换矩阵的参数及其微分方程为9个，积分计算量较大。在本技术领域《惯性器件(下)》(中国宇航出版社)第18章中给出了基于方向余弦运动学方程和四元数运动学方程的坐标变换矩阵解算方法。
6、在全姿态的前提下，为实现坐标变换矩阵与参数一一对应，同时解决方向余弦阵9个参数过多引起计算量大的问题，中国专利cn202210617894.3中提出了一种基于正交性的惯性导航5参数坐标变换矩阵解算方法，但该方法在进一步减少微分方程的个数并提高惯性导航及飞行控制的快速性和精度方面仍有提升的空间。
技术实现思路
1、本发明要解决的技术问题是：克服现有技术的不足，提供了一种惯性导航结合归一性和正交性的4参数坐标变换矩阵解算方法。
2、本发明目的通过以下技术方案予以实现：
3、一种惯性导航结合归一性和正交性的4参数坐标变换矩阵解算方法，以正交安装于捷联式惯性系统本体上的陀螺仪输出的角速率作为坐标变换矩阵4个参数运动学方程的输入信息，通过积分解算，实现惯性导航坐标变换矩阵的实时更新，在解算过程中不出现奇异值，保证了本体坐标系相对导航坐标系的全姿态、计算量小的要求。
4、具体包括：
5、计算得到捷联式导航系统本体坐标系相对导航坐标系在tk时刻的坐标变换矩阵；
6、计算在tk时刻本体坐标系相对于导航坐标系的角速度；
7、从所述坐标变换矩阵中选取4个参数；
8、根据所述坐标变换矩阵和角速度，对所述4个参数进行更新解算，得到更新后的坐标变换矩阵；
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、其中，ωxb、ωyb、ωzb分别为本体坐标系x、y、z轴上的角速度。
19、优选的，所述从坐标变换矩阵中选取9个参数中的4个参数，具体为：
20、对第三列的3个元素的绝对值|a13,k|、|a23,k|和|a33,k|进行排序，选取相对较小的2个元素作为参数，然后在第一列中选取对应行位置的2个元素作为另外2个参数。
21、优选的：
22、当|a13,k|在第三列中最大时，选取4个参数a21,k、a23,k、a31,k和a33,k；计算在下一时刻tk+1＝tk+δt经积分后a21,k+1、a23,k+1、a31,k+1和a33,k+1的值；
23、当|a23,k|在第三列中最大时，选取4个参数a11,k、a13,k、a31,k和a33,k；计算在下一时刻tk+1＝tk+δt经积分后a11,k+1、a13,k+1、a31,k+1和a33,k+1的值；
24、当|a33,k|在第三列中最大时，选取4个参数a11,k、a13,k、a21,k和a23,k；计算在下一时刻tk+1＝tk+δt经积分后a11,k+1、a13,k+1、a21,k+1和a23,k+1的值。
25、优选的，当|a13,k|在第三列中最大时，选取4个参数a21,k、a23,k、a31,k和a33,k；在下一时刻tk+1＝tk+δt经积分后的值a21,k+1、a23,k+1、a31,k+1和a33,k+1，具体计算公式为：
26、
27、sign()为取符号函数；
28、积分计算采用三角函数解算，当时，在下一时刻tk+1＝tk+δt经积分后的值a21,k+1、a23,k+1、a31,k+1和a33,k+1，计算公式为：
29、
30、式中，为角速度计算采样时间δt内的角增量；
31、把a21,k+1、a23,k+1、a31,k+1和a33,k+1代入下式，求解出tk+1时刻的a11,k+1、a13,k+1为
32、
33、把a21,k+1、a23,k+1、a31,k+1和a33,k+1代入下式，求解出tk+1时刻的从导航坐标系至本体坐标系的坐标变换矩阵为
34、
35、优选的，当|a23,k|在第三列中最大时，选取4个参数a11,k、a13,k、a31,k和a33,k；在下一时刻tk+1＝tk+δt经积分后的值a11,k+1、a13,k+1、a31,k+1和a33,k+1，计算公式为：
36、
37、sign()为取符号函数；
38、积分计算采用三角函数解算，当时，在下一时刻tk+1＝tk+δt经积分后的值a11,k+1、a13,k+1、a31,k+1和a33,k+1，计算公式为：
39、
40、式中，为角速度计算采样时间δt内的角增量；
41、把a11,k+1、a13,k+1、a31,k+1和a33,k+1代入下式，求解出tk+1时刻的a21,k+1、a23,k+1为
42、
43、把a11,k+1、a13,k+1、a31,k+1和a33,k+1代入下式，求解出tk+1时刻的从导航坐标系至本体坐标系的坐标变换矩阵为
44、
45、优选的，当|a33,k|在第三列中最大时，选取4个参数a11,k、a13,k、a21,k和a23,k；在下一时刻tk+1＝tk+δt经积分后的值a11,k+1、a13,k+1、a21,k+1和a23,k+1，计算公式为：
46、
47、sign()为取符号函数；
48、积分计算采用三角函数解算，当时，在下一时刻tk+1＝tk+δt经积分后的值a11,k+1、a13,k+1、a21,k+1和a23,k+1，计算公式为：
49、
50、
51、式中，为角速度计算采样时间δt内的角增量；
52、把a11,k+1、a13,k+1、a21,k+1和a23,k+1代入下式，求解出tk+1时刻的a31,k+1、a33,k+1为
53、
54、把a13,k+1、a21,k+1、a23,k+1和a31,k+1代入下式，求解出tk+1时刻的从导航坐标系至本体坐标系的坐标变换矩阵为：
55、
56、优选的，根据所述更新后的坐标变换矩阵，进行速度更新和位置更新，具体为：
57、当p为惯性系时，导航方程为：
58、
59、其中，为更新后的坐标变换矩阵，为视加速度，为重力加速度，v为更新后的速度，t为矩阵的转置，r为更新后的位置。
60、一种存储在非暂时性计算机可读介质上的计算机程序产品，所述计算机程序产品包括用于实行上述4参数坐标变换矩阵解算方法的程序代码。
61、本发明相比于现有技术具有如下有益效果：
62、(1)本发明公开了惯性导航基于归一化的4参数坐标变换矩阵解算方法，相对于9参数的方向余弦法少了5个微分方程，减小了计算量；
63、(2)本发明公开了惯性导航基于归一化的4参数坐标变换矩阵解算方法，相对于四元数表征的坐标变换矩阵，具有4个参数与坐标变换矩阵之间的关系具有唯一性；
64、(3)本发明公开了惯性导航基于归一化的4参数坐标变换矩阵解算方法，相对于3个克雷洛夫角表征的坐标变换矩阵，可实现全姿态无奇异高精度解算；
65、(4)本发明公开了惯性导航基于归一化的4参数坐标变换矩阵解算方法，结构简单，易于工程实现。
1.一种惯性导航结合归一性和正交性的4参数坐标变换矩阵解算方法，其特征在于，包括：
2.根据权利要求1所述的4参数坐标变换矩阵解算方法，其特征在于，捷联式导航系统本体坐标系相对导航坐标系的坐标变换矩阵为：
3.根据权利要求2所述的4参数坐标变换矩阵解算方法，其特征在于，tk时刻本体坐标系相对于导航坐标系的角速度，具体为：
4.根据权利要求3所述的4参数坐标变换矩阵解算方法，其特征在于，所述从坐标变换矩阵中选取9个参数中的4个参数，具体为：
5.根据权利要求4所述的4参数坐标变换矩阵解算方法，其特征在于：
6.根据权利要求5所述的4参数坐标变换矩阵解算方法，其特征在于，当|a13,k|在第三列中最大时，选取4个参数a21,k、a23,k、a31,k和a33,k；在下一时刻tk+1＝tk+δt经积分后的值a21,k+1、a23,k+1、a31,k+1和a33,k+1，具体计算公式为：
7.根据权利要求1所述的4参数坐标变换矩阵解算方法，其特征在于，当|a23,k|在第三列中最大时，选取4个参数a11,k、a13,k、a31,k和a33,k；在下一时刻tk+1＝tk+δt经积分后的值a11,k+1、a13,k+1、a31,k+1和a33,k+1，计算公式为：
8.根据权利要求1所述的4参数坐标变换矩阵解算方法，其特征在于，当|a33,k|在第三列中最大时，选取4个参数a11,k、a13,k、a21,k和a23,k；在下一时刻tk+1＝tk+δt经积分后的值a11,k+1、a13,k+1、a21,k+1和a23,k+1，计算公式为：
9.根据权利要求1所述的4参数坐标变换矩阵解算方法，其特征在于，根据所述更新后的坐标变换矩阵，进行速度更新和位置更新，具体为：
10.一种存储在非暂时性计算机可读介质上的计算机程序产品，所述计算机程序产品包括用于实行如权利要求1至9中任一项所述的方法的程序代码。
