CN107577238A - 基于ukf的无人机气压计异常数据处理的高度控制方法 - Google Patents
基于ukf的无人机气压计异常数据处理的高度控制方法 Download PDFInfo
- Publication number
- CN107577238A CN107577238A CN201710743039.6A CN201710743039A CN107577238A CN 107577238 A CN107577238 A CN 107577238A CN 201710743039 A CN201710743039 A CN 201710743039A CN 107577238 A CN107577238 A CN 107577238A
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- mtd
- mover
- mtr
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Landscapes
- Traffic Control Systems (AREA)
- Navigation (AREA)
Abstract
本发明涉及无人机领域，具体涉及一种基于UKF的无人机气压计异常数据处理的高度控制方法，利用加速度计的Z方向的加速度信息以及加速度计和气压计第一次UKF滤波出来的速度后对其差分得到的加速度信息做差,利用差值来量化气流扰动对气压计的影响,具体包括带加速度计和气压计的飞行器，气压计通过降噪处理获得高度信息，后经第一次UKF滤波，第一次UKF滤波后更新气压计数据，更新后进行第二次UKF滤波。本发明基于UKF的滤波算法，对气压计的高度异常数据进行判断并对野值进行剔除，解决了飞行器在地面效应下难以着陆和飞行中的急刹掉高问题，提高了飞机的高度飞行控制效果，大大提升用户的飞行体验。
Description
技术领域
本发明涉及无人机领域，具体涉及一种基于UKF的无人机气压计异常数据处理的高度控制方法。
背景技术
飞行器想要稳定飞行，需要15个状态量：三维角度，三维角度对应的三维速度、三维加速度、三维角速度，三维位置。而这些状态量的获取大都是通过多传感器数据滤波得到。常用的传感器的测量如下：
而在实际中，光流/GPS/双目视觉等方法都对环境有特定的要求，如光流在地面纹理相似或草地上会失效；GPS在室内会失效；双目视觉在光照条件差时会失效。而惯性测量单元IMU和气压计基本在绝大部分场景都可以使用，但气压计存在当无人飞行器在地面效应或急速刹车时高度数据异常的情况，导致飞机难以降落和急刹掉高的现象。
发明内容
本发明针对在只有IMU模块(加速度计)和气压计有效的情况下，飞行器在起飞着陆时的地面效应问题和飞行过程中的急刹掉高现象，对气压计异常数据进行相应处理。本文基于UKF的滤波算法，对气压计的高度异常数据进行判断并对野值进行剔除，解决了飞行器在地面效应下难以着陆和飞行中的急刹掉高问题，提高了飞机的高度飞行控制效果，大大提升用户的飞行体验。
本发明是通过以下技术方案来实现的：一种基于UKF的无人机气压计异常数据处理的高度控制方法，利用加速度计的Z方向的加速度信息以及加速度计和气压计第一次UKF滤波出来的速度后对其差分得到的加速度信息做差,利用差值来量化气流扰动对气压计的影响,具体包括带加速度计和气压计的飞行器，加速度计通过降噪处理获得加速度信息，气压计通过降噪处理获得高度信息，后经第一次UKF滤波，第一次UKF滤波后更新气压计数据，更新后进行第二次UKF滤波，第二次UKF滤波后将第二次滤波位置信息以及第二次滤波速度信息传递给飞控位置控制模块。
具体步骤如下：
步骤一、构建无人机高度方向的系统方程和量测方程：
在只考虑气压计(Barometer)和加速度计(Accelerometer)的情况下，构造无人机高度方向的系统方程和量测方程为：
系统方程：
量测方程：
式中，hk、vek、acc_biask分别为k时刻Z方向的位置、速度和加速度偏移量；Ts为时间步长；为加速度计经旋转矩阵后得到的NED坐标系下Z方向的加速度；Zk为k时刻气压计的高度数据，在实际使用中由于气压计数据噪声太大，会首先对其进行一阶低通滤波处理；Wk-1、Vk分别为为系统方程和量测方程的协方差矩阵，在实际使用过程中会通过调节协方差矩阵的大小来使得滤波出来的结果跟随气压计的特性；
步骤二、第一次UKF滤波：
对于离散时间非线性系统：
式中，xk是n×1维的状态向量；yk是m×1维的量测向量，Bk-1是n×l维的系统噪声分配矩阵；ωk-1是l×1维的系统噪声向量，νk-1是m×1维的量测噪声向量，f(·)和h(·)均为时间参数离散而状态连续的非线性向量函数，无人机系统常用的扩展卡尔曼滤波算法和无迹卡尔曼滤波算法，整体框架一样：
式中，Kk为卡尔曼增益；Px,k/k-1、Py,k/k-1、Pxy,k/k-1为系统和量测协方差矩阵；为系统变量和量测变量预测值；Px,k为系统变量和协方差更新值；
在UKF滤波算法中，需要对状态预测和量测预测分别进行UT变换来更新系统状态；
状态预测UT变换：
量测预测UT变换：
步骤三、高度Z的更新：
(一)由步骤2中解算得到的速度差分得到气压计和加速度计滤波得到高度方向的加速度AccZ_acc+baro；再与加速度计获取的高度方向的加速度AccZ_acc做差值；得到加速度差值AccZ_err,以此参数来量化气流扰动对气压计的影响；
(二)通过AccZ_err的曲线变化，设定合适的阈值AccZ_threshold，当当前时刻的AccZ_err的绝对值小于阈值时，用气压计数据更新高度；而当当前时刻的AccZ_err的绝对值大于阈值时，则认为此时气压计受气流扰动较大，此时气压计数据不准确，则用前一时刻的气压计数据更新高度；
步骤四、第二次UKF滤波；
利用步骤3中更新的高度信息Z_new作为量测值来更新UKF的状态和协方差信息；
此时，量测值yk＝Z_new,系统状态Xk和协方差Pk的信息依旧由UT变换得到。最后，将第二次UKF滤波位置解算得到的高度方向的位置、速度信息输入给飞控位置控制模块，从而使得高度控制更为准确，飞行体验大大提高。
本发明的有益效果是：本发明基于UKF的滤波算法，对气压计的高度异常数据进行判断并对野值进行剔除，解决了飞行器在地面效应下难以着陆和飞行中的急刹掉高问题，提高了飞机的高度飞行控制效果，大大提升用户的飞行体验。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明的整体流程图；
图2为本发明的气压计高度与UKF滤波高度对比图；
图3为本发明的停刹车时刻气压计高度与更新后高度对比图；
图4为本发明的起飞着陆时刻气压计高度与更新后高度对比图。
具体实施方式
本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任何方式组合。
本说明书(包括任何附加权利要求、摘要和附图)中公开的任一特征，除非特别叙述，均可被其他等效或具有类似目的的替代特征加以替换。即，除非特别叙述，每个特征只是一系列等效或类似特征中的一个例子而已。
如图1所示，利用加速度计的Z方向的加速度信息以及加速度计和气压计第一次UKF滤波出来的速度后对其差分得到的加速度信息做差,利用差值来量化气流扰动对气压计的影响,具体包括带加速度计和气压计的飞行器，加速度计通过降噪处理获得加速度信息，气压计通过降噪处理获得高度信息，后经第一次UKF滤波，第一次UKF滤波后更新气压计数据，更新后进行第二次UKF滤波，第二次UKF滤波后将第二次滤波位置信息以及第二次滤波速度信息传递给飞控位置控制模块。
具体步骤如下：
步骤一、构建无人机高度方向的系统方程和量测方程：
在只考虑气压计(Barometer)和加速度计(Accelerometer)的情况下，构造无人机高度方向的系统方程和量测方程为：
系统方程：
量测方程：
式中，hk、vek、acc_biask分别为k时刻Z方向的位置、速度和加速度偏移量；Ts为时间步长；为加速度计经旋转矩阵后得到的NED坐标系下Z方向的加速度；Zk为k时刻气压计的高度数据，在实际使用中由于气压计数据噪声太大，会首先对其进行一阶低通滤波处理；Wk-1、Vk分别为为系统方程和量测方程的协方差矩阵，在实际使用过程中会通过调节协方差矩阵的大小来使得滤波出来的结果跟随气压计的特性；
步骤二、第一次UKF滤波：
对于离散时间非线性系统：
式中，xk是n×1维的状态向量；yk是m×1维的量测向量，Bk-1是n×l维的系统噪声分配矩阵；ωk-1是l×1维的系统噪声向量，νk-1是m×1维的量测噪声向量，f(·)和h(·)均为时间参数离散而状态连续的非线性向量函数，无人机系统常用的扩展卡尔曼滤波算法和无迹卡尔曼滤波算法，整体框架一样：
式中，Kk为卡尔曼增益；Px,k/k-1、Py,k/k-1、Pxy,k/k-1为系统和量测协方差矩阵；为系统变量和量测变量预测值；Px,k为系统变量和协方差更新值；
在UKF滤波算法中，需要对状态预测和量测预测分别进行UT变换来更新系统状态；
状态预测UT变换：
量测预测UT变换：
步骤三、高度Z的更新：
(一)由步骤2中解算得到的速度差分得到气压计和加速度计滤波得到高度方向的加速度AccZ_acc+baro；再与加速度计获取的高度方向的加速度AccZ_acc做差值；得到加速度差值AccZ_err,以此参数来量化气流扰动对气压计的影响；
(二)通过AccZ_err的曲线变化，设定合适的阈值AccZ_threshold，当当前时刻的AccZ_err的绝对值小于阈值时，用气压计数据更新高度；而当当前时刻的AccZ_err的绝对值大于阈值时，则认为此时气压计受气流扰动较大，此时气压计数据不准确，则用前一时刻的气压计数据更新高度；
步骤四、第二次UKF滤波；
利用步骤3中更新的高度信息Z_new作为量测值来更新UKF的状态和协方差信息；
此时，量测值yk＝Z_new,系统状态Xk和协方差Pk的信息依旧由UT变换得到。最后，将第二次UKF滤波位置解算得到的高度方向的位置、速度信息输入给飞控位置控制模块，从而使得高度控制更为准确，飞行体验大大提高。
利用MATLAB搭建无人机高度方向的UKF仿真模块，利用AccZ_err对气压计的异常数据进行处理。对将气压计野值进行剔除更新后的高度Z_new输入给UKF位置解算模块，解算出来的高度与原始气压计数据进行对比如图2(为了便于观察，图2将UKF滤波高度数据上移5m)。
图中方框部分即为起飞着陆时刻，由于气流的扰动导致气压计数据的突变，突变值在3-4m。通过对气压计野值剔除更新后，由Z_new作为量测值得到的UKF高度曲线Z_ukf，此时高度突变值明显变小，高度控制效果大大提高。
实际飞行测试：
将前面对气压计异常数据处理的UKF滤波算法添加到基于C++的飞控源码中，在只有IMU模块和气压计模块工作的条件下，分别对飞行器急停刹车和起飞着陆的高度变化进行测试。
急停刹车测试
如图3急停刹车时刻气压计高度与更新后高度对比图所示(为了便于观察，图3将气压计高度数据上移3m)，图中方框部分为急停刹车时刻AccZ_err判定气压计数据异常时刻，此时用前一时刻高度代替当前高度(图中Z_new直线部分都为异常数据的截取值，图中只框选出了两处)。实测结果表明，在室内或室外无风情况下，高度基本不掉；室外大风情况下，高度略有起伏，但变化不大。与没有对气压计异常数据进行处理的结果相比，急刹掉高问题明显改善，高度控制效果大大提高。
起飞着陆测试
如图4起飞着陆时刻气压计高度与更新后高度对比图所示(为了便于观察，图4将气压计高度数据上移2m)，图中方框部分为起飞着陆时刻AccZ_err判定气压计数据异常时刻，此时用前一时刻高度代替当前高度。实测结果表明，与之前飞行器着陆时刻由于地面效应经常出现的难以降落，在近地面上下震荡的现象相比，飞行器着陆时的地面效应问题明显改善，着陆效果大大提升。
本发明的有益效果是：本发明基于UKF的滤波算法，对气压计的高度异常数据进行判断并对野值进行剔除，解决了飞行器在地面效应下难以着陆和飞行中的急刹掉高问题，提高了飞机的高度飞行控制效果，大大提升用户的飞行体验。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何不经过创造性劳动想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书所限定的保护范围为准。
Claims (2)
1.一种基于UKF的无人机气压计异常数据处理的高度控制方法，其特征在于：利用加速度计的Z方向的加速度信息以及加速度计和气压计第一次UKF滤波出来的速度后对其差分得到的加速度信息做差,利用差值来量化气流扰动对气压计的影响,具体包括带加速度计和气压计的飞行器，加速度计通过降噪处理获得加速度信息，气压计通过降噪处理获得高度信息，后经第一次UKF滤波，第一次UKF滤波后更新气压计数据，更新后进行第二次UKF滤波，第二次UKF滤波后将第二次滤波位置信息以及第二次滤波速度信息传递给飞控位置控制模块。
2.如权利要求1所述的基于UKF的无人机气压计异常数据处理的高度控制方法，其特征在于，具体步骤如下：
步骤一、构建无人机高度方向的系统方程和量测方程：
在只考虑气压计(Barometer)和加速度计(Accelerometer)的情况下，构造无人机高度方向的系统方程和量测方程为：
系统方程：
量测方程：
式中，hk、vek、acc_biask分别为k时刻Z方向的位置、速度和加速度偏移量；Ts为时间步长；为加速度计经旋转矩阵后得到的NED坐标系下Z方向的加速度；Zk为k时刻气压计的高度数据，在实际使用中由于气压计数据噪声太大，会首先对其进行一阶低通滤波处理；Wk-1、Vk分别为为系统方程和量测方程的协方差矩阵，在实际使用过程中会通过调节协方差矩阵的大小来使得滤波出来的结果跟随气压计的特性；
步骤二、第一次UKF滤波：
对于离散时间非线性系统：
式中，xk是n×1维的状态向量；yk是m×1维的量测向量，Bk-1是n×l维的系统噪声分配矩阵；ωk-1是l×1维的系统噪声向量，νk-1是m×1维的量测噪声向量，f(·)和h(·)均为时间参数离散而状态连续的非线性向量函数，无人机系统常用的扩展卡尔曼滤波算法和无迹卡尔曼滤波算法，整体框架一样：
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mi>P</mi>
<mrow>
<mi>x</mi>
<mi>y</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>P</mi>
<mrow>
<mi>y</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>y</mi>
<mi>k</mi>
</msub>
<mo>-</mo>
<msub>
<mover>
<mi>y</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>P</mi>
<mrow>
<mi>x</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mi>P</mi>
<mrow>
<mi>x</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<msub>
<mi>P</mi>
<mrow>
<mi>y</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>K</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
式中，Kk为卡尔曼增益；Px,k/k-1、Py,k/k-1、Pxy,k/k-1为系统和量测协方差矩阵；为系统变量和量测变量预测值；Px,k为系统变量和协方差更新值；
在UKF滤波算法中，需要对状态预测和量测预测分别进行UT变换来更新系统状态；
状态预测UT变换：
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&chi;</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<msub>
<mrow>
<mo>&lsqb;</mo>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>&rsqb;</mo>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<mi>&gamma;</mi>
<msqrt>
<msub>
<mi>P</mi>
<mrow>
<mi>x</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</msqrt>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mrow>
<mo>&lsqb;</mo>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>&rsqb;</mo>
</mrow>
<mi>n</mi>
</msub>
<mo>-</mo>
<mi>&gamma;</mi>
<msqrt>
<msub>
<mi>P</mi>
<mrow>
<mi>x</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</msqrt>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>&chi;</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mo>*</mo>
</msubsup>
<mo>=</mo>
<mi>f</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&chi;</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>0</mn>
</mrow>
<mrow>
<mn>2</mn>
<mi>n</mi>
</mrow>
</munderover>
<msubsup>
<mi>W</mi>
<mi>i</mi>
<mi>m</mi>
</msubsup>
<msubsup>
<mi>&chi;</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mo>*</mo>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>P</mi>
<mrow>
<mi>x</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>0</mn>
</mrow>
<mrow>
<mn>2</mn>
<mi>n</mi>
</mrow>
</munderover>
<msubsup>
<mi>W</mi>
<mi>i</mi>
<mi>c</mi>
</msubsup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>&chi;</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mo>*</mo>
</msubsup>
<mo>-</mo>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>&chi;</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mo>*</mo>
</msubsup>
<mo>-</mo>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mi>T</mi>
</msup>
<mo>+</mo>
<msub>
<mi>B</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>Q</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>B</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mi>T</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
量测预测UT变换：
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&chi;</mi>
<mrow>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<msub>
<mrow>
<mo>&lsqb;</mo>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>&rsqb;</mo>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<mi>&gamma;</mi>
<msqrt>
<msub>
<mi>P</mi>
<mrow>
<mi>x</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</msqrt>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mrow>
<mo>&lsqb;</mo>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>&rsqb;</mo>
</mrow>
<mi>n</mi>
</msub>
<mo>-</mo>
<mi>&gamma;</mi>
<msqrt>
<msub>
<mi>P</mi>
<mrow>
<mi>x</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</msqrt>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&eta;</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<mi>h</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&chi;</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>y</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>0</mn>
</mrow>
<mrow>
<mn>2</mn>
<mi>n</mi>
</mrow>
</munderover>
<msubsup>
<mi>W</mi>
<mi>i</mi>
<mi>m</mi>
</msubsup>
<msub>
<mi>&eta;</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>P</mi>
<mrow>
<mi>y</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>0</mn>
</mrow>
<mrow>
<mn>2</mn>
<mi>n</mi>
</mrow>
</munderover>
<msubsup>
<mi>W</mi>
<mi>i</mi>
<mi>c</mi>
</msubsup>
<mrow>
<mo>(</mo>
<msub>
<mi>&eta;</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mover>
<mi>y</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>&eta;</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mover>
<mi>y</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mi>T</mi>
</msup>
<mo>+</mo>
<msub>
<mi>R</mi>
<mi>k</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>P</mi>
<mrow>
<mi>x</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>0</mn>
</mrow>
<mrow>
<mn>2</mn>
<mi>n</mi>
</mrow>
</munderover>
<msubsup>
<mi>W</mi>
<mi>i</mi>
<mi>c</mi>
</msubsup>
<mrow>
<mo>(</mo>
<msub>
<mi>&chi;</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>&eta;</mi>
<mrow>
<mi>i</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mover>
<mi>y</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mi>T</mi>
</msup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
步骤三、高度Z的更新：
(一)由步骤2中解算得到的速度差分得到气压计和加速度计滤波得到高度方向的加速度AccZ_acc+baro；再与加速度计获取的高度方向的加速度AccZ_acc做差值；得到加速度差值AccZ_err,以此参数来量化气流扰动对气压计的影响；
(二)通过AccZ_err的曲线变化，设定合适的阈值AccZ_threshold，当当前时刻的AccZ_err的绝对值小于阈值时，用气压计数据更新高度；而当当前时刻的AccZ_err的绝对值大于阈值时，则认为此时气压计受气流扰动较大，此时气压计数据不准确，则用前一时刻的气压计数据更新高度；
<mrow>
<mi>Z</mi>
<mo>_</mo>
<mi>n</mi>
<mi>e</mi>
<mi>w</mi>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>Z</mi>
<mo>_</mo>
<mi>b</mi>
<mi>a</mi>
<mi>r</mi>
<mi>o</mi>
<mo>;</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>|</mo>
<mi>A</mi>
<mi>c</mi>
<mi>c</mi>
<mi>Z</mi>
<mo>_</mo>
<mi>e</mi>
<mi>r</mi>
<mi>r</mi>
<mo>|</mo>
<mo><</mo>
<mi>A</mi>
<mi>c</mi>
<mi>c</mi>
<mi>Z</mi>
<msub>
<mo>_</mo>
<mrow>
<mi>t</mi>
<mi>h</mi>
<mi>r</mi>
<mi>e</mi>
<mi>s</mi>
<mi>h</mi>
<mi>o</mi>
<mi>l</mi>
<mi>d</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>Z</mi>
<mo>_</mo>
<mi>p</mi>
<mi>r</mi>
<mi>e</mi>
<mo>;</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>|</mo>
<mi>A</mi>
<mi>c</mi>
<mi>c</mi>
<mi>Z</mi>
<mo>_</mo>
<mi>e</mi>
<mi>r</mi>
<mi>r</mi>
<mo>|</mo>
<mo>></mo>
<mo>=</mo>
<mi>A</mi>
<mi>c</mi>
<mi>c</mi>
<mi>Z</mi>
<msub>
<mo>_</mo>
<mrow>
<mi>t</mi>
<mi>h</mi>
<mi>r</mi>
<mi>e</mi>
<mi>s</mi>
<mi>h</mi>
<mi>o</mi>
<mi>l</mi>
<mi>d</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
步骤四、第二次UKF滤波；
利用步骤3中更新的高度信息Z_new作为量测值来更新UKF的状态和协方差信息；
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mi>P</mi>
<mrow>
<mi>x</mi>
<mi>y</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>P</mi>
<mrow>
<mi>y</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>y</mi>
<mi>k</mi>
</msub>
<mo>-</mo>
<msub>
<mover>
<mi>y</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>P</mi>
<mrow>
<mi>x</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mi>P</mi>
<mrow>
<mi>x</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<msub>
<mi>P</mi>
<mrow>
<mi>y</mi>
<mo>,</mo>
<mi>k</mi>
<mo>/</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>K</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
此时，量测值yk＝Z_new,系统状态Xk和协方差Pk的信息依旧由UT变换得到，最后，将第二次UKF滤波位置解算得到的高度方向的位置、速度信息输入给飞控位置控制模块，从而使得高度控制更为准确，飞行体验大大提高。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710743039.6A CN107577238B (zh) | 2017-08-25 | 2017-08-25 | 基于ukf的无人机气压计异常数据处理的高度控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710743039.6A CN107577238B (zh) | 2017-08-25 | 2017-08-25 | 基于ukf的无人机气压计异常数据处理的高度控制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107577238A true CN107577238A (zh) | 2018-01-12 |
| CN107577238B CN107577238B (zh) | 2020-12-18 |
Family
ID=61034846
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710743039.6A Active CN107577238B (zh) | 2017-08-25 | 2017-08-25 | 基于ukf的无人机气压计异常数据处理的高度控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107577238B (zh) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110658831A (zh) * | 2019-10-22 | 2020-01-07 | 深圳市道通智能航空技术有限公司 | 对地高度校正方法及装置、无人飞行器 |
| CN112762893A (zh) * | 2021-04-06 | 2021-05-07 | 北京三快在线科技有限公司 | 无人机状态确定方法、装置、介质、电子设备及无人机 |
| CN112882072A (zh) * | 2021-01-15 | 2021-06-01 | 中国人民解放军海军航空大学 | 一种基于误差反馈的飞行器惯性加gps混合高度测量方法 |
| CN114091629A (zh) * | 2022-01-21 | 2022-02-25 | 西安羚控电子科技有限公司 | 一种试飞数据智能处理系统及其方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103528587A (zh) * | 2013-10-15 | 2014-01-22 | 西北工业大学 | 自主组合导航系统 |
| US20140122012A1 (en) * | 2012-11-01 | 2014-05-01 | Hti Ip, Llc | Method and system for determining whether to reset a height in a height determining device based on the occurrence of steps |
| CN103868521A (zh) * | 2014-02-20 | 2014-06-18 | 天津大学 | 基于激光雷达的四旋翼无人机自主定位及控制方法 |
| CN104932522A (zh) * | 2015-05-27 | 2015-09-23 | 深圳市大疆创新科技有限公司 | 一种飞行器的自主降落方法和系统 |
| CN105628024A (zh) * | 2015-12-29 | 2016-06-01 | 中国电子科技集团公司第二十六研究所 | 基于多传感器融合的单人定位导航器和定位导航方法 |
-
2017
- 2017-08-25 CN CN201710743039.6A patent/CN107577238B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20140122012A1 (en) * | 2012-11-01 | 2014-05-01 | Hti Ip, Llc | Method and system for determining whether to reset a height in a height determining device based on the occurrence of steps |
| CN103528587A (zh) * | 2013-10-15 | 2014-01-22 | 西北工业大学 | 自主组合导航系统 |
| CN103868521A (zh) * | 2014-02-20 | 2014-06-18 | 天津大学 | 基于激光雷达的四旋翼无人机自主定位及控制方法 |
| CN104932522A (zh) * | 2015-05-27 | 2015-09-23 | 深圳市大疆创新科技有限公司 | 一种飞行器的自主降落方法和系统 |
| CN105628024A (zh) * | 2015-12-29 | 2016-06-01 | 中国电子科技集团公司第二十六研究所 | 基于多传感器融合的单人定位导航器和定位导航方法 |
Non-Patent Citations (2)
| Title |
|---|
| 李志敏等: "一种改进的UAV高度无缝融合导航方法", 《传感器与微系统》 * |
| 焦竹青等: "小型无人机组合导航系统的姿态确定方法研究", 《常州大学学报(自然科学版)》 * |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110658831A (zh) * | 2019-10-22 | 2020-01-07 | 深圳市道通智能航空技术有限公司 | 对地高度校正方法及装置、无人飞行器 |
| CN110658831B (zh) * | 2019-10-22 | 2022-04-15 | 深圳市道通智能航空技术股份有限公司 | 对地高度校正方法及装置、无人飞行器 |
| CN112882072A (zh) * | 2021-01-15 | 2021-06-01 | 中国人民解放军海军航空大学 | 一种基于误差反馈的飞行器惯性加gps混合高度测量方法 |
| CN112882072B (zh) * | 2021-01-15 | 2022-03-08 | 中国人民解放军海军航空大学 | 一种基于误差反馈的飞行器惯性加gps混合高度测量方法 |
| CN112762893A (zh) * | 2021-04-06 | 2021-05-07 | 北京三快在线科技有限公司 | 无人机状态确定方法、装置、介质、电子设备及无人机 |
| CN114091629A (zh) * | 2022-01-21 | 2022-02-25 | 西安羚控电子科技有限公司 | 一种试飞数据智能处理系统及其方法 |
| CN114091629B (zh) * | 2022-01-21 | 2022-07-15 | 西安羚控电子科技有限公司 | 一种试飞数据智能处理系统及其方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107577238B (zh) | 2020-12-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111433612B (zh) | 用于估计飞行器空中数据的基于模型和飞行信息的组合训练的神经网络系统 | |
| CN107577238A (zh) | 基于ukf的无人机气压计异常数据处理的高度控制方法 | |
| US9650152B2 (en) | Flight envelope protection system for unmanned aerial vehicles | |
| US20100268414A1 (en) | Method and system for estimating the angular speed of a mobile | |
| CN102607639A (zh) | 基于bp神经网络的大攻角飞行状态下大气数据测量方法 | |
| CN106444800A (zh) | 多旋翼无人机飞行用外置式安全控制装置、方法及系统 | |
| CN106885918B (zh) | 一种面向多旋翼飞行器的多信息融合实时风速估计方法 | |
| CN103712598B (zh) | 一种小型无人机姿态确定方法 | |
| CN105518763B (zh) | 飞机航行用信息生成装置以及飞机航行用信息生成方法 | |
| CN101853531A (zh) | 基于预分类技术和rbf神经网络的直升机飞行状态识别方法 | |
| CN108897334A (zh) | 一种基于模糊神经网络的仿昆虫扑翼飞行器姿态控制方法 | |
| EP3479181A1 (de) | Verfahren und assistenzsystem zur detektion einer flugleistungsdegradierung | |
| CN106249744A (zh) | 一种基于二级互补滤波的小型旋翼飞行器高度控制方法 | |
| CN104503258B (zh) | 基于光缆敷设的共轴双旋翼无人直升机建模方法 | |
| CA3067151C (en) | Systems and methods for tail-specific parameter computation | |
| CN103149929A (zh) | 飞行器纵向运动故障诊断和容错控制方法 | |
| CN104391364B (zh) | 基于光缆敷设的共轴双旋翼无人直升机参数辨识试验系统 | |
| CN111811472B (zh) | 一种气压计高度动态补偿方法、装置、计算机存储介质 | |
| CN103149927A (zh) | 飞行器大迎角运动四元数模型的故障诊断和容错控制方法 | |
| CN111062092B (zh) | 一种直升机飞行谱编制方法和装置 | |
| CN103149928A (zh) | 飞行器大迎角运动三元数模型的故障诊断和容错控制方法 | |
| Lee | Helicopter autonomous ship landing system | |
| CN115205582A (zh) | 一种拟图智能化航空传感器故障检测与分类方法 | |
| McCrink et al. | Aerodynamic Parameter Estimation for Derived Angle-of-Attack Systems | |
| Medeiros et al. | An independent control system for testing and analysis of UAVs in indoor environments |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |