[发明专利]自旋类飞行器的姿态解算方法

说明书

本发明公开了一种自旋类飞行器的姿态解算方法，包括以下步骤：获取飞行器陀螺仪三轴角速度数值、磁力计三轴地磁场强度数值和加速度计三轴加速度数值；根据陀螺仪三轴角速度计算飞行器桨叶挥舞角，桨距角和自旋角速度；根据磁力计任一数值的变化确定飞行器旋转周期，根据旋转周期计算平均自旋角速度；根据加速度计在旋转周期内每个时刻采集的X轴方向的加速度数值计算桨盘倾斜角；根据得到的挥舞角和桨距角以及得到的桨盘倾斜角计算得到飞行器的偏航角；对陀螺仪积分计算的姿态角和计算的姿态角进行互补滤波，得到飞行器最终的姿态角。本发明计算结果准确，适用于自旋类飞行器的姿态解算。

技术领域

本发明涉及飞行器姿态解算方法，具体涉及一种自旋类飞行器的姿态解算方法。

背景技术

自旋类飞行器是一种依靠整个机体自旋产生升力的飞行器，其主要特点是在整个飞行过程中，飞行器所有部件均随机体一同进行高速旋转，而无与地面保持相对稳定的部件。这种飞行器具有结构简单，成本低廉等优势，但由于其上述飞行特点，导致常规的加速度计+陀螺仪+磁力计的九轴姿态解算方法失效，主要原因是，在高速自旋状态下，加速度计偏离旋转轴微小距离，便将产生巨大的向心加速度，从而干扰加速度计在姿态解算中的正常工作，而仅依靠陀螺仪和磁力计将产生很大的时间累积误差，因此常规的九轴姿态解算算法在自旋类飞行器上失效，从而限制了自旋类飞行器的应用。

发明内容

发明目的：本发明的目的是提供一种自旋类飞行器的姿态解算方法，解决现有方法不适于自旋类飞行器姿态解算，计算结果误差大的问题。

技术方案：本发明涉及一种自旋类飞行器的姿态解算方法，包括以下步骤：

(1)获取飞行器陀螺仪三轴角速度数值、磁力计三轴地磁场强度数值和加速度计三轴加速度数值；

(2)根据陀螺仪三轴角速度计算飞行器桨叶挥舞角，桨距角和自旋角速度；

(3)根据磁力计任一数值的变化确定飞行器旋转周期，根据旋转周期计算平均自旋角速度；

(4)根据加速度计在旋转周期内每个时刻采集的X轴方向的加速度数值计算桨盘倾斜角；

(5)根据步骤(2)的得到的挥舞角和桨距角以及步骤(4)得到的桨盘倾斜角计算得到飞行器的偏航角；

(6)对陀螺仪积分计算的姿态角和步骤(5)计算的姿态角进行互补滤波，得到飞行器最终的姿态角。

其中，所述步骤(2)桨距角、自旋角速度和挥舞角的计算公式如下：

其中，p、q、r为陀螺仪三轴角速度数值，r＞＞p,r＞＞q，β为挥舞角，为桨距角，Ω_l为瞬时自旋角速度

所述步骤(3)具体为：

设在一个自旋周期内，飞行器状态基本保持不变，当前磁力计x轴读数为记录此时时刻为T_s，当下一次的数值越过时，记录此时时刻为T_s+1，那么一个旋转周期内的平均自旋角速度计算如下：

所述步骤(4)具体为：

将旋转周期按照加速度计采样的时刻细分为T_s¹、T_s²、…、T_s^k、T_s+1，每个时刻采样得到的x轴方向上加速度数值记为将其按照两两进行分组，若k/2不能整除，则取k/2等于不大于它的最大整数，则可得到：

两式相减得到：