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

权利要求书

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

步骤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=-1TΔΩz+kTuzr·=-8mCTΩiΔΩzZ·=r]]>

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

所述的状态方程为：

x·=Ax+Bu=-1T00100010x1x2x3+-8kmTCTΩi00uz]]>

y=Cxy1y2=100001x1x2x3]]>

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

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

x·^=Ax+Bu+Kz(y-y^)y^=Cx^]]>

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

所述的卡尔曼增益K_z＝PC^TR^-1，

其中，C^T为输出矩阵C的转置矩阵，P为代数里卡蒂方程的解，R^-1为控制输入变量u_z噪声的方差矩阵R的逆；

所述的代数里卡蒂方程为AP+PA^T+BQB^T-PC^TRCP＝0，

其中，A^T、B^T分别代表系统矩阵A和控制矩阵B的转置矩阵；矩阵Q为状态方程输出值矩阵y噪声的方差，R为电机转速控制量u_z噪声的方差。