基于UKF的水下航行器IMU故障检测与诊断方法研究

(西北工业大学 自动化学院, 西安 710072)

0 引言

惯性测量单元(inertial measurement unit,IMU)作为水下航行器(autonomous underwater vehicle，AUV)控制系统的核心，其故障可能导致严重的后果，因此实现IMU故障的检测与诊断对于提高控制系统的容错性能具有重要意义[1]。目前，国内外主要是基于硬件余度技术来实现航行器传感器的容错性能。硬件余度技术虽然可以大幅度提升关键传感器的容错能力，但其具有成本较高及容易发生共性故障的缺陷[2]。因此，基于解析模型的方法一直是水下航行器传感器容错研究的热点。

卡尔曼滤波器(kalman filter, KF)广泛应用于最优估计领域中。经典的卡尔曼滤波器只适用于线性系统，并在一定条件下可以准确地估计状态[3]。扩展卡尔曼滤波器(extended kalman filter, EKF)要将非线性系统模型线性化，并且仅在特定范围的初始条件下收敛[4]，而无迹卡尔曼滤波器(unscented kalman filter, UKF)[5-6]适用于强非线性系统，它改善了状态估计的准确性和收敛性。基于UKF优良估计性能，本文根据水下航行器动力学方程建立惯导系统解析模型和滤波方程，将IMU传感器的输出作为卡尔曼滤波器系统方程的输入，量测传感器作为卡尔曼滤波器的输出，因此，将IMU传感器的故障称为输入故障。在出现输入故障时，滤波的结果将偏离真实状态，根据滤波解算特性进行IMU的故障诊断。

国内外一些学者已经在IMU传感器故障诊断领域做了一定的研究工作。文献[7]使用了增广无迹卡尔曼滤波器(augmented unscented kalmanFilter, AUKF)，AUKF将输入故障增广到状态向量中，并采用随机游走过程对故障进行建模，从而使其不仅能够估计常值故障，而且还能够估计随机故障。文献[8]介绍了最优二步无迹卡尔曼滤波器，该滤波器能够实现输入故障的诊断和信息重构。基于上述研究成果，本文提出一种基于UKF的水下航行器IMU故障诊断方法，以提高水下航行器惯性导航系统的容错能力，仿真结果表明了本文所提出算法的有效性。

1 滤波器模型的建立

利用卡尔曼滤波实现IMU的故障检测与诊断，关键在于建立IMU信号与各导航状态量之间的解析关系，而解析信号的精度很大程度取决于模型的准确性。文献[9]给出了水下航行器的动力学方程：

(1)

式中，m表示运载体的质量，(Fx,Fy,Fz)表示总流体动力R∑与发动机推力T之和(不包括重力G)在载体系内的投影，(u,v,w)表示地速在载体系内的投影，(φ,θ,ψ)分别表示滚转角、俯仰角和偏航角，(p,q,r)分别为运载体的角速度在载体系内的投影。

加速度计测量的是运载体单位质量上作用的非引力外力，称之为比力，记为(ax,ay,az)，则水下航行器的动力学方程可改写为：

(2)

水下航行器的运动学方程如下：

(3)

考虑到三轴加速度计和三轴陀螺仪的故障(fax,fay,faz,fp,fq,fr)T及测量噪声(ωax,ωay,ωaz,ωp,ωq,ωr)T，可得到滤波器的状态方程：

(4)

多普勒测速仪可提供水下航行器在载体系下的三轴速度[10]，罗经可提供水下航行器的姿态角信息，则可得到滤波器的量测方程：

(5)

2 无迹卡尔曼滤波器基本原理

系统方程和量测方程具有离散形式：

(6)

UKF的实现原理为：在原状态分布中按某一规则取一些Sigma点，使Sigma点的均值和协方差等于原状态分布的值，再将Sigma点通过非线性方程传播并作相应的加权处理，产生“后验”均值和协方差，即可实现状态估计。文献[11]证明了噪声高斯分布情况下，UKF算法状态估计的均值可达到泰勒级数三阶截断的精度，状态估计的协方差也可达到泰勒级数二阶截断的精度。UKF算法如下：

(7)

3 IMU故障检测

在设计IMU故障检测与诊断算法的过程中，需要引入新息的概念，新息的定义如下：

(8)

当IMU信号中未出现故障时，即滤波器运行在最优状况下时，新息的分布服从零均值高斯分布。

对于新息的每一维，分别记为r1,r2,···,rm，m表示新息的维数，当系统输入u=[axm,aym,azm,pm,qm,rm]T中不出现故障时，均服从一维正态分布N(0,σ2),σ2未知。而当u=[axm,aym,azm,pm,qm,rm]T中出现故障时，新息的均值便不再为零。基于此，应用统计学中的假设检验理论，可以实现对各维新息的均值是否为零的假设检验。文献[12]中介绍了t检验法，用来处理方差未知时，单个正态总体均值的检验。应用t检验法，可以准确地检测到每个量测更新时刻各维新息变量是否存在异常。基于此，本文提出了两种方法：阈值法和解耦矩阵法，用来实现IMU的故障检测。

3.1 阈值法

对于各维新息r1,r2,···,rm，分别选取不同的阈值σi,i=1,2,...,m，当新息满足

E[ri]>σi

(9)

时，则认为新息的均值不为零。当各维新息的阈值选取得相对较大时，便可以部分地消除各维新息和IMU各通道故障的耦合关系，即可以直接将各维新息的异常和对应IMU通道的故障对应起来，从而实现IMU传感器各通道的故障检测。阈值的选取不易确定，需要大量的仿真实验和经验判断，故阈值法的性能具有一定程度的局限性。

3.2 解耦矩阵法

根据滤波器的状态方程，可以得到一个6×6的矩阵，用来描述IMU传感器各通道的故障与各维新息变量的耦合关系。再结合t检验法对于各维新息均值是否为零的检验结果，就可以实现实时地检测IMU传感器各通道的故障。

解耦矩阵法的具体步骤如下：

步骤1：

对各维新息分别运用t检验法进行均值是否为零的假设检验，并分别记录假设检验的结果，记为m维的列向量Finno，m是新息向量的维数；

Finno=(0 … 1 … -1)T

(10)

式中，“1”元素表示在置信水平α下当前时刻对应维新息均值大于0，“0”元素表示在置信水平α下当前时刻对应维新息的均值等于0，“-1”元素表示在置信水平α下当前时刻对应维新息的均值小于0。

步骤2：

根据滤波器的状态方程，写出表示各通道输入故障与各个状态变量的耦合关系的矩阵，记为:

faxfayfazfpfqfr

(11)

矩阵C中，“1”元素表示对应列的故障信号对对应行的新息变量产生正影响，“0”元素表示对应列的故障信号对对应行的新息变量无影响，“-1”元素表示对应列的故障信号对对应行的新息变量产生负影响。

步骤3：

(12)

并将其保存在矩阵F′中。

步骤4：

确定窗口长度l，对矩阵F′中的每一行，在窗口内取均值，若均值的绝对值小于1，则记为0。将计算结果保存在矩阵F中。F中的每一列，就是各个量测更新时刻IMU传感器故障检测的结果了。

应用解耦矩阵法实现IMU故障检测的伪代码描述如下：

算法1:IMU故障检测

1)选取窗口长度n

2)计算k时刻各维新息变量的均值和

修正样本标准差

forj=(k-n+1):k

MeanofInno(i)=MeanofInno(i)+Inno(i,j)

endfor

forj=(k-n+1):k

VarofInno(i)=VarofInno(i)+(Inno(i,j)-MeanofInno(i))2

endfor

3)构造检验统计量T

4)确定置信水平α，查t分布上侧分位数表得到tα(n-1)的具体数值

5)进行假设检验,并将结果保存在矩阵flagofInno中

fori=1:m

ifT(i)≥tα(n-1)

flag(i)=1

endif

ifT(i)≤tα(n-1)

flag(i)=-1

endif

endfor

flagofInno(:,k)=flag

6)确定耦合矩阵C，并计算表示当前时刻kIMU故障检测情况的列向量，并将结果保存在矩阵Flag中

Flag(:,k)=C-1·flag

7)对矩阵Flag去野点处理，并将结果保存在矩阵Mat_Flag中

formi=1:m

formj=(k-n+1):k

Mat_Flag(mi,k)=Mat_Flag(mi,k)+Flag(mi,mj)

endfor

if |Mat_Flag(mi,k)|<1

Mat_Flag(mi,k)=0

endif

endfor

4 IMU故障诊断

IMU传感器的故障诊断，即输入故障fi的实时估计，根据下式实现：

(13)

式中，Gk是系统的噪声分配矩阵，Hk是量测矩阵，rk表示新息，E[·]表示均值。

一般的，带输入故障的非线性系统的连续型方程描述如下：

(14)

经过线性化、离散化之后，得到:

(15)

(16)

根据新息的定义，有:

(17)

两边同时取均值，即:

E[HkGkfi,k]=

(18)

当系统输入u中不存在未知故障fi时，显然:

(19)

则有:

E[rk]=HkGk·E[fi,k]

(20)

那么，对于输入故障fi,k，有:

(21)

根据式(21)，我们便可以实现实时地估计系统输入故障fi,k，即IMU传感器的故障。

5 仿真验证

UKF的初始化数据如式(22)所示，其中，q和R的特性由IMU和量测传感器的误差特性决定：

(22)

5.1 IMU故障检测仿真结果

仿真时，考虑了IMU传感器的各个通道，即在一段时间内，给IMU传感器的6个通道注入常值偏差故障E1、漂移故障E2、正弦振荡故障E3三种典型故障。

5.1.1 阈值法

对于6个新息变量ru、rv、rw、rφ、rθ和rψ，选取的阈值分别为：

1.35e-1,1.35e-1,2.5e-2,

1.4e-3,6e-3,5e-3

(23)

故障注入情况如表1所示。

表1 加速度计和陀螺典型故障注入情形一

其中，故障的数学表达式为：

(24)

式中，a,b,c均为常数。

IMU传感器各通道的故障的检测结果如图1、图2所示。

图1 阈值法三轴加计故障的检测结果

图2 阈值法三轴陀螺故障的检测结果

图2中，为了较明显地显示出三轴陀螺的故障注入情况，作图时将注入到三轴陀螺中的故障的幅值人为扩大了20倍。

图1、图2中，虚线表示注入到IMU传感器各通道的故障，实线的取值只有“0”和“1”，分别表示当前时刻未检测到故障和当前时刻检测到故障。从图1、图2中可以看出，针对IMU三种典型故障类型，阈值法能够实时检测出IMU传感器各通道的故障，且误检率为0。

5.1.2 解耦矩阵法

故障注入情况如表2所示。

表2 加速度计和陀螺典型故障注入情形二

IMU传感器故障的检测结果如图3、图4所示，图3中，向三轴加计fax,fay,faz在11-20 s同时注入了漂移故障、偏差故障和正弦振荡故障。图4中，为了较明显地显示出三轴陀螺的故障注入情况，作图时将注入到三轴陀螺中的故障的幅值人为扩大了20倍。

图3 解耦矩阵法三轴加计故障检测结果

图4 解耦矩阵法三轴陀螺故障检测结果

从图3、图4可以看到，对于IMU传感器的每个通道，解耦矩阵法都能够实时地准确地检测到故障的发生，故障检测率达到99%以上，且误检率为0，表明解耦矩阵法实现IMU故障检测的高性能。

5.2 IMU故障诊断仿真结果

IMU故障的实时诊断，通过第4部分式(13)来实现。对于第1部分描述的滤波器模型，由于量测方程为线性，即Hk=I6×6，故式(13)进一步简化为：

(25)

式中，Gk是系统的噪声分配矩阵，rk表示k时刻的新息向量，E[·]表示均值。

仿真时，时间更新过程中得到了Gk矩阵，再根据量测更新过程中求得的新息rk，代入式(25)，便可估计出当前时刻的IMU故障。

当IMU传感器故障注入情况如表2所示时，其各通道故障的实时估计结果如图5、图6所示。

图5 三轴加计故障的真实值和估计值

图6 三轴陀螺故障的真实值和估计值

图5、图6中，虚线表示IMU各通道实际注入的故障，实线代表故障的实时估计值。从图5、图6中可以看出，对于IMU传感器中常见的三种故障类型：偏差故障、漂移故

障和正弦振荡故障，本文提出的方法都能够实时地准确地跟踪上相应故障的动态特性，得到IMU传感器各通道故障的最优估计，即实现IMU故障诊断。

6 结论

UKF算法适用于强非线性对象滤波。IMU作为水下航行器导航系统的关键传感器，发生故障会严重影响UKF滤波性能，因此，实时检测和诊断IMU故障是必要的。本文首先基于水下航行器的动力学方程和运动学方程，将IMU故障视为动态偏差，以多普勒测速仪提供的载体系下的三轴速度和罗经提供的姿态角构成无迹卡尔曼滤波器的量测量，建立了IMU传感器故障存在条件下的滤波器模型，进一步基于UKF设计了IMU故障检测与诊断算法，实现了IMU的故障检测以及故障的无偏估计。通过本文的算法设计，可以得到以下结论：

1)阈值法和解耦矩阵法均可以实现IMU的故障检测；

2)可以实现IMU故障的实时诊断；

3)能够应对实际中IMU常见的三种典型故障类型。