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

权利要求书

1.一种自旋类飞行器的姿态解算方法，其特征在于，包括以下步骤：

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

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

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

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

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

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

2.根据权利要求1所述的自旋类飞行器的姿态解算方法，其特征在于，所述步骤(2)桨距角、自旋角速度和挥舞角的计算公式如下：

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

3.根据权利要求1所述的自旋类飞行器的姿态解算方法，其特征在于，所述步骤(3)具体为：

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

4.根据权利要求3所述的自旋类飞行器的姿态解算方法，其特征在于，所述步骤(4)具体为：

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

两式相减得到：

根据上述式即可得到：

令：

则

由此计算得到其中绝对值最大的一项记为此时的即为桨盘平面的倾斜角θ_p表示桨盘倾斜角理论值，Ω表示自旋角速度理论值，表示时间介于T_s和T_s+1之间时，测量所得的桨盘倾斜角，g表示重力加速度，g′表示重力加速度分量。

5.根据权利要求4所述的自旋类飞行器的姿态解算方法，其特征在于，所述步骤(5)中具体为：

根据步骤(2)和(4)结果，可以得到T_s^m时刻，飞行器的欧拉角，其中且若则反之，则

偏航角的计算公式如下：

分别指T_s^m时刻，根据传感器数据计算得到的飞行器滚转角、俯仰角和偏航角。

6.根据权利要求5所述的自旋类飞行器的姿态解算方法，其特征在于，所述步骤(6)中互补滤波过程具体为：

假定T_s＜t^l＜T_s+1，

当(t^l+1-T_s^m)(t^l-T_s^m)＜0时，t^l+1时刻的姿态角计算如下：

其中i≥1；

当(t^l+1-T_s^m)(t^l-T_s^m)＞0时，输出t^l+1时刻飞行器滚转角、俯仰角、偏航角这三个姿态角如下：

其中，表示利用陀螺仪积分计算得到的姿态角，计算公式如下：

以上，t^l和t^l+1表示相邻两个采样点时刻，θ^l、ψ^l和θ^l+1、ψ^l+1分别表示t^l时刻和t^l+1时刻最终输出的滚转角、俯仰角、偏航角；

当步骤(2)中计算得到的Ω_l与步骤(3)中计算得到的Ω_h近似相等，则满足步骤(2)-(5)成立的条件，即飞行器相对桨盘平面的姿态保持稳定，此时互补滤波器输出更接近于反之，互补滤波器输出则更接近于陀螺仪积分值