CN108168509A - 一种升力模型辅助的四旋翼飞行器高度容错估计方法 - Google Patents

一种升力模型辅助的四旋翼飞行器高度容错估计方法 Download PDF

Info

Publication number
CN108168509A
CN108168509A CN201711274710.3A CN201711274710A CN108168509A CN 108168509 A CN108168509 A CN 108168509A CN 201711274710 A CN201711274710 A CN 201711274710A CN 108168509 A CN108168509 A CN 108168509A
Authority
CN
China
Prior art keywords
moment
barometer
fault
ins
subfilters
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
Application number
CN201711274710.3A
Other languages
English (en)
Other versions
CN108168509B (zh
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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN201711274710.3A priority Critical patent/CN108168509B/zh
Publication of CN108168509A publication Critical patent/CN108168509A/zh
Application granted granted Critical
Publication of CN108168509B publication Critical patent/CN108168509B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • 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/06—Measuring height; Measuring distances transverse to line of sight; Levelling between separated points; Surveyors' levels by using barometric means

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了一种升力模型辅助的四旋翼飞行器高度容错估计方法。首先，通过飞行实验获得四旋翼飞行器升力模型的升力系数；其次，通过两个平行的联邦卡尔曼滤波器进行导航滤波；然后，通过故障检测策略在导航滤波过程中进行四旋翼飞行器高度方向传感器故障检测；最后，如果检测到四旋翼飞行器高度方向传感器故障，则进行故障传感器的故障隔离与恢复，并滤除故障传感器的导航信息。本发明利用四旋翼飞行器的升力模型，与其机载传感器相结合，形成四旋翼飞行器高度方向传感器的冗余，从而实现高度方向传感器的故障诊断和容错，提高导航系统鲁棒性。

Description

一种升力模型辅助的四旋翼飞行器高度容错估计方法
技术领域
本发明属于容错导航技术领域，特别涉及了一种升力模型辅助的四旋翼飞行器高度容错估计方法。
背景技术
近几年，四旋翼无人机不仅在航空摄影领域得到广泛应用，而且在一些专业领域也得到了很好的应用，例如精细农业、城市监控、生态等。对于四旋翼飞行器来说，要执行不同的任务，高度信息是必不可少的，例如悬停、垂直起飞着陆和一些监视和探测任务。然而高度量测传感器故障也许会导致四旋翼飞行器坠毁。此外，为了提供值得信赖的高度测量值，介绍容错策略也是非常重要的。
高度估计通常需要融合两种传感器信息，一种是加速度计，它是通过测量加速度提供高度预测。另一个传感器是通过测量绝对高度信息进行高度的校正，例如高度量测传感器、GPS、运动捕获系统，或者通过使用声呐传感器测量相对高度。为了实现高度的故障容错估计，以上两类传感器均被需要。现有的故障检测方法通常是将惯性导航系统作为参考系统，假设无故障，然而微小型飞行器的MEMS惯性器件并非完全可靠，需要同时考虑惯性传感器故障和量测传感器故障。
发明内容
为了解决上述背景技术提出的技术问题，本发明旨在提供一种升力模型辅助的四旋翼飞行器高度容错估计方法，同时考虑惯性传感器故障和量测传感器故障，提高导航系统鲁棒性，
为了实现上述技术目的，本发明的技术方案为：
一种升力模型辅助的四旋翼飞行器高度容错估计方法，包括以下步骤：
(1)通过飞行实验获得四旋翼飞行器升力模型的升力系数
(2)通过两个平行的联邦卡尔曼滤波器进行导航滤波，获得四旋翼飞行器的导航信息；
(3)通过故障检测策略在导航滤波过程中对四旋翼飞行器高度方向传感器进行故障检测；
(4)如果步骤(3)检测到四旋翼飞行器高度方向传感器故障，则进行故障传感器的故障定位、隔离与恢复，并滤除故障传感器的导航信息，输出正确的导航信息。
采用上述技术方案带来的有益效果：
(1)本发明利用四旋翼飞行器的升力模型，结合四旋翼飞行器机载传感器，形成一个新型的故障诊断滤波器，能够对四旋翼飞行器垂直方向上的高度传感器进行故障诊断，包括z轴加速度计故障和高度量测传感器故障；
(2)本发明通过使用四旋翼飞行器的气动模型，形成四旋翼的高度冗余，提高四旋翼飞行器的导航系统鲁棒性。
附图说明
图1为本发明的总体结构图；
图2为本发明导航系统故障检测结构图；
图3为本发明故障检测流程图。
具体实施方式
以下将结合附图，对本发明的技术方案进行详细说明。
本发明提出了一种升力模型辅助的四旋翼飞行器高度容错估计方法，通过使用四旋翼飞行器的升力模型，与其机载传感器(高度计)相结合，提供四旋翼飞行器的高度估计，形成四旋翼飞行器高度方向传感器的冗余，从而实现四旋翼飞行器传感器的故障诊断，其高度方向传感器包括惯性传感器和量测传感器，如图1、图2、图3所示。
步骤1：通过飞行实验获得四旋翼飞行器升力模型的升力系数
四旋翼飞行器的升力模型：
上式中，fbz为机体系下的z轴加速度，为四旋翼飞行器升力模型的升力系数，ωir为第i个旋翼的角速度，i＝1,2,3,4。
通过四旋翼飞行器的上下飞行实验，获取z轴加速度计的输出fbz和4个电子调速器的输出ωir，将其作为升力模型的输入，作为升力模型的输出，使用最小二乘法进行升力系数的拟合，获得参数
步骤2：通过两个平行的联邦卡尔曼滤波器进行导航滤波，获得四旋翼飞行器的导航信息。
两个平行的联邦卡尔曼滤波器分别为INS/GPS/MS/barometer联邦卡尔曼滤波器和IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器，其中，INS为惯性导航系统，GPS为全球定位系统，MS为磁传感器，barometer为气压计，IS为缺少z轴加速度计的惯性传感器，ESC为测量旋翼转速的电子调速器。
所述INS/GPS/MS/barometer联邦卡尔曼滤波器包含两个子滤波器，分别为INS/barometer子滤波器和INS/GPS/MS子滤波器；所述IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器包含两个子滤波器，分别为IS/ESC/barometer子滤波器和IS/ESC/GPS/MS子滤波器。
INS/GPS/MS/barometer联邦卡尔曼滤波器的导航滤波过程如下：
(201)建立INS/barometer子滤波器和INS/GPS/MS子滤波器的状态方程：
上式中，φ为横滚角，分别为机体系相对于导航系的角速度在机体系下在x、y、z轴上分量的表示，θ为俯仰角，ψ为偏航角，为机体系相对于导航系的线速度在机体系下的表示，为机体系相对于导航系的角速度在机体系下的表示，g为重力加速度，fbx,fby,fbz为机体系相对于导航系的加速度在机体系下在x、y、z轴上分量的表示，PE为东向位置，PN为北向位置，h为高度；(202)建立INS/barometer子滤波器的量测方程：
hb＝h
上式中，hb为由气压计获得的量测量，h为高度；
(203)建立INS/GPS/MS子滤波器的量测方程：
上式中，ψm为由磁传感器获得的航向角，VEG、VNG、PEG、VNG为由GPS获得的东向速度、北向速度、东向位置、北向位置；
(204)状态方程和量测方程离散化：
(204a)状态方程离散化：
XI(k)＝AI(k,k-1)XI(k-1)+BI(k-1)UI(k-1)
上式中，XI(k)为k时刻的状态量，AI(k，k-1)为k-1到k时刻的一步转移矩阵，XI(k-1)为k-1时刻的状态量，BI(k-1)为k-1时刻的输入系数矩阵，UI(k-1)为k-1时刻的输入矩阵：
Ω3×1＝[sinφcosθ -sinθ -cosφcosθ]T
上式中，ΔT为离散采样时间，I3×3为3×3的单位矩阵，03×3为3×3的零矩阵，为机体系相对于导航系的线速度在机体系下在x、y、z轴上分量的表示，上标T表示转置；
(204b)INS/barometer子滤波器的量测方程离散化：
Y1(k)＝HI1(k)XI(k)
上式中，Y1(k)为INS/barometer子滤波器k时刻的量测量，HI1(k)为INS/barometer子滤波器k时刻的量测矩阵：
Y1(k)＝h
HI1(k)＝[0 0 0 0 0 0 0 0 1]
(204c)INS/GPS/MS子滤波器的量测方程离散化：
Y2(k)＝HI2(k)XI(k)
上式中，Y2(k)为INS/GPS/MS子滤波器k时刻的量测量，HI2(k)为INS/GPS/MS子滤波器k时刻的量测矩阵：
Y2(k)＝[ψm VEG VNG PEG PNG]T
上式中，01×2、01×6、02×3分别为1×2的零矩阵、1×6的零矩阵、2×3的零矩阵；
(205)更新INS/barometer子滤波器和INS/GPS/MS子滤波器的卡尔曼滤波：
(205a)INS/barometer子滤波器和INS/GPS/MS子滤波器状态一步预测和一步预测均方差：
状态一步预测：
一步预测均方差：
PI(k|k-1)＝AI(k,k-1)PI(k-1|k-1)AI(k,k-1)T+GI(k-1)WI(k-1)GI(k-1)T上式中，为两个子滤波器状态向量在k时刻的一步预测，为两个子滤波器的状态向量在k-1时刻的状态估计，PI(k|k-1)为两个子滤波器在k时刻的一步预测均方差，PI(k-1|k-1)为两个子滤波器在k-1时刻估计均方误差，GI(k-1)为k-1时刻噪声系数矩阵，WI(k-1)为k-1时刻状态噪声；
(205b)子滤波器量测更新：
KIi(k)＝PI(k|k-1)HIi(k)T[HIi(k)PI(k|k-1)HIi(k)T+Ri(k)]-1
PIi(k|k)＝[I-KIi(k)HIi(k)]PI(k|k-1)
上式中，i＝1,2，分别代表INS/barometer子滤波器和INS/GPS/MS子滤波器的量测更新过程，KIi(k)为两个子滤波器在k时刻的滤波增益，HIi(k)为两个子滤波器在k时刻量测矩阵，Ri(k)为两个自滤波器在k时刻的量测噪声，上标-1表示求逆，为两个子滤波器在k时刻的状态估计，为两个子滤波器状态向量在k时刻的一步预测，Yi(k)为两个子滤波器在k时刻的量测输出，PIi(k|k)为两个子滤波器k时刻估计均方误差，I为单位矩阵；
(205c)主滤波器的全局融合及联邦重置：
PIg(k|k)＝[PI1(k|k)-1+PI2(k|k)-1]-1
上式中，PIg(k|k)为INS/GPS/MS/barometer联邦卡尔曼滤波器在k时刻的融合状态和融合状态协方差，PI1(k|k)、PI2(k|k)分别表示INS/barometer子滤波器和INS/GPS/MS子滤波器在k时刻的估计均方误差，分别表示INS/barometer子滤波器和INS/GPS/MS子滤波器在k时刻的状态估计，PI(k|k)为两个子滤波器在k时刻的状态变量和状态协方差的重置；
IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器的导航滤波过程：
(211)建立IS/ESC/barometer子滤波器和IS/ESC/GPS/MS子滤波器的状态方程：
上式中，φ为横滚角，分别为机体系相对于导航系的角速度在机体系下在x、y、z轴上分量的表示，θ为俯仰角，ψ为偏航角，为机体系相对于导航系的线速度在机体系下的表示，为机体系相对于导航系的角速度在机体系下的表示，g为重力加速度，fbx,fby为机体系相对于导航系的加速度在机体系下在x、y轴上分量的表示，为升力模型获得的z轴加速度，PE为东向位置，PN为北向位置，h为高度；
(212)建立IS/ESC/barometer子滤波器的量测方程：
hb＝h
上式中，hb为由加计获得的量测量，h为高度。
(213)建立IS/ESC/GPS/MS子滤波器的量测方程：
上式中，ψm为由磁传感器获得的航向角，VEG、VNG、PEG、VNG为由GPS获得的东向速度、北向速度、东向位置、北向位置；
(214)状态方程和量测方程离散化：
(214a)状态方程离散化：
XIM(k)＝AIM(k,k-1)XIM(k-1)+BIM(k-1)UIM(k-1)
上式中，XIM(k)为k时刻的状态量，AIM(k，k-1)为k-1到k时刻的一步转移矩阵，XIM(k-1)为k-1时刻的状态量，BIM(k-1)为k-1时刻的输入系数矩阵，UIM(k-1)为k-1时刻的输入矩阵：
上式中，ΔT为离散采样时间，I3×3为3×3的单位矩阵，03×3为3×3的零矩阵，为机体系相对于导航系的线速度在机体系下在x、y、z轴上分量的表示，上标T表示转置；
(214b)IS/ESC/barometer子滤波器的量测方程离散化：
Y1(k)＝HIM1(k)XIM(k)
上式中，Y1(k)为IS/ESC/barometer子滤波器k时刻的量测量，HIM1(k)为IS/ESC/barometer子滤波器k时刻的量测矩阵：
Y1(k)＝h
HIM1(k)＝[0 0 0 0 0 0 0 0 1]
(214c)IS/GPS/MS子滤波器的量测方程离散化：
Y2(k)＝HIM2(k)XIM(k)
上式中，Y2(k)为k时刻的量测量，HIM2(k)为k时刻的量测矩阵：
Y2(k)＝[ψm VEG VNG PEG PNG]T
上式中，01×2、01×6、02×3分别为1×2的零矩阵、1×6的零矩阵、2×3的零矩阵；
(215)更新IS/ESC/barometer子滤波器和IS/ESC/GPS/MS子滤波器的卡尔曼滤波，该过程与步骤(205)中INS/barometer子滤波器和INS/GPS/MS子滤波器的滤波更新过程相同。
步骤3：通过故障检测策略在导航滤波过程中对四旋翼飞行器高度方向传感器进行故障检测，滤除故障传感器的导航信息。
故障检测方法包括标准χ2检测和n步预测χ2检测，气压计、z轴加速度计和升力模型的故障检测通过统计参数与阈值的大小比较，求得故障检测函数的值，具体步骤如下：
(31)建立z轴加速度计和升力模型故障检测函数：
上式中，J0(k)为故障检测函数，T0为故障检测阈值，为通过升力模型获得的z轴加速度，ωir为第i个旋翼的角速度，i＝1,2,3,4，fbz(k)通过z轴加速度计获得的z轴加速度，z轴加速度计和升力模型不同时发生故障，J0(k)＝0，表示z轴加速度计和升力模型均无故障，J0(k)＝1，表示z轴加速度计或者升力模型故障；
(32)标准χ2检测故障检测：
(32a)建立INS/barometer子滤波器用于故障检测的统计参数λI(k)和故障检测函数J1(k)：
λI(k)＝rI(k)PIr(k)-1rI(k)T
上式中，PIr(k)＝HI1(k)PI(k|k-1)HI1(k)T+R1(k)，λI(k)为k时刻的故障统计参数，rI(k)为k时刻的残差，PIr(k)为k时刻的残差方差，上标T表示转置，上标-1表示求逆，Y1(k)为INS/barometer子滤波器k时刻的量测量，HI1(k)为INS/barometer子滤波器k时刻的量测矩阵，为两个子滤波器状态向量在k-1到k时刻的一步预测，PI(k|k-1)为两个子滤波器在k时刻的一步预测均方差，R1(k)为INS/barometer子滤波器在k时刻的量测噪声；
上式中，J1(k)为故障检测函数，T1为阈值，如果J1(k)＝0，表示z轴加速度计故障和气压计均无故障，J1(k)＝1，表示z轴加速度计或者气压计故障；
(32b)建立IS/ESC/barometer子滤波器的统计参数λIM(k)和故障阈值函数J2(k)：
λIM(k)＝rIM(k)PIMr(k)-1rIM(k)T
上式中，PIMr(k)＝HIM1(k)PIM(k|k-1)HIM1(k)T+R1(k)，λIM(k)为k时刻的故障统计参数，rIM(k)为k时刻的残差，PIMr(k)为k时刻的残差方差，Y1(k)为IS/ESC/barometer子滤波器k时刻的量测量，HIM1(k)为IS/ESC/barometer子滤波器k时刻的量测矩阵，为IS/ESC/barometer子滤波器状态向量在k时刻的一步预测，PIM(k|k-1)为IS/ESC/barometer子滤波器在k时刻的一步预测均方差，R1(k)为IS/ESC/barometer子滤波器在k时刻的量测噪声；
上式中，J2(k)为故障检测函数，T2为阈值，如果J2(k)＝0，表示升力模型故障和气压计均无故障，J2(k)＝1，表示升力模型或者气压计故障；
(33)如果J0(k)＝1，进行n步预测χ2检测：
(33a)构建状态向量它是来自系统状态的n步预测，缺少量测更新过程，状态变量的一步预测值即为最终状态的估计值，通过使用n步预测的状态估计值求得故障统计参数，INS/barometer子滤波器的n步预测状态和协方差更新过程如下：
上式中，为状态向量在k-n时刻的n步预测滤波估计，为INS/barometer子滤波器在k-n时刻的滤波估计，为k-i时刻的状态估计值，AI(k-i,k-i-1)为k-i-1到k-i时刻的一步转移矩阵，为k-i-1时刻的状态估计值，BI(k-i-1)为k-i-1时刻的输入系数矩阵，UI(k-i-1)为k-i-1时刻的输入矩阵，为k-n时刻的n步预测均方误差，PI1(k-n|k-n)为INS/barometer子滤波器估计均方误差，为k-i时刻n步预测均方误差，AI(k-i,k-i-1)为k-i-1到k-i时刻的一步转移矩阵，为k-i-1时刻n步预测均方误差，GI(k-i-1)为k-i-1时刻的噪声系数矩阵，WI(k-i-1)为k-i-1时刻噪声矩阵；
(33b)建立INS/barometer子滤波器故障检测的统计参数和故障检测函数J3(k)：
上式中，为k时刻的故障统计参数，为k时刻的残差，为k时刻的残差方差，Y1(k)为INS/barometer子滤波器k时刻的量测量，HI1(k)为INS/barometer子滤波器k时刻的量测矩阵，为状态在k时刻的n步预测滤波估计，为k时刻的n步预测均方差，R1(k)为INS/barometer子滤波器在k时刻的量测噪声；
上式中，J3(k)为故障检测函数，T3为阈值，如果J3(k)＝0，表示z轴加速度计无故障，J3(k)＝0，表示z轴加速度计故障；
(33c)构建状态向量它是来自系统状态的预测，缺少量测更新过程，状态变量的一步预测值即为最终状态的估计值，通过使用n步预测的状态估计值求得故障统计参数，IS/ESC/barometer子滤波器的n步预测状态和协方差更新过程如下：
上式中，为状态向量在k-n时刻的n步预测滤波估计，为IS/ESC/barometer子滤波器在k-n时刻的滤波估计，为k-i时刻的状态估计值，AIM(k-i,k-i-1)为k-i-1到k-i时刻的一步转移矩阵，为k-i-1时刻的状态估计值，BIM(k-i-1)为k-i-1时刻的输入系数矩阵，UIM(k-i-1)为k-i-1时刻的输入矩阵，为k-n时刻的n步预测均方误差，PIM1(k-n|k-n)为INS/barometer子滤波器估计均方误差，为k-i时刻n步预测均方误差，AIM(k-i,k-i-1)为k-i-1到k-i时刻的一步转移矩阵，为k-i-1时刻n步预测均方误差，GIM(k-i-1)为k-i-1时刻的噪声系数矩阵，WIM(k-i-1)为k-i-1时刻噪声矩阵；
(33d)建立IS/ESC/barometer子滤波器故障检测的统计参数和故障检测函数J4(k)：
上式中， 为k时刻的故障统计参数，为k时刻的残差，为k时刻的残差方差，Y1(k)为INS/barometer子滤波器k时刻的量测量，HIM1(k)为IS/ESC/barometer子滤波器k时刻的量测矩阵，为k时刻的n步预测，为k时刻的n步预测均方差，R1(k)为IS/ESC/barometer子滤波器在k时刻的量测噪声；
上式中，J4(k)为故障检测函数，T4为阈值，如果J4(k)＝0，表示升力模型故障，J4(k)＝1，表示升力模型无故障。
标准χ2检测和n步预测χ2检测均可以检测z轴加速度计、气压计和升力模型的故障，n步预测χ2检测对于z轴加速度计和升力模型故障更敏感，对于气压计故障，通过标准χ2检测得到的J1(k)、J2(k)进行故障的定位隔离，对于z轴加速度计和升力模型故障，通过n步预测χ2检测和标准χ2检测进行故障的定位隔离。
步骤4：若检测到四旋翼飞行器高度方向传感器故障，则通过故障定位、隔离、重构与恢复策略进行故障传感器的故障隔离与恢复，具体步骤如下：
(A)故障定位策略：
(A1)建立无故障定位函数：
F0(k)＝J1(k)∨J2(k)∨J0(k)∨J3(k)∨J4(k)＝0
上式中，“∨”为逻辑运算符的“或”运算，J1(k)、J2(k)分别为INS/barometer和IS/ESC/barometer标准χ2检测的故障检测函数，J3(k)、J4(k)为INS/barometer和IS/ESC/barometer的n步预测χ2检测的故障检测函数，J0(k)为z轴加速度故障检测函数，F0(k)＝0表示高度方向传感器无故障；
(A2)建立气压计故障定位函数：
上式中，“∧”为逻辑运算符的“与”运算，Fb(k)＝0表示气压计无故障，Fb(k)＝1表示气压计故障；
(A3)建立z轴加速度计故障定位函数：
上式中，“—”为逻辑运算的“非”运算符，Fa(k)＝0表示z轴加速度计无故障，Fa(k)＝1表示z轴加速度计故障；
(A4)建立升力模型故障定位函数：
上式中，Fm(k)＝0表示升力模型无故障，Fm(k)＝1表示升力模型故障；
(B)故障隔离、重构和恢复策略：
(B1)无故障时的滤波策略：
高度方向传感器无故障发生，此时全局故障隔离输出：
上式中，为状态变量在k时刻的全局故障隔离输出，为INS/GPS/MS/barometer联邦卡尔曼滤波器在k时刻的融合输出；
(B2)z轴加速度计故障隔离和恢复策略：
当Fa(k)＝1，即z轴加速度计故障时，在INS/GPS/MS/barometer联邦卡尔曼滤波器中，INS/barometer子滤波器被隔离，在全局故障隔离输出中，INS/GPS/MS/barometer联邦卡尔曼滤波器被隔离，此时全局故障隔离输出：
上式中，为全局状态变量在k时刻的全局故障隔离输出，为IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器在k时刻的融合输出，INS/GPS/MS/barometer联邦卡尔曼滤波器，使用全局故障隔离输出结果进行状态重置及估计均方误差重置：
NS/barometer和INS/GPS/MS子滤波器，联邦重置：
IS/ESC/barometer和IS/ESC/GPS/MS子滤波器，联邦重置：
上式中，PIMg(k|k)为IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器在k时刻的融合输出及均方误差，PIg(k|k)为INS/GPS/MS/barometer联邦卡尔曼滤波器在k时刻的重置状态及重置均方误差，PI(k|k)为INS/barometer和INS/GPS/MS子滤波器在k时刻的重置状态及重置均方误差，PIM(k|k)为IS/ESC/barometer和IS/ESC/GPS/MS子滤波器在k时刻的重置状态及重置均方误差；
此时进入隔离周期，以防止频繁的故障模型切换。在隔离周期内，全局故障隔离输出中使用IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器的融合输出进行全局故障隔离输出的更新，如果Z轴加速度计故障再次被检测出来，则隔离周期被复位，重新进入隔离周期；
(B3)升力模型故障隔离和恢复策略：
当Fm(k)＝1，即升力模型故障时，在IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器中，IS/ESC/barometer子滤波器被隔离，在全局故障隔离输出中，IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器被隔离，此时全局故障隔离输出：
上式中，为全局状态变量在k时刻的全局故障隔离输出，为INS/GPS/MS/barometer联邦卡尔曼滤波器在k时刻的融合输出,IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器，使用全局故障隔离输出结果进行状态重置：
IS/ESC/barometer和IS/ESC/GPS/MS子滤波器，联邦重置：
INS/barometer和IS/GPS/MS子滤波器，联邦重置：
上式中，PIg(k|k)为INS/GPS/MS/barometer联邦卡尔曼滤波器在k时刻的融合输出及均方误差，PIMg(k|k)为IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器在k时刻的重置状态及重置均方误差，PIM(k|k)为IS/ESC/barometer和IS/ESC/GPS/MS子滤波器在k时刻的重置状态及重置均方误差，PI(k|k)为INS/barometer和INS/GPS/MS子滤波器在k时刻的重置状态及重置均方误差；
此时进入隔离周期，以防止频繁的故障模型切换。在隔离周期内，全局故障隔离输出中使用INS/GPS/MS/barometer联邦卡尔曼滤波器的融合输出进行全局故障隔离输出的更新，如果升力模型故障再次被检测出来，则隔离周期被复位，重新进入隔离周期；
(B4)气压计故障隔离和恢复策略：
当Fb(k)＝1，即气压计故障时，INS/barometer和IS/ESC/barometer子滤波器均被隔离，此时需要使用额外高度传感器加入到INS/GPS/MS/barometer联邦滤波器中代替气压计，进行量测校正：
上式中，为状态变量在k时刻的全局故障隔离输出，为加入额外高度量测传感器后INS/GPS/MS/barometer联邦滤波器的输出。
实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (6)

1.一种升力模型辅助的四旋翼飞行器高度容错估计方法，其特征在于，包括以下步骤：
(1)通过飞行实验获得四旋翼飞行器升力模型的升力系数
(2)通过两个平行的联邦卡尔曼滤波器进行导航滤波，获得四旋翼飞行器的导航信息；
(3)通过故障检测策略在导航滤波过程中对四旋翼飞行器高度方向传感器进行故障检测；
(4)如果步骤(3)检测到四旋翼飞行器高度方向传感器故障，则进行故障传感器的故障定位、隔离与恢复，并滤除故障传感器的导航信息，输出正确的导航信息。
2.根据权利要求1所述升力模型辅助的四旋翼飞行器高度容错估计方法，其特征在于，步骤(1)的具体过程如下：
建立四旋翼飞行器的升力模型：
上式中，fbz为机体系下的z轴加速度，为四旋翼飞行器升力模型的升力系数，ωir为第i个旋翼的角速度，i＝1,2,3,4；
通过四旋翼飞行器的上下飞行实验，获取z轴加速度计的输出fbz和4个电子调速器的输出ωir，将其作为升力模型的输入，作为升力模型的输出，使用最小二乘法进行升力系数的拟合，获得参数
3.根据权利要求1所述升力模型辅助的四旋翼飞行器高度容错估计方法，其特征在于，步骤(2)中所述两个平行的联邦卡尔曼滤波器分别为INS/GPS/MS/barometer联邦卡尔曼滤波器和IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器，其中，INS为惯性导航系统，GPS为全球定位系统，MS为磁传感器，barometer为气压计，IS为缺少z轴加速度计的惯性传感器，ESC为测量旋翼转速的电子调速器；
所述INS/GPS/MS/barometer联邦卡尔曼滤波器包含两个子滤波器，分别为INS/barometer子滤波器和INS/GPS/MS子滤波器；所述IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器包含两个子滤波器，分别为IS/ESC/barometer子滤波器和IS/ESC/GPS/MS子滤波器。
4.根据权利要求3所述升力模型辅助的四旋翼飞行器高度容错估计方法，其特征在于，在步骤(2)中，INS/GPS/MS/barometer联邦卡尔曼滤波器的导航滤波过程如下：
(201)建立INS/barometer子滤波器和INS/GPS/MS子滤波器的状态方程：
上式中，φ为横滚角，分别为机体系相对于导航系的角速度在机体系下在x、y、z轴上分量的表示，θ为俯仰角，ψ为偏航角，为机体系相对于导航系的线速度在机体系下的表示，为机体系相对于导航系的角速度在机体系下的表示，g为重力加速度，fbx,fby,fbz为机体系相对于导航系的加速度在机体系下在x、y、z轴上分量的表示，PE为东向位置，PN为北向位置，h为高度；
(202)建立INS/barometer子滤波器的量测方程：
hb＝h
上式中，hb为由气压计获得的量测量，h为高度；
(203)建立INS/GPS/MS子滤波器的量测方程：
上式中，ψm为由磁传感器获得的航向角，VEG、VNG、PEG、VNG为由GPS获得的东向速度、北向速度、东向位置、北向位置；
(204)状态方程和量测方程离散化：
(204a)状态方程离散化：
XI(k)＝AI(k,k-1)XI(k-1)+BI(k-1)UI(k-1)
上式中，XI(k)为k时刻的状态量，AI(k，k-1)为k-1到k时刻的一步转移矩阵，XI(k-1)为k-1时刻的状态量，BI(k-1)为k-1时刻的输入系数矩阵，UI(k-1)为k-1时刻的输入矩阵：
Ω3×1＝[sinφcosθ -sinθ -cosφcosθ]T
上式中，ΔT为离散采样时间，I3×3为3×3的单位矩阵，03×3为3×3的零矩阵，为机体系相对于导航系的线速度在机体系下在x、y、z轴上分量的表示，上标T表示转置；
(204b)INS/barometer子滤波器的量测方程离散化：
Y1(k)＝HI1(k)XI(k)
上式中，Y1(k)为INS/barometer子滤波器k时刻的量测量，HI1(k)为INS/barometer子滤波器k时刻的量测矩阵：
Y1(k)＝h
HI1(k)＝[0 0 0 0 0 0 0 0 1]
(204c)INS/GPS/MS子滤波器的量测方程离散化：
Y2(k)＝HI2(k)XI(k)
上式中，Y2(k)为INS/GPS/MS子滤波器k时刻的量测量，HI2(k)为INS/GPS/MS子滤波器k时刻的量测矩阵：
Y2(k)＝[ψm VEG VNG PEG PNG]T
上式中，01×2、01×6、02×3分别为1×2的零矩阵、1×6的零矩阵、2×3的零矩阵；
(205)更新INS/barometer子滤波器和INS/GPS/MS子滤波器的卡尔曼滤波：
(205a)INS/barometer子滤波器和INS/GPS/MS子滤波器状态一步预测和一步预测均方差：
状态一步预测：
一步预测均方差：
PI(k|k-1)＝AI(k,k-1)PI(k-1|k-1)AI(k,k-1)T+GI(k-1)WI(k-1)GI(k-1)T
上式中，为两个子滤波器状态向量在k时刻的一步预测，为两个子滤波器的状态向量在k-1时刻的状态估计，PI(k|k-1)为两个子滤波器在k时刻的一步预测均方差，PI(k-1|k-1)为两个子滤波器在k-1时刻估计均方误差，GI(k-1)为k-1时刻噪声系数矩阵，WI(k-1)为k-1时刻状态噪声；
(205b)子滤波器量测更新：
KIi(k)＝PI(k|k-1)HIi(k)T[HIi(k)PI(k|k-1)HIi(k)T+Ri(k)]-1
PIi(k|k)＝[I-KIi(k)HIi(k)]PI(k|k-1)
上式中，i＝1,2，分别代表INS/barometer子滤波器和INS/GPS/MS子滤波器的量测更新过程，KIi(k)为两个子滤波器在k时刻的滤波增益，HIi(k)为两个子滤波器在k时刻量测矩阵，Ri(k)为两个自滤波器在k时刻的量测噪声，上标-1表示求逆，为两个子滤波器在k时刻的状态估计，为两个子滤波器状态向量在k时刻的一步预测，Yi(k)为两个子滤波器在k时刻的量测输出，PIi(k|k)为两个子滤波器k时刻估计均方误差，I为单位矩阵；
(205c)主滤波器的全局融合及联邦重置：
PIg(k|k)＝[PI1(k|k)-1+PI2(k|k)-1]-1
上式中，PIg(k|k)为INS/GPS/MS/barometer联邦卡尔曼滤波器在k时刻的融合状态和融合状态协方差，PI1(k|k)、PI2(k|k)分别表示INS/barometer子滤波器和INS/GPS/MS子滤波器在k时刻的估计均方误差，分别表示INS/barometer子滤波器和INS/GPS/MS子滤波器在k时刻的状态估计，为两个子滤波器在k时刻的状态变量和状态协方差的重置；
IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器的导航滤波过程：
(211)建立IS/ESC/barometer子滤波器和IS/ESC/GPS/MS子滤波器的状态方程：
上式中，φ为横滚角，分别为机体系相对于导航系的角速度在机体系下在x、y、z轴上分量的表示，θ为俯仰角，ψ为偏航角，为机体系相对于导航系的线速度在机体系下的表示，为机体系相对于导航系的角速度在机体系下的表示，g为重力加速度，fbx,fby为机体系相对于导航系的加速度在机体系下在x、y轴上分量的表示，为升力模型获得的z轴加速度，PE为东向位置，PN为北向位置，h为高度；
(212)建立IS/ESC/barometer子滤波器的量测方程：
hb＝h
上式中，hb为由加计获得的量测量，h为高度。
(213)建立IS/ESC/GPS/MS子滤波器的量测方程：
上式中，ψm为由磁传感器获得的航向角，VEG、VNG、PEG、VNG为由GPS获得的东向速度、北向速度、东向位置、北向位置；
(214)状态方程和量测方程离散化：
(214a)状态方程离散化：
XIM(k)＝AIM(k,k-1)XIM(k-1)+BIM(k-1)UIM(k-1)
上式中，XIM(k)为k时刻的状态量，AIM(k，k-1)为k-1到k时刻的一步转移矩阵，XIM(k-1)为k-1时刻的状态量，BIM(k-1)为k-1时刻的输入系数矩阵，UIM(k-1)为k-1时刻的输入矩阵：
上式中，ΔT为离散采样时间，I3×3为3×3的单位矩阵，03×3为3×3的零矩阵，为机体系相对于导航系的线速度在机体系下在x、y、z轴上分量的表示，上标T表示转置；
(214b)IS/ESC/barometer子滤波器的量测方程离散化：
Y1(k)＝HIM1(k)XIM(k)
上式中，Y1(k)为IS/ESC/barometer子滤波器k时刻的量测量，HIM1(k)为IS/ESC/barometer子滤波器k时刻的量测矩阵：
Y1(k)＝h
HIM1(k)＝[0 0 0 0 0 0 0 0 1]
(214c)IS/GPS/MS子滤波器的量测方程离散化：
Y2(k)＝HIM2(k)XIM(k)
上式中，Y2(k)为k时刻的量测量，HIM2(k)为k时刻的量测矩阵：
Y2(k)＝[ψm VEG VNG PEG PNG]T
上式中，01×2、01×6、02×3分别为1×2的零矩阵、1×6的零矩阵、2×3的零矩阵；
(215)更新IS/ESC/barometer子滤波器和IS/ESC/GPS/MS子滤波器的卡尔曼滤波，该过程与步骤(205)中INS/barometer子滤波器和INS/GPS/MS子滤波器的滤波更新过程相同。
5.根据权利要求1所述升力模型辅助的四旋翼飞行器高度容错估计方法，其特征在于，在步骤(3)中，对四旋翼飞行器高度方向传感器进行故障检测的具体方法如下：
(31)建立z轴加速度计和升力模型故障检测函数：
上式中，J0(k)为故障检测函数，T0为故障检测阈值，为通过升力模型获得的z轴加速度，ωir为第i个旋翼的角速度，i＝1,2,3,4，fbz(k)通过z轴加速度计获得的z轴加速度，z轴加速度计和升力模型不同时发生故障，J0(k)＝0，表示z轴加速度计和升力模型均无故障，J0(k)＝1，表示z轴加速度计或者升力模型故障；
(32)标准χ2检测故障检测：
(32a)建立INS/barometer子滤波器用于故障检测的统计参数λI(k)和故障检测函数J1(k)：
λI(k)＝rI(k)PIr(k)-1rI(k)T
上式中，PIr(k)＝HI1(k)PI(k|k-1)HI1(k)T+R1(k)，λI(k)为k时刻的故障统计参数，rI(k)为k时刻的残差，PIr(k)为k时刻的残差方差，上标T表示转置，上标-1表示求逆，Y1(k)为INS/barometer子滤波器k时刻的量测量，HI1(k)为INS/barometer子滤波器k时刻的量测矩阵，为两个子滤波器状态向量在k-1到k时刻的一步预测，PI(k|k-1)为两个子滤波器在k时刻的一步预测均方差，R1(k)为INS/barometer子滤波器在k时刻的量测噪声；
上式中，J1(k)为故障检测函数，T1为阈值，如果J1(k)＝0，表示z轴加速度计故障和气压计均无故障，J1(k)＝1，表示z轴加速度计或者气压计故障；
(32b)建立IS/ESC/barometer子滤波器的统计参数λIM(k)和故障阈值函数J2(k)：
λIM(k)＝rIM(k)PIMr(k)-1rIM(k)T
上式中，PIMr(k)＝HIM1(k)PIM(k|k-1)HIM1(k)T+R1(k)，λIM(k)为k时刻的故障统计参数，rIM(k)为k时刻的残差，PIMr(k)为k时刻的残差方差，Y1(k)为IS/ESC/barometer子滤波器k时刻的量测量，HIM1(k)为IS/ESC/barometer子滤波器k时刻的量测矩阵，为IS/ESC/barometer子滤波器状态向量在k时刻的一步预测，PIM(k|k-1)为IS/ESC/barometer子滤波器在k时刻的一步预测均方差，R1(k)为IS/ESC/barometer子滤波器在k时刻的量测噪声；
上式中，J2(k)为故障检测函数，T2为阈值，如果J2(k)＝0，表示升力模型故障和气压计均无故障，J2(k)＝1，表示升力模型或者气压计故障；
(33)如果J0(k)＝1，进行n步预测χ2检测：
(33a)构建状态向量它是来自系统状态的n步预测，缺少量测更新过程，状态变量的一步预测值即为最终状态的估计值，通过使用n步预测的状态估计值求得故障统计参数，INS/barometer子滤波器的n步预测状态和协方差更新过程如下：
上式中，为状态向量在k-n时刻的n步预测滤波估计，为INS/barometer子滤波器在k-n时刻的滤波估计，为k-i时刻的状态估计值，AI(k-i,k-i-1)为k-i-1到k-i时刻的一步转移矩阵，为k-i-1时刻的状态估计值，BI(k-i-1)为k-i-1时刻的输入系数矩阵，UI(k-i-1)为k-i-1时刻的输入矩阵，为k-n时刻的n步预测均方误差，PI1(k-n|k-n)为INS/barometer子滤波器估计均方误差，为k-i时刻n步预测均方误差，AI(k-i,k-i-1)为k-i-1到k-i时刻的一步转移矩阵，为k-i-1时刻n步预测均方误差，GI(k-i-1)为k-i-1时刻的噪声系数矩阵，WI(k-i-1)为k-i-1时刻噪声矩阵；
(33b)建立INS/barometer子滤波器故障检测的统计参数和故障检测函数J3(k)：
上式中，为k时刻的故障统计参数，为k时刻的残差，为k时刻的残差方差，Y1(k)为INS/barometer子滤波器k时刻的量测量，HI1(k)为INS/barometer子滤波器k时刻的量测矩阵，为状态在k时刻的n步预测滤波估计，为k时刻的n步预测均方差，R1(k)为INS/barometer子滤波器在k时刻的量测噪声；
上式中，J3(k)为故障检测函数，T3为阈值，如果J3(k)＝0，表示z轴加速度计无故障，J3(k)＝0，表示z轴加速度计故障；
(33c)构建状态向量它是来自系统状态的预测，缺少量测更新过程，状态变量的一步预测值即为最终状态的估计值，通过使用n步预测的状态估计值求得故障统计参数，IS/ESC/barometer子滤波器的n步预测状态和协方差更新过程如下：
上式中，为状态向量在k-n时刻的n步预测滤波估计，为IS/ESC/barometer子滤波器在k-n时刻的滤波估计，为k-i时刻的状态估计值，AIM(k-i,k-i-1)为k-i-1到k-i时刻的一步转移矩阵，为k-i-1时刻的状态估计值，BIM(k-i-1)为k-i-1时刻的输入系数矩阵，UIM(k-i-1)为k-i-1时刻的输入矩阵，为k-n时刻的n步预测均方误差，PIM1(k-n|k-n)为INS/barometer子滤波器估计均方误差，为k-i时刻n步预测均方误差，AIM(k-i,k-i-1)为k-i-1到k-i时刻的一步转移矩阵，为k-i-1时刻n步预测均方误差，GIM(k-i-1)为k-i-1时刻的噪声系数矩阵，WIM(k-i-1)为k-i-1时刻噪声矩阵；
(33d)建立IS/ESC/barometer子滤波器故障检测的统计参数和故障检测函数J4(k)：
上式中， 为k时刻的故障统计参数，为k时刻的残差，为k时刻的残差方差，Y1(k)为INS/barometer子滤波器k时刻的量测量，HIM1(k)为IS/ESC/barometer子滤波器k时刻的量测矩阵，为k时刻的n步预测，为k时刻的n步预测均方差，R1(k)为IS/ESC/barometer子滤波器在k时刻的量测噪声；
上式中，J4(k)为故障检测函数，T4为阈值，如果J4(k)＝0，表示升力模型故障，J4(k)＝1，表示升力模型无故障。
6.根据权利要求5所述升力模型辅助的四旋翼飞行器高度容错估计方法，其特征在于，若检测到四旋翼飞行器高度方向传感器故障，则通过故障定位、隔离、重构与恢复策略进行故障传感器的故障隔离与恢复，具体步骤如下：
(A)故障定位策略：
(A1)建立无故障定位函数：
F0(k)＝J1(k)∨J2(k)∨J0(k)∨J3(k)∨J4(k)＝0
上式中，“∨”为逻辑运算符的“或”运算，J1(k)、J2(k)分别为INS/barometer和IS/ESC/barometer标准χ2检测的故障检测函数，J3(k)、J4(k)为INS/barometer和IS/ESC/barometer的n步预测χ2检测的故障检测函数，J0(k)为z轴加速度故障检测函数，F0(k)＝0表示高度方向传感器无故障；
(A2)建立气压计故障定位函数：
上式中，“∧”为逻辑运算符的“与”运算，Fb(k)＝0表示气压计无故障，Fb(k)＝1表示气压计故障；
(A3)建立z轴加速度计故障定位函数：
上式中，“—”为逻辑运算的“非”运算符，Fa(k)＝0表示z轴加速度计无故障，Fa(k)＝1表示z轴加速度计故障；
(A4)建立升力模型故障定位
NS/barometer和INS/GPS/MS子滤波器，函数：
上式中，Fm(k)＝0表示升力模型无故障，Fm(k)＝1表示升力模型故障；
(B)故障隔离、重构和恢复策略：
(B1)无故障时的滤波策略：
高度方向传感器无故障发生，此时全局故障隔离输出：
上式中，为状态变量在k时刻的全局故障隔离输出，为INS/GPS/MS/barometer联邦卡尔曼滤波器在k时刻的融合输出；
(B2)z轴加速度计故障隔离和恢复策略：
当Fa(k)＝1，即z轴加速度计故障时，在INS/GPS/MS/barometer联邦卡尔曼滤波器中，INS/barometer子滤波器被隔离，在全局故障隔离输出中，INS/GPS/MS/barometer联邦卡尔曼滤波器被隔离，此时全局故障隔离输出：
上式中，为全局状态变量在k时刻的全局故障隔离输出，为IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器在k时刻的融合输出，INS/GPS/MS/barometer联邦卡尔曼滤波器，使用全局故障隔离输出结果进行状态重置及估计均方误差重置：
PIg(k|k)＝PIMg(k)联邦重置：
IS/ESC/barometer和IS/ESC/GPS/MS子滤波器，联邦重置：
上式中，PIMg(k|k)为IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器在k时刻的融合输出及均方误差，PIg(k|k)为INS/GPS/MS/barometer联邦卡尔曼滤波器在k时刻的重置状态及重置均方误差，PI(k|k)为INS/barometer和INS/GPS/MS子滤波器在k时刻的重置状态及重置均方误差，PIM(k|k)为IS/ESC/barometer和IS/ESC/GPS/MS子滤波器在k时刻的重置状态及重置均方误差；
此时进入隔离周期，以防止频繁的故障模型切换。在隔离周期内，全局故障隔离输出中使用IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器的融合输出进行全局故障隔离输出的更新，如果Z轴加速度计故障再次被检测出来，则隔离周期被复位，重新进入隔离周期；
(B3)升力模型故障隔离和恢复策略：
当Fm(k)＝1，即升力模型故障时，在IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器中，IS/ESC/barometer子滤波器被隔离，在全局故障隔离输出中，IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器被隔离，此时全局故障隔离输出：
上式中，为全局状态变量在k时刻的全局故障隔离输出，为INS/GPS/MS/barometer联邦卡尔曼滤波器在k时刻的融合输出,IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器，使用全局故障隔离输出结果进行状态重置：
IS/ESC/barometer和IS/ESC/GPS/MS子滤波器，联邦重置：
INS/barometer和IS/GPS/MS子滤波器，联邦重置：
上式中，PIg(k|k)为INS/GPS/MS/barometer联邦卡尔曼滤波器在k时刻的融合输出及均方误差，PIMg(k|k)为IS/ESC/GPS/MS/barometer联邦卡尔曼滤波器在k时刻的重置状态及重置均方误差，PIM(k|k)为IS/ESC/barometer和IS/ESC/GPS/MS子滤波器在k时刻的重置状态及重置均方误差，PI(k|k)为INS/barometer和INS/GPS/MS子滤波器在k时刻的重置状态及重置均方误差；
此时进入隔离周期，以防止频繁的故障模型切换。在隔离周期内，全局故障隔离输出中使用INS/GPS/MS/barometer联邦卡尔曼滤波器的融合输出进行全局故障隔离输出的更新，如果升力模型故障再次被检测出来，则隔离周期被复位，重新进入隔离周期；
(B4)气压计故障隔离和恢复策略：
当Fb(k)＝1，即气压计故障时，INS/barometer和IS/ESC/barometer子滤波器均被隔离，此时需要使用额外高度传感器加入到INS/GPS/MS/barometer联邦滤波器中代替气压计，进行量测校正：
上式中，为状态变量在k时刻的全局故障隔离输出，为加入额外高度量测传感器后INS/GPS/MS/barometer联邦滤波器的输出。
CN201711274710.3A 2017-12-06 2017-12-06 一种升力模型辅助的四旋翼飞行器高度容错估计方法 Active CN108168509B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711274710.3A CN108168509B (zh) 2017-12-06 2017-12-06 一种升力模型辅助的四旋翼飞行器高度容错估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711274710.3A CN108168509B (zh) 2017-12-06 2017-12-06 一种升力模型辅助的四旋翼飞行器高度容错估计方法

Publications (2)

Publication Number Publication Date
CN108168509A true CN108168509A (zh) 2018-06-15
CN108168509B CN108168509B (zh) 2019-08-13

Family

ID=62525239

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711274710.3A Active CN108168509B (zh) 2017-12-06 2017-12-06 一种升力模型辅助的四旋翼飞行器高度容错估计方法

Country Status (1)

Country Link
CN (1) CN108168509B (zh)

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108981709A (zh) * 2018-08-02 2018-12-11 南京航空航天大学 基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法
CN108981708A (zh) * 2018-08-02 2018-12-11 南京航空航天大学 四旋翼扭矩模型/航向陀螺/磁传感器容错组合导航方法
CN109612459A (zh) * 2018-11-15 2019-04-12 南京航空航天大学 基于动力学模型的四旋翼飞行器惯性传感器容错导航方法
CN110244335A (zh) * 2019-06-04 2019-09-17 深圳供电局有限公司 双天线抗干扰导航装置及无人机
CN110426032A (zh) * 2019-07-30 2019-11-08 南京航空航天大学 一种解析式冗余的飞行器容错导航估计方法
CN111044051A (zh) * 2019-12-30 2020-04-21 星际(江苏)航空科技有限公司 一种复合翼无人机容错组合导航方法
CN112882072A (zh) * 2021-01-15 2021-06-01 中国人民解放军海军航空大学 一种基于误差反馈的飞行器惯性加gps混合高度测量方法
CN113821059A (zh) * 2021-11-24 2021-12-21 中航金城无人系统有限公司 一种多旋翼无人机传感器故障安全飞行控制系统及方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103323007A (zh) * 2013-06-17 2013-09-25 南京航空航天大学 一种基于时变量测噪声的鲁棒联邦滤波方法
CN103837151A (zh) * 2014-03-05 2014-06-04 南京航空航天大学 一种四旋翼飞行器的气动模型辅助导航方法
CN104913781A (zh) * 2015-06-04 2015-09-16 南京航空航天大学 一种基于动态信息分配的非等间隔联邦滤波方法
CN106679693A (zh) * 2016-12-14 2017-05-17 南京航空航天大学 一种基于故障检测的矢量信息分配自适应联邦滤波方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103323007A (zh) * 2013-06-17 2013-09-25 南京航空航天大学 一种基于时变量测噪声的鲁棒联邦滤波方法
CN103837151A (zh) * 2014-03-05 2014-06-04 南京航空航天大学 一种四旋翼飞行器的气动模型辅助导航方法
CN104913781A (zh) * 2015-06-04 2015-09-16 南京航空航天大学 一种基于动态信息分配的非等间隔联邦滤波方法
CN106679693A (zh) * 2016-12-14 2017-05-17 南京航空航天大学 一种基于故障检测的矢量信息分配自适应联邦滤波方法

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108981709A (zh) * 2018-08-02 2018-12-11 南京航空航天大学 基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法
CN108981708A (zh) * 2018-08-02 2018-12-11 南京航空航天大学 四旋翼扭矩模型/航向陀螺/磁传感器容错组合导航方法
CN108981708B (zh) * 2018-08-02 2021-10-19 南京航空航天大学 四旋翼扭矩模型/航向陀螺/磁传感器容错组合导航方法
CN109612459A (zh) * 2018-11-15 2019-04-12 南京航空航天大学 基于动力学模型的四旋翼飞行器惯性传感器容错导航方法
CN109612459B (zh) * 2018-11-15 2023-03-17 南京航空航天大学 基于动力学模型的四旋翼飞行器惯性传感器容错导航方法
CN110244335A (zh) * 2019-06-04 2019-09-17 深圳供电局有限公司 双天线抗干扰导航装置及无人机
CN110426032A (zh) * 2019-07-30 2019-11-08 南京航空航天大学 一种解析式冗余的飞行器容错导航估计方法
CN111044051A (zh) * 2019-12-30 2020-04-21 星际(江苏)航空科技有限公司 一种复合翼无人机容错组合导航方法
CN111044051B (zh) * 2019-12-30 2023-11-03 星际智航(太仓)航空科技有限公司 一种复合翼无人机容错组合导航方法
CN112882072A (zh) * 2021-01-15 2021-06-01 中国人民解放军海军航空大学 一种基于误差反馈的飞行器惯性加gps混合高度测量方法
CN112882072B (zh) * 2021-01-15 2022-03-08 中国人民解放军海军航空大学 一种基于误差反馈的飞行器惯性加gps混合高度测量方法
CN113821059A (zh) * 2021-11-24 2021-12-21 中航金城无人系统有限公司 一种多旋翼无人机传感器故障安全飞行控制系统及方法

Also Published As

Publication number Publication date
CN108168509B (zh) 2019-08-13

Similar Documents

Publication Publication Date Title
CN108168509B (zh) 一种升力模型辅助的四旋翼飞行器高度容错估计方法
CN111433612B (zh) 用于估计飞行器空中数据的基于模型和飞行信息的组合训练的神经网络系统
CN108931258B (zh) 用于监测和估计与飞行器的飞行相关的参数的方法和设备
Johansen et al. On estimation of wind velocity, angle-of-attack and sideslip angle of small UAVs using standard sensors
CN104567799B (zh) 基于多传感器信息融合的小型旋翼无人机高度测量方法
Rhudy et al. Aircraft model-independent airspeed estimation without pitot tube measurements
CN108106635A (zh) 惯性卫导组合导航系统的长航时抗干扰姿态航向校准方法
Zhou et al. Integrated navigation system for a low-cost quadrotor aerial vehicle in the presence of rotor influences
Mahony et al. A non-linear observer for attitude estimation of a fixed-wing unmanned aerial vehicle without GPS measurements
CN110426032B (zh) 一种解析式冗余的飞行器容错导航估计方法
CN108981708B (zh) 四旋翼扭矩模型/航向陀螺/磁传感器容错组合导航方法
Leutenegger et al. Robust state estimation for small unmanned airplanes
CN109612459B (zh) 基于动力学模型的四旋翼飞行器惯性传感器容错导航方法
CN108981709B (zh) 基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法
ES2651369T3 (es) Sistema y proceso para medición y evaluación de datos aéreos e inerciales
Ducard et al. Strategies for sensor-fault compensation on UAVs: review, discussions & additions
Bao et al. Aerodynamic model/INS/GPS failure-tolerant navigation method for multirotor UAVs based on federated Kalman Filter
Dolega et al. Possibilities of using software redundancy in low cost aeronautical control systems
KR100648882B1 (ko) 무인 항공기 자동 항법 시스템의 관성 값 계산 장치 및 그방법
CN113821059B (zh) 一种多旋翼无人机传感器故障安全飞行控制系统及方法
Hazbon Alvarez et al. Digital twin concept for aircraft sensor failure
Sanz et al. Attitude estimation using low-cost sensors: A comparative analysis
CN112629521A (zh) 一种旋翼飞行器双冗余组合的导航系统建模方法
Jain et al. Inertial Angle of Attack estimation for a small transport aircraft
Notaro et al. HW VS SW sensor redundancy: Fault detection and isolation observer based approaches for inertial measurement units

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