[发明专利]基于UKF的无人机气压计异常数据处理的高度控制方法

权利要求书

1.一种基于UKF的无人机气压计异常数据处理的高度控制方法，其特征在于：利用加速度计的Z方向的加速度信息以及加速度计和气压计第一次UKF滤波出来的速度后对其差分得到的加速度信息做差,利用差值来量化气流扰动对气压计的影响,具体包括带加速度计和气压计的飞行器，加速度计通过降噪处理获得加速度信息，气压计通过降噪处理获得高度信息，后经第一次UKF滤波，第一次UKF滤波后更新气压计数据，更新后进行第二次UKF滤波，第二次UKF滤波后将第二次滤波位置信息以及第二次滤波速度信息传递给飞控位置控制模块；

具体步骤如下：

步骤一、构建无人机高度方向的系统方程和量测方程：

在只考虑气压计(Barometer)和加速度计(Accelerometer)的情况下，构造无人机高度方向的系统方程和量测方程为：

系统方程：

量测方程：

式中，h_k、ve_k、acc_bias_k分别为k时刻Z方向的位置、速度和加速度偏移量；Ts为时间步长；为加速度计经旋转矩阵后得到的NED坐标系下Z方向的加速度；Z_k为k时刻气压计的高度数据，在实际使用中由于气压计数据噪声太大，会首先对其进行一阶低通滤波处理；W_k-1、V_k分别为为系统方程和量测方程的协方差矩阵，在实际使用过程中会通过调节协方差矩阵的大小来使得滤波出来的结果跟随气压计的特性；

步骤二、第一次UKF滤波：

对于离散时间非线性系统：

式中，x_k是n×1维的状态向量；y_k是m×1维的量测向量，B_k-1是n×l维的系统噪声分配矩阵；ω_k-1是l×1维的系统噪声向量，ν_k-1是m×1维的量测噪声向量，f(·)和h(·)均为时间参数离散而状态连续的非线性向量函数，无人机系统常用的扩展卡尔曼滤波算法和无迹卡尔曼滤波算法，整体框架一样：

式中，K_k为卡尔曼增益；P_x,k/k-1、P_y,k/k-1、P_xy,k/k-1为系统和量测协方差矩阵；为系统变量和量测变量预测值；P_x,k为系统变量和协方差更新值；

在UKF滤波算法中，需要对状态预测和量测预测分别进行UT变换来更新系统状态；

状态预测UT变换：

量测预测UT变换：

步骤三、高度Z的更新：

(一)由步骤2中解算得到的速度差分得到气压计和加速度计滤波得到高度方向的加速度AccZ__acc+baro；再与加速度计获取的高度方向的加速度AccZ__acc做差值；得到加速度差值AccZ_err,以此参数来量化气流扰动对气压计的影响；

(二)通过AccZ_err的曲线变化，设定合适的阈值AccZ__threshold，当当前时刻的AccZ_err的绝对值小于阈值时，用气压计数据更新高度；而当当前时刻的AccZ_err的绝对值大于阈值时，则认为此时气压计受气流扰动较大，此时气压计数据不准确，则用前一时刻的气压计数据更新高度；

步骤四、第二次UKF滤波；

利用步骤三中更新的高度信息Z_new作为量测值来更新UKF的状态和协方差信息；

此时，量测值yk＝Z_new,系统状态Xk和协方差Pk的信息依旧由UT变换得到，最后，将第二次UKF滤波位置解算得到的高度方向的位置、速度信息输入给飞控位置控制模块，从而使得高度控制更为准确，飞行体验大大提高；

其中，bias_k：气压计高度估计的偏置量；

γ：状态方差量的加权值；

Q_k-1：状态方差量的协方差矩阵；

R_k：量测方差量的协方差矩阵。