CN106885918A - 一种面向多旋翼飞行器的多信息融合实时风速估计方法 - Google Patents

Abstract

本发明公开了一种面向多旋翼飞行器的多信息融合实时风速估计方法，首先采集机载传感器数据和各旋翼转速数据；其次构建多旋翼飞行器的动力学模型，得到多旋翼飞行器阻力与风速的关系；建立基于多旋翼飞行器动力学模型特性、惯性传感器信息和GPS信息的多信息融合导航系统的状态方程和量测方程；最后采用卡尔曼滤波对导航信息进行更新，得到风速的实时估计。本发明针对现有风速估计方法的不足，充分利用动力学模型气动阻力对风速的敏感特性，与惯性器件、GPS构建卡尔曼滤波器，实现对风速较准确的实时估计。

Description

一种面向多旋翼飞行器的多信息融合实时风速估计方法

技术领域

本发明属于飞行器导航技术领域，特别涉及了一种面向多旋翼飞行器的风速估计方法。

背景技术

多旋翼飞行器具有结构简单、体积小、操控性好、垂直起降等特性，对不同环境具有良好的适应性。目前，其在农业播种、航空拍摄、气象监测、城市反恐、灾区救援、军事侦察等方面均已有广泛的应用，展现出广阔的前景。

难以预计的风是多旋翼飞行器稳定飞行最主要的干扰因素。风会增加飞行的控制难度，当风速较大时，如果飞控没有良好的容错能力，会导致多旋翼飞行器极不稳定，甚至出现失控的危险。此外，风也会降低多旋翼飞行器的定点与定高精度，因此对风速的估计对多旋翼飞行器导航、制导与控制有重要的意义。

目前，空速管是飞行器通常采用的风速估计设备。但将空速管应用于多旋翼飞行器中存在以下限制：1、成本、体积的限制，一方面多旋翼能量转换效率低下，搭载过多的传感器会极大牺牲续航时间，另一方面增加新的传感器会减小微小型多旋翼飞行器体积、成本的优势。2、气流干扰，与固定翼飞行器不同，多旋翼飞行器旋翼的旋转会扰乱气流，干扰空速管的测量。

除空速管估计风速外，还有采用构建风场模型的方法来估计风速，但结果不够精确，效果一般。

发明内容

为了解决上述背景技术提出的技术问题，本发明旨在提供一种面向多旋翼飞行器的多信息融合实时风速估计方法，针对现有风速估计方法的不足，充分利用动力学模型气动阻力对风速的敏感特性，与惯性器件、GPS构建卡尔曼滤波器，实现对风速较准确的实时估计。

为了实现上述技术目的，本发明的技术方案为：

一种面向多旋翼飞行器的多信息融合实时风速估计方法，包括以下步骤：

(1)采集机载传感器数据，包括三轴加速度计的输出数据、三轴陀螺仪的输出数据和各旋翼转速数据；

(2)根据步骤(1)获取的数据，构建多旋翼飞行器的动力学模型，得到多旋翼飞行器阻力与风速的关系；

(3)基于多旋翼飞行器动力学模型特性、惯性传感器信息和GPS信息，根据多旋翼飞行器运动学模型，建立多信息融合导航系统的状态方程和量测方程；

(4)采用卡尔曼滤波对导航信息进行更新，得到风速的实时估计。

进一步地，在步骤(2)中，所述多旋翼飞行器的动力学模型：

上式中，F_H为多旋翼飞行器受到的阻力，m为飞行器质量，k为阻力系数，Ω_i为飞行器第i个旋翼的转速，l为飞行器的旋翼总数，V_A ^H为空速在机体系下的水平分量，为地速在机体系下的水平分量，为风速在机体系下的水平分量。

进一步地，在步骤(3)中，所述多信息融合导航系统的状态方程：

上式中，φ、θ、分别为横滚角、俯仰角、航向角，分别为三轴陀螺仪x、y、z轴上的输出量，分别为地速在机体系x、y、z轴上的投影，分别为风速在机体系x、y轴上的投影，为三轴加速度计z轴上的输出量，ω_wx、ω_wy分别为的驱动高斯白噪声，g是重力加速度，k₁、k₂分别为机体系x、y轴上的阻力系数，Ω＝Ω₁+Ω₂+...+Ω_l；

所述多信息融合导航系统的量测方程：

上式中，分别为GPS速度在机体系x、y、z轴上的投影，分别为三轴加速度计x、y轴上的输出量。

进一步地，在步骤(4)中，首先将多信息融合导航系统的状态方程和量测方程离散化，得到卡尔曼滤波器的状态方程和量测方程：

Z＝HX+V

上式中，状态量控制量W为系统噪声阵，是均值为0的高斯白噪声；量测量V为量测噪声阵，是均值为0的高斯白噪声，其中：

进一步地，在步骤(4)中，采用卡尔曼滤波器对多信息融合导航系统进行更新：

X_k+1＝Φ_k+1,kX_k+B_kU_k+W_k

Z_k+1＝H_k+1X_k+1+V_k+1

上式中，X_k+1为k+1时刻的状态向量，U_k为k时刻的控制向量，Φ_k+1,k为k到k+1时刻的系统一步转移矩阵，B_k为U_k的系数矩阵；W_k为k时刻的系统噪声阵，Z_k+1为k+1时刻的状态向量，H_k+1为k+1时刻的量测矩阵，V_k+1为k+1时刻的量测噪声；

首先进行状态一步预测：

其次求解一步预测的均方差误差：

上式中，为k时刻状态向量X_k的卡尔曼滤波估值，是利用计算得到的对k+1时刻状态向量X_k+1的预测值，P_k为k时刻估计均方差误差，Q_k为W_k的方差矩阵，P_k+1|k为的均方差误差；

然后求解滤波增益：

上式中，R_k+1为V_k+1的方差矩阵；

最后对状态估值，得到滤波结果：

同时更新估计均方差误差：

P_k+1＝[I-K_k+1H_k+1]P_k+1|k

按照上述步骤进行循环更新，卡尔曼滤波器输出风速的估计。

采用上述技术方案带来的有益效果：

本发明充分利用动力学模型气动阻力对风速的敏感特性，与惯性器件、GPS构建卡尔曼滤波器，实现对风速的实时估计。另外，不需要增加新的硬件设备，具有成本与载荷方面的优势。

附图说明

图1是本发明的流程框架示意图。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

一种面向多旋翼飞行器的多信息融合实时风速估计方法，如图1所示，包括以下步骤：

步骤1：采集机载传感器数据，包括三轴加速度计的输出数据、三轴陀螺仪的输出数据和各旋翼转速数据。

步骤2：根据步骤1获取的数据，构建多旋翼飞行器的动力学模型，得到多旋翼飞行器阻力与风速的关系。

所述多旋翼飞行器的动力学模型：

上式中，F_H为多旋翼飞行器受到的阻力，m为飞行器质量，k为阻力系数，Ω_i为飞行器第i个旋翼的转速，l为飞行器的旋翼总数，V_A ^H为空速在机体系下的水平分量，为地速在机体系下的水平分量，为风速在机体系下的水平分量。

步骤3：基于多旋翼飞行器动力学模型特性、惯性传感器信息和GPS信息，根据多旋翼飞行器运动学模型，建立多信息融合导航系统的状态方程和量测方程。

所述多信息融合导航系统的状态方程：

上式中，φ、θ、分别为横滚角、俯仰角、航向角，分别为三轴陀螺仪x、y、z轴上的输出量，分别为地速在机体系x、y、z轴上的投影，分别为风速在机体系x、y轴上的投影，为三轴加速度计z轴上的输出量，ω_wx、ω_wy分别为的驱动高斯白噪声，g是重力加速度，k₁、k₂分别为机体系x、y轴上的阻力系数，Ω＝Ω₁+Ω₂+...+Ω_l；

所述多信息融合导航系统的量测方程：

上式中，分别为GPS速度在机体系x、y、z轴上的投影，分别为三轴加速度计x、y轴上的输出量。

步骤4：采用卡尔曼滤波对导航信息进行更新，得到风速的实时估计。

首先将多信息融合导航系统的状态方程和量测方程离散化，得到卡尔曼滤波器的状态方程和量测方程：

Z＝HX+V

上式中，状态量控制量W为系统噪声阵，是均值为0的高斯白噪声；量测量V为量测噪声阵，是均值为0的高斯白噪声，其中：

采用卡尔曼滤波器对多信息融合导航系统进行更新：

X_k+1＝Φ_k+1,kX_k+B_kU_k+W_k

Z_k+1＝H_k+1X_k+1+V_k+1

上式中，X_k+1为k+1时刻的状态向量，U_k为k时刻的控制向量，Φ_k+1,k为k到k+1时刻的系统一步转移矩阵，B_k为U_k的系数矩阵；W_k为k时刻的系统噪声阵，Z_k+1为k+1时刻的状态向量，H_k+1为k+1时刻的量测矩阵，V_k+1为k+1时刻的量测噪声。

首先进行状态一步预测：

其次求解一步预测的均方差误差：

上式中，为k时刻状态向量X_k的卡尔曼滤波估值，是利用计算得到的对k+1时刻状态向量X_k+1的预测值，P_k为k时刻估计均方差误差，Q_k为W_k的方差矩阵，P_k+1|k为的均方差误差；

然后求解滤波增益：

上式中，R_k+1为V_k+1的方差矩阵；

最后对状态估值，得到滤波结果：

同时更新估计均方差误差：

P_k+1＝[I-K_k+1H_k+1]P_k+1|k

按照上述步骤进行循环更新，卡尔曼滤波器输出风速的估计。

实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (5)

1.一种面向多旋翼飞行器的多信息融合实时风速估计方法，其特征在于，包括以下步骤：

(1)采集机载传感器数据，包括三轴加速度计的输出数据、三轴陀螺仪的输出数据和各旋翼转速数据；

(2)根据步骤(1)获取的数据，构建多旋翼飞行器的动力学模型，得到多旋翼飞行器阻力与风速的关系；

(3)基于多旋翼飞行器动力学模型特性、惯性传感器信息和GPS信息，根据多旋翼飞行器运动学模型，建立多信息融合导航系统的状态方程和量测方程；

(4)采用卡尔曼滤波对导航信息进行更新，得到风速的实时估计。

2.根据权利要求1所述面向多旋翼飞行器的多信息融合实时风速估计方法，其特征在于：在步骤(2)中，所述多旋翼飞行器的动力学模型：

$\begin{array}{c}{F}_H=mk\left(\underset{i=1}{\overset{l}{\Σ}}{\mathrm{\Ω}}_i\right)V_A^H\\ =mk\left(\underset{i=1}{\overset{l}{\Σ}}{\mathrm{\Ω}}_i\right)(V_{nb}^{bH}-V_w^{bH})\end{array}$

上式中，F_H为多旋翼飞行器受到的阻力，m为飞行器质量，k为阻力系数，Ω_i为飞行器第i个旋翼的转速，l为飞行器的旋翼总数，V_A ^H为空速在机体系下的水平分量，为地速在机体系下的水平分量，为风速在机体系下的水平分量。

3.根据权利要求2所述面向多旋翼飞行器的多信息融合实时风速估计方法，其特征在于：在步骤(3)中，所述多信息融合导航系统的状态方程：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}=sin\mathrm{\φ}\tan \mathrm{\θ}{\mathrm{\ω}}_{nbx}^b+{\mathrm{\ω}}_{nby}^b-cos\mathrm{\φ}tan\mathrm{\θ}{\mathrm{\ω}}_{nbz}^b\\ \stackrel{\·}{\mathrm{\θ}}={\mathrm{cos\φ\ω}}_{nbx}^b+{\mathrm{sin\φ\ω}}_{nbz}^b\\ \stackrel{\·}{\mathrm{\ψ}}={\mathrm{sin\φ\ω}}_{nbx}^b/\cos \mathrm{\θ}-{\mathrm{cos\φ\ω}}_{nbz}^b/cos\mathrm{\θ}\\ {\stackrel{\·}{V}}_{nbx}^b=-g\sin \mathrm{\φ}\cos \mathrm{\θ}+k_1\mathrm{\Ω}(V_{nbx}^b-V_{wx}^b)\\ {\stackrel{\·}{V}}_{nby}^b=g\sin \mathrm{\θ}+k_2\mathrm{\Ω}(V_{nby}^b-V_{wy}^b)\\ {\stackrel{\·}{V}}_{nbz}^b=g\cos \mathrm{\φ}\cos \mathrm{\θ}-f_{nbz}^b\\ {\stackrel{\·}{V}}_{wx}^b={\mathrm{\ω}}_{wx}\\ {\stackrel{\·}{V}}_{wy}^b={\mathrm{\ω}}_{wy}\end{array}\right.$

上式中，φ、θ、分别为横滚角、俯仰角、航向角，分别为三轴陀螺仪x、y、z轴上的输出量，分别为地速在机体系x、y、z轴上的投影，分别为风速在机体系x、y轴上的投影，为三轴加速度计z轴上的输出量，ω_wx、ω_wy分别为的驱动高斯白噪声，g是重力加速度，k₁、k₂分别为机体系x、y轴上的阻力系数，Ω＝Ω₁+Ω₂+...+Ω_l；

所述多信息融合导航系统的量测方程：

$\left\{\begin{array}{c}{V}_{GPSx}^b=V_{nbx}^b\\ V_{GPSy}^b=V_{nby}^b\\ V_{GPSz}^b=V_{nbz}^b\\ f_{nbx}^b=k_1({\mathrm{\Ω}}_1+{\mathrm{\Ω}}_2+...+{\mathrm{\Ω}}_i)(V_{nbx}^b-V_{wx}^b)\\ f_{nby}^b=k_2({\mathrm{\Ω}}_1+{\mathrm{\Ω}}_2+...+{\mathrm{\Ω}}_i)(V_{nby}^b-V_{wy}^b)\end{array}\right.$

上式中，分别为GPS速度在机体系x、y、z轴上的投影，分别为三轴加速度计x、y轴上的输出量。

4.根据权利要求3所述面向多旋翼飞行器的多信息融合实时风速估计方法，其特征在于：在步骤(4)中，首先将多信息融合导航系统的状态方程和量测方程离散化，得到卡尔曼滤波器的状态方程和量测方程：

$\stackrel{\·}{X}=AX+BU+W$

Z＝HX+V

上式中，状态量控制量W为系统噪声阵，是均值为0的高斯白噪声；量测量V为量测噪声阵，是均值为0的高斯白噪声，其中：

$A=\left[\begin{array}{cccccccc}0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& k_1\mathrm{\Ω}& 0& 0& -k_1\mathrm{\Ω}& 0\\ 0& 0& 0& 0& k_2\mathrm{\Ω}& 0& 0& -k_2\mathrm{\Ω}\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

$B=\left[\begin{array}{ccccccc}\sin \mathrm{\φ}\tan \mathrm{\θ}& 1& -\cos \mathrm{\φ}\tan \mathrm{\θ}& 0& 0& 0& 0\\ \cos \mathrm{\φ}& 0& \sin \mathrm{\φ}& 0& 0& 0& 0\\ \sin \mathrm{\φ}/\cos & 0& -\cos \mathrm{\φ}/\cos \mathrm{\θ}& 0& 0& 0& 0\\ 0& 0& 0& -\sin \mathrm{\φ}\cos \mathrm{\θ}& 0& 0& 0\\ 0& 0& 0& \sin \mathrm{\θ}& 0& 0& 0\\ 0& 0& 0& \cos \mathrm{\φ}\cos \mathrm{\θ}& -1& 0& 0\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]$

$H=\left[\begin{array}{cccccccc}0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& k_1\mathrm{\Ω}& 0& 0& -k_1\mathrm{\Ω}& 0\\ 0& 0& 0& 0& k_2\mathrm{\Ω}& 0& 0& -k_2\mathrm{\Ω}\end{array}\right].$

5.根据权利要求4所述面向多旋翼飞行器的多信息融合实时风速估计方法，其特征在于：在步骤(4)中，采用卡尔曼滤波器对多信息融合导航系统进行更新：

X_k+1＝Φ_k+1,kX_k+B_kU_k+W_k

Z_k+1＝H_k+1X_k+1+V_k+1

上式中，X_k+1为k+1时刻的状态向量，U_k为k时刻的控制向量，Φ_k+1,k为k到k+1时刻的系统一步转移矩阵，B_k为U_k的系数矩阵；W_k为k时刻的系统噪声阵，Z_k+1为k+1时刻的状态向量，H_k+1为k+1时刻的量测矩阵，V_k+1为k+1时刻的量测噪声；

首先进行状态一步预测：

${\hat{X}}_{k+1|k}={\mathrm{\Φ}}_{k+1,k}{\hat{X}}_{k|k}+B_k{U}_k$

其次求解一步预测的均方差误差：

$P_{k+1|k}={\mathrm{\Φ}}_{k+1,k}{P}_k{\mathrm{\Φ}}_{k+1,k}^T+Q_k$

上式中，为k时刻状态向量X_k的卡尔曼滤波估值，是利用计算得到的对k+1时刻状态向量X_k+1的预测值，P_k为k时刻估计均方差误差，Q_k为W_k的方差矩阵，P_k+1|k为的均方差误差；

然后求解滤波增益：

$K_{k+1}=P_{k+1|k}{H}_{k+1}^T{\[H_{k+1}{P}_{k+1|k}{H}_{k+1}^T+R_{k+1}\]}^{-1}$

上式中，R_k+1为V_k+1的方差矩阵；

最后对状态估值，得到滤波结果：

${\hat{X}}_{k+1|k+1}={\hat{X}}_{k+1|k}+K_{k+1}\[Z_{k+1}-H_{k+1}{\hat{X}}_{k+1|k}\]$

同时更新估计均方差误差：

P_k+1＝[I-K_k+1H_k+1]P_k+1|k

按照上述步骤进行循环更新，卡尔曼滤波器输出风速的估计。