本发明属于惯性导航技术领域，涉及一种基于捷联惯导的双欧拉全姿态解算方法，该方法通过更新姿态矩阵，简化了惯导系统姿态更新的计算过程；通过反欧拉角的定义域调整，克服了反欧拉法角输出值存在理解歧义的缺陷。
背景技术：
捷联惯导系统解算姿态的过程就是计算实时欧拉角的过程。当俯仰角p处于±90°附近时，滚转角r和航向角h出现耦合，无法求得欧拉角r和h的值，因此需要双欧拉法解决这一问题。传统双欧拉法会分别计算正欧拉角和反欧拉角的微分方程，并进行正反欧拉角的相互转换，最后利用精华区切换选择合适的欧拉角输出。然而，解算两种欧拉角的微分方程会导致计算量翻倍，同时由于反欧拉角pr的定义域处于[-180°， 180°]范围内，用户会对处于[-180°，-90°]以及[ 90°， 180°]范围内的反欧拉角输出值存在理解上的歧义。
技术实现要素：
(一)发明目的
本发明的目的是：提供一种基于捷联惯导的双欧拉全姿态解算方法，实现对欧拉角的准确求解。
(二)技术方案
为了解决上述技术问题，本发明提供一种基于捷联惯导的双欧拉全姿态解算方法，包括4个步骤，具体实施如下：
步骤一姿态矩阵的更新
设tk时刻的载体坐标系为p(k)，导航坐标系为n(k)，tk 1时刻的载体坐标系为p(k 1)，导航坐标系为n(k 1)。记p(k)至p(k 1)的转动四元数为q(h)，n(k)至n(k 1)的转动四元数为p(h)，n(k)至p(k)的转动四元数为q(tk)，n(k 1)至p(k 1)的转动四元数为q(tk 1)，其中h＝tk 1-tk为更新周期。
对于空间中任一矢量r，用姿态矩阵描述，有
其中，rn(k 1)为tk 1时刻导航坐标系的矢量，rp(k 1)为tk 1时刻载体坐标系的矢量，
根据四元数与姿态矩阵的等效关系，可以得到
又因为
于是得到
其中，
因此tk 1时刻的
其中，各参数含义
步骤二利用姿态矩阵求解正反欧拉角
正、反欧拉角的姿态矩阵
为正欧拉角对应的姿态矩阵的形式。
为反欧拉角对应的姿态矩阵的形式。
其中，p、r、h分别表示正欧拉俯仰角、滚转角和航向角；
其中，pr、rr、hr分别表示反欧拉俯仰角、滚转角和航向角；
(1)无效姿态处理
求解正欧拉角时，当俯仰角p为±90°时，航向角h与滚转角r均为无意义的值，要对h和r赋零值。同理，求解反欧拉角时，当滚转角rr为±90°时，俯仰角pr与航向角hr均为无意义的值，要对pr和hr赋零值。
(2)常规姿态求解
正、反欧拉角对应的姿态矩阵虽然形式不同，但矩阵对应元素的值相等。常规姿态下，p、rr处于定义域(-90°, 90°)，可根据
求解正欧拉角，p可通过
求解反欧拉角，rr可通过
步骤三对反欧拉角进行定义域调整
定义域调整的目的是把反欧拉俯仰角pr的范围从[-180°， 180°]调整到[-90°, 90°]。具体方法是对上一步计算出来的pr取正方向的补角，对hr和rr取反方向的补角。若pr大于 90°，则pr＝ 180°–pr；若pr小于-90°，则pr＝-180°–pr。当pr不处于[-90°, 90°]范围时，若hr<0，则hr＝ 180° hr；若hr>0，则hr＝-180° hr；同理，若rr<0，则rr＝ 180° rr；若rr>0，则rr＝-180° rr。
步骤四正、反欧拉角精华区切换
双欧拉法输出正欧拉角还是反欧拉角取决于正欧拉的俯仰角p的范围，具体实现如下：
如果|p|<＝45°，则输出正欧拉角(r,p,h)表示姿态；
否则，输出反欧拉角(rr,pr,hr)表示姿态。
(三)有益效果
上述技术方案所提供的基于捷联惯导的双欧拉全姿态解算方法，在捷联惯导系统输出全姿态欧拉角的基础上，利用正欧拉角和反欧拉角的姿态矩阵对应元素值相同的特点，使用姿态矩阵代替正反欧拉角的微分方程实现姿态更新。同时，在求解反欧拉角的过程中将反欧拉俯仰角pr的定义域调整到[-90°， 90°]，对反欧拉滚转角rr和反欧拉航向角hr的输出值进行适应性调整，该方法不仅降低了计算全姿态双欧拉角的复杂度，也解决了反欧拉角输出值可能存在歧义的问题。
附图说明
图1是正、反欧拉角精华区切换示意图。
具体实施方式
为使本发明的目的、内容、和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。
本发明基于捷联惯导的双欧拉全姿态解算方法包括4个步骤，具体实施如下：
步骤一姿态矩阵的更新
设tk时刻的载体坐标系为p(k)，导航坐标系为n(k)，tk 1时刻的载体坐标系为p(k 1)，导航坐标系为n(k 1)。记p(k)至p(k 1)的转动四元数为q(h)，n(k)至n(k 1)的转动四元数为p(h)，n(k)至p(k)的转动四元数为q(tk)，n(k 1)至p(k 1)的转动四元数为q(tk 1)，其中h＝tk 1-tk为更新周期。
对于空间中任一矢量r，用姿态矩阵描述，有
其中，rn(k 1)为tk 1时刻导航坐标系的矢量，rp(k 1)为tk 1时刻载体坐标系的矢量，
根据四元数与姿态矩阵的等效关系，可以得到
又因为
于是得到
其中，
因此tk 1时刻的
其中，各参数含义
步骤二利用姿态矩阵求解正反欧拉角
正、反欧拉角的姿态矩阵
为正欧拉角对应的姿态矩阵的形式。
为反欧拉角对应的姿态矩阵的形式。
其中，p、r、h分别表示正欧拉俯仰角、滚转角和航向角；
其中，pr、rr、hr分别表示反欧拉俯仰角、滚转角和航向角；
(1)无效姿态处理
求解正欧拉角时，当俯仰角p为±90°时，航向角h与滚转角r均为无意义的值，要对h和r赋零值。同理，求解反欧拉角时，当滚转角rr为±90°时，俯仰角pr与航向角hr均为无意义的值，要对pr和hr赋零值。
(2)常规姿态求解
正、反欧拉角对应的姿态矩阵虽然形式不同，但矩阵对应元素的值相等。常规姿态下，p、rr处于定义域(-90°, 90°)，可根据
求解正欧拉角，p可通过
求解反欧拉角，rr可通过
步骤三对反欧拉角进行定义域调整
定义域调整的目的是把反欧拉俯仰角pr的范围从[-180°， 180°]调整到[-90°, 90°]。具体方法是对上一步计算出来的pr取正方向的补角，对hr和rr取反方向的补角。若pr大于 90°，则pr＝ 180°–pr；若pr小于-90°，则pr＝-180°–pr。当pr不处于[-90°, 90°]范围时，若hr<0，则hr＝ 180° hr；若hr>0，则hr＝-180° hr；同理，若rr<0，则rr＝ 180° rr；若rr>0，则rr＝-180° rr。
步骤四正、反欧拉角精华区切换
如图1所示，双欧拉法输出正欧拉角还是反欧拉角取决于正欧拉的俯仰角p的范围，具体实现如下：
如果|p|<＝45°，则输出正欧拉角(r,p,h)表示姿态；
否则，输出反欧拉角(rr,pr,hr)表示姿态。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。
1.一种基于捷联惯导的双欧拉全姿态解算方法，其特征在于，包括如下步骤：
步骤一姿态矩阵的更新；
步骤二利用姿态矩阵求解正反欧拉角；
步骤三对反欧拉角进行定义域调整；
步骤四正、反欧拉角精华区切换。
2.如权利要求1所述的基于捷联惯导的双欧拉全姿态解算方法，其特征在于，所述步骤一中，姿态矩阵更新时，需预设的参数包括：tk时刻的载体坐标系为p(k)，导航坐标系为n(k)，tk 1时刻的载体坐标系为p(k 1)，导航坐标系为n(k 1)；记p(k)至p(k 1)的转动四元数为q(h)，n(k)至n(k 1)的转动四元数为p(h)，n(k)至p(k)的转动四元数为q(tk)，n(k 1)至p(k 1)的转动四元数为q(tk 1)，其中h＝tk 1-tk为更新周期。
3.如权利要求2所述的基于捷联惯导的双欧拉全姿态解算方法，其特征在于，所述步骤一中，姿态矩阵更新的过程为：对于空间中任一矢量r，用姿态矩阵描述，有
其中，rn(k 1)为tk 1时刻导航坐标系的矢量，rp(k 1)为tk 1时刻载体坐标系的矢量，
根据四元数与姿态矩阵的等效关系，得到：
因为
于是得到
其中，
4.如权利要求3所述的基于捷联惯导的双欧拉全姿态解算方法，其特征在于，所述步骤二中，利用姿态矩阵求解正反欧拉角时，首先确定正欧拉角对应的姿态矩阵的形式、反欧拉角对应的姿态矩阵的形式；其次进行无效姿态处理；最后进行常规姿态求解。
5.如权利要求4所述的基于捷联惯导的双欧拉全姿态解算方法，其特征在于，所述步骤二中，正欧拉角对应的姿态矩阵的形式
其中，p、r、h分别表示正欧拉俯仰角、滚转角和航向角。
6.如权利要求5所述的基于捷联惯导的双欧拉全姿态解算方法，其特征在于，所述步骤二中，反欧拉角对应的姿态矩阵的形式
其中，pr、rr、hr分别表示反欧拉俯仰角、滚转角和航向角。
7.如权利要求6所述的基于捷联惯导的双欧拉全姿态解算方法，其特征在于，所述步骤二中，无效姿态处理的过程为：
求解正欧拉角时，当俯仰角p为±90°时，航向角h与滚转角r均为无意义的值，对h和r赋零值；同理，求解反欧拉角时，当滚转角rr为±90°时，俯仰角pr与航向角hr均为无意义的值，对pr和hr赋零值。
8.如权利要求7所述的基于捷联惯导的双欧拉全姿态解算方法，其特征在于，所述步骤二中，常规姿态求解的过程为：
常规姿态下，p、rr处于定义域(-90°, 90°)，根据
求解正欧拉角，p通过
求解反欧拉角，rr通过
9.如权利要求8所述的基于捷联惯导的双欧拉全姿态解算方法，其特征在于，所述步骤三中，对反欧拉角进行定义域调整的目的是把反欧拉俯仰角pr的范围从[-180°， 180°]调整到[-90°, 90°]，其过程为：对上一步计算出来的pr取正方向的补角，对hr和rr取反方向的补角；若pr大于 90°，则pr＝ 180°–pr；若pr小于-90°，则pr＝-180°–pr；当pr不处于[-90°, 90°]范围时，若hr<0，则hr＝ 180° hr；若hr>0，则hr＝-180° hr；同理，若rr<0，则rr＝ 180° rr；若rr>0，则rr＝-180° rr。
10.如权利要求9所述的基于捷联惯导的双欧拉全姿态解算方法，其特征在于，所述步骤四中，正、反欧拉角精华区切换的过程为：
如果|p|<＝45°，则输出正欧拉角(r,p,h)表示姿态；否则，输出反欧拉角(rr,pr,hr)表示姿态。
技术总结