王延東，賈宏光(中國科學院長春光學精密機械與物理研究所，長春130033)

組合導航系統(tǒng)濾波器截斷誤差抑制方法

王延東*，賈宏光
(中國科學院長春光學精密機械與物理研究所，長春130033)

組合導航系統(tǒng)作為重要的定位和姿態(tài)測量的技術(shù)手段，其基本設(shè)計思想是將GPS和SINS等導航設(shè)備輸出的信息經(jīng)過濾波器進行最優(yōu)估計。但在采用Riccati方程更新協(xié)方差矩陣和計算Kalman增益過程中，截斷誤差隨著迭代次數(shù)的增大而累積，破壞協(xié)方差矩陣的正定性和對稱性，降低濾波器計算的數(shù)值穩(wěn)定性，嚴重時導致組合系統(tǒng)故障發(fā)散。本文建立了Riccati方程一階誤差模型，從理論上分析截斷誤差對濾波器估計性能的影響，引入基于Bierman算法和Thorton算法的Kalman濾波器進行更新方法，解決了截斷誤差引起的濾波器數(shù)值穩(wěn)定性的問題。通過強實時半物理仿真系統(tǒng)驗證表明，相比于基于Kalman濾波器的系統(tǒng)，基于Bierman-Thorton算法的組合導航系統(tǒng)有更強的數(shù)值穩(wěn)定性和較高的導航精度。

組合導航;Bierman-Thorton算法;半物理仿真;截斷誤差;Kalman濾波器

GPS/SINS組合導航系統(tǒng)系統(tǒng)采用最優(yōu)估計技術(shù)，使各傳感器發(fā)揮各自優(yōu)勢，提高了系統(tǒng)的導航精度，降低了系統(tǒng)，廣泛應(yīng)用于地面車輛、航空、航天和航海等領(lǐng)域。組合導航系統(tǒng)中，由于各狀態(tài)變量及其初始對準估計誤差的數(shù)值范圍較大，導致其協(xié)方差矩陣通常為病態(tài)矩陣，而截斷誤差作為較小的擾動極易破壞其數(shù)值穩(wěn)定性，甚至造成濾波器發(fā)散。并且組合導航系統(tǒng)濾波器、矩陣維數(shù)大、反復的矩陣乘法和求逆運算，都會引起較大的截斷誤差。因此在組合導航系統(tǒng)中減小截斷誤差就非常重要。

組合導航系統(tǒng)中無論采用線性Kalman濾波器，還是EKF等非線性濾波器，都須對Riccati方程進行迭代運算［1］，本文探討了Riccati方程更新過程中的共性問題。前人針對以上問題提出了許多數(shù)值計算方法，如Swerling求逆公式，Potter求逆算法，Joseph穩(wěn)定算法，信息濾波器，Calson-Schmidt平方根算法和Bierman-Thorton算法，參見文獻［1］。本文對比了截斷誤差引起的協(xié)方差矩陣誤差的均方根，說明了基于UD分解的Bierman-Thorton量測更新和時間更新算法，對減小舍入誤差引起的數(shù)值計算不穩(wěn)定，抑制系統(tǒng)發(fā)散，減小計算量有明顯的作用。并以某制導武器組合導航系統(tǒng)進行半物理仿真，分析Bierman-Thorton算法的在抑制濾波器抑制截斷誤差的作用［2］。

1 組合導航系統(tǒng)算法

組合導航系統(tǒng)的本質(zhì)是應(yīng)用信息融合技術(shù)，特別是濾波技術(shù)，對多傳感器輸出的信息量進行最優(yōu)估計的過程。在組合導航系統(tǒng)的設(shè)計過程中，首先建立組合導航系統(tǒng)的狀態(tài)方程和量測方程;采用Kalman等濾波器對系統(tǒng)狀態(tài)進行最優(yōu)估計，從噪聲中估計出狀態(tài)變量的最優(yōu)值;利用這些狀態(tài)估計值修正系統(tǒng)誤差，進而得到準確的導航狀態(tài)，達到提高定位和測姿精度的目的。

1.1 組合導航模型

本文的組合導航系統(tǒng)采用SINS誤差量和慣性器件誤差量作為狀態(tài)變量，為了使系統(tǒng)方程為線性方程，SINS誤差忽略了二階及以上的誤差小量，參見文獻［3］。慣性器件應(yīng)先根據(jù)慣性器件模型進行標定，補償大部分的系統(tǒng)誤差，而后對隨機誤差進行辨識和建模。組合導航系統(tǒng)原理，見圖1［3-4］。

圖1 組合導航系統(tǒng)原理圖

1.2 組合導航系統(tǒng)Kalman濾波計算方法

卡爾曼濾波器的實質(zhì)是基于最小方差的估計算法，利用確定性和隨機性的先驗信息，通過初始值不

由以上分析，組合導航系統(tǒng)狀態(tài)向量為:

其中:δP表示當?shù)氐乩硐碌奈恢谜`差，分別為緯度誤差、經(jīng)度誤差和高度誤差;δV表示當?shù)氐乩碜鴺讼迪碌娜S速度誤差;δΦ表示載體系相對于當?shù)氐乩碜鴺讼档恼`差角;δBg表示三軸陀螺的零偏; δBa表示三軸加速度計的零偏。

則組合導航連續(xù)系統(tǒng)的狀態(tài)方程為:其中:F為組合導航系統(tǒng)狀態(tài)矩陣;G為噪聲驅(qū)動矩陣;w為系統(tǒng)噪聲。

系統(tǒng)噪聲w由陀螺零偏穩(wěn)定性wgb和白噪聲wgr、加速度計零偏穩(wěn)定性wab和白噪聲war組成，見式(3)。

系統(tǒng)的量測方程為式(4)，量測值為GPS與SINS二者輸出的速度位置之差，見式(5)。斷的遞推，得出最小方差的估計值?？柭鼮V波器5個要素包含:式(1)狀態(tài)方程;式(6)系統(tǒng)噪聲方差矩陣;式(8)協(xié)方差矩陣;式(5)量測方程;式(8)量測噪聲方差矩陣。

Riccati方程的迭代計算完成的是協(xié)方差矩陣P和Kalman增益K的更新，見式(9)～(11)。式(9)為協(xié)方差矩陣的時間更新，式(10)為Kalman增益的計算，式(11)為協(xié)方差矩陣的量測更新［5］。

在完成P的更新和K計算后，計算組合導航系統(tǒng)誤差修正量，閉環(huán)修正系統(tǒng)誤差修正量見式(13)，并以此校正SINS解算值和陀螺輸出值，見式(13)。

1 截斷誤差對組合導航系統(tǒng)卡爾曼濾波器的影響

2.1 病態(tài)矩陣和矩陣的條件數(shù)

在數(shù)值計算中，當矩陣為“病態(tài)”時，數(shù)據(jù)存在微小擾動(誤差)時，引起的輸出數(shù)據(jù)(問題解)相對誤差很大，該矩陣即為病態(tài)矩陣。矩陣病態(tài)特性以矩陣的條件數(shù)表示。

設(shè)A為非奇異陣，cond(A)υ為矩陣的條件數(shù)，見式(14)。

根據(jù)定義可知cond(A)≥1;當狀態(tài)數(shù)值cond(A)接近∞時，A為奇異矩陣。cond(A)越接近于1，狀態(tài)越好［4］。

2.2 截斷誤差對卡爾曼濾波器的影響

理論上在Kalman濾波中，Riccati方程協(xié)方差矩陣應(yīng)與實際系統(tǒng)估計不確定性相同，如果二者存在差異，則認為存在“病態(tài)”問題。但是，在組合導航系統(tǒng)工程實現(xiàn)中存在許多問題可能引起協(xié)方差矩陣的“病態(tài)”。具體地說，①當建模不準確時，特別是對于采用系統(tǒng)誤差量作為狀態(tài)變量時(忽略了二階小量)，導致理論模型與實際系統(tǒng)存在差異;②當濾波器選取的狀態(tài)變量的方差估計值的數(shù)值范圍相差較大;③式(16)中矩陣［HPHT+R］求逆計算;④數(shù)值計算的截斷誤差;⑤矩陣維數(shù)過大;⑥處理器的計算精度較低等。這些情況中，一些是不能解決的，一些是能夠避免的。例如對慣性器件進行精密的標定，采用導航輸出值作為狀態(tài)變量并采用非線性濾波器進行估計能夠一定程度地避免建模不準確的問題;矩陣求逆和大維數(shù)問題通過將稀疏矩陣分塊計算，向量計算轉(zhuǎn)化為標量的序貫處理方法解決;截斷誤差可采用對正定對稱的協(xié)方差矩陣進行分解，并對分解后的三角矩陣和對角矩陣分別進行更新的方法克服［6］。例如，將P進行Cholesky分解，見式(15)，對矩陣C進行更新計算，這個思想即為平方根濾波器的基本思想。

考慮截斷誤差在Kalman濾波器傳播方式，見式(9)。協(xié)方差矩陣P的一階傳播模型可寫成式(16)。其中，δ項表示為累積誤差，Δ表示為本次迭代引入的截斷誤差，f1為δPk(-)的一階函數(shù)。

經(jīng)過量測更新后的δPk+1(+)為式(17)。

其中，A1=Φ-KkH。

平方根濾波器對P進行Cholesky，將分解后的矩陣進行量測更新和時間更新。平方根濾波器的截斷誤差一階傳播過程見式(18)。

通過與式(17)對比看出，式(24)減少了對稱性誤差Φ(δPk(-)-δ(-))ΦT和非對稱誤差Φ (δPk(-)-δ(-)的引入。

不同的Kalman濾波器數(shù)值計算方法中的截斷誤差對協(xié)方差矩陣估計精度的影響不同，見圖2。從圖2中看出，Bierman與Calson量測更新算法在截斷誤差為小數(shù)點后9位時仍能得到較高的計算精度。

式(18)與式(17)相比說明平方根濾波器在抑

圖2 Kalman濾波量測更新算法截斷誤差對協(xié)方差矩陣的影響

3 Bierman-Thorton算法基本原理

制截斷誤差在Riccati方程中傳播具有明顯的優(yōu)勢。基于改進Cholesky分解的量測更新和時間更新方法的稱為改進的平方根濾波器或稱為UD濾波器［7-8］。

3.1 改進Cholesky分解

對于一個正定對稱矩陣能夠分解為成式(19)的形式，其中U為上三角矩陣，D為對角矩陣，矩陣對角線元素為非零值。

3.2 過程噪聲方差矩陣的對角化

在Thorton-Bierman算法中，須對過程噪聲進行對角化處理。對角化方法即為改進Cholesky分解，見式(20)。

3.3 Bierman量測更新

Bierman算法完成的Riccati方程的量測更新，利用序貫處理的基本思想，設(shè)h為量測矩陣H的任意一維行向量，r為量測噪聲R的對角元素，則協(xié)方差矩陣本次更新見式(21)和式(22)［9］。

3.4 Thorton時間更新

Thorton算法完成的是協(xié)方差矩陣的時間更新，Thorton時間更新算法也稱作改進的加權(quán)Gram-Schmidt(MWGS)算法，相較于標準Gram-Schmidt算法有更強的數(shù)值穩(wěn)定性。按照式(29)對系統(tǒng)噪聲矩陣GkQkGk進行對角化處理，并定義矩陣A和Dw，見式(23)和式(24)。

采用加權(quán)Gram-Schmit正交化的思想，對A進行分解:

則LT和Dβ為時間更新后的上三角矩陣和對角矩陣，見(25)～(26)。

根據(jù)以上分析，基于Bierman-Thorton算法的組合導航系統(tǒng)設(shè)計過程見圖3。

圖3 UD濾波器組合導航系統(tǒng)原理圖

4 組合導航系統(tǒng)半物理仿真

4.1半物理仿真系統(tǒng)

組合導航半物理仿真原理如圖4所示。半物理仿真也稱硬件在回路仿真，通過仿真機生成數(shù)字彈道，作用于仿真設(shè)備，仿真設(shè)備按照輸入為被測對象提供物理效應(yīng)，被測對象通過其上的傳感器感知物理效應(yīng)，按照設(shè)計的算法輸出仿真結(jié)果［8］。

圖4 組合導航半物理仿真原理圖

組合導航系統(tǒng)半物理仿真系統(tǒng)由軌跡仿真機、三軸轉(zhuǎn)臺、GPS模擬器組成和數(shù)據(jù)顯示和記錄計算機組成。為保證仿真的強實時性，在VxWorks實時系統(tǒng)下開發(fā)，運行軌跡生成器，通過光纖反射內(nèi)存與三軸仿真轉(zhuǎn)臺進行通信，通信內(nèi)容為當前角位置;組合導航系統(tǒng)中的三軸陀螺固聯(lián)安裝在仿真轉(zhuǎn)臺上，輸出角速度至導航計算機;仿真機根據(jù)加速度計標定后的誤差模型，通過422串口按照加速度計協(xié)議將比力發(fā)送至導航計算機;仿真機計算當前地球坐標系的位置、速度、加速度、角位置、角速度和角加速度通過TCP/IP協(xié)議發(fā)送至GPS模擬器，GPS模擬器根據(jù)以上信息計算當前位置和時間計算生成GPS衛(wèi)星的射頻信號，將射頻信號輸入至組合導航系統(tǒng)中GPS接收機的接收端，GPS接收機將解算的位置和速度發(fā)送至導航計算機，完成組合導航計算，參見文獻［10-11］。

根據(jù)Bierman-Thorton算法，在嵌入式系統(tǒng)中進行工程實現(xiàn)。嵌入式處理器采用TI公司的TMS320F28335浮點處理器，陀螺選擇美國AD公司生產(chǎn)的ADIS16136三軸MEMS陀螺，性能指標見表1;加速度計選擇COLBRYS公司生產(chǎn)的MEMS加速度計MS8000-10加速度計，性能指標見表2;GNSS接收機選擇東方聯(lián)星公司生產(chǎn)的CNS50 GPS接收機，性能指標見表3，參加文獻［12-13］。

表1 ADIS16136陀螺關(guān)鍵指標

表2 MS9000-50加速度計關(guān)鍵指標

表3 CNS50 GPS接收機關(guān)鍵指標

4.2半物理仿真結(jié)果

根據(jù)以上基于Bierman-Thorton的組合導航系統(tǒng)算法，對其進行工程實現(xiàn)，并進行半物理仿真，仿真結(jié)果見圖5～圖8和表4。

圖5 仿真基準彈道

圖6 位置誤差

圖7 速度誤差

圖8 姿態(tài)角誤差

表4 自由慣導系統(tǒng)、Kalman濾波器和改進平方根濾波器半物理仿真結(jié)果對比

從仿真結(jié)果看出，相對于自由慣導系統(tǒng)，組合導航系統(tǒng)精度明顯提高。改進平方根濾波器與傳統(tǒng)Kalman濾波器相比，由于截斷誤差影響較小，姿態(tài)解算精度優(yōu)于Kalman濾波器。位置和速度的解算精度相當。

根據(jù)式(20)，對比Kalman濾波器和改進平方根濾波器的協(xié)方差矩陣的條件數(shù)，取矩陣的∞范數(shù)，見圖9。由于狀態(tài)變量之間的估計誤差差異較大，協(xié)方差矩陣都為病態(tài)矩陣，但結(jié)果表明，基于改進平方根濾波器的條件數(shù)小于Kalman濾波器一個量級，說明基于改進平方根濾波器數(shù)值穩(wěn)定性優(yōu)越。

圖9 改進平方根濾波器與Kalman濾波器組合導航系統(tǒng)協(xié)方差矩陣條件數(shù)

5 結(jié)論

本文介紹了組合導航系統(tǒng)的設(shè)計方法，論證了截斷誤差在Kalman濾波器Riccati方程迭代運算中的傳播的方式;并提出了抑制截斷誤差的方法，即Bierman-Thorton量測更新和時間更新算法。通過半物理仿真驗證，基于改進平方根濾波器的組合導航系統(tǒng)具有數(shù)值穩(wěn)定性強，導航精度高的優(yōu)勢。文中對截斷誤差對導航精度和可靠性進行了系統(tǒng)分析，但對于組合導航系統(tǒng)，影響其精度和可靠性的誤差源還包括粗大誤差，傳感器測量野值等重要因素，這些因此也能夠以適當數(shù)值算法克服，因此在解決截斷誤差的組合導航系統(tǒng)的同時，也需深入研究粗大誤差，傳感器測量野值等誤差機理和解決方法。

［1］Brown R G，Hwang P Y C.Introduction to Random Signals and Applied Kalman Filtering［M］.2nd Ed.New York:Wiley，1992:156 -162

［2］Verhaegen M，Van Dooren P.Numerical Aspects of Different Kalman Filter Implementations［J］.IEEE Transactions on Automatic Control，1986(31):907-917

［3］Rogers R M.Applied Mathematics in Integrated Navigation Systems［M］.3rd Ed.New York:American Institute of Aeronautics and Astronautics，Inc.，2007:267-277

［4］馬駿，楊功流.緊耦合MINS/GPS組合導航系統(tǒng)數(shù)據(jù)融合的分析與處理［J］.傳感技術(shù)學報，2011，24(9):1284-1289

［5］Gibbs，Richard G.Square Root Modified Bryson-Frazier Smoother［J］. IEEE Transactions on Automatic Control，2011，56(2)，452-456.

［6］Grewal M S，Andrews A P.Kalman Filtering Theory and Practice U-sing MATLAB［M］.New York:Wiley，2008:238-242

［7］Richard G Gibbs.New Kalman Filter and Smoother Consistency Tests［J］.Automatica，2013，49:3141-3144.

［8］Michael K T，Jeffrey L A，Craig H B.Ensemble Square Root Filters［J］.Notes and Correspondence，2002，25(6):862-863.

［9］白亮，秦永元，成研.基于序貫處理的航位推算組合導航算法研究［J］.傳感技術(shù)學報，2010，23(5):687-690.

［10］方靖，顧啟泰，劉學斌，等.MINS/GPS組合導航系統(tǒng)設(shè)計與實驗［J］.清華大學學報(自然科學版)，2007，47(8):1316-1319.

［11］隋樹林，袁健，張文霞，等.基于UD-EKF自主光學導航方法仿真［J］.系統(tǒng)仿真學報，2007，19(3):482-485.

［12］Liu Guangfu，Zhang Weigong，Liu Xu，et al.Data Synchronization in IMU/GPS Integrated Measurement System of Vehicle Motion Parameters［J］.Journal of Southeast University(English Edition)，2006，22(2):200-203.

［13］高憲軍，翟林培.GPS/INS組合導航系統(tǒng)的研究［J］.光學精密工程，2004，12(2):146-149.

王延東(1985-)，男，遼寧本溪人，畢業(yè)于北京理工大學。助理研究員，碩士，主要從事組合導航技術(shù)和半物理仿真研究，wyd321@126.com;

賈宏光(1971-)，男，黑龍江五常人，畢業(yè)于中科院長春光機所。研究員，博士生導師，主要從事飛行器總體設(shè)計。

Roundoff Error Restraining Method of Integrated Navigation System

WANG Yandong*，JIA Hongguang

(Changchun Institute of Optics，F(xiàn)ine Mechanics and Physics，Chinese Academy of Sciences，Changchun 130033，China)

As the most important technology for positioning and attitude measuring，the integrated navigation system is made use of SINS and GPS information for optimal estimation by Kalman filtering.It is reviewed that the roundoff error accumulating would undermine the numerical stability of the filter，when the estimation covariance matrix is updated by virtue of Riccati equation.Severely，it ruined the property of positive-definite of covariance matrix，leading the navigation system to divergence.It is presented the one order model of roundoff error propagation of Riccati equation，and analyzed how roundoff error affect Kalman filtering in theory.It is introduced the Bierman-Thorton algorithm in the essay，and the element of the algorithm is described to how to solve the numerical stability problem due to roundoff error in integrated navigation system.Consequently，it is proved that Riccati equation updating by Bierman-Thorton algorithm has better numerical stability and accuracy than which by conventional Kalman filtering，which is borne on hardware-in-the-loop simulation.

integrated navigation system;Bierman-Thorton algorithm;hardware-in-the-loop simulation;roundoff error;Kalman filtering

V249.3

A

1004－1699(2014)05－0616－06

10.3969/j.issn.1004－1699.2014.05.009

2014-01-16

2014-04-16