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

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

基于改進聯(lián)邦Kalman濾波的組合校準方法研究

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

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

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

信息分配；信息融合；聯(lián)邦濾波；組合校準

0 引言

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

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

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

1 聯(lián)邦濾波算法

1.1 聯(lián)邦濾波器概述

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

圖1 聯(lián)邦Kalman濾波器結構Fig.1 The structure of the federated Kalman filter

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

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

1.2 聯(lián)邦濾波算法及過程

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

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

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

(1)

聯(lián)邦Kalman濾波器的過程如下。

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

(2)

(3)

(4)

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

(5)

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

2 改進聯(lián)邦濾波算法設計

2.1 信息分配和重置優(yōu)化

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

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

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

(6)

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

(7)

(8)

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

(9)

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

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

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

(10)

式中，狀態(tài)變量為

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

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

狀態(tài)噪聲為

W1= [vrxvryvrzvgxvgyvgz

vaxvayvazwc1wc2wc3]T

將捷聯(lián)慣導和電子羅盤輸出姿態(tài)之差作為量測值，得到觀測方程為

(11)

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

2.3 SINS/GPS子濾波器設計

把GPS誤差項擴充進SINS狀態(tài)空間，得到狀態(tài)方程為

(12)

其中，狀態(tài)向量為

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

系統(tǒng)噪聲為

W1= [vrxvryvrzvgxvgyvgzvaxvayvazwD]T

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

(13)

3 算法仿真與驗證

初始位置為緯度φ=30°，經(jīng)度λ=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。電子羅盤的航向經(jīng)度為0.5°，縱搖和橫搖角精度為0.2°。

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

圖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 姿態(tài)角誤差估計及其誤差殘差Fig.4 The estimation of the attitude errors and their residuals

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

4 結論

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

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

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

[3] 馬衛(wèi)華,羅建軍,袁建平.空間轉(zhuǎn)移飛行器自主導航系統(tǒng)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] 胡建,馬大為,程向紅,等.聯(lián)邦濾波信息分配方法及其在傳遞對準中的應用[J].南京理工大學學報, 2011, 35(2): 224-229.

[8] 劉瑞華,劉建業(yè).聯(lián)邦濾波信息分配新方法[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(yè)務費(HEUCFX41309)。

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

U666.12

A

2095-8110(2016)06-0021-05