[发明专利]一种基于卡尔曼滤波的四旋翼飞行器高度估计方法

说明书

技术领域

本发明涉及一种飞行器高度估计方法，尤其涉及一种基于卡尔曼滤波的四旋翼飞行器高度估计方法，属于飞行器导航、控制技术领域。

背景技术

四旋翼飞行器领域的研究日趋于广泛，目前许多国家和地区已经投入了大量的人力和物力。大多数无人机的使用主要体现在民用和军用两方面，如军事侦察、战场评估、航拍摄影、电力巡检、环境监测、森林防火、防恐救生等方面。四旋翼无人机具有体积小、机动性强、效费比高、无人员伤亡的特点。无论是从经济角度还是从人身安全角度来说，四旋翼无人机都将会在未来世界中得到广泛使用。

高度控制是四旋翼飞行器自主飞行控制的重要环节。为了解决飞行器定高控制，需要得到精确的飞行高度值。目前飞行器高度的测量方法有：1、采用气压计。虽然价格相对较低，但气压计是通过大气压变化来推算飞行高度值，但因为气压值容易受到天气变化的影响，所以气压计不适用于天气状况不是很稳定的环境，而且气压计数据噪声严重。2、使用GPS。GPS价格相对较贵，且GPS的高度测量值精度较低，数据频率低，不适用于飞行器高度控制。

发明内容

本发明的目的是提出一种基于卡尔曼滤波的四旋翼飞行器高度估计方法，该方法具有精度高、成本低、实时性好、适用范围广的特点，可以为飞行器提供精确的高度值。

本发明采用的技术方案如下：一种基于卡尔曼滤波的四旋翼飞行器高度估计方法，其特征在于：本发明根据四旋翼飞行器飞行特点，对高度模型进行建模，并进行简化处理，通过采集飞行数据，进行模型辨识，验证模型的有效性；接着通过卡尔曼滤波算法实现对飞行高度的实时、准确估计，该方法可用于复杂环境下估计出准确的高度值，且仅需一个低成本的气压计和一个三轴加速度计。

具体步骤包括：

一种基于卡尔曼滤波的四旋翼飞行器高度估计方法，包括以下步骤：

步骤1、建立多旋翼飞行器动力学微分方程，根据多旋翼飞行器动力学微分方程建立多旋翼飞行器高度模型的状态方程，选择垂直方向的加速度为状态变量x₁、垂直方向的速度为状态变量x₂、垂直方向的高度为状态变量x₃，并选择垂直方向的加速度与垂直方向的高度分别为输出y₁和y₂；

步骤2、采集飞行数据，包括电机转速控制量u_z、垂直方向的加速度y₁以及垂直方向的高度y₂；

步骤3、根据步骤2采集的飞行数据，利用阶跃响应曲线法对步骤1所得到的状态方程进行系统辨识，得到状态方程的系统矩阵A和控制矩阵B；

步骤4、根据步骤3获得的系统矩阵A和控制矩阵B构建卡尔曼滤波器，将步骤2采集到的电机转速控制量u_z、垂直方向的加速度y₁以及垂直方向的高度y₂作为卡尔曼滤波器的输入，卡尔曼滤波器输出重新估计后的状态方程中的状态变量x₁、x₂和x₃，以及输出值y₁和y₂，从而得到重新估计后的垂直方向的高度。

如上所述的动力学微分方程为：

其中：表示电机转速变化的导数，ΔΩ_z为电机转速的变化，k、T分别为一阶惯性环节的比例系数与时间常数，表示垂直方向加速度矢量，m为飞行器的质量，C_T为旋翼升力系数，Ω_i为第i个旋翼的转速，和r表示垂直方向的速度矢量。

如上所述的状态方程为：

其中：A为状态方程的系统矩阵，B为状态方程的控制矩阵，C为状态方程的输出矩阵，x为状态变量矩阵，y为输出变量矩阵，u为控制量，即电机转速控制量u_z，k、T分别为一阶惯性环节的比例系数与时间常数，m为多旋翼飞行器的质量，C_T为旋翼升力系数，Ω_i为第i个旋翼的转速。

如上所述的卡尔曼滤波器基于以下公式：

其中，表示状态变量矩阵x估计值的导数，A、B分别为状态方程的系统矩阵和控制矩阵，y为状态方程的输出值矩阵，即加速度计和气压计的测量数据，为卡尔曼滤波器估计的输出值矩阵，为状态变量矩阵x的估计值，即卡尔曼滤波器的输出，K_z为卡尔曼增益。