一种基于气压计/IMU/GPS多传感器融合的旋翼无人机定高算法的制作方法

本发明属于无人机定高领域，具体涉及一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法。

背景技术：

旋翼类无人机在室外进行高速飞行时，常需要进行定高控制，保证无人机高度的稳定性。机载端能提供高度信息的传感器主要有气压计、gps、超声波等。其中，gps的高度测量误差通常在5-10m之间，无法用于无人机的高度控制；超声波提供的是距离地面的相对高度，且量测范围有限；通常商业无人机飞控普遍使用气压计获取无人机飞行高度，进行高度控制。

大气压与海拔高度的关系可由如下公式表示。通过气压计测量气压数据，经过换算即可得出当前位置点的海拔高度。然而在无人机高速飞行时，常受气流、阵风影响使得气压计测量偏离真实气压值。图1为无人机做定高航线飞行时，机载sd卡记录的无人机飞行速度和气压计换算出的高度的关系曲线。图1中alt为无人机高度基准，baralt为气压计推算出的高度，spd为无人机的飞行速度，如图1所示，每当无人机做加速和减速以及高速飞行时，气压计的高度数据受气流影响，将产生2～5米的高度误差。若按照此高度数据进行定高控制，必然造成无人机坠地发生事故。

一些飞控厂家尝试在无人机高速飞行时，通过调整无人机的动力输出来维持高度不变，这显然没有从本质上解决问题，也有使用超声波或者毫米波雷达进行定高，但这提供的都是相对高度且受无人机下方复杂环境影响。在一些高端行业应用里，常使用rtk进行高度控制，但其成本很高，且使用条件较为复杂。

技术实现要素：

本发明是为了解决上述问题而进行的，目的在于提供一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法。

本发明提供了一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法，具有这样的特征，包括以下步骤：步骤1，对气压计进行预处理并对气压计的量测高度进行高度建模；

步骤2，取地理坐标系下的垂向高度xz、垂向速度vz、加速度计z轴零偏bz以及气压计零偏bp作为状态量x来构造卡尔曼滤波算法的状态方程；

步骤3，根据状态预测方程构造气压计高度的量测方程与gps速度的量测方程；

步骤4，对卡尔曼滤波算法中的的状态量x以及误差协方差阵进行更新；

步骤5，在气压计的数据更新时，对气压计高度进行量测更新来获得气压计的高度即垂向高度，并在gps速度更新时，对gps速度进行量测更新来获得gps速度即垂向速度。

在本发明提供的一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法中，还可以具有这样的特征：其中，步骤2中，状态预测方程为：

公式(1)中，x(t)为t时刻的状态量，为t+1时刻的预测状态量，f(t)为状态转移矩阵，b(t)为控制矩阵，u(t)为控制量，u(t)＝ⁿaz，其中ⁿaz为地理坐标系下的垂向加速度，通过无人机的航姿系统与加速度计数据计算得到，w(t)＝[wpzwvzwbzwbp]^t，w(t)为零均值白噪声且满足w～n(0,q)，q为系统噪声矩阵。

在本发明提供的一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法中，还可以具有这样的特征：其中，步骤3中，气压计高度的量测方程为：

z^baro(t)＝h^baro(t)x(t)+v^baro(t)(2)

gps速度的量测方程为：

z^gps(t)＝h^gps(t)x(t)+v^gps(t)(3)

公式(2)中，h^baro为气压计的量测矩阵，h^baro＝[1001]，z^baro(t)为t时刻的气压计的高度，v^baro(t)为零均值白噪声且满足v^baro(t)～n(0,vb)，vb为气压计的量测噪声矩阵，

公式(3)中，h^gps为gps速度的量测矩阵，h^gps＝[0100]，z^gps(t)为t时刻gps测量的垂向速度，v^gps为零均值白噪声且满足v^gps～n(0,vg)，vg为垂向速度的噪声矩阵。

在本发明提供的一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法中，还可以具有这样的特征：其中，步骤4中，卡尔曼滤波算法中的状态量x以及误差协方差阵更新方程为：

公式(4)和公式(5)中，为k时刻的预测状态量，为公式(1)中的xk-1为k-1时刻的状态量，xk-1为公式(1)中的x(t)，φk|k-1为公式(1)中的f(t)，b为公式(1)中的b(t)，uk为公式(1)中的u(t)，为k时刻的预测误差协方差阵，pk-1为k-1时刻的误差协方差阵，r为uk的噪声矩阵，qk为q，即系统噪声矩阵。

在本发明提供的一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法中，还可以具有这样的特征：其中，步骤5中，气压计高度的量测更新方程为：

gps速度的量测更新方程为：

公式(6)、公式(7)和公式(8)中，kk为卡尔曼增益系数，hk为公式(2)中的h^baro，vb为气压计对应的传感器量测噪声矩阵，i为单位矩阵，pk为更新后的误差协方差阵，xk为更新后的k时刻的状态量，通过该状态量xk中获得k时刻的垂向高度xz和气压计零偏bp，为k时刻的预测状态量，z^baro为气压计的高度即公式(2)中的z^baro(t)，h^baro＝[1001]，

公式(9)、公式(10)和公式(11)中，kk为卡尔曼增益系数，hk公式(3)中为h^gps，vg为垂向速度的噪声矩阵，i为单位矩阵，pk为更新后的误差协方差阵，xk为更新后的状态量，通过该状态量xk来获得k时刻的垂向速度vz，为k时刻的状态量，z^gps为垂向速度即公式(3)中的z^gps(t)，h^gps＝[0100]。

在本发明提供的一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法中，还可以具有这样的特征：其中，步骤1中还包括以下子步骤：

步骤1-1，在气压计上压盖海绵来防止短时气流和光照对气压计进行干扰；

步骤1-2，对量测高度进行滑动平滑滤波，从而降低量测噪声对量测高度的影响，量测高度的计算公式为：

h＝h0+εb+w，

其中，h为气压计的量测高度即为地理坐标系下的垂向高度xz，h0为当前真实理想高度，εb为气压计的常值漂移噪声，由温度因素、湿度因素以及气候因素引起，w为白噪声，由系统量化噪声引起。

在本发明提供的一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法中，还可以具有这样的特征：其中，气压计的量测噪声矩阵vb在无人机的运动加速度大、无人机的转动角速度大以及无人机的飞行速度快时进行增大来降低气压计高度对卡尔曼滤波器中高度数据的修正力度，通过imu的三轴加速度计ax,ay,az来对无人机的运动加速度进行判断，当时，ε为运动加速度判断阈值，增大气压计对应的量测噪声矩阵vb，来降低量测高度h对滤波器高度数据的修正力度，从而完成对应于气压计的量测更新，通过imu的三轴陀螺gx,gy,gz来判断无人机在x轴或y轴的转动角速度，当时，β为转动角速度判断阈值，增大气压计对应的量测噪声矩阵vb，来降低量测高度h对滤波器高度数据的修正力度，从而完成对应于气压计的量测更新，当无人机高速飞行时，即当|vh|＞vγ时，vh为无人机的水平速度，vγ为速度判断阈值，增大气压计对应的量测噪声矩阵vb，来降低量测高度h对滤波器高度数据的修正力度，从而完成对应于气压计的量测更新。

发明的作用与效果

根据本发明所涉及的一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法，因为能够在无人机进行强机动以及高速飞行时，通过调整卡尔曼滤波算法中的量测噪声来减小气压计扰动对于滤波器高度的影响，并且通过融合gps测量的垂向速度来保证卡尔曼滤波算法在弱量测修正力度情况下的垂向速度精度，所以能够提高卡尔曼滤波算法对于无人机高度输出的稳定性；因为通过gps/imu/气压计的数据融合并利用卡尔曼滤波算法对无人机垂向高度与速度进行计算，所以，在不需要引入如rtk，毫米波雷达、超声波测距模块等额外传感器，就能够实现旋翼无人机在高机动状态下的定高飞行，保证了飞行安全，提高了飞行作业品质。因此本发明的的一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法方法简便，数据处理快捷，能够在不添加额外传感器的情况下为无人机提供稳定可靠的高度数据。

附图说明

图1是本发明的背景技术中的无人机定高航线飞行的飞行速度和气压计高度的关系曲线图；

图2是本发明的实施例中无人机定高飞行时气压计/imu/gps数据融合的流程图；

图3是本发明的实施例中无人机的飞行轨迹图；

图4是本发明的实施例中一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法计算得到的高度与rtk高度测量数据对比曲线图。

具体实施方式

为了使本发明实现的技术手段与功效易于明白了解，以下结合实施例及附图对本发明作具体阐述。

实施例：

本实施例的一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法，包括以下步骤:

步骤1，对气压计进行预处理并对气压计的量测高度进行高度建模。

步骤1中还包括以下子步骤：步骤1-1，在气压计上压盖海绵来防止短时气流和光照对气压计进行干扰。

步骤1-2，对量测高度进行滑动平滑滤波，从而降低量测噪声对量测高度的影响，量测高度的计算公式为：

h＝h0+εb+w，

其中，h为气压计的量测高度即为地理坐标系下的垂向高度xz，h0为当前真实理想高度，εb为气压计的常值漂移噪声，由温度因素、湿度因素以及气候因素引起，w为白噪声，由系统量化噪声引起。

图2是本发明的实施例中无人机定高飞行时气压计/imu/gps数据融合的流程图。

如图2所示，步骤2，取地理坐标系下的垂向高度xz、垂向速度vz、加速度计z轴零偏bz以及气压计零偏bp作为状态量x来构造卡尔曼滤波算法的状态方程。

步骤2中，状态方程为：

公式(1)中，x(t)为t时刻的状态量，为t+1时刻的预测状态量，f(t)为状态转移矩阵，b(t)为控制矩阵，u(t)为控制量，u(t)＝ⁿaz，其中ⁿaz为地理坐标系下的垂向加速度，通过无人机的航姿系统与加速度计数据计算得到，w(t)＝[wpzwvzwbzwbp]^t，w(t)为零均值白噪声且满足w～n(0,q)，q为系统噪声矩阵。

步骤3，根据状态预测方程构造气压计高度的量测方程与gps速度的量测方程。

步骤3中，气压计高度的量测方程为：

z^baro(t)＝h^baro(t)x(t)+v^baro(t)(2)

gps速度的量测方程为：

z^gps(t)＝h^gps(t)x(t)+v^gps(t)(3)

公式(2)中，h^baro为气压计的量测矩阵，h^baro＝[1001]，z^baro(t)为t时刻的气压计的高度，v^baro(t)为零均值白噪声且满足v^baro(t)～n(0,vb)，vb为气压计的量测噪声矩阵，

公式(3)中，h^gps为gps速度的量测矩阵，h^gps＝[0100]，z^gps(t)为t时刻gps测量的垂向速度，v^gps为零均值白噪声且满足v^gps～n(0,vg)，vg为垂向速度的噪声矩阵。

气压计的量测噪声矩阵vb在无人机的运动加速度大、无人机的转动角速度大以及无人机的飞行速度快时进行增大来降低气压计高度对卡尔曼滤波器中高度数据的修正力度，

通过imu的三轴加速度计ax,ay,az来对无人机的运动加速度进行判断，当时，ε为运动加速度判断阈值，增大气压计对应的量测噪声矩阵vb，来降低量测高度h对滤波器高度数据的修正力度，从而完成对应于气压计的量测更新，

通过imu的三轴陀螺gx,gy,gz来判断无人机在x轴或y轴的转动角速度，当时，β为转动角速度判断阈值，增大气压计对应的量测噪声矩阵vb，来降低量测高度h对滤波器高度数据的修正力度，从而完成对应于气压计的量测更新，

当无人机高速飞行时，即当|vh|＞vγ时，vh为无人机的水平速度，vγ为速度判断阈值，增大气压计对应的量测噪声矩阵vb，来降低量测高度h对滤波器高度数据的修正力度，从而完成对应于气压计的量测更新。

步骤4，对卡尔曼滤波算法中的的状态量x以及误差协方差阵进行更新。

步骤4中，卡尔曼滤波算法中的状态量x以及误差协方差阵更新方程为：

公式(4)和公式(5)中，为k时刻的预测状态量，为公式(1)中的xk-1为k-1时刻的状态量，xk-1为公式(1)中的x(t)，φk|k-1为公式(1)中的f(t)，b为公式(1)中的b(t)，uk为公式(1)中的u(t)，为k时刻的预测误差协方差阵，pk-1为k-1时刻的误差协方差阵，r为uk的噪声矩阵，qk为q，即系统噪声矩阵。

步骤5，在气压计的数据更新时，对气压计高度进行量测更新来获得气压计的高度即垂向高度，并在gps速度更新时，对gps速度进行量测更新来获得gps速度即垂向速度，在进行gps速度的量测更新前，通过gps的可见星数以及垂向精度因子等指标来衡量gps信号，并在gps信号良好的情况下开始gps速度量测更新。

气压计高度的量测更新方程为：

gps速度的量测更新方程为：

公式(6)、公式(7)和公式(8)中，kk为卡尔曼增益系数，hk为公式(2)中的h^baro，vb为气压计对应的传感器量测噪声矩阵，i为单位矩阵，pk为更新后的误差协方差阵，xk为更新后的k时刻的状态量，通过该状态量xk中获得k时刻的垂向高度xz和气压计零偏bp，为k时刻的预测状态量，z^baro为气压计的高度即公式(2)中的z^baro(t)，h^baro＝[1001]，

公式(9)、公式(10)和公式(11)中，kk为卡尔曼增益系数，hk公式(3)中为h^gps，vg为垂向速度的噪声矩阵，i为单位矩阵，pk为更新后的误差协方差阵，xk为更新后的状态量，通过该状态量xk来获得k时刻的垂向速度vz，为k时刻的状态量，z^gps为垂向速度即公式(3)中的z^gps(t)，h^gps＝[0100]。

采用本实施例的一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法进行无人机定高的效果测试过程：

将本实施例的基于气压计/imu/gps多传感器融合的旋翼无人机定高算法在开源飞控硬件平台pixhawk2上编程实现，其硬件组成主要包括：cpu：stm32f427

imu：mpu9250

气压计：ms5611

gps：ublox-m8n

为了便于定量的分析高度数据，搭载了rtk高精度定位系统novateloem615板卡，该板卡最高支持50hz的rtk定位输出，位置测量精度优于3cm。

图3是本发明的实施例中无人机的飞行轨迹图。

如图3所示，无人机按照线路飞行，并在飞行过程中通过机载sd卡实时记录飞行数据，通过配套的地面站missionplaner进行数据分析，待rtk固定解后起飞进行定高航线飞行，并将本实施例一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法计算得到的高度与rtk高度进行对比。

图4是本发明的实施例中一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法计算得到的高度与rtk高度测量数据对比曲线图。

如图4所示，在整个飞行过程中本实施例的一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法计算得到的高度与rtk高度基准相差不超过0.5m，且得到的高度数据平滑稳定。

综上可知，本实施例的一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法能够有效的保证计算得到高度的稳定性。

实施例的作用与效果

根据本实施例所涉及的一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法，因为能够在无人机进行强机动以及高速飞行时，通过调整卡尔曼滤波算法中的量测噪声来减小气压计扰动对于滤波器高度的影响，并且通过融合gps测量的垂向速度来保证卡尔曼滤波算法在弱量测修正力度情况下的垂向速度精度，所以能够提高卡尔曼滤波算法对于无人机高度输出的稳定性；因为通过gps/imu/气压计的数据融合并利用卡尔曼滤波算法对无人机垂向高度与速度进行计算，所以，在不需要引入如rtk，毫米波雷达、超声波测距模块等额外传感器，就能够实现旋翼无人机在高机动状态下的定高飞行，保证了飞行安全，提高了飞行作业品质。因此本实施例的的一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法方法简便，数据处理快捷，能够在不添加额外传感器的情况下为无人机提供稳定可靠的高度数据。

上述实施方式为本发明的优选案例，并不用来限制本发明的保护范围。