基于矢量化檢測聯邦濾波的INS/BDS/地磁組合導航容錯方法

邵宇強

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

0 引言

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

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

然而,上述傳統方法均是對整個系統采取相同的檢測機制,沒有針對不同導航方式的特點矢量化地檢測故障是否發生。對于導航信息豐富的系統而言,浪費了本可以使用的信息,而對于需要快速檢測的系統,則增加了計算量。因此,本文采用INS/BDS/地磁導航的多源組合導航模型,設計了一種基于矢量化檢測聯邦濾波的INS/BDS/地磁組合導航容錯方法。通過構建面向不同導航信息的故障檢測函數,能夠實現更加準確的矢量化信息分配,從而提高導航精度和容錯性能。

1 INS/BDS/地磁組合導航模型

本文選擇“東-北-天”導航坐標系作為參考坐標系。在該坐標系下,系統的狀態變量包括位置誤差、速度誤差、姿態角誤差以及陀螺儀和加速度計的隨機常值漂移。

1.1 組合導航系統狀態方程

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為系統矩陣,Xi為狀態變量,Gi為系統噪聲矩陣,下標i表示第i個子系統。系統噪聲為

(3)

1.2 組合導航系統量測方程

針對INS、BDS和地磁導航3種導航方式的特點,以慣性導航作為基礎導航方式,分別構造INS/BDS組合導航為子系統1,INS/地磁組合導航為子系統2,各子系統的量測方程如下。

子系統1:對INS與BDS的位置信息和速度信息分別做差構建觀測量。

(4)

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

子系統2: 觀測量選取為INS與地磁羅盤的姿態角誤差。

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

(5)

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

2 矢量化檢測容錯聯邦濾波器設計

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

2.1 面向不同導航信息的故障檢測方法

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

子系統1的矢量形式的故障檢測函數為

(6)

(7)

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

(8)

子系統2的系統層面的故障檢測函數為

(9)

(10)

(11)

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

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

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

設子系統i的量測噪聲系數為

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

(12)

子系統1和2的觀測量維數分別為m和n。對于出現故障的觀測量,令其量測噪聲系數為0.01;而無故障觀測量的量測噪聲應保持不變,其量測噪聲系數設置為1。以此規則構造一個新的量測噪聲系數矩陣Bi,從而實現對量測噪聲方差陣的動態調整。具體如下

(13)

系統的估計精度可以由當前的誤差估計方差體現,對第i個子系統的協方差陣Pi進行特征值分解,得到信息分配系數Gi[18]。

(14)

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

(15)

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

(16)

(17)

估計的準確性可以通過狀態估計的方差來衡量。狀態估計的方差越大,說明精度越低,因此,相應的分配系數應該越小。此外,還要確保每個子系統的狀態協方差矩陣保持對稱性,信息分配方式為

(18)

(19)

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

2.3 容錯聯邦濾波算法

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

根據上文提及的量測噪聲系數Bi及信息分配系數矩陣Gi,設計容錯聯邦濾波算法:

(1)子濾波器時間更新

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

(20)

(21)

(2)故障診斷

建立故障檢測函數Λi,對觀測量進行故障診斷,得到量測噪聲系數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)構建信息分配矢量系數Gi

(26)

(5)主濾波器信息融合

當子系統1中的部分信息失效時,采用如下的融合方式

(27)

(28)

當子系統2失效時,對其整體隔離,因此系統誤差狀態的整體估計為

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

(29)

此時,子系統2的輸出估計值為

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

(30)

(6)子濾波器信息分配

(31)

(32)

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

(33)

由以上公式可得容錯聯邦濾波器結構如圖1 所示。

圖1 聯邦濾波器結構圖Fig.1 Structure of the federated filter

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

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

3 仿真實驗與分析

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

3.1 無故障發生

(a) 位置誤差曲線比較

(b) 速度誤差曲線比較

通過對比可知,當無故障發生時,本文算法的誤差波動區間明顯減小,曲線更為平緩,導航精度有所提升。因此,本文所述算法優于傳統聯邦濾波平均分配方法。

3.2 系統出現故障

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

(1)BDS位置信息異常

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

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

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

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

(2)BDS速度信息異常

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

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

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

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

(3)地磁羅盤故障

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

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

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

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

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

4 結論

本文提出了一種基于矢量化檢測聯邦濾波的INS/BDS/地磁組合導航容錯方法,實驗結果表明,該方法可以提高系統在不同故障情況下的穩定性和魯棒性。主要結論如下:

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

2)通過使用矢量形式的信息分配方式來進行主濾波器的信息反饋,可以充分發揮各子系統的優點,并有效利用其中正常的狀態估計信息,從而提高了整體組合導航系統的精度。

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