[发明专利]一种基于四元数的跨象限姿态角的计算方法

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

其中，ψ_nm为本周期的偏航角中间值；q_0n、q_1n、q_2n、q_3n为本周期的四元数；n为本周期的周期数，且n为大于或等于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为偏航角的计数，n为本周期的周期数，且n为大于或等于2的正整数；

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

γ_nt＝γ_nm+bπ；

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