[发明专利]适用于空中飞鼠的姿态解算方法

权利要求书

1.一种适用于空中飞鼠的姿态解算方法，所述空中飞鼠基于六轴传感器，其特征在于，包括：

获取角速度ω和加速度a；

估计水平方向的加速度的大小，确定增益因子α的大小；

归一化加速度a，并求出加速度a与水平方向夹角的正弦余弦值；

定义D是上次姿态解算估计出的重力矢量[cosφ,sinφ]^T，φ为翻滚角，从而计算误差矢量e＝a x D；

误差矢量累加计算误差积分In

I_n＝I_n-1+eΔt，Δt是算法执行周期，e是上一步计算出来的误差；

角速度融合

ω′＝ω+α(K_pe+K_iI_n)；K_p是误差的比例增益，K_i是误差的积分增益，e是上一步计算出来的误差，I_n是误差积分；

得出估计的重力矢量D_n，并进行归一化

D_n＝D_n-1+ω′ΔtD′，D'是D的导数；

“估计水平方向的加速度的大小，确定增益因子α的大小；”具体包括：

根据陀螺仪得到的原始数据偏航角速度ω1和俯仰角速度为ω2，以及上次解算得到的重力矢量[cosφ,sinφ]^T，求出此时的水平加速度ωx＝ω1sinφ+ω2cosφ；

根据水平加速度的大小来调节增益因子α，当水平加速度的大小未达到第一阈值时，增益因子α是恒定的并且等于1；如果水平加速度增大并且幅度超过第一阈值，则增益因子α随着幅度误差的增加而线性地减小，直到大于第二阈值时增益因子α减小到0；

根据陀螺仪得到的原始数据偏航角速度ω1和俯仰角速度为ω2去判断是否处于静止状态，如果偏航角速度ω1的绝对值和俯仰角速度为ω2的绝对值之和小于第三阈值，则认为处于静止状态，增益因子α为1。

2.一种空中飞鼠，所述空中飞鼠基于六轴传感器，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现权利要求1所述方法的步骤。

3.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现权利要求1所述方法的步骤。

4.一种处理器，其特征在于，所述处理器用于运行程序，其中，所述程序运行时执行权利要求1所述的方法。