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

摘要

本发明公开了一种基于卡尔曼滤波的四旋翼飞行器高度估计方法。本方法通过建立多旋翼飞行器高度的动力学模型，根据其飞行特点，简化成线性模型，并转换成状态方程形式，采用加速度计结合气压计，并利用卡尔曼滤波算法在线推测飞行器高度值，同时还具有一定的滤波效果。本发明方法使用低成本传感器，且不受天气变化影响，具有精度高、成本低、实时性好、适用范围广等显著优点。可用于飞行器定高控制。

主权项

一种基于卡尔曼滤波的四旋翼飞行器高度估计方法，其特征在于，包括以下步骤：步骤1、建立多旋翼飞行器动力学微分方程，根据多旋翼飞行器动力学微分方程建立多旋翼飞行器高度模型的状态方程，选择垂直方向的加速度为状态变量x1、垂直方向的速度为状态变量x2、垂直方向的高度为状态变量x3，并选择垂直方向的加速度与垂直方向的高度分别为输出y1和y2；步骤2、采集飞行数据，包括电机转速控制量uz、垂直方向的加速度y1以及垂直方向的高度y2；步骤3、根据步骤2采集的飞行数据，利用阶跃响应曲线法对步骤1所得到的状态方程进行系统辨识，得到状态方程的系统矩阵A和控制矩阵B；步骤4、根据步骤3获得的系统矩阵A和控制矩阵B构建卡尔曼滤波器，将步骤2采集到的电机转速控制量uz、垂直方向的加速度y1以及垂直方向的高度y2作为卡尔曼滤波器的输入，卡尔曼滤波器输出重新估计后的状态方程中的状态变量x1、x2和x3，以及输出值y1和y2，从而得到重新估计后的垂直方向的高度；所述的动力学微分方程为：ΔΩ·z=-1TΔΩz+kTuzr·=-8mCTΩiΔΩzZ·=r]]>其中：表示电机转速变化的导数，△Ωz为电机转速的变化，k、T分别为一阶惯性环节的比例系数与时间常数，表示垂直方向加速度矢量，m为飞行器的质量，CT为旋翼升力系数，Ωi为第i个旋翼的转速，和r表示垂直方向的速度矢量；所述的状态方程为：x·=Ax+Bu=-1T00100010x1x2x3+-8kmTCTΩi00uz]]>y=Cxy1y2=100001x1x2x3]]>其中：A为状态方程的系统矩阵，B为状态方程的控制矩阵，C为状态方程的输出矩阵，x为状态变量矩阵，y为输出变量矩阵，u为控制量，即电机转速控制量uz，k、T分别为一阶惯性环节的比例系数与时间常数，m为多旋翼飞行器的质量，CT为旋翼升力系数，Ωi为第i个旋翼的转速；所述的卡尔曼滤波器基于以下公式：x·^=Ax+Bu+Kz(y-y^)y^=Cx^]]>其中，表示状态变量矩阵x估计值的导数，A、B分别为状态方程的系统矩阵和控制矩阵，y为状态方程的输出值矩阵，即加速度计和气压计的测量数据，为卡尔曼滤波器估计的输出值矩阵，为状态变量矩阵x的估计值，即卡尔曼滤波器的输出，Kz为卡尔曼增益；所述的卡尔曼增益Kz＝PCTR‑1，其中，CT为输出矩阵C的转置矩阵，P为代数里卡蒂方程的解，R‑1为控制输入变量uz噪声的方差矩阵R的逆；所述的代数里卡蒂方程为AP+PAT+BQBT‑PCTRCP＝0，其中，AT、BT分别代表系统矩阵A和控制矩阵B的转置矩阵；矩阵Q为状态方程输出值矩阵y噪声的方差，R为电机转速控制量uz噪声的方差。