[发明专利]一种四旋翼飞行器横滚轴角速度和俯仰轴角速度估计方法

说明书

本发明公开了一种四旋翼飞行器横滚轴角速度及俯仰轴角速度估计方法，首先，采集多组试验数据，通过最小二乘法辨识横滚力矩模型参数、俯仰力矩模型参数；其次，周期读取k时刻四旋翼飞行器机载传感器信息并计算k时刻的横滚轴角加速度、俯仰轴角加速度；然后，预测k时刻四旋翼飞行器的横滚轴角速度、俯仰轴角速度、横滚力矩模型及俯仰力矩模型的一阶马尔可夫、横滚力矩模型参数、俯仰力矩模型参数、四元数；最后，通过扩展卡尔曼滤波器，对k时刻四旋翼飞行器的横滚轴角速度、俯仰轴角速度、四元数进行校正。采用本发明无需增加外部设备，可以在缺少横滚轴角速度、俯仰轴角速度传感器时，通过力矩模型辅助完成对横滚轴角速度、俯仰轴角速度估计的估计。

技术领域

本发明属于组合导航与自主导航领域，具体涉及一种四旋翼飞行器横滚轴角速度和俯仰轴角速度估计方法。

背景技术

四旋翼飞行器具有体积小、结构简单、可悬停和垂直起降等优点，特别适合在近地面环境，如室内、城区和丛林等中执行监视、侦察等任务，具有广阔的军事和民用前景。导航系统为四旋翼飞行器提供其飞行控制系统所必须的导航信息，是其完成各种复杂飞行任务的必要保障。

受四旋翼飞行器体积、载重、成本所限，其通常选用低成本、小型化的低精度机载导航传感器。目前四旋翼飞行器常用的导航方案包含惯性传感器、卫星导航系统、磁传感器以及气压高度计；其中惯性传感器与磁传感器组成航姿系统，可提供姿态信息；惯性传感器与卫星导航系统进行信息融合，可提供速度与位置信息；气压高度计用以修正高度信息。

当横滚轴陀螺、俯仰轴陀螺在使用过程中出现故障时，将无法获得横滚轴角速度、俯仰轴角速度信息，通过使用横滚力矩模型、俯仰力矩模型可以在缺少横滚轴角速度、俯仰轴角速度量测传感器时，实现对横滚轴角速度、俯仰轴角速度的实时估计。

发明内容

发明目的：本发明所要解决的技术问题是：提出一种四旋翼飞行器力矩模型/加速度计融合滤波方法，通过采用四旋翼飞行器的横滚力矩模型、俯仰力矩模型，与其机载传感器相结合，解决四旋翼飞行器在缺少横滚轴角速度、俯仰轴角速度量测时，横滚轴角速度、俯仰轴角速度实时估计问题。

技术方案：本发明所述的一种四旋翼飞行器横滚轴角速度及俯仰轴角速度估计方法，包括以下步骤：

(1)采集多组传感器数据，通过最小二乘法辨识横滚力矩模型参数、俯仰力矩模型参数；

(2)周期读取k时刻四旋翼飞行器机载传感器信息；

(3)计算k时刻的横滚轴角加速度、俯仰轴角加速度；

(4)预测k时刻四旋翼飞行器的横滚轴角速度、俯仰轴角速度、横滚力矩模型的一阶马尔可夫、俯仰力矩模型的一阶马尔可夫、横滚力矩模型参数、俯仰力矩模型参数、四元数；

(5)通过扩展卡尔曼滤波器，对k时刻四旋翼飞行器的横滚轴角速度、俯仰轴角速度、四元数进行校正，得到横滚轴角速度、俯仰轴角速度的估计结果。

所述步骤(1)包括以下步骤：

(11)采集一组传感器数据，主要包括旋翼转速传感器信息、陀螺信息；

(12)通过建立最小二乘方程辨识横滚力矩模型参数：

f＝k_x0x₀+k_x1x₁+k_x2x₂

其中，ω_mx机体系相对于导航系的角速度在机体系x轴上的分量，k_x0、k_x1、k_x2为模型参数；

(13)通过建立最小二乘方程辨识俯仰力矩模型参数：