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

说明书

本发明公开了一种固定翼无人机落体运动水平姿态估计方法。包括步骤一：对陀螺仪和加速度计数据进行预处理，将传感器数据转换到前右下机体坐标系并归一化；步骤二：根据天向加速度计初步判断机腹朝向；步骤三：启动发动机后使用机动策略估计出陀螺仪和加速度计的初步常值零偏；步骤四：根据陀螺仪和加速度计数据，使用一种基于四元数的改进自适应扩展卡尔曼滤波方法对传感器数据进行融合，进一步估计陀螺仪和加速度计的零偏，同时得到水平姿态角的实时估计。本发明克服了传统传递对准方法对投放载体要求高的缺点，能够在固定翼无人机投放过程中利用惯性传感器确定打开机翼和启动发动机时间并实现水平姿态的估计任务，实现传感器误差的在线估计。

技术领域

本发明属于惯性导航与传感器数据融合领域，具体为一种固定翼无人机落体运动水平姿态估计方法。

背景技术

获取载体姿态是操纵、控制载体和实现一些其他功能的重要前提，近年来微机电(MEMS)传感器发展迅速，由于其具有成本低、体积小、使用灵活不受限制的优点而被广泛使用。固定翼无人机是一种先进的无人机系统，其机动性强、反应能力迅速，适合攻击各种活动目标，具有很高的军事意义。固定翼无人机的水平姿态即横滚、俯仰角对准估计及确定打开机翼启动发动机时间是控制固定翼无人机的研究重点和难点，传统的传递初始对准方法对投放载体具有较高的要求，并且在进行火力打击时容易受到卫星拒止情况，因此利用固定翼无人机装载六自由度MEMS惯性传感器(包含3轴陀螺仪和3轴加速度计)和空速表实现固定翼无人机自由落体运动时打开机翼启动发动机时机选择和水平姿态的估计是本专利深入研究的内容。

发明内容

为了解决上述背景技术提到的技术问题，本发明提出了一种固定翼无人机自由落体运动时水平姿态估计方法。

为了实现上述技术目的，本发明的技术方案为：

一种固定翼无人机落体运动水平姿态估计方法，包括以下步骤：

(1)无人机自由落体一定时间后获取机载MEMS陀螺仪、MEMS加速度计和空速表的数据，对陀螺仪和加速度计数据进行预处理，将传感器数据转换到前右下机体坐标系并归一化；

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

(3)启动发动机后使用机动策略估计出陀螺仪和加速度计的初步常值零偏；

(4)根据陀螺仪和加速度计数据，使用一种基于四元数的改进自适应扩展卡尔曼滤波方法对传感器数据进行融合，进一步估计陀螺仪和加速度计的零偏，同时得到水平姿态角的实时估计。

进一步地，在步骤(1)中，选取北东地地理坐标系作为导航坐标系，将陀螺仪和加速度计数据相应转换到前右下机体坐标系并归一化。根据空速表求取前向阻力的公式如下:其中ρ为空气密度，v是载体相对空气的速度，由空速表获得，C为空气阻力系数，A为无人机横截面积。

由前向阻力求取其引起的前向运动加速度的公式如下：其中m为无人机质量，将a_m作为前向运动加速度的补偿的公式如下：a_x补＝a_x-a_m，其中a_x补为补偿后的前向加速度计数据，a_x为补偿前的前向加速度计数据。

进一步地，在步骤(2)中，根据天向加速度计a_z判断机腹朝向，具体过程如下：设定天向加速度计阈值a(0.6ga0.8g)，若a_z为负且绝对值大于a，可判断机腹朝下且姿态与水平相差不大，则可打开机翼并启动发动机进入下一阶段。若a_z为正且绝对值大于a，则通过飞控旋转180°后打开机翼并启动发动机进入下一阶段。若a_z的绝对值小于a，则可旋转90°后再进行前述判断。