CN116756686A - 一种飞行器的强抗扰高度状态估计方法及系统 - Google Patents

一种飞行器的强抗扰高度状态估计方法及系统 Download PDF

Info

Publication number
CN116756686A
CN116756686A CN202310726951.6A CN202310726951A CN116756686A CN 116756686 A CN116756686 A CN 116756686A CN 202310726951 A CN202310726951 A CN 202310726951A CN 116756686 A CN116756686 A CN 116756686A
Authority
CN
China
Prior art keywords
sensor
aircraft
altitude
observed quantity
estimating
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.)
Pending
Application number
CN202310726951.6A
Other languages
English (en)
Inventor
陶杰
徐煦荣
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangdong University of Technology
Original Assignee
Guangdong University of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Guangdong University of Technology filed Critical Guangdong University of Technology
Priority to CN202310726951.6A priority Critical patent/CN116756686A/zh
Publication of CN116756686A publication Critical patent/CN116756686A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C5/00—Measuring height; Measuring distances transverse to line of sight; Levelling between separated points; Surveyors' levels
    • G01C5/005—Measuring height; Measuring distances transverse to line of sight; Levelling between separated points; Surveyors' levels altimeters for aircraft
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G06F17/13—Differential equations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/10—Pre-processing; Data cleansing
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Physics (AREA)
  • General Engineering & Computer Science (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Computational Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Evolutionary Computation (AREA)
  • Evolutionary Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Artificial Intelligence (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Operations Research (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Navigation (AREA)

Abstract

本发明提供了一种飞行器的强抗扰高度状态估计方法及系统，方法包括以下步骤；步骤S1、获取传感器的数据并估计传感器时延，再对齐传感器时延，获得传感器观测量；步骤S2、根据传感器观测量，估计传感器的误差方差；步骤S3、计算传感器观测量的二次微分，并计算二次微分与惯性测量加速度的相关度；步骤S4、将传感器观测量和相关度进行数据加权融合，计算飞行器距离地面的相对高度；步骤S5、分别对相对高度和绝对高度用三阶卡尔曼滤波算法进行滤波处理，再采用高度观测量切换机制，切换高度观测量计算飞行器的高度，最后用一阶卡尔曼滤波算法估计飞行器离地高度。本发明在飞行器中高度状态观测准确，飞行安全性与可靠性高。

Description

一种飞行器的强抗扰高度状态估计方法及系统
技术领域
本发明涉及飞行器技术领域，尤其涉及一种飞行器的强抗扰高度状态估计方法及系统。
背景技术
目前，由于微电子领域发展迅速，各种构型的无人机，在侦察、救援、植保等领域已有广泛应用。而在无人机飞行控制系统的设计中，主要包括无人机运动控制以及系统状态估计两方面，系统状态估计指利用传感器对无人机的位置、速度、姿态角度、角速度等状态量进行估计，所计算的无人机运动状态的精度将直接影响无人机飞行的稳定与安全。
现有在这方面，已经有许多飞行器控制器开发人员开发出飞行器高度状态估计的方法：APM飞控使用3阶互补滤波算法估计飞行器的高度状态。PX4飞控使用扩张卡尔曼滤波算法来估计飞行器的高度状态。
然而，以上飞行器所使用的飞行器高度状态估计方法只考虑传感器工作正常的情况，在传感器故障检测这方面考虑不足，当传感器发生故障，传感器数据发生跳变时，如果未进行特殊处理，就会错误估计飞行器的高度以及速度，并容易导致飞行器失控；其次，PX4飞控所使用的状态估计算法包含了一个24维度的状态方程，集成度高，不利于分布式计算，飞行器高度状态估计效果差。
发明内容
针对以上相关技术的不足，本发明提出一种飞行器的强抗扰高度状态估计方法，用以解决现有飞行器飞行时容易失控、可靠性和稳定性差问题。
为了解决上述技术问题，本发明实施例提供了一种飞行器的强抗扰高度状态估计方法，所述方法包括以下步骤：
步骤S1、获取传感器的数据并估计传感器时延，再对齐所述传感器时延，获得传感器观测量；
步骤S2、根据所述传感器观测量，估计所述传感器的误差方差；
步骤S3、计算所述传感器观测量的二次微分，并计算所述二次微分与惯性测量加速度的相关度；
步骤S4、将所述传感器观测量和所述相关度进行数据加权融合，计算飞行器距离地面的相对高度；
步骤S5、获取当前的绝对高度，分别对所述相对高度和所述绝对高度用三阶卡尔曼滤波算法进行滤波处理，再采用高度观测量切换机制，切换高度观测量计算飞行器的高度，最后用一阶卡尔曼滤波算法估计飞行器离地高度。
优选的，所述步骤S1中，所述传感器时延表示传感器采集到数据后，处理并发送数据需要预设一定时间，处理器接收到的所述数据与实际数据的时间差值。
优选的，定义某个所述传感器输出的当前时刻的数据为z(k)，以及所述传感器输出的历史数据为z(k+1)，z(k+2)，…z(k+n)，其中，n为整数，经过所述传感器延迟对齐后的数据为所述传感器观测量，所述传感器观测量由表达式(1)所示，所述传感器时延由表达式(2)所示：
Zdelay＝z(k+Td)…(1)；
Td＝Tdelay/ΔT…(2)；
其中，Tdelay为相对于时延最大的传感器的时延，ΔT为传感器的采样周期。
优选的，所述步骤S2具体包括以下子步骤：
根据所述当前时刻的数据和所述历史数据；设置所述当前时刻的数据为z(0)，所述历史数据为z(k)，其中，k＝1,2，…n；
估计所述传感器所测得的实际距离，所述实际距离如表达式(3)所示：
Yest(k)＝b+ka，k＝0,1,2…n…(3)；
计算所述传感器的误差，所述误差如表达式(4)所示：
Err(k)＝Yest(k)-Z(k)，k＝0,1,2…n…(4)；
计算所述误差的方差，并带入到卡尔曼滤波算法中，如表达式(5)所示：
Variance＝E(Err2)-E(Err)2…(5)；
其中，Yest表达式中的系数a和系数b分别由以下表达式(6)和表达式(7)计算：
优选的，所述步骤S3具体包括以下子步骤：
根据所述当前时刻的数据为z(0)，所述历史数据为z(k)，k＝0,1,2,…n-1，对所述传感器观测量一次微分计算如下：
再记录所述传感器观测量的一次微分的序列值z1(k)，k＝0,1,2,…n-1，并计算所述传感器观测量的二次微分：
记录下所述传感器观测量的二次微分序列值，计算并记录惯性坐标系下天向加速度的序值：
z2(k)(k＝0,1,2,…n-1)…(10)；
az＝Cbnaiz(iz＝[0 0 1]T)…(11)；
az(k)(k＝0,1,2,…n-1)…(12)；
其中，a为加速度计三轴的值，Cbn为从飞行器坐标系到惯性坐标系的转换矩阵；最后计算观测量二次微分与无人机天向加速度的相关函数：
所述相关函数
优选的，所述步骤S4中，所述相对高度的计算方法具体如下：
预设飞行器搭载n个测距传感器，每个所述测距传感器的观测噪声方差为Ri，每个测距传感器的二次微分项与天向加速度的相关系数为Gi，每个所述测距传感器的观测量为Zi，其中，i＝1,2,…n，n为正整数；
则每个所述测距传感器的加权系数Ki，i＝1,2,…n，计算如下：
令
则
将所述观测量进行加权融合，计算所述飞行器距离地面的相对高度：
r_H＝K1Z1+K2Z2+…+KnZn…(16)；
其中，H为所述飞行器的高度。
优选的，所述步骤S5具体包括以下子步骤：
先定义卡尔曼滤波状态方程：
其中，h为飞行器高度状态，v为飞行器天向速度状态，abiase为天向加速度Az的偏置误差，Δt为采样周期；
定义卡尔曼滤波观测方程：
将卡尔曼滤波状态方程和卡尔曼滤波观测方程得到卡尔曼滤波算法；
再用一阶卡尔曼滤波算法融合所述测距传感器的观测量与所述天向加速度数据，估计无人机的距地距离；
其中，一阶尔曼滤波的状态方程和观测方程如下：
其中，r-H为所述飞行器的相对高度，Vz为由三阶卡尔曼滤波算法估计的无人机天向速度。
优选的，所述高度观测量切换机制具体为：
步骤S51、预设高度切换阈值；
步骤S52、判断所述相对高度是否小于所述高度切换阈值；若是，则对所述相对高度标志位置1；若否，则对所述相对高度标志位置0；其中，所述位置1和位置0为赋值；
步骤S53、判断所述相对高度标志位置是否发声跳变，若是，则记录当前的相对高度、绝对高度和所述飞行器的高度；若否，则返回所述步骤S52。
第二方面，本发明实施例提供一种飞行器的强抗扰高度状态估计系统，所述系统包括：
获取模块，用于获取传感器的数据并估计传感器时延，再对齐所述传感器时延，获得传感器观测量；
估计模块，用于根据所述传感器观测量，估计所述传感器的误差方差；
微分计算模块，用于计算所述传感器观测量的二次微分，并计算所述二次微分与惯性测量加速度的相关度；
高度计算模块，用于将所述传感器观测量和所述相关度进行数据加权融合，计算飞行器距离地面的相对高度；
处理模块，用于获取当前的绝对高度，分别对所述相对高度和所述绝对高度用三阶卡尔曼滤波算法进行滤波处理，再采用高度观测量切换机制，切换高度观测量计算飞行器的高度，最后用一阶卡尔曼滤波算法估计飞行器离地高度。
与相关技术相比，本发明通过上述步骤S1-步骤S5，采用观测噪声方差自适应估计技术，实时估计每个传感器的观测噪声，再将所估计的噪声方差带入卡尔曼滤波算法中，增强滤波效果；利用传感器数据的二次微分与飞行器天向加速度的相关度进行传感器故障诊断，避免当传感器发生故障但观测噪声较少的情况；根据飞行器与地面的相对高度，切换绝对高度与相对高度的观测量，增强高度数据稳定性。结构简单易于调试，并通过实时估计传感器的观测噪声方差，通过卡尔曼滤波算法，计算无人机的高度与速度状态，减少由传感器故障引起的无人机失控的概率，提高无人机飞行安全与可靠性。
附图说明
下面结合附图详细说明本发明。通过结合以下附图所作的详细描述，本发明的上述或其他方面的内容将变得更清楚和更容易理解。附图中：
图1为本发明飞行器的强抗扰高度状态估计方法的流程图；
图2为本发明飞行器的强抗扰高度状态估计方法的整体流程图；
图3为本发明步骤S5的高度观测量切换机制的流程图；
图4为本发明步骤S5的高度观测量切换机制的具体流程图；
图5为本发明飞行器的强抗扰高度状态估计分割系统的模块图。
具体实施方式
下面结合附图详细说明本发明的具体实施方式。
在此记载的具体实施方式/实施例为本发明的特定的具体实施方式，用于说明本发明的构思，均是解释性和示例性的，不应解释为对本发明实施方式及本发明范围的限制。除在此记载的实施例外，本领域技术人员还能够基于本申请权利要求书和说明书所公开的内容采用显而易见的其它技术方案，这些技术方案包括采用对在此记载的实施例的做出任何显而易见的替换和修改的技术方案，都在本发明的保护范围之内。
实施例一
如图1-图2所示，本发明提供一种飞行器的强抗扰高度状态估计方法，其中，飞行器包括无人机、飞机、卫星等，本发明主要使用无人机，无人机飞行控制好，功能多样。
所述方法包括以下步骤：
步骤S1、获取传感器的数据并估计传感器时延，再对齐所述传感器时延，获得传感器观测量；
步骤S2、根据所述传感器观测量，估计所述传感器的误差方差；
步骤S3、计算所述传感器观测量的二次微分，并计算所述二次微分与惯性测量加速度的相关度。其中，惯性测量加速度为IMU(Inertial Measurement Unit)加速度，即惯性测量单元，用于测量物体三轴姿态角(或角速率)以及加速度。
步骤S4、将所述传感器观测量和所述相关度进行数据加权融合，计算飞行器距离地面的相对高度；
步骤S5、获取当前的绝对高度，分别对所述相对高度和所述绝对高度用三阶卡尔曼滤波算法进行滤波处理，再采用高度观测量切换机制，切换高度观测量计算飞行器的高度，最后用一阶卡尔曼滤波算法估计飞行器离地高度。
具体的，通过获取传感器的数据并估计传感器时延，再对齐所述传感器时延，获得传感器观测量；根据所述传感器观测量，估计所述传感器的误差方差；计算所述传感器观测量的二次微分，并计算所述二次微分与惯性测量加速度的相关度；将所述传感器观测量和所述相关度进行数据加权融合，计算飞行器距离地面的相对高度；获取当前的绝对高度，分别对所述相对高度和所述绝对高度用三阶卡尔曼滤波算法进行滤波处理，再采用高度观测量切换机制，切换高度观测量计算飞行器的高度，最后用一阶卡尔曼滤波算法估计飞行器离地高度。这样采用观测噪声方差自适应估计技术，实时估计每个传感器的观测噪声，再将所估计的噪声方差带入卡尔曼滤波算法中，增强滤波效果；利用传感器数据的二次微分与飞行器天向加速度的相关度进行传感器故障诊断，避免当传感器发生故障但观测噪声较少的情况；根据飞行器与地面的相对高度，切换绝对高度与相对高度的观测量，增强高度数据稳定性。结构简单易于调试，并通过实时估计传感器的观测噪声方差，通过卡尔曼滤波算法，计算无人机的高度与速度状态，减少由传感器故障引起的无人机失控的概率，提高无人机飞行安全与可靠性。
本实施例中，所述步骤S1中，所述传感器时延表示传感器采集到数据后，处理并发送数据需要预设一定时间，处理器接收到的所述数据与实际数据的时间差值。
具体的，通过传感器采集到数据后，处理并发送数据需要一定时间，处理器接收到的数据与实际数据的时间差值即是传感器时延。不同种类不同型号的传感器的数据时延不一致，因此需要将时延小的观测量向时延大的观测量进行时延对齐，保证数据的时间一致性。
更具体地，定义某个所述传感器输出的当前时刻的数据为z(k)，以及所述传感器输出的历史数据为z(k+1)，z(k+2)，…z(k+n)，其中，n为整数，经过所述传感器延迟对齐后的数据为所述传感器观测量，所述传感器观测量由表达式(1)所示，所述传感器时延由表达式(2)所示：
Zdelay＝z(k+Td)…(1)；
Td＝Tdelay/ΔT…(2)；
其中，Tdelay为相对于时延最大的传感器的时延，ΔT为传感器的采样周期。
本实施例中，所述步骤S2具体包括以下子步骤：
根据所述当前时刻的数据和所述历史数据；设置所述当前时刻的数据为z(0)，所述历史数据为z(k)，其中，k＝1,2，…n；
估计所述传感器所测得的实际距离，所述实际距离如表达式(3)所示：
Yest(k)＝b+ka，k＝0,1,2…n…(3)；
计算所述传感器的误差，所述误差如表达式(4)所示：
Err(k)＝Yest(k)-Z(k)，k＝0,1,2…n…(4)；
计算所述误差的方差，并带入到卡尔曼滤波算法中，如表达式(5)所示：
Variance＝E(Err2)-E(Err)2…(5)；
其中，Yest表达式中的系数a和系数b分别由以下表达式(6)和表达式(7)计算：
具体的，采用传感器自适应观测噪声方差估计算法，根据传感器观测量序列，估计传感器测量的实际值，再用这个实际的值计算传感器测量误差，再计算误差方差；减少由传感器故障引起的无人机失控的概率，提高无人机飞行安全与可靠性。
本实施例中，所述步骤S3具体包括以下子步骤：
根据所述当前时刻的数据为z(0)，所述历史数据为z(k)，k＝0,1,2,…n-1，对所述传感器观测量一次微分计算如下：
再记录所述传感器观测量的一次微分的序列值z1(k)，k＝0,1,2,…n-1，并计算所述传感器观测量的二次微分：
记录下所述传感器观测量的二次微分序列值，计算并记录惯性坐标系下天向加速度的序值：
z2(k)(k＝0,1,2,…n-1) … (10)；
az＝Cbnaiz(iz＝[0 0 1]T)…(11)；
az(k)(k＝0,1,2,…n-1)…(12)；
其中，a为加速度计三轴的值，Cbn为从飞行器坐标系到惯性坐标系的转换矩阵；最后计算观测量二次微分与无人机天向加速度的相关函数：
所述相关函数
本实施例中，所述步骤S4中，所述相对高度的计算方法具体如下：
预设飞行器搭载n个测距传感器，每个所述测距传感器的观测噪声方差为Ri，每个测距传感器的二次微分项与天向加速度的相关系数为Gi，每个所述测距传感器的观测量为Zi，其中，i＝1,2,…n，n为正整数；
则每个所述测距传感器的加权系数Ki，i＝1,2,…n，计算如下：
令
则
将所述观测量进行加权融合，计算所述飞行器距离地面的相对高度：
r_H＝K1Z1+K2Z2+…+KnZn…(16)；
其中，H为所述飞行器的高度。
本实施例中，所述步骤S5具体包括以下子步骤：
先定义卡尔曼滤波状态方程：
其中，h为飞行器高度状态，v为飞行器天向速度状态，abiase为天向加速度Az的偏置误差，Δt为采样周期；
定义卡尔曼滤波观测方程：
将卡尔曼滤波状态方程和卡尔曼滤波观测方程得到卡尔曼滤波算法；其中，上述(17)和(18)中，卡尔曼滤波状态方程为X＝AX+BU；是通过引用一个离散控制过程的系统，该系统可用一个线性随机微分方程表示。其中，Z＝HX中，X表示当前时刻的系统状态，U是当前时刻对系统的控制量，A是系统状态转移矩阵，B是系统的控制矩阵，对于多模型系统，他们也可称为矩阵。Z是当前时刻的测量值，H是测量系统的参数，对于多测量系统，H为测量矩阵。
再用一阶卡尔曼滤波算法融合所述测距传感器的观测量与所述天向加速度数据，估计无人机的距地距离；
其中，一阶尔曼滤波的状态方程和观测方程如下：
其中，r-H为所述飞行器的相对高度，Vz为由三阶卡尔曼滤波算法估计的无人机天向速度。
具体的，r_H为无人机机载的相对传感器融合得到的无人机离地高度，Vz为由三阶卡尔曼滤波算法估计的无人机天向速度。上述线性卡尔曼滤波算法中，有两个参数需要整定：过程噪声Q和观测噪声R，Q越大，系统状态向观测量Z的收敛速度越快，Q越小系统状态越依赖系统模型，Q参数需要多次试凑以获取最佳滤波效果。观测噪声R可以通过高度观测量的观测误差的方差估计中获得。
本实施例中，如图3-图4所示，所述高度观测量切换机制具体为：
步骤S51、预设高度切换阈值；
步骤S52、判断所述相对高度是否小于所述高度切换阈值；若是，则对所述相对高度标志位置1；若否，则对所述相对高度标志位置0；其中，所述位置1和位置0为赋值；
步骤S53、判断所述相对高度标志位置是否发声跳变，若是，则记录当前的相对高度、绝对高度和所述飞行器的高度；若否，则返回所述步骤S52。
具体的，令高度切换阈值为H_switch，令融合滤波后的相对高度为r_H,令融合滤波后的绝对高度为abs_H。
本发明的切换高度观测量的机制具体为，当无人机处于近地面时，采用相对距离作为高度观测量，此时高度与天向速度状态由相对距离传感器测量的数据计算，当无人机向高空飞行时无人机高度状态由气压计、gps等绝对高度传感器的数据计算。根据无人机与地面的相对高度，切换绝对高度与相对高度的观测量，增强高度数据稳定性。
实施例二
如图5所示，本发明实施例提供一种飞行器的强抗扰高度状态估计系统200，所述系统200包括：
获取模块201，用于获取传感器的数据并估计传感器时延，再对齐所述传感器时延，获得传感器观测量；
估计模块202，用于根据所述传感器观测量，估计所述传感器的误差方差；
微分计算模块203，用于计算所述传感器观测量的二次微分，并计算所述二次微分与惯性测量加速度的相关度；
高度计算模块204，用于将所述传感器观测量和所述相关度进行数据加权融合，计算飞行器距离地面的相对高度；
处理模块205，用于获取当前的绝对高度，分别对所述相对高度和所述绝对高度用三阶卡尔曼滤波算法进行滤波处理，再采用高度观测量切换机制，切换高度观测量计算飞行器的高度，最后用一阶卡尔曼滤波算法估计飞行器离地高度。
具体的，获取模块201获取传感器的数据并估计传感器时延，再对齐所述传感器时延，获得传感器观测量；估计模块202根据所述传感器观测量，估计所述传感器的误差方差；微分计算模块203计算所述传感器观测量的二次微分，并计算所述二次微分与惯性测量加速度的相关度；高度计算模块204将所述传感器观测量和所述相关度进行数据加权融合，计算飞行器距离地面的相对高度；处理模块205获取当前的绝对高度，分别对所述相对高度和所述绝对高度用三阶卡尔曼滤波算法进行滤波处理，再采用高度观测量切换机制，切换高度观测量计算飞行器的高度，最后用一阶卡尔曼滤波算法估计飞行器离地高度。这样采用观测噪声方差自适应估计技术，实时估计每个传感器的观测噪声，再将所估计的噪声方差带入卡尔曼滤波算法中，增强滤波效果；利用传感器数据的二次微分与飞行器天向加速度的相关度进行传感器故障诊断，避免当传感器发生故障但观测噪声较少的情况；根据飞行器与地面的相对高度，切换绝对高度与相对高度的观测量，增强高度数据稳定性。结构简单易于调试，并通过实时估计传感器的观测噪声方差，通过卡尔曼滤波算法，计算无人机的高度与速度状态，减少由传感器故障引起的无人机失控的概率，提高无人机飞行安全与可靠性。
本实施例中，所述获取模块中，所述传感器时延表示传感器采集到数据后，处理并发送数据需要预设一定时间，处理器接收到的所述数据与实际数据的时间差值。
具体的，通过传感器采集到数据后，处理并发送数据需要一定时间，处理器接收到的数据与实际数据的时间差值即是传感器时延。不同种类不同型号的传感器的数据时延不一致，因此需要将时延小的观测量向时延大的观测量进行时延对齐，保证数据的时间一致性。
本实施方式中，通过飞行器的强抗扰高度状态估计系统200所实现的技术效果及原理与本发明上述实施例一提供的飞行器的强抗扰高度状态估计方法所实现的技术效果和原理相同，在此不再赘述。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何纂改、等同替换、改进等，均应包含在本发明的权利要求范围之内。

Claims (9)

1.一种飞行器的强抗扰高度状态估计方法，其特征在于，所述方法包括以下步骤：
步骤S1、获取传感器的数据并估计传感器时延，再对齐所述传感器时延，获得传感器观测量；
步骤S2、根据所述传感器观测量，估计所述传感器的误差方差；
步骤S3、计算所述传感器观测量的二次微分，并计算所述二次微分与惯性测量加速度的相关度；
步骤S4、将所述传感器观测量和所述相关度进行数据加权融合，计算飞行器距离地面的相对高度；
步骤S5、获取当前的绝对高度，分别对所述相对高度和所述绝对高度用三阶卡尔曼滤波算法进行滤波处理，再采用高度观测量切换机制，切换高度观测量计算飞行器的高度，最后用一阶卡尔曼滤波算法估计飞行器离地高度。
2.如权利要求1所述的飞行器的强抗扰高度状态估计方法，其特征在于，所述步骤S1中，所述传感器时延表示传感器采集到数据后，处理并发送数据需要预设一定时间，处理器接收到的所述数据与实际数据的时间差值。
3.如权利要求2所述的飞行器的强抗扰高度状态估计方法，其特征在于，定义某个所述传感器输出的当前时刻的数据为z(k)，以及所述传感器输出的历史数据为z(k+1)，z(k+2)，…z(k+n)，其中，n为整数，经过所述传感器延迟对齐后的数据为所述传感器观测量，所述传感器观测量由表达式(1)所示，所述传感器时延由表达式(2)所示：
Zdelay＝z(k+Td)…(1)；
Td＝Tdelay/ΔT…(2)；
其中，Tdelay为相对于时延最大的传感器的时延，ΔT为传感器的采样周期。
4.如权利要求3所述的飞行器的强抗扰高度状态估计方法，其特征在于，所述步骤S2具体包括以下子步骤：
根据所述当前时刻的数据和所述历史数据；设置所述当前时刻的数据为z(0)，所述历史数据为z(k)，其中，k＝1,2，…n；
估计所述传感器所测得的实际距离，所述实际距离如表达式(3)所示：
Yest(k)＝b+ka，k＝0,1,2…n…(3)；
计算所述传感器的误差，所述误差如表达式(4)所示：
Err(k)＝Yest(k)-Z(k)，k＝0,1,2…n…(4)；
计算所述误差的方差，并带入到卡尔曼滤波算法中，如表达式(5)所示：
Variance＝E(Err2)-E(Err)2…(5)；
其中，Yest表达式中的系数a和系数b分别由以下表达式(6)和表达式(7)计算：
5.如权利要求4所述的飞行器的强抗扰高度状态估计方法，其特征在于，所述步骤S3具体包括以下子步骤：
根据所述当前时刻的数据为z(0)，所述历史数据为z(k)，k＝0,1,2,…n-1，对所述传感器观测量一次微分计算如下：
再记录所述传感器观测量的一次微分的序列值z1(k)，k＝0,1,2,…n-1，并计算所述传感器观测量的二次微分：
记录下所述传感器观测量的二次微分序列值，计算并记录惯性坐标系下天向加速度的序值：
z2(k)(k＝0,1,2,…n-1)…(10)；
az＝Cbnaiz(iz＝[0 0 1]T)…(11)；
az(k)(k＝0,1,2,…n-1)…(12)；
其中，a为加速度计三轴的值，Cbn为从飞行器坐标系到惯性坐标系的转换矩阵；最后计算观测量二次微分与无人机天向加速度的相关函数：
所述相关函数
6.如权利要求1所述的飞行器的强抗扰高度状态估计方法，其特征在于，所述步骤S4中，所述相对高度的计算方法具体如下：
预设飞行器搭载n个测距传感器，每个所述测距传感器的观测噪声方差为Ri，每个测距传感器的二次微分项与天向加速度的相关系数为Gi，每个所述测距传感器的观测量为Zi，其中，i＝1,2,…n，n为正整数；
则每个所述测距传感器的加权系数Ki，i＝1,2,…n，计算如下：
令
则
将所述观测量进行加权融合，计算所述飞行器距离地面的相对高度：
r_H＝K1Z1+K2Z2+…+KnZn…(16)；
其中，H为所述飞行器的高度。
7.如权利要求6所述的飞行器的强抗扰高度状态估计方法，其特征在于，所述步骤S5具体包括以下子步骤：
先定义卡尔曼滤波状态方程：
其中，h为飞行器高度状态，v为飞行器天向速度状态，abiase为天向加速度Az的偏置误差，Δt为采样周期；
定义卡尔曼滤波观测方程：
将卡尔曼滤波状态方程和卡尔曼滤波观测方程得到卡尔曼滤波算法；
再用一阶卡尔曼滤波算法融合所述测距传感器的观测量与所述天向加速度数据，估计无人机的距地距离；
其中，一阶尔曼滤波的状态方程和观测方程如下：
其中，r-H为所述飞行器的相对高度，Vz为由三阶卡尔曼滤波算法估计的无人机天向速度。
8.如权利要求7所述的飞行器的强抗扰高度状态估计方法，其特征在于，所述高度观测量切换机制具体为：
步骤S51、预设高度切换阈值；
步骤S52、判断所述相对高度是否小于所述高度切换阈值；若是，则对所述相对高度标志位置1；若否，则对所述相对高度标志位置0；其中，所述位置1和位置0为赋值；
步骤S53、判断所述相对高度标志位置是否发声跳变，若是，则记录当前的相对高度、绝对高度和所述飞行器的高度；若否，则返回所述步骤S52。
9.一种飞行器的强抗扰高度状态估计系统，其特征在于，所述系统包括：
获取模块，用于获取传感器的数据并估计传感器时延，再对齐所述传感器时延，获得传感器观测量；
估计模块，用于根据所述传感器观测量，估计所述传感器的误差方差；
微分计算模块，用于计算所述传感器观测量的二次微分，并计算所述二次微分与惯性测量加速度的相关度；
高度计算模块，用于将所述传感器观测量和所述相关度进行数据加权融合，计算飞行器距离地面的相对高度；
处理模块，用于获取当前的绝对高度，分别对所述相对高度和所述绝对高度用三阶卡尔曼滤波算法进行滤波处理，再采用高度观测量切换机制，切换高度观测量计算飞行器的高度，最后用一阶卡尔曼滤波算法估计飞行器离地高度。
CN202310726951.6A 2023-06-19 2023-06-19 一种飞行器的强抗扰高度状态估计方法及系统 Pending CN116756686A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310726951.6A CN116756686A (zh) 2023-06-19 2023-06-19 一种飞行器的强抗扰高度状态估计方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310726951.6A CN116756686A (zh) 2023-06-19 2023-06-19 一种飞行器的强抗扰高度状态估计方法及系统

Publications (1)

Publication Number Publication Date
CN116756686A true CN116756686A (zh) 2023-09-15

Family

ID=87951006

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310726951.6A Pending CN116756686A (zh) 2023-06-19 2023-06-19 一种飞行器的强抗扰高度状态估计方法及系统

Country Status (1)

Country Link
CN (1) CN116756686A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117235650A (zh) * 2023-11-13 2023-12-15 国网浙江省电力有限公司温州供电公司 一种高空作业状态检测方法、装置、设备及介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117235650A (zh) * 2023-11-13 2023-12-15 国网浙江省电力有限公司温州供电公司 一种高空作业状态检测方法、装置、设备及介质
CN117235650B (zh) * 2023-11-13 2024-02-13 国网浙江省电力有限公司温州供电公司 一种高空作业状态检测方法、装置、设备及介质

Similar Documents

Publication Publication Date Title
US6760663B2 (en) Solution separation method and apparatus for ground-augmented global positioning system
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
US7711482B2 (en) Hybrid INS/GNSS system with integrity monitoring and method for integrity monitoring
Guo et al. Multisensor data-fusion-based approach to airspeed measurement fault detection for unmanned aerial vehicles
CN107543540B (zh) 一种飞行设备的数据融合和飞行模式切换方法及装置
JP4446604B2 (ja) Ｇｐｓ信号障害隔離モニタ
CN112505737B (zh) 一种gnss/ins组合导航方法
CN108981708B (zh) 四旋翼扭矩模型/航向陀螺/磁传感器容错组合导航方法
Hajiyev et al. Two-stage Kalman filter for fault tolerant estimation of wind speed and UAV flight parameters
CN116756686A (zh) 一种飞行器的强抗扰高度状态估计方法及系统
US9828111B2 (en) Method of estimation of the speed of an aircraft relative to the surrounding air, and associated system
CN111679680A (zh) 一种无人机自主着舰方法及系统
US20230341563A1 (en) System and method for computing positioning protection levels
CN109612459A (zh) 基于动力学模型的四旋翼飞行器惯性传感器容错导航方法
CN114415224B (zh) 一种复杂受限环境中车辆融合定位系统及方法
US9234909B2 (en) Method and system for determining an airspeed of an aircraft
Ducard et al. Strategies for sensor-fault compensation on UAVs: review, discussions & additions
CN113483759A (zh) 一种GNSS、INS、Vision组合导航系统完好性保护级计算方法
CN115291253A (zh) 一种基于残差检测的车辆定位完好性监测方法及系统
EP3995780A1 (en) Sensor data fusion system with noise reduction and fault protection
Hazbon et al. Digital twin concept for aircraft system failure detection and correction
CN115468559A (zh) 一种多源联邦滤波组合导航系统的自适应容错方法
US11585950B2 (en) Method for identifying a static phase of a vehicle
Hazbon Alvarez et al. Digital twin concept for aircraft sensor failure
CN113376664B (zh) 一种无人蜂群协同导航多故障检测方法

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