基于UKF的水下航行器IMU故障檢測(cè)與診斷方法研究

(西北工業(yè)大學(xué) 自動(dòng)化學(xué)院, 西安 710072)

0 引言

慣性測(cè)量單元(inertial measurement unit,IMU)作為水下航行器(autonomous underwater vehicle，AUV)控制系統(tǒng)的核心，其故障可能導(dǎo)致嚴(yán)重的后果，因此實(shí)現(xiàn)IMU故障的檢測(cè)與診斷對(duì)于提高控制系統(tǒng)的容錯(cuò)性能具有重要意義[1]。目前，國(guó)內(nèi)外主要是基于硬件余度技術(shù)來(lái)實(shí)現(xiàn)航行器傳感器的容錯(cuò)性能。硬件余度技術(shù)雖然可以大幅度提升關(guān)鍵傳感器的容錯(cuò)能力，但其具有成本較高及容易發(fā)生共性故障的缺陷[2]。因此，基于解析模型的方法一直是水下航行器傳感器容錯(cuò)研究的熱點(diǎn)。

卡爾曼濾波器(kalman filter, KF)廣泛應(yīng)用于最優(yōu)估計(jì)領(lǐng)域中。經(jīng)典的卡爾曼濾波器只適用于線性系統(tǒng)，并在一定條件下可以準(zhǔn)確地估計(jì)狀態(tài)[3]。擴(kuò)展卡爾曼濾波器(extended kalman filter, EKF)要將非線性系統(tǒng)模型線性化，并且僅在特定范圍的初始條件下收斂[4]，而無(wú)跡卡爾曼濾波器(unscented kalman filter, UKF)[5-6]適用于強(qiáng)非線性系統(tǒng)，它改善了狀態(tài)估計(jì)的準(zhǔn)確性和收斂性?；赨KF優(yōu)良估計(jì)性能，本文根據(jù)水下航行器動(dòng)力學(xué)方程建立慣導(dǎo)系統(tǒng)解析模型和濾波方程，將IMU傳感器的輸出作為卡爾曼濾波器系統(tǒng)方程的輸入，量測(cè)傳感器作為卡爾曼濾波器的輸出，因此，將IMU傳感器的故障稱為輸入故障。在出現(xiàn)輸入故障時(shí)，濾波的結(jié)果將偏離真實(shí)狀態(tài)，根據(jù)濾波解算特性進(jìn)行IMU的故障診斷。

國(guó)內(nèi)外一些學(xué)者已經(jīng)在IMU傳感器故障診斷領(lǐng)域做了一定的研究工作。文獻(xiàn)[7]使用了增廣無(wú)跡卡爾曼濾波器(augmented unscented kalmanFilter, AUKF)，AUKF將輸入故障增廣到狀態(tài)向量中，并采用隨機(jī)游走過(guò)程對(duì)故障進(jìn)行建模，從而使其不僅能夠估計(jì)常值故障，而且還能夠估計(jì)隨機(jī)故障。文獻(xiàn)[8]介紹了最優(yōu)二步無(wú)跡卡爾曼濾波器，該濾波器能夠?qū)崿F(xiàn)輸入故障的診斷和信息重構(gòu)?；谏鲜鲅芯砍晒?，本文提出一種基于UKF的水下航行器IMU故障診斷方法，以提高水下航行器慣性導(dǎo)航系統(tǒng)的容錯(cuò)能力，仿真結(jié)果表明了本文所提出算法的有效性。

1 濾波器模型的建立

利用卡爾曼濾波實(shí)現(xiàn)IMU的故障檢測(cè)與診斷，關(guān)鍵在于建立IMU信號(hào)與各導(dǎo)航狀態(tài)量之間的解析關(guān)系，而解析信號(hào)的精度很大程度取決于模型的準(zhǔn)確性。文獻(xiàn)[9]給出了水下航行器的動(dòng)力學(xué)方程：

(1)

式中，m表示運(yùn)載體的質(zhì)量，(Fx,Fy,Fz)表示總流體動(dòng)力R∑與發(fā)動(dòng)機(jī)推力T之和(不包括重力G)在載體系內(nèi)的投影，(u,v,w)表示地速在載體系內(nèi)的投影，(φ,θ,ψ)分別表示滾轉(zhuǎn)角、俯仰角和偏航角，(p,q,r)分別為運(yùn)載體的角速度在載體系內(nèi)的投影。

加速度計(jì)測(cè)量的是運(yùn)載體單位質(zhì)量上作用的非引力外力，稱之為比力，記為(ax,ay,az)，則水下航行器的動(dòng)力學(xué)方程可改寫為：

(2)

水下航行器的運(yùn)動(dòng)學(xué)方程如下：

(3)

考慮到三軸加速度計(jì)和三軸陀螺儀的故障(fax,fay,faz,fp,fq,fr)T及測(cè)量噪聲(ωax,ωay,ωaz,ωp,ωq,ωr)T，可得到濾波器的狀態(tài)方程：

(4)

多普勒測(cè)速儀可提供水下航行器在載體系下的三軸速度[10]，羅經(jīng)可提供水下航行器的姿態(tài)角信息，則可得到濾波器的量測(cè)方程：

(5)

2 無(wú)跡卡爾曼濾波器基本原理

系統(tǒng)方程和量測(cè)方程具有離散形式：

(6)

UKF的實(shí)現(xiàn)原理為：在原狀態(tài)分布中按某一規(guī)則取一些Sigma點(diǎn)，使Sigma點(diǎn)的均值和協(xié)方差等于原狀態(tài)分布的值，再將Sigma點(diǎn)通過(guò)非線性方程傳播并作相應(yīng)的加權(quán)處理，產(chǎn)生“后驗(yàn)”均值和協(xié)方差，即可實(shí)現(xiàn)狀態(tài)估計(jì)。文獻(xiàn)[11]證明了噪聲高斯分布情況下，UKF算法狀態(tài)估計(jì)的均值可達(dá)到泰勒級(jí)數(shù)三階截?cái)嗟木?，狀態(tài)估計(jì)的協(xié)方差也可達(dá)到泰勒級(jí)數(shù)二階截?cái)嗟木?。UKF算法如下：

(7)

3 IMU故障檢測(cè)

在設(shè)計(jì)IMU故障檢測(cè)與診斷算法的過(guò)程中，需要引入新息的概念，新息的定義如下：

(8)

當(dāng)IMU信號(hào)中未出現(xiàn)故障時(shí)，即濾波器運(yùn)行在最優(yōu)狀況下時(shí)，新息的分布服從零均值高斯分布。

對(duì)于新息的每一維，分別記為r1,r2,···,rm，m表示新息的維數(shù)，當(dāng)系統(tǒng)輸入u=[axm,aym,azm,pm,qm,rm]T中不出現(xiàn)故障時(shí)，均服從一維正態(tài)分布N(0,σ2),σ2未知。而當(dāng)u=[axm,aym,azm,pm,qm,rm]T中出現(xiàn)故障時(shí)，新息的均值便不再為零。基于此，應(yīng)用統(tǒng)計(jì)學(xué)中的假設(shè)檢驗(yàn)理論，可以實(shí)現(xiàn)對(duì)各維新息的均值是否為零的假設(shè)檢驗(yàn)。文獻(xiàn)[12]中介紹了t檢驗(yàn)法，用來(lái)處理方差未知時(shí)，單個(gè)正態(tài)總體均值的檢驗(yàn)。應(yīng)用t檢驗(yàn)法，可以準(zhǔn)確地檢測(cè)到每個(gè)量測(cè)更新時(shí)刻各維新息變量是否存在異常?；诖?，本文提出了兩種方法：閾值法和解耦矩陣法，用來(lái)實(shí)現(xiàn)IMU的故障檢測(cè)。

3.1 閾值法

對(duì)于各維新息r1,r2,···,rm，分別選取不同的閾值σi,i=1,2,...,m，當(dāng)新息滿足

E[ri]>σi

(9)

時(shí)，則認(rèn)為新息的均值不為零。當(dāng)各維新息的閾值選取得相對(duì)較大時(shí)，便可以部分地消除各維新息和IMU各通道故障的耦合關(guān)系，即可以直接將各維新息的異常和對(duì)應(yīng)IMU通道的故障對(duì)應(yīng)起來(lái)，從而實(shí)現(xiàn)IMU傳感器各通道的故障檢測(cè)。閾值的選取不易確定，需要大量的仿真實(shí)驗(yàn)和經(jīng)驗(yàn)判斷，故閾值法的性能具有一定程度的局限性。

3.2 解耦矩陣法

根據(jù)濾波器的狀態(tài)方程，可以得到一個(gè)6×6的矩陣，用來(lái)描述IMU傳感器各通道的故障與各維新息變量的耦合關(guān)系。再結(jié)合t檢驗(yàn)法對(duì)于各維新息均值是否為零的檢驗(yàn)結(jié)果，就可以實(shí)現(xiàn)實(shí)時(shí)地檢測(cè)IMU傳感器各通道的故障。

解耦矩陣法的具體步驟如下：

步驟1：

對(duì)各維新息分別運(yùn)用t檢驗(yàn)法進(jìn)行均值是否為零的假設(shè)檢驗(yàn)，并分別記錄假設(shè)檢驗(yàn)的結(jié)果，記為m維的列向量Finno，m是新息向量的維數(shù)；

Finno=(0 … 1 … -1)T

(10)

式中，“1”元素表示在置信水平α下當(dāng)前時(shí)刻對(duì)應(yīng)維新息均值大于0，“0”元素表示在置信水平α下當(dāng)前時(shí)刻對(duì)應(yīng)維新息的均值等于0，“-1”元素表示在置信水平α下當(dāng)前時(shí)刻對(duì)應(yīng)維新息的均值小于0。

步驟2：

根據(jù)濾波器的狀態(tài)方程，寫出表示各通道輸入故障與各個(gè)狀態(tài)變量的耦合關(guān)系的矩陣，記為:

faxfayfazfpfqfr

(11)

矩陣C中，“1”元素表示對(duì)應(yīng)列的故障信號(hào)對(duì)對(duì)應(yīng)行的新息變量產(chǎn)生正影響，“0”元素表示對(duì)應(yīng)列的故障信號(hào)對(duì)對(duì)應(yīng)行的新息變量無(wú)影響，“-1”元素表示對(duì)應(yīng)列的故障信號(hào)對(duì)對(duì)應(yīng)行的新息變量產(chǎn)生負(fù)影響。

步驟3：

(12)

并將其保存在矩陣F′中。

步驟4：

確定窗口長(zhǎng)度l，對(duì)矩陣F′中的每一行，在窗口內(nèi)取均值，若均值的絕對(duì)值小于1，則記為0。將計(jì)算結(jié)果保存在矩陣F中。F中的每一列，就是各個(gè)量測(cè)更新時(shí)刻IMU傳感器故障檢測(cè)的結(jié)果了。

應(yīng)用解耦矩陣法實(shí)現(xiàn)IMU故障檢測(cè)的偽代碼描述如下：

算法1:IMU故障檢測(cè)

1)選取窗口長(zhǎng)度n

2)計(jì)算k時(shí)刻各維新息變量的均值和

修正樣本標(biāo)準(zhǔn)差

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)構(gòu)造檢驗(yàn)統(tǒng)計(jì)量T

4)確定置信水平α，查t分布上側(cè)分位數(shù)表得到tα(n-1)的具體數(shù)值

5)進(jìn)行假設(shè)檢驗(yàn),并將結(jié)果保存在矩陣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，并計(jì)算表示當(dāng)前時(shí)刻kIMU故障檢測(cè)情況的列向量，并將結(jié)果保存在矩陣Flag中

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

7)對(duì)矩陣Flag去野點(diǎn)處理，并將結(jié)果保存在矩陣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的實(shí)時(shí)估計(jì)，根據(jù)下式實(shí)現(xiàn)：

(13)

式中，Gk是系統(tǒng)的噪聲分配矩陣，Hk是量測(cè)矩陣，rk表示新息，E[·]表示均值。

一般的，帶輸入故障的非線性系統(tǒng)的連續(xù)型方程描述如下：

(14)

經(jīng)過(guò)線性化、離散化之后，得到:

(15)

(16)

根據(jù)新息的定義，有:

(17)

兩邊同時(shí)取均值，即:

E[HkGkfi,k]=

(18)

當(dāng)系統(tǒng)輸入u中不存在未知故障fi時(shí)，顯然:

(19)

則有:

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

(20)

那么，對(duì)于輸入故障fi,k，有:

(21)

根據(jù)式(21)，我們便可以實(shí)現(xiàn)實(shí)時(shí)地估計(jì)系統(tǒng)輸入故障fi,k，即IMU傳感器的故障。

5 仿真驗(yàn)證

UKF的初始化數(shù)據(jù)如式(22)所示，其中，q和R的特性由IMU和量測(cè)傳感器的誤差特性決定：

(22)

5.1 IMU故障檢測(cè)仿真結(jié)果

仿真時(shí)，考慮了IMU傳感器的各個(gè)通道，即在一段時(shí)間內(nèi)，給IMU傳感器的6個(gè)通道注入常值偏差故障E1、漂移故障E2、正弦振蕩故障E3三種典型故障。

5.1.1 閾值法

對(duì)于6個(gè)新息變量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 加速度計(jì)和陀螺典型故障注入情形一

其中，故障的數(shù)學(xué)表達(dá)式為：

(24)

式中，a,b,c均為常數(shù)。

IMU傳感器各通道的故障的檢測(cè)結(jié)果如圖1、圖2所示。

圖1 閾值法三軸加計(jì)故障的檢測(cè)結(jié)果

圖2 閾值法三軸陀螺故障的檢測(cè)結(jié)果

圖2中，為了較明顯地顯示出三軸陀螺的故障注入情況，作圖時(shí)將注入到三軸陀螺中的故障的幅值人為擴(kuò)大了20倍。

圖1、圖2中，虛線表示注入到IMU傳感器各通道的故障，實(shí)線的取值只有“0”和“1”，分別表示當(dāng)前時(shí)刻未檢測(cè)到故障和當(dāng)前時(shí)刻檢測(cè)到故障。從圖1、圖2中可以看出，針對(duì)IMU三種典型故障類型，閾值法能夠?qū)崟r(shí)檢測(cè)出IMU傳感器各通道的故障，且誤檢率為0。

5.1.2 解耦矩陣法

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

表2 加速度計(jì)和陀螺典型故障注入情形二

IMU傳感器故障的檢測(cè)結(jié)果如圖3、圖4所示，圖3中，向三軸加計(jì)fax,fay,faz在11-20 s同時(shí)注入了漂移故障、偏差故障和正弦振蕩故障。圖4中，為了較明顯地顯示出三軸陀螺的故障注入情況，作圖時(shí)將注入到三軸陀螺中的故障的幅值人為擴(kuò)大了20倍。

圖3 解耦矩陣法三軸加計(jì)故障檢測(cè)結(jié)果

圖4 解耦矩陣法三軸陀螺故障檢測(cè)結(jié)果

從圖3、圖4可以看到，對(duì)于IMU傳感器的每個(gè)通道，解耦矩陣法都能夠?qū)崟r(shí)地準(zhǔn)確地檢測(cè)到故障的發(fā)生，故障檢測(cè)率達(dá)到99%以上，且誤檢率為0，表明解耦矩陣法實(shí)現(xiàn)IMU故障檢測(cè)的高性能。

5.2 IMU故障診斷仿真結(jié)果

IMU故障的實(shí)時(shí)診斷，通過(guò)第4部分式(13)來(lái)實(shí)現(xiàn)。對(duì)于第1部分描述的濾波器模型，由于量測(cè)方程為線性，即Hk=I6×6，故式(13)進(jìn)一步簡(jiǎn)化為：

(25)

式中，Gk是系統(tǒng)的噪聲分配矩陣，rk表示k時(shí)刻的新息向量，E[·]表示均值。

仿真時(shí)，時(shí)間更新過(guò)程中得到了Gk矩陣，再根據(jù)量測(cè)更新過(guò)程中求得的新息rk，代入式(25)，便可估計(jì)出當(dāng)前時(shí)刻的IMU故障。

當(dāng)IMU傳感器故障注入情況如表2所示時(shí)，其各通道故障的實(shí)時(shí)估計(jì)結(jié)果如圖5、圖6所示。

圖5 三軸加計(jì)故障的真實(shí)值和估計(jì)值

圖6 三軸陀螺故障的真實(shí)值和估計(jì)值

圖5、圖6中，虛線表示IMU各通道實(shí)際注入的故障，實(shí)線代表故障的實(shí)時(shí)估計(jì)值。從圖5、圖6中可以看出，對(duì)于IMU傳感器中常見的三種故障類型：偏差故障、漂移故

障和正弦振蕩故障，本文提出的方法都能夠?qū)崟r(shí)地準(zhǔn)確地跟蹤上相應(yīng)故障的動(dòng)態(tài)特性，得到IMU傳感器各通道故障的最優(yōu)估計(jì)，即實(shí)現(xiàn)IMU故障診斷。

6 結(jié)論

UKF算法適用于強(qiáng)非線性對(duì)象濾波。IMU作為水下航行器導(dǎo)航系統(tǒng)的關(guān)鍵傳感器，發(fā)生故障會(huì)嚴(yán)重影響UKF濾波性能，因此，實(shí)時(shí)檢測(cè)和診斷IMU故障是必要的。本文首先基于水下航行器的動(dòng)力學(xué)方程和運(yùn)動(dòng)學(xué)方程，將IMU故障視為動(dòng)態(tài)偏差，以多普勒測(cè)速儀提供的載體系下的三軸速度和羅經(jīng)提供的姿態(tài)角構(gòu)成無(wú)跡卡爾曼濾波器的量測(cè)量，建立了IMU傳感器故障存在條件下的濾波器模型，進(jìn)一步基于UKF設(shè)計(jì)了IMU故障檢測(cè)與診斷算法，實(shí)現(xiàn)了IMU的故障檢測(cè)以及故障的無(wú)偏估計(jì)。通過(guò)本文的算法設(shè)計(jì)，可以得到以下結(jié)論：

1)閾值法和解耦矩陣法均可以實(shí)現(xiàn)IMU的故障檢測(cè)；

2)可以實(shí)現(xiàn)IMU故障的實(shí)時(shí)診斷；

3)能夠應(yīng)對(duì)實(shí)際中IMU常見的三種典型故障類型。