CN104808673A - 一种基于卡尔曼滤波的四旋翼飞行器高度估计方法 - Google Patents

Abstract

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

Description

一种基于卡尔曼滤波的四旋翼飞行器高度估计方法

技术领域

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

背景技术

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

高度控制是四旋翼飞行器自主飞行控制的重要环节。为了解决飞行器定高控制，需要得到精确的飞行高度值。目前飞行器高度的测量方法有：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₂，从而得到重新估计后的垂直方向的高度。

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

$\left\{\begin{array}{c}\mathrm{\Δ}{\stackrel{\·}{\mathrm{\Ω}}}_z=-\frac{1}{T}{\mathrm{\Δ\Ω}}_z+\frac{k}{T}{u}_z\\ \stackrel{\·}{r}=-\frac{8}{m}{C}_T{\mathrm{\Ω}}_i{\mathrm{\Δ\Ω}}_z\\ \stackrel{\·}{Z}=r\end{array}\right.$

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

如上所述的状态方程为：

$\begin{array}{c}\stackrel{\·}{x}=\mathrm{Ax}+\mathrm{Bu}\\ =\left[\begin{array}{ccc}-\frac{1}{T}& 0& 0\\ 1& 0& 0\\ 0& 1& 0\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]\left[\begin{array}{ccc}-\frac{8k}{\mathrm{mT}}{C}_T{\mathrm{\Ω}}_i& 0& 0\end{array}\right]u_z\end{array}$

$\left\{\begin{array}{c}y=\mathrm{Cx}\\ \left[\begin{array}{c}{y}_1\\ y_2\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]\end{array}\right.$

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

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

$\left\{\begin{array}{c}\widehat{\stackrel{\·}{x}}=\mathrm{Ax}+\mathrm{Bu}+K_z(y-\hat{y})\\ \hat{y}=C\hat{x}\end{array}\right.$

其中，表示状态变量矩阵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噪声的方差。

本发明与现有技术相比，其显著优点：

1)本发明根据飞行器高度模型设计出卡尔曼滤波器在线推测飞行高度值，估计的结果可以满足实际应用的精度与实时性要求，同时还具有一定的滤波效果；

2)本发明具有成本低、计算量小、精度高、得到的信息量大的优点；

3)本发明仅需一个低成本的气压计和三轴加速度计，不需要GPS数据参与，具有成本低的优点，便于大规模推广。

附图说明

图1是卡尔曼滤波器结构图；

图2是高度模型辨识结果；

图3是高度估计结果。

具体实施方式

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

步骤1，建立多旋翼飞行器高度模型的状态方程，具体为：

步骤1.1，对多旋翼飞行器进行受力分析，推导其动力学微分方程。

假设多旋翼飞行器为一个六自由度的刚体、忽略弹性振动及变形、不考虑旋翼间的相互作用，且认为多旋翼飞行器的外形与质量均是对称的。

根据牛顿运动定律，惯性坐标系中多旋翼飞行器的质心运动方程为：

$\stackrel{\→}{F}=\frac{d\left(m\stackrel{\→}{V}\right)}{\mathrm{dt}}---\left(1\right)$

式中，m为飞行器的质量， $\stackrel{\→}{V}={\left[\begin{array}{ccc}u& v& w\end{array}\right]}^T$ 为飞行器质心相对于地面惯性坐标系o_gx_gy_gz_g的速度矢量；为所有作用于飞行器的合力。

多旋翼飞行器主要是通过调整各个旋翼的转速实现各自由度运动。垂直方向的升力记为T_i，其大小表示为：

$T_i=C_T{\mathrm{\Ω}}_i^2---\left(2\right)$

式中，C_T为旋翼升力系数，Ω_i为第i个旋翼的转速。通过理论推导出高度的动力学微分方程为：

式中，和表示垂直方向加速度矢量，g为重力加速度常量，m为飞行器的质量，表示飞行器横滚角，θ表示飞行器俯仰角，f_Z为空气阻力。

由于多旋翼飞行器主要以空中悬停和慢速飞行的运动状态来执行任务，近似认为旋翼升力只与转速平方成正比，则悬停状态下

$\stackrel{\·\·}{Z}=\stackrel{\·}{r}=g-\frac{1}{m}\underset{i=1}{\overset{4}{\mathrm{\Σ}}}{C}_T{\mathrm{\Ω}}_i^2=0---\left(4\right)$

飞行器定高控制时，控制器的输入为电机转速控制量u_z，转速的变化为ΔΩ_z，当ΔΩ_z很小时，忽略二次项，则

$\stackrel{\·\·}{Z}=\stackrel{\·}{r}=g-\frac{1}{m}\underset{i=1}{\overset{4}{\mathrm{\Σ}}}{C}_T{({\mathrm{\Ω}}_i+{\mathrm{\Δ\Ω}}_z)}^2=-\frac{8}{m}{C}_T{\mathrm{\Ω}}_i{\mathrm{\Δ\Ω}}_z---\left(5\right)$

假设电机转速控制量u_z与电机的转速变化ΔΩ_z在复数域的关系看作一阶惯性环节：

${\mathrm{\Δ\Ω}}_z\left(s\right)=\frac{k}{1+\mathrm{Ts}}{u}_z\left(s\right)---\left(6\right)$

式中，k、T分别为一阶惯性环节的比例系数与时间常数。将式(6)转换成时域的微分方程形式

$\mathrm{\Δ}{\stackrel{\·}{\mathrm{\Ω}}}_z=-\frac{1}{T}{\mathrm{\Δ\Ω}}_z+\frac{k}{T}{u}_z---\left(7\right)$

式中，表示转速变化的导数。

综上可得出多旋翼飞行器高度的动力学微分方程：

$\left\{\begin{array}{c}\mathrm{\Δ}{\stackrel{\·}{\mathrm{\Ω}}}_z=-\frac{1}{T}{\mathrm{\Δ\Ω}}_z+\frac{k}{T}{u}_z\\ \stackrel{\·}{r}=-\frac{8}{m}{C}_T{\mathrm{\Ω}}_i{\mathrm{\Δ\Ω}}_z\\ \stackrel{\·}{Z}=r\end{array}\right.---\left(8\right)$

式中，和r表示垂直方向的速度矢量。

步骤1.2，为方便设计滤波器，将步骤1.1得到的动力学方程式(8)转化成状态方程形式。选择垂直方向的加速度为状态变量x₁、垂直方向的速度为状态变量x₂、垂直方向的位置，即高度为状态变量x₃，并选择垂直方向的加速度(与状态变量x₁相同)与垂直方向的高度(与状态变量x₃相同)分别为输出y₁和y₂。

$\begin{array}{c}\stackrel{\·}{x}=\mathrm{Ax}+\mathrm{Bu}\\ =\left[\begin{array}{ccc}-\frac{1}{T}& 0& 0\\ 1& 0& 0\\ 0& 1& 0\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]+\left[\begin{array}{ccc}-\frac{8k}{\mathrm{mT}}{C}_T{\mathrm{\Ω}}_i& 0& 0\end{array}\right]u_z\\ \left\{\begin{array}{c}y=\mathrm{Cx}\\ \left[\begin{array}{c}{y}_1\\ y_2\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 0& 1\end{array}\right]\end{array}\right.\end{array}---\left(9\right)$

式中，A为状态方程的系统矩阵，B为状态方程的控制矩阵，C为状态方程的输出矩阵，x为状态变量矩阵，y为输出变量矩阵，u为控制量(标量)，这里即电机转速控制量u_z。

步骤2，采集飞行数据，包括需要进行滤波的电机转速控制量u_z、垂直方向的加速度y₁(加速度计测量值)以及垂直方向的高度y₂(气压计测量值)。

步骤3，根据步骤2中采集到的电机转速控制量u_z、垂直方向的加速度数据y₁以及垂直方向的高度y₂，利用阶跃响应曲线法对步骤1.2所得到的状态方程式(9)进行系统辨识，得出状态方程中和的参数数值。同时得到状态方程的系统矩阵A和状态方程的控制矩阵B。

步骤4，根据步骤3所获得的状态方程设计卡尔曼滤波器，即根据前述步骤中获得的A、B、x、y以及后续求得的K_z构建卡尔曼滤波器。如图1所示，将步骤2所采集的电机转速控制量u_z、垂直方向的加速度数据y₁以及垂直方向的高度y₂作为卡尔曼滤波器的输入，滤波器的输出和为经过卡尔曼滤波器重新估计后的状态方程中的状态变量矩阵x和输出值矩阵y。

根据式(9)状态方程建立的卡尔曼滤波器可表示为

$\left\{\begin{array}{c}\widehat{\stackrel{\·}{x}}=\mathrm{Ax}+\mathrm{Bu}+K_z(y-\hat{y})\\ \hat{y}=C\hat{x}\end{array}\right.---\left(10\right)$

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

K_z＝PC^TR^-1                  (11)

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

AP+PA^T+BQB^T-PC^TRCP＝0             (12)

代数里卡蒂方程中A^T、B^T分别代表A、B矩阵的转置矩阵；矩阵Q为状态方程输出值矩阵y(气压计和加速度计数据)噪声的方差，R为电机转速控制量u_z噪声的方差，通过调整Q,R矩阵中的系数值则可得到相对准确的高度信息。Q的经验值一般为[100，0；0，1]。式中的100和1分别代表加速度计和气压计所测量数据的置信度，一般加速度计测量数据的置信度比气压计测量数据的置信度高；R的经验值范围大概在2000～3000左右。同时卡尔曼滤波器具有一定的滤波效果，可消除高频的传感器噪声，提高了推测的高度的准确度。

步骤5，根据步骤4所设计的卡尔曼滤波器，得到式9状态方程的状态变量x和输出值y的估计值和其中矩阵中的第三个状态变量即最终我们所要求的高度。

图1为卡尔曼滤波器结构图，y为传感器得到的高度和加速度数据，为卡尔曼滤波器估计的输出值，u为控制器的输出控制量，A为状态方程的系统矩阵，B为状态方程的控制矩阵，C为状态方程的输出矩阵，K_z为式11得到的卡尔曼增益，滤波器的输出为状态变量的估计值

图2为状态方程的系统辨识结果图，图中实线为飞行实验时的实际高度，虚线为辨识出的高度模型的输出，两条曲线基本吻合，从而验证了模型的准确性。

图3为卡尔曼滤波的结果图，图中实线是传感器量测值，虚线是卡尔曼滤波器估计值，两条曲线基本吻合，设计的卡尔曼滤波器可以很好的估计。对于高频部分，滤波效果较好，对于低频部分可以跟踪其变化趋势。因此，所设计的卡尔曼滤波器具有很好的状态估计和滤波效果，具有一定的实际应用价值。

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (6)

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₂，从而得到重新估计后的垂直方向的高度。

2.根据权利要求1所述的一种基于卡尔曼滤波的四旋翼飞行器高度估计方法，其特征在于，所述的动力学微分方程为：

$\left\{\begin{array}{c}\mathrm{\Δ}{\stackrel{.}{\mathrm{\Ω}}}_z=-\frac{1}{T}\mathrm{\Δ}{\mathrm{\Ω}}_z+\frac{k}{T}{u}_z\\ \stackrel{.}{r}=-\frac{8}{m}{C}_T{\mathrm{\Ω}}_i\mathrm{\Δ}{\mathrm{\Ω}}_z\\ \stackrel{.}{Z}=r\end{array}\right.$

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

3.根据权利要求2所述的一种基于卡尔曼滤波的四旋翼飞行器高度估计方法，其特征在于，所述的状态方程为：

$\begin{array}{c}\stackrel{.}{x}=\mathrm{Ax}+\mathrm{Bu}\\ =\left[\begin{array}{ccc}-\frac{1}{T}& 0& 0\\ 1& 0& 0\\ 0& 1& 0\\ & & \end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]+\left[\begin{array}{ccc}-\frac{8k}{\mathrm{mT}}{C}_T{\mathrm{\Ω}}_i& 0& 0\end{array}\right]u_z\end{array}$

$\left\{\begin{array}{c}y=\mathrm{Cx}\\ \left[\begin{array}{c}{y}_1\\ y_2\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]\end{array}\right.$

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

4.根据权利要求1所述的一种基于卡尔曼滤波的四旋翼飞行器高度估计方法，其特征在于，所述的卡尔曼滤波器基于以下公式：

$\left\{\begin{array}{c}\widehat{\stackrel{.}{x}}=\mathrm{Ax}+\mathrm{Bu}+K_z(y-\hat{y})\\ \hat{y}=C\hat{x}\end{array}\right.$

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

5.根据权利要求4所述的一种基于卡尔曼滤波的四旋翼飞行器高度估计方法，其特征在于，所述的卡尔曼增益K_z＝PC^TR^-1，

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

6.根据权利要求4所述的一种基于卡尔曼滤波的四旋翼飞行器高度估计方法，其特征在于，所述的代数里卡蒂方程为AP+PA^T+BQB^T-PC^TRCP＝0，

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