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

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

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

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

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

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

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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)的過程信息分配給各子濾波器和主濾波器。按照如下的分配原則進(jìn)行分配：

(2)

(3)

(4)

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

(5)

信息分配系數(shù)的確定在聯(lián)邦濾波器的設(shè)計(jì)中非常重要，不同的取值會(huì)得到不同的聯(lián)邦濾波器結(jié)構(gòu)和性能。現(xiàn)在常用的信息分配方案均采用標(biāo)量形式，無法反應(yīng)出每個(gè)狀態(tài)變量的具體變化特性。

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

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

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

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

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

(6)

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

(7)

(8)

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

(9)

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

2.2SINS/電子羅盤子濾波器設(shè)計(jì)

選取東-北-天坐標(biāo)系作為導(dǎo)航坐標(biāo)系，將電子羅盤誤差項(xiàng)擴(kuò)充進(jìn)到捷聯(lián)慣導(dǎo)(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)慣導(dǎo)和電子羅盤輸出姿態(tài)之差作為量測(cè)值，得到觀測(cè)方程為

(11)

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

2.3 SINS/GPS子濾波器設(shè)計(jì)

把GPS誤差項(xiàng)擴(kuò)充進(jìn)SINS狀態(tài)空間，得到狀態(tài)方程為

(12)

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

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

系統(tǒng)噪聲為

W1= [vrxvryvrzvgxvgyvgzvaxvayvazwD]T

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

(13)

3 算法仿真與驗(yàn)證

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

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

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

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

圖4 姿態(tài)角誤差估計(jì)及其誤差殘差Fig.4 The estimation of the attitude errors and their residuals

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

4 結(jié)論

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

[1] 崔鵬程,邱宏波.艦載導(dǎo)彈用捷聯(lián)慣導(dǎo)系統(tǒng)校準(zhǔn)技術(shù)研究[J].中國(guó)慣性技術(shù)學(xué)報(bào),2007,15(1):12-15.

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

[3] 馬衛(wèi)華,羅建軍,袁建平.空間轉(zhuǎn)移飛行器自主導(dǎo)航系統(tǒng)SINS/GPS/SS的卡爾曼濾波器降階方案[J].宇航學(xué)報(bào),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)邦濾波信息分配方法及其在傳遞對(duì)準(zhǔn)中的應(yīng)用[J].南京理工大學(xué)學(xué)報(bào), 2011, 35(2): 224-229.

[8] 劉瑞華,劉建業(yè).聯(lián)邦濾波信息分配新方法[J].中國(guó)慣性技術(shù)學(xué)報(bào),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。

國(guó)家自然科學(xué)基金(61374007，61104036)； 中央高?；究蒲袠I(yè)務(wù)費(fèi)(HEUCFX41309)。

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

U666.12

A

2095-8110(2016)06-0021-05