一种解析式冗余的飞行器容错导航估计方法与流程

本发明属于导航技术领域，特别涉及了一种飞行器容错导航估计方法。

背景技术：

机载导航传感器是飞行器进行导航解算的数据来源，解算出的导航信息是飞行器进行稳定飞行的基础。目前常用的机载导航传感器包括惯性传感器——imu和量测传感器——磁传感器、gps、气压计等。但在一些特殊的环境中，机载导航传感器性能下降甚至失效，例如声波干扰会破坏imu的性能，城市遮挡会导致gps信号丢失等。如果使用故障的传感器参与导航解算，会导致飞行器失控，甚至坠毁。

技术实现要素：

为了解决上述背景技术提到的技术问题，本发明提出了一种解析式冗余的飞行器容错导航估计方法，提高飞行器导航系统的容错性能。

为了实现上述技术目的，本发明的技术方案为：

一种解析式冗余的飞行器容错导航估计方法，包括如下步骤：

步骤一：周期读取k时刻载体的旋翼转速量测系统、角加速度计、imu、磁传感器、gps和气压计的输出，通过旋翼飞行器的动力学模型，估计旋翼飞行器的运动加速度信息和角加速度信息；

步骤二：利用步骤一中的机载传感器数据输出，构建角加速度计/动力学模型/imu故障检测滤波器s1和imu/磁传感器/gps/气压计故障检测滤波器s2；s1中包括三个并行的子检测器，分别为角加速度计/动力学模型子检测器d1、角加速度计/imu子检测器d2和动力学模型/imu子检测器d3；s2中包括三个并行的子检测器，分别为imu/磁传感器子检测器d4、imu/gps子检测器d5和imu/气压计子检测器d6；

步骤三：根据s1中各个子检测器的故障检测结果，构建故障定位策略，实现对角加速度计、动力学模型和imu的故障定位，输出定位结果；根据s1的故障定位结果与s2中各个子检测器的故障检测结果，构建全局故障定位策略，定位故障传感器；

步骤四：根据步骤三得到的故障定位结果，制定故障隔离策略，从状态滤波估计中隔离故障传感器，完成导航状态估计；

当机载传感器无故障或动力学模型故障时，使用角加速度计、imu的加速度计作为状态方程输入，imu中的陀螺、磁传感器、gps、气压计作为量测传感器；

当imu故障时，使用角加速度计和动力学模型的气动力模型作为状态方程输入，磁传感器、gps、气压计作为量测传感器；

当角加速度计故障时，使用动力学模型的气动力矩模型和imu中的加速度计作为状态方程输入，imu中的陀螺、磁传感器、gps、气压计作为量测传感器；

当gps或气压计或磁传感器故障时，使用角加速度计、imu的加速度计作为状态方程输入，隔离故障传感器，使之不参与全局融合滤波；

步骤五：根据步骤四得到的状态估计结果，对k时刻的s1和s2中的状态量进行校正。

进一步地，在步骤一中，通过下式估计旋翼飞行器的运动加速度信息和角加速度信息：

fmz＝kt0+kt1(ω1²+ω2²+ω3²+ω4²)

其中，fmx、fmy、fmz为通过动力学模型计算得到的机体系x、y、z轴的加速度；为通过动力学模型计算得到的机体系x、y、z轴的角加速度，ωmz为z轴的角速度；khx0、khx1、khx2、khx3、khy0、khy1、khy2、khy3、kt0、kt1、kx0、kx1、kx2、kx3、ky0、ky1、ky2、ky3、kz0、kz1、kz2、kz3为旋翼飞行器动力学模型参数，均为常值；为机体系相对于导航系的线速度在机体系x、y轴上的分量；βx、βy为机体系x、y轴的角加速度，通过角加速度计获得；ωi为第i个旋翼的转速，i＝1,2,3,4，为第i个旋翼转速的角加速度；fax、fay为机体系x、y轴的加速度，通过imu中的加速度计获得。

进一步地，在步骤二中，构建角加速度计/动力学模型子检测器d1的方法如下：

1、利用角加速度计输出和动力学模型输出，计算z轴角加速度残差：

上式中，r1(k)为角加速度计和动力学模型计算得到的z轴角加速度残差；βz(k)为k时刻机体系z轴角加速度，通过z轴角加速度计输出获得；为通过动力学模型计算得到的机体系z轴的角加速度；abs(*)表示取绝对值；

2、判断子检测器d1是否发生故障：

上式中，τ1为故障判断阈值；t1为故障检测结果；如果t1＝1，则角加速度计或动力学模型故障；如果t1＝0，则角加速度计和动力学模型无故障；

构建角加速度计/imu子检测器d2的方法如下：

1、利用角加速度计输出计算x、y、z轴角速度：

上式中，ωx(k)、ωy(k)、ωz(k)为k时刻的机体系x、y、z轴角速度，通过角加速度计的输出βx、βy、βz积分获得；ωx(k-1)、ωy(k-1)、ωz(k-1)为k-1时刻的机体系x、y、z轴角速度，通过k-1时刻联邦卡尔曼滤波器输出获得；δt为离散采样时间；

2、计算子检测器d2的统计参数：

r2(k)＝[ωgx(k)ωgy(k)ωgz(k)]^t-[ωx(k)ωy(k)ωz(k)]^t

a2(k)＝h2(k)p2(k|k-1)h2(k)+r2(k)

λ2＝r2(k)a2(k)^-1r2(k)

p2(k|k-1)＝f2(k)p2(k-1)f2^t(k)+g2(k-1)q2(k-1)g2^t(k-1)

上式中，r2(k)为k时刻imu和角加速度计输出计算得到的x、y、z轴角速度残差；ωgx(k)、ωgy(k)、ωgz(k)为机体系x、y、z轴角速度，通过陀螺输出获得；a2(k)为k时刻imu和角加速度计输出计算得到的残差方差；h2(k)＝[i3×3]为k时刻量测系数矩阵，i3×3为3×3的单位矩阵；p2(k|k-1)为k时刻一步预测均方误差矩阵；r2(k)＝diag([εωgx(k)εωgy(k)εωgz(k)]²)为k时刻量测噪声矩阵，εωgx(k)、εωgy(k)、εωgz(k)为机体系x、y、z轴陀螺噪声；λ2(k)为k时刻imu和角加速度计输出计算得到的统计参数；f2(k)＝[i3×3]为k时刻imu和角加速度计检测系统的状态系数矩阵；p2(k-1)为k-1时刻的均方误差矩阵；g2(k-1)＝[δt*i3×3]为k-1时刻imu和角加速度计检测系统的状态噪声系数矩阵；q2(k-1)＝diag([εβx(k-1)εβy(k-1)εβz(k-1)]²)为k-1时刻imu和角加速度计检测系统的状态噪声，εβx(k-1)、εβy(k-1)、εβz(k-1)为k-1时刻的x、y、z轴角加速度计噪声；上标t表示转置；

3、判断子检测器d2是否发生故障：

上式中，τ2为故障判断阈值；t2为故障检测结果；如果t2＝1，则角加速度计或imu故障；如果t3＝0，则角加速度计和imu无故障；

构建动力学模型/imu子检测器d3的方法如下：

1、利用动力学模型输出计算z轴角速度：

上式中，ωmz(k-1)为k-1时刻的角速度，通过k-1时刻联邦卡尔曼滤波器输出获得；ωmz(k)为k时刻的角速度；为k时刻的角加速度，通过动力学模型获得；δt为离散采样时间；

2、计算子检测器d3的残差和统计参数：

r3a(k)＝abs(faz(k)-fmz(k))

r3b(k)＝ωgz(k)-ωmz(k)

a3b(k)＝h3b(k)p3b(k|k-1)h3b(k)+r3b(k)

λ3b＝r3b(k)a3b(k)^-1r3b(k)

p3b(k|k-1)＝f3b(k)p3b(k-1)f3b^t(k)+g3b(k-1)q3b(k-1)g3b^t(k-1)

上式中，r3a(k)为k时刻imu和动力学模型计算得到的z轴加速度残差；faz(k)为k时刻机体系z轴的加速度，通过加速度计获得；r3b(k)为k时刻imu和动力学模型计算得到的z轴角速度残差；a3b(k)为k时刻imu和动力学模型计算得到的z轴角速度残差方差；h3b(k)＝1为k时刻量测系数矩阵；p3b(k|k-1)为k时刻一步预测均方误差矩阵；r3b(k)＝diag([εωgz(k)]²)为k时刻量测噪声矩阵；λ3b(k)为k时刻imu和动力学模型计算得到的z轴角速度统计参数；为k时刻z轴陀螺和动力学模型检测系统的状态系数矩阵；p3b(k-1)为k-1时刻的均方误差矩阵；g3b(k-1)＝1为k-1时刻z轴陀螺和动力学模型检测系统的状态噪声系数矩阵；q3b(k-1)＝diag([εωmz(k-1)]²)为k-1时刻z轴陀螺和动力学模型检测系统的状态噪声，εωmz(k-1)为k-1时刻通过动力学模型获得的机体系z轴角速度噪声；

3、判断子检测器d3是否发生故障：

上式中，τ3a、τ3b为故障判断阈值；t3a、t3b为故障检测结果；如果t3a(k)＝1或t3b(k)＝1，imu或动力学模型故障；如果t3a(k)＝0和t3b(k)＝0，imu和动力学模型无故障。

进一步地，在步骤二中，计算k时刻子检测器d4、d5、d6的状态预测：

q0(k)＝q0(k-1)-0.5ωgx(k)q1(k-1)-0.5ωgy(k)q2(k-1)-0.5ωgz(k)q3(k-1)

q1(k)＝0.5ωgx(k)q0(k-1)+q1(k-1)+0.5ωgz(k)q2(k-1)-0.5ωgy(k)q3(k-1)

q2(k)＝0.5ωgy(k)q0(k-1)-0.5ωgz(k)q1(k-1)+q2(k-1)+0.5ωgx(k)q3(k-1)

q3(k)＝0.5ωgz(k)q0(k-1)+0.5ωgy(k)q1(k-1)-0.5ωgx(k)q2(k-1)+q3(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时刻四元数，通过k-1时刻联邦卡尔曼滤波器输出获得；为通过状态方程获得的k时刻机体系相对于导航系的线速度在机体系z轴上的分量；为k-1时刻机体系相对于导航系的线速度在机体系z轴上的分量，通过k-1时刻联邦卡尔曼滤波器输出获得；g为重力加速度；pn(k)、pe(k)、pd(k)为通过状态方程获得的k时刻北向、东向、地向位置；pn(k-1)、pe(k-1)、pd(k-1)为k-1时刻北向、东向、地向位置，通过k-1时刻联邦卡尔曼滤波器输出获得；

构建imu/磁传感器子检测器d4：

1、计算子检测器d4的统计参数：

使用k时刻的状态预测计算航向：

a4(k)＝h4(k)p4(k|k-1)h4(k)+r4(k)

λ4(k)＝r4(k)a4(k)^-1r4(k)

p4(k|k-1)＝f4(k)p4(k-1)f4^t(k)+g4(k-1)q4(k-1)g4^t(k-1)

q4(k-1)＝diag([εωgx(k-1)εωgy(k-1)εωgz(k-1)εvbx(k-1)εvby(k-1)εvbz(k-1)εpn(k-1)εpe(k-1)εpd(k-1)]²)

上式中，为通过状态预测计算得到的k时刻航向角；r4(k)为k时刻d4检测器的航向角残差；ψm(k)为k时刻磁传感器输出的航向；a4(k)为k时刻d4检测器的残差方差；h4(k)为k时刻d4检测器的量测系数矩阵；p4(k|k-1)为k-1时刻到k时刻的一步预测均方误差；r4(k)＝diag([εψm(k)]²)为k时刻的噪声矩阵，εψm(k)为k时刻磁传感器的高度噪声；λ4(k)为k时刻的统计参数；0i×j为i×j的零矩阵；ii×j为i×j的单位矩阵；f4(k)为k时刻imu和磁传感器检测器的状态系数矩阵；p4(k-1)为k-1时刻的均方误差矩阵；g4(k-1)为k-1时刻imu和磁传感器检测器的状态噪声系数矩阵；q4(k-1)为k-1时刻imu和磁传感器检测器的状态噪声，εvbx(k-1)、εvby(k-1)、εvbz(k-1)为k时刻的x、y、z轴线速度噪声；εpn(k-1)、εpe(k-1)、εpd(k-1)为k-1时刻的北向、东向、地向位置噪声；

3、判断子检测器d4是否发生故障

上式中，τ4为故障判断阈值；t4为故障检测结果；如果t4＝1，imu或磁传感器故障；如果t4＝0，imu和磁传感器无故障；

构建imu/gps子检测器d5：

1、计算子检测器d5的统计参数：

使用k时刻的状态预测计算东向速度：

使用k时刻的状态预测计算北向速度：

a5(k)＝h5(k)p5(k|k-1)h5(k)+r5(k)

λ5(k)＝r5(k)a5(k)^-1r5(k)

h5(k)＝[υ2×4ω2×302×3]

上式中，为通过状态预测计算得到的k时刻东向速度预测、北向速度；r5(k)为k时刻imu和gps检测系统的北向速度、东向速度残差；vng(k)、veg(k)为k时刻gps输出的北向速度、东向速度；a5(k)为k时刻imu和gps检测系统的残差方差；h5(k)为k时刻imu和gps检测系统的量测系数矩阵；p5(k|k-1)＝p4(k|k-1)为k-1时刻到k时刻的一步预测均方误差；为k时刻的噪声矩阵，为k时刻的gps北向、东向速度噪声；λ5(k)为k时刻的统计参数；

3、判断检测器d5是否发生故障：

上式中，τ5为故障判断阈值；t5为故障检测结果；如果t5＝1，imu或gps故障；如果t5＝0，imu和gps无故障；

构建imu/气压计子检测器d6：

1、计算检测器d6的统计参数：

使用k时刻的状态预测计算高度：

a6(k)＝h6(k)p6(k|k-1)h6(k)+r6(k)

λ6(k)＝r6(k)a6(k)^-1r6(k)

上式中，为通过状态预测计算得到的k时刻高度；r6(k)为k时刻imu和气压计检测系统的高度残差；hb(k)为k时刻气压计输出的高度；a6(k)为k时刻imu和气压计检测系统的残差方差；h6(k)＝-1为k时刻imu和气压计检测系统的量测系数矩阵；p6(k|k-1)＝p4(k|k-1)为k-1时刻到k时刻的一步预测均方误差；r6(k)＝diag([εhb(k)]²)为k时刻的噪声矩阵，εhb(k)为k时刻的气压计的高度噪声；λ6(k)为k时刻的统计参数；

3、判断子检测器d6是否发生故障：

上式中，τ6为故障判断阈值；t6为故障检测结果；如果t6＝1，imu或气压计故障；如果t6＝0，imu和气压计无故障。

进一步地，在步骤三中，首先根据s1构建故障定位策略：

然后根据s1和s2构建全局的故障定位策略：

上式中，fgacc、fdm、fimu、fmag、fgps、fbaro分别为角加速度计、动力学模型、imu、磁传感器、gps、气压计的故障定位函数，故障定位函数等于“1”，表示对应的传感器发生故障，故障定位函数等于“0”，表示对应的传感器未发生故障；“||”表示逻辑运算符“或”；“&”表示逻辑运算符“与”；“-”表示逻辑运算符“非”。

进一步地，在步骤四中，将故障情况分为以下7种情况：

情况1：机载传感器无故障：

(1)计算k时刻的状态和均方误差预测值：

q1(k)＝0.5ωgx(k)q0(k-1)+q1(k-1)+0.5ωgz(k)q2(k-1)-0.5ωgy(k)q3(k-1)

q2(k)＝0.5ωgy(k)q0(k-1)-0.5ωgz(k)q1(k-1)+q2(k-1)+0.5ωgx(k)q3(k-1)

q3(k)＝0.5ωgz(k)q0(k-1)+0.5ωgy(k)q1(k-1)-0.5ωgx(k)q2(k-1)+q3(k-1)

p(k|k-1)＝f(k)p(k-1)f^t(k)+g(k)q(k)g^t(k)

q(k-1)＝diag([εβx(k-1)εβy(k-1)εβz(k-1)εωx(k-1)εωy(k-1)εωz(k-1)εvbx(k-1)εvby(k-1)εvbz(k-1)εpn(k-1)εpe(k-1)εpd(k-1)]²)

上式中，为k时刻机体系相对于导航系的角速度在机体系x、y、z轴上的分量；p(k|k-1)为k-1时刻到k时刻的一步预测均方误差；p(k-1)为k-1时刻的均方误差；f(k)为k时刻的状态系数矩阵；g(k-1)为k-1时刻的噪声系数矩阵；q(k-1)为k-1时刻的噪声系数矩阵；εωx(k-1)、εωy(k-1)、εωz(k-1)为k-1时刻的x、y、z轴角速度噪声；

(2)计算k时刻第i个子滤波器的扩展卡尔曼滤波器的增益kci：

kci(k)＝p(k|k-1)hci(k)^t[hci(k)p(k|k-1)hci(k)^t+rci(k)]^-1

上式中，为k时刻的gps地向速度噪声；为k时刻的gps地向速度噪声；为k时刻的gps东向位置、北向位置噪声；

(3)计算k时刻第i个子滤波器的扩展卡尔曼滤波器状态估计值xci(k)及均方误差pci(k)：

xci(k)＝x(k|k-1)+kci(k)[yci(k)-hci(xci(k|k-1))]

pci(k)＝[i-kci(k)hci(k)]p(k|k-1)

yci(k)为第i个子滤波器量测量：

上式中，vdg(k)、png(k)、peg(k)为k时刻gps地向速度、北向位置、东向位置；hbaro(k)为k时刻气压计高度；

(4)计算k时刻子滤波器的全局融合状态估计xg(k)及均方误差估计pg(k)：

xg(k)＝pg(k)[pc1(k)^-1xc1(k)+pc2(k)^-1xc2(k)+pc3(k)^-1xc3(k)+pc4(k)^-1xc4(k)]^-1

pg(k)＝[pc1(k)^-1+pc2(k)^-1+pc3(k)^-1+pc4(k)^-1]^-1

x(k)＝xg(k)

p(k)＝4pg(k)

上式中，x(k)、p(k)为k时刻各子滤波器的状态重置及均方误差重置；

情况2：imu故障时，imu被隔离，使用imu中加速度计作为输入的速度微分方程修改如下：

使用imu中的陀螺作为量测信息的子滤波器被切断，不参与融合滤波；根据传感器隔离情况在情况1的基础上修改状态估计相关矩阵；

情况3：动力学模型故障时，滤波更新过程与情况1相同；

情况4：角加速度计故障时，角加速度计被隔离，使用角加速度计作为输入的角速度微分方程修改如下：

量测更新过程与情况1相同；

情况5：磁传感器故障时，磁传感器被隔离，使用冗余的航向信息作为量测量；量测更新过程中，将情况1中的步骤(3)中的yc2(k)＝ψm(k)，修改为使用冗余航向传感器输出的数据；

情况6：gps故障时，gps被隔离，使用冗余的速度位置信息作为量测量；量测更新过程中，将情况1中步骤(3)中的yc3(k)＝[vng(k)veg(k)vdg(k)png(k)peg(k)]^t，修改为使用冗余速度量测传感器输出的数据；

情况7：气压计故障时，气压计被隔离，使用冗余的高度信息作为量测量；量测更新过程中，将情况1中步骤(3)中的yc4(k)＝hbaro(k)，修改为使用冗余高度量测传感器输出的数据。

采用上述技术方案带来的有益效果：

本发明通过引入动力学模型和角加速度计，可以实现对角加速度计、动力学模型、imu、量测传感器的故障检测，同时可以使用动力学模型完全代替故障imu进行导航解算，提高导航系统的容错性能。并且在imu故障时，可以使用角加速度计输出作为动力学模型输入，提高大姿态角变化下的动力学模型估计精度。

附图说明

图1为本发明的整体容错框图；

图2为本发明中故障诊断框图；

图3为本发明中滤波框图；

图4为本发明的流程图；

图5为本发明在imu故障时的故障检测结果图；

图6为本发明在imu故障时的x轴角速度估计结果图；

图7为本发明在imu故障时的y轴角速度估计结果图；

图8为本发明在imu故障时的z轴角速度估计结果图；

图9为本发明在imu故障时的横滚角估计结果图；

图10为本发明在imu故障时的俯仰角估计结果图；

图11为本发明在imu故障时的航向角估计结果图；

图12为本发明在imu故障时的x轴线速度估计结果图；

图13为本发明在imu故障时的y轴线速度估计结果图。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

本发明设计了一种解析式冗余的飞行器容错导航估计方法，如图1所示，包括如下步骤：

步骤一：周期读取k时刻载体的旋翼转速量测系统、角加速度计、imu、磁传感器、gps和气压计的输出，通过旋翼飞行器的动力学模型，估计旋翼飞行器的运动加速度信息和角加速度信息；

步骤二：利用步骤一中的机载传感器数据输出，构建角加速度计/动力学模型/imu故障检测滤波器s1和imu/磁传感器/gps/气压计故障检测滤波器s2；s1中包括三个并行的子检测器，分别为角加速度计/动力学模型子检测器d1、角加速度计/imu子检测器d2和动力学模型/imu子检测器d3；s2中包括三个并行的子检测器，分别为imu/磁传感器子检测器d4、imu/gps子检测器d5和imu/气压计子检测器d6；

步骤三：根据s1中各个子检测器的故障检测结果，构建故障定位策略，实现对角加速度计、动力学模型和imu的故障定位，输出定位结果；根据s1的故障定位结果与s2中各个子检测器的故障检测结果，构建全局故障定位策略，定位故障传感器；

步骤四：根据步骤三得到的故障定位结果，制定故障隔离策略，从状态滤波估计中隔离故障传感器，完成导航状态估计；

当机载传感器无故障或动力学模型故障时，使用角加速度计、imu的加速度计作为状态方程输入，imu中的陀螺、磁传感器、gps、气压计作为量测传感器；

当imu故障时，使用角加速度计和动力学模型的气动力模型作为状态方程输入，磁传感器、gps、气压计作为量测传感器；

当角加速度计故障时，使用动力学模型的气动力矩模型和imu中的加速度计作为状态方程输入，imu中的陀螺、磁传感器、gps、气压计作为量测传感器；

当gps或气压计或磁传感器故障时，使用角加速度计、imu的加速度计作为状态方程输入，隔离故障传感器，使之不参与全局融合滤波；

步骤五：根据步骤四得到的状态估计结果，对k时刻的s1和s2中的状态量进行校正。

在本实施例中，步骤一采用如下优选方案实现：

在步骤一中，通过下式估计旋翼飞行器的运动加速度信息和角加速度信息：

fmz＝kt0+kt1(ω1²+ω2²+ω3²+ω4²)

其中，fmx、fmy、fmz为通过动力学模型计算得到的机体系x、y、z轴的加速度；为通过动力学模型计算得到的机体系x、y、z轴的角加速度，ωmz为z轴的角速度；khx0、khx1、khx2、khx3、khy0、khy1、khy2、khy3、kt0、kt1、kx0、kx1、kx2、kx3、ky0、ky1、ky2、ky3、kz0、kz1、kz2、kz3为旋翼飞行器动力学模型参数，均为常值；为机体系相对于导航系的线速度在机体系x、y轴上的分量；βx、βy为机体系x、y轴的角加速度，通过角加速度计获得；ωi为第i个旋翼的转速，i＝1,2,3,4，为第i个旋翼转速的角加速度；fax、fay为机体系x、y轴的加速度，通过imu中的加速度计获得。

在本实施例中，步骤二采用如下优选方案实现：

在步骤二中，构建角加速度计/动力学模型子检测器d1的方法如下：

1、利用角加速度计输出和动力学模型输出，计算z轴角加速度残差：

上式中，r1(k)为角加速度计和动力学模型计算得到的z轴角加速度残差；βz(k)为k时刻机体系z轴角加速度，通过z轴角加速度计输出获得；为通过动力学模型计算得到的机体系z轴的角加速度；abs(*)表示取绝对值；

2、判断子检测器d1是否发生故障：

上式中，τ1为故障判断阈值；t1为故障检测结果；如果t1＝1，则角加速度计或动力学模型故障；如果t1＝0，则角加速度计和动力学模型无故障；

构建角加速度计/imu子检测器d2的方法如下：

1、利用角加速度计输出计算x、y、z轴角速度：

上式中，ωx(k)、ωy(k)、ωz(k)为k时刻的机体系x、y、z轴角速度，通过角加速度计的输出βx、βy、βz积分获得；ωx(k-1)、ωy(k-1)、ωz(k-1)为k-1时刻的机体系x、y、z轴角速度，通过k-1时刻联邦卡尔曼滤波器输出获得；δt为离散采样时间；

2、计算子检测器d2的统计参数：

r2(k)＝[ωgx(k)ωgy(k)ωgz(k)]^t-[ωx(k)ωy(k)ωz(k)]^t

a2(k)＝h2(k)p2(k|k-1)h2(k)+r2(k)

λ2＝r2(k)a2(k)^-1r2(k)

p2(k|k-1)＝f2(k)p2(k-1)f2^t(k)+g2(k-1)q2(k-1)g2^t(k-1)

上式中，r2(k)为k时刻imu和角加速度计输出计算得到的x、y、z轴角速度残差；ωgx(k)、ωgy(k)、ωgz(k)为机体系x、y、z轴角速度，通过陀螺输出获得；a2(k)为k时刻imu和角加速度计输出计算得到的残差方差；h2(k)＝[i3×3]为k时刻量测系数矩阵，i3×3为3×3的单位矩阵；p2(k|k-1)为k时刻一步预测均方误差矩阵；r2(k)＝diag([εωgx(k)εωgy(k)εωgz(k)]²)为k时刻量测噪声矩阵，εωgx(k)、εωgy(k)、εωgz(k)为机体系x、y、z轴陀螺噪声；λ2(k)为k时刻imu和角加速度计输出计算得到的统计参数；f2(k)＝[i3×3]为k时刻imu和角加速度计检测系统的状态系数矩阵；p2(k-1)为k-1时刻的均方误差矩阵；g2(k-1)＝[δt*i3×3]为k-1时刻imu和角加速度计检测系统的状态噪声系数矩阵；q2(k-1)＝diag([εβx(k-1)εβy(k-1)εβz(k-1)]²)为k-1时刻imu和角加速度计检测系统的状态噪声，εβx(k-1)、εβy(k-1)、εβz(k-1)为k-1时刻的x、y、z轴角加速度计噪声；上标t表示转置；

3、判断子检测器d2是否发生故障：

上式中，τ2为故障判断阈值；t2为故障检测结果；如果t2＝1，则角加速度计或imu故障；如果t3＝0，则角加速度计和imu无故障；

构建动力学模型/imu子检测器d3的方法如下：

1、利用动力学模型输出计算z轴角速度：

上式中，ωmz(k-1)为k-1时刻的角速度，通过k-1时刻联邦卡尔曼滤波器输出获得；ωmz(k)为k时刻的角速度；为k时刻的角加速度，通过动力学模型获得；δt为离散采样时间；

2、计算子检测器d3的残差和统计参数：

r3a(k)＝abs(faz(k)-fmz(k))

r3b(k)＝ωgz(k)-ωmz(k)

a3b(k)＝h3b(k)p3b(k|k-1)h3b(k)+r3b(k)

λ3b＝r3b(k)a3b(k)^-1r3b(k)

p3b(k|k-1)＝f3b(k)p3b(k-1)f3b^t(k)+g3b(k-1)q3b(k-1)g3b^t(k-1)

上式中，r3a(k)为k时刻imu和动力学模型计算得到的z轴加速度残差；faz(k)为k时刻机体系z轴的加速度，通过加速度计获得；r3b(k)为k时刻imu和动力学模型计算得到的z轴角速度残差；a3b(k)为k时刻imu和动力学模型计算得到的z轴角速度残差方差；h3b(k)＝1为k时刻量测系数矩阵；p3b(k|k-1)为k时刻一步预测均方误差矩阵；r3b(k)＝diag([εωgz(k)]²)为k时刻量测噪声矩阵；λ3b(k)为k时刻imu和动力学模型计算得到的z轴角速度统计参数；为k时刻z轴陀螺和动力学模型检测系统的状态系数矩阵；p3b(k-1)为k-1时刻的均方误差矩阵；g3b(k-1)＝1为k-1时刻z轴陀螺和动力学模型检测系统的状态噪声系数矩阵；q3b(k-1)＝diag([εωmz(k-1)]²)为k-1时刻z轴陀螺和动力学模型检测系统的状态噪声，εωmz(k-1)为k-1时刻通过动力学模型获得的机体系z轴角速度噪声；

3、判断子检测器d3是否发生故障：

上式中，τ3a、τ3b为故障判断阈值；t3a、t3b为故障检测结果；如果t3a(k)＝1或t3b(k)＝1，imu或动力学模型故障；如果t3a(k)＝0和t3b(k)＝0，imu和动力学模型无故障。

在步骤二中，计算k时刻子检测器d4、d5、d6的状态预测：

q0(k)＝q0(k-1)-0.5ωgx(k)q1(k-1)-0.5ωgy(k)q2(k-1)-0.5ωgz(k)q3(k-1)

q1(k)＝0.5ωgx(k)q0(k-1)+q1(k-1)+0.5ωgz(k)q2(k-1)-0.5ωgy(k)q3(k-1)

q2(k)＝0.5ωgy(k)q0(k-1)-0.5ωgz(k)q1(k-1)+q2(k-1)+0.5ωgx(k)q3(k-1)

q3(k)＝0.5ωgz(k)q0(k-1)+0.5ωgy(k)q1(k-1)-0.5ωgx(k)q2(k-1)+q3(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时刻四元数，通过k-1时刻联邦卡尔曼滤波器输出获得；为通过状态方程获得的k时刻机体系相对于导航系的线速度在机体系z轴上的分量；为k-1时刻机体系相对于导航系的线速度在机体系z轴上的分量，通过k-1时刻联邦卡尔曼滤波器输出获得；g为重力加速度；pn(k)、pe(k)、pd(k)为通过状态方程获得的k时刻北向、东向、地向位置；pn(k-1)、pe(k-1)、pd(k-1)为k-1时刻北向、东向、地向位置，通过k-1时刻联邦卡尔曼滤波器输出获得；

构建imu/磁传感器子检测器d4：

1、计算子检测器d4的统计参数：

使用k时刻的状态预测计算航向：

a4(k)＝h4(k)p4(k|k-1)h4(k)+r4(k)

λ4(k)＝r4(k)a4(k)^-1r4(k)

p4(k|k-1)＝f4(k)p4(k-1)f4^t(k)+g4(k-1)q4(k-1)g4^t(k-1)

q4(k-1)＝diag([εωgx(k-1)εωgy(k-1)εωgz(k-1)εvbx(k-1)εvby(k-1)εvbz(k-1)εpn(k-1)εpe(k-1)εpd(k-1)]²)

上式中，为通过状态预测计算得到的k时刻航向角；r4(k)为k时刻d4检测器的航向角残差；ψm(k)为k时刻磁传感器输出的航向；a4(k)为k时刻d4检测器的残差方差；h4(k)为k时刻d4检测器的量测系数矩阵；p4(k|k-1)为k-1时刻到k时刻的一步预测均方误差；r4(k)＝diag([εψm(k)]²)为k时刻的噪声矩阵，εψm(k)为k时刻磁传感器的高度噪声；λ4(k)为k时刻的统计参数；0i×j为i×j的零矩阵；ii×j为i×j的单位矩阵；f4(k)为k时刻imu和磁传感器检测器的状态系数矩阵；p4(k-1)为k-1时刻的均方误差矩阵；g4(k-1)为k-1时刻imu和磁传感器检测器的状态噪声系数矩阵；q4(k-1)为k-1时刻imu和磁传感器检测器的状态噪声，εvbx(k-1)、εvby(k-1)、εvbz(k-1)为k时刻的x、y、z轴线速度噪声；εpn(k-1)、εpe(k-1)、εpd(k-1)为k-1时刻的北向、东向、地向位置噪声；

4、判断子检测器d4是否发生故障

上式中，τ4为故障判断阈值；t4为故障检测结果；如果t4＝1，imu或磁传感器故障；如果t4＝0，imu和磁传感器无故障；

构建imu/gps子检测器d5：

1、计算子检测器d5的统计参数：

使用k时刻的状态预测计算东向速度：

使用k时刻的状态预测计算北向速度：

a5(k)＝h5(k)p5(k|k-1)h5(k)+r5(k)

λ5(k)＝r5(k)a5(k)^-1r5(k)

h5(k)＝[υ2×4ω2×302×3]

上式中，为通过状态预测计算得到的k时刻东向速度预测、北向速度；r5(k)为k时刻imu和gps检测系统的北向速度、东向速度残差；vng(k)、veg(k)为k时刻gps输出的北向速度、东向速度；a5(k)为k时刻imu和gps检测系统的残差方差；h5(k)为k时刻imu和gps检测系统的量测系数矩阵；p5(k|k-1)＝p4(k|k-1)为k-1时刻到k时刻的一步预测均方误差；为k时刻的噪声矩阵，为k时刻的gps北向、东向速度噪声；λ5(k)为k时刻的统计参数；

4、判断检测器d5是否发生故障：

上式中，τ5为故障判断阈值；t5为故障检测结果；如果t5＝1，imu或gps故障；如果t5＝0，imu和gps无故障；

构建imu/气压计子检测器d6：

1、计算检测器d6的统计参数：

使用k时刻的状态预测计算高度：

a6(k)＝h6(k)p6(k|k-1)h6(k)+r6(k)

λ6(k)＝r6(k)a6(k)^-1r6(k)

上式中，为通过状态预测计算得到的k时刻高度；r6(k)为k时刻imu和气压计检测系统的高度残差；hb(k)为k时刻气压计输出的高度；a6(k)为k时刻imu和气压计检测系统的残差方差；h6(k)＝-1为k时刻imu和气压计检测系统的量测系数矩阵；p6(k|k-1)＝p4(k|k-1)为k-1时刻到k时刻的一步预测均方误差；r6(k)＝diag([εhb(k)]²)为k时刻的噪声矩阵，εhb(k)为k时刻的气压计的高度噪声；λ6(k)为k时刻的统计参数；

4、判断子检测器d6是否发生故障：

上式中，τ6为故障判断阈值；t6为故障检测结果；如果t6＝1，imu或气压计故障；如果t6＝0，imu和气压计无故障。

在本实施例中，步骤三采用如下优选方案实现：

在步骤三中，首先根据s1构建故障定位策略：

然后根据s1和s2构建全局的故障定位策略：

上式中，fgacc、fdm、fimu、fmag、fgps、fbaro分别为角加速度计、动力学模型、imu、磁传感器、gps、气压计的故障定位函数，故障定位函数等于“1”，表示对应的传感器发生故障，故障定位函数等于“0”，表示对应的传感器未发生故障；“||”表示逻辑运算符“或”；“&”表示逻辑运算符“与”；“-”表示逻辑运算符“非”。

上述故障诊断过程如图2所示。

在本实施例中，步骤四采用如下优选方案实现：

在步骤四中，将故障情况分为以下7种情况：

情况1：机载传感器无故障：

(1)计算k时刻的状态和均方误差预测值：

q1(k)＝0.5ωgx(k)q0(k-1)+q1(k-1)+0.5ωgz(k)q2(k-1)-0.5ωgy(k)q3(k-1)

q2(k)＝0.5ωgy(k)q0(k-1)-0.5ωgz(k)q1(k-1)+q2(k-1)+0.5ωgx(k)q3(k-1)

q3(k)＝0.5ωgz(k)q0(k-1)+0.5ωgy(k)q1(k-1)-0.5ωgx(k)q2(k-1)+q3(k-1)

p(k|k-1)＝f(k)p(k-1)f^t(k)+g(k)q(k)g^t(k)

q(k-1)＝diag([εβx(k-1)εβy(k-1)εβz(k-1)εωx(k-1)εωy(k-1)εωz(k-1)εvbx(k-1)εvby(k-1)εvbz(k-1)εpn(k-1)εpe(k-1)εpd(k-1)]²)上式中，为k时刻机体系相对于导航系的角速度在机体系x、y、z轴上的分量；p(k|k-1)为k-1时刻到k时刻的一步预测均方误差；p(k-1)为k-1时刻的均方误差；f(k)为k时刻的状态系数矩阵；g(k-1)为k-1时刻的噪声系数矩阵；q(k-1)为k-1时刻的噪声系数矩阵；εωx(k-1)、εωy(k-1)、εωz(k-1)为k-1时刻的x、y、z轴角速度噪声；

(2)计算k时刻第i个子滤波器的扩展卡尔曼滤波器的增益kci：

kci(k)＝p(k|k-1)hci(k)^t[hci(k)p(k|k-1)hci(k)^t+rci(k)]^-1

上式中，为k时刻的gps地向速度噪声；为k时刻的gps地向速度噪声；为k时刻的gps东向位置、北向位置噪声；

(3)计算k时刻第i个子滤波器的扩展卡尔曼滤波器状态估计值xci(k)及均方误差pci(k)：

xci(k)＝x(k|k-1)+kci(k)[yci(k)-hci(xci(k|k-1))]

pci(k)＝[i-kci(k)hci(k)]p(k|k-1)

yci(k)为第i个子滤波器量测量：

上式中，vdg(k)、png(k)、peg(k)为k时刻gps地向速度、北向位置、东向位置；hbaro(k)为k时刻气压计高度；

(4)计算k时刻子滤波器的全局融合状态估计xg(k)及均方误差估计pg(k)：

xg(k)＝pg(k)[pc1(k)^-1xc1(k)+pc2(k)^-1xc2(k)+pc3(k)^-1xc3(k)+pc4(k)^-1xc4(k)]^-1

pg(k)＝[pc1(k)^-1+pc2(k)^-1+pc3(k)^-1+pc4(k)^-1]^-1

x(k)＝xg(k)

p(k)＝4pg(k)

上式中，x(k)、p(k)为k时刻各子滤波器的状态重置及均方误差重置；

情况2：imu故障时，imu被隔离，使用imu加速度计作为输入的速度微分方程修改如下：

使用imu中的陀螺作为量测信息的子滤波器被切断，不参与融合滤波；根据传感器隔离情况在情况1的基础上修改状态估计相关矩阵；

情况3：动力学模型故障时，滤波更新过程与情况1相同；

情况4：角加速度计故障时，角加速度计被隔离，使用角加速度计作为输入的角速度微分方程修改如下：

量测更新过程与情况1相同；

情况5：磁传感器故障时，磁传感器被隔离，使用冗余的航向信息作为量测量；量测更新过程根据量测方程在情况1的基础上进行修改，将情况1中的步骤(3)中的yc2(k)＝ψm(k)，修改为使用冗余航向传感器输出的数据；

情况6：gps故障时，gps被隔离，使用冗余的速度位置信息作为量测量；量测更新过程根据量测方程在情况1的基础上进行修改，将情况1中步骤(3)中的yc3(k)＝[vng(k)veg(k)vdg(k)png(k)peg(k)]^t，修改为使用冗余速度量测传感器输出的数据；

情况7：气压计故障时，气压计被隔离，使用冗余的高度信息作为量测量；量测更新过程根据量测方程在情况1的基础上进行修改，将情况1中步骤(3)中的yc4(k)＝hbaro(k)，修改为使用冗余高度量测传感器输出的数据。

上述滤波过程如图3所示。

在本实施例中，步骤五采用如下优选方案实现：

在步骤五中，使用步骤四中状态估计结果，对角加速度计/imu子检测器d2的角速度、动力学模型/imu子检测器d3的角速度以及imu/磁传感器/gps/气压计故障检测滤波器s2的四元数、线速度、高度进行校正更新。

上述整个过程如图4所示。

下文通过具体仿真实例来说明本发明的效果：

采用实际的飞行数据进行半物理仿真验证的方式，对使用本发明后的容错估计性能进行验证。对载体在的故障检测性能进行验证，采集飞行器在不同机动运动下的数据：悬停、横滚运动、沿着俯仰轴的侧飞运动、悬停、俯仰运动、沿着俯仰轴的侧飞运动、慢速航向轴的转动、快速航向轴的转动。由于所使用的无人机缺少角加速度计传感器，所以通过使用陀螺数据微分进行模拟角加速度计的输出，在不同的传感器数据中注入常值仿真故障进行故障检测验证，本实施例仅给出imu故障时的故障检测及状态估计结果，其余传感器故障结果类似。

图5为在imu数据中注入零偏故障时，采用本发明方法后的故障检测结果。由图中可以看出，检测系统能够及时准确的检测出imu发生故障。

图6为在imu数据中注入零偏故障时，采用本发明方法后的x轴角速度估计结果。由图中可以看出，角速度估计误差小于10度/秒。

图7为在imu数据中注入零偏故障时，采用本发明方法后的y轴角速度估计结果。由图中可以看出，角速度估计误差小于10度/秒。

图8为在imu数据中注入零偏故障时，采用本发明方法后的z轴角速度估计结果。由图中可以看出，角速度估计误差小于2度/秒。

图9为在imu数据中注入零偏故障时，采用本发明方法后的横滚角估计结果。由图中可以看出，横滚角估计误差小于5度。

图10为在imu数据中注入零偏故障时，采用本发明方法后的俯仰角估计结果。由图中可以看出，俯仰角估计误差小于5度。

图11为在imu数据中注入零偏故障时，采用本发明方法后的航向角估计结果。由图中可以看出，航向角估计误差小于1度。

图12为在imu数据中注入零偏故障时，采用本发明方法后的x轴线速度估计结果。由图中可以看出，x轴线速度估计误差小于0.2m/s。

图13为在imu数据中注入零偏故障时，采用本发明方法后的y轴线速度估计结果。由图中可以看出，y轴线速度估计误差小于0.2m/s。