专利汇可以提供基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法专利检索,专利查询,专利分析的服务。并且本 发明 公开一种基于 力 矩模型辅助的四旋翼 横滚 角 、 俯仰 角容错估计方法,步骤是:周期读取k时刻四旋翼 飞行器 机载 传感器 信息,执行故障检测 滤波器 及故障 定位 策略,判断x轴陀螺、y轴陀螺、横滚力矩模型、俯仰力矩模型的故障;根据故障定位结果,确定各个子滤波器的 状态方程 ,进行各子滤波器的数据融合;根据故障检测结果,执行全局滤波器,对各子滤波器进行故障隔离,对无故障的子滤波器进行数据融合,得到x轴 角速度 、y轴角速度、横滚角、俯仰角信息;根据全局滤波器结果,进行重置更新,并执行系统重置策略。此种方法通过四旋翼飞行器的动力学模型,形成x、y轴陀螺的冗余,实现x、y轴陀螺的故障检测及系统重置,在陀螺故障时仍能获得 姿态 角、航向角的准确估计。,下面是基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法专利的具体信息内容。
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×4d3],Ω1×4d3=[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×4d4]; Ω1×4d4=
[-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×4d5,Ω1×4d5=Ω1×4d3,
步骤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时刻的残差方差,
d6 d6 d4
Hd6(k)=Ω1×4 , Ω1×4 =Ω1×4 ;
步骤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)
T T
PC(k|k-1)=ΦC(k|k-1)PC(k-1)ΦC(k|k-1) +GC(k-1)QC(k-1)GC(k-1)
式中,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为
2
k-1时刻的状态噪声;QC(k-1)=diag(WC(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
式中, 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轴、横滚力矩模型、俯仰力矩模型故障后,进入故障隔离周期,在隔离周期内认为一直存在故障,在隔离周期内若再次检测到故障,重新开始隔离周期。
标题 | 发布/更新时间 | 阅读量 |
---|---|---|
一种四旋翼飞行器横滚轴角速度和俯仰轴角速度估计方法 | 2020-05-12 | 386 |
基于田间图像特征点匹配的农业装备横滚角获取方法 | 2020-05-13 | 968 |
高级辅助驾驶视觉感知传感器的横滚角标定方法及系统 | 2020-05-12 | 767 |
一种卫星天线横滚角自稳态调节装置 | 2020-05-11 | 289 |
一种基于MEMS加速度计的载体姿态横滚角获取方法 | 2020-05-12 | 426 |
用于旋转弹的横滚角的计算方法 | 2020-05-11 | 356 |
横滚极限检测系统 | 2020-05-11 | 545 |
横滚式葛根粉碎及分离加工一体机 | 2020-05-11 | 367 |
一种基于俯仰轴和横滚轴相对夹角的倾角仪正交性评估方法 | 2020-05-12 | 366 |
一种新型扑翼飞行器的横滚控制结构 | 2020-05-13 | 824 |
高效检索全球专利专利汇是专利免费检索,专利查询,专利分析-国家发明专利查询检索分析平台,是提供专利分析,专利查询,专利检索等数据服务功能的知识产权数据服务商。
我们的产品包含105个国家的1.26亿组数据,免费查、免费专利分析。
专利汇分析报告产品可以对行业情报数据进行梳理分析,涉及维度包括行业专利基本状况分析、地域分析、技术分析、发明人分析、申请人分析、专利权人分析、失效分析、核心专利分析、法律分析、研发重点分析、企业专利处境分析、技术处境分析、专利寿命分析、企业定位分析、引证分析等超过60个分析角度,系统通过AI智能系统对图表进行解读,只需1分钟,一键生成行业专利分析报告。