[发明专利]一种基于气压计/IMU/GPS多传感器融合的旋翼无人机定高算法

说明书

本发明提供了一种基于气压计/IMU/GPS多传感器融合的旋翼无人机定高算法，包括以下步骤：步骤1，对气压计进行预处理并对气压计的量测高度进行高度建模；步骤2，取地理坐标系下的垂向高度x_z、垂向速度v_z、加速度计Z轴零偏b_z以及气压计零偏b_p作为状态量X来构造卡尔曼滤波算法的状态方程；步骤3，根据状态预测方程构造气压计高度的量测方程与GPS速度的量测方程；步骤4，对卡尔曼滤波算法中的的状态量X以及误差协方差阵进行更新；步骤5，在气压计的数据更新时，对气压计高度进行量测更新来获得气压计的高度即垂向高度，并在GPS速度更新时，对GPS速度进行量测更新来获得GPS速度即垂向速度。

技术领域

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

背景技术

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

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

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

发明内容

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

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

步骤2，取地理坐标系下的垂向高度x_z、垂向速度v_z、加速度计Z轴零偏b_z以及气压计零偏b_p作为状态量X来构造卡尔曼滤波算法的状态方程；

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

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

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

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