CN105651285A - 一种基于四元数的跨象限姿态角的计算方法 - Google Patents

Abstract

本发明公开一种基于四元数的跨象限姿态角的计算方法，包括如下步骤：分别计算获得俯仰角初值、偏航角初值和滚转角初值；分别计算获得本周期的偏航角中间值和本周期的滚转角中间值；确定偏航角的计数；确定滚转角的计数；计算得到本周期的偏航角终值；计算得到本周期的滚转角终值；计算得到本周期的偏航角的解耦矩阵；计算得到本周期的滚转角的解耦矩阵；确定本周期的四元数转换矩阵；计算得到本周期的俯仰角的转换矩阵；计算得到本周期的俯仰角对应的四元数；计算得到本周期的俯仰角中间值；确定俯仰角的计数；计算得到本周期的俯仰角终值。所述计算方法能够避免现有技术的传统的解算方法导致的奇异性和精度下降，以保证解算的精度。

Description

一种基于四元数的跨象限姿态角的计算方法

技术领域

本发明涉及基于四元数的姿态角的计算方法技术领域。更具体地，涉及一种基于四元数的跨象限姿态角的计算方法。

背景技术

现有技术中，由四元数求解欧拉角通常会涉及到反三角函数运算。因此，反三角函数运算存在的值域要求和奇异性问题就不可避免。通常对欧拉角进行解算时，要对欧拉角范围进行限定，并且在接近象限的范围内进行解算时精度会有所下降。

因此，需要提供一种基于四元数的跨象限姿态角的计算方法。

发明内容

本发明的目的在于提供一种基于四元数的跨象限姿态角的计算方法。

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

一种基于四元数的跨象限姿态角的计算方法，包括如下步骤：

由初始四元数分别计算获得俯仰角初值、偏航角初值和滚转角初值；

由本周期的四元数分别计算获得本周期的偏航角中间值和本周期的滚转角中间值；

根据本周期的偏航角中间值和上一周期的偏航角中间值确定偏航角的计数；根据本周期的滚转角中间值和上一周期的滚转角中间值确定滚转角的计数；

由本周期的偏航角中间值和偏航角的计数计算得到本周期的偏航角终值；由本周期的滚转角中间值和滚转角的计数计算得到本周期的滚转角终值；

由本周期的偏航角终值计算得到本周期的偏航角的解耦矩阵；由本周期的滚转角终值计算得到本周期的滚转角的解耦矩阵；

由本周期的四元数确定本周期的四元数转换矩阵；

由本周期的偏航角的解耦矩阵、本周期的滚转角的解耦矩阵和本周期的姿态转换矩阵计算得到本周期的俯仰角的转换矩阵；

由本周期的俯仰角的转换矩阵计算得到本周期的俯仰角对应的四元数；

由本周期的俯仰角对应的四元数计算得到本周期的俯仰角中间值；

根据本周期的俯仰角中间值确定俯仰角的计数；

由本周期的俯仰角中间值和俯仰角的计数计算得到本周期的俯仰角终值。

优选地，所述俯仰角初值的计算公式为：

θ₀＝sin^-1[2(q₁₀q₂₀+q₀₀q₃₀)]；

其中，θ₀为俯仰角初值；q₀₀、q₁₀、q₂₀、q₃₀为初始四元数；

所述偏航角初值的计算公式为：

${\mathrm{\ψ}}_0={\tan }^{-1}\[\frac{-2(q_{10}{q}_{30}-q_{00}{q}_{20})}{{q_{00}}^2+{q_{10}}^2-{q_{20}}^2-{q_{30}}^2}\];$

其中，ψ₀为偏航角初值；

所述滚转角初值的计算公式为：

${\mathrm{\γ}}_0={\tan }^{-1}\[\frac{-2(q_{20}{q}_{30}-q_{10}{q}_{20})}{{q_{00}}^2-{q_{10}}^2+{q_{20}}^2-{q_{30}}^2}\];$

其中，γ₀为滚转角初值；

所述俯仰角初值、所述偏航角初值和所述滚转角初值的数值都小于90°。

优选地，所述本周期的偏航角中间值的计算公式为：

${\mathrm{\ψ}}_{nm}={\tan }^{-1}\[\frac{2(q_{0n}{q}_{2n}-q_{1n}{q}_{3n})}{{q_{0n}}^2+{q_{1n}}^2-{q_{2n}}^2-{q_{3n}}^2}\];$

其中，ψ_nm为本周期的偏航角中间值；q_0n、q_1n、q_2n、q_3n为本周期的四元数；n为本周期的周期数，且n为大于或等于2的正整数；

所述本周期的滚转角中间值的计算公式为：

${\mathrm{\γ}}_{nm}={\tan }^{-1}\[\frac{2(-q_{2n}{q}_{3n}+q_{0n}{q}_{ln})}{{q_{0n}}^2-{q_{1n}}^2+{q_{2n}}^2-{q_{3n}}^2}\];$

其中，γ_nm为本周期的滚转角中间值；q_0n、q_1n、q_2n、q_3n为本周期的四元数。

优选地，当上一周期的偏航角中间值大于π/3且所述本周期的偏航角中间值小于-π/3时，偏航角的计数增加1；当上一周期的偏航角中间值小于-π/3且所述本周期的偏航角中间值大于π/3时，偏航角的计数减小1；当上一周期的滚转角中间值大于π/3且所述本周期的滚转角中间值小于-π/3时，滚转角的计数增加1；当上一周期的滚转角中间值小于-π/3且所述本周期的滚转角中间值大于π/3时，滚转角的计数减小1。

优选地，所述本周期的偏航角终值的计算公式为：

ψ_nt＝ψ_nm+aπ；

其中，ψ_nt为本周期的偏航角终值；a为偏航角的计数；

所述本周期的滚转角终值的计算公式为：

γ_nt＝γ_nm+bπ；

其中，γ_nt为本周期的滚转角终值；b为滚转角的计数。

优选地，所述本周期的偏航角的解耦矩阵的计算公式为：

$C_{{\mathrm{\ψ}}_{nt}}=\left[\begin{array}{ccc}cos\left({\mathrm{\ψ}}_{nt}\right)& sin\left({\mathrm{\ψ}}_{nt}\right)& 0\\ 0& 1& 0\\ -sin\left({\mathrm{\ψ}}_{nt}\right)& 0& cos\left({\mathrm{\ψ}}_{nt}\right)\end{array}\right];$

其中，为本周期的偏航角的解耦矩阵；ψ_nt为本周期的偏航角终值；

所述本周期的滚转角的解耦矩阵的计算公式为：

$C_{{\mathrm{\γ}}_{nt}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\left({\mathrm{\γ}}_{nt}\right)& -sin\left({\mathrm{\γ}}_{nt}\right)\\ 0& sin\left({\mathrm{\γ}}_{nt}\right)& \cos \left({\mathrm{\γ}}_{nt}\right)\end{array}\right];$

其中，为本周期的滚转角的解耦矩阵；γ_nt为本周期的滚转角终值。

优选地，所述本周期的四元数转换矩阵的表达式为：

$C_{nq}=\left[\begin{array}{ccc}{q}_{0n}{q}_{0n}+q_{1n}{q}_{1n}-q_{2n}{q}_{2n}-q_{3n}{q}_{3n}& 2(q_{1n}{q}_{2n}+q_{0n}{q}_{3n})& 2(q_{1n}{q}_{3n}-q_{0n}{q}_{2n})\\ 3(q_{1n}{q}_{2n}-q_{0n}{q}_{3n})& q_{0n}{q}_{0n}-q_{1n}{q}_{1n}+q_{2n}{q}_{2n}-q_{3n}{q}_{3n}& 2(q_{0n}{q}_{1n}+q_{2n}{q}_{3n})\\ 2(q_{0n}{q}_{2n}+q_{1n}{q}_{3n})& 2(q_{2n}{q}_{3n}-q_{0n}{q}_{1n})& q_{0n}{q}_{0n}-q_{1n}{q}_{1n}-q_{2n}{q}_{2n}+q_{3n}{q}_{3n}\end{array}\right];$

其中，为本周期的四元数转换矩阵；q_0n、q_1n、q_2n、q_3n为本周期的四元数。

优选地，所述本周期的俯仰角的转换矩阵的计算公式为：

$C_{{\mathrm{\θ}}_{nt}}=C_{{\mathrm{\γ}}_{nt}}^{-1}{C}_{nq}{C}_{{\mathrm{\ψ}}_{nt}}^{-1};$

其中，为本周期的俯仰角的转换矩阵；

所述本周期的俯仰角的转换矩阵的表达式为：

$C_{{\mathrm{\θ}}_{nt}}=\left[\begin{array}{ccc}{C}_{20}& C_{21}& C_{22}\\ C_{23}& C_{24}& C_{25}\\ C_{26}& C_{27}& C_{28}\end{array}\right];$

其中，C₂₀、C₂₁、C₂₂、C₂₃、C₂₄、C₂₅、C₂₆、C₂₇和C₂₈为本周期的俯仰角的转换矩阵的矩阵元。

优选地，所述本周期的俯仰角对应的四元数的计算公式为：

$\begin{array}{c}{q}_{0n-\mathrm{\θ}}=0.5sqrt\left(\right|1.0+C_{20}+C_{24}+C_{28}\left|\right)\\ q_{1n-\mathrm{\θ}}=\frac{1.0}{4q_{10}}(C_{25}-C_{27})\\ q_{2n-\mathrm{\θ}}=\frac{1.0}{4q_{10}}(C_{26}-C_{22})\\ q_{3n-\mathrm{\θ}}=\frac{1.0}{4q_{10}}(C_{21}-C_{23})\end{array};$

其中，q_0n-θ、q_1n-θ、q_2n-θ、q_3n-θ为本周期的俯仰角对应的四元数。

优选地，所述本周期的俯仰角中间值的计算公式为：

θ_nm＝sin^-1[2(q_1n-θq_2n-θ+q_0n-θq_3n-θ)]；

其中，θ_nm为本周期的俯仰角中间值；

当所述本周期的俯仰角中间值大于π/4时，偏航角的计数增加1；当所述本周期的俯仰角中间值小于-π/4时，偏航角的计数减小1；

所述本周期的俯仰角终值的计算公式为：

${\mathrm{\θ}}_{nt}={\mathrm{\θ}}_{nm}+d\frac{\mathrm{\π}}{4};$

其中，θ_nt为本周期的俯仰角终值；d为俯仰角的计数。

本发明的有益效果如下：

本发明的所述计算方法根据四元数解算的特点，对三个姿态角进行解耦处理，同时对基准坐标系进行定量旋转，使姿态角解算得到的值始终在小于45°的范围内，从而能够避免现有技术的传统的解算方法导致的奇异性和精度下降，以保证解算的精度。

附图说明

下面结合附图对本发明的具体实施方式作进一步详细的说明。

图1为本发明实施例提供的基于四元数的跨象限姿态角的计算方法的流程图。

具体实施方式

为了更清楚地说明本发明，下面结合优选实施例和附图对本发明做进一步的说明。附图中相似的部件以相同的附图标记进行表示。本领域技术人员应当理解，下面所具体描述的内容是说明性的而非限制性的，不应以此限制本发明的保护范围。

如图1所示，本实施例提供的基于四元数的跨象限姿态角的计算方法包括如下步骤：

S1：由初始四元数分别计算获得俯仰角初值、偏航角初值和滚转角初值；

S2：由本周期的四元数分别计算获得本周期的偏航角中间值和本周期的滚转角中间值；

S3：根据本周期的偏航角中间值和上一周期的偏航角中间值确定偏航角的计数；根据本周期的滚转角中间值和上一周期的滚转角中间值确定滚转角的计数；

S4：由本周期的偏航角中间值和偏航角的计数计算得到本周期的偏航角终值；由本周期的滚转角中间值和滚转角的计数计算得到本周期的滚转角终值；

S5：由本周期的偏航角终值计算得到本周期的偏航角的解耦矩阵；由本周期的滚转角终值计算得到本周期的滚转角的解耦矩阵；

S6：由本周期的四元数确定本周期的四元数转换矩阵；

S7：由本周期的偏航角的解耦矩阵、本周期的滚转角的解耦矩阵和本周期的姿态转换矩阵计算得到本周期的俯仰角的转换矩阵；

S8：由本周期的俯仰角的转换矩阵计算得到本周期的俯仰角对应的四元数；

S9：由本周期的俯仰角对应的四元数计算得到本周期的俯仰角中间值；

S10：根据本周期的俯仰角中间值确定俯仰角的计数；

S11：由本周期的俯仰角中间值和俯仰角的计数计算得到本周期的俯仰角终值。

在本实施例的一种优选实施方式中，在上述步骤S11之后还包括如下步骤：

S12：将本周期的偏航角终值、本周期的滚转角终值和本周期的俯仰角终值输出以供用户使用，并作为下一周期计算的初值。

上述步骤S1中，俯仰角初值的计算公式为：

θ₀＝sin^-1[2(q₁₀q₂₀+q₀₀q₃₀)]；公式(1)

其中，θ₀为俯仰角初值；q₀₀、q₁₀、q₂₀、q₃₀为初始四元数。

上述步骤S1中，偏航角初值的计算公式为：

${\mathrm{\ψ}}_0={\tan }^{-1}\[\frac{-2(q_{10}{q}_{30}-q_{00}{q}_{20})}{{q_{00}}^2+{q_{10}}^2-{q_{20}}^2-{q_{30}}^2}\];$ 公式(2)

其中，ψ₀为偏航角初值。

上述步骤S1中，滚转角初值的计算公式为：

${\mathrm{\γ}}_0={\tan }^{-1}\[\frac{-2(q_{20}{q}_{30}-q_{10}{q}_{20})}{{q_{00}}^2-{q_{10}}^2+{q_{20}}^2-{q_{30}}^2}\];$ 公式(3)

其中，γ₀为滚转角初值。

上述步骤S1中，俯仰角初值θ₀、偏航角初值ψ₀和滚转角初值γ₀的数值都小于90°。

上述步骤S2中，本周期的偏航角中间值的计算公式为：

${\mathrm{\ψ}}_{nm}={\tan }^{-1}\[\frac{2(q_{0n}{q}_{2n}-q_{ln}{q}_{3n})}{{q_{0n}}^2+{q_{1n}}^2-{q_{2n}}^2-{q_{3n}}^2}\];$ 公式(4)

其中，ψ_nm为本周期的偏航角中间值；q_0n、q_1n、q_2n、q_3n为本周期的四元数；n为本周期的周期数，且n为大于或等于2的正整数。

上述步骤S2中，本周期的滚转角中间值的计算公式为：

${\mathrm{\γ}}_{nm}={\tan }^{-1}\[\frac{2(-q_{2n}{q}_{3n}+q_{0n}{q}_{ln})}{{q_{0n}}^2-{q_{1n}}^2+{q_{2n}}^2-{q_{3n}}^2}\];$ 公式(5)

其中，γ_nm为本周期的滚转角中间值；q_0n、q_1n、q_2n、q_3n为本周期的四元数。

上述步骤S3中，当上一周期的偏航角中间值大于π/3且本周期的偏航角中间值小于-π/3时，偏航角的计数增加1；当上一周期的偏航角中间值小于-π/3且本周期的偏航角中间值大于π/3时，偏航角的计数减小1；当上一周期的滚转角中间值大于π/3且本周期的滚转角中间值小于-π/3时，滚转角的计数增加1；当上一周期的滚转角中间值小于-π/3且本周期的滚转角中间值大于π/3时，滚转角的计数减小1。

上述步骤S4中，本周期的偏航角终值的计算公式为：

ψ_nt＝ψ_nm+aπ；公式(6)

其中，ψ_nt为本周期的偏航角终值；a为偏航角的计数。

上述步骤S4中，本周期的滚转角终值的计算公式为：

γ_nt＝γ_nm+bπ；公式(7)

其中，γ_nt为本周期的滚转角终值；b为滚转角的计数。

上述步骤S5中，本周期的偏航角的解耦矩阵的计算公式为：

$C_{{\mathrm{\ψ}}_{nt}}=\left[\begin{array}{ccc}cos\left({\mathrm{\ψ}}_{nt}\right)& sin\left({\mathrm{\ψ}}_{nt}\right)& 0\\ 0& 1& 0\\ -sin\left({\mathrm{\ψ}}_{nt}\right)& 0& cos\left({\mathrm{\ψ}}_{nt}\right)\end{array}\right];$ 公式(8)

其中，为本周期的偏航角的解耦矩阵；ψ_nt为本周期的偏航角终值。

上述步骤S5中，本周期的滚转角的解耦矩阵的计算公式为：

$C_{{\mathrm{\γ}}_{nt}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\left({\mathrm{\γ}}_{nt}\right)& -sin\left({\mathrm{\γ}}_{nt}\right)\\ 0& sin\left({\mathrm{\γ}}_{nt}\right)& \cos \left({\mathrm{\γ}}_{nt}\right)\end{array}\right];$ 公式(9)

其中，为本周期的滚转角的解耦矩阵；γ_nt为本周期的滚转角终值。

上述步骤S6中，本周期的四元数转换矩阵的表达式为：

$C_{nq}=\left[\begin{array}{ccc}{q}_{0n}{q}_{0n}+q_{1n}{q}_{1n}-q_{2n}{q}_{2n}-q_{3n}{q}_{3n}& 2(q_{1n}{q}_{2n}+q_{0n}{q}_{3n})& 2(q_{1n}{q}_{3n}-q_{0n}{q}_{2n})\\ 3(q_{1n}{q}_{2n}-q_{0n}{q}_{3n})& q_{0n}{q}_{0n}-q_{1n}{q}_{1n}+q_{2n}{q}_{2n}-q_{3n}{q}_{3n}& 2(q_{0n}{q}_{1n}+q_{2n}{q}_{3n})\\ 2(q_{0n}{q}_{2n}+q_{1n}{q}_{3n})& 2(q_{2n}{q}_{3n}-q_{0n}{q}_{1n})& q_{0n}{q}_{0n}-q_{1n}{q}_{1n}-q_{2n}{q}_{2n}+q_{3n}{q}_{3n}\end{array}\right];$

公式(10)

其中，为本周期的四元数转换矩阵；q_0n、q_1n、q_2n、q_3n为本周期的四元数。

上述步骤S7中，本周期的俯仰角的转换矩阵的计算公式为：

$C_{{\mathrm{\θ}}_{nt}}=C_{{\mathrm{\γ}}_{nt}}^{-1}{C}_{nq}{C}_{{\mathrm{\ψ}}_{nt}}^{-1};$ 公式(11)

其中，为本周期的俯仰角的转换矩阵。

本周期的俯仰角的转换矩阵的表达式为：

$C_{{\mathrm{\θ}}_{nt}}=\left[\begin{array}{ccc}{C}_{20}& C_{21}& C_{22}\\ C_{23}& C_{24}& C_{25}\\ C_{26}& C_{27}& C_{28}\end{array}\right];$ 公式(12)

其中，C₂₀、C₂₁、C₂₂、C₂₃、C₂₄、C₂₅、C₂₆、C₂₇和C₂₈为本周期的俯仰角的转换矩阵的矩阵元。

上述步骤S8中，本周期的俯仰角对应的四元数的计算公式为：

$\begin{array}{c}{q}_{0n-\mathrm{\θ}}=0.5sqrt\left(\right|1.0+C_{20}+C_{24}+C_{28}\left|\right)\\ q_{1n-\mathrm{\θ}}=\frac{1.0}{4q_{10}}(C_{25}-C_{27})\\ q_{2n-\mathrm{\θ}}=\frac{1.0}{4q_{10}}(C_{26}-C_{22})\\ q_{3n-\mathrm{\θ}}=\frac{1.0}{4q_{10}}(C_{21}-C_{23})\end{array};$ 公式(13)

其中，q_0n-θ、q_1n-θ、q_2n-θ、q_3n-θ为本周期的俯仰角对应的四元数。

上述步骤S9中，本周期的俯仰角中间值的计算公式为：

θ_nm＝sin^-1[2(q_1n-θq_2n-θ+q_0n-θq_3n-θ)]；公式(14)

其中，θ_nm为本周期的俯仰角中间值。

上述步骤S10中，当本周期的俯仰角中间值大于π/4时，偏航角的计数增加1；当本周期的俯仰角中间值小于-π/4时，偏航角的计数减小1。

上述步骤S11中，本周期的俯仰角终值的计算公式为：

${\mathrm{\θ}}_{nt}={\mathrm{\θ}}_{nm}+d\frac{\mathrm{\π}}{4};$ 公式(15)

其中，θ_nt为本周期的俯仰角终值；d为俯仰角的计数。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定，对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。

Claims (10)

1.一种基于四元数的跨象限姿态角的计算方法，其特征在于，包括如下步骤：

由初始四元数分别计算获得俯仰角初值、偏航角初值和滚转角初值；

由本周期的四元数分别计算获得本周期的偏航角中间值和本周期的滚转角中间值；

根据本周期的偏航角中间值和上一周期的偏航角中间值确定偏航角的计数；根据本周期的滚转角中间值和上一周期的滚转角中间值确定滚转角的计数；

由本周期的偏航角中间值和偏航角的计数计算得到本周期的偏航角终值；由本周期的滚转角中间值和滚转角的计数计算得到本周期的滚转角终值；

由本周期的偏航角终值计算得到本周期的偏航角的解耦矩阵；由本周期的滚转角终值计算得到本周期的滚转角的解耦矩阵；

由本周期的四元数确定本周期的四元数转换矩阵；

由本周期的偏航角的解耦矩阵、本周期的滚转角的解耦矩阵和本周期的姿态转换矩阵计算得到本周期的俯仰角的转换矩阵；

由本周期的俯仰角的转换矩阵计算得到本周期的俯仰角对应的四元数；

由本周期的俯仰角对应的四元数计算得到本周期的俯仰角中间值；

根据本周期的俯仰角中间值确定俯仰角的计数；

由本周期的俯仰角中间值和俯仰角的计数计算得到本周期的俯仰角终值。

2.根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，所述俯仰角初值的计算公式为：

${\mathrm{\θ}}_0={\sin }^{-1}\[2(q_{10}{q}_{20}+q_{00}{q}_{30})\];$

其中，为俯仰角初值；q₀₀、q₁₀、q₂₀、q₃₀为初始四元数；

所述偏航角初值的计算公式为：

${\mathrm{\ψ}}_0={\tan }^{-1}\[\frac{-2(q_{10}{q}_{30}-q_{00}{q}_{20})}{{q_{00}}^2+{q_{10}}^2-{q_{20}}^2-{q_{30}}^2}\];$

其中，ψ₀为偏航角初值；

所述滚转角初值的计算公式为：

${\mathrm{\γ}}_0={\tan }^{-1}\[\frac{-2(q_{20}{q}_{30}-q_{10}{q}_{20})}{{q_{00}}^2-{q_{10}}^2+{q_{20}}^2-{q_{30}}^2}\];$

其中，γ₀为滚转角初值；

所述俯仰角初值、所述偏航角初值和所述滚转角初值的数值都小于90°。

3.根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，所述本周期的偏航角中间值的计算公式为：

${\mathrm{\ψ}}_{nm}={\tan }^{-1}\[\frac{2(q_{0n}{q}_{2n}-q_{1n}{q}_{3n})}{{q_{0n}}^2+{q_{1n}}^2-{q_{2n}}^2-{q_{3n}}^2}\];$

其中，ψ_nm为本周期的偏航角中间值；q_0n、q_1n、q_2n、q_3n为本周期的四元数；n为本周期的周期数，且n为大于或等于2的正整数；

所述本周期的滚转角中间值的计算公式为：

${\mathrm{\γ}}_{nm}={\tan }^{-1}\[\frac{2(-q_{2n}{q}_{3n}+q_{0n}{q}_{1n})}{{q_{0n}}^2-{q_{1n}}^2+{q_{2n}}^2-{q_{3n}}^2}\];$

其中，γ_nm为本周期的滚转角中间值；q_0n、q_1n、q_2n、q_3n为本周期的四元数。

4.根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，当上一周期的偏航角中间值大于π/3且所述本周期的偏航角中间值小于-π/3时，偏航角的计数增加1；当上一周期的偏航角中间值小于-π/3且所述本周期的偏航角中间值大于π/3时，偏航角的计数减小1；当上一周期的滚转角中间值大于π/3且所述本周期的滚转角中间值小于-π/3时，滚转角的计数增加1；当上一周期的滚转角中间值小于-π/3且所述本周期的滚转角中间值大于π/3时，滚转角的计数减小1。

5.根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，所述本周期的偏航角终值的计算公式为：

ψ_nt＝ψ_nm+aπ；

其中，ψ_nt为本周期的偏航角终值；a为偏航角的计数；

所述本周期的滚转角终值的计算公式为：

γ_nt＝γ_nm+bπ；

其中，γ_nt为本周期的滚转角终值；b为滚转角的计数。

6.根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，所述本周期的偏航角的解耦矩阵的计算公式为：

$C_{{\mathrm{\ψ}}_{nt}}=\left[\begin{array}{ccc}cos\left({\mathrm{\ψ}}_{nt}\right)& sin\left({\mathrm{\ψ}}_{nt}\right)& 0\\ 0& 1& 0\\ -sin\left({\mathrm{\ψ}}_{nt}\right)& 0& cos\left({\mathrm{\ψ}}_{nt}\right)\end{array}\right];$

其中，为本周期的偏航角的解耦矩阵；ψ_nt为本周期的偏航角终值；

所述本周期的滚转角的解耦矩阵的计算公式为：

$C_{{\mathrm{\γ}}_{nt}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\left({\mathrm{\γ}}_{nt}\right)& -sin\left({\mathrm{\γ}}_{nt}\right)\\ 0& sin\left({\mathrm{\γ}}_{nt}\right)& \cos \left({\mathrm{\γ}}_{nt}\right)\end{array}\right];$

其中，为本周期的滚转角的解耦矩阵；γ_nt为本周期的滚转角终值。

7.根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，所述本周期的四元数转换矩阵的表达式为：

$C_{nq}=\left[\begin{array}{ccc}{q}_{0n}{q}_{0n}+q_{1n}{q}_{1n}-q_{2n}{q}_{2n}-q_{3n}{q}_{3n}& 2(q_{1n}{q}_{2n}-q_{0n}{q}_{3n})& 2(q_{1n}{q}_{3n}-q_{0n}{q}_{2n})\\ 2(q_{1n}{q}_{2n}-q_{0n}{q}_{3n})& q_{0n}{q}_{0n}-q_{1n}{q}_{1n}+q_{2n}{q}_{2n}-q_{3n}{q}_{3n}& 2(q_{0n}{q}_{1n}-q_{2n}{q}_{3n})\\ 2(q_{0n}{q}_{2n}+q_{1n}{q}_{3n})& 2(q_{2n}{q}_{3n}-q_{0n}{q}_{1n})& q_{0n}{q}_{0n}-q_{1n}{q}_{1n}-q_{2n}{q}_{2n}+q_{3n}{q}_{3n}\end{array}\right];$

其中，C_nq为本周期的四元数转换矩阵；q_0n、q_1n、q_2n、q_3n为本周期的四元数。

8.根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，所述本周期的俯仰角的转换矩阵的计算公式为：

$C_{{\mathrm{\θ}}_{nt}}=C_{{\mathrm{\γ}}_{nt}}^{-1}{C}_{nq}{C}_{{\mathrm{\ψ}}_{nt}}^{-1};$

其中，为本周期的俯仰角的转换矩阵；

所述本周期的俯仰角的转换矩阵的表达式为：

$C_{{\mathrm{\θ}}_{nt}}=\left[\begin{array}{ccc}{C}_{20}& C_{21}& C_{22}\\ C_{23}& C_{24}& C_{25}\\ C_{26}& C_{27}& C_{28}\end{array}\right];$

其中，C₂₀、C₂₁、C₂₂、C₂₃、C₂₄、C₂₅、C₂₆、C₂₇和C₂₈为本周期的俯仰角的转换矩阵的矩阵元。

9.根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，所述本周期的俯仰角对应的四元数的计算公式为：

$q_{0n-\mathrm{\θ}}=0.5sqrt\left(\right|1.0+C_{20}+C_{24}+C_{28}\left|\right)$

$q_{1n-\mathrm{\θ}}=\frac{1.0}{4q_{10}}(C_{25}-C_{27})$

$q_{2n-\mathrm{\θ}}=\frac{1.0}{4q_{10}}(C_{26}-C_{22});$

$q_{3n-\mathrm{\θ}}=\frac{1.0}{4q_{10}}(C_{21}-C_{23})$

其中，为本周期的俯仰角对应的四元数。

10.根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，所述本周期的俯仰角中间值的计算公式为：

${\mathrm{\θ}}_{nm}={\sin }^{-1}\[2(q_{1n-\mathrm{\θ}}{q}_{2n-\mathrm{\θ}}+q_{0n-\mathrm{\θ}}{q}_{3n-\mathrm{\θ}})\];$

其中，为本周期的俯仰角中间值；

当所述本周期的俯仰角中间值大于π/4时，偏航角的计数增加1；当所述本周期的俯仰角中间值小于-π/4时，偏航角的计数减小1；

所述本周期的俯仰角终值的计算公式为：

${\mathrm{\θ}}_{nt}={\mathrm{\θ}}_{nm}+d\frac{\mathrm{\π}}{4};$

其中，为本周期的俯仰角终值；d为俯仰角的计数。