本發(fā)明涉及一種基于故障檢測的矢量信息分配自適應(yīng)聯(lián)邦濾波方法，屬于組合導(dǎo)航技術(shù)領(lǐng)域。

背景技術(shù)：

隨著導(dǎo)航技術(shù)的發(fā)展，其應(yīng)用領(lǐng)域越來越廣泛，對導(dǎo)航系統(tǒng)的性能要求也越來越高。如何處理多種導(dǎo)航傳感器的輸出信息，獲得最優(yōu)的導(dǎo)航定位解算結(jié)果是其中的關(guān)鍵內(nèi)容之一。在導(dǎo)航系統(tǒng)多傳感器信息融合領(lǐng)域，Carlson提出的分散化聯(lián)邦濾波器由于設(shè)計的靈活性、計算量小、容錯性能好等優(yōu)點而受到國內(nèi)外導(dǎo)航界的關(guān)注。在聯(lián)邦濾波器的基本結(jié)構(gòu)中，無重置結(jié)構(gòu)由于各子濾波器獨立進行濾波，容錯性能好，但是由于沒有全局最優(yōu)估計的重置，導(dǎo)致局部估計精度不高；而有重置結(jié)構(gòu)中，其中的任何一個子系統(tǒng)發(fā)生故障，都會通過全局濾波的反饋重置而使得其他無故障的子系統(tǒng)局部濾波也受到影響，導(dǎo)致聯(lián)邦濾波器的整體性能下降。因此，如何提高融合重置模式下聯(lián)邦濾波器的魯棒性具有重要的研究意義。

針對傳統(tǒng)聯(lián)邦濾波融合重置模式中子系統(tǒng)故障存在相互污染的缺陷，為提高聯(lián)邦濾波的故障檢測靈敏度和魯棒性，國內(nèi)外學(xué)者提出了許多改進方法，包括針對子濾波器估計的次優(yōu)性對殘差卡方故障檢測算法靈敏度的影響進行故障檢測算法的改進、為故障子濾波器分配較大或較小的信息分配系數(shù)等。經(jīng)分析不難發(fā)現(xiàn)，現(xiàn)有的關(guān)于子系統(tǒng)故障條件下的信息分配系數(shù)選擇問題還沒有統(tǒng)一的結(jié)論，對于子系統(tǒng)故障條件下如何同時提高全局估計精度和降低故障污染的影響，還需要進行更深入的研究。

技術(shù)實現(xiàn)要素：

本發(fā)明所要解決的技術(shù)問題是：提供一種基于故障檢測的矢量信息分配自適應(yīng)聯(lián)邦濾波方法，在子系統(tǒng)故障條件下，既提高了全局估計精度，又降低了故障污染。

本發(fā)明為解決上述技術(shù)問題采用以下技術(shù)方案：

一種基于故障檢測的矢量信息分配自適應(yīng)聯(lián)邦濾波方法，包括如下步驟：

步驟1，在慣性導(dǎo)航系統(tǒng)中，選取18維系統(tǒng)誤差狀態(tài)量及東北天坐標系，建立慣性導(dǎo)航系統(tǒng)的誤差狀態(tài)方程；

步驟2，在東北天坐標系下，根據(jù)各個導(dǎo)航傳感器的數(shù)據(jù)輸出特性，建立各組合導(dǎo)航子系統(tǒng)的量測方程；

步驟3，根據(jù)慣性導(dǎo)航系統(tǒng)的誤差狀態(tài)方程和各組合導(dǎo)航子系統(tǒng)的量測方程，設(shè)計各個組合導(dǎo)航子系統(tǒng)的閉環(huán)卡爾曼濾波器；

步驟4，在各個組合導(dǎo)航子系統(tǒng)的閉環(huán)卡爾曼濾波器內(nèi)，加入雙狀態(tài)卡方檢測函數(shù)，獲得各導(dǎo)航傳感器的故障檢測信息；

步驟5，根據(jù)步驟3獲得的各個組合導(dǎo)航子系統(tǒng)的閉環(huán)卡爾曼濾波器計算狀態(tài)估計量，利用聯(lián)邦主濾波器對狀態(tài)估計量進行信息融合，得到全局最優(yōu)估計結(jié)果；

步驟6，根據(jù)步驟4獲得的各導(dǎo)航傳感器的故障檢測信息，以及故障檢測閾值設(shè)計各個子濾波器對應(yīng)的矢量信息分配參數(shù)，公式如下：

其中，B_i,k(j,j)表示矩陣B_i,k對角線的第j個分量，B_i,k表示第i個子濾波器k時刻的矢量信息分配參數(shù)，η_i,k表示第i個子濾波器k時刻系統(tǒng)誤差狀態(tài)量的故障檢測信息，表示η_i,k的第j個分量，表示第i個子濾波器的故障檢測閾值的第j個分量，i＝1,2,…,N，N為子濾波器的數(shù)目；

根據(jù)各個子濾波器對應(yīng)的矢量信息分配參數(shù)完成聯(lián)邦主濾波器的信息分配過程，公式如下：

其中，P_i,k|k表示第i個子濾波器k時刻的系統(tǒng)狀態(tài)估計誤差協(xié)方差陣，Ρ_g表示全局狀態(tài)估計量的協(xié)方差矩陣，Q_i,k表示第i個子濾波器k時刻的系統(tǒng)噪聲方差矩陣，Β_iQ為Β_i,k后9個對角線元素組成的矩陣，Q_g為主濾波器中設(shè)置的系統(tǒng)噪聲方差矩陣，表示第i個子濾波器k時刻的系統(tǒng)狀態(tài)估計值，表示狀態(tài)量的全局最優(yōu)估計結(jié)果。

作為本發(fā)明的一種優(yōu)選方案，該方法還包括步驟7：根據(jù)步驟6獲得的矢量信息分配參數(shù)，計算各個子系統(tǒng)的量測噪聲矩陣，實現(xiàn)量測誤差的自適應(yīng)調(diào)節(jié)。

作為本發(fā)明的一種優(yōu)選方案，步驟1所述慣性導(dǎo)航系統(tǒng)的誤差狀態(tài)方程如下：

X_k＝Φ_k|k-1X_k-1+G_k-1W_k-1

其中，X_k、X_k-1分別為k、k-1時刻系統(tǒng)誤差狀態(tài)量，Φ_k|k-1為慣性導(dǎo)航系統(tǒng)k-1到k時刻的狀態(tài)轉(zhuǎn)移矩陣，G_k-1為慣性導(dǎo)航系統(tǒng)k-1時刻的白噪聲誤差系數(shù)矩陣，W_k-1為k-1時刻白噪聲隨機誤差向量。

作為本發(fā)明的一種優(yōu)選方案，步驟2所述各組合導(dǎo)航子系統(tǒng)的量測方程如下：

Z_i,k＝H_i,kX_k+V_i,k

其中，Z_i,k為k時刻通過第i個組合導(dǎo)航子系統(tǒng)獲得的量測信息，H_i,k為第i個子濾波器k時刻的量測系數(shù)矩陣，X_k為k時刻系統(tǒng)誤差狀態(tài)量，V_i,k為第i個子系統(tǒng)k時刻的量測噪聲，i＝1,2,…,N，N為子系統(tǒng)或子濾波器的數(shù)目。

作為本發(fā)明的一種優(yōu)選方案，步驟3所述各個組合導(dǎo)航子系統(tǒng)的閉環(huán)卡爾曼濾波器如下：

其中，表示第i個子濾波器k時刻的一步預(yù)測估計值，P_i,k|k-1表示第i個子濾波器k時刻的一步預(yù)測估計誤差協(xié)方差陣，Φ_k|k-1為慣性導(dǎo)航系統(tǒng)k-1到k時刻的狀態(tài)轉(zhuǎn)移矩陣，P_i,k-1|k-1表示第i個子濾波器k-1時刻的系統(tǒng)狀態(tài)估計誤差協(xié)方差陣，G_k-1為慣性導(dǎo)航系統(tǒng)k-1時刻的白噪聲誤差系數(shù)矩陣，Q_i,k-1表示第i個子濾波器k-1時刻的系統(tǒng)噪聲方差矩陣，P_i,k|k表示第i個子濾波器k時刻的系統(tǒng)狀態(tài)估計誤差協(xié)方差陣，K_i,k表示第i個子濾波器k時刻的濾波增益矩陣，H_i,k為第i個子濾波器k時刻的量測系數(shù)矩陣，R_i,k表示第i個子濾波器k時刻的量測誤差方差矩陣，表示第i個子濾波器k時刻的系統(tǒng)狀態(tài)估計值，Z_i,k為k時刻通過第i個組合導(dǎo)航子系統(tǒng)獲得的量測信息，I為18×18的單位矩陣。

作為本發(fā)明的一種優(yōu)選方案，步驟4所述各導(dǎo)航傳感器的故障檢測信息，表示如下：

其中，η_i,k表示第i個子濾波器k時刻系統(tǒng)誤差狀態(tài)量的故障檢測信息，表示η_i,k的第j個分量，且服從χ²(1)分布，表示第i個子濾波器k時刻狀態(tài)差值的第j個分量，T_i,k(j,j)表示矩陣T_i,k對角線的第j個分量，T_i,k表示第i個子濾波器k時刻狀態(tài)差值協(xié)方差陣。

作為本發(fā)明的一種優(yōu)選方案，步驟5所述利用聯(lián)邦主濾波器對狀態(tài)估計量進行信息融合，得到全局最優(yōu)估計結(jié)果，表示如下：

其中，表示狀態(tài)估計量的全局最優(yōu)估計結(jié)果，Ρ_g表示全局狀態(tài)估計量的協(xié)方差矩陣，P_i,k|k表示第i個子濾波器k時刻的系統(tǒng)狀態(tài)估計誤差協(xié)方差陣，表示第i個子濾波器k時刻的系統(tǒng)狀態(tài)估計值，i＝1,2,…,N，N為子濾波器的數(shù)目。

本發(fā)明采用以上技術(shù)方案與現(xiàn)有技術(shù)相比，具有以下技術(shù)效果：

1、本發(fā)明根據(jù)子濾波器對每個狀態(tài)量的狀態(tài)卡方故障檢測值，設(shè)計了動態(tài)矢量信息分配系數(shù)，使得無故障子濾波器分配到較大的信息分配系數(shù)，有故障的子濾波器分配到較小的信息分配系數(shù)，減小子濾波器故障對聯(lián)邦濾波全局估計的影響，避免故障子濾波器在信息重置過程中對系統(tǒng)造成的污染。

2、本發(fā)明根據(jù)子濾波器的狀態(tài)卡方故障檢測值，計算出量測噪聲矩陣的修正系數(shù)，動態(tài)調(diào)節(jié)量測噪聲，使得當量測信息異常時，減小對應(yīng)量測信息的使用權(quán)重，提高子濾波器的估計性能。同時本發(fā)明中通過對量測噪聲矩陣的動態(tài)調(diào)整，減少了一般算法結(jié)構(gòu)中的故障隔離模塊，提高了聯(lián)邦濾波架構(gòu)的魯棒性。

附圖說明

圖1是本發(fā)明基于故障檢測的矢量信息分配自適應(yīng)聯(lián)邦濾波方法的整體架構(gòu)圖。

圖2是本發(fā)明中雙狀態(tài)卡方檢測算法的結(jié)構(gòu)原理圖。

具體實施方式

下面詳細描述本發(fā)明的實施方式，所述實施方式的示例在附圖中示出。下面通過參考附圖描述的實施方式是示例性的，僅用于解釋本發(fā)明，而不能解釋為對本發(fā)明的限制。

如圖1所示，本發(fā)明的原理是：通過建立慣性導(dǎo)航系統(tǒng)的誤差狀態(tài)方程及傳感器子系統(tǒng)的量測方程，構(gòu)建卡爾曼子濾波器，在濾波器中設(shè)計雙狀態(tài)卡方檢測函數(shù)，獲得狀態(tài)量的故障檢測值。再通過故障檢測值以及故障檢測閾值構(gòu)建聯(lián)邦濾波器的矢量信息分配系數(shù)，完成聯(lián)邦濾波器的信息分配，最終實現(xiàn)對組合導(dǎo)航誤差狀態(tài)量的最優(yōu)估計。由于矢量信息分配系數(shù)是由故障檢測值計算獲得，通過對量測噪聲的自適應(yīng)調(diào)節(jié)，能夠在子系統(tǒng)故障狀態(tài)下將故障信息進行“軟隔離”。

本發(fā)明的具體實施過程如下：

(1)建立慣性導(dǎo)航系統(tǒng)的狀態(tài)方程

在慣性導(dǎo)航系統(tǒng)中，選取18維系統(tǒng)誤差狀態(tài)量：

其中，φ_E,φ_N,φ_U分別表示慣性導(dǎo)航系統(tǒng)誤差狀態(tài)量中東向、北向和天向的平臺誤差角狀態(tài)量；δv_E,δv_N,δv_U分別表示東向、北向和天向的速度誤差狀態(tài)量；δL,δλ,δh分別表示緯度、經(jīng)度和高度的誤差狀態(tài)量；ε_bx,ε_by,ε_bz和ε_rx,ε_ry,ε_rz分別表示X軸、Y軸和Z軸方向的陀螺常值漂移誤差狀態(tài)量以及X軸、Y軸和Z軸方向的陀螺一階馬爾可夫漂移誤差狀態(tài)量；分別表示X軸、Y軸和Z軸方向的加速度計一階馬爾科夫狀態(tài)量。

導(dǎo)航坐標系選取為東北天坐標系，可以獲得慣性導(dǎo)航系統(tǒng)的誤差狀態(tài)方程為：

X_k＝Φ_k|k-1X_k-1+G_k-1W_k-1 (1)

其中，Φ_k|k-1為慣性導(dǎo)航系統(tǒng)誤差方程所對應(yīng)的狀態(tài)轉(zhuǎn)移矩陣，G_k-1為慣性導(dǎo)航系統(tǒng)誤差方程所對應(yīng)的白噪聲誤差系數(shù)矩陣，W_k-1為慣性導(dǎo)航系統(tǒng)誤差方程所對應(yīng)的白噪聲隨機誤差向量，X_k為k時刻系統(tǒng)誤差狀態(tài)量。

(2)建立組合導(dǎo)航子系統(tǒng)的量測方程

在東北天導(dǎo)航坐標系下，根據(jù)各個傳感器的數(shù)據(jù)輸出特性，建立線性化的量測方程，如式(2)所示：

Z_i,k＝H_i,kX_k+V_i,k (2)

其中，Z_i,k為k時刻通過第i個子系統(tǒng)獲得的量測信息，H_i,k為k時刻第i個子濾波器的量測系數(shù)矩陣，V_i,k為k時刻第i個子系統(tǒng)的量測噪聲，i＝1,2,…,N，N為子系統(tǒng)的數(shù)目。

(3)子系統(tǒng)閉環(huán)卡爾曼濾波器設(shè)計

根據(jù)(1)和(2)中建立的系統(tǒng)狀態(tài)方程和量測方程，設(shè)計對應(yīng)子系統(tǒng)的閉環(huán)卡爾曼濾波器，一個傳感器對應(yīng)一個組合導(dǎo)航子系統(tǒng)，一個組合導(dǎo)航子系統(tǒng)對應(yīng)一個子濾波器，如式(3)所示：

其中，表示第i個子濾波器k時刻的一步預(yù)測估計值，表示第i個子濾波器k時刻的系統(tǒng)狀態(tài)估計值，K_i,k表示第i個子濾波器k時刻的濾波增益矩陣，P_i,k|k-1表示第i個子濾波器k時刻的一步預(yù)測估計誤差協(xié)方差陣，P_i,k|k表示第i個子濾波器k時刻的系統(tǒng)狀態(tài)估計誤差協(xié)方差陣，P_i,k-1|k-1表示第i個子濾波器k-1時刻的系統(tǒng)狀態(tài)估計誤差協(xié)方差陣，Q_i,k-1表示第i個子濾波器k-1時刻的系統(tǒng)噪聲方差矩陣，R_i,k表示第i個子濾波器k時刻的量測誤差方差矩陣，該參數(shù)通過信息分配矢量計算獲得，在步驟(7)中詳細說明，I為18×18的單位矩陣。

(4)雙狀態(tài)卡方檢測

在第i個卡爾曼濾波子系統(tǒng)中，設(shè)計雙狀態(tài)殘差卡方檢測函數(shù)。定義狀態(tài)變量及狀態(tài)估計誤差協(xié)方差陣

定義狀態(tài)估計誤差和

則狀態(tài)的差值δe_i,k可表示為：

狀態(tài)的差值協(xié)方差陣為：

當有則：

所以，與狀態(tài)量X_k中每個分量對應(yīng)的故障檢測函數(shù)為：

其中，服從χ²(1)分布。

在式(4)的遞推過程中，由于系統(tǒng)噪聲及建模誤差等的影響，將越來越偏離真實值。因此，設(shè)計了雙狀態(tài)遞推的方法，其結(jié)構(gòu)原理如圖2所示，兩個狀態(tài)遞推器交替工作，并用卡爾曼濾波的系統(tǒng)噪聲協(xié)方差值P_i,k|k進行周期性重置。將整個時間段分為t₁∈[2nT^*,(2n+1)T^*)、t₂∈[(2n+1)T^*，2(n+1)T^*)、t₃∈{nT^*}，其中，n∈{0,1,2,…}，T^*為狀態(tài)遞推器的重置周期。將狀態(tài)遞推器設(shè)置為相同的初始值，在t∈t₁時，狀態(tài)遞推器A和B按各自流程進行遞推，狀態(tài)卡方檢測函數(shù)使用計算；t∈t₂時，狀態(tài)遞推器A和B按各自流程進行遞推，狀態(tài)卡方檢測函數(shù)使用計算；t∈t₃且n為奇數(shù)時，狀態(tài)遞推器A進行重置，即t∈t₃且n為偶數(shù)時，狀態(tài)遞推器B進行重置，即通過對狀態(tài)遞推器的交替性重置，能夠避免系統(tǒng)誤差及建模誤差等引起的發(fā)散，從而使得故障檢測函數(shù)值異常，提高故障檢測準確率。

(5)聯(lián)邦主濾波器信息融合過程

根據(jù)各個子濾波器計算獲得的狀態(tài)估計量，按式(10)進行融合，得到全局最優(yōu)估計。

是狀態(tài)量的全局最優(yōu)估計結(jié)果，Ρ_g是全局估計狀態(tài)量的協(xié)方差矩陣。

(6)矢量信息分配系數(shù)的設(shè)計及信息分配過程

根據(jù)(4)中計算獲得各個子系統(tǒng)狀態(tài)的故障檢測值結(jié)合子系統(tǒng)的誤警率參數(shù)按照卡方分布表選擇故障檢測閾值按式(11)設(shè)計各個子系統(tǒng)對應(yīng)狀態(tài)的矢量信息分配系數(shù)。

當系統(tǒng)沒有發(fā)生故障時，當故障發(fā)生后，增大并超過閾值且越小，表明該子系統(tǒng)對應(yīng)的狀態(tài)量發(fā)生的故障程度越大。

根據(jù)式(11)可知，B_i,k為18×18的矩陣，且滿足信息守恒原理。

聯(lián)邦濾波信息分配的表達式為：

其中，Β_iQ為Β_i,k后9個對角線元素組成的矩陣，對應(yīng)系統(tǒng)噪聲W的分配系數(shù)，Q_g為主濾波器中設(shè)置的系統(tǒng)噪聲方差矩陣。

(7)量測噪聲的自適應(yīng)調(diào)節(jié)方法

本步驟主要說明步驟(3)中R_i,k的計算方法，根據(jù)矢量信息分配參數(shù)自適應(yīng)計算量測誤差矩陣。在信息分配矢量中，各個分量與系統(tǒng)狀態(tài)量不完全一一對應(yīng)，因此需要將其轉(zhuǎn)換到量測信息上來。下面分別通過姿態(tài)量測信息、位置量測信息以及速度量測信息說明轉(zhuǎn)換方法，并以此計算自適應(yīng)調(diào)節(jié)參數(shù)。

A.姿態(tài)量測信息轉(zhuǎn)換方法

系統(tǒng)狀態(tài)量中的采用的參數(shù)為平臺誤差角[φ_Eφ_Nφ_U]^T，而量測信息中采用的參數(shù)為姿態(tài)誤差角[δγ δθ δψ]^T(δγ、δθ、δψ分別表示橫滾角誤差、俯仰角誤差及航向角誤差)，因此矢量分配信息不能直接使用。其轉(zhuǎn)換關(guān)系為：

其中，

因此，姿態(tài)量測信息對應(yīng)的分配系數(shù)為：

歸一化后，得：

為轉(zhuǎn)換后的姿態(tài)量測誤差自適應(yīng)調(diào)節(jié)參數(shù)，則修正后的姿態(tài)量測噪聲方差陣為：

其中，為初始姿態(tài)量測噪聲方差陣。

B.速度量測信息轉(zhuǎn)換方法

在速度量測信息中，轉(zhuǎn)換矩陣

狀態(tài)信息與量測信息一一對應(yīng)，因此

為速度量測誤差自適應(yīng)調(diào)節(jié)參數(shù)，則修正后的速度量測噪聲方差陣為：

其中，為初始速度量測噪聲方差陣。

C.位置量測信息轉(zhuǎn)換方法

在位置量測信息中，轉(zhuǎn)換矩陣

其中，R_M為地球子午圈曲率半徑，R_N為地球卯酉圈曲率半徑，L為當前緯度信息。

狀態(tài)信息與量測信息一一對應(yīng)，因此

為位置量測誤差自適應(yīng)調(diào)節(jié)參數(shù)，則修正后的位置量測噪聲方差陣為：

其中，為初始位置量測噪聲方差陣。

以上實施例僅為說明本發(fā)明的技術(shù)思想，不能以此限定本發(fā)明的保護范圍，凡是按照本發(fā)明提出的技術(shù)思想，在技術(shù)方案基礎(chǔ)上所做的任何改動，均落入本發(fā)明保護范圍之內(nèi)。