方位捷聯(lián)平臺(tái)重力儀分布式Kalman濾波初始對(duì)準(zhǔn)算法

楊 曄，毋興濤，楊建林，高 巍，裴 志

（1.天津航海儀器研究所，天津 300131；2.天津地區(qū)軍事代表室，天津 300131）

方位捷聯(lián)平臺(tái)重力儀分布式Kalman濾波初始對(duì)準(zhǔn)算法

楊 曄1，毋興濤1，楊建林2，高 巍1，裴 志1

（1.天津航海儀器研究所，天津 300131；2.天津地區(qū)軍事代表室，天津 300131）

為充分利用分布式架構(gòu)重力儀各處理器并行計(jì)算的能力，解決單個(gè)處理器運(yùn)行整體式 Kalman濾波所遇到的非實(shí)時(shí)性問題，設(shè)計(jì)了一種分布式 Kalman濾波對(duì)準(zhǔn)算法。首先，給出了方位捷聯(lián)平臺(tái)重力儀的誤差方程，建立了系統(tǒng)的狀態(tài)方程和觀測(cè)方程。然后，用協(xié)方差分析法對(duì)系統(tǒng)初始對(duì)準(zhǔn)濾波方程進(jìn)行處理，將原系統(tǒng)分解成維數(shù)相同的兩個(gè)子系統(tǒng)，得到由兩個(gè)子濾波器構(gòu)成的初始對(duì)準(zhǔn)濾波器。最后，利用Matlab建立了方位捷聯(lián)平臺(tái)慣導(dǎo)模型，分別應(yīng)用整體式濾波和分布式濾波進(jìn)行靜基座初始對(duì)準(zhǔn)。仿真結(jié)果表明，分布式濾波算法與整體式濾波算法具有相同的濾波精度，并且分布式濾波用時(shí)只有整體式濾波的60%，更有利于保證濾波算法的實(shí)時(shí)性。

方位捷聯(lián)平臺(tái)；分布式Kalman濾波；正交化；初始對(duì)準(zhǔn)

某型重力儀采用多個(gè)低成本處理器構(gòu)成分布式控制與計(jì)算系統(tǒng)。為充分利用系統(tǒng)多處理器并行計(jì)算的能力，解決單個(gè)處理器難以保證濾波實(shí)時(shí)性的問題，需要研究與系統(tǒng)硬件相匹配的濾波器解耦并行實(shí)現(xiàn)技術(shù)。國內(nèi)外許多文獻(xiàn)都針對(duì) Kalman濾波解耦并行實(shí)現(xiàn)進(jìn)行過研究。文獻(xiàn)[1]研究了計(jì)算資源受限的多機(jī)器人系統(tǒng)中GPS-INS狀態(tài)估計(jì)問題，通過比較濾波器協(xié)方差矩陣非對(duì)角元素值的大小定性分析變量間的耦合關(guān)系，并進(jìn)一步使用相關(guān)系數(shù)定量分析變量間的耦合性強(qiáng)弱，從而將整體式濾波器進(jìn)行解耦。文獻(xiàn)[2]將MIMO控制系統(tǒng)中的動(dòng)態(tài)結(jié)構(gòu)耦合性分析方法應(yīng)用于導(dǎo)航系統(tǒng)，通過分析動(dòng)態(tài)直接增益矩陣對(duì)平臺(tái)式慣導(dǎo)初始對(duì)準(zhǔn)變量間的耦合度進(jìn)行分析，忽略系統(tǒng)水平回路交叉耦合項(xiàng)后，得到兩個(gè)解耦的子系統(tǒng)，從而降低了濾波器的維數(shù)。以上兩種方法雖然能夠減小濾波計(jì)算量，但由于忽略了一些變量間的耦合關(guān)系，本質(zhì)上都是次優(yōu)濾波，一定程度上會(huì)損失濾波精度。文獻(xiàn)[3]將敏感器的常值零偏項(xiàng)當(dāng)做偏差量處理，顯著加快了計(jì)算速度。該方法雖然能夠保證濾波的最優(yōu)性但只適用于偏差量較多的情況。文獻(xiàn)[4]使用逐次正交化法將整體式Kalman濾波器進(jìn)行分解，得到的分布式濾波方法僅對(duì)低階子系統(tǒng)進(jìn)行計(jì)算，不僅結(jié)果最優(yōu)，而且大大減少了濾波計(jì)算量，特別適合多處理器并行計(jì)算。

本文研究了方位捷聯(lián)平臺(tái)系統(tǒng)的誤差特性，在文獻(xiàn)[1]和文獻(xiàn)[4]工作的基礎(chǔ)上，對(duì)系統(tǒng)的初始對(duì)準(zhǔn)濾波器進(jìn)行設(shè)計(jì)，得到了適合在分布式架構(gòu)方位捷聯(lián)平臺(tái)重力儀上實(shí)現(xiàn)的濾波方案。

1 方位捷聯(lián)平臺(tái)系統(tǒng)的誤差方程

系統(tǒng)誤差模型的推導(dǎo)采用擾動(dòng)法，在靜基座條件下，可以得到地理坐標(biāo)系中誤差方程為：

水平角誤差方程：

航向角誤差方程：

速度誤差方程：

式中，φ為地理緯度，ieω為地球自轉(zhuǎn)角速度，α為方位角，xε、yε、zε為陀螺常值漂移項(xiàng)；ΔAx、ΔAy分別為水平加表常值零偏項(xiàng)。

2 Kalman濾波狀態(tài)方程和觀測(cè)方程

以速度誤差為觀測(cè)量，由以上誤差方程可以建立系統(tǒng)的狀態(tài)方程和觀測(cè)方程：

3 分布式濾波設(shè)計(jì)

第一步：依據(jù)觀測(cè)量的個(gè)數(shù)，通過比較相關(guān)系數(shù)大小[1]將系統(tǒng)劃分為兩個(gè)子系統(tǒng)。

待濾波器收斂后，得到協(xié)方差矩陣P。相關(guān)系數(shù)ijρ能夠表示狀態(tài)變量i、j間的耦合性強(qiáng)弱，ijρ表達(dá)式如式(8)所示：

計(jì)算各對(duì)狀態(tài)變量間的相關(guān)系數(shù)，經(jīng)比較分析后，得到子系統(tǒng)劃分方法為：作為子系統(tǒng)1的狀態(tài)變量，作為子系統(tǒng)2的狀態(tài)變量。這種劃分方法保證了兩個(gè)子系統(tǒng)具有大致相同的計(jì)算量，便于算法在不同節(jié)點(diǎn)上并行執(zhí)行。

第二步：逐次正交化分布式濾波。

將系統(tǒng)劃分為兩個(gè)子系統(tǒng)后，可以得到子系統(tǒng)1濾波狀態(tài)方程和觀測(cè)方程：

子系統(tǒng)2濾波狀態(tài)方程和觀測(cè)方程：

對(duì)文獻(xiàn)[4]中濾波算法進(jìn)行分析設(shè)計(jì)后，得到兩個(gè)子系統(tǒng)逐次正交化分布式濾波計(jì)算流程如圖1所示。

4 仿真及結(jié)果分析

4.1 仿真條件

仿真參數(shù)設(shè)置如表1所示。整體式濾波器參數(shù)設(shè)置為：

狀態(tài)協(xié)方差陣初始值：

系統(tǒng)噪聲協(xié)方差陣：

表1 仿真參數(shù)設(shè)置Tab.1 Parameters of simulation

觀測(cè)噪聲協(xié)方差陣：

分布式濾波器參數(shù)設(shè)置為：

狀態(tài)協(xié)方差陣初始值：

系統(tǒng)噪聲協(xié)方差陣：

觀測(cè)噪聲協(xié)方差陣：

4.2 仿真結(jié)果及分析

圖2～圖5給出了整體式濾波對(duì)準(zhǔn)與相同條件下分布式濾波對(duì)準(zhǔn)仿真曲線的對(duì)比，1200 s濾波精度及用時(shí)的量化比較如表2所示。

圖2 東向誤差角估計(jì)誤差Fig.2 Estimated error of eastern error angle

從圖2～圖5及表2可以分析出：

① 1200 s濾波結(jié)束時(shí)，兩種濾波方式狀態(tài)協(xié)方差陣Pii(k/k)對(duì)角線上元素基本相同，證明逐次正交化分布式濾波仍然是最優(yōu)濾波，表2中濾波估計(jì)值的對(duì)比也符合這一論斷；

② 分布式濾波用時(shí)只有整體式濾波的 60%，明顯地減小了運(yùn)算量，增強(qiáng)了濾波實(shí)時(shí)性。

圖3 北向誤差角估計(jì)誤差Fig.3 Estimated error of northern error angle

圖4 航向誤差角估計(jì)誤差Fig.4 Estimated error of heading error angle

圖5 平臺(tái)系y軸方向陀螺常值漂移Fig.5 Estimation ofy-axis gyroscope drift

表2 濾波精度及用時(shí)的量化比較Tab.2 Comparison of filtering accuracy and cost of time

5 結(jié) 論

通過對(duì)方位捷聯(lián)平臺(tái)重力儀初始對(duì)準(zhǔn)濾波器進(jìn)行逐次正交化設(shè)計(jì)，將整體式濾波器分解成兩個(gè)維數(shù)較低的子濾波器。仿真結(jié)果證明：逐次正交化分布式濾波在保持濾波最優(yōu)性的同時(shí)大大減小了計(jì)算量，適合在分布式架構(gòu)重力儀上實(shí)現(xiàn)，并且系統(tǒng)階次越高，分割子系統(tǒng)就越多，這種優(yōu)勢(shì)越明顯。下一步可以研究分布式濾波技術(shù)在重力匹配組合導(dǎo)航系統(tǒng)中的應(yīng)用，解決組合導(dǎo)航系統(tǒng)的濾波非實(shí)時(shí)性問題。

（References）:

[1]Wachter L M,Ray L E.GPS-INS state estimation for multi-robot systems with computational resource constrains[C]//2009 American Control Conference.2009: 561-567.

[2]郝穎，談?wù)穹顤|明.動(dòng)態(tài)結(jié)構(gòu)耦合性分析方法在導(dǎo)航系統(tǒng)中的應(yīng)用[J].中國慣性技術(shù)學(xué)報(bào)，2005，13(3)：26-29.HAO Ying,TAN Zhen-fan,Li Dong-ming.Application of dynamic measurement of structural interaction in navigation system[J].Journal of Chinese Inertial Technology,2005,13(3): 26-29.

[3]柳明，向禮，劉雨，等.一種新的慣導(dǎo)平臺(tái)靜基座快速初始對(duì)準(zhǔn)方法[J].上海航天，2010(5)：51-54.LIU Ming,XIANG Li,LIU Yu,et al.A new fast inertial alignment method of inertial navigation platform on stationary base[J].Aerospace Shanghai,2010(5): 51-54.

[4]Hassan M,Salut G,Sigh M,et al.A decentralized algorithm for the global Kalman filter[J].IEEE Transactions on Automatic Control,1978,23(2): 262-267.

[5]Weihua Ma,Jianjun Luo,Jianping Yuan,et al.SINS/GPS/SS reduced-order Kalman filter design based state decoupling for space application[C]//4thIEEE Conference on Industrial Electronics And Applications.2009: 2596-2600.

[6]FU Qiang-wen,QIN Yong-yuan,LI Si-hai,et al.Optimal two-position alignment method based on parameter identification[J].Journal of Chinese Inertial Technology,2013,21(4): 430-434.

[7]Takaya K.Transputer-like multicore parallel processing on the array of arm Cortex-M0 microprocessors[C]//2012 25thIEEE Canadian Conference on Electrical and Computer Engineering.2012: 1433-1437.

[8]Yang Chao,Wu Junfeng,Zhang Wei,et al.Schedule communication for decentralized state estimation[J].IEEE Transactions on signal processing,2013,61(10): 2525-2536.

[9]Wang J H,Chen J B.Adaptive unscented Kalman filter for initial alignment of strapdown inertial navigation system[C]//2010 International Conference on Machine Learning and Cybernetics.2010,Vol.3: 1384-1389.

[10]Oruc S,Sijs J.Optimal decentralized Kalman filter[C]//2009 17thMediterranean Conference on Control &Automation.Thessalonlkl,Greece,2009: 803-809.

Distributed Kalman filter initial alignment algorithm for azimuth strapdown platform gravimeter

YANG Ye1,WU Xing-tao1,YANG Jian-lin2,GAO Wei1,PEI Zhi1
(1.Tianjin Navigation Instrument Research Institute,Tianjin 300131,China;2.Tianjin Military Representative Office,Tianjin 300131,China)

A distributed Kalman filter alignment algorithm is developed in order to use the parallel computing ability of the distributed architecture gravimeter to solve the non real-time implementation of filtering based on one single processor.Firstly,the error equations of the azimuth strapdown platform gravimeter are deduced,and the state equations and observation equations are built.Secondly,an error covariance analytical method is applied to the filtering equations,and the system is decentralised into two subsystems with the same dimension.In this way we get the initial alignment filter formed by the two subfilters.Finally,the azimuth strapdown platform model is built by using Matlab,and stationary base alignment is implemented by using global Kalman filter and distributed Kalman filter separately.The simulation results show that the distributed filter has the same filtering accuracy and costs only 60% of time compared with the global one,which is favorable to ensure the real-time performance of the algorithm.

azimuth strapdown platform; distributed Kalman filter; orthogonalization; initial alignment

U666.1

：A

1005-6734(2014)02-0191-04

10.13695/j.cnki.12-1222/o3.2014.02.009

2013-12-20；

：2014-01-27

國家“863”計(jì)劃課題（2011AA060501）

楊曄（1968—）男，研究員，碩士生導(dǎo)師，從事導(dǎo)航、制導(dǎo)與控制系統(tǒng)研究。E-mail：liuyuyangye@eyou.com