[发明专利]一种固定翼无人机落体运动水平姿态估计方法

权利要求书

1.一种固定翼无人机落体运动水平姿态估计方法，其特征在于，包括如下步骤：

1)无人机上搭载MEMS陀螺仪、MEMS加速度计、空速表，其中MEMS陀螺仪和MEMS加速度计都分别包含前向轴x、侧向轴y、天向轴z三轴数据，在无人机自由落体一定时间后获取MEMS陀螺仪、MEMS加速度计和空速表的数据，对MEMS陀螺仪、MEMS加速度计数据进行预处理；

2)通过MEMS加速度计天向轴z数据初步判断机腹朝向，若机腹朝地则无人机可直接打开机翼并启动发动机，反之通过无人机飞控调整姿态角度后打开机翼并启动发动机；

3)启动发动机后估计MEMS陀螺仪、MEMS加速度计的初步零偏；

4)根据MEMS陀螺仪、MEMS加速度计数据，通过四元数扩展卡尔曼滤波EKF姿态更新算法，进一步估计MEMS陀螺仪、MEMS加速度计的零偏，同时得到无人机水平姿态角的实时估计。

2.如权利要求1所述的一种固定翼无人机落体运动水平姿态估计方法，其特征在于，步骤1)预处理的实现过程如下：

1.1)选取北东地地理坐标系作为导航坐标系，将MEMS陀螺仪、MEMS加速度计数据相应转换到前右下机体坐标系并统一为国际单位制，得到MEMS陀螺仪三轴数据ω_x、ω_y、ω_z，MEMS加速度计三轴数据a_x、a_y、a_z；根据空速表数据求解前向阻力F_f，公式如下:其中χ为空气密度，v是载体相对空气的速度，由空速表获得，C为空气阻力系数，A为无人机横截面积；

1.2)求解前向运动加速度a_m，公式如下：其中m为无人机质量；求解前向运动加速度的补偿a_x补，公式如下：a_x补＝a_x-a_m。

3.如权利要求2所述的一种固定翼无人机落体运动水平姿态估计方法，其特征在于，步骤2)判断机腹朝向分为三种情况：首先设定MEMS加速度计天向轴z数据为a_z，并设定阈值a，阈值a满足：0.6ga0.8g；

A)若a_z为负且绝对值大于阈值a，则判断机腹朝下且无人机姿态在水平线附近，无人机打开机翼并启动发动机；

B)若a_z为正且绝对值大于阈值a，则通过飞控旋转180°后打开机翼并启动发动机；

C)若a_z的绝对值小于阈值a，则通过飞控旋转90°后再进行分别通过A)和B)情况进行判断。