基于矢量化檢測聯(lián)邦濾波的INS/BDS/地磁組合導(dǎo)航容錯方法

邵宇強(qiáng)

(哈爾濱理工大學(xué)測控技術(shù)與通信工程學(xué)院, 哈爾濱 150080)

0 引言

隨著科技的不斷發(fā)展和進(jìn)步,人類對于邁入深空、挺進(jìn)深海、探索地球各個角落的熱情日益高漲。導(dǎo)航是人類進(jìn)行各種探索活動與科學(xué)研究的重要基礎(chǔ),目前對于高精度、高可靠導(dǎo)航手段[1]的需求與日俱增。以全球定位系統(tǒng)(global positioning system, GPS)、北斗衛(wèi)星導(dǎo)航系統(tǒng)(Beidou naviga-tion satellite system, BDS)為代表的全球衛(wèi)星導(dǎo)航系統(tǒng)是一種重要的導(dǎo)航手段,但衛(wèi)星信號極易受到遮擋與干擾,難以在復(fù)雜環(huán)境下提供可靠的導(dǎo)航信息[2-3]。而其他常用的導(dǎo)航系統(tǒng),例如慣性導(dǎo)航系統(tǒng)(inertial navigation system, INS)等也存在著各自的局限性[4],單獨(dú)運(yùn)行時無法提供連續(xù)精確的導(dǎo)航信息[5]。單一的導(dǎo)航方式已經(jīng)難以滿足日益增長的需求,一般采用多種導(dǎo)航手段相融合的方式,取長補(bǔ)短,進(jìn)而提升導(dǎo)航系統(tǒng)的整體性能[6]。聯(lián)邦卡爾曼濾波[7]是實(shí)現(xiàn)多種導(dǎo)航信息融合的主要方式[8],是一種分散化的多傳感器數(shù)據(jù)融合方法[9],能夠消除不同導(dǎo)航信息之間的相關(guān)性,并保證信息守恒,因此具有良好的容錯性、精確性與高效性,是一種理想的容錯信息融合算法。

保證基于聯(lián)邦卡爾曼濾波的組合導(dǎo)航系統(tǒng)的性能的關(guān)鍵問題之一,是對各導(dǎo)航信息源進(jìn)行有效的故障診斷,該問題已成為國內(nèi)外相關(guān)領(lǐng)域?qū)＜液蛯W(xué)者的重點(diǎn)研究方向,并在理論層面已經(jīng)有了很多成果。在眾多故障診斷方法中,χ2檢驗(yàn)法[10]是應(yīng)用最為廣泛的故障診斷方法(包含狀態(tài)χ2檢驗(yàn)法和殘差χ2檢驗(yàn)法)。在進(jìn)行組合導(dǎo)航系統(tǒng)故障檢測時,一般將故障子系統(tǒng)當(dāng)作一個整體來分析,并假設(shè)所有觀測量具有相同的估計精度和收斂速度,按照統(tǒng)一的權(quán)重來分配信息[11],可稱謂“基于系統(tǒng)層次的故障檢測方法”。但是,在實(shí)際情況中,故障子系統(tǒng)里通常仍有部分觀測量沒有出現(xiàn)異常,這樣就會造成對導(dǎo)航子系統(tǒng)信息利用不充分。為解決該問題,何廣軍等[12]提出了一種基于矢量信息分配的容錯聯(lián)邦濾波算法,能對每個狀態(tài)變量單獨(dú)進(jìn)行故障判斷和信息調(diào)整。呂建新等[13]提出了一種基于觀測殘差的自適應(yīng)聯(lián)邦卡爾曼濾波導(dǎo)航方法。當(dāng)載體受到異常干擾或動力學(xué)模型有較大誤差時,該方法能夠根據(jù)觀測殘差自動調(diào)整信息分配權(quán)重,減小導(dǎo)航誤差。不過,該方法只能影響子濾波器的狀態(tài)估計質(zhì)量,不會對主濾波器的狀態(tài)估計質(zhì)量產(chǎn)生影響。許麗佳等[14]提出了一種基于模糊推理系統(tǒng)和卡爾曼濾波的方法,用于克服聯(lián)合卡爾曼濾波法的缺點(diǎn)。該方法能在線調(diào)整噪聲統(tǒng)計特性,實(shí)現(xiàn)子濾波器和主濾波器的自適應(yīng)融合。此外,他們還提出了一種新穎的信息融合算法,根據(jù)模糊推理系統(tǒng)給出的各狀態(tài)估計量的信任權(quán)重,動態(tài)地進(jìn)行加權(quán)融合,從而提高了融合精度。蔣銳等[15]在聯(lián)邦卡爾曼子濾波器和主濾波器之間加入故障檢測,自適應(yīng)調(diào)整故障子濾波器的濾波增益矩陣。該方法可以有效檢測系統(tǒng)故障并對其進(jìn)行實(shí)時處理,提高系統(tǒng)的可靠性。

然而,上述傳統(tǒng)方法均是對整個系統(tǒng)采取相同的檢測機(jī)制,沒有針對不同導(dǎo)航方式的特點(diǎn)矢量化地檢測故障是否發(fā)生。對于導(dǎo)航信息豐富的系統(tǒng)而言,浪費(fèi)了本可以使用的信息,而對于需要快速檢測的系統(tǒng),則增加了計算量。因此,本文采用INS/BDS/地磁導(dǎo)航的多源組合導(dǎo)航模型,設(shè)計了一種基于矢量化檢測聯(lián)邦濾波的INS/BDS/地磁組合導(dǎo)航容錯方法。通過構(gòu)建面向不同導(dǎo)航信息的故障檢測函數(shù),能夠?qū)崿F(xiàn)更加準(zhǔn)確的矢量化信息分配,從而提高導(dǎo)航精度和容錯性能。

1 INS/BDS/地磁組合導(dǎo)航模型

本文選擇“東-北-天”導(dǎo)航坐標(biāo)系作為參考坐標(biāo)系。在該坐標(biāo)系下,系統(tǒng)的狀態(tài)變量包括位置誤差、速度誤差、姿態(tài)角誤差以及陀螺儀和加速度計的隨機(jī)常值漂移。

1.1 組合導(dǎo)航系統(tǒng)狀態(tài)方程

X=[δE,δN,δh,δve,δvn,δvu,φE,

(1)

Xi(k)=Fi(k|k-1)Xi(k-1)+
Gi(k|k-1)Wi(k-1)

(2)

其中,Fi為系統(tǒng)矩陣,Xi為狀態(tài)變量,Gi為系統(tǒng)噪聲矩陣,下標(biāo)i表示第i個子系統(tǒng)。系統(tǒng)噪聲為

(3)

1.2 組合導(dǎo)航系統(tǒng)量測方程

針對INS、BDS和地磁導(dǎo)航3種導(dǎo)航方式的特點(diǎn),以慣性導(dǎo)航作為基礎(chǔ)導(dǎo)航方式,分別構(gòu)造INS/BDS組合導(dǎo)航為子系統(tǒng)1,INS/地磁組合導(dǎo)航為子系統(tǒng)2,各子系統(tǒng)的量測方程如下。

子系統(tǒng)1:對INS與BDS的位置信息和速度信息分別做差構(gòu)建觀測量。

(4)

其中:RM,RN分別為地球子午面及卯酉面主曲率半徑;NBe,NBn,NBu和vBe,vBn,vBu分別為北斗衛(wèi)星導(dǎo)航沿東、北、天方向的位置誤差和速度誤差。

子系統(tǒng)2: 觀測量選取為INS與地磁羅盤的姿態(tài)角誤差。

Z2(k)=HaX2(k)+Va(k)

(5)

其中,量測矩陣Ha=[03×6,I3×3,03×6],Va(k)=[VMγ,VMθ,VMΦ]T為地磁羅盤量測噪聲。

2 矢量化檢測容錯聯(lián)邦濾波器設(shè)計

在本文提出的INS/BDS/地磁組合導(dǎo)航系統(tǒng)中,根據(jù)各子系統(tǒng)的特點(diǎn),分別使用不同的故障檢測方式。子系統(tǒng)1中,BDS易受環(huán)境影響從而產(chǎn)生誤差,但其所能提供的導(dǎo)航信息較為豐富,若采用傳統(tǒng)的故障檢測方法,進(jìn)行系統(tǒng)層面的故障診斷和隔離,會丟失正常的觀測量信息,降低子系統(tǒng)1的信息利用率。因此,本文對子系統(tǒng)1的各狀態(tài)變量分別進(jìn)行故障診斷,只隔離其故障信息,對無故障的信息加以保留和利用,從而增加信息利用率。子系統(tǒng)2中,地磁羅盤通過測量地磁場提供姿態(tài)角信息[16],當(dāng)存在磁場干擾或其他因素使得地磁場的測量存在故障時,信息不再可靠,需要快速檢測隔離,且由于子系統(tǒng)2信息量相對較少,對其使用系統(tǒng)層面的故障診斷。

2.1 面向不同導(dǎo)航信息的故障檢測方法

在卡爾曼濾波中,當(dāng)故障發(fā)生時,其產(chǎn)生的殘差會偏離零均值的高斯白噪聲。因此,檢測系統(tǒng)中的故障有賴于對殘差的分析。故障檢測通常采用基于殘差的χ2檢測法,構(gòu)建一個故障檢測函數(shù),該函數(shù)可以捕捉到殘差的統(tǒng)計特性,然后使用該函數(shù)來確定是否發(fā)生了故障。這種方法能夠有效地檢測出系統(tǒng)中的故障,并采取糾正措施以保持?jǐn)?shù)據(jù)的完整性?；讦?檢測法,本文構(gòu)建了針對不同類型導(dǎo)航信息的故障檢測函數(shù)。

子系統(tǒng)1的矢量形式的故障檢測函數(shù)為

(6)

(7)

其中,r1表示子系統(tǒng)1在k時刻的殘差值,Λ1為m階方陣,表示為

(8)

子系統(tǒng)2的系統(tǒng)層面的故障檢測函數(shù)為

(9)

(10)

(11)

其中,r2表示子系統(tǒng)2在k時刻的殘差值,V2(k)表示其方差。Λ2(k)～χ2(n),n為量測Z2(k)的維數(shù)。當(dāng)Λ2(k)≤TD,判定為子系統(tǒng)2無故障;當(dāng)Λ2(k)>TD,判定為子系統(tǒng)2存在故障。TD為預(yù)先設(shè)置的故障檢測門限。

2.2 存在故障時的信息分配方法

信息分配系數(shù)的設(shè)置關(guān)系到聯(lián)邦濾波器的性能。信息分配方案的不同會導(dǎo)致狀態(tài)估計的精度和系統(tǒng)的魯棒性的不同,兩者往往難以兼顧[17]。對于各子系統(tǒng),過大的系數(shù)會使全局融合精度下降;過小的系數(shù)雖然能提升全局估計精度,但也會致使系統(tǒng)穩(wěn)定性更容易受故障量測信息干擾。因此,在聯(lián)邦濾波器設(shè)計時,要同時考慮狀態(tài)估計精度和系統(tǒng)魯棒性,做好平衡。本文提出的方法通過動態(tài)地調(diào)整狀態(tài)估計方差、系統(tǒng)噪聲方差和量測噪聲方差,可同時提高系統(tǒng)的精度和魯棒性。

設(shè)子系統(tǒng)i的量測噪聲系數(shù)為

Bi=diag(bi,1,bi,2,…,bi,j)

(12)

子系統(tǒng)1和2的觀測量維數(shù)分別為m和n。對于出現(xiàn)故障的觀測量,令其量測噪聲系數(shù)為0.01;而無故障觀測量的量測噪聲應(yīng)保持不變,其量測噪聲系數(shù)設(shè)置為1。以此規(guī)則構(gòu)造一個新的量測噪聲系數(shù)矩陣Bi,從而實(shí)現(xiàn)對量測噪聲方差陣的動態(tài)調(diào)整。具體如下

(13)

系統(tǒng)的估計精度可以由當(dāng)前的誤差估計方差體現(xiàn),對第i個子系統(tǒng)的協(xié)方差陣Pi進(jìn)行特征值分解,得到信息分配系數(shù)Gi[18]。

(14)

Li=diag(λi,1,λi,2,…,λi,n)

(15)

Αi=diag(αi,1,αi,2,…,αi,n)

(16)

(17)

估計的準(zhǔn)確性可以通過狀態(tài)估計的方差來衡量。狀態(tài)估計的方差越大,說明精度越低,因此,相應(yīng)的分配系數(shù)應(yīng)該越小。此外,還要確保每個子系統(tǒng)的狀態(tài)協(xié)方差矩陣保持對稱性,信息分配方式為

(18)

(19)

其中,Pg,Qg分別為主濾波器全局信息融合后的協(xié)方差矩陣和系統(tǒng)噪聲矩陣;Pi,Qi分別為子系統(tǒng)i信息重置后的協(xié)方差陣和系統(tǒng)噪聲矩陣。

2.3 容錯聯(lián)邦濾波算法

為減小主濾波器出現(xiàn)故障時反饋信息對子濾波器的不利影響,本文采用了如下的應(yīng)對方法。首先,子濾波器獨(dú)立完成其測量更新和時間更新,然后將所得信息輸入主濾波器完成融合。融合后的信息按照設(shè)計的信息分配方法在各子濾波器之間進(jìn)行分配,各子濾波器根據(jù)分配的信息再進(jìn)行新的時間更新和測量更新。這種方法有效地解決了主濾波器故障對子濾波器信息的污染問題,同時保證了濾波過程的連續(xù)性和魯棒性[19]。

根據(jù)上文提及的量測噪聲系數(shù)Bi及信息分配系數(shù)矩陣Gi,設(shè)計容錯聯(lián)邦濾波算法:

(1)子濾波器時間更新

Xi[(k+1)/k]=Fi[(k+1)/k]Xi(k)

(20)

(21)

(2)故障診斷

建立故障檢測函數(shù)Λi,對觀測量進(jìn)行故障診斷,得到量測噪聲系數(shù)Bi和量測噪聲矩陣Ri,減少對故障觀測量信息的利用。

(22)

(3)子濾波器量測更新

即

{Hi(k+1)Pi[(k+1)/k]·

(23)

Xi(k+1)=Xi[(k+1)/k]+Ki(k+1)·{Zi(k+1)

-Hi(k+1)Xi[(k+1)/k]}

(24)

Pi(k+1)=[I-Ki(k+1)Hi(k+1)]·Pi[(k+

1)/k][I-Ki(k+1)Hi(k+1)]T+

Ki(k+1)Ri(k+1)[Ki(k+1)]T

(25)

(4)構(gòu)建信息分配矢量系數(shù)Gi

(26)

(5)主濾波器信息融合

當(dāng)子系統(tǒng)1中的部分信息失效時,采用如下的融合方式

(27)

(28)

當(dāng)子系統(tǒng)2失效時,對其整體隔離,因此系統(tǒng)誤差狀態(tài)的整體估計為

Xg(k+1)=X1(k+1)

(29)

此時,子系統(tǒng)2的輸出估計值為

Z2(k+1)=H2Xg(k+1)

(30)

(6)子濾波器信息分配

(31)

(32)

Xi(k+1)=Xg(k+1)

(33)

由以上公式可得容錯聯(lián)邦濾波器結(jié)構(gòu)如圖1 所示。

圖1 聯(lián)邦濾波器結(jié)構(gòu)圖Fig.1 Structure of the federated filter

子系統(tǒng)2中,由于地磁羅盤僅能提供姿態(tài)角信息,對于速度和位置狀態(tài)的影響隨時間累積較快,因此當(dāng)其發(fā)生故障時,將其快速隔離有利于提高系統(tǒng)的整體精度。子系統(tǒng)2的故障檢測函數(shù)Λ2相比于子系統(tǒng)1的Λ1,計算量減小,故障檢測更快,從而能更快速地隔離故障。

當(dāng)系統(tǒng)出現(xiàn)故障時,通過噪聲系數(shù)放大故障觀測量的測量噪聲Ri,從而使協(xié)方差Pi變大,故障子系統(tǒng)在信息融合中的權(quán)重減小,進(jìn)而減少了對它的信息利用。通過調(diào)節(jié)噪聲系數(shù)bi,j來處理故障觀測量,如果噪聲系數(shù)較小,狀態(tài)估計的精度就會降低,全局融合中對故障狀態(tài)變量的使用減少;如果噪聲系數(shù)較大,狀態(tài)估計的精度就會提高,對故障子系統(tǒng)錯誤觀測量使用減少,同時正確觀測量的使用率增加。

3 仿真實(shí)驗(yàn)與分析

通過仿真實(shí)驗(yàn)對本文提出方法的有效性進(jìn)行驗(yàn)證。采用INS/BDS/地磁羅盤組合導(dǎo)航系統(tǒng),其中,INS/BDS組成子系統(tǒng)1;INS/地磁羅盤組成子系統(tǒng)2。模擬勻速直線運(yùn)動,航向角為45°,真實(shí)速度為:東向速度vE=10 m/s,北向速度vN=10 m/s,天向速度vU=1 m/s。濾波周期為1 s,仿真時間為1 000 s。假設(shè)陀螺隨機(jī)常值漂移為0.1 (°)/h,加速度計隨機(jī)常值漂移為10-4g,BDS位置誤差為10 m,速度誤差為0.5 m/s,電子羅盤的航向角精度為0.5°,俯仰角、橫滾角精度為0.1°。

3.1 無故障發(fā)生

(a) 位置誤差曲線比較

(b) 速度誤差曲線比較

通過對比可知,當(dāng)無故障發(fā)生時,本文算法的誤差波動區(qū)間明顯減小,曲線更為平緩,導(dǎo)航精度有所提升。因此,本文所述算法優(yōu)于傳統(tǒng)聯(lián)邦濾波平均分配方法。

3.2 系統(tǒng)出現(xiàn)故障

模擬BDS出現(xiàn)故障的場景。由于BDS能提供位置和速度信息,因此分別對其位置信息和速度信息添加誤差。

(1)BDS位置信息異常

在400 s時,在東、北、天3個方向上各施加30 m 的位置誤差,該情況下本文算法的位置誤差曲線如圖3(a)所示;速度誤差曲線如圖3(b)所示。

(a) 出現(xiàn)位置故障時位置誤差曲線

(b) 出現(xiàn)位置故障時速度誤差曲線圖3 出現(xiàn)位置故障時本文算法的誤差曲線Fig.3 Error curves of the algorithm in this paper with failure of position

在圖3(a)中,400 s時,3個方向上的位置誤差突然增大,而后降為正常水平。由于系統(tǒng)可對BDS的導(dǎo)航信息分別檢測和利用,當(dāng)出現(xiàn)位置誤差時,速度信息仍能在系統(tǒng)中發(fā)揮作用。因此,在圖3(b)中,速度誤差在該時間段內(nèi)變化不明顯,與其他時間段內(nèi)持平。說明系統(tǒng)可以提高故障子系統(tǒng)中無故障信息的利用率。

(2)BDS速度信息異常

在600 s時,在東、北、天3個方向上各施加1.5 m/s 的速度誤差,該情況下的本文算法的位置誤差曲線如圖4(a)所示;速度誤差曲線如圖4(b)所示。

(a) 出現(xiàn)速度故障時位置誤差曲線

(b) 出現(xiàn)速度故障時速度誤差曲線圖4 出現(xiàn)速度故障時本文算法的誤差曲線Fig.4 Error curves of the algorithm in this paper with failure of velocity

在圖4(b)中,600 s時,3個方向上的速度誤差突然增大,故障被隔離后誤差降為正常水平。在圖4(a)中,雖然系統(tǒng)可對異常的速度信息進(jìn)行隔離,但檢測需要一定時間,受此影響,在600 s左右位置誤差有小幅度增加,而后由于故障信息被隔離,且BDS繼續(xù)提供位置信息用于校正,故位置誤差在一段時間后回歸正常水平。由此可見,BDS所提供的無故障信息發(fā)揮了重要的作用。

(3)地磁羅盤故障

模擬地磁羅盤出現(xiàn)故障的場景。地磁羅盤能提供姿態(tài)角信息,但3個姿態(tài)角存在相關(guān)性。因此當(dāng)其出現(xiàn)故障時需要對其進(jìn)行整體隔離,以免故障擴(kuò)散到整個系統(tǒng)中。在600 s時,在航向、俯仰、橫滾3個方向上各施加0.2°的角度誤差,該情況下本文算法的位置誤差曲線如圖5(a)所示;速度誤差曲線如圖5(b)所示;姿態(tài)角誤差曲線如圖5(c)所示。

(a) 地磁羅盤故障下位置誤差曲線

(b) 地磁羅盤故障下速度誤差曲線

(c) 地磁羅盤故障下姿態(tài)角誤差曲線圖5 地磁羅盤故障下本文算法的誤差曲線Fig.5 Error curves of the algorithm in this paper with failure of geomagnetic compass

在圖5(c)中,角度誤差增大后迅速下降,快速回歸正常水平,表明故障信息被系統(tǒng)隔離。而在圖5(a)和5(b)中,對應(yīng)時間內(nèi),誤差無明顯變化,INS和BDS繼續(xù)發(fā)揮作用,提供速度和位置信息。

4 結(jié)論

本文提出了一種基于矢量化檢測聯(lián)邦濾波的INS/BDS/地磁組合導(dǎo)航容錯方法,實(shí)驗(yàn)結(jié)果表明,該方法可以提高系統(tǒng)在不同故障情況下的穩(wěn)定性和魯棒性。主要結(jié)論如下:

1)構(gòu)建了面向不同導(dǎo)航信息的故障檢測方法。針對BDS信息多,但易受干擾的特點(diǎn),對其子系統(tǒng)的每個觀測量均進(jìn)行檢測,在隔離故障信息的同時最大程度利用剩余的無故障信息,提高了信息利用率;針對地磁羅盤各觀測量相關(guān)且誤差對導(dǎo)航結(jié)果影響大的特點(diǎn),對其組成的子系統(tǒng)使用計算量小的系統(tǒng)檢測方式,快速檢測隔離故障,防止誤差擴(kuò)散到整體系統(tǒng)中,提高了組合導(dǎo)航系統(tǒng)的魯棒性。

2)通過使用矢量形式的信息分配方式來進(jìn)行主濾波器的信息反饋,可以充分發(fā)揮各子系統(tǒng)的優(yōu)點(diǎn),并有效利用其中正常的狀態(tài)估計信息,從而提高了整體組合導(dǎo)航系統(tǒng)的精度。

3)通過動態(tài)調(diào)整測量噪聲矩陣,提高導(dǎo)航各子系統(tǒng)量測信息利用的可靠性,從而有效減小故障觀測量對狀態(tài)估計的影響。