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

摘要

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

主权项

1.一种基于四元数的跨象限姿态角的计算方法，其特征在于，包括如下步骤：由初始四元数分别计算获得俯仰角初值、偏航角初值和滚转角初值；由本周期的四元数分别计算获得本周期的偏航角中间值和本周期的滚转角中间值；根据本周期的偏航角中间值和上一周期的偏航角中间值确定偏航角的计数；根据本周期的滚转角中间值和上一周期的滚转角中间值确定滚转角的计数；由本周期的偏航角中间值和偏航角的计数计算得到本周期的偏航角终值；由本周期的滚转角中间值和滚转角的计数计算得到本周期的滚转角终值；由本周期的偏航角终值计算得到本周期的偏航角的解耦矩阵；由本周期的滚转角终值计算得到本周期的滚转角的解耦矩阵；由本周期的四元数确定本周期的四元数转换矩阵；由本周期的偏航角的解耦矩阵、本周期的滚转角的解耦矩阵和本周期的姿态转换矩阵计算得到本周期的俯仰角的转换矩阵；由本周期的俯仰角的转换矩阵计算得到本周期的俯仰角对应的四元数；由本周期的俯仰角对应的四元数计算得到本周期的俯仰角中间值；根据本周期的俯仰角中间值确定俯仰角的计数；由本周期的俯仰角中间值和俯仰角的计数计算得到本周期的俯仰角终值。