張怡，趙凱華，姚斌

西北工業(yè)大學電子信息學院，西安 710072

變增益聯(lián)邦KF組合導航定位算法研究

張怡，趙凱華，姚斌

西北工業(yè)大學電子信息學院，西安 710072

1 引言

GPS導航系統(tǒng)雖然具有定位精度高，誤差不隨時間累積，良好的通用性等優(yōu)點。但是易受到外界的干擾以及多徑效應的影響，使得GPS系統(tǒng)在某些信號強度低、干擾大的場合導航效果表現(xiàn)的并不理想。INS導航系統(tǒng)可以很好地解決這個問題，但INS導航系統(tǒng)的誤差會隨時間積累。因此，用INS輔助GPS進行導航定位越來越受到青睞，INS/GPS組合后可以大大提高系統(tǒng)的綜合性能，適應各種復雜環(huán)境下的定位導航[1-3]，并且緊耦合組合方式在整體性能上較松耦合組合方式有較大提高[4]。

濾波算法在衛(wèi)星導航定位中發(fā)揮著非常大的作用，針對GPS/INS組合導航系統(tǒng)導航定位算法，已有大量研究工作[5-7]。在傳統(tǒng)的GPS/INS緊耦合組合導航系統(tǒng)中，由于偽距和多普勒頻移誤差的存在，系統(tǒng)存在一定的誤差偏移。針對這種誤差偏移，為進一步提高GPS/INS緊耦合組合導航算法的定位精度，以聯(lián)邦卡爾曼濾波為基礎(chǔ)的GPS/INS緊耦合組合導航定位算法，針對偽距測量誤差對GPS/INS組合導航系統(tǒng)定位精度的影響，對偽距進行預濾波，提高了定位精度[8]。

傳統(tǒng)的卡爾曼濾波是無限增長記憶的濾波，某時刻的最優(yōu)估計要用到該時刻以前的觀測數(shù)據(jù)。因此，隨著步數(shù)的增大，濾波值中的老數(shù)據(jù)比重太大，而新數(shù)據(jù)比重太小，當系統(tǒng)存在模型誤差和計算誤差時，系統(tǒng)預測誤差方差陣逐漸趨于零，新的觀測數(shù)據(jù)對修正狀態(tài)估計的作用太小，不能有效地抑制誤差對狀態(tài)估計值的影響，從而導致濾波發(fā)散。為解決這個問題，F(xiàn)agin與Sorenson等人提出了衰減記憶濾波算法，該算法通過引入衰減因子限制卡爾曼濾波器的記憶長度，提高新量測值的利用權(quán)重從而抑制濾波發(fā)散[9]。

聯(lián)邦卡爾曼濾波算法雖然提高了定位精度，隨著濾波步數(shù)的增加，同樣存在發(fā)散問題。本文研究了一種改進的聯(lián)邦卡爾曼濾波算法，通過深入研究衰減記憶濾波算法，引入一種可變加權(quán)系數(shù)來持續(xù)地增加誤差狀態(tài)估計中新量測值的修正作用。仿真表明，該算法能夠進一步提高組合導航系統(tǒng)的定位精度，有效抑制了濾波的發(fā)散作用，對工程應用有一定參考價值。

2 變增益聯(lián)邦卡爾曼濾波器設(shè)計

2.1 濾波器結(jié)構(gòu)設(shè)計

組合導航系統(tǒng)的主要誤差來源就是GPS接收模塊解算出的偽距誤差，為削弱該誤差帶來的影響，運用一種二級卡爾曼濾波器進行導航定位解。濾波器結(jié)構(gòu)如圖1所示，在數(shù)據(jù)送到主卡爾曼濾波器進行融合前先對偽距進行預濾波，削弱偽距誤差對整個系統(tǒng)定位精度的影響[8]。由于INS系統(tǒng)短時間定位精度較高，為降低整個系統(tǒng)的復雜程度，不對慣導信息進行預濾波。在完成預濾波之后，將經(jīng)過預濾波的偽距信息和由INS得到的慣導信息導入到主濾波器中，兩路數(shù)據(jù)在主濾波器中進行信息數(shù)據(jù)融合。

圖1 聯(lián)邦卡爾曼濾波器結(jié)構(gòu)圖

2.2 狀態(tài)方程的建立

為了全面反映載體運動狀態(tài)的變化，并充分考慮載體運動中的位置、速度、加速度等，狀態(tài)向量設(shè)為：

式中，δr為位置誤差矢量，δν為速度誤差矢量，δε為姿態(tài)角誤差，δω為陀螺儀零偏，δa為加速度計零偏；假設(shè)δω、δa為常值偏差。

系統(tǒng)狀態(tài)誤差方程為[10]：

式中，F(xiàn)i是比力向量fi的反對稱矩陣，代表由載體坐標系到慣性坐標系的方向余弦矩陣，Ni代表引力系數(shù)矩陣。

將該連續(xù)的狀態(tài)方程離散化，即可得到離散系統(tǒng)的卡爾曼濾波狀態(tài)方程：

2.3 量測方程的建立

在系統(tǒng)量測方程中，若直接采用偽距與多普勒頻移測量殘余作為觀測量，則觀測量中存在著本地時鐘與衛(wèi)星時鐘的鐘差。因此，將相對于不同衛(wèi)星得到的偽距與多普勒頻移測量殘余相減[11]，得到偽距與多普勒頻移單差：

2.4 主卡爾曼濾波器濾波方程的建立

在主濾波器中，采用應用卡爾曼濾波算法作為數(shù)據(jù)融合算法。卡爾曼濾波首先利用上一個狀態(tài)的狀態(tài)量，通過狀態(tài)方程估計出這一個狀態(tài)下量測量的值，完成預測過程，然后利用預測過程得到的信息量以及系統(tǒng)的觀測量即可完成卡爾曼濾波的校正過程[11]，濾波方程為：

理論上，聯(lián)邦卡爾曼濾波隨著時間的推移，觀測數(shù)據(jù)數(shù)量不斷增多，濾波估計的精度應當越來越高。但是，在實際工程應用中，系統(tǒng)模型描述和噪聲統(tǒng)計特性的誤差會使得系統(tǒng)預測誤差方差陣隨著濾波時間的推移逐漸趨于零，從而狀態(tài)估計會過分依賴舊量測值，新量測值對預測值的修正作用下降，導致濾波發(fā)散，影響系統(tǒng)定位精度。因此，可以通過逐漸降低舊量測值的權(quán)值，相應增大新量測值的權(quán)值，從而抑制濾波的發(fā)散。

式（10）中，增益矩陣取時刻k=N，則：

經(jīng)分析，逐漸減小N時刻以前的KN值，相對地突出KN，便能在一定程度上抑制濾泡發(fā)散，考慮增大Pk及P0的值來減小Zi(i＜N)及對的影響。在Pk/k-1方程中引入可變權(quán)值系數(shù)：(1-sk)/(1-s)，(0＜s＜1)，可以看出隨k值增大，權(quán)值系數(shù)會不斷增大并大于1，這樣便可保證對新量測值的利用，于是Pk/k-1方程可以改寫為[12]：

本文采用偽距與多普勒頻移測量殘余為觀測量，量測方程為非線性方程，需要采用擴展卡爾曼濾波的方法來進行濾波。改進后的變增益聯(lián)邦卡爾曼主濾波器解算過程如下所示[13-15]。

3 仿真結(jié)果與分析

為驗證變增益聯(lián)邦卡爾曼濾波算法相對于原始聯(lián)邦卡爾曼濾波算法對導航系統(tǒng)定位精度的改善效果，本文采用Matlab進行一組仿真測試，初始位置設(shè)為：（1 545 203.817 775 705，4 245 412.597 793 822，4 487 631.251 578 394），初始速度為（0，0，0），初始方向余弦矩陣為{1 0 0；0 1 0；0 0 1}。

載體軌跡仿真如圖2所示。由圖2可知，模擬載體經(jīng)歷了加速、爬升、滾動、轉(zhuǎn)彎等多個過程，涵蓋了現(xiàn)有大部分的運動狀態(tài)。

圖2 載體仿真軌跡

原始組合導航算法的位置誤差和速度誤差仿真如圖3所示；原始聯(lián)邦卡爾曼濾波算法與改進后的聯(lián)邦卡爾曼濾波算法分別應用在GPS/INS緊耦合組合導航系統(tǒng)后，對于位置誤差和速度誤差的仿真如圖4和圖5所示。

圖3 原始組合導航位置和速度誤差

圖4 位置誤差對比

圖5 速度誤差對比

從圖4可以明顯看出，在X、Y、Z軸方向上位置偏差均有一定的減小，Z軸方向上的位置偏差減小量最為明顯，這說明聯(lián)邦卡爾曼濾波算法利用緊耦合方法計算偽距與多普勒頻移單差可以有效提高組合導航系統(tǒng)的整體性能，其中Z軸上的改善更加明顯。并且還可以看出，改進后的變增益聯(lián)邦卡爾曼濾波算法位置誤差減小量更大，說明改進的變增益卡爾曼濾波算法在加大對新量測值的利用權(quán)值后，能有效抑制系統(tǒng)的發(fā)散，可以進一步提高系統(tǒng)的定位精度。

從圖5可以明顯看出，兩種聯(lián)邦卡爾曼濾波算法均能有效減小速度誤差，并且本文的變增益聯(lián)邦卡爾曼濾波算法在速度定位上的誤差相對于傳原始的聯(lián)邦卡爾曼濾波算法有了一定的提高。另外，從圖中速度誤差的偏移還可以看出，當載體位置出現(xiàn)較大變化時，系統(tǒng)可以很快速地修正這個速度偏差。這主要是由于本文方法在主濾波器之前，已經(jīng)完成了修正量的計算，大大減小了運算量，因此可以快速地修正速度誤差偏移。

4 結(jié)束語

提出了一種變增益聯(lián)邦卡爾曼濾波算法，以二級聯(lián)邦卡爾曼濾波器為基礎(chǔ)，通過研究衰減卡爾曼濾波算法，引入變增益加權(quán)系數(shù)。針對偽距測量誤差對于GPS/INS組合導航系統(tǒng)定位精度的影響，利用子濾波器對GPS接收模塊解算出的偽距數(shù)據(jù)先進行預濾波，然后在主濾波器數(shù)據(jù)融合過程中引進變增益加權(quán)系數(shù)，加大新量測值對預測值的修正作用。本文算法吸收了聯(lián)邦卡爾曼濾波算法的優(yōu)點，考慮了最新量測值。仿真結(jié)果表明，該算法能有效改善濾波性能，提高導航系統(tǒng)的定位精度，抑制濾波發(fā)散。

[1]Li Yong，Wang Jinling.Low-cost tightly coupled GPS/INS integration based on a nonlinear Kalman filtering design[C]// Proceedings of IONNTM Conference，Monterey，CA，2006.

[2]Kim K，Park C G.INS/GPS tightly coupled approach using an INS error predictor[C]//Proc of GNSS 18th International Technical Meeting of the Satellite Division，Long Beach，CA，2005.

[3]Dikshit VG，MahapatraP R.Medium-coupledbusbased INS/GPS sensor fusion for accurate and reliable positioning[C]// Proceedings of ESAV Conference，Capri，Italy，2008.

[4]趙思浩，陸明泉，馮振明.EKF與UKF在緊耦合組合導航系統(tǒng)中的應用[J].系統(tǒng)工程與電子技術(shù)，2009，31（10）：2450-2454.

[5]向禮.非線性濾波方法及其在導航中的應用研究[D].哈爾濱：哈爾濱工業(yè)大學，2009.

[6]楊柳慶，肖前貴，牛妍.基于漸消卡爾曼濾波器的定位系統(tǒng)設(shè)計[J].南京航空航天大學學報，2012，44（1）：134-138.

[7]宋清昆，劉小磊.神經(jīng)網(wǎng)絡修正動態(tài)GPS卡爾曼濾波算法研究[J].計算機工程與應用，2011，47（15）：152-155.

[8]何偉，廉保旺，馮曉明.基于聯(lián)邦卡爾曼濾波的GPS/INS緊耦合組合導航定位算法[J].火力與指揮控制，2012，36（8）：149-152.

[9]李楊，胡柏青.基于改進衰減記憶濾波的組合導航算法研究[J].計算機仿真，2012，29（3）：117-119.

[10]董緒榮，張守信，華仲春.GPS/INS組合導航定位及其應用[M].長沙：國防科學技術(shù)大學出版社，1998：245-248.

[11]謝剛.GPS原理與接收機設(shè)計[M].北京：電子工業(yè)出版社，2009.

[12]王虎，王解先，白貴霞.改進的漸消卡爾曼濾波在GPS動態(tài)定位中的應用[J].同濟大學學報，2011，39（1）：124-127.

[13]吳志華，丁楊斌，申功勛.改進的非線性魯棒EKF算法及其應用[J].計算機工程與應用，2011，47（3）：207-209.

[14]Guo L，Ljung L.Exponential stability of general tracking algorithms[J].IEEE Tran on Autom Contr，1995，40：1376-1387.

[15]Lippiello V，Siciliano B，Villani L.Adaptive extended Kalman filtering for visual motion estimation of 3D objects[J].Control Engineering Practice，2007，15：123-134.

ZHANG Yi,ZHAO Kaihua,YAO Bin

School of Electronics and Information,Northwestern Polychechnical University,Xi’an 710072,China

The pseudorange error worked out by the GPS receiver module is the major errors of GPS/INS navigation system, which can be weakened by using one federal Kalman filter algorithm of integrated navigation.This algorithm has two Kalman filters.The first Kalman filter filters the pseudo range and Doppler shift got from the GPS module.Then,the results got from the first Kalman filter is filtered by the second Kalman filter with the data got from the INS module.And it gets the correction and the optimum estimate.With the filter step increasing,the system prediction error variance tends to zero gradually,then the state estimation will excessively dependence on the old measurements,which will cause filter divergence and affect the position accuracy. To effectively increase the correction amount of the new measurements,this paper introduces a variable weighting factor in federal Kalman filter integrated navigation algorithm.The simulation results show that the improved variable gain federal Kalman filtering algorithm has the advantages of federal Kalman filter,and the filtering effect of this algorithm is improved obviously,which can effectively restrain the filter divergence and improve the positioning precision of the system.

federation Kalman filter;variable gain;location algorithm;accuracy;divergence

GPS接收模塊解算出的偽距誤差是GPS/INS組合導航系統(tǒng)的主要誤差，采用一種二級聯(lián)邦卡爾曼濾波組合導航算法加以削弱，將衛(wèi)星接收模塊解算出的偽距信息和多普勒頻移信息在第一級卡爾曼濾波后，再通過主濾波器與INS模塊解算出的信息進行修正處理，得到校正量和定位位置最優(yōu)估計。隨著濾波步數(shù)增加，系統(tǒng)預測誤差方差陣逐漸趨于零，狀態(tài)估計會過分依賴舊量測值，從而導致濾波發(fā)散，影響系統(tǒng)定位精度。為有效提高新量測值的修正作用，在聯(lián)邦卡爾曼濾波組合導航算法中引入一種可變加權(quán)系數(shù)。仿真結(jié)果表明，改進后的變增益聯(lián)邦卡爾曼濾波算法具備聯(lián)邦卡爾曼濾波的優(yōu)點，并且該算法濾波效果有較明顯的改善，能有效抑制濾波發(fā)散，提高系統(tǒng)的定位精度。

聯(lián)邦卡爾曼濾波；變增益；定位算法；精度；發(fā)散

A

TP301.6

10.3778/j.issn.1002-8331.1304-0099

ZHANG Yi,ZHAO Kaihua,YAO Bin.Research on variable gain federation Kalman filter location algorithm of coupled system.Computer Engineering and Applications,2013,49（19）：196-199.

國家自然科學基金（No.61202394）；西北工業(yè)大學研究生創(chuàng)業(yè)種子基金（No.Z2013095）。

張怡（1958—），女，教授，主要研究方向為通信、導航、遙測、遙控系統(tǒng)的信息傳輸與處理，多媒體通信與計算機通信；趙凱華（1988—），男，碩士，主要研究方向為GPS的導航與定位。E-mail：huakai051838@163.com

2013-04-08

2013-06-01

1002-8331（2013）19-0196-04

CNKI出版日期：2013-06-08http://www.cnki.net/kcms/detail/11.2127.TP.20130608.1001.028.html