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

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

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

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

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

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

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

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

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

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

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

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

水平角誤差方程：

航向角誤差方程：

速度誤差方程：

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

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

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

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

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

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

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

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

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

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

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

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

4.1 仿真條件

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

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

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

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

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

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

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

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

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

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

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

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

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

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

② 分布式濾波用時只有整體式濾波的 60%，明顯地減小了運算量，增強了濾波實時性。

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

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

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

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

5 結(jié) 論

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

（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ù)穹?，李東明.動態(tài)結(jié)構(gòu)耦合性分析方法在導(dǎo)航系統(tǒng)中的應(yīng)用[J].中國慣性技術(shù)學(xué)報，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)平臺靜基座快速初始對準方法[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”計劃課題（2011AA060501）

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