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

说明书

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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