CN108981709A - 基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法 - Google Patents
基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法 Download PDFInfo
- Publication number
- CN108981709A CN108981709A CN201810872417.5A CN201810872417A CN108981709A CN 108981709 A CN108981709 A CN 108981709A CN 201810872417 A CN201810872417 A CN 201810872417A CN 108981709 A CN108981709 A CN 108981709A
- Authority
- CN
- China
- Prior art keywords
- moment
- fault
- model
- axis gyro
- subfilter
- 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
Classifications
-
- 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/20—Instruments for performing navigational calculations
-
- 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
- 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/18—Stabilised platforms, e.g. by gyroscope
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
本发明公开一种基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法,步骤是:周期读取k时刻四旋翼飞行器机载传感器信息,执行故障检测滤波器及故障定位策略,判断x轴陀螺、y轴陀螺、横滚力矩模型、俯仰力矩模型的故障;根据故障定位结果,确定各个子滤波器的状态方程,进行各子滤波器的数据融合;根据故障检测结果,执行全局滤波器,对各子滤波器进行故障隔离,对无故障的子滤波器进行数据融合,得到x轴角速度、y轴角速度、横滚角、俯仰角信息;根据全局滤波器结果,进行重置更新,并执行***重置策略。此种方法通过四旋翼飞行器的动力学模型,形成x、y轴陀螺的冗余,实现x、y轴陀螺的故障检测及***重置,在陀螺故障时仍能获得姿态角、航向角的准确估计。
Description
技术领域
本发明属于组合导航及容错导航领域,具体涉及一种基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法的容错导航方法。
背景技术
四旋翼飞行器具有体积小、结构简单、可悬停和垂直起降等优点,特别适合在近地面环境(如室内、城区和丛林等)中执行监视、侦察等任务,具有广阔的军事和民用前景。导航***为四旋翼飞行器提供其飞行控制***所必须的导航信息,是其完成各种复杂飞行任务的必要保障。
目前四旋翼飞行器常用的传感器包括惯性传感器、GNSS(卫星导航***)、磁传感器、气压高度计,其中惯性传感器包括陀螺仪与加速度计。受成本、体积所限,四旋翼飞行器中选用的惯性传感器精度、可靠性较低,易受外界温度、振动干扰而产生性能下降,甚至失效。此时,会导致导航***精度下降,影响飞行安全。目前,尚未有针对惯性传感器失效情况下的四旋翼飞行器导航方法。本专利针对x轴、y轴陀螺故障的情况,提出了一种基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法,通过力矩模型构建x轴、y轴陀螺的冗余信息,可以实现对x轴陀螺、y轴陀螺、横滚力矩模型、俯仰力矩模型的故障检测,在检测到x轴陀螺或y轴陀螺故障时,可以使用横滚力矩模型或俯仰力矩模型代替故障的陀螺进行导航解算,获得准确的姿态角估计结果。
发明内容
本发明的目的,在于提供一种基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法,通过四旋翼飞行器的动力学模型,形成x、y轴陀螺的冗余,实现x、y轴陀螺的故障检测及***重置,在陀螺故障时仍能获得姿态角、航向角的准确估计。
为了达成上述目的,本发明的解决方案是:
一种基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法,包括如下步骤:
步骤一:周期读取k时刻四旋翼飞行器机载传感器信息,包括旋翼转速传感器信息ω1(k)、ω2(k)、ω3(k)、ω4(k),其分别为四个旋翼的转速;GPS信息VNG(k)、VEG(k)、VDG(k),其分别为北向速度、东向速度、地向速度;磁传感器信息ψm(k);陀螺信息其分别为机体系相对于导航系的角速度在机体系x、y、z轴上的分量;加计信息分别为机体系相对于导航系的加速度在机体系x、y、z轴上的分量;
步骤二:执行故障检测滤波器及故障定位策略,判断x轴陀螺、y轴陀螺、横滚力矩模型、俯仰力矩模型的故障;
步骤三:根据步骤二的故障定位结果,确定各个子滤波器的状态方程,进行力矩模型/x轴陀螺子滤波器、力矩模型/y轴陀螺子滤波器、力矩模型/加速度计/GPS子滤波器、力矩模型/磁传感器子滤波器的数据融合;
步骤四:根据故障检测结果,执行全局滤波器,对力矩模型/x轴陀螺子滤波器、力矩模型/y轴陀螺子滤波器、力矩模型/加速度计/GPS子滤波器、力矩模型/磁传感器子滤波器进行故障隔离,对无故障的子滤波器进行数据融合,得到x轴角速度、y轴角速度、横滚角、俯仰角信息;
步骤五:根据全局滤波器结果,对各个子滤波器、故障检测滤波器状态量、均方误差进行重置更新,并执行***重置策略。
采用上述方案后,本发明利用四旋翼飞行器的横滚力矩、俯仰力矩模型,与机载陀螺、加速度计、磁传感器、GPS相融合,可实现x轴、y轴陀螺故障情况下对横滚角、俯仰角的准确估计。在该方法中,通过横滚力矩模型、俯仰力矩模型构建x轴、y轴陀螺的冗余信息,建立故障检测函数,实现对x轴、y轴陀螺故障以及力矩故障的检测;同时,在x轴、y轴陀螺故障情况下,通过横滚力矩模型、俯仰力矩对其角速度信号进行重构,代替故障的陀螺进行导航解算,实现容错导航。该方法无需增加额外的惯性传感器,可实现对x轴、y轴陀螺的故障检测、隔离、信号重构,保障陀螺失效情况下姿态角的估计精度。
附图说明
图1为本发明方法的滤波结构图;
图2为本发明方法的故障检测结构图;
图3为本发明方法的流程图;
图4为x轴陀螺故障时,故障检测结果;
图5为x轴陀螺故障时,x轴角速度估计结果;
图6为x轴陀螺故障时,y轴角速度估计结果;
图7为x轴陀螺故障时,横滚角估计结果;
图8为x轴陀螺故障时,俯仰角估计结果;
图9为y轴陀螺故障时,故障检测结果;
图10为y轴陀螺故障时,x轴角速度估计结果;
图11为y轴陀螺故障时,y轴角速度估计结果;
图12为y轴陀螺故障时,横滚角估计结果;
图13为y轴陀螺故障时,俯仰角估计结果;
图14为横滚力矩模型故障时,故障检测结果;
图15为俯仰力矩模型故障时,故障检测结果。
具体实施方式
以下将结合附图,对本发明的技术方案及有益效果进行详细说明。
本发明方法的滤波流程如图1所示,故障检测结构如图2所示,其采用半物理仿真的形式,四旋翼无人机连续飞行三个矩形,采集机载传感器实验数据,其包含惯性传感器实验数据、电子调速器数据、GPS数据、磁传感器数据。
本发明提供的一种基于力矩模型的四旋翼横滚角、俯仰角容错估计方法,包括如下步骤:
步骤一:周期读取k时刻四旋翼飞行器机载传感器信息,包括旋翼转速传感器信息ω1(k)、ω2(k)、ω3(k)、ω4(k),其分别为四个旋翼的转速;GPS信息VNG(k)、VEG(k)、VDG(k),其分别为北向速度、东向速度、地向速度;磁传感器信息ψm(k);陀螺信息其分别为机体系相对于导航系的角速度在机体系x、y、z轴上的分量;加计信息分别为机体系相对于导航系的加速度在机体系x、y、z轴上的分量;
步骤二:执行故障检测滤波器及故障定位策略,判断x轴陀螺、y轴陀螺、横滚力矩模型、俯仰力矩模型的故障,故障检测滤波器由横滚力矩模型/x轴陀螺检测滤波器、横滚力矩模型/y轴陀螺检测滤波器、俯仰力矩模型/加速度计/GPS检测滤波器、x轴陀螺/加速度计/GPS检测滤波器、y轴陀螺/加速度计/GPS检测滤波器等6个检测滤波器组成,上述6个检测滤波器的状态更新、故障定位更新步骤如下:
步骤21,横滚力矩模型/x轴陀螺检测滤波器故障检测步骤如下:
步骤211,计算k时刻横滚力矩模型/x轴陀螺检测滤波器的状态估计值及估计均方误差
kx0(k)=kx0(k-1)
式中,kx0(k)为k时刻横滚力矩模型零偏系数;kx0(k-1)为k-1时刻横滚力矩模型零偏系数;kx1、kx2为横滚力矩模型系数,为常值;ωmx(k)为k时刻通过横滚力矩模型计算得到的机体系相对于导航系的角速度在机体系x轴上的分量;ωmx(k-1)为k-1时刻通过横滚力矩模型计算得到的机体系相对于导航系的角速度在机体系x轴上的分量;为k时刻机体系相对于导航系的加速度在机体系y轴上的分量,通过加计输出获得;ωi(k)为k时刻第i个旋翼的转速,i=1,2,3,4;ΔT为离散采样周期;上标T表示转置;为k时刻的估计均方误差;为k-1时刻的估计均方误差;为雅可比矩阵;Gd1(k-1)=[I2×2],为***噪声系数,I2×2为2×2的单位矩阵;Wd1(k-1)=[wkx0(k-1) wmx(k-1)]T,为状态噪声,wkx0(k-1)为k-1时刻横滚力矩模型零偏白噪声;wmx(k-1)为k-1时刻横滚力矩模型的白噪声;
步骤212,计算k时刻横滚力矩模型/x轴陀螺检测滤波器的故障统计参数
式中,为k时刻横滚力矩模型/x轴陀螺检测滤波器的统计参数;为k时刻的残差;为k时刻机体系相对于导航系的角速度在机体系x轴上的分量,通过x轴陀螺输出获得;为k时刻的残差方差;Hd1(k)=[01];Rd1(k)=diag([wgx(k)]2),wgx为机体系x轴陀螺的白噪声;
步骤213,根据故障统计参数结果,计算k时刻横滚力矩模型/x轴陀螺检测滤波器的检测函数,判断横滚力矩模型和x轴陀螺是否发生故障
式中,T1是阈值,当J1(k)=1时,横滚力矩模型或x轴陀螺故障;当J1(k)=0时,横滚力矩模型和x轴陀螺均无故障;
步骤22,俯仰力矩模型/y轴陀螺检测滤波器故障检测步骤如下:
步骤221,计算k时刻俯仰力矩模型/y轴陀螺检测滤波器的状态估计值及估计均方误差
ky0(k)=ky0(k-1)
式中,ky0(k)为k时刻俯仰力矩模型零偏系数;ky0(k-1)为k-1时刻俯仰力矩模型零偏系数;ky1、ky2为俯仰力矩模型系数;ωmy(k)为k时刻通过俯仰力矩模型计算得到的机体系相对于导航系的角速度在机体系y轴上的分量;ωmy(k-1)为k-1时刻通过俯仰力矩模型计算得到的机体系相对于导航系的角速度在机体系y轴上的分量;为雅可比矩阵;Gd2(k-1)=[I2×2],为***噪声系数;Wd2(k-1)=[wky0(k-1) wmy(k-1)]T,为状态噪声,wky0(k-1)为k-1时刻俯仰力矩模型零偏白噪声,wmy(k-1)为k-1时刻俯仰力矩模型的白噪声;
步骤222,计算k时刻俯仰力矩模型/y轴陀螺检测滤波器的故障统计参数
式中,为k时刻俯仰力矩模型/y轴陀螺检测器的统计参数;为k时刻的残差;为k时刻机体系相对于导航系的角速度在机体系y轴上的分量,通过陀螺输出获得;为k时刻的残差方差;Hd2(k)=[0 1];Rd2(k)=diag([wgy(k)]2),wgy为机体系y轴陀螺的白噪声;
步骤223,根据故障统计参数结果,计算k时刻俯仰力矩模型/y轴陀螺检测滤波器的检测函数,判断俯仰力矩模型和y轴陀螺是否发生故障
式中,T2是阈值,当J2(k)=1时,俯仰力矩模型或y轴陀螺故障;当J2(k)=0时,俯仰力矩模型和y轴陀螺均无故障;
步骤23,横滚力矩模型/加速度计/GPS检测滤波器故障检测步骤如下:
步骤231,计算k时刻横滚力矩模型/加速度计/GPS检测滤波器的状态估计值及估计均方误差
kx0(k)=kx0(k-1)
式中,q0(k)、q1(k)、q2(k)、q3(k)为k时刻四元素;q0(k-1)、q1(k-1)、q2(k-1)、q3(k-1)为k-1时刻四元素;为机体系相对于导航系的角速度在机体系z轴上的分量,通过z轴陀螺输出获得;为雅可比矩阵;为k-1时刻***噪声系数;I2×2为2×2的单位矩阵;02×3为2×3的零矩阵;04×2为4×2的零矩阵;Wd3(k-1)=[wkx0(k-1) wmx(k-1) wωx(k-1)wωy(k-1) wgz(k-1)]T,为k-1时刻状态噪声,wωx(k-1)为k-1时刻机体系x轴角速度白噪声;wgz(k-1)为k-1时刻机体系z轴陀螺白噪声;
步骤232,计算k时刻横滚力矩模型/加速度计/GPS检测滤波器的故障统计参数
式中,为k时刻横滚力矩模型/加速度计/GPS检测滤波器的统计参数;为k时刻的残差;
VDG(k)分别为导航系下北向、东向、地向速度,通过GPS获得, 为k-1时刻***滤波四元素估计;分别为k时刻机体系相对于导航系的线速度在机体系y轴的分量;g为重力加速度;为k时刻的残差方差,Hd3(k)=[01×2 Ω1×4 d3],Ω1×4 d3=[2gq1(k) 2gq0(k) 2gq3(k)2gq2(k)],为加速度计和GPS的组合白噪声在机体系y轴上的分量;
步骤233,根据故障统计参数结果,计算k时刻横滚力矩模型/加速度计/GPS检测滤波器的检测函数,判断横滚力矩模型是否发生故障
式中,T3是阈值,当J3(k)=1时,横滚力矩模型故障;当J3(k)=0时,横滚力矩模型无故障;
步骤24,俯仰力矩模型/加速度计/GPS检测滤波器故障检测步骤如下:
步骤241,计算k时刻俯仰力矩模型/加速度计/GPS检测滤波器的状态估计值及估计均方误差
ky0(k)=ky0(k-1)
式中,为雅可比矩阵;Gd4(k-1)=Gd3(k-1),为***噪声系数矩阵;Wd4(k-1)=Wd3(k-1),为状态噪声;
步骤242,计算k时刻俯仰力矩模型/加速度计/GPS检测滤波器的故障统计参数
式中,为k时刻俯仰力矩模型/加速度计/GPS检测滤波器的统计参数;为k时刻的残差;
为k时刻的残差方差;Hd4(k)=[01×2 Ω1×4 d4];Ω1×4 d4=[-2gq2(k) 2gq3(k) -2gq0(k) 2gq1(k)];为加速度计和GPS的组合白噪声在机体系x轴上的分量;
步骤243,根据故障统计参数结果,计算k时刻检测滤波器的检测函数,判断俯仰力矩模型是否发生故障
式中,T4是阈值,当J4(k)=1时,俯仰力矩模型故障;当J4(k)=0时,俯仰力矩模型无故障;
步骤25,x轴陀螺/加速度计/GPS检测滤波器检测步骤如下:
步骤251,计算k时刻x轴陀螺/加速度计/GPS检测滤波器的状态估计值及估计均方误差
式中,为雅可比矩阵;为***噪声系数;Wd5(k-1)=[wgx(k-1) wgy(k-1) wgz(k-1)]T,为k-1时刻状态噪声;
步骤252,计算k时刻x轴陀螺/加速度计/GPS检测滤波器的故障统计参数
式中,为k时刻x轴陀螺/加速度计/GPS检测滤波器的统计参数,为k时刻的残差,为k时刻的残差方差,Hd5(k)=Ω1×4 d5,Ω1×4 d5=Ω1×4 d3,
步骤253,根据故障统计参数结果,计算k时刻x轴陀螺/加速度计/GPS检测滤波器的检测函数,判断x轴陀螺是否发生故障
式中,T5是阈值,当J5(k)=1时,x轴陀螺故障;当J5(k)=0时,无故障;
步骤26,y轴陀螺/加速度计/GPS检测滤波器检测步骤如下:
步骤261,计算k时刻y轴陀螺/加速度计/GPS检测滤波器的状态估计值及估计均方误差
式中,Φd6(k)=Φd5(k),为雅可比矩阵;Gd6(k-1)=Gd5(k-1),为***噪声系数;Wd6(k-1)=Wd5(k-1),为状态噪声;
步骤262,计算k时刻y轴陀螺/加速度计/GPS检测滤波器的故障统计参数
式中,为k时刻y轴陀螺/加速度计/GPS检测滤波器的统计参数,为k时刻的残差,为k时刻的残差方差,Hd6(k)=Ω1×4 d6,Ω1×4 d6=Ω1×4 d4;
步骤263,根据故障统计参数结果,计算k时刻y轴陀螺/加速度计/GPS检测滤波器的检测函数,判断y轴陀螺故障是否发生故障
式中,T6是阈值,当J6(k)=1时,y轴陀螺故障;当J6(k)=0时,无故障;
步骤27,根据各个检测滤波器的检测函数结果,计算k时刻故障定位函数及根据故障定位函数计算结果,进行故障定位,步骤如下:
步骤271,计算k时刻预发生故障定位函数,判断***是否进入预发生故障
式中,“∨”表示逻辑运算中“或”运算符,“∧”表示逻辑运算中“与”运算符;当Fpre(k)=1时,进入预发生故障阶段,当Fpre(k)=0时,未进入预发生故障;
步骤272,计算k时刻x轴陀螺故障定位函数,判断x轴陀螺是否发生故障:
当FGX(k)=1时,x轴陀螺故障;当FGX(k)=0时,x轴陀螺无故障;
步骤273,计算k时刻y轴陀螺故障定位函数,判断y轴陀螺是否发生故障:
当FGY(k)=1时,y轴陀螺故障;当FGY(k)=0时,y轴陀螺无故障;
步骤274,计算横滚力矩模型故障定位函数,判断横滚力矩模型是否发生故障:
当FMX(k)=1时,横滚力矩模型故障;当FMX(k)=0时,横滚力矩模型无故障;
步骤275,计算俯仰力矩模型故障定位函数,判断俯仰力矩模型是否发生故障:
当FMY(k)=1时,俯仰力矩模型故障;当FMY(k)=0时,俯仰力矩模型无故障;
步骤276,计算无故障定位函数,判断***是否发生故障:
当Fno(k)=1时,无故障。
步骤三:根据步骤二的故障定位结果,确定各个子滤波器的状态方程,进行力矩模型/x轴陀螺子滤波器、力矩模型/y轴陀螺子滤波器、力矩模型/加速度计/GPS子滤波器、力矩模型/磁传感器子滤波器的数据融合:
步骤31,计算k时刻四个子滤波器的状态预测及预测均方误差
情况311,无故障或x轴陀螺或y轴陀螺故障时,四个子滤波器均按照如下步骤进行状态预测及均方误差预测:
步骤3111,计算k时刻横滚力矩模型零偏及俯仰力矩模型零偏预测
kx0(k|k-1)=kx0(k-1)
ky0(k|k-1)=ky0(k-1)
式中,kx0(k|k-1)、ky0(k|k-1)分别为横滚力矩模型、俯仰力矩模型零偏项在k-1时刻到k时刻的状态一步预测;
步骤3112,计算k时刻x轴角速度预测及y轴角速度预测
式中,ωmx(k|k-1)、ωmy(k|k-1)分别为x、y轴角速度在k-1时刻到k时刻的状态一步预测;
步骤3113,计算k时刻四元素预测
式中,q0(k|k-1)、q1(k|k-1)、q2(k|k-1)、q3(k|k-1)为四元素在k-1时刻到k时刻的状态一步预测;
步骤3114,计算一步预测均方误差PC(k|k-1)
PC(k|k-1)=ΦC(k|k-1)PC(k-1)ΦC(k|k-1)T+GC(k-1)QC(k-1)GC(k-1)T式中,PC(k|k-1)为k-1到k时刻的一步预测均方误差;PC(k-1)为k-1时刻估计均方误差;为雅可比矩阵,为非线性状态方程,为k-1时刻状态量,
QC(k-1)=diag(WC(k-1)2);
WC(k-1)=[wkx0(k-1) wky0(k-1) wmx(k-1) wmy(k-1) wωx(k-1) wωy(k-1) wgz(k-1)]T为k-1时刻的状态噪声;
情况312,横滚力矩模型/x轴陀螺检测滤波器预发生故障或横滚力矩模型故障时,四个子滤波器状态预测均按照如下步骤进行状态预测及均方误差预测:
3个子滤波器的状态方程中关于x轴角速度的预测方程修改为ωmx(k|k-1)=ωmx(k-1),雅可比矩阵修改为: 其余同无故障情况;
情况313,俯仰力矩模型/y轴陀螺检测滤波器预发生故障或俯仰力矩模型故障时,四个子滤波器状态预测均按照如下步骤进行状态预测及均方误差预测:
3个子滤波器的状态方程中关于y轴角速度的预测方程修改为ωmy(k|k-1)=ωmy(k-1),雅可比矩阵修改为: 其余同无故障情况;
步骤32,力矩模型/x轴陀螺子滤波器量测更新步骤如下:
步骤321,计算k时刻的力矩模型/x轴陀螺子滤波器的滤波增益
KC1(k)=PC(k|k-1)HC1(k)T[HC1(k)PC(k|k-1)HC1(k)T+RC1(k)]-1
式中,HC1(k)=[01×2 1 01×5]T,01×2为1×2的零矩阵;01×5为1×5的零矩阵;KC1(k)为k时刻的滤波增益,RC1(k)=diag([wgx(k)]2),为k时刻的量测噪声;
步骤322,计算k时刻力矩模型/x轴陀螺子滤波器状态估计值
式中,为k时刻状态量的估计值,为k时刻的量测量,
步骤323,计算k时刻力矩模型/x轴陀螺子滤波器估计均方误差PC1(k)
PC1(k)=[I-KC1(k)HC1(k)]PC(k|k-1)
式中,PC1(k)为k时刻估计均方误差;I为单位矩阵;
步骤33,力矩模型/y轴陀螺子滤波器量测更新步骤如下:
步骤331,计算k时刻的力矩模型/y轴陀螺子滤波器的滤波增益
KC2(k)=PC(k|k-1)HC2(k)T[HC2(k)PC(k|k-1)HC2(k)T+RC2(k)]-1
式中,HC2(k)=[01×3 1 01×4]T,01×3为1×3的零矩阵,01×4为1×4的零矩阵;KC2(k)为k时刻的滤波增益,RC2(k)=diag([wgy(k)]2),为k时刻的量测噪声;
步骤332,计算k时刻力矩模型/y轴陀螺子滤波器状态估计值
式中,为k时刻状态量的估计值,为k时刻的量测量,
步骤333,计算k时刻力矩模型/y轴陀螺子滤波器估计均方误差PC2(k)
PC2(k)=[I-KC2(k)HC2(k)]PC(k|k-1)
式中,PC2(k)为k时刻估计均方误差;I为单位矩阵;
步骤34,力矩模型/加速度计/GPS子滤波器量测更新步骤如下:
步骤341,计算k时刻的力矩模型/加速度计/GPS子滤波器的滤波增益
KC3(k)=PC(k|k-1)HC3(k)T[HC3(k)PC(k|k-1)HC3(k)T+RC3(k)]-1
式中,KC3(k)为k时刻的滤波增益;
01×4为1×4的零矩阵,g为重力加速度;为k时刻的量测噪声;
步骤342,计算k时刻力矩模型/加速度计/GPS子滤波器状态估计值
式中,为k时刻状态量的估计值,为k时刻的量测量,
步骤343,计算k时刻力矩模型/加速度计/GPS子滤波器估计均方误差PC3(k)
PC3(k)=[I-KC3(k)HC3(k)]PC(k|k-1)
式中,PC3(k)为k时刻估计均方误差;I为单位矩阵;
步骤35,力矩模型/磁传感器子滤波器更新步骤如下:
步骤351,计算k时刻的力矩模型/磁传感器子滤波器的滤波增益
KC4(k)=PC(k|k-1)HC4(k)T[HC4(k)PC(k|k-1)HC4(k)T+RC4(k)]-1
式中,HC4(k)=[01×4 N1×4],01×4为1×4的零矩阵,
KC4(k)为k时刻的滤波增益,为k时刻的量测噪声,为k时刻磁航向角白噪声;
步骤352,计算k时刻力矩模型/磁传感器子滤波器状态估计值
式中,为k时刻状态量的估计值;YC4(k)=ψm(k)为k时刻磁航向角;
步骤353,计算k时刻力矩模型/磁传感器子滤波器估计均方误差PC4(k)
PC4(k)=[I-KC4(k)HC4(k)]PC(k|k-1)
式中,PC4(k)为k时刻估计均方误差;I为单位矩阵。
步骤四:根据步骤二的故障定位结果和步骤三的子滤波器滤波结果,执行全局滤波器,对力矩模型/x轴陀螺子滤波器、力矩模型/y轴陀螺子滤波器、力矩模型/加速度计/GPS子滤波器、力矩模型/磁传感器子滤波器进行故障隔离,对无故障的子滤波器进行数据融合,得到x轴角速度、y轴角速度、姿态角信息:
情况41,无故障时,按照如下步骤进行子滤波器的故障隔离和全局滤波:
步骤411,根据故障定位结果,隔离故障子滤波器
四个子滤波器均参与全局滤波,无子滤波器被隔离;
步骤412,计算k时刻的全局滤波器的估计均方误差
Pg(k)=[PC1(k)-1+PC2(k)-1+PC3(k)-1+PC4(k)-1]-1
步骤413,计算k时刻的全局滤波器的状态估计值
情况42,横滚力矩模型/x轴陀螺检测滤波器预发生故障时,按照如下步骤进行子滤波器的故障隔离和全局滤波:
步骤421,根据故障定位结果,隔离故障子滤波器
采用横滚力矩模型故障时的状态方程,隔离横滚力矩模型/x轴陀螺检测滤波器;
步骤422,计算k时刻的全局滤波器的估计均方误差
Pg(k)=[PC2(k)-1+PC3(k)-1+PC4(k)-1]-1
步骤423,计算k时刻的全局滤波器的状态估计值
情况43,俯仰力矩模型/y轴陀螺检测滤波器预发生故障时,按照如下步骤进行自滤波器的故障隔离和全局滤波:
步骤431,根据故障定位结果,隔离故障子滤波器
采用俯仰力矩模型故障时的状态方程,隔离俯仰力矩模型/y轴陀螺检测滤波器;
步骤432,计算k时刻的全局滤波器的估计均方误差
Pg(k)=[PC1(k)-1+PC3(k)-1+PC4(k)-1]-1
步骤433,计算k时刻的全局滤波器的状态估计值
情况44,x轴陀螺故障时,按照如下步骤进行子滤波器的故障隔离和全局滤波:
步骤441,根据故障定位结果,隔离故障子滤波器
横滚力矩模型/x轴陀螺子滤波器被隔离,其余三个子滤波器参与全局滤波;
步骤442,计算k时刻的全局滤波器的估计均方误差
Pg(k)=[PC2(k)-1+PC3(k)-1+PC4(k)-1]-1
步骤443,计算k时刻的全局滤波器的状态估计值
情况45,y轴陀螺故障时,按照如下步骤进行子滤波器的故障隔离和全局滤波
步骤451,根据故障定位结果,隔离故障子滤波器
俯仰力矩模型/y轴陀螺子滤波器被隔离,其余三个子滤波器参与全局滤波;
步骤452,计算k时刻的全局滤波器的估计均方误差
Pg(k)=[PC1(k)-1+PC3(k)-1+PC4(k)-1]-1
步骤453,计算k时刻的全局滤波器的状态估计值
情况46,横滚力矩模型故障时,按照如下步骤进行子滤波器的故障隔离和全局滤波:
步骤461,根据故障定位结果,隔离故障子滤波器
采用横滚力矩模型故障时的状态方程,无子滤波器被隔离,其余四个子滤波器参与全局滤波;
步骤462,计算k时刻的全局滤波器的估计均方误差
Pg(k)=[PC1(k)-1+PC2(k)-1+PC3(k)-1+PC4(k)-1]-1
步骤463,计算k时刻的全局滤波器的状态估计值
情况47,俯仰力矩模型故障时,按照如下步骤进行子滤波器的故障隔离和全局滤波
步骤471,根据故障定位结果,隔离故障子滤波器
采用俯仰力矩模型故障时的状态方程,无子滤波器被隔离,其余四个子滤波器参与全局滤波;
步骤472,计算k时刻的全局滤波器的估计均方误差
Pg(k)=[PC1(k)-1+PC2(k)-1+PC3(k)-1+PC4(k)-1]-1
步骤473,计算k时刻的全局滤波器的状态估计值
步骤五:根据全局滤波器结果,对各个子滤波器、故障检测滤波器状态量、误差估计进行重置,执行***重置策略:
步骤51,根据故障检测结果及全局滤波结果对各个子滤波器进行状态量及均方误差重置
情况511,无故障时,四个子滤波器状态量及均方误差重置
PC(k)=4Pg(k)
情况512,横滚力矩模型/x轴陀螺检测滤波器预发生故障时,四个子滤波器状态量及均方误差重置
PC(k)=3Pg(k)
情况513,俯仰力矩模型/y轴陀螺检测滤波器预发生故障时,四个子滤波器状态量及均方误差重置
PC(k)=3Pg(k)
情况514,x轴陀螺故障时,四个子滤波器状态量及均方误差重置
PC(k)=3Pg(k)
情况515,y轴陀螺故障时,四个子滤波器状态量及均方误差重置
PC(k)=3Pg(k)
情况516,横滚力矩模型故障时,四个子滤波器状态量及均方误差重置
PC(k)=4Pg(k)
情况517,俯仰力矩模型故障时,四个子滤波器状态量及均方误差重置
PC(k)=4Pg(k)
步骤52,根据故障检测结果及全局滤波结果对各个检测滤波器进行状态量及均方误差重置
检测滤波器为n步预测卡方检测器,设置n步重置周期,在n步之内不进行重置,使用自身的状态估计结果进行状态递推,在第n步时使用全局滤波结果进行状态量重置,状态及均方误差重置根据滤波结果采用如下方式:
情况521,无故障时,第n步时,检测滤波器状态及均方误差重置为如下:
式中,表示使用状态向量中的第1列中,第i到第j行元素进行重置;Pg(k)[i:j;i:j]表示使用误差估计矩阵中的第i到第j列、第i到第j行元素进行重置;
情况522,横滚力矩模型/x轴陀螺检测滤波器预发生故障时,检测滤波器状态及均方误差重置为如下:
横滚力矩模型/x轴陀螺检测滤波器、横滚力矩模型/加速度计/GPS检测滤波器、x轴陀螺/加速度计/GPS检测滤波器不进行重置更新,只进行累积直至确定故障位置;俯仰力矩模型/y轴陀螺检测滤波器、俯仰力矩模型/加速度计/GPS检测滤波器、y轴陀螺/加速度计/GPS检测滤波器采用n步重置规则,采用如下方法进行重置:
情况523,俯仰力矩模型/y轴陀螺检测滤波器预发生故障时,检测滤波器状态及均方误差重置为如下:
俯仰力矩模型/y轴陀螺检测滤波器、俯仰力矩模型/加速度计/GPS检测滤波器、y轴陀螺/加速度计/GPS检测滤波器不进行重置更新,只进行累积直至确定故障位置;横滚力矩模型/x轴陀螺检测滤波器、横滚力矩模型/加速度计/GPS检测滤波器、x轴陀螺/加速度计/GPS检测滤波器采用n步重置规则,采用如下方法进行重置:
情况524,x轴陀螺故障时,检测滤波器状态及均方误差重置为如下:
当检测到x轴陀螺故障时,对横滚力矩模型/x轴陀螺检测滤波器、x轴陀螺/加速度计/GPS检测滤波器进行立即重置,其他的检测器按照n步重置规则,采用如下方法进行重置:
情况525,y轴陀螺故障时,检测滤波器状态及均方误差重置为如下:
当检测到y轴陀螺故障时,对俯仰力矩模型/y轴陀螺检测滤波器、y轴陀螺/加速度计/GPS检测滤波器进行立即重置,其他的检测器按照n步重置规则,采用如下方法进行重置:
情况526,横滚力矩模型故障时,检测滤波器状态及均方误差重置
当检测到横滚力矩模型故障时,对横滚力矩模型/x轴陀螺检测滤波器、横滚力矩模型/加速度计/GPS检测滤波器进行立即重置,其他的检测器按照n步重置规则,采用如下方法进行重置:
情况527,俯仰力矩模型故障时,检测滤波器状态及均方误差重置
当检测到俯仰力矩模型故障时,对俯仰力矩模型/y轴陀螺检测滤波器、俯仰力矩模型/加速度计/GPS检测滤波器进行立即重置,其他的检测器按照n步重置规则,采用如下方法进行重置:
步骤53,当检测到故障后,基于故障特性,执行以下***重置策略
设置x陀螺、y轴、横滚力矩模型、俯仰力矩模型故障隔离周期,当检测到x陀螺、y轴、横滚力矩模型、俯仰力矩模型故障后,进入故障隔离周期,在隔离周期内认为一直存在故障,在隔离周期内若再次检测到故障,重新开始隔离周期。
以上实施例仅为说明本发明的技术思想,不能以此限定本发明的保护范围,凡是按照本发明提出的技术思想,在技术方案基础上所做的任何改动,均落入本发明保护范围之内。
Claims (5)
1.一种基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法,其特征在于包括如下步骤:
步骤一:周期读取k时刻四旋翼飞行器机载传感器信息,包括旋翼转速传感器信息ω1(k)、ω2(k)、ω3(k)、ω4(k),其分别为四个旋翼的转速;GPS信息VNG(k)、VEG(k)、VDG(k),其分别为北向速度、东向速度、地向速度;磁传感器信息ψm(k);陀螺信息其分别为机体系相对于导航系的角速度在机体系x、y、z轴上的分量;加计信息分别为机体系相对于导航系的加速度在机体系x、y、z轴上的分量;
步骤二:执行故障检测滤波器及故障定位策略,判断x轴陀螺、y轴陀螺、横滚力矩模型、俯仰力矩模型的故障;
步骤三:根据步骤二的故障定位结果,确定各个子滤波器的状态方程,进行力矩模型/x轴陀螺子滤波器、力矩模型/y轴陀螺子滤波器、力矩模型/加速度计/GPS子滤波器、力矩模型/磁传感器子滤波器的数据融合;
步骤四:根据故障检测结果,执行全局滤波器,对力矩模型/x轴陀螺子滤波器、力矩模型/y轴陀螺子滤波器、力矩模型/加速度计/GPS子滤波器、力矩模型/磁传感器子滤波器进行故障隔离,对无故障的子滤波器进行数据融合,得到x轴角速度、y轴角速度、横滚角、俯仰角信息;
步骤五:根据全局滤波器结果,对各个子滤波器、故障检测滤波器状态量、均方误差进行重置更新,并执行***重置策略。
2.如权利要求1所述的基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法,其特征在于:所述步骤二中,故障检测滤波器由横滚力矩模型/x轴陀螺检测滤波器、俯仰力矩模型/y轴陀螺检测滤波器、横滚力矩模型/加速度计/GPS检测滤波器、俯仰力矩模型/加速度计/GPS检测滤波器、x轴陀螺/加速度计/GPS检测滤波器、y轴陀螺/加速度计/GPS检测滤波器组成,上述6个检测滤波器的状态更新、故障定位步骤如下:
步骤21,横滚力矩模型/x轴陀螺检测滤波器故障检测步骤如下:
步骤211,计算k时刻横滚力矩模型/x轴陀螺检测滤波器的状态估计值及估计均方误差
kx0(k)=kx0(k-1)
式中,kx0(k)为k时刻横滚力矩模型零偏系数;kx0(k-1)为k-1时刻横滚力矩模型零偏系数;kx1、kx2为横滚力矩模型系数,为常值;ωmx(k)为k时刻通过横滚力矩模型计算得到的机体系相对于导航系的角速度在机体系x轴上的分量;ωmx(k-1)为k-1时刻通过横滚力矩模型计算得到的机体系相对于导航系的角速度在机体系x轴上的分量;为k时刻机体系相对于导航系的加速度在机体系y轴上的分量,通过加计输出获得;ωi(k)为k时刻第i个旋翼的转速,i=1,2,3,4;ΔT为离散采样周期;上标T表示转置;为k时刻的估计均方误差;为k-1时刻的估计均方误差;为雅可比矩阵;Gd1(k-1)=[I2×2],为***噪声系数,I2×2为2×2的单位矩阵;Wd1(k-1)=[wkx0(k-1) wmx(k-1)]T,为状态噪声,wkx0(k-1)为k-1时刻横滚力矩模型零偏白噪声;wmx(k-1)为k-1时刻横滚力矩模型的白噪声;
步骤212,计算k时刻横滚力矩模型/x轴陀螺检测滤波器的故障统计参数
式中,为k时刻横滚力矩模型/x轴陀螺检测滤波器的统计参数;为k时刻的残差; 为k时刻机体系相对于导航系的角速度在机体系x轴上的分量,通过x轴陀螺输出获得; 为k时刻的残差方差;Hd1(k)=[0 1];Rd1(k)=diag([wgx(k)]2),wgx为机体系x轴陀螺的白噪声;
步骤213,根据故障统计参数结果,计算k时刻横滚力矩模型/x轴陀螺检测滤波器的检测函数,判断横滚力矩模型和x轴陀螺是否发生故障
式中,T1是阈值,当J1(k)=1时,横滚力矩模型或x轴陀螺故障;当J1(k)=0时,横滚力矩模型和x轴陀螺均无故障;
步骤22,俯仰力矩模型/y轴陀螺检测滤波器故障检测步骤如下:
步骤221,计算k时刻俯仰力矩模型/y轴陀螺检测滤波器的状态估计值及估计均方误差
ky0(k)=ky0(k-1)
式中,ky0(k)为k时刻俯仰力矩模型零偏系数;ky0(k-1)为k-1时刻俯仰力矩模型零偏系数;ky1、ky2为俯仰力矩模型系数;ωmy(k)为k时刻通过俯仰力矩模型计算得到的机体系相对于导航系的角速度在机体系y轴上的分量;ωmy(k-1)为k-1时刻通过俯仰力矩模型计算得到的机体系相对于导航系的角速度在机体系y轴上的分量;为雅可比矩阵;Gd2(k-1)=[I2×2],为***噪声系数;Wd2(k-1)=[wky0(k-1) wmy(k-1)]T,为状态噪声,wky0(k-1)为k-1时刻俯仰力矩模型零偏白噪声,wmy(k-1)为k-1时刻俯仰力矩模型的白噪声;
步骤222,计算k时刻俯仰力矩模型/y轴陀螺检测滤波器的故障统计参数
式中,为k时刻俯仰力矩模型/y轴陀螺检测器的统计参数;为k时刻的残差; 为k时刻机体系相对于导航系的角速度在机体系y轴上的分量,通过陀螺输出获得; 为k时刻的残差方差;Hd2(k)=[0 1];Rd2(k)=diag([wgy(k)]2),wgy为机体系y轴陀螺的白噪声;
步骤223,根据故障统计参数结果,计算k时刻俯仰力矩模型/y轴陀螺检测滤波器的检测函数,判断俯仰力矩模型和y轴陀螺是否发生故障
式中,T2是阈值,当J2(k)=1时,俯仰力矩模型或y轴陀螺故障;当J2(k)=0时,俯仰力矩模型和y轴陀螺均无故障;
步骤23,横滚力矩模型/加速度计/GPS检测滤波器故障检测步骤如下:
步骤231,计算k时刻横滚力矩模型/加速度计/GPS检测滤波器的状态估计值及估计均方误差
kx0(k)=kx0(k-1)
式中,q0(k)、q1(k)、q2(k)、q3(k)为k时刻四元素;q0(k-1)、q1(k-1)、q2(k-1)、q3(k-1)为k-1时刻四元素;为机体系相对于导航系的角速度在机体系z轴上的分量,通过z轴陀螺输出获得;为雅可比矩阵; 为k-1时刻***噪声系数;I2×2为2×2的单位矩阵;02×3为2×3的零矩阵;04×2为4×2的零矩阵;Wd3(k-1)=[wkx0(k-1) wmx(k-1) wωx(k-1) wωy(k-1) wgz(k-1)]T,为k-1时刻状态噪声,wωx(k-1)为k-1时刻机体系x轴角速度白噪声;wgz(k-1)为k-1时刻机体系z轴陀螺白噪声;
步骤232,计算k时刻横滚力矩模型/加速度计/GPS检测滤波器的故障统计参数
式中,为k时刻横滚力矩模型/加速度计/GPS检测滤波器的统计参数;为k时刻的残差; VDG(k)分别为导航系下北向、东向、地向速度,通过GPS获得, 为k-1时刻***滤波四元素估计;分别为k时刻机体系相对于导航系的线速度在机体系y轴的分量;g为重力加速度;为k时刻的残差方差,Hd3(k)=[01×2 Ω1×4 d3],Ω1×4 d3=[2gq1(k) 2gq0(k) 2gq3(k) 2gq2(k)], 为加速度计和GPS的组合白噪声在机体系y轴上的分量;
步骤233,根据故障统计参数结果,计算k时刻横滚力矩模型/加速度计/GPS检测滤波器的检测函数,判断横滚力矩模型是否发生故障
式中,T3是阈值,当J3(k)=1时,横滚力矩模型故障;当J3(k)=0时,横滚力矩模型无故障;
步骤24,俯仰力矩模型/加速度计/GPS检测滤波器故障检测步骤如下:
步骤241,计算k时刻俯仰力矩模型/加速度计/GPS检测滤波器的状态估计值及估计均方误差
ky0(k)=ky0(k-1)
式中,为雅可比矩阵;Gd4(k-1)=Gd3(k-1),为***噪声系数矩阵;Wd4(k-1)=Wd3(k-1),为状态噪声;
步骤242,计算k时刻俯仰力矩模型/加速度计/GPS检测滤波器的故障统计参数
式中,为k时刻俯仰力矩模型/加速度计/GPS检测滤波器的统计参数;为k时刻的残差;
为k时刻的残差方差;Hd4(k)=[01×2 Ω1×4 d4];Ω1×4 d4=[-2gq2(k) 2gq3(k) -2gq0(k) 2gq1(k)];为加速度计和GPS的组合白噪声在机体系x轴上的分量;
步骤243,根据故障统计参数结果,计算k时刻检测滤波器的检测函数,判断俯仰力矩模型是否发生故障
式中,T4是阈值,当J4(k)=1时,俯仰力矩模型故障;当J4(k)=0时,俯仰力矩模型无故障;
步骤25,x轴陀螺/加速度计/GPS检测滤波器检测步骤如下:
步骤251,计算k时刻x轴陀螺/加速度计/GPS检测滤波器的状态估计值及估计均方误差
式中,为雅可比矩阵;为***噪声系数;Wd5(k-1)=[wgx(k-1) wgy(k-1) wgz(k-1)]T,为k-1时刻状态噪声;
步骤252,计算k时刻x轴陀螺/加速度计/GPS检测滤波器的故障统计参数
式中,为k时刻x轴陀螺/加速度计/GPS检测滤波器的统计参数,为k时刻的残差, 为k时刻的残差方差,Hd5(k)=Ω1×4 d5,Ω1×4 d5=Ω1×4 d3,
步骤253,根据故障统计参数结果,计算k时刻x轴陀螺/加速度计/GPS检测滤波器的检测函数,判断x轴陀螺是否发生故障
式中,T5是阈值,当J5(k)=1时,x轴陀螺故障;当J5(k)=0时,无故障;
步骤26,y轴陀螺/加速度计/GPS检测滤波器检测步骤如下:
步骤261,计算k时刻y轴陀螺/加速度计/GPS检测滤波器的状态估计值及估计均方误差
式中,Φd6(k)=Φd5(k),为雅可比矩阵;Gd6(k-1)=Gd5(k-1),为***噪声系数;Wd6(k-1)=Wd5(k-1),为状态噪声;
步骤262,计算k时刻y轴陀螺/加速度计/GPS检测滤波器的故障统计参数
式中,为k时刻y轴陀螺/加速度计/GPS检测滤波器的统计参数,为k时刻的残差, 为k时刻的残差方差,Hd6(k)=Ω1×4 d6,Ω1×4 d6=Ω1×4 d4;
步骤263,根据故障统计参数结果,计算k时刻y轴陀螺/加速度计/GPS检测滤波器的检测函数,判断y轴陀螺故障是否发生故障
式中,T6是阈值,当J6(k)=1时,y轴陀螺故障;当J6(k)=0时,无故障;
步骤27,根据各个检测滤波器的检测函数结果,计算k时刻故障定位函数及根据故障定位函数计算结果,进行故障定位,步骤如下:
步骤271,计算k时刻预发生故障定位函数,判断***是否进入预发生故障:
式中,“∨”表示逻辑运算中“或”运算符,“∧”表示逻辑运算中“与”运算符;当Fpre(k)=1时,进入预发生故障阶段,当Fpre(k)=0时,未进入预发生故障;
步骤272,计算k时刻x轴陀螺故障定位函数,判断x轴陀螺是否发生故障:
当FGX(k)=1时,x轴陀螺故障;当FGX(k)=0时,x轴陀螺无故障;
步骤273,计算k时刻y轴陀螺故障定位函数,判断y轴陀螺是否发生故障:
当FGY(k)=1时,y轴陀螺故障;当FGY(k)=0时,y轴陀螺无故障;
步骤274,计算横滚力矩模型故障定位函数,判断横滚力矩模型是否发生故障:
当FMX(k)=1时,横滚力矩模型故障;当FMX(k)=0时,横滚力矩模型无故障;
步骤275,计算俯仰力矩模型故障定位函数,判断俯仰力矩模型是否发生故障:
当FMY(k)=1时,俯仰力矩模型故障;当FMY(k)=0时,俯仰力矩模型无故障;
步骤276,计算无故障定位函数,判断***是否发生故障:
当Fno(k)=1时,无故障。
3.如权利要求1所述的基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法,其特征在于:所述步骤三中,根据故障定位结果,确定子滤波器的状态方程,进行子滤波器的状态递推,按照如下步骤执行四个子滤波器:
步骤31,计算k时刻四个子滤波器的状态预测及预测均方误差
情况311,无故障或x轴陀螺或y轴陀螺故障时,四个子滤波器均按照如下步骤进行状态预测及均方误差预测:
步骤3111,计算k时刻横滚力矩模型零偏及俯仰力矩模型零偏预测
kx0(k|k-1)=kx0(k-1)
ky0(k|k-1)=ky0(k-1)
式中,kx0(k|k-1)、ky0(k|k-1)分别为横滚力矩模型、俯仰力矩模型零偏项在k-1时刻到k时刻的状态一步预测;
步骤3112,计算k时刻x轴角速度预测及y轴角速度预测
式中,ωmx(k|k-1)、ωmy(k|k-1)分别为x、y轴角速度在k-1时刻到k时刻的状态一步预测;
步骤3113,计算k时刻四元素预测
式中,q0(k|k-1)、q1(k|k-1)、q2(k|k-1)、q3(k|k-1)为四元素在k-1时刻到k时刻的状态一步预测;
步骤3114,计算一步预测均方误差PC(k|k-1)
PC(k|k-1)=ΦC(k|k-1)PC(k-1)ΦC(k|k-1)T+GC(k-1)QC(k-1)GC(k-1)T
式中,PC(k|k-1)为k-1到k时刻的一步预测均方误差;PC(k-1)为k-1时刻估计均方误差;为雅可比矩阵,为非线性状态方程,为k-1时刻状态量,
WC(k-1)=[wkx0(k-1) wky0(k-1) wmx(k-1) wmy(k-1) wωx(k-1) wωy(k-1) wgz(k-1)]T为k-1时刻的状态噪声;QC(k-1)=diag(WC(k-1)2),
情况312,横滚力矩模型/x轴陀螺检测滤波器预发生故障或横滚力矩模型故障时,四个子滤波器状态预测均按照如下步骤进行状态预测及均方误差预测:
3个子滤波器的状态方程中关于x轴角速度的预测方程修改为ωmx(k|k-1)=ωmx(k-1),雅可比矩阵修改为: 其余同无故障情况;
情况313,俯仰力矩模型/y轴陀螺检测滤波器预发生故障或俯仰力矩模型故障时,四个子滤波器状态预测均按照如下步骤进行状态预测及均方误差预测:
3个子滤波器的状态方程中关于y轴角速度的预测方程修改为ωmy(k|k-1)=ωmy(k-1),雅可比矩阵修改为: 其余同无故障情况;
步骤32,力矩模型/x轴陀螺子滤波器量测更新步骤如下:
步骤321,计算k时刻的力矩模型/x轴陀螺子滤波器的滤波增益
KC1(k)=PC(k|k-1)HC1(k)T[HC1(k)PC(k|k-1)HC1(k)T+RC1(k)]-1
式中,HC1(k)=[01×2 1 01×5]T,01×2为1×2的零矩阵;01×5为1×5的零矩阵;KC1(k)为k时刻的滤波增益,RC1(k)=diag([wgx(k)]2),为k时刻的量测噪声;
步骤322,计算k时刻力矩模型/x轴陀螺子滤波器状态估计值
式中,为k时刻状态量的估计值,为k时刻的量测量,
步骤323,计算k时刻力矩模型/x轴陀螺子滤波器估计均方误差PC1(k)
PC1(k)=[I-KC1(k)HC1(k)]PC(k|k-1)
式中,PC1(k)为k时刻估计均方误差;I为单位矩阵;
步骤33,力矩模型/y轴陀螺子滤波器量测更新步骤如下:
步骤331,计算k时刻的力矩模型/y轴陀螺子滤波器的滤波增益
KC2(k)=PC(k|k-1)HC2(k)T[HC2(k)PC(k|k-1)HC2(k)T+RC2(k)]-1
式中,HC2(k)=[01×3 1 01×4]T,01×3为1×3的零矩阵,01×4为1×4的零矩阵;KC2(k)为k时刻的滤波增益,RC2(k)=diag([wgy(k)]2),为k时刻的量测噪声;
步骤332,计算k时刻力矩模型/y轴陀螺子滤波器状态估计值
式中,为k时刻状态量的估计值,为k时刻的量测量,
步骤333,计算k时刻力矩模型/y轴陀螺子滤波器估计均方误差PC2(k)
PC2(k)=[I-KC2(k)HC2(k)]PC(k|k-1)
式中,PC2(k)为k时刻估计均方误差;I为单位矩阵;
步骤34,力矩模型/加速度计/GPS子滤波器量测更新步骤如下:
步骤341,计算k时刻的力矩模型/加速度计/GPS子滤波器的滤波增益
KC3(k)=PC(k|k-1)HC3(k)T[HC3(k)PC(k|k-1)HC3(k)T+RC3(k)]-1
式中,01×4为1×4的零矩阵;g为重力加速度;KC3(k)为k时刻的滤波增益,为k时刻的量测噪声;
步骤342,计算k时刻力矩模型/加速度计/GPS子滤波器状态估计值
式中,为k时刻状态量的估计值,为k时刻的量测量,
步骤343,计算k时刻力矩模型/加速度计/GPS子滤波器估计均方误差PC3(k)PC3(k)=[I-KC3(k)HC3(k)]PC(k|k-1)
式中,PC3(k)为k时刻估计均方误差;I为单位矩阵;
步骤35,力矩模型/磁传感器子滤波器更新步骤如下:
步骤351,计算k时刻的力矩模型/磁传感器子滤波器的滤波增益
KC4(k)=PC(k|k-1)HC4(k)T[HC4(k)PC(k|k-1)HC4(k)T+RC4(k)]-1
式中,HC4(k)=[01×4 N1×4],01×4为1×4的零矩阵,KC4(k)为k时刻的滤波增益,为k时刻的量测噪声,为k时刻磁航向角白噪声;
步骤352,计算k时刻力矩模型/磁传感器子滤波器状态估计值
式中,为k时刻状态量的估计值;YC4(k)=ψm(k)为k时刻磁航向角;其中,q0=q0(k|k-1)、q1=q1(k|k-1)、q2=q2(k|k-1)、q3=q3(k|k-1);
步骤353,计算k时刻力矩模型/磁传感器子滤波器估计均方误差PC4(k)
PC4(k)=[I-KC4(k)HC4(k)]PC(k|k-1)
式中,PC4(k)为k时刻估计均方误差;I为单位矩阵。
4.如权利要求1所述的基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法,其特征在于:所述步骤四的具体内容是:根据故障检测结果,执行全局滤波器,对力矩模型/x轴陀螺子滤波器、力矩模型/y轴陀螺子滤波器、力矩模型/加速度计/GPS子滤波器、力矩模型/磁传感器子滤波器进行故障隔离,对无故障的子滤波器进行数据融合,得到x轴角速度、y轴角速度、横滚角、俯仰角信息:
情况41,无故障时,按照如下步骤进行子滤波器的故障隔离和全局滤波
步骤411,根据故障定位结果,隔离故障子滤波器
四个子滤波器均参与全局滤波,无子滤波器被隔离;
步骤412,计算k时刻的全局滤波器的估计均方误差
Pg(k)=[PC1(k)-1+PC2(k)-1+PC3(k)-1+PC4(k)-1]-1
步骤413,计算k时刻的全局滤波器的状态估计值:
情况42,横滚力矩模型/x轴陀螺检测滤波器预发生故障时,按照如下步骤进行子滤波器的故障隔离和全局滤波
步骤421,根据故障定位结果,隔离故障子滤波器
采用横滚力矩模型故障时的状态方程,隔离横滚力矩模型/x轴陀螺检测滤波器;
步骤422,计算k时刻的全局滤波器的估计均方误差
Pg(k)=[PC2(k)-1+PC3(k)-1+PC4(k)-1]-1
步骤423,计算k时刻的全局滤波器的状态估计值
情况43,俯仰力矩模型/y轴陀螺检测滤波器预发生故障时,按照如下步骤进行子滤波器的故障隔离和全局滤波
步骤431,根据故障定位结果,隔离故障子滤波器
采用俯仰力矩模型故障时的状态方程,隔离俯仰力矩模型/y轴陀螺检测滤波器;
步骤432,计算k时刻的全局滤波器的估计均方误差
Pg(k)=[PC1(k)-1+PC3(k)-1+PC4(k)-1]-1
步骤433,计算k时刻的全局滤波器的状态估计值
情况44,x轴陀螺故障时,按照如下步骤进行子滤波器的故障隔离和全局滤波
步骤441,根据故障定位结果,隔离故障子滤波器
横滚力矩模型/x轴陀螺子滤波器被隔离,其余三个子滤波器参与全局滤波;
步骤442,计算k时刻的全局滤波器的估计均方误差
Pg(k)=[PC2(k)-1+PC3(k)-1+PC4(k)-1]-1
步骤443,计算k时刻的全局滤波器的状态估计值
情况45,y轴陀螺故障时,按照如下步骤进行子滤波器的故障隔离和全局滤波:
步骤451,根据故障定位结果,隔离故障子滤波器
俯仰力矩模型/y轴陀螺子滤波器被隔离,其余三个子滤波器参与全局滤波;
步骤452,计算k时刻的全局滤波器的估计均方误差
Pg(k)=[PC1(k)-1+PC3(k)-1+PC4(k)-1]-1
步骤453,计算k时刻的全局滤波器的状态估计值
情况46,横滚力矩模型故障时,按照如下步骤进行子滤波器的故障隔离和全局滤波
步骤461,根据故障定位结果,隔离故障子滤波器
采用横滚力矩模型故障时的状态方程,无子滤波器被隔离,其余四个子滤波器参与全局滤波;
步骤462,计算k时刻的全局滤波器的估计均方误差
Pg(k)=[PC1(k)-1+PC2(k)-1+PC3(k)-1+PC4(k)-1]-1
步骤463,计算k时刻的全局滤波器的状态估计值
情况47,俯仰力矩模型故障时,按照如下步骤进行子滤波器的故障隔离和全局滤波
步骤471,根据故障定位结果,隔离故障子滤波器
采用俯仰力矩模型故障时的状态方程,无子滤波器被隔离,其余四个子滤波器参与全局滤波;
步骤472,计算k时刻的全局滤波器的估计均方误差
Pg(k)=[PC1(k)-1+PC2(k)-1+PC3(k)-1+PC4(k)-1]-1
步骤473,计算k时刻的全局滤波器的状态估计值
5.如权利要求1所述的基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法,其特征在于:所述步骤五包含如下内容,根据全局滤波器结果,对各个子滤波器、故障检测滤波器状态量、均方误差进行重置更新,并执行***重置策略:
步骤51,根据故障检测结果及全局滤波结果对各个子滤波器进行状态量及均方误差重置
情况511,无故障时,四个子滤波器状态量及均方误差重置
PC(k)=4Pg(k)
情况512,横滚力矩模型/x轴陀螺检测滤波器预发生故障时,四个子滤波器状态量及均方误差重置
PC(k)=3Pg(k)
情况513,俯仰力矩模型/y轴陀螺检测滤波器预发生故障时,四个子滤波器状态量及均方误差重置
PC(k)=3Pg(k)
情况514,x轴陀螺故障时,四个子滤波器状态量及均方误差重置
PC(k)=3Pg(k)
情况515,y轴陀螺故障时,四个子滤波器状态量及均方误差重置
PC(k)=3Pg(k)
情况516,横滚力矩模型故障时,四个子滤波器状态量及均方误差重置
PC(k)=4Pg(k)
情况517,俯仰力矩模型故障时,四个子滤波器状态量及均方误差重置
PC(k)=4Pg(k)
步骤52,根据故障检测结果及全局滤波结果对各个检测滤波器进行状态量及均方误差重置
检测滤波器为n步预测卡方检测器,设置n步重置周期,在n步之内不进行重置,使用自身的状态估计结果进行状态递推,在第n步时使用全局滤波结果进行状态量重置,状态及均方误差重置根据滤波结果采用如下方式:
情况521,无故障时,第n步时,检测滤波器状态及均方误差重置为如下:
式中,表示使用状态向量中的第1列中,第i到第j行元素进行重置;Pg(k)[i:j;i:j]表示使用误差估计矩阵中的第i到第j列、第i到第j行元素进行重置;
情况522,横滚力矩模型/x轴陀螺检测滤波器预发生故障时,检测滤波器状态及均方误差重置为如下:
横滚力矩模型/x轴陀螺检测滤波器、横滚力矩模型/加速度计/GPS检测滤波器、x轴陀螺/加速度计/GPS检测滤波器不进行重置更新,只进行累积直至确定故障位置;俯仰力矩模型/y轴陀螺检测滤波器、俯仰力矩模型/加速度计/GPS检测滤波器、y轴陀螺/加速度计/GPS检测滤波器采用n步重置规则,采用如下方法进行重置:
情况523,俯仰力矩模型/y轴陀螺检测滤波器预发生故障时,检测滤波器状态及均方误差重置为如下:
俯仰力矩模型/y轴陀螺检测滤波器、俯仰力矩模型/加速度计/GPS检测滤波器、y轴陀螺/加速度计/GPS检测滤波器不进行重置更新,只进行累积直至确定故障位置;横滚力矩模型/x轴陀螺检测滤波器、横滚力矩模型/加速度计/GPS检测滤波器、x轴陀螺/加速度计/GPS检测滤波器采用n步重置规则,采用如下方法进行重置:
情况524,x轴陀螺故障时,检测滤波器状态及均方误差重置为如下:
当检测到x轴陀螺故障时,对横滚力矩模型/x轴陀螺检测滤波器、x轴陀螺/加速度计/GPS检测滤波器进行立即重置,其他的检测器按照n步重置规则,采用如下方法进行重置:
情况525,y轴陀螺故障时,检测滤波器状态及均方误差重置为如下:
当检测到y轴陀螺故障时,对俯仰力矩模型/y轴陀螺检测滤波器、y轴陀螺/加速度计/GPS检测滤波器进行立即重置,其他的检测器按照n步重置规则,采用如下方法进行重置:
情况526,横滚力矩模型故障时,检测滤波器状态及均方误差重置
当检测到横滚力矩模型故障时,对横滚力矩模型/x轴陀螺检测滤波器、横滚力矩模型/加速度计/GPS检测滤波器进行立即重置,其他的检测器按照n步重置规则,采用如下方法进行重置:
情况527,俯仰力矩模型故障时,检测滤波器状态及均方误差重置
当检测到俯仰力矩模型故障时,对俯仰力矩模型/y轴陀螺检测滤波器、俯仰力矩模型/加速度计/GPS检测滤波器进行立即重置,其他的检测器按照n步重置规则,采用如下方法进行重置:
步骤53,当检测到故障后,基于故障特性,执行以下***重置策略
设置x陀螺、y轴、横滚力矩模型、俯仰力矩模型故障隔离周期,当检测到x陀螺、y轴、横滚力矩模型、俯仰力矩模型故障后,进入故障隔离周期,在隔离周期内认为一直存在故障,在隔离周期内若再次检测到故障,重新开始隔离周期。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810872417.5A CN108981709B (zh) | 2018-08-02 | 2018-08-02 | 基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810872417.5A CN108981709B (zh) | 2018-08-02 | 2018-08-02 | 基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108981709A true CN108981709A (zh) | 2018-12-11 |
CN108981709B CN108981709B (zh) | 2021-09-21 |
Family
ID=64555034
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810872417.5A Active CN108981709B (zh) | 2018-08-02 | 2018-08-02 | 基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108981709B (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109709576A (zh) * | 2018-12-20 | 2019-05-03 | 安徽优思天成智能科技有限公司 | 一种用于废气激光雷达的姿态估计方法 |
CN110426032A (zh) * | 2019-07-30 | 2019-11-08 | 南京航空航天大学 | 一种解析式冗余的飞行器容错导航估计方法 |
CN111352433A (zh) * | 2018-12-20 | 2020-06-30 | 中国科学院沈阳自动化研究所 | 一种无人机水平姿态角的故障诊断方法 |
WO2020220729A1 (zh) * | 2019-04-29 | 2020-11-05 | 南京航空航天大学 | 基于角加速度计/陀螺/加速度计的惯性导航解算方法 |
CN112947508A (zh) * | 2019-12-10 | 2021-06-11 | 广州极飞科技股份有限公司 | 故障原因确定方法及装置 |
Citations (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US4573351A (en) * | 1984-02-13 | 1986-03-04 | Litton Systems, Inc. | Hub moment sensor for a horizontal rotor aircraft |
US20080270027A1 (en) * | 2007-04-30 | 2008-10-30 | Stecko Stephen M | Fault detection and reconfiguration of an automated refueling boom |
CN102050226A (zh) * | 2009-10-30 | 2011-05-11 | 航天科工惯性技术有限公司 | 一种航空应急仪表及其***初始对准方法和组合导航算法 |
CN102809377A (zh) * | 2012-08-15 | 2012-12-05 | 南京航空航天大学 | 飞行器惯性/气动模型组合导航方法 |
CN102854874A (zh) * | 2012-06-18 | 2013-01-02 | 南京航空航天大学 | 一种基于联合多观测器的故障诊断与容错控制装置及方法 |
CN103149927A (zh) * | 2013-03-24 | 2013-06-12 | 西安费斯达自动化工程有限公司 | 飞行器大迎角运动四元数模型的故障诊断和容错控制方法 |
CN104007663A (zh) * | 2014-05-13 | 2014-08-27 | 南京航空航天大学 | 一种含参数不确定性的四旋翼姿态自适应容错控制方法 |
CN104238565A (zh) * | 2014-09-30 | 2014-12-24 | 清华大学 | 一种应用于容错飞行控制***的鲁棒控制分配方法 |
CN105045105A (zh) * | 2015-07-30 | 2015-11-11 | 南京航空航天大学 | 一种针对状态时滞的四旋翼直升机容错控制装置及方法 |
US20160102994A1 (en) * | 2014-10-08 | 2016-04-14 | Honeywell International Inc. | Systems and methods for attitude fault detection based on air data and aircraft control settings |
CN105758427A (zh) * | 2016-02-26 | 2016-07-13 | 南京航空航天大学 | 一种基于动力学模型辅助的卫星完好性监测方法 |
US20160290825A1 (en) * | 2014-10-20 | 2016-10-06 | Honeywell International Inc. | Systems and methods for attitude fault detection in one or more inertial measurement units |
CN106248082A (zh) * | 2016-09-13 | 2016-12-21 | 北京理工大学 | 一种飞行器自主导航***及导航方法 |
CN107101636A (zh) * | 2017-05-23 | 2017-08-29 | 南京航空航天大学 | 一种使用卡尔曼滤波器辨识多旋翼动力学模型参数的方法 |
CN107651214A (zh) * | 2017-10-10 | 2018-02-02 | 南京航空航天大学 | 多旋翼无人机整机试验装置及其试验方法 |
CN108168509A (zh) * | 2017-12-06 | 2018-06-15 | 南京航空航天大学 | 一种升力模型辅助的四旋翼飞行器高度容错估计方法 |
-
2018
- 2018-08-02 CN CN201810872417.5A patent/CN108981709B/zh active Active
Patent Citations (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US4573351A (en) * | 1984-02-13 | 1986-03-04 | Litton Systems, Inc. | Hub moment sensor for a horizontal rotor aircraft |
US20080270027A1 (en) * | 2007-04-30 | 2008-10-30 | Stecko Stephen M | Fault detection and reconfiguration of an automated refueling boom |
CN102050226A (zh) * | 2009-10-30 | 2011-05-11 | 航天科工惯性技术有限公司 | 一种航空应急仪表及其***初始对准方法和组合导航算法 |
CN102854874A (zh) * | 2012-06-18 | 2013-01-02 | 南京航空航天大学 | 一种基于联合多观测器的故障诊断与容错控制装置及方法 |
CN102809377A (zh) * | 2012-08-15 | 2012-12-05 | 南京航空航天大学 | 飞行器惯性/气动模型组合导航方法 |
CN103149927A (zh) * | 2013-03-24 | 2013-06-12 | 西安费斯达自动化工程有限公司 | 飞行器大迎角运动四元数模型的故障诊断和容错控制方法 |
CN104007663A (zh) * | 2014-05-13 | 2014-08-27 | 南京航空航天大学 | 一种含参数不确定性的四旋翼姿态自适应容错控制方法 |
CN104238565A (zh) * | 2014-09-30 | 2014-12-24 | 清华大学 | 一种应用于容错飞行控制***的鲁棒控制分配方法 |
US20160102994A1 (en) * | 2014-10-08 | 2016-04-14 | Honeywell International Inc. | Systems and methods for attitude fault detection based on air data and aircraft control settings |
US20160290825A1 (en) * | 2014-10-20 | 2016-10-06 | Honeywell International Inc. | Systems and methods for attitude fault detection in one or more inertial measurement units |
CN105045105A (zh) * | 2015-07-30 | 2015-11-11 | 南京航空航天大学 | 一种针对状态时滞的四旋翼直升机容错控制装置及方法 |
CN105758427A (zh) * | 2016-02-26 | 2016-07-13 | 南京航空航天大学 | 一种基于动力学模型辅助的卫星完好性监测方法 |
CN106248082A (zh) * | 2016-09-13 | 2016-12-21 | 北京理工大学 | 一种飞行器自主导航***及导航方法 |
CN107101636A (zh) * | 2017-05-23 | 2017-08-29 | 南京航空航天大学 | 一种使用卡尔曼滤波器辨识多旋翼动力学模型参数的方法 |
CN107651214A (zh) * | 2017-10-10 | 2018-02-02 | 南京航空航天大学 | 多旋翼无人机整机试验装置及其试验方法 |
CN108168509A (zh) * | 2017-12-06 | 2018-06-15 | 南京航空航天大学 | 一种升力模型辅助的四旋翼飞行器高度容错估计方法 |
Non-Patent Citations (5)
Title |
---|
BAN WANG等: "Adaptive Robust Tracking Control of Quadrotor Helicopter with Parametric Uncertainty and External Disturbance", 《2017 INTERNATIONAL CONFERENCE ON UNMANNED AIRCRAFT SYSTEMS (ICUAS)》 * |
JING CHANG等: "Adaptive Second-order Sliding Mode Observer for Quadrotor Attitude Estimation", 《2016 AMERICAN CONTROL CONFERENCE(ACC)》 * |
RONGZHI WANG等: "An aerodynamic model-aided state estimator for multi-rotor UAVs", 《2017 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS (IROS)》 * |
冯旭光: "四旋翼无人机自主控制***设计", 《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》 * |
王剑非: "卫星姿态控制***的故障诊断研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109709576A (zh) * | 2018-12-20 | 2019-05-03 | 安徽优思天成智能科技有限公司 | 一种用于废气激光雷达的姿态估计方法 |
CN111352433A (zh) * | 2018-12-20 | 2020-06-30 | 中国科学院沈阳自动化研究所 | 一种无人机水平姿态角的故障诊断方法 |
CN111352433B (zh) * | 2018-12-20 | 2021-04-06 | 中国科学院沈阳自动化研究所 | 一种无人机水平姿态角的故障诊断方法 |
CN109709576B (zh) * | 2018-12-20 | 2022-05-17 | 安徽优思天成智能科技有限公司 | 一种用于废气激光雷达的姿态估计方法 |
WO2020220729A1 (zh) * | 2019-04-29 | 2020-11-05 | 南京航空航天大学 | 基于角加速度计/陀螺/加速度计的惯性导航解算方法 |
CN110426032A (zh) * | 2019-07-30 | 2019-11-08 | 南京航空航天大学 | 一种解析式冗余的飞行器容错导航估计方法 |
CN110426032B (zh) * | 2019-07-30 | 2021-05-28 | 南京航空航天大学 | 一种解析式冗余的飞行器容错导航估计方法 |
CN112947508A (zh) * | 2019-12-10 | 2021-06-11 | 广州极飞科技股份有限公司 | 故障原因确定方法及装置 |
CN112947508B (zh) * | 2019-12-10 | 2023-12-05 | 广州极飞科技股份有限公司 | 故障原因确定方法及装置 |
Also Published As
Publication number | Publication date |
---|---|
CN108981709B (zh) | 2021-09-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108981709A (zh) | 基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法 | |
Bryne et al. | Nonlinear observers for integrated INS\/GNSS navigation: implementation aspects | |
CN108981708B (zh) | 四旋翼扭矩模型/航向陀螺/磁传感器容错组合导航方法 | |
RU2236697C2 (ru) | Резервная система для индикации курса и пространственного положения на самолете | |
CN103837151B (zh) | 一种四旋翼飞行器的气动模型辅助导航方法 | |
CN110426032B (zh) | 一种解析式冗余的飞行器容错导航估计方法 | |
Rhudy et al. | Aircraft model-independent airspeed estimation without pitot tube measurements | |
CN108168509B (zh) | 一种升力模型辅助的四旋翼飞行器高度容错估计方法 | |
WO2006099436A1 (en) | Low cost flight instrumentation system | |
CN101858748A (zh) | 高空长航无人机的多传感器容错自主导航方法 | |
Leutenegger et al. | Robust state estimation for small unmanned airplanes | |
CN107643088A (zh) | 无人机导航方法、装置、无人机及存储介质 | |
US10488432B2 (en) | Systems and methods for compensating for the absence of a sensor measurement in a heading reference system | |
CN110849360B (zh) | 面向多机协同编队飞行的分布式相对导航方法 | |
CN112525188B (zh) | 一种基于联邦滤波的组合导航方法 | |
CN109612459A (zh) | 基于动力学模型的四旋翼飞行器惯性传感器容错导航方法 | |
CN108693372B (zh) | 一种四旋翼飞行器的航向轴角速度估计方法 | |
Al-Jlailaty et al. | Efficient attitude estimators: A tutorial and survey | |
Youn et al. | Model-aided synthetic airspeed estimation of UAVs for analytical redundancy | |
Rohac et al. | Validation of nonlinear integrated navigation solutions | |
Barczyk et al. | An experimental validation of magnetometer integration into a GPS-aided helicopter UAV navigation system | |
RU2594631C1 (ru) | Способ определения углов пространственной ориентации летательного аппарата и устройство для его осуществления | |
Hazbon Alvarez et al. | Digital twin concept for aircraft sensor failure | |
Martin et al. | A general symmetry-preserving observer for aided attitude heading reference systems | |
McLaren | Velocity estimate following air data system failure |
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 |