基于改進聯邦Kalman濾波的組合校準方法研究

陳 晶，袁書明，程建華，曹新宇

(1. 海軍裝備研究院，北京 100161；2. 哈爾濱工程大學自動化學院，哈爾濱 150001)

基于改進聯邦Kalman濾波的組合校準方法研究

陳 晶1，袁書明1，程建華2，曹新宇2

(1. 海軍裝備研究院，北京 100161；2. 哈爾濱工程大學自動化學院，哈爾濱 150001)

聯邦濾波器廣泛應用于多傳感器信息融合領域，聯邦濾波中的信息分配原則影響濾波精度。針對聯邦Kalman濾波器進行改進，采用基于估計協方差陣奇異值動態確定信息分配系數。對子濾波器進行重置時，采用新的重置方法，保證了子濾波器誤差協方差陣的對稱性，確保Kalman濾波器的一致收斂穩定性。新的聯邦濾波算法允許每個狀態分量擁有不同的動態信息分配因子，從而改進了聯邦濾波信息融合的精度。設計了SINS/GPS/電子羅盤組合導航系統，仿真結果說明，與傳統聯邦濾波算法相比，改進的聯邦濾波器估計精度得到了提高，可以更好地對SINS誤差進行校準，提高系統的精度。

信息分配；信息融合；聯邦濾波；組合校準

0 引言

捷聯慣導系統(Strapdown Inertial Navigation System，SINS)是一種自主式導航系統，其優點在于不受外界干擾，獨立地進行三維空間定位解算，提供比較完備的導航信息，并且成本低、體積小、易維護，因此在軍事領域有著不可替代的作用。由于慣性器件受加工材料和工藝的限制而存在誤差，從而導致SINS導航誤差隨時間積累，尤其是經度誤差隨時間發散，這限制了捷聯慣導系統導航時間長度。因此有必要利用其他導航方式和SINS聯合應用，取長補短并對SINS進行修正。

GPS導航定位信號在真空和大氣層中傳播不受限制，可以進行全球導航定位，具有定位精度高、長期穩定、誤差不隨時間累積等優點，因此可全天候地在全球范圍內提供實時的導航信息[1-2]。但其缺點在于易受干擾、動態性能差，因此不能作為絕對依賴的導航方式[3-4]。電子羅盤(Electronic Compass, CE)是一種結構簡單的航向測量裝置，啟動速度快，可以全天候精確地提供航向、俯仰、翻滾等姿態角信息，利用它的這些特點可以作為一種輔助導航設備。電子羅盤的原理是測量地球磁場，但若使用的環境中有地球以外的磁場且這些磁場無法有效屏蔽時，那么電子羅盤的使用就有很大的問題。

本文根據以上三種導航方式的優缺點，將其構成組合系統，采用改進的聯邦Kalman濾波器進行誤差估計。其中SINS/GPS構成一個子系統，SINS和電子羅盤構成另一個子系統，對帶重置聯邦濾波器的信息分配方式進行改進，相對于傳統的聯邦濾波形式，該濾波算法可以提高對SINS誤差的估計精度。

1 聯邦濾波算法

1.1 聯邦濾波器概述

聯邦Kalman濾波理論是一種特殊形式的分布式Kalman濾波方法，由若干個子濾波器和一個主濾波器組成，是一個具有分塊估計、兩步級聯的分散化濾波方法，有重置的聯邦濾波器的結構如圖1所示。

圖1 聯邦Kalman濾波器結構Fig.1 The structure of the federated Kalman filter

聯邦濾波器的關鍵在于它采用信息分配原理，需要向各子濾波器分配動態信息，這些信息包括兩大類：狀態方程的信息和觀測方程的信息。最早的信息分配是固定比例的，而在實際導航環境中，各子濾波器的性能是不斷變化的。為了提高聯邦濾波器的性能，國外學者提出了動態信息分配方法，例如基于估計協方差特征值或Frobenius范數、基于可觀測矩陣條件數等[7-8]。動態信息分配方法在一定程度上可以對系統狀態進行實時跟蹤，濾波效果得到了改善。但是上邊的信息分配系數都是標量，實質是把每個狀態分量的特征看成是相同的。但是實際情況卻不是如此，每個子系統的特性和精度不同，使得各子濾波器的狀態方程和量測方程不同，進而導致狀態分量具有不同的估計精度和收斂速度。所以，信息分配系數是標量時，不能很好地反映每個狀態分量的變化情況。

針對上述問題，本文提出了一種基于系統協方差陣信息的動態矢量信息分配方法。該方法能夠充分體現每個狀態分量的變化，實時地得到不同的信息分配因子。對子濾波器協方差陣采用的反饋重置方法，可以保證其對稱性，從而確保濾波器的一致收斂穩定性。

1.2 聯邦濾波算法及過程

聯邦濾波算法的基本思想是先將各個傳感器信息分散處理，然后在主濾波器中進行信息融合。也即子濾波器之間并行運行，他們的輸出再在主濾波器中周期性的融合處理，得到全局狀態估計。在這個過程中，主濾波器將子濾波器的估計值融合成系統全局估計值，同時按照信息分配原理將全局估計值分配給各個子濾波器，進而完成一次濾波。根據聯邦Kalman濾波器基本原理，假設有n個子濾波器，第i個子濾波器的狀態方程和觀測方程為：

Xi(k)=Φ(k,k-1)Xi(k-1)+W(k)

Zi(k)=Hi(k)Xi(k)+Vi(k)

(1)

聯邦Kalman濾波器的過程如下。

1)信息分配過程：信息分配就是將系統的過程信息分配給各子濾波器和主濾波器。按照如下的分配原則進行分配：

(2)

(3)

(4)

4)信息融合：在主濾波器中將各子濾波器估計值進行融合，得到全局最優估計值：

(5)

信息分配系數的確定在聯邦濾波器的設計中非常重要，不同的取值會得到不同的聯邦濾波器結構和性能?，F在常用的信息分配方案均采用標量形式，無法反應出每個狀態變量的具體變化特性。

2 改進聯邦濾波算法設計

2.1 信息分配和重置優化

在Kalman濾波過程中，狀態估計誤差協方差P(k)反映了X(k)的估計精度，P(k)越大，X(k)的估計精度就越差；P(k)越小，X(k)的估計精度就越好。從聯邦濾波過程可知，每當主濾波器對子濾波器的信息融合一次，都會將融合后的全局估計信息通過信息分配系數βi反饋給各子濾波器。從而改變子濾波器的濾波模型，實現對子濾波器數學模型的誤差補償，信息分配系數的大小反映了對誤差的補償程度。

根據各子濾波器估計的精度來動態地確定信息分配系數，可以更好地反映估計精度的變化情況，減小系統精度下降或者失效帶來的影響。假設有n個子濾波器，先確定主濾波器的信息分配系數βm，之后可以用下述方法確定子濾波器的信息分配系數βi。

估計誤差協方差陣Pi可以按照它的特征值分解為Pi=LΛiLT。其中，Λi=diag{λi1,λi2,…,λin}，λi1,λi2,…,λin是Pi的特征值。信息分配系數采用對角矩陣形式，βi=diag{ai1,ai2,…,aij}，i=1,2,…，N，j=1,2,…,n

(6)

這種信息分配形式對每一個狀態分量獨立進行信息分配，可以避免子濾波器的方差陣太小而導致主濾波器無法求逆。信息分配系數由標量變成矢量，但信息分配過程不變

(7)

(8)

可以看出此時的誤差協方差陣是對稱的。系統的過程信息分配按照式(9)進行：

(9)

該方案中，如式(6)所示，每進行一次濾波信息融合，就利用該次的估計誤差協方差陣Pi確定信息分配系數βi，從而實現信息分配系數的動態確定過程。

2.2SINS/電子羅盤子濾波器設計

選取東-北-天坐標系作為導航坐標系，將電子羅盤誤差項擴充進到捷聯慣導(SINS)狀態空間，可得狀態方程為

(10)

式中，狀態變量為

X1= [δVxδVyδVzδVφδVλδVhδφx

δφyδφzεrxεryεrzεbxεbyεbz

狀態噪聲為

W1= [vrxvryvrzvgxvgyvgz

vaxvayvazwc1wc2wc3]T

將捷聯慣導和電子羅盤輸出姿態之差作為量測值，得到觀測方程為

(11)

SINS輸出的姿態值是在導航坐標系下的，電子羅盤輸出的姿態角是在載體坐標系下的，需要進行坐標轉換，轉換到導航坐標系下，這里省略不寫。

2.3 SINS/GPS子濾波器設計

把GPS誤差項擴充進SINS狀態空間，得到狀態方程為

(12)

其中，狀態向量為

X1= [δVxδVyδVzδLδλδhδφxδφy

系統噪聲為

W1= [vrxvryvrzvgxvgyvgzvaxvayvazwD]T

將捷聯慣導和GPS輸出的速度和位置信息作為量測值，得到觀測方程為

(13)

3 算法仿真與驗證

初始位置為緯度φ=30°，經度λ=120°；東向和北向速度都是6m/s，陀螺的常值漂移為0.01(°)/h；陀螺的隨機漂移為0.001(°)/h；加速度計零偏為100μg；加速度計隨機偏差為5μg；初始航向誤差為0.02°；初始速度誤差均為0.1m/s；初始位置誤差分別為0.05′。GPS的水平位置誤差均方根為0.002′，速度誤差均方根為0.1m/s。電子羅盤的航向經度為0.5°，縱搖和橫搖角精度為0.2°。

圖2～圖4中，藍色曲線表示傳統的信息分配方法，信息分配系數都是1/3，紅色曲線為采用本文的動態信息分配矩陣和重置方法的結果。

圖2 位置誤差估計及其誤差殘差Fig.2 The estimation of the position errors and their residuals

圖3 水平速度誤差估計及其誤差殘差Fig.3 The estimation of the velocity errors and their residuals

圖4 姿態角誤差估計及其誤差殘差Fig.4 The estimation of the attitude errors and their residuals

根據圖2～圖4可以看出，將SINS/GPS/電子羅盤組合后，可以有效地跟蹤慣導系統的誤差傳播規律，實現對慣導系統誤差的準確估計。其中，位置估計誤差的殘差控制在10m左右，水平速度估計誤差的殘差在0.1m/s左右，縱搖和橫搖角的估計誤差的殘差控制在0.17′以內，航向角估計誤差的殘差控制在0.15′左右。組合后利用各導航誤差的估計值，對慣導系統解算結果進行誤差補償，可以顯著地遏制SINS誤差隨時間發散的趨勢。紅色曲線代表使用本文的信息分配方法，與傳統的固定信息分配系數相比，可以看出緯度、東向速度、縱搖和橫搖角誤差估計精度得到了顯著提升，經度、北向速度和航向角的誤差估計精度也稍好于傳統的信息分配方法。

4 結論

本文從系統狀態估計誤差協方差表示估計精度出發，研究了一種動態信息分配的聯邦Kalman濾波器，與傳統固定信息分配系數相比，可以反映每個狀態分量的情況。在對子濾波器重置時，采用新的重置方法保證了子濾波器誤差協方差陣的對稱性，避免了因協方差陣不對稱導致濾波誤差增大，甚至失去濾波效果。本文通過建立SINS/GPS/電子羅盤組合導航系統，采用改進的聯邦Kalman濾波器對捷聯慣導系統進行組合校準。通過計算機仿真結果表明，采用本文提出的信息分配和重置方法的SINS/GPS/電子羅盤聯邦濾波算法有效地提高了組合校準精度。

[1] 崔鵬程,邱宏波.艦載導彈用捷聯慣導系統校準技術研究[J].中國慣性技術學報,2007,15(1):12-15.

[2] 陳建國,練軍想,張倫東，等.一種用于艦船CNS/RLG/SINS組合導航系統校準的分段估計方法研究[J].導航與控制, 2010,15(1):12-15.

[3] 馬衛華,羅建軍,袁建平.空間轉移飛行器自主導航系統SINS/GPS/SS的卡爾曼濾波器降階方案[J].宇航學報,2008,29(5):1556-1662.

[4]JwoDJ,HsiehMY,LaiSY.GPSnavigationprocessingusingthequaternion-baseddivideddifferencefilter[J].GPSSolutions,2010,14(3):217 -228.

[5]CarlsonNA.Federatedfilterforfault-tolerantintegratednavigationsystems[C].PositionLocationandNavigationSymposium,IEEEPLANS,1988: 110-119.

[6]CarlsonNA,BerarducciMP.FederatedKalmanfiltersimulationresults[J].Navigation,1994,41(3):297-321.

[7] 胡建,馬大為,程向紅,等.聯邦濾波信息分配方法及其在傳遞對準中的應用[J].南京理工大學學報, 2011, 35(2): 224-229.

[8] 劉瑞華,劉建業.聯邦濾波信息分配新方法[J].中國慣性技術學報,2001,9(2):28-32.

Research on Integrated Calibration Based on Improved Federated Kalman Filter

CHEN Jing1, YUAN Shu-ming1, CHENG Jian-hua2, CAO Xin-yu2

(1. Naval Armament Research Institute, Beijing 100161, China;2.College of Automation, Harbin Engineering University, Harbin 150001, China)

Federated filter has been widely used in the field of multi-sensor information fusion, and information distribution rule can directly affect the filtering accuracy. In this article, federated Kalman filter is improved by determining the information-distributing coefficients dynamically for federated filter with resetting configuration based on singular value of the covariance matrix of the estimated errors. The sub-filter is reset with new methods to ensure the symmetry of the sub-filters’ error covariance matrix and the stability of the Kalman filter’s uniform convergence. The new algorithm allows each system state variable to have different information distribution factors, and hence improves the estimation accuracy of the federated filter. An INS/GPS/electrical compass integrated navigation system is designed by using federated Kalman filter technique. The simulation shows that compared with the traditional method, the new federated Kalman filter can improve the precision of the estimated errors and enable better error-correction and accuracy of SINS.

Information distribution; Information fusion; Federated filter; Integrated calibration

10.19306/j.cnki.2095-8110.2016.06.005

2016-08-23；

2016-10-18。

國家自然科學基金(61374007，61104036)； 中央高?；究蒲袠I務費(HEUCFX41309)。

陳晶(1966 - )，女，高級工程師，主要從事艦船綜合導航方面的研究。

U666.12

A

2095-8110(2016)06-0021-05